Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

main.cpp@22:7ba09c0af0d0, 2012-05-03 (annotated)

Committer:
narshu
Date:
Thu May 03 14:20:04 2012 +0000
Revision:
22:7ba09c0af0d0
Parent:
21:15da49f18c63
Child:
23:1901cb6d0d95
added 90sec timer and tigger

Who changed what in which revision?

UserRevisionLine numberNew contents of line
narshu 0:f3bf6c7e2283 1 #include "mbed.h"
narshu 0:f3bf6c7e2283 2 #include "rtos.h"
narshu 0:f3bf6c7e2283 3 #include "TSH.h"
narshu 0:f3bf6c7e2283 4 #include "Kalman.h"
narshu 0:f3bf6c7e2283 5 #include "globals.h"
narshu 0:f3bf6c7e2283 6 #include "motors.h"
narshu 0:f3bf6c7e2283 7 #include "math.h"
narshu 0:f3bf6c7e2283 8 #include "system.h"
narshu 1:bbabbd997d21 9 #include "geometryfuncs.h"
narshu 10:294b9adbc9d3 10 #include "motion.h"
narshu 2:cffa347bb943 11 #include "ai.h"
narshu 2:cffa347bb943 12 #include "ui.h"
narshu 0:f3bf6c7e2283 13
narshu 0:f3bf6c7e2283 14 //#include <iostream>
narshu 0:f3bf6c7e2283 15
narshu 0:f3bf6c7e2283 16 //Interface declaration
narshu 0:f3bf6c7e2283 17 Serial pc(USBTX, USBRX); // tx, rx
narshu 0:f3bf6c7e2283 18
narshu 22:7ba09c0af0d0 19 DigitalIn StartTrig(p12);
narshu 22:7ba09c0af0d0 20 Ticker StopTicker;
narshu 22:7ba09c0af0d0 21
narshu 0:f3bf6c7e2283 22 Motors motors;
narshu 2:cffa347bb943 23 UI ui;
narshu 4:7b7334441da9 24 Kalman kalman(motors,ui,p23,p14,p14,p14,p15,p15,p15,p5,p6,p7,p8,p11);
narshu 2:cffa347bb943 25 AI ai;
narshu 10:294b9adbc9d3 26 Motion motion(motors, ai, kalman);
narshu 1:bbabbd997d21 27
narshu 0:f3bf6c7e2283 28 void vMotorThread(void const *argument);
narshu 0:f3bf6c7e2283 29 void vPrintState(void const *argument);
narshu 0:f3bf6c7e2283 30 void motion_thread(void const *argument);
narshu 22:7ba09c0af0d0 31 void vStop (void);
narshu 1:bbabbd997d21 32
narshu 0:f3bf6c7e2283 33 //Main loop
narshu 0:f3bf6c7e2283 34 int main() {
narshu 0:f3bf6c7e2283 35 pc.baud(115200);
narshu 21:15da49f18c63 36 // no motor motions till we pull the trig
narshu 21:15da49f18c63 37 ai.flag_motorStop = true;
narshu 21:15da49f18c63 38 //Init kalman, this should be done in the mid of the arena before the game starts
narshu 2:cffa347bb943 39 kalman.KalmanInit();
narshu 1:bbabbd997d21 40
narshu 9:377560539b74 41 //Thread tMotorThread(vMotorThread,NULL,osPriorityNormal,256);
narshu 22:7ba09c0af0d0 42 Thread tUpdateState(vPrintState,NULL,osPriorityNormal,1024);
narshu 22:7ba09c0af0d0 43
narshu 22:7ba09c0af0d0 44
narshu 0:f3bf6c7e2283 45 pc.printf("We got to main! ;D\r\n");
narshu 0:f3bf6c7e2283 46
narshu 0:f3bf6c7e2283 47 //REMEMBERT TO PUT PULL UP RESISTORS ON I2C!!!!!!!!!!!!!!
narshu 0:f3bf6c7e2283 48 while (1) {
narshu 22:7ba09c0af0d0 49 // we use main loop to estimate the cpu usage
narshu 22:7ba09c0af0d0 50
narshu 9:377560539b74 51 osThreadSetPriority (osThreadGetId(), osPriorityIdle);
narshu 22:7ba09c0af0d0 52
narshu 9:377560539b74 53 Timer timer;
narshu 9:377560539b74 54 ui.regid(10, 1);
narshu 22:7ba09c0af0d0 55
narshu 22:7ba09c0af0d0 56 while (1) {
narshu 9:377560539b74 57 timer.reset();
narshu 9:377560539b74 58 timer.start();
narshu 9:377560539b74 59 nopwait(1000);
narshu 22:7ba09c0af0d0 60
narshu 9:377560539b74 61 ui.updateval(10, timer.read_us());
narshu 9:377560539b74 62 }
narshu 22:7ba09c0af0d0 63
narshu 0:f3bf6c7e2283 64 // do nothing
narshu 10:294b9adbc9d3 65 //Thread::wait(osWaitForever);
narshu 0:f3bf6c7e2283 66 }
narshu 0:f3bf6c7e2283 67 }
narshu 0:f3bf6c7e2283 68
narshu 9:377560539b74 69 void AI::ai_thread () {
narshu 9:377560539b74 70 /*
narshu 9:377560539b74 71 //printf("aithreadstart\r\n");
narshu 9:377560539b74 72 Thread::signal_wait(0x01);
narshu 9:377560539b74 73 settarget(660, 400, PI/2, true);
narshu 9:377560539b74 74
narshu 9:377560539b74 75 Thread::signal_wait(0x01);
narshu 9:377560539b74 76 settarget(660, 570, PI, true);
narshu 9:377560539b74 77
narshu 9:377560539b74 78 Thread::signal_wait(0x01);
narshu 9:377560539b74 79 settarget(400, 870, PI, true);
narshu 9:377560539b74 80
narshu 9:377560539b74 81 Thread::signal_wait(0x01);
narshu 9:377560539b74 82 settarget(660, 870, PI, false);
narshu 9:377560539b74 83
narshu 9:377560539b74 84 flag_terminate = true;
narshu 9:377560539b74 85 */
narshu 22:7ba09c0af0d0 86
narshu 22:7ba09c0af0d0 87 printf("Waiting for the trigger pull ....\r\n");
narshu 22:7ba09c0af0d0 88
narshu 22:7ba09c0af0d0 89 // wait for the start triger
narshu 22:7ba09c0af0d0 90 while (StartTrig) {
narshu 22:7ba09c0af0d0 91 Thread::wait(10);
narshu 22:7ba09c0af0d0 92 };
narshu 22:7ba09c0af0d0 93
narshu 22:7ba09c0af0d0 94 // attach a 90 seconds stop timer
narshu 22:7ba09c0af0d0 95 StopTicker.attach(&vStop, 90);
narshu 22:7ba09c0af0d0 96
narshu 22:7ba09c0af0d0 97
narshu 21:15da49f18c63 98 // starts motors
narshu 21:15da49f18c63 99 ai.flag_motorStop = false;
narshu 22:7ba09c0af0d0 100 #ifdef STARTLOC_RED
narshu 22:7ba09c0af0d0 101 // strat 1 RED ==================================
narshu 19:06610e1c0895 102 // goto middle x
narshu 19:06610e1c0895 103 settarget(1500, 250, PI/2, true);
narshu 19:06610e1c0895 104 Thread::signal_wait(0x01);
narshu 19:06610e1c0895 105 Thread::wait(2000);
narshu 22:7ba09c0af0d0 106
narshu 19:06610e1c0895 107 // to palm tree
narshu 19:06610e1c0895 108 settarget(1500, 1000, PI, true);
narshu 19:06610e1c0895 109 Thread::signal_wait(0x01);
narshu 19:06610e1c0895 110 Thread::wait(2000);
narshu 22:7ba09c0af0d0 111
narshu 19:06610e1c0895 112 // run over totem
narshu 19:06610e1c0895 113 settarget(640,1000,PI, true);
narshu 19:06610e1c0895 114 Thread::signal_wait(0x01);
narshu 19:06610e1c0895 115 Thread::wait(2000);
narshu 22:7ba09c0af0d0 116
narshu 19:06610e1c0895 117 // back to ship
narshu 19:06610e1c0895 118 settarget(220,780,PI,true);
narshu 19:06610e1c0895 119 Thread::signal_wait(0x01);
narshu 19:06610e1c0895 120 Thread::wait(2000);
narshu 22:7ba09c0af0d0 121
narshu 22:7ba09c0af0d0 122 #else
narshu 22:7ba09c0af0d0 123 // strat 1 BLUE ==================================
narshu 22:7ba09c0af0d0 124 // goto middle x
narshu 22:7ba09c0af0d0 125 settarget(3000-1500, 250, PI/2, true);
narshu 22:7ba09c0af0d0 126 Thread::signal_wait(0x01);
narshu 22:7ba09c0af0d0 127 Thread::wait(2000);
narshu 22:7ba09c0af0d0 128
narshu 22:7ba09c0af0d0 129 // to palm tree
narshu 22:7ba09c0af0d0 130 settarget(3000-1500, 1000, 0, true);
narshu 22:7ba09c0af0d0 131 Thread::signal_wait(0x01);
narshu 22:7ba09c0af0d0 132 Thread::wait(2000);
narshu 22:7ba09c0af0d0 133
narshu 22:7ba09c0af0d0 134 // run over totem
narshu 22:7ba09c0af0d0 135 settarget(3000-640,1000,0, true);
narshu 22:7ba09c0af0d0 136 Thread::signal_wait(0x01);
narshu 22:7ba09c0af0d0 137 Thread::wait(2000);
narshu 22:7ba09c0af0d0 138
narshu 22:7ba09c0af0d0 139 // back to ship
narshu 22:7ba09c0af0d0 140 settarget(3000-220,780,0,true);
narshu 22:7ba09c0af0d0 141 Thread::signal_wait(0x01);
narshu 22:7ba09c0af0d0 142 Thread::wait(2000);
narshu 22:7ba09c0af0d0 143 #endif
narshu 22:7ba09c0af0d0 144
narshu 22:7ba09c0af0d0 145 // going from ship to ship for the remaining secs
narshu 22:7ba09c0af0d0 146 while (true){
narshu 22:7ba09c0af0d0 147 // back to ship, RED
narshu 22:7ba09c0af0d0 148 settarget(220,780,PI,true);
narshu 22:7ba09c0af0d0 149 Thread::signal_wait(0x01);
narshu 22:7ba09c0af0d0 150 Thread::wait(2000);
narshu 19:06610e1c0895 151
narshu 22:7ba09c0af0d0 152 // back to ship, BLUE
narshu 22:7ba09c0af0d0 153 settarget(3000-220,780,0,true);
narshu 22:7ba09c0af0d0 154 Thread::signal_wait(0x01);
narshu 22:7ba09c0af0d0 155 Thread::wait(2000);
narshu 22:7ba09c0af0d0 156 }
narshu 22:7ba09c0af0d0 157
narshu 21:15da49f18c63 158 // terminate thread, stopps motors permanently
narshu 21:15da49f18c63 159 ai.flag_terminate = true;
narshu 22:7ba09c0af0d0 160 while (true) {
narshu 19:06610e1c0895 161 Thread::wait(osWaitForever);
narshu 19:06610e1c0895 162 }
narshu 22:7ba09c0af0d0 163
narshu 22:7ba09c0af0d0 164
narshu 19:06610e1c0895 165 // end of strat 1 ===========================
narshu 9:377560539b74 166 }
narshu 9:377560539b74 167
narshu 0:f3bf6c7e2283 168
narshu 0:f3bf6c7e2283 169 void vMotorThread(void const *argument) {
narshu 0:f3bf6c7e2283 170 motors.resetEncoders();
narshu 0:f3bf6c7e2283 171 while (1) {
narshu 1:bbabbd997d21 172 motors.setSpeed(20,20);
narshu 1:bbabbd997d21 173 Thread::wait(2000);
narshu 1:bbabbd997d21 174 motors.stop();
narshu 1:bbabbd997d21 175 Thread::wait(5000);
narshu 1:bbabbd997d21 176 motors.setSpeed(-20,-20);
narshu 0:f3bf6c7e2283 177 Thread::wait(2000);
narshu 0:f3bf6c7e2283 178 motors.stop();
narshu 0:f3bf6c7e2283 179 Thread::wait(5000);
narshu 0:f3bf6c7e2283 180 motors.setSpeed(-20,20);
narshu 0:f3bf6c7e2283 181 Thread::wait(2000);
narshu 0:f3bf6c7e2283 182 motors.stop();
narshu 0:f3bf6c7e2283 183 Thread::wait(5000);
narshu 1:bbabbd997d21 184 motors.setSpeed(20,-20);
narshu 1:bbabbd997d21 185 Thread::wait(2000);
narshu 1:bbabbd997d21 186 motors.stop();
narshu 1:bbabbd997d21 187 Thread::wait(5000);
narshu 0:f3bf6c7e2283 188 }
narshu 0:f3bf6c7e2283 189 }
narshu 0:f3bf6c7e2283 190
narshu 0:f3bf6c7e2283 191
narshu 0:f3bf6c7e2283 192 void vPrintState(void const *argument) {
narshu 0:f3bf6c7e2283 193 float state[3];
narshu 1:bbabbd997d21 194 float SonarMeasures[3];
narshu 1:bbabbd997d21 195 float IRMeasures[3];
narshu 1:bbabbd997d21 196
narshu 22:7ba09c0af0d0 197 Thread::wait(5000);
narshu 0:f3bf6c7e2283 198 while (1) {
narshu 0:f3bf6c7e2283 199 kalman.statelock.lock();
narshu 0:f3bf6c7e2283 200 state[0] = kalman.X(0);
narshu 0:f3bf6c7e2283 201 state[1] = kalman.X(1);
narshu 0:f3bf6c7e2283 202 state[2] = kalman.X(2);
narshu 1:bbabbd997d21 203 SonarMeasures[0] = kalman.SonarMeasures[0];
narshu 1:bbabbd997d21 204 SonarMeasures[1] = kalman.SonarMeasures[1];
narshu 1:bbabbd997d21 205 SonarMeasures[2] = kalman.SonarMeasures[2];
narshu 1:bbabbd997d21 206 IRMeasures[0] = kalman.IRMeasures[0];
narshu 1:bbabbd997d21 207 IRMeasures[1] = kalman.IRMeasures[1];
narshu 1:bbabbd997d21 208 IRMeasures[2] = kalman.IRMeasures[2];
narshu 0:f3bf6c7e2283 209 kalman.statelock.unlock();
narshu 0:f3bf6c7e2283 210 pc.printf("\r\n");
narshu 22:7ba09c0af0d0 211 pc.printf("current: %0.4f %0.4f %0.4f \r\n", state[0]*1000, state[1]*1000,state[2]*180/PI);
narshu 22:7ba09c0af0d0 212 pc.printf("Sonar: %0.4f %0.4f %0.4f \r\n",SonarMeasures[0]*1000,SonarMeasures[1]*1000,SonarMeasures[2]*1000);
narshu 1:bbabbd997d21 213 pc.printf("IR : %0.4f %0.4f %0.4f \r\n",IRMeasures[0]*180/PI,IRMeasures[1]*180/PI,IRMeasures[2]*180/PI);
narshu 1:bbabbd997d21 214 Thread::wait(100);
narshu 0:f3bf6c7e2283 215 }
narshu 0:f3bf6c7e2283 216 }
narshu 22:7ba09c0af0d0 217
narshu 22:7ba09c0af0d0 218 void vStop (void) {
narshu 22:7ba09c0af0d0 219 // while (true) {
narshu 22:7ba09c0af0d0 220 motors.stop();
narshu 22:7ba09c0af0d0 221 ai.flag_motorStop = true;
narshu 22:7ba09c0af0d0 222 // terminate thread, stopps motors permanently
narshu 22:7ba09c0af0d0 223 ai.flag_terminate = true;
narshu 22:7ba09c0af0d0 224 // };
narshu 22:7ba09c0af0d0 225 }