Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

main.cpp@23:1901cb6d0d95, 2012-05-04 (annotated)

Committer:
narshu
Date:
Fri May 04 02:50:07 2012 +0000
Revision:
23:1901cb6d0d95
Parent:
22:7ba09c0af0d0
Child:
24:7a3906c2f5d5
working commit, on red

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 23:1901cb6d0d95 13 #include "front_arms.h"
narshu 0:f3bf6c7e2283 14
narshu 0:f3bf6c7e2283 15 //#include <iostream>
narshu 0:f3bf6c7e2283 16
narshu 0:f3bf6c7e2283 17 //Interface declaration
narshu 0:f3bf6c7e2283 18 Serial pc(USBTX, USBRX); // tx, rx
narshu 0:f3bf6c7e2283 19
narshu 23:1901cb6d0d95 20 bool Colour = 1; // 1 for red, 0 for blue
narshu 23:1901cb6d0d95 21 pos beaconpos[] = {{3000, 1000},{0,0}, {0,2000}}; //predefined red start
narshu 23:1901cb6d0d95 22
narshu 22:7ba09c0af0d0 23 DigitalIn StartTrig(p12);
narshu 23:1901cb6d0d95 24 DigitalIn ColourToggle(p16); //high for red, low for blue(purple)
narshu 22:7ba09c0af0d0 25 Ticker StopTicker;
narshu 22:7ba09c0af0d0 26
narshu 0:f3bf6c7e2283 27 Motors motors;
narshu 2:cffa347bb943 28 UI ui;
narshu 4:7b7334441da9 29 Kalman kalman(motors,ui,p23,p14,p14,p14,p15,p15,p15,p5,p6,p7,p8,p11);
narshu 2:cffa347bb943 30 AI ai;
narshu 10:294b9adbc9d3 31 Motion motion(motors, ai, kalman);
narshu 1:bbabbd997d21 32
narshu 0:f3bf6c7e2283 33 void vMotorThread(void const *argument);
narshu 0:f3bf6c7e2283 34 void vPrintState(void const *argument);
narshu 0:f3bf6c7e2283 35 void motion_thread(void const *argument);
narshu 22:7ba09c0af0d0 36 void vStop (void);
narshu 1:bbabbd997d21 37
narshu 0:f3bf6c7e2283 38 //Main loop
narshu 0:f3bf6c7e2283 39 int main() {
narshu 21:15da49f18c63 40 // no motor motions till we pull the trig
narshu 21:15da49f18c63 41 ai.flag_motorStop = true;
narshu 23:1901cb6d0d95 42
narshu 23:1901cb6d0d95 43 Colour = ColourToggle;
narshu 23:1901cb6d0d95 44 // re-defines beacon positions by the toggle switch
narshu 23:1901cb6d0d95 45 kalman.statelock.lock();
narshu 23:1901cb6d0d95 46 if (Colour) {
narshu 23:1901cb6d0d95 47 beaconpos[0].x = 3000;
narshu 23:1901cb6d0d95 48 beaconpos[0].y = 1000;
narshu 23:1901cb6d0d95 49 beaconpos[1].x = 0;
narshu 23:1901cb6d0d95 50 beaconpos[1].y = 0;
narshu 23:1901cb6d0d95 51 beaconpos[2].x = 0;
narshu 23:1901cb6d0d95 52 beaconpos[2].y = 2000;
narshu 23:1901cb6d0d95 53 //beaconpos[] = {{3000, 1000},{0,0}, {0,2000}};
narshu 23:1901cb6d0d95 54 } else {
narshu 23:1901cb6d0d95 55 beaconpos[0].x = 0;
narshu 23:1901cb6d0d95 56 beaconpos[0].y = 1000;
narshu 23:1901cb6d0d95 57 beaconpos[1].x = 3000;
narshu 23:1901cb6d0d95 58 beaconpos[1].y = 0;
narshu 23:1901cb6d0d95 59 beaconpos[2].x = 3000;
narshu 23:1901cb6d0d95 60 beaconpos[2].y = 2000;
narshu 23:1901cb6d0d95 61 //beaconpos[] = {{0, 1000},{3000,0}, {3000,2000}};
narshu 23:1901cb6d0d95 62 }
narshu 23:1901cb6d0d95 63 kalman.statelock.unlock();
narshu 23:1901cb6d0d95 64
narshu 23:1901cb6d0d95 65 pc.baud(115200);
narshu 23:1901cb6d0d95 66 ArmsEnable();
narshu 23:1901cb6d0d95 67 ArmsClose();
narshu 23:1901cb6d0d95 68
narshu 23:1901cb6d0d95 69
narshu 21:15da49f18c63 70 //Init kalman, this should be done in the mid of the arena before the game starts
narshu 2:cffa347bb943 71 kalman.KalmanInit();
narshu 1:bbabbd997d21 72
narshu 9:377560539b74 73 //Thread tMotorThread(vMotorThread,NULL,osPriorityNormal,256);
narshu 23:1901cb6d0d95 74 //Thread tUpdateState(vPrintState,NULL,osPriorityNormal,1024);
narshu 22:7ba09c0af0d0 75
narshu 22:7ba09c0af0d0 76
narshu 0:f3bf6c7e2283 77 pc.printf("We got to main! ;D\r\n");
narshu 23:1901cb6d0d95 78 if (Colour)
narshu 23:1901cb6d0d95 79 printf("I'm in Red \n\r");
narshu 23:1901cb6d0d95 80 else
narshu 23:1901cb6d0d95 81 printf("I'm in Blue \n\r");
narshu 0:f3bf6c7e2283 82
narshu 0:f3bf6c7e2283 83 //REMEMBERT TO PUT PULL UP RESISTORS ON I2C!!!!!!!!!!!!!!
narshu 0:f3bf6c7e2283 84 while (1) {
narshu 22:7ba09c0af0d0 85 // we use main loop to estimate the cpu usage
narshu 22:7ba09c0af0d0 86
narshu 9:377560539b74 87 osThreadSetPriority (osThreadGetId(), osPriorityIdle);
narshu 22:7ba09c0af0d0 88
narshu 9:377560539b74 89 Timer timer;
narshu 9:377560539b74 90 ui.regid(10, 1);
narshu 22:7ba09c0af0d0 91
narshu 22:7ba09c0af0d0 92 while (1) {
narshu 9:377560539b74 93 timer.reset();
narshu 9:377560539b74 94 timer.start();
narshu 9:377560539b74 95 nopwait(1000);
narshu 22:7ba09c0af0d0 96
narshu 9:377560539b74 97 ui.updateval(10, timer.read_us());
narshu 9:377560539b74 98 }
narshu 22:7ba09c0af0d0 99
narshu 0:f3bf6c7e2283 100 // do nothing
narshu 10:294b9adbc9d3 101 //Thread::wait(osWaitForever);
narshu 0:f3bf6c7e2283 102 }
narshu 0:f3bf6c7e2283 103 }
narshu 0:f3bf6c7e2283 104
narshu 9:377560539b74 105 void AI::ai_thread () {
narshu 9:377560539b74 106 /*
narshu 9:377560539b74 107 //printf("aithreadstart\r\n");
narshu 9:377560539b74 108 Thread::signal_wait(0x01);
narshu 9:377560539b74 109 settarget(660, 400, PI/2, true);
narshu 9:377560539b74 110
narshu 9:377560539b74 111 Thread::signal_wait(0x01);
narshu 9:377560539b74 112 settarget(660, 570, PI, true);
narshu 9:377560539b74 113
narshu 9:377560539b74 114 Thread::signal_wait(0x01);
narshu 9:377560539b74 115 settarget(400, 870, PI, true);
narshu 9:377560539b74 116
narshu 9:377560539b74 117 Thread::signal_wait(0x01);
narshu 9:377560539b74 118 settarget(660, 870, PI, false);
narshu 9:377560539b74 119
narshu 9:377560539b74 120 flag_terminate = true;
narshu 9:377560539b74 121 */
narshu 22:7ba09c0af0d0 122
narshu 22:7ba09c0af0d0 123 printf("Waiting for the trigger pull ....\r\n");
narshu 22:7ba09c0af0d0 124
narshu 22:7ba09c0af0d0 125 // wait for the start triger
narshu 23:1901cb6d0d95 126 while (!StartTrig) {
narshu 22:7ba09c0af0d0 127 Thread::wait(10);
narshu 22:7ba09c0af0d0 128 };
narshu 23:1901cb6d0d95 129
narshu 23:1901cb6d0d95 130 printf("GO! \r\n");
narshu 23:1901cb6d0d95 131 kalman.KalmanReset();
narshu 23:1901cb6d0d95 132 Thread::wait(100);
narshu 23:1901cb6d0d95 133
narshu 23:1901cb6d0d95 134
narshu 22:7ba09c0af0d0 135
narshu 23:1901cb6d0d95 136 // attach a 87 seconds stop timer
narshu 23:1901cb6d0d95 137 StopTicker.attach(&vStop, 87);
narshu 22:7ba09c0af0d0 138
narshu 22:7ba09c0af0d0 139
narshu 21:15da49f18c63 140 // starts motors
narshu 21:15da49f18c63 141 ai.flag_motorStop = false;
narshu 23:1901cb6d0d95 142
narshu 23:1901cb6d0d95 143 // no override
narshu 23:1901cb6d0d95 144 ai.flag_manOverride = false;
narshu 23:1901cb6d0d95 145
narshu 23:1901cb6d0d95 146
narshu 23:1901cb6d0d95 147 //if (Colour){
narshu 22:7ba09c0af0d0 148 // strat 1 RED ==================================
narshu 23:1901cb6d0d95 149 ArmsOpen();
narshu 23:1901cb6d0d95 150 Thread::wait(500);
narshu 23:1901cb6d0d95 151
narshu 19:06610e1c0895 152 // goto middle x
narshu 23:1901cb6d0d95 153 settarget(1500, 250, PI/2, true,Colour);
narshu 19:06610e1c0895 154 Thread::signal_wait(0x01);
narshu 19:06610e1c0895 155 Thread::wait(2000);
narshu 22:7ba09c0af0d0 156
narshu 23:1901cb6d0d95 157
narshu 19:06610e1c0895 158 // to palm tree
narshu 23:1901cb6d0d95 159 settarget(1500, 1050, PI, true,Colour);
narshu 19:06610e1c0895 160 Thread::signal_wait(0x01);
narshu 19:06610e1c0895 161 Thread::wait(2000);
narshu 22:7ba09c0af0d0 162
narshu 19:06610e1c0895 163 // run over totem
narshu 23:1901cb6d0d95 164 settarget(640,1050,PI, true,Colour);
narshu 19:06610e1c0895 165 Thread::signal_wait(0x01);
narshu 19:06610e1c0895 166 Thread::wait(2000);
narshu 22:7ba09c0af0d0 167
narshu 23:1901cb6d0d95 168 ArmsClose();
narshu 19:06610e1c0895 169 // back to ship
narshu 23:1901cb6d0d95 170 settarget(220,780,PI,true,Colour);
narshu 19:06610e1c0895 171 Thread::signal_wait(0x01);
narshu 19:06610e1c0895 172 Thread::wait(2000);
narshu 23:1901cb6d0d95 173 //}
narshu 22:7ba09c0af0d0 174
narshu 23:1901cb6d0d95 175 /*else{
narshu 22:7ba09c0af0d0 176 // strat 1 BLUE ==================================
narshu 22:7ba09c0af0d0 177 // goto middle x
narshu 22:7ba09c0af0d0 178 settarget(3000-1500, 250, PI/2, true);
narshu 22:7ba09c0af0d0 179 Thread::signal_wait(0x01);
narshu 22:7ba09c0af0d0 180 Thread::wait(2000);
narshu 22:7ba09c0af0d0 181
narshu 22:7ba09c0af0d0 182 // to palm tree
narshu 22:7ba09c0af0d0 183 settarget(3000-1500, 1000, 0, true);
narshu 22:7ba09c0af0d0 184 Thread::signal_wait(0x01);
narshu 22:7ba09c0af0d0 185 Thread::wait(2000);
narshu 22:7ba09c0af0d0 186
narshu 22:7ba09c0af0d0 187 // run over totem
narshu 22:7ba09c0af0d0 188 settarget(3000-640,1000,0, true);
narshu 22:7ba09c0af0d0 189 Thread::signal_wait(0x01);
narshu 22:7ba09c0af0d0 190 Thread::wait(2000);
narshu 22:7ba09c0af0d0 191
narshu 22:7ba09c0af0d0 192 // back to ship
narshu 22:7ba09c0af0d0 193 settarget(3000-220,780,0,true);
narshu 22:7ba09c0af0d0 194 Thread::signal_wait(0x01);
narshu 23:1901cb6d0d95 195 Thread::wait(2000);
narshu 23:1901cb6d0d95 196 }
narshu 23:1901cb6d0d95 197 */
narshu 22:7ba09c0af0d0 198 // going from ship to ship for the remaining secs
narshu 22:7ba09c0af0d0 199 while (true){
narshu 23:1901cb6d0d95 200 // back to home, RED
narshu 23:1901cb6d0d95 201 settarget(500,400,PI,true);
narshu 22:7ba09c0af0d0 202 Thread::signal_wait(0x01);
narshu 22:7ba09c0af0d0 203 Thread::wait(2000);
narshu 19:06610e1c0895 204
narshu 22:7ba09c0af0d0 205 // back to ship, BLUE
narshu 23:1901cb6d0d95 206 settarget(500,1600,0,true);
narshu 22:7ba09c0af0d0 207 Thread::signal_wait(0x01);
narshu 22:7ba09c0af0d0 208 Thread::wait(2000);
narshu 22:7ba09c0af0d0 209 }
narshu 22:7ba09c0af0d0 210
narshu 21:15da49f18c63 211 // terminate thread, stopps motors permanently
narshu 21:15da49f18c63 212 ai.flag_terminate = true;
narshu 22:7ba09c0af0d0 213 while (true) {
narshu 19:06610e1c0895 214 Thread::wait(osWaitForever);
narshu 19:06610e1c0895 215 }
narshu 22:7ba09c0af0d0 216
narshu 22:7ba09c0af0d0 217
narshu 19:06610e1c0895 218 // end of strat 1 ===========================
narshu 9:377560539b74 219 }
narshu 9:377560539b74 220
narshu 0:f3bf6c7e2283 221
narshu 0:f3bf6c7e2283 222 void vMotorThread(void const *argument) {
narshu 0:f3bf6c7e2283 223 motors.resetEncoders();
narshu 0:f3bf6c7e2283 224 while (1) {
narshu 1:bbabbd997d21 225 motors.setSpeed(20,20);
narshu 1:bbabbd997d21 226 Thread::wait(2000);
narshu 1:bbabbd997d21 227 motors.stop();
narshu 1:bbabbd997d21 228 Thread::wait(5000);
narshu 1:bbabbd997d21 229 motors.setSpeed(-20,-20);
narshu 0:f3bf6c7e2283 230 Thread::wait(2000);
narshu 0:f3bf6c7e2283 231 motors.stop();
narshu 0:f3bf6c7e2283 232 Thread::wait(5000);
narshu 0:f3bf6c7e2283 233 motors.setSpeed(-20,20);
narshu 0:f3bf6c7e2283 234 Thread::wait(2000);
narshu 0:f3bf6c7e2283 235 motors.stop();
narshu 0:f3bf6c7e2283 236 Thread::wait(5000);
narshu 1:bbabbd997d21 237 motors.setSpeed(20,-20);
narshu 1:bbabbd997d21 238 Thread::wait(2000);
narshu 1:bbabbd997d21 239 motors.stop();
narshu 1:bbabbd997d21 240 Thread::wait(5000);
narshu 0:f3bf6c7e2283 241 }
narshu 0:f3bf6c7e2283 242 }
narshu 0:f3bf6c7e2283 243
narshu 0:f3bf6c7e2283 244
narshu 0:f3bf6c7e2283 245 void vPrintState(void const *argument) {
narshu 0:f3bf6c7e2283 246 float state[3];
narshu 1:bbabbd997d21 247 float SonarMeasures[3];
narshu 1:bbabbd997d21 248 float IRMeasures[3];
narshu 1:bbabbd997d21 249
narshu 22:7ba09c0af0d0 250 Thread::wait(5000);
narshu 0:f3bf6c7e2283 251 while (1) {
narshu 0:f3bf6c7e2283 252 kalman.statelock.lock();
narshu 0:f3bf6c7e2283 253 state[0] = kalman.X(0);
narshu 0:f3bf6c7e2283 254 state[1] = kalman.X(1);
narshu 0:f3bf6c7e2283 255 state[2] = kalman.X(2);
narshu 1:bbabbd997d21 256 SonarMeasures[0] = kalman.SonarMeasures[0];
narshu 1:bbabbd997d21 257 SonarMeasures[1] = kalman.SonarMeasures[1];
narshu 1:bbabbd997d21 258 SonarMeasures[2] = kalman.SonarMeasures[2];
narshu 1:bbabbd997d21 259 IRMeasures[0] = kalman.IRMeasures[0];
narshu 1:bbabbd997d21 260 IRMeasures[1] = kalman.IRMeasures[1];
narshu 1:bbabbd997d21 261 IRMeasures[2] = kalman.IRMeasures[2];
narshu 0:f3bf6c7e2283 262 kalman.statelock.unlock();
narshu 0:f3bf6c7e2283 263 pc.printf("\r\n");
narshu 22:7ba09c0af0d0 264 pc.printf("current: %0.4f %0.4f %0.4f \r\n", state[0]*1000, state[1]*1000,state[2]*180/PI);
narshu 22:7ba09c0af0d0 265 pc.printf("Sonar: %0.4f %0.4f %0.4f \r\n",SonarMeasures[0]*1000,SonarMeasures[1]*1000,SonarMeasures[2]*1000);
narshu 1:bbabbd997d21 266 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 267 Thread::wait(100);
narshu 0:f3bf6c7e2283 268 }
narshu 0:f3bf6c7e2283 269 }
narshu 22:7ba09c0af0d0 270
narshu 22:7ba09c0af0d0 271 void vStop (void) {
narshu 22:7ba09c0af0d0 272 // while (true) {
narshu 23:1901cb6d0d95 273 motors.coastStop();
narshu 22:7ba09c0af0d0 274 ai.flag_motorStop = true;
narshu 22:7ba09c0af0d0 275 // terminate thread, stopps motors permanently
narshu 22:7ba09c0af0d0 276 ai.flag_terminate = true;
narshu 22:7ba09c0af0d0 277 // };
narshu 22:7ba09c0af0d0 278 }