Initial Commit
robot.cpp@1:282467cbebb3, 2014-10-11 (annotated)
- Committer:
- Throwbot
- Date:
- Sat Oct 11 03:53:04 2014 +0000
- Revision:
- 1:282467cbebb3
- Parent:
- 0:b74b6d70347d
- Child:
- 2:37a19a9e5f2c
uLTRASONIC SEPERATE CLASS
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 | 0:b74b6d70347d | 49 | int lMotor = -1; |
Throwbot | 0:b74b6d70347d | 50 | int m_speed = 100; |
Throwbot | 0:b74b6d70347d | 51 | int speed; |
Throwbot | 0:b74b6d70347d | 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 | 1:282467cbebb3 | 73 | int r_time () |
Throwbot | 1:282467cbebb3 | 74 | { |
Throwbot | 1:282467cbebb3 | 75 | int mseconds = (int)time(NULL)+(int)t.read_ms(); |
Throwbot | 1:282467cbebb3 | 76 | return mseconds; |
Throwbot | 1:282467cbebb3 | 77 | } |
Throwbot | 0:b74b6d70347d | 78 | void initRobot() |
Throwbot | 0:b74b6d70347d | 79 | { |
Throwbot | 0:b74b6d70347d | 80 | key = 0; |
Throwbot | 0:b74b6d70347d | 81 | //btSwitch = 1; |
Throwbot | 0:b74b6d70347d | 82 | bt.start(); |
Throwbot | 0:b74b6d70347d | 83 | myled = 0; |
Throwbot | 0:b74b6d70347d | 84 | wait_ms(500); |
Throwbot | 0:b74b6d70347d | 85 | bt.baud(BaudRate_bt); |
Throwbot | 0:b74b6d70347d | 86 | myled = 1; |
Throwbot | 1:282467cbebb3 | 87 | accelgyro.initialize(); |
Throwbot | 1:282467cbebb3 | 88 | t.start(); |
Throwbot | 1:282467cbebb3 | 89 | SRX = 0; |
Throwbot | 0:b74b6d70347d | 90 | } |
Throwbot | 0:b74b6d70347d | 91 | |
Throwbot | 0:b74b6d70347d | 92 | //*********************************MOTORS*********************************// |
Throwbot | 0:b74b6d70347d | 93 | void motor_control(int Lspeed, int Rspeed) |
Throwbot | 0:b74b6d70347d | 94 | { |
Throwbot | 0:b74b6d70347d | 95 | //Controls the motors. 0 = stopped, 100 = full speed, -100 = reverse speed |
Throwbot | 0:b74b6d70347d | 96 | if (!Lspeed && !Rspeed) //stop// |
Throwbot | 0:b74b6d70347d | 97 | { STBY = 0; |
Throwbot | 0:b74b6d70347d | 98 | } |
Throwbot | 0:b74b6d70347d | 99 | else |
Throwbot | 0:b74b6d70347d | 100 | STBY = 1; |
Throwbot | 0:b74b6d70347d | 101 | //make sure 'speeds' are between -100 and 100, and not between abs(0 to 10) |
Throwbot | 0:b74b6d70347d | 102 | if(Lspeed > 100) Lspeed = 100; |
Throwbot | 0:b74b6d70347d | 103 | else if (Lspeed < -100) Lspeed = -100; |
Throwbot | 0:b74b6d70347d | 104 | else if (Lspeed < 0 && Lspeed > -15) Lspeed = -15; //prevent speed from being less than 15 |
Throwbot | 0:b74b6d70347d | 105 | else if (Lspeed > 0 && Lspeed < 15) Lspeed = 15; |
Throwbot | 0:b74b6d70347d | 106 | if(Rspeed > 100) Rspeed = 100; |
Throwbot | 0:b74b6d70347d | 107 | else if (Rspeed < -100) Rspeed = -100; |
Throwbot | 0:b74b6d70347d | 108 | else if (Rspeed < 0 && Rspeed > -15) Rspeed = -15; |
Throwbot | 0:b74b6d70347d | 109 | else if (Rspeed > 0 && Rspeed < 15) Rspeed = 15; |
Throwbot | 0:b74b6d70347d | 110 | if (!Rspeed) { //if right motor is stopped |
Throwbot | 0:b74b6d70347d | 111 | AIN1 = 0; |
Throwbot | 0:b74b6d70347d | 112 | AIN2 = 0; |
Throwbot | 0:b74b6d70347d | 113 | PWMA = 0; |
Throwbot | 0:b74b6d70347d | 114 | } else if (!Lspeed) { //if left motor is stopped |
Throwbot | 0:b74b6d70347d | 115 | BIN1 = 0; |
Throwbot | 0:b74b6d70347d | 116 | BIN2 = 0; |
Throwbot | 0:b74b6d70347d | 117 | PWMB = 0; |
Throwbot | 0:b74b6d70347d | 118 | } |
Throwbot | 0:b74b6d70347d | 119 | //RIGHT MOTOR// |
Throwbot | 0:b74b6d70347d | 120 | if(Rspeed > 0) { //Right motor fwd |
Throwbot | 0:b74b6d70347d | 121 | AIN1 = MOTOR_R_DIRECTION; //set the motor direction |
Throwbot | 0:b74b6d70347d | 122 | AIN2 = !AIN1; |
Throwbot | 0:b74b6d70347d | 123 | } else { //Right motor reverse |
Throwbot | 0:b74b6d70347d | 124 | AIN2 = MOTOR_R_DIRECTION; |
Throwbot | 0:b74b6d70347d | 125 | AIN1 = !AIN2; |
Throwbot | 0:b74b6d70347d | 126 | } |
Throwbot | 0:b74b6d70347d | 127 | PWMA = abs(Rspeed/100.0); |
Throwbot | 0:b74b6d70347d | 128 | //LEFT MOTOR// |
Throwbot | 0:b74b6d70347d | 129 | if(Lspeed >0) { |
Throwbot | 0:b74b6d70347d | 130 | BIN1 = MOTOR_L_DIRECTION; |
Throwbot | 0:b74b6d70347d | 131 | BIN2 = !BIN1; |
Throwbot | 0:b74b6d70347d | 132 | } else { |
Throwbot | 0:b74b6d70347d | 133 | BIN2 = MOTOR_L_DIRECTION; |
Throwbot | 0:b74b6d70347d | 134 | BIN1 = !BIN2; |
Throwbot | 0:b74b6d70347d | 135 | } |
Throwbot | 0:b74b6d70347d | 136 | PWMB = abs(Lspeed/100.0); |
Throwbot | 0:b74b6d70347d | 137 | } |
Throwbot | 0:b74b6d70347d | 138 | |
Throwbot | 0:b74b6d70347d | 139 | void stop() |
Throwbot | 0:b74b6d70347d | 140 | { |
Throwbot | 0:b74b6d70347d | 141 | motor_control(0,0); |
Throwbot | 0:b74b6d70347d | 142 | } |
Throwbot | 0:b74b6d70347d | 143 | void set_speed(int Speed) |
Throwbot | 0:b74b6d70347d | 144 | { |
Throwbot | 0:b74b6d70347d | 145 | speed = Speed; |
Throwbot | 0:b74b6d70347d | 146 | motor_control(speed,speed); |
Throwbot | 0:b74b6d70347d | 147 | } |
Throwbot | 0:b74b6d70347d | 148 | |
Throwbot | 0:b74b6d70347d | 149 | double ldrread1() |
Throwbot | 0:b74b6d70347d | 150 | { |
Throwbot | 0:b74b6d70347d | 151 | double r = LDRsensor1.read(); |
Throwbot | 0:b74b6d70347d | 152 | return r; |
Throwbot | 0:b74b6d70347d | 153 | |
Throwbot | 0:b74b6d70347d | 154 | } |
Throwbot | 0:b74b6d70347d | 155 | double ldrread2() |
Throwbot | 0:b74b6d70347d | 156 | { |
Throwbot | 0:b74b6d70347d | 157 | double r = LDRsensor2.read(); |
Throwbot | 0:b74b6d70347d | 158 | return r; |
Throwbot | 0:b74b6d70347d | 159 | |
Throwbot | 0:b74b6d70347d | 160 | } |
Throwbot | 0:b74b6d70347d | 161 | void Led_on() |
Throwbot | 0:b74b6d70347d | 162 | { |
Throwbot | 0:b74b6d70347d | 163 | // pulseIn |
Throwbot | 0:b74b6d70347d | 164 | myled= 0; |
Throwbot | 0:b74b6d70347d | 165 | } |
Throwbot | 0:b74b6d70347d | 166 | void Led_off() |
Throwbot | 0:b74b6d70347d | 167 | { |
Throwbot | 0:b74b6d70347d | 168 | // pulseIn |
Throwbot | 0:b74b6d70347d | 169 | myled= 1; |
Throwbot | 0:b74b6d70347d | 170 | } |
Throwbot | 0:b74b6d70347d | 171 | /*double pl_buzzer() |
Throwbot | 0:b74b6d70347d | 172 | { |
Throwbot | 0:b74b6d70347d | 173 | for (int i=0;i<1000;i++) |
Throwbot | 0:b74b6d70347d | 174 | { |
Throwbot | 0:b74b6d70347d | 175 | int freq = 150+(i*10); |
Throwbot | 0:b74b6d70347d | 176 | buzzer=1; |
Throwbot | 0:b74b6d70347d | 177 | wait_us(1000000/freq); |
Throwbot | 0:b74b6d70347d | 178 | buzzer=0; |
Throwbot | 0:b74b6d70347d | 179 | wait_us(1000000/freq); |
Throwbot | 0:b74b6d70347d | 180 | wait_ms(1); |
Throwbot | 0:b74b6d70347d | 181 | } |
Throwbot | 0:b74b6d70347d | 182 | |
Throwbot | 0:b74b6d70347d | 183 | } |
Throwbot | 0:b74b6d70347d | 184 | */ |
Throwbot | 0:b74b6d70347d | 185 | void pl_buzzer(void const *args) |
Throwbot | 0:b74b6d70347d | 186 | { |
Throwbot | 0:b74b6d70347d | 187 | while(true) |
Throwbot | 0:b74b6d70347d | 188 | { |
Throwbot | 0:b74b6d70347d | 189 | stdio_mutex.lock(); |
Throwbot | 0:b74b6d70347d | 190 | float pulse_delay = 150+((float)freq*10); |
Throwbot | 0:b74b6d70347d | 191 | pulse_delay= 1000/pulse_delay; |
Throwbot | 0:b74b6d70347d | 192 | stdio_mutex.unlock(); |
Throwbot | 0:b74b6d70347d | 193 | // bt.lock(); |
Throwbot | 0:b74b6d70347d | 194 | //bt.printf("s;%lf;s\n\r",pulse_delay); |
Throwbot | 0:b74b6d70347d | 195 | //bt.unlock(); |
Throwbot | 0:b74b6d70347d | 196 | buzzer=1; |
Throwbot | 1:282467cbebb3 | 197 | //Thread::wait(pulse_delay); |
Throwbot | 0:b74b6d70347d | 198 | buzzer=0; |
Throwbot | 1:282467cbebb3 | 199 | //Thread::wait(pulse_delay); |
Throwbot | 0:b74b6d70347d | 200 | } |
Throwbot | 0:b74b6d70347d | 201 | |
Throwbot | 0:b74b6d70347d | 202 | //freq = 150+(freq*10); |
Throwbot | 0:b74b6d70347d | 203 | //buzzer.period_us(1000000/freq); // 4 second period |
Throwbot | 0:b74b6d70347d | 204 | //buzz.pulsewidth(2); // 2 second pulse (on) |
Throwbot | 0:b74b6d70347d | 205 | //buzzer.write(0.5f); // 50% duty cycle |
Throwbot | 1:282467cbebb3 | 206 | } |
Throwbot | 1:282467cbebb3 | 207 | void Imu_yaw(void const *args) |
Throwbot | 1:282467cbebb3 | 208 | { |
Throwbot | 1:282467cbebb3 | 209 | while(true) |
Throwbot | 1:282467cbebb3 | 210 | |
Throwbot | 1:282467cbebb3 | 211 | { |
Throwbot | 1:282467cbebb3 | 212 | accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); |
Throwbot | 1:282467cbebb3 | 213 | int m_seconds = r_time(); |
Throwbot | 1:282467cbebb3 | 214 | float dt = ((float)(m_seconds-last_time))/1000; |
Throwbot | 1:282467cbebb3 | 215 | last_time=m_seconds; |
Throwbot | 1:282467cbebb3 | 216 | if ((gz)<800&& gz>-800) { |
Throwbot | 1:282467cbebb3 | 217 | gz=0; |
Throwbot | 1:282467cbebb3 | 218 | } |
Throwbot | 1:282467cbebb3 | 219 | stdio_mutex.lock(); |
Throwbot | 1:282467cbebb3 | 220 | heading = heading + (dt*gz)*3/4/100; |
Throwbot | 1:282467cbebb3 | 221 | if(heading>360) |
Throwbot | 1:282467cbebb3 | 222 | heading= heading -360; |
Throwbot | 1:282467cbebb3 | 223 | else if (heading <0) |
Throwbot | 1:282467cbebb3 | 224 | heading = heading +360; |
Throwbot | 1:282467cbebb3 | 225 | stdio_mutex.unlock(); |
Throwbot | 1:282467cbebb3 | 226 | Thread:: wait(20); |
Throwbot | 1:282467cbebb3 | 227 | } |
Throwbot | 1:282467cbebb3 | 228 | } |
Throwbot | 1:282467cbebb3 | 229 | void encoder_thread(void const *args) |
Throwbot | 1:282467cbebb3 | 230 | { |
Throwbot | 1:282467cbebb3 | 231 | while(true) |
Throwbot | 1:282467cbebb3 | 232 | { |
Throwbot | 1:282467cbebb3 | 233 | left_current_reading=left.getPulses()*(-1)/5; |
Throwbot | 1:282467cbebb3 | 234 | right_current_reading= right.getPulses()/5; |
Throwbot | 1:282467cbebb3 | 235 | left_change = left_current_reading- left_prev_read; |
Throwbot | 1:282467cbebb3 | 236 | right_change =right_current_reading- right_prev_read; |
Throwbot | 1:282467cbebb3 | 237 | left_prev_read=left_current_reading; |
Throwbot | 1:282467cbebb3 | 238 | right_prev_read=right_current_reading; |
Throwbot | 1:282467cbebb3 | 239 | delta_y=(left_change *cos(encoder_yaw) + right_change *cos(encoder_yaw))/2; |
Throwbot | 1:282467cbebb3 | 240 | delta_x=(left_change *sin(encoder_yaw) + right_change *sin(encoder_yaw))/2; |
Throwbot | 1:282467cbebb3 | 241 | delta_thetha=atan((right_change-left_change)/100); |
Throwbot | 1:282467cbebb3 | 242 | encoder_yaw =encoder_yaw+delta_thetha; |
Throwbot | 1:282467cbebb3 | 243 | G_thetha=encoder_yaw*180/M_PI; //global thetha, overall |
Throwbot | 1:282467cbebb3 | 244 | Encoder_x=Encoder_x+delta_x; |
Throwbot | 1:282467cbebb3 | 245 | Encoder_y=Encoder_y+delta_y; |
Throwbot | 1:282467cbebb3 | 246 | stdio_mutex.lock(); |
Throwbot | 1:282467cbebb3 | 247 | dx=delta_x+dx; |
Throwbot | 1:282467cbebb3 | 248 | dy=delta_y+dy; |
Throwbot | 1:282467cbebb3 | 249 | dtheta=delta_thetha*180/M_PI; |
Throwbot | 1:282467cbebb3 | 250 | stdio_mutex.unlock(); |
Throwbot | 1:282467cbebb3 | 251 | //bt.lock(); |
Throwbot | 1:282467cbebb3 | 252 | //bt.printf("%0.2lf\t%0.2lf;\t%d;\t%d;\t%d;\t%d;\t%lf;\t%lf;\t :s\n\r",left_current_reading,right_current_reading,Encoder_x,Encoder_y,delta_y,delta_x,delta_thetha,G_thetha); |
Throwbot | 1:282467cbebb3 | 253 | //bt.unlock(); |
Throwbot | 1:282467cbebb3 | 254 | Thread:: wait(50); |
Throwbot | 1:282467cbebb3 | 255 | } |
Throwbot | 0:b74b6d70347d | 256 | } |