EdgeBotix / Robot

Initial Commit

robot.cpp@3:3e3314102e56, 2014-10-12 (annotated)

Committer:
Throwbot
Date:
Sun Oct 12 23:27:33 2014 +0000
Revision:
3:3e3314102e56
Parent:
2:37a19a9e5f2c
Child:
4:0eeea5f05e28
Ebot demo working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Throwbot 0:b74b6d70347d 1 /* mbed ROBOT Library, for SUTD evobot project, Generation 1
Throwbot 0:b74b6d70347d 2 * Copyright (c) 2013, SUTD
Throwbot 0:b74b6d70347d 3 * Author: Mark VanderMeulen
Throwbot 0:b74b6d70347d 4 * Modified: Mayuran Saravanapavanantham (this code used for STARS)
Throwbot 0:b74b6d70347d 5 *
Throwbot 0:b74b6d70347d 6 * April 22, 2013
Throwbot 0:b74b6d70347d 7 */
Throwbot 0:b74b6d70347d 8
Throwbot 0:b74b6d70347d 9 #include "robot.h"
Throwbot 0:b74b6d70347d 10 #include "math.h"
Throwbot 0:b74b6d70347d 11 //*********************************CONSTRUCTOR*********************************//
Throwbot 0:b74b6d70347d 12 //*********************************CONSTRUCTOR*********************************//
Throwbot 0:b74b6d70347d 13 HC05 bt(tx_bt,rx_bt,EN_BT);
Throwbot 0:b74b6d70347d 14 //QEI wheel (PTA16, PTA17, NC, 24);
Throwbot 1:282467cbebb3 15 QEI right (PTA16, PTA17, NC, 150, QEI::X4_ENCODING);
Throwbot 1:282467cbebb3 16 QEI left (PTA14, PTA13, NC, 150, QEI::X4_ENCODING);
Throwbot 0:b74b6d70347d 17 //Serial bt(rx_bt,tx_bt);
Throwbot 0:b74b6d70347d 18 //MPU6050 mpu(PTE0, PTE1);
Throwbot 0:b74b6d70347d 19 DigitalOut myled(myledd);
Throwbot 0:b74b6d70347d 20 DigitalOut key(PTA15);
Throwbot 0:b74b6d70347d 21 DigitalOut btSwitch(EN_BT);
Throwbot 0:b74b6d70347d 22 //AnalogIn currentSensor(CURRENTSENSOR_PIN);
Throwbot 0:b74b6d70347d 23 DigitalOut buzzer(buzz);
Throwbot 0:b74b6d70347d 24
Throwbot 0:b74b6d70347d 25 AnalogIn LDRsensor1(LDR1);
Throwbot 0:b74b6d70347d 26 AnalogIn LDRsensor2(LDR2);
Throwbot 0:b74b6d70347d 27 //AnalogIn voltageSensor(VOLTAGESENSOR_PIN);
Throwbot 0:b74b6d70347d 28 //PwmOut buzzer(buzz);
Throwbot 0:b74b6d70347d 29 PwmOut PWMA(MOT_PWMA_PIN);
Throwbot 0:b74b6d70347d 30 PwmOut PWMB(MOT_PWMB_PIN);
Throwbot 0:b74b6d70347d 31 DigitalOut AIN1(MOT_AIN1_PIN);
Throwbot 0:b74b6d70347d 32 DigitalOut AIN2(MOT_AIN2_PIN);
Throwbot 0:b74b6d70347d 33 DigitalOut BIN1(MOT_BIN1_PIN);
Throwbot 0:b74b6d70347d 34 DigitalOut BIN2(MOT_BIN2_PIN);
Throwbot 0:b74b6d70347d 35 DigitalOut STBY(MOT_STBY_PIN);
Throwbot 1:282467cbebb3 36
Throwbot 1:282467cbebb3 37 DigitalOut SRX(PTB10);
Throwbot 1:282467cbebb3 38
Throwbot 1:282467cbebb3 39 AnalogIn uL(ulL);
Throwbot 1:282467cbebb3 40 AnalogIn uF(ulF);
Throwbot 1:282467cbebb3 41 AnalogIn uR(ulR);
Throwbot 1:282467cbebb3 42 AnalogIn urR(ulrR);
Throwbot 1:282467cbebb3 43 AnalogIn urL(ulrL);
Throwbot 1:282467cbebb3 44 AnalogIn uB(ulB);
Throwbot 1:282467cbebb3 45 MPU6050 accelgyro;
Throwbot 1:282467cbebb3 46 int16_t ax, ay, az;
Throwbot 1:282467cbebb3 47 int16_t gx, gy, gz;
Throwbot 1:282467cbebb3 48 int rMotor = 1;
Throwbot 2:37a19a9e5f2c 49 int lMotor = 1;
Throwbot 0:b74b6d70347d 50 int m_speed = 100;
Throwbot 0:b74b6d70347d 51 int speed;
Throwbot 2:37a19a9e5f2c 52 Mutex stdio_mutex;
Throwbot 0:b74b6d70347d 53 int freq=0;
Throwbot 1:282467cbebb3 54 Timer t;
Throwbot 1:282467cbebb3 55 int heading=0;
Throwbot 1:282467cbebb3 56 int last_time=0;
Throwbot 1:282467cbebb3 57 int dy =0;
Throwbot 1:282467cbebb3 58 int dx=0;
Throwbot 1:282467cbebb3 59 float left_current_reading=0;
Throwbot 1:282467cbebb3 60 float right_current_reading= 0;
Throwbot 1:282467cbebb3 61 float left_change = 0;
Throwbot 1:282467cbebb3 62 float right_change =0;
Throwbot 1:282467cbebb3 63 float left_prev_read=0;
Throwbot 1:282467cbebb3 64 float right_prev_read=0;
Throwbot 1:282467cbebb3 65 int delta_y=0;
Throwbot 1:282467cbebb3 66 int delta_x=0;
Throwbot 1:282467cbebb3 67 float delta_thetha=0;
Throwbot 1:282467cbebb3 68 float encoder_yaw =0;
Throwbot 1:282467cbebb3 69 float G_thetha=0;
Throwbot 1:282467cbebb3 70 int Encoder_x=0;
Throwbot 1:282467cbebb3 71 int Encoder_y=0;
Throwbot 1:282467cbebb3 72 int dtheta=0;
Throwbot 2:37a19a9e5f2c 73 int software_interuupt;
Throwbot 3:3e3314102e56 74 int Rmotor_speed=0;
Throwbot 3:3e3314102e56 75 int Lmotor_speed=0;
Throwbot 1:282467cbebb3 76 int r_time ()
Throwbot 1:282467cbebb3 77 {
Throwbot 1:282467cbebb3 78 int mseconds = (int)time(NULL)+(int)t.read_ms();
Throwbot 1:282467cbebb3 79 return mseconds;
Throwbot 1:282467cbebb3 80 }
Throwbot 0:b74b6d70347d 81 void initRobot()
Throwbot 0:b74b6d70347d 82 {
Throwbot 0:b74b6d70347d 83 key = 0;
Throwbot 0:b74b6d70347d 84 //btSwitch = 1;
Throwbot 0:b74b6d70347d 85 bt.start();
Throwbot 0:b74b6d70347d 86 myled = 0;
Throwbot 0:b74b6d70347d 87 wait_ms(500);
Throwbot 0:b74b6d70347d 88 bt.baud(BaudRate_bt);
Throwbot 1:282467cbebb3 89 accelgyro.initialize();
Throwbot 1:282467cbebb3 90 t.start();
Throwbot 3:3e3314102e56 91 SRX = 1;
Throwbot 3:3e3314102e56 92 wait_us(30);
Throwbot 3:3e3314102e56 93 SRX=0;
Throwbot 3:3e3314102e56 94 wait_ms(300);
Throwbot 3:3e3314102e56 95 SRX = 1;
Throwbot 3:3e3314102e56 96 wait_us(30);
Throwbot 3:3e3314102e56 97 SRX=0;
Throwbot 3:3e3314102e56 98 wait(1);
Throwbot 3:3e3314102e56 99 myled = 1;
Throwbot 0:b74b6d70347d 100 }
Throwbot 0:b74b6d70347d 101
Throwbot 0:b74b6d70347d 102 //*********************************MOTORS*********************************//
Throwbot 0:b74b6d70347d 103 void motor_control(int Lspeed, int Rspeed)
Throwbot 0:b74b6d70347d 104 {
Throwbot 0:b74b6d70347d 105 //Controls the motors. 0 = stopped, 100 = full speed, -100 = reverse speed
Throwbot 2:37a19a9e5f2c 106 if (!Lspeed && !Rspeed) { //stop//
Throwbot 2:37a19a9e5f2c 107 STBY = 0;
Throwbot 2:37a19a9e5f2c 108 } else
Throwbot 0:b74b6d70347d 109 STBY = 1;
Throwbot 0:b74b6d70347d 110 //make sure 'speeds' are between -100 and 100, and not between abs(0 to 10)
Throwbot 0:b74b6d70347d 111 if(Lspeed > 100) Lspeed = 100;
Throwbot 0:b74b6d70347d 112 else if (Lspeed < -100) Lspeed = -100;
Throwbot 0:b74b6d70347d 113 else if (Lspeed < 0 && Lspeed > -15) Lspeed = -15; //prevent speed from being less than 15
Throwbot 0:b74b6d70347d 114 else if (Lspeed > 0 && Lspeed < 15) Lspeed = 15;
Throwbot 0:b74b6d70347d 115 if(Rspeed > 100) Rspeed = 100;
Throwbot 0:b74b6d70347d 116 else if (Rspeed < -100) Rspeed = -100;
Throwbot 0:b74b6d70347d 117 else if (Rspeed < 0 && Rspeed > -15) Rspeed = -15;
Throwbot 0:b74b6d70347d 118 else if (Rspeed > 0 && Rspeed < 15) Rspeed = 15;
Throwbot 0:b74b6d70347d 119 if (!Rspeed) { //if right motor is stopped
Throwbot 0:b74b6d70347d 120 AIN1 = 0;
Throwbot 0:b74b6d70347d 121 AIN2 = 0;
Throwbot 0:b74b6d70347d 122 PWMA = 0;
Throwbot 0:b74b6d70347d 123 } else if (!Lspeed) { //if left motor is stopped
Throwbot 0:b74b6d70347d 124 BIN1 = 0;
Throwbot 0:b74b6d70347d 125 BIN2 = 0;
Throwbot 0:b74b6d70347d 126 PWMB = 0;
Throwbot 0:b74b6d70347d 127 }
Throwbot 0:b74b6d70347d 128 //RIGHT MOTOR//
Throwbot 0:b74b6d70347d 129 if(Rspeed > 0) { //Right motor fwd
Throwbot 0:b74b6d70347d 130 AIN1 = MOTOR_R_DIRECTION; //set the motor direction
Throwbot 0:b74b6d70347d 131 AIN2 = !AIN1;
Throwbot 0:b74b6d70347d 132 } else { //Right motor reverse
Throwbot 0:b74b6d70347d 133 AIN2 = MOTOR_R_DIRECTION;
Throwbot 0:b74b6d70347d 134 AIN1 = !AIN2;
Throwbot 0:b74b6d70347d 135 }
Throwbot 0:b74b6d70347d 136 PWMA = abs(Rspeed/100.0);
Throwbot 0:b74b6d70347d 137 //LEFT MOTOR//
Throwbot 0:b74b6d70347d 138 if(Lspeed >0) {
Throwbot 0:b74b6d70347d 139 BIN1 = MOTOR_L_DIRECTION;
Throwbot 0:b74b6d70347d 140 BIN2 = !BIN1;
Throwbot 0:b74b6d70347d 141 } else {
Throwbot 0:b74b6d70347d 142 BIN2 = MOTOR_L_DIRECTION;
Throwbot 0:b74b6d70347d 143 BIN1 = !BIN2;
Throwbot 0:b74b6d70347d 144 }
Throwbot 0:b74b6d70347d 145 PWMB = abs(Lspeed/100.0);
Throwbot 0:b74b6d70347d 146 }
Throwbot 0:b74b6d70347d 147
Throwbot 0:b74b6d70347d 148 void stop()
Throwbot 0:b74b6d70347d 149 {
Throwbot 0:b74b6d70347d 150 motor_control(0,0);
Throwbot 0:b74b6d70347d 151 }
Throwbot 0:b74b6d70347d 152 void set_speed(int Speed)
Throwbot 0:b74b6d70347d 153 {
Throwbot 0:b74b6d70347d 154 speed = Speed;
Throwbot 0:b74b6d70347d 155 motor_control(speed,speed);
Throwbot 0:b74b6d70347d 156 }
Throwbot 0:b74b6d70347d 157
Throwbot 0:b74b6d70347d 158 double ldrread1()
Throwbot 0:b74b6d70347d 159 {
Throwbot 0:b74b6d70347d 160 double r = LDRsensor1.read();
Throwbot 0:b74b6d70347d 161 return r;
Throwbot 0:b74b6d70347d 162
Throwbot 0:b74b6d70347d 163 }
Throwbot 0:b74b6d70347d 164 double ldrread2()
Throwbot 0:b74b6d70347d 165 {
Throwbot 0:b74b6d70347d 166 double r = LDRsensor2.read();
Throwbot 0:b74b6d70347d 167 return r;
Throwbot 0:b74b6d70347d 168
Throwbot 0:b74b6d70347d 169 }
Throwbot 0:b74b6d70347d 170 void Led_on()
Throwbot 0:b74b6d70347d 171 {
Throwbot 0:b74b6d70347d 172 // pulseIn
Throwbot 0:b74b6d70347d 173 myled= 0;
Throwbot 0:b74b6d70347d 174 }
Throwbot 0:b74b6d70347d 175 void Led_off()
Throwbot 0:b74b6d70347d 176 {
Throwbot 0:b74b6d70347d 177 // pulseIn
Throwbot 0:b74b6d70347d 178 myled= 1;
Throwbot 0:b74b6d70347d 179 }
Throwbot 0:b74b6d70347d 180 /*double pl_buzzer()
Throwbot 0:b74b6d70347d 181 {
Throwbot 0:b74b6d70347d 182 for (int i=0;i<1000;i++)
Throwbot 0:b74b6d70347d 183 {
Throwbot 0:b74b6d70347d 184 int freq = 150+(i*10);
Throwbot 0:b74b6d70347d 185 buzzer=1;
Throwbot 0:b74b6d70347d 186 wait_us(1000000/freq);
Throwbot 0:b74b6d70347d 187 buzzer=0;
Throwbot 0:b74b6d70347d 188 wait_us(1000000/freq);
Throwbot 0:b74b6d70347d 189 wait_ms(1);
Throwbot 0:b74b6d70347d 190 }
Throwbot 0:b74b6d70347d 191
Throwbot 0:b74b6d70347d 192 }
Throwbot 0:b74b6d70347d 193 */
Throwbot 0:b74b6d70347d 194 void pl_buzzer(void const *args)
Throwbot 0:b74b6d70347d 195 {
Throwbot 2:37a19a9e5f2c 196 while(true) {
Throwbot 2:37a19a9e5f2c 197 stdio_mutex.lock();
Throwbot 0:b74b6d70347d 198 float pulse_delay = 150+((float)freq*10);
Throwbot 0:b74b6d70347d 199 pulse_delay= 1000/pulse_delay;
Throwbot 0:b74b6d70347d 200 stdio_mutex.unlock();
Throwbot 2:37a19a9e5f2c 201 // bt.lock();
Throwbot 0:b74b6d70347d 202 //bt.printf("s;%lf;s\n\r",pulse_delay);
Throwbot 0:b74b6d70347d 203 //bt.unlock();
Throwbot 0:b74b6d70347d 204 buzzer=1;
Throwbot 1:282467cbebb3 205 //Thread::wait(pulse_delay);
Throwbot 0:b74b6d70347d 206 buzzer=0;
Throwbot 1:282467cbebb3 207 //Thread::wait(pulse_delay);
Throwbot 2:37a19a9e5f2c 208 }
Throwbot 2:37a19a9e5f2c 209
Throwbot 0:b74b6d70347d 210 //freq = 150+(freq*10);
Throwbot 0:b74b6d70347d 211 //buzzer.period_us(1000000/freq); // 4 second period
Throwbot 0:b74b6d70347d 212 //buzz.pulsewidth(2); // 2 second pulse (on)
Throwbot 0:b74b6d70347d 213 //buzzer.write(0.5f); // 50% duty cycle
Throwbot 1:282467cbebb3 214 }
Throwbot 1:282467cbebb3 215 void Imu_yaw(void const *args)
Throwbot 1:282467cbebb3 216 {
Throwbot 2:37a19a9e5f2c 217 while(true)
Throwbot 1:282467cbebb3 218
Throwbot 2:37a19a9e5f2c 219 {
Throwbot 2:37a19a9e5f2c 220 accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
Throwbot 2:37a19a9e5f2c 221 int m_seconds = r_time();
Throwbot 1:282467cbebb3 222 float dt = ((float)(m_seconds-last_time))/1000;
Throwbot 2:37a19a9e5f2c 223 last_time=m_seconds;
Throwbot 1:282467cbebb3 224 if ((gz)<800&& gz>-800) {
Throwbot 1:282467cbebb3 225 gz=0;
Throwbot 1:282467cbebb3 226 }
Throwbot 1:282467cbebb3 227 stdio_mutex.lock();
Throwbot 2:37a19a9e5f2c 228 heading = heading + (dt*gz)*3/4/100;
Throwbot 2:37a19a9e5f2c 229 if(heading>360)
Throwbot 2:37a19a9e5f2c 230 heading= heading -360;
Throwbot 2:37a19a9e5f2c 231 else if (heading <0)
Throwbot 2:37a19a9e5f2c 232 heading = heading +360;
Throwbot 1:282467cbebb3 233 stdio_mutex.unlock();
Throwbot 2:37a19a9e5f2c 234 Thread:: wait(50);
Throwbot 2:37a19a9e5f2c 235 }
Throwbot 1:282467cbebb3 236 }
Throwbot 1:282467cbebb3 237 void encoder_thread(void const *args)
Throwbot 1:282467cbebb3 238 {
Throwbot 2:37a19a9e5f2c 239 while(true) {
Throwbot 2:37a19a9e5f2c 240 left_current_reading=left.getPulses()*(-1)/5;
Throwbot 2:37a19a9e5f2c 241 right_current_reading= right.getPulses()/5;
Throwbot 2:37a19a9e5f2c 242 left_change = left_current_reading- left_prev_read;
Throwbot 2:37a19a9e5f2c 243 right_change =right_current_reading- right_prev_read;
Throwbot 2:37a19a9e5f2c 244 left_prev_read=left_current_reading;
Throwbot 2:37a19a9e5f2c 245 right_prev_read=right_current_reading;
Throwbot 2:37a19a9e5f2c 246 delta_y=(left_change *cos(encoder_yaw) + right_change *cos(encoder_yaw))/2;
Throwbot 2:37a19a9e5f2c 247 delta_x=(left_change *sin(encoder_yaw) + right_change *sin(encoder_yaw))/2;
Throwbot 2:37a19a9e5f2c 248 delta_thetha=atan((right_change-left_change)/100);
Throwbot 2:37a19a9e5f2c 249 encoder_yaw =encoder_yaw+delta_thetha;
Throwbot 2:37a19a9e5f2c 250 G_thetha=encoder_yaw*180/M_PI; //global thetha, overall
Throwbot 2:37a19a9e5f2c 251 Encoder_x=Encoder_x+delta_x;
Throwbot 2:37a19a9e5f2c 252 Encoder_y=Encoder_y+delta_y;
Throwbot 2:37a19a9e5f2c 253 stdio_mutex.lock();
Throwbot 2:37a19a9e5f2c 254 dx=delta_x+dx;
Throwbot 2:37a19a9e5f2c 255 dy=delta_y+dy;
Throwbot 2:37a19a9e5f2c 256 dtheta=delta_thetha*180/M_PI;
Throwbot 3:3e3314102e56 257 //bt.lock();
Throwbot 3:3e3314102e56 258 //bt.printf(">>D;%0.2lf;%0.2lf;%0.2lf;%0.2lf;%d;%d;%d;\r\n",
Throwbot 3:3e3314102e56 259 // left_current_reading,right_current_reading,left_change ,right_change,\
Throwbot 3:3e3314102e56 260 // dx,dy,\
Throwbot 3:3e3314102e56 261 // dtheta);
Throwbot 3:3e3314102e56 262 //bt.unlock();
Throwbot 2:37a19a9e5f2c 263 stdio_mutex.unlock();
Throwbot 2:37a19a9e5f2c 264 Thread:: wait(50);
Throwbot 2:37a19a9e5f2c 265 }
Throwbot 0:b74b6d70347d 266 }