Eurobot2012_Primary
Dependencies: mbed Eurobot_2012_Primary
main.cpp
- Committer:
- narshu
- Date:
- 2012-05-03
- Revision:
- 22:7ba09c0af0d0
- Parent:
- 21:15da49f18c63
- Child:
- 23:1901cb6d0d95
File content as of revision 22:7ba09c0af0d0:
#include "mbed.h" #include "rtos.h" #include "TSH.h" #include "Kalman.h" #include "globals.h" #include "motors.h" #include "math.h" #include "system.h" #include "geometryfuncs.h" #include "motion.h" #include "ai.h" #include "ui.h" //#include <iostream> //Interface declaration Serial pc(USBTX, USBRX); // tx, rx DigitalIn StartTrig(p12); Ticker StopTicker; Motors motors; UI ui; Kalman kalman(motors,ui,p23,p14,p14,p14,p15,p15,p15,p5,p6,p7,p8,p11); AI ai; Motion motion(motors, ai, kalman); void vMotorThread(void const *argument); void vPrintState(void const *argument); void motion_thread(void const *argument); void vStop (void); //Main loop int main() { pc.baud(115200); // no motor motions till we pull the trig ai.flag_motorStop = true; //Init kalman, this should be done in the mid of the arena before the game starts kalman.KalmanInit(); //Thread tMotorThread(vMotorThread,NULL,osPriorityNormal,256); Thread tUpdateState(vPrintState,NULL,osPriorityNormal,1024); pc.printf("We got to main! ;D\r\n"); //REMEMBERT TO PUT PULL UP RESISTORS ON I2C!!!!!!!!!!!!!! while (1) { // we use main loop to estimate the cpu usage osThreadSetPriority (osThreadGetId(), osPriorityIdle); Timer timer; ui.regid(10, 1); while (1) { timer.reset(); timer.start(); nopwait(1000); ui.updateval(10, timer.read_us()); } // do nothing //Thread::wait(osWaitForever); } } void AI::ai_thread () { /* //printf("aithreadstart\r\n"); Thread::signal_wait(0x01); settarget(660, 400, PI/2, true); Thread::signal_wait(0x01); settarget(660, 570, PI, true); Thread::signal_wait(0x01); settarget(400, 870, PI, true); Thread::signal_wait(0x01); settarget(660, 870, PI, false); flag_terminate = true; */ printf("Waiting for the trigger pull ....\r\n"); // wait for the start triger while (StartTrig) { Thread::wait(10); }; // attach a 90 seconds stop timer StopTicker.attach(&vStop, 90); // starts motors ai.flag_motorStop = false; #ifdef STARTLOC_RED // strat 1 RED ================================== // goto middle x settarget(1500, 250, PI/2, true); Thread::signal_wait(0x01); Thread::wait(2000); // to palm tree settarget(1500, 1000, PI, true); Thread::signal_wait(0x01); Thread::wait(2000); // run over totem settarget(640,1000,PI, true); Thread::signal_wait(0x01); Thread::wait(2000); // back to ship settarget(220,780,PI,true); Thread::signal_wait(0x01); Thread::wait(2000); #else // strat 1 BLUE ================================== // goto middle x settarget(3000-1500, 250, PI/2, true); Thread::signal_wait(0x01); Thread::wait(2000); // to palm tree settarget(3000-1500, 1000, 0, true); Thread::signal_wait(0x01); Thread::wait(2000); // run over totem settarget(3000-640,1000,0, true); Thread::signal_wait(0x01); Thread::wait(2000); // back to ship settarget(3000-220,780,0,true); Thread::signal_wait(0x01); Thread::wait(2000); #endif // going from ship to ship for the remaining secs while (true){ // back to ship, RED settarget(220,780,PI,true); Thread::signal_wait(0x01); Thread::wait(2000); // back to ship, BLUE settarget(3000-220,780,0,true); Thread::signal_wait(0x01); Thread::wait(2000); } // terminate thread, stopps motors permanently ai.flag_terminate = true; while (true) { Thread::wait(osWaitForever); } // end of strat 1 =========================== } void vMotorThread(void const *argument) { motors.resetEncoders(); while (1) { motors.setSpeed(20,20); Thread::wait(2000); motors.stop(); Thread::wait(5000); motors.setSpeed(-20,-20); Thread::wait(2000); motors.stop(); Thread::wait(5000); motors.setSpeed(-20,20); Thread::wait(2000); motors.stop(); Thread::wait(5000); motors.setSpeed(20,-20); Thread::wait(2000); motors.stop(); Thread::wait(5000); } } void vPrintState(void const *argument) { float state[3]; float SonarMeasures[3]; float IRMeasures[3]; Thread::wait(5000); while (1) { kalman.statelock.lock(); state[0] = kalman.X(0); state[1] = kalman.X(1); state[2] = kalman.X(2); SonarMeasures[0] = kalman.SonarMeasures[0]; SonarMeasures[1] = kalman.SonarMeasures[1]; SonarMeasures[2] = kalman.SonarMeasures[2]; IRMeasures[0] = kalman.IRMeasures[0]; IRMeasures[1] = kalman.IRMeasures[1]; IRMeasures[2] = kalman.IRMeasures[2]; kalman.statelock.unlock(); pc.printf("\r\n"); pc.printf("current: %0.4f %0.4f %0.4f \r\n", state[0]*1000, state[1]*1000,state[2]*180/PI); pc.printf("Sonar: %0.4f %0.4f %0.4f \r\n",SonarMeasures[0]*1000,SonarMeasures[1]*1000,SonarMeasures[2]*1000); pc.printf("IR : %0.4f %0.4f %0.4f \r\n",IRMeasures[0]*180/PI,IRMeasures[1]*180/PI,IRMeasures[2]*180/PI); Thread::wait(100); } } void vStop (void) { // while (true) { motors.stop(); ai.flag_motorStop = true; // terminate thread, stopps motors permanently ai.flag_terminate = true; // }; }