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?
User | Revision | Line number | New 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 | } |