Initial Commit
robot.cpp@5:a95a6243c118, 2014-10-21 (annotated)
- Committer:
- Throwbot
- Date:
- Tue Oct 21 21:29:54 2014 +0000
- Revision:
- 5:a95a6243c118
- Parent:
- 4:0eeea5f05e28
Initial (Test) Commit
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 | 5:a95a6243c118 | 15 | QEI right (PTA16, PTA17, NC, 300, QEI::X4_ENCODING); |
Throwbot | 5:a95a6243c118 | 16 | QEI left (PTA14, PTA13, NC, 300, 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 | 4:0eeea5f05e28 | 38 | TB6612 MotorA(MOT_PWMA_PIN, MOT_AIN1_PIN, MOT_AIN2_PIN); |
Throwbot | 4:0eeea5f05e28 | 39 | TB6612 MotorB(MOT_PWMB_PIN, MOT_BIN1_PIN, MOT_BIN2_PIN); |
Throwbot | 4:0eeea5f05e28 | 40 | Motors motors( &MotorA, &MotorB, MOT_STBY_PIN); |
Throwbot | 1:282467cbebb3 | 41 | |
Throwbot | 1:282467cbebb3 | 42 | AnalogIn uL(ulL); |
Throwbot | 1:282467cbebb3 | 43 | AnalogIn uF(ulF); |
Throwbot | 1:282467cbebb3 | 44 | AnalogIn uR(ulR); |
Throwbot | 1:282467cbebb3 | 45 | AnalogIn urR(ulrR); |
Throwbot | 1:282467cbebb3 | 46 | AnalogIn urL(ulrL); |
Throwbot | 1:282467cbebb3 | 47 | AnalogIn uB(ulB); |
Throwbot | 1:282467cbebb3 | 48 | MPU6050 accelgyro; |
Throwbot | 1:282467cbebb3 | 49 | int16_t ax, ay, az; |
Throwbot | 1:282467cbebb3 | 50 | int16_t gx, gy, gz; |
Throwbot | 1:282467cbebb3 | 51 | int rMotor = 1; |
Throwbot | 2:37a19a9e5f2c | 52 | int lMotor = 1; |
Throwbot | 0:b74b6d70347d | 53 | int m_speed = 100; |
Throwbot | 0:b74b6d70347d | 54 | int speed; |
Throwbot | 2:37a19a9e5f2c | 55 | Mutex stdio_mutex; |
Throwbot | 0:b74b6d70347d | 56 | int freq=0; |
Throwbot | 1:282467cbebb3 | 57 | Timer t; |
Throwbot | 1:282467cbebb3 | 58 | int heading=0; |
Throwbot | 1:282467cbebb3 | 59 | int last_time=0; |
Throwbot | 1:282467cbebb3 | 60 | int dy =0; |
Throwbot | 1:282467cbebb3 | 61 | int dx=0; |
Throwbot | 1:282467cbebb3 | 62 | float left_current_reading=0; |
Throwbot | 1:282467cbebb3 | 63 | float right_current_reading= 0; |
Throwbot | 1:282467cbebb3 | 64 | float left_change = 0; |
Throwbot | 1:282467cbebb3 | 65 | float right_change =0; |
Throwbot | 1:282467cbebb3 | 66 | float left_prev_read=0; |
Throwbot | 1:282467cbebb3 | 67 | float right_prev_read=0; |
Throwbot | 1:282467cbebb3 | 68 | int delta_y=0; |
Throwbot | 1:282467cbebb3 | 69 | int delta_x=0; |
Throwbot | 1:282467cbebb3 | 70 | float delta_thetha=0; |
Throwbot | 1:282467cbebb3 | 71 | float encoder_yaw =0; |
Throwbot | 1:282467cbebb3 | 72 | float G_thetha=0; |
Throwbot | 1:282467cbebb3 | 73 | int Encoder_x=0; |
Throwbot | 1:282467cbebb3 | 74 | int Encoder_y=0; |
Throwbot | 1:282467cbebb3 | 75 | int dtheta=0; |
Throwbot | 2:37a19a9e5f2c | 76 | int software_interuupt; |
Throwbot | 3:3e3314102e56 | 77 | int Rmotor_speed=0; |
Throwbot | 3:3e3314102e56 | 78 | int Lmotor_speed=0; |
Throwbot | 4:0eeea5f05e28 | 79 | char Selected_robot; |
Throwbot | 4:0eeea5f05e28 | 80 | bool bt_connected=false; |
Throwbot | 1:282467cbebb3 | 81 | int r_time () |
Throwbot | 1:282467cbebb3 | 82 | { |
Throwbot | 1:282467cbebb3 | 83 | int mseconds = (int)time(NULL)+(int)t.read_ms(); |
Throwbot | 1:282467cbebb3 | 84 | return mseconds; |
Throwbot | 1:282467cbebb3 | 85 | } |
Throwbot | 0:b74b6d70347d | 86 | void initRobot() |
Throwbot | 0:b74b6d70347d | 87 | { |
Throwbot | 0:b74b6d70347d | 88 | key = 0; |
Throwbot | 0:b74b6d70347d | 89 | //btSwitch = 1; |
Throwbot | 0:b74b6d70347d | 90 | bt.start(); |
Throwbot | 0:b74b6d70347d | 91 | myled = 0; |
Throwbot | 0:b74b6d70347d | 92 | wait_ms(500); |
Throwbot | 0:b74b6d70347d | 93 | bt.baud(BaudRate_bt); |
Throwbot | 1:282467cbebb3 | 94 | accelgyro.initialize(); |
Throwbot | 4:0eeea5f05e28 | 95 | MotorA.scale = R_MOTOR_SCALE; |
Throwbot | 4:0eeea5f05e28 | 96 | MotorB.scale = L_MOTOR_SCALE; |
Throwbot | 1:282467cbebb3 | 97 | t.start(); |
Throwbot | 3:3e3314102e56 | 98 | SRX = 1; |
Throwbot | 3:3e3314102e56 | 99 | wait_us(30); |
Throwbot | 3:3e3314102e56 | 100 | SRX=0; |
Throwbot | 3:3e3314102e56 | 101 | wait_ms(300); |
Throwbot | 3:3e3314102e56 | 102 | SRX = 1; |
Throwbot | 3:3e3314102e56 | 103 | wait_us(30); |
Throwbot | 3:3e3314102e56 | 104 | SRX=0; |
Throwbot | 3:3e3314102e56 | 105 | wait(1); |
Throwbot | 3:3e3314102e56 | 106 | myled = 1; |
Throwbot | 0:b74b6d70347d | 107 | } |
Throwbot | 0:b74b6d70347d | 108 | |
Throwbot | 0:b74b6d70347d | 109 | //*********************************MOTORS*********************************// |
Throwbot | 0:b74b6d70347d | 110 | void motor_control(int Lspeed, int Rspeed) |
Throwbot | 0:b74b6d70347d | 111 | { |
Throwbot | 0:b74b6d70347d | 112 | //Controls the motors. 0 = stopped, 100 = full speed, -100 = reverse speed |
Throwbot | 2:37a19a9e5f2c | 113 | if (!Lspeed && !Rspeed) { //stop// |
Throwbot | 2:37a19a9e5f2c | 114 | STBY = 0; |
Throwbot | 2:37a19a9e5f2c | 115 | } else |
Throwbot | 0:b74b6d70347d | 116 | STBY = 1; |
Throwbot | 0:b74b6d70347d | 117 | //make sure 'speeds' are between -100 and 100, and not between abs(0 to 10) |
Throwbot | 0:b74b6d70347d | 118 | if(Lspeed > 100) Lspeed = 100; |
Throwbot | 0:b74b6d70347d | 119 | else if (Lspeed < -100) Lspeed = -100; |
Throwbot | 0:b74b6d70347d | 120 | else if (Lspeed < 0 && Lspeed > -15) Lspeed = -15; //prevent speed from being less than 15 |
Throwbot | 0:b74b6d70347d | 121 | else if (Lspeed > 0 && Lspeed < 15) Lspeed = 15; |
Throwbot | 0:b74b6d70347d | 122 | if(Rspeed > 100) Rspeed = 100; |
Throwbot | 0:b74b6d70347d | 123 | else if (Rspeed < -100) Rspeed = -100; |
Throwbot | 0:b74b6d70347d | 124 | else if (Rspeed < 0 && Rspeed > -15) Rspeed = -15; |
Throwbot | 0:b74b6d70347d | 125 | else if (Rspeed > 0 && Rspeed < 15) Rspeed = 15; |
Throwbot | 0:b74b6d70347d | 126 | if (!Rspeed) { //if right motor is stopped |
Throwbot | 0:b74b6d70347d | 127 | AIN1 = 0; |
Throwbot | 0:b74b6d70347d | 128 | AIN2 = 0; |
Throwbot | 0:b74b6d70347d | 129 | PWMA = 0; |
Throwbot | 0:b74b6d70347d | 130 | } else if (!Lspeed) { //if left motor is stopped |
Throwbot | 0:b74b6d70347d | 131 | BIN1 = 0; |
Throwbot | 0:b74b6d70347d | 132 | BIN2 = 0; |
Throwbot | 0:b74b6d70347d | 133 | PWMB = 0; |
Throwbot | 0:b74b6d70347d | 134 | } |
Throwbot | 0:b74b6d70347d | 135 | //RIGHT MOTOR// |
Throwbot | 0:b74b6d70347d | 136 | if(Rspeed > 0) { //Right motor fwd |
Throwbot | 0:b74b6d70347d | 137 | AIN1 = MOTOR_R_DIRECTION; //set the motor direction |
Throwbot | 0:b74b6d70347d | 138 | AIN2 = !AIN1; |
Throwbot | 0:b74b6d70347d | 139 | } else { //Right motor reverse |
Throwbot | 0:b74b6d70347d | 140 | AIN2 = MOTOR_R_DIRECTION; |
Throwbot | 0:b74b6d70347d | 141 | AIN1 = !AIN2; |
Throwbot | 0:b74b6d70347d | 142 | } |
Throwbot | 0:b74b6d70347d | 143 | PWMA = abs(Rspeed/100.0); |
Throwbot | 0:b74b6d70347d | 144 | //LEFT MOTOR// |
Throwbot | 0:b74b6d70347d | 145 | if(Lspeed >0) { |
Throwbot | 0:b74b6d70347d | 146 | BIN1 = MOTOR_L_DIRECTION; |
Throwbot | 0:b74b6d70347d | 147 | BIN2 = !BIN1; |
Throwbot | 0:b74b6d70347d | 148 | } else { |
Throwbot | 0:b74b6d70347d | 149 | BIN2 = MOTOR_L_DIRECTION; |
Throwbot | 0:b74b6d70347d | 150 | BIN1 = !BIN2; |
Throwbot | 0:b74b6d70347d | 151 | } |
Throwbot | 0:b74b6d70347d | 152 | PWMB = abs(Lspeed/100.0); |
Throwbot | 0:b74b6d70347d | 153 | } |
Throwbot | 0:b74b6d70347d | 154 | |
Throwbot | 0:b74b6d70347d | 155 | void stop() |
Throwbot | 0:b74b6d70347d | 156 | { |
Throwbot | 0:b74b6d70347d | 157 | motor_control(0,0); |
Throwbot | 0:b74b6d70347d | 158 | } |
Throwbot | 0:b74b6d70347d | 159 | void set_speed(int Speed) |
Throwbot | 0:b74b6d70347d | 160 | { |
Throwbot | 0:b74b6d70347d | 161 | speed = Speed; |
Throwbot | 0:b74b6d70347d | 162 | motor_control(speed,speed); |
Throwbot | 0:b74b6d70347d | 163 | } |
Throwbot | 0:b74b6d70347d | 164 | |
Throwbot | 0:b74b6d70347d | 165 | double ldrread1() |
Throwbot | 0:b74b6d70347d | 166 | { |
Throwbot | 0:b74b6d70347d | 167 | double r = LDRsensor1.read(); |
Throwbot | 0:b74b6d70347d | 168 | return r; |
Throwbot | 0:b74b6d70347d | 169 | |
Throwbot | 0:b74b6d70347d | 170 | } |
Throwbot | 0:b74b6d70347d | 171 | double ldrread2() |
Throwbot | 0:b74b6d70347d | 172 | { |
Throwbot | 0:b74b6d70347d | 173 | double r = LDRsensor2.read(); |
Throwbot | 0:b74b6d70347d | 174 | return r; |
Throwbot | 0:b74b6d70347d | 175 | |
Throwbot | 0:b74b6d70347d | 176 | } |
Throwbot | 0:b74b6d70347d | 177 | void Led_on() |
Throwbot | 0:b74b6d70347d | 178 | { |
Throwbot | 0:b74b6d70347d | 179 | // pulseIn |
Throwbot | 0:b74b6d70347d | 180 | myled= 0; |
Throwbot | 0:b74b6d70347d | 181 | } |
Throwbot | 0:b74b6d70347d | 182 | void Led_off() |
Throwbot | 0:b74b6d70347d | 183 | { |
Throwbot | 0:b74b6d70347d | 184 | // pulseIn |
Throwbot | 0:b74b6d70347d | 185 | myled= 1; |
Throwbot | 0:b74b6d70347d | 186 | } |
Throwbot | 0:b74b6d70347d | 187 | /*double pl_buzzer() |
Throwbot | 0:b74b6d70347d | 188 | { |
Throwbot | 0:b74b6d70347d | 189 | for (int i=0;i<1000;i++) |
Throwbot | 0:b74b6d70347d | 190 | { |
Throwbot | 0:b74b6d70347d | 191 | int freq = 150+(i*10); |
Throwbot | 0:b74b6d70347d | 192 | buzzer=1; |
Throwbot | 0:b74b6d70347d | 193 | wait_us(1000000/freq); |
Throwbot | 0:b74b6d70347d | 194 | buzzer=0; |
Throwbot | 0:b74b6d70347d | 195 | wait_us(1000000/freq); |
Throwbot | 0:b74b6d70347d | 196 | wait_ms(1); |
Throwbot | 0:b74b6d70347d | 197 | } |
Throwbot | 0:b74b6d70347d | 198 | |
Throwbot | 0:b74b6d70347d | 199 | } |
Throwbot | 0:b74b6d70347d | 200 | */ |
Throwbot | 4:0eeea5f05e28 | 201 | void pl_buzzer(int freq, int f_time) |
Throwbot | 0:b74b6d70347d | 202 | { |
Throwbot | 4:0eeea5f05e28 | 203 | int elapsed_time=0; |
Throwbot | 4:0eeea5f05e28 | 204 | while (elapsed_time < f_time*10) { |
Throwbot | 0:b74b6d70347d | 205 | buzzer=1; |
Throwbot | 4:0eeea5f05e28 | 206 | wait_us(1000000/freq); |
Throwbot | 0:b74b6d70347d | 207 | buzzer=0; |
Throwbot | 4:0eeea5f05e28 | 208 | wait_us(1000000/freq); |
Throwbot | 4:0eeea5f05e28 | 209 | elapsed_time++; |
Throwbot | 4:0eeea5f05e28 | 210 | } |
Throwbot | 2:37a19a9e5f2c | 211 | |
Throwbot | 1:282467cbebb3 | 212 | } |
Throwbot | 1:282467cbebb3 | 213 | void Imu_yaw(void const *args) |
Throwbot | 1:282467cbebb3 | 214 | { |
Throwbot | 2:37a19a9e5f2c | 215 | while(true) |
Throwbot | 1:282467cbebb3 | 216 | |
Throwbot | 2:37a19a9e5f2c | 217 | { |
Throwbot | 2:37a19a9e5f2c | 218 | accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); |
Throwbot | 2:37a19a9e5f2c | 219 | int m_seconds = r_time(); |
Throwbot | 1:282467cbebb3 | 220 | float dt = ((float)(m_seconds-last_time))/1000; |
Throwbot | 2:37a19a9e5f2c | 221 | last_time=m_seconds; |
Throwbot | 1:282467cbebb3 | 222 | if ((gz)<800&& gz>-800) { |
Throwbot | 1:282467cbebb3 | 223 | gz=0; |
Throwbot | 1:282467cbebb3 | 224 | } |
Throwbot | 1:282467cbebb3 | 225 | stdio_mutex.lock(); |
Throwbot | 2:37a19a9e5f2c | 226 | heading = heading + (dt*gz)*3/4/100; |
Throwbot | 2:37a19a9e5f2c | 227 | if(heading>360) |
Throwbot | 2:37a19a9e5f2c | 228 | heading= heading -360; |
Throwbot | 2:37a19a9e5f2c | 229 | else if (heading <0) |
Throwbot | 2:37a19a9e5f2c | 230 | heading = heading +360; |
Throwbot | 1:282467cbebb3 | 231 | stdio_mutex.unlock(); |
Throwbot | 2:37a19a9e5f2c | 232 | Thread:: wait(50); |
Throwbot | 2:37a19a9e5f2c | 233 | } |
Throwbot | 1:282467cbebb3 | 234 | } |
Throwbot | 1:282467cbebb3 | 235 | void encoder_thread(void const *args) |
Throwbot | 1:282467cbebb3 | 236 | { |
Throwbot | 2:37a19a9e5f2c | 237 | while(true) { |
Throwbot | 5:a95a6243c118 | 238 | left_current_reading=left.getPulses()*(-1)/9.85; |
Throwbot | 5:a95a6243c118 | 239 | right_current_reading= right.getPulses()/10; |
Throwbot | 2:37a19a9e5f2c | 240 | left_change = left_current_reading- left_prev_read; |
Throwbot | 2:37a19a9e5f2c | 241 | right_change =right_current_reading- right_prev_read; |
Throwbot | 2:37a19a9e5f2c | 242 | left_prev_read=left_current_reading; |
Throwbot | 2:37a19a9e5f2c | 243 | right_prev_read=right_current_reading; |
Throwbot | 2:37a19a9e5f2c | 244 | delta_y=(left_change *cos(encoder_yaw) + right_change *cos(encoder_yaw))/2; |
Throwbot | 2:37a19a9e5f2c | 245 | delta_x=(left_change *sin(encoder_yaw) + right_change *sin(encoder_yaw))/2; |
Throwbot | 2:37a19a9e5f2c | 246 | delta_thetha=atan((right_change-left_change)/100); |
Throwbot | 2:37a19a9e5f2c | 247 | encoder_yaw =encoder_yaw+delta_thetha; |
Throwbot | 2:37a19a9e5f2c | 248 | G_thetha=encoder_yaw*180/M_PI; //global thetha, overall |
Throwbot | 2:37a19a9e5f2c | 249 | Encoder_x=Encoder_x+delta_x; |
Throwbot | 2:37a19a9e5f2c | 250 | Encoder_y=Encoder_y+delta_y; |
Throwbot | 2:37a19a9e5f2c | 251 | stdio_mutex.lock(); |
Throwbot | 2:37a19a9e5f2c | 252 | dx=delta_x+dx; |
Throwbot | 2:37a19a9e5f2c | 253 | dy=delta_y+dy; |
Throwbot | 2:37a19a9e5f2c | 254 | dtheta=delta_thetha*180/M_PI; |
Throwbot | 3:3e3314102e56 | 255 | //bt.lock(); |
Throwbot | 3:3e3314102e56 | 256 | //bt.printf(">>D;%0.2lf;%0.2lf;%0.2lf;%0.2lf;%d;%d;%d;\r\n", |
Throwbot | 3:3e3314102e56 | 257 | // left_current_reading,right_current_reading,left_change ,right_change,\ |
Throwbot | 3:3e3314102e56 | 258 | // dx,dy,\ |
Throwbot | 3:3e3314102e56 | 259 | // dtheta); |
Throwbot | 3:3e3314102e56 | 260 | //bt.unlock(); |
Throwbot | 2:37a19a9e5f2c | 261 | stdio_mutex.unlock(); |
Throwbot | 2:37a19a9e5f2c | 262 | Thread:: wait(50); |
Throwbot | 2:37a19a9e5f2c | 263 | } |
Throwbot | 0:b74b6d70347d | 264 | } |