Shuto Naruse
/
Eurobot_2012_Secondary
Eurobot_2012_Secondary
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "rtos.h" 00003 #include "TSH.h" 00004 #include "Kalman.h" 00005 #include "globals.h" 00006 #include "motors.h" 00007 #include "math.h" 00008 #include "system.h" 00009 #include "geometryfuncs.h" 00010 00011 //#include <iostream> 00012 00013 //Interface declaration 00014 //I2C i2c(p28, p27); // sda, scl 00015 TSI2C i2c(p28, p27); 00016 Serial pc(USBTX, USBRX); // tx, rx 00017 Serial IRturret(p13, p14); 00018 00019 DigitalOut OLED1(LED1); 00020 DigitalOut OLED2(LED2); 00021 DigitalOut OLED3(LED3); 00022 DigitalOut OLED4(LED4); 00023 00024 Motors motors(i2c); 00025 Kalman kalman(motors); 00026 00027 float targetX = 1000, targetY = 1000, targetTheta = 0; 00028 00029 // bytes packing/unpacking for IR turret serial comm 00030 union IRValue_t { 00031 float IR_floats[3]; 00032 int IR_ints[3]; 00033 unsigned char IR_chars[12]; 00034 } IRValues; 00035 00036 char Alignment_char[4] = {0xFF,0xFE,0xFD,0xFC}; 00037 int Alignment_ptr = 0; 00038 bool data_flag = false; 00039 int buff_pointer = 0; 00040 bool angleInit = false; 00041 float angleOffset = 0; 00042 00043 void vIRValueISR (void); 00044 void vKalmanInit(void); 00045 00046 //TODO mutex on kalman state, and on motor commands (i.e. on the i2c bus) 00047 //NOTE! Recieving data with RF12B now DISABLED due to interferance with rtos! 00048 00049 00050 void vMotorThread(void const *argument); 00051 void vPrintState(void const *argument); 00052 void ai_thread (void const *argument); 00053 void motion_thread(void const *argument); 00054 00055 00056 float getAngle (float x, float y); 00057 void getIRValue(void const *argument); 00058 00059 // Thread pointers 00060 Thread *AI_Thread_Ptr; 00061 Thread *Motion_Thread_Ptr; 00062 00063 Mutex targetlock; 00064 bool flag_terminate = false; 00065 00066 float temp = 0; 00067 00068 //Main loop 00069 int main() { 00070 pc.baud(115200); 00071 IRturret.baud(115200); 00072 IRturret.format(8,Serial::Odd,1); 00073 IRturret.attach(&vIRValueISR,Serial::RxIrq); 00074 vKalmanInit(); 00075 00076 //Thread tMotorThread(vMotorThread,NULL,osPriorityNormal,256); 00077 Thread tUpdateState(vPrintState,NULL,osPriorityNormal,1024); 00078 00079 Thread thr_AI(ai_thread,NULL,osPriorityNormal,1024); 00080 Thread thr_motion(motion_thread,NULL,osPriorityNormal,1024); 00081 AI_Thread_Ptr = &thr_AI; 00082 Motion_Thread_Ptr = &thr_motion; 00083 00084 //measure cpu usage. output updated once per second to symbol cpupercent 00085 //Thread mCPUthread(measureCPUidle, NULL, osPriorityIdle, 1024); //check if stack overflow with such a small staack 00086 00087 00088 pc.printf("We got to main! ;D\r\n"); 00089 00090 //REMEMBERT TO PUT PULL UP RESISTORS ON I2C!!!!!!!!!!!!!! 00091 while (1) { 00092 // do nothing 00093 Thread::wait(osWaitForever); 00094 } 00095 } 00096 00097 00098 void vMotorThread(void const *argument) { 00099 motors.resetEncoders(); 00100 while (1) { 00101 motors.setSpeed(20,20); 00102 Thread::wait(2000); 00103 motors.stop(); 00104 Thread::wait(5000); 00105 motors.setSpeed(-20,-20); 00106 Thread::wait(2000); 00107 motors.stop(); 00108 Thread::wait(5000); 00109 motors.setSpeed(-20,20); 00110 Thread::wait(2000); 00111 motors.stop(); 00112 Thread::wait(5000); 00113 motors.setSpeed(20,-20); 00114 Thread::wait(2000); 00115 motors.stop(); 00116 Thread::wait(5000); 00117 } 00118 } 00119 00120 00121 00122 void vPrintState(void const *argument) { 00123 float state[3]; 00124 float SonarMeasures[3]; 00125 float IRMeasures[3]; 00126 00127 00128 while (1) { 00129 kalman.statelock.lock(); 00130 state[0] = kalman.X(0); 00131 state[1] = kalman.X(1); 00132 state[2] = kalman.X(2); 00133 SonarMeasures[0] = kalman.SonarMeasures[0]; 00134 SonarMeasures[1] = kalman.SonarMeasures[1]; 00135 SonarMeasures[2] = kalman.SonarMeasures[2]; 00136 IRMeasures[0] = kalman.IRMeasures[0]; 00137 IRMeasures[1] = kalman.IRMeasures[1]; 00138 IRMeasures[2] = kalman.IRMeasures[2]; 00139 kalman.statelock.unlock(); 00140 pc.printf("\r\n"); 00141 pc.printf("current: %0.4f %0.4f %0.4f \r\n", state[0], state[1],state[2]); 00142 pc.printf("Sonar: %0.4f %0.4f %0.4f \r\n",SonarMeasures[0],SonarMeasures[1],SonarMeasures[2]); 00143 pc.printf("IR : %0.4f %0.4f %0.4f \r\n",IRMeasures[0]*180/PI,IRMeasures[1]*180/PI,IRMeasures[2]*180/PI); 00144 pc.printf("Angle_Offset: %0.4f \r\n",angleOffset*180/PI); 00145 Thread::wait(100); 00146 } 00147 } 00148 00149 00150 // AI thread ------------------------------------ 00151 void ai_thread (void const *argument) { 00152 // goes to the mid 00153 Thread::signal_wait(0x01); 00154 targetlock.lock(); 00155 targetX = 1500; 00156 targetY = 1000; 00157 targetTheta = PI/2; 00158 targetlock.unlock(); 00159 00160 // left roll 00161 Thread::signal_wait(0x01); 00162 targetlock.lock(); 00163 targetX = 500; 00164 targetY = 1700; 00165 targetTheta = PI/2; 00166 targetlock.unlock(); 00167 00168 // mid 00169 Thread::signal_wait(0x01); 00170 targetlock.lock(); 00171 targetX = 1500; 00172 targetY = 1000; 00173 targetTheta = PI/2; 00174 targetlock.unlock(); 00175 00176 // map 00177 Thread::signal_wait(0x01); 00178 targetlock.lock(); 00179 targetX = 1500; 00180 targetY = 1700; 00181 targetTheta = PI/2; 00182 targetlock.unlock(); 00183 00184 // mid 00185 Thread::signal_wait(0x01); 00186 targetlock.lock(); 00187 targetX = 1500; 00188 targetY = 1000; 00189 targetTheta = -PI/2; 00190 targetlock.unlock(); 00191 00192 // home 00193 Thread::signal_wait(0x01); 00194 targetlock.lock(); 00195 targetX = 500; 00196 targetY = 500; 00197 targetTheta = 0; 00198 targetlock.unlock(); 00199 00200 Thread::signal_wait(0x01); 00201 flag_terminate = true; 00202 //OLED3 = true; 00203 00204 while (true) { 00205 Thread::wait(osWaitForever); 00206 } 00207 } 00208 00209 // motion control thread ------------------------ 00210 void motion_thread(void const *argument) { 00211 motors.resetEncoders(); 00212 motors.setSpeed(MOVE_SPEED/2,MOVE_SPEED/2); 00213 Thread::wait(1000); 00214 motors.stop(); 00215 (*AI_Thread_Ptr).signal_set(0x01); 00216 00217 00218 00219 float currX, currY,currTheta; 00220 float speedL,speedR; 00221 float diffDir,diffSpeed; 00222 float lastdiffSpeed = 0; 00223 00224 while (1) { 00225 if (flag_terminate) { 00226 terminate(); 00227 } 00228 00229 // get kalman localization estimate ------------------------ 00230 kalman.statelock.lock(); 00231 currX = kalman.X(0)*1000.0f; 00232 currY = kalman.X(1)*1000.0f; 00233 currTheta = kalman.X(2); 00234 kalman.statelock.unlock(); 00235 00236 00237 // check if target reached ---------------------------------- 00238 if ( ( abs(currX - targetX) < POSITION_TOR ) 00239 &&( abs(currY - targetY) < POSITION_TOR ) 00240 ) { 00241 00242 diffDir = rectifyAng(currTheta - targetTheta); 00243 diffSpeed = diffDir / PI; 00244 00245 if (abs(diffDir) > ANGLE_TOR) { 00246 if (abs(diffSpeed) < 0.5) { 00247 diffSpeed = 0.5*diffSpeed/abs(diffSpeed); 00248 } 00249 motors.setSpeed( int(diffSpeed*MOVE_SPEED), -int(diffSpeed*MOVE_SPEED)); 00250 00251 00252 } else { 00253 motors.stop(); 00254 Thread::wait(4000); 00255 (*AI_Thread_Ptr).signal_set(0x01); 00256 } 00257 } 00258 00259 // adjust motion to reach target ---------------------------- 00260 else { 00261 00262 // calc direction to target 00263 float targetDir = atan2(targetY - currY, targetX - currX); 00264 00265 00266 diffDir = rectifyAng(currTheta - targetDir); 00267 diffSpeed = diffDir / PI; 00268 00269 if (abs(diffDir) > ANGLE_TOR*2) { 00270 if (abs(diffSpeed) < 0.5) { 00271 diffSpeed = 0.5*diffSpeed/abs(diffSpeed); 00272 } 00273 motors.setSpeed( int(diffSpeed*MOVE_SPEED), -int(diffSpeed*MOVE_SPEED)); 00274 } else { 00275 00276 00277 if (abs(diffSpeed-lastdiffSpeed) > MAX_STEP_RATIO ) { 00278 if (diffSpeed-lastdiffSpeed >= 0) { 00279 diffSpeed = lastdiffSpeed + MAX_STEP_RATIO; 00280 } else { 00281 diffSpeed = lastdiffSpeed - MAX_STEP_RATIO; 00282 } 00283 } 00284 lastdiffSpeed = diffSpeed; 00285 00286 // calculte the motor speeds 00287 if (diffSpeed <= 0) { 00288 speedL = (1-2*abs(diffSpeed))*MOVE_SPEED; 00289 speedR = MOVE_SPEED; 00290 00291 } else { 00292 speedR = (1-2*abs(diffSpeed))*MOVE_SPEED; 00293 speedL = MOVE_SPEED; 00294 } 00295 00296 motors.setSpeed( int(speedL), int(speedR)); 00297 } 00298 } 00299 00300 // wait 00301 Thread::wait(MOTION_UPDATE_PERIOD); 00302 } 00303 } 00304 00305 void vIRValueISR (void) { 00306 00307 OLED3 = !OLED3; 00308 // A workaround for mbed UART ISR bug 00309 // Clear the RBR flag to make sure the interrupt doesn't loop 00310 // UART3 for the port on pins 9/10, UART2 for pins 28/27, and UART1 for pins 13/14. 00311 // UART0 for USB UART 00312 unsigned char RBR = LPC_UART1->RBR; 00313 00314 00315 if (!data_flag) { // look for alignment bytes 00316 if (RBR == Alignment_char[Alignment_ptr]) { 00317 Alignment_ptr ++; 00318 } 00319 if (Alignment_ptr >= 4) { 00320 Alignment_ptr = 0; 00321 data_flag = true; // set the dataflag 00322 } 00323 } else { // fetch data bytes 00324 IRValues.IR_chars[buff_pointer] = RBR; 00325 buff_pointer ++; 00326 if (buff_pointer >= 12) { 00327 buff_pointer = 0; 00328 data_flag = false; // dessert the dataflag 00329 if (angleInit) { 00330 kalman.runupdate(Kalman::measurement_t(IRValues.IR_ints[0]+3),rectifyAng(IRValues.IR_floats[1]+angleOffset),IRValues.IR_floats[2]); 00331 } else { 00332 kalman.runupdate(Kalman::measurement_t(IRValues.IR_ints[0]+3),IRValues.IR_floats[1],IRValues.IR_floats[2]); 00333 } 00334 } 00335 00336 } 00337 } 00338 00339 void vKalmanInit(void) { 00340 float SonarMeasures[3]; 00341 float IRMeasures[3]; 00342 int beacon_cnt = 0; 00343 wait(1); 00344 IRturret.attach(NULL,Serial::RxIrq); 00345 kalman.statelock.lock(); 00346 SonarMeasures[0] = kalman.SonarMeasures[0]*1000.0f; 00347 SonarMeasures[1] = kalman.SonarMeasures[1]*1000.0f; 00348 SonarMeasures[2] = kalman.SonarMeasures[2]*1000.0f; 00349 IRMeasures[0] = kalman.IRMeasures[0]; 00350 IRMeasures[1] = kalman.IRMeasures[1]; 00351 IRMeasures[2] = kalman.IRMeasures[2]; 00352 kalman.statelock.unlock(); 00353 //printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasures[0]*180/PI, IRMeasures[1]*180/PI, IRMeasures[2]*180/PI); 00354 float d = beaconpos[2].y - beaconpos[1].y; 00355 float i = beaconpos[0].y - beaconpos[1].y; 00356 float j = beaconpos[0].x - beaconpos[1].x; 00357 float y_coor = (SonarMeasures[1]*SonarMeasures[1]- SonarMeasures[2]*SonarMeasures[2] + d*d) / (2*d); 00358 float x_coor = (SonarMeasures[1]*SonarMeasures[1] - SonarMeasures[0]*SonarMeasures[0] + i*i + j*j)/(2*j) - i*y_coor/j; 00359 angleOffset = 0; 00360 for (int i = 0; i < 3; i++) { 00361 float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor); 00362 if (IRMeasures[i] != 0){ 00363 beacon_cnt ++; 00364 float angle_temp = angle_est - IRMeasures[i]; 00365 angle_temp -= (floor(angle_temp/(2*PI)))*2*PI; 00366 angleOffset += angle_temp; 00367 } 00368 } 00369 angleOffset = angleOffset/float(beacon_cnt); 00370 //printf("\n\r"); 00371 angleInit = true; 00372 kalman.statelock.lock(); 00373 kalman.X(0) = x_coor/1000.0f; 00374 kalman.X(1) = y_coor/1000.0f; 00375 kalman.X(2) = 0; 00376 kalman.statelock.unlock(); 00377 //printf("x: %0.4f, y: %0.4f, offset: %0.4f \n\r", x_coor, y_coor, angleOffset*180/PI); 00378 IRturret.attach(&vIRValueISR,Serial::RxIrq); 00379 }
Generated on Sat Jul 16 2022 01:26:17 by 1.7.2