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 #include "motion.h"
00011 #include "ai.h"
00012 #include "ui.h"
00013 #include "front_arms.h"
00014 #include "motion.h"
00015 
00016 //#include <iostream>
00017 
00018 //Interface declaration
00019 Serial pc(USBTX, USBRX); // tx, rx
00020 
00021 
00022 
00023 
00024 bool Colour = 1; // 1 for red, 0 for blue
00025 pos beaconpos[] = {{3000, 1000},{0,0}, {0,2000}}; //predefined red start
00026 
00027 DigitalIn StartTrig(p12);
00028 DigitalIn ColourToggle(p22); //high for red, low for blue(purple)
00029 Ticker StopTicker;
00030 
00031 Motors motors;
00032 UI ui;
00033 Kalman kalman(motors,ui,p23,p14,p14,p14,p15,p15,p15,p5,p6,p7,p8,p11);
00034 AI ai;
00035 Motion motion(motors, ai, kalman);
00036 
00037 void vMotorThread(void const *argument);
00038 void vPrintState(void const *argument);
00039 void motion_thread(void const *argument);
00040 void vStop (void);
00041 
00042 //Main loop
00043 int main() {
00044     AnalogIn ObsAvoidPin(p20);
00045     // no motor motions till we pull the trig
00046     ai.flag_motorStop = true;
00047     Colour = !(ObsAvoidPin > 0.5);
00048     OLED3 = Colour;
00049     //nopwait(1000);
00050     //Colour = ColourToggle;
00051     // re-defines beacon positions by the toggle switch
00052     kalman.statelock.lock();
00053     if (true) {
00054         beaconpos[0].x = 3000;
00055         beaconpos[0].y = 1000;
00056         beaconpos[1].x = 0;
00057         beaconpos[1].y = 0;
00058         beaconpos[2].x = 0;
00059         beaconpos[2].y = 2000;
00060         //beaconpos[] = {{3000, 1000},{0,0}, {0,2000}};
00061     } else {
00062         beaconpos[0].x = 0;
00063         beaconpos[0].y = 1000;
00064         beaconpos[1].x = 3000;
00065         beaconpos[1].y = 0;
00066         beaconpos[2].x = 3000;
00067         beaconpos[2].y = 2000;
00068         //beaconpos[] = {{0, 1000},{3000,0}, {3000,2000}};
00069     }
00070     kalman.statelock.unlock();
00071 
00072     pc.baud(115200);
00073     ArmsEnable();
00074     ArmsClose();
00075 
00076 
00077     //Init kalman, this should be done in the mid of the arena before the game starts
00078     kalman.KalmanInit();
00079 
00080     //Thread tMotorThread(vMotorThread,NULL,osPriorityNormal,256);
00081     Thread tUpdateState(vPrintState,NULL,osPriorityNormal,1024);
00082 
00083 
00084     pc.printf("We got to main! ;D\r\n");
00085     if (Colour)
00086         printf("I'm in Red \n\r");
00087     else
00088         printf("I'm in Blue \n\r");
00089 
00090     //REMEMBERT TO PUT PULL UP RESISTORS ON I2C!!!!!!!!!!!!!!
00091     while (1) {
00092         // we use main loop to estimate the cpu usage
00093 
00094         osThreadSetPriority (osThreadGetId(), osPriorityIdle);
00095 
00096         Timer timer;
00097         ui.regid(10, 1);
00098 
00099         while (1) {
00100             timer.reset();
00101             timer.start();
00102             nopwait(1000);
00103 
00104             ui.updateval(10, timer.read_us());
00105         }
00106 
00107         // do nothing
00108         //Thread::wait(osWaitForever);
00109     }
00110 }
00111 
00112 void AI::ai_thread () {
00113 
00114     motors.accelerationRegister = 1;
00115     /*
00116     //printf("aithreadstart\r\n");
00117     Thread::signal_wait(0x01);
00118     settarget(660, 400, PI/2, true);
00119 
00120     Thread::signal_wait(0x01);
00121     settarget(660, 570, PI, true);
00122 
00123     Thread::signal_wait(0x01);
00124     settarget(400, 870, PI, true);
00125 
00126     Thread::signal_wait(0x01);
00127     settarget(660, 870, PI, false);
00128 
00129     flag_terminate = true;
00130     */
00131 
00132     printf("Waiting for the trigger pull ....\r\n");
00133 
00134     // wait for the start triger
00135     while (!StartTrig) {
00136         Thread::wait(10);
00137     };
00138 
00139     printf("GO! \r\n");
00140     kalman.KalmanReset();
00141     Thread::wait(100);
00142 
00143 
00144 
00145     // attach a 87 seconds stop timer
00146 
00147 
00148     //REPLACE TICKER!!!!
00149     StopTicker.attach(&vStop, 87);
00150 
00151 
00152     // starts motors
00153     ai.flag_motorStop = false;
00154 
00155     // no override
00156     ai.flag_manOverride = false;
00157 
00158 
00159 //if (Colour){
00160     // strat 1 RED ==================================
00161     ArmsOpen();
00162     //Thread::wait(500);
00163 
00164     // goto middle x
00165     settarget(1500, 250, PI/2, true,Colour, 35);
00166     Thread::signal_wait(0x01);
00167     Thread::wait(2000);
00168 
00169     // to palm tree
00170     settarget(1500, 1000, PI, true,Colour, 35);
00171     Thread::signal_wait(0x01);
00172     Thread::wait(4000);
00173 
00174     // run over totem
00175     settarget(840,1000,PI, true,Colour, 80);
00176     motors.accelerationRegister = 0;
00177     Thread::wait(5000);
00178     //Thread::signal_wait(0x01);
00179 
00180     while (hypot(kalman.X(0) - 1.1f, kalman.X(1) - 1.0f) < 0.10) {
00181         // to palm tree
00182         settarget(1500, 1000, PI, false,Colour, 35);
00183         Thread::signal_wait(0x01);
00184         Thread::wait(4000);
00185 
00186         // run over totem
00187         settarget(840,1000,PI, true,Colour, 80);
00188         motors.accelerationRegister = 0;
00189         Thread::wait(5000);
00190     }
00191     Thread::signal_wait(0x01);
00192 
00193     motors.accelerationRegister = 1;
00194    
00195     // back to ship
00196     settarget(220,1000,0,true,Colour, 50);
00197     Thread::signal_wait(0x01);
00198     
00199     ArmsClose();
00200     
00201     settarget(840,1000,PI, true,Colour, 40);
00202     Thread::signal_wait(0x01);
00203     
00204      
00205      
00206      settarget(220,1000,PI,true,Colour, 40);
00207     Thread::signal_wait(0x01);
00208     
00209      settarget(840,1000,PI,false,Colour, 40);
00210     Thread::signal_wait(0x01);
00211     
00212    
00213 //}
00214 
00215     /*else{
00216         // strat 1 BLUE ==================================
00217         // goto middle x
00218         settarget(3000-1500, 250, PI/2, true);
00219         Thread::signal_wait(0x01);
00220         Thread::wait(2000);
00221 
00222         // to palm tree
00223         settarget(3000-1500, 1000, 0, true);
00224         Thread::signal_wait(0x01);
00225         Thread::wait(2000);
00226 
00227         // run over totem
00228         settarget(3000-640,1000,0, true);
00229         Thread::signal_wait(0x01);
00230         Thread::wait(2000);
00231 
00232         // back to ship
00233         settarget(3000-220,780,0,true);
00234         Thread::signal_wait(0x01);
00235         Thread::wait(2000);
00236     }
00237     */
00238     /*
00239     // going from ship to ship for the remaining secs
00240         while (true){
00241         // back to home, RED
00242         settarget(500,400,PI,true);
00243         Thread::signal_wait(0x01);
00244         Thread::wait(2000);
00245 
00246         // back to ship, BLUE
00247         settarget(500,1600,0,true);
00248         Thread::signal_wait(0x01);
00249         Thread::wait(2000);
00250         }
00251         */
00252 
00253     // terminate thread, stopps motors permanently
00254     ai.flag_terminate = true;
00255     while (true) {
00256         Thread::wait(osWaitForever);
00257     }
00258 
00259 
00260     // end of strat 1 ===========================
00261 }
00262 
00263 
00264 void vMotorThread(void const *argument) {
00265     motors.resetEncoders();
00266     while (1) {
00267         motors.setSpeed(20,20);
00268         Thread::wait(2000);
00269         motors.stop();
00270         Thread::wait(5000);
00271         motors.setSpeed(-20,-20);
00272         Thread::wait(2000);
00273         motors.stop();
00274         Thread::wait(5000);
00275         motors.setSpeed(-20,20);
00276         Thread::wait(2000);
00277         motors.stop();
00278         Thread::wait(5000);
00279         motors.setSpeed(20,-20);
00280         Thread::wait(2000);
00281         motors.stop();
00282         Thread::wait(5000);
00283     }
00284 }
00285 
00286 
00287 void vPrintState(void const *argument) {
00288     float state[3];
00289     float SonarMeasures[3];
00290     float IRMeasures[3];
00291 
00292     Thread::wait(5000);
00293     while (1) {
00294         kalman.statelock.lock();
00295         state[0] = kalman.X(0);
00296         state[1] = kalman.X(1);
00297         state[2] = kalman.X(2);
00298         SonarMeasures[0] = kalman.SonarMeasures[0];
00299         SonarMeasures[1] = kalman.SonarMeasures[1];
00300         SonarMeasures[2] = kalman.SonarMeasures[2];
00301         IRMeasures[0] = kalman.IRMeasures[0];
00302         IRMeasures[1] = kalman.IRMeasures[1];
00303         IRMeasures[2] = kalman.IRMeasures[2];
00304         kalman.statelock.unlock();
00305         pc.printf("\r\n");
00306         pc.printf("current: %0.4f %0.4f %0.4f \r\n", state[0]*1000, state[1]*1000,state[2]*180/PI);
00307         pc.printf("Sonar: %0.4f %0.4f %0.4f \r\n",SonarMeasures[0]*1000,SonarMeasures[1]*1000,SonarMeasures[2]*1000);
00308         pc.printf("IR   : %0.4f %0.4f %0.4f \r\n",IRMeasures[0]*180/PI,IRMeasures[1]*180/PI,IRMeasures[2]*180/PI);
00309         Thread::wait(100);
00310     }
00311 }
00312 
00313 void vStop (void) {
00314 //    while (true) {
00315     motors.coastStop();
00316     ai.flag_motorStop = true;
00317     // terminate thread, stopps motors permanently
00318     ai.flag_terminate = true;
00319 //    };
00320 }