Eurobot2012_Primary
Dependencies: mbed Eurobot_2012_Primary
Diff: main.cpp
- Revision:
- 22:7ba09c0af0d0
- Parent:
- 21:15da49f18c63
- Child:
- 23:1901cb6d0d95
--- a/main.cpp Tue May 01 16:54:44 2012 +0000 +++ b/main.cpp Thu May 03 14:20:04 2012 +0000 @@ -16,18 +16,19 @@ //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); -//TODO mutex on kalman state, and on motor commands (i.e. on the i2c bus) - - void vMotorThread(void const *argument); void vPrintState(void const *argument); void motion_thread(void const *argument); +void vStop (void); //Main loop int main() { @@ -38,28 +39,28 @@ kalman.KalmanInit(); //Thread tMotorThread(vMotorThread,NULL,osPriorityNormal,256); - //Thread tUpdateState(vPrintState,NULL,osPriorityNormal,1024); - - + 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 - + // we use main loop to estimate the cpu usage + osThreadSetPriority (osThreadGetId(), osPriorityIdle); - + Timer timer; ui.regid(10, 1); - - while(1) { + + while (1) { timer.reset(); timer.start(); nopwait(1000); - + ui.updateval(10, timer.read_us()); } - + // do nothing //Thread::wait(osWaitForever); } @@ -82,44 +83,85 @@ flag_terminate = true; */ - - - // - // Put some code here so it's started by the pull trigger - // start a 90s timer here as well - // - //while (!Tiggered); - + + 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; - // strat 1 ================================== +#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){ + while (true) { Thread::wait(osWaitForever); } - - + + // end of strat 1 =========================== } @@ -152,7 +194,7 @@ float SonarMeasures[3]; float IRMeasures[3]; - + Thread::wait(5000); while (1) { kalman.statelock.lock(); state[0] = kalman.X(0); @@ -166,9 +208,18 @@ 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], state[1],state[2]); - pc.printf("Sonar: %0.4f %0.4f %0.4f \r\n",SonarMeasures[0],SonarMeasures[1],SonarMeasures[2]); + 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; +// }; +} \ No newline at end of file