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 }