Shuto Naruse / Mbed 2 deprecated Eurobot2012_Secondary

Eurobot2012_Secondary

Fork of Eurobot_2012_Secondary by Shuto Naruse

main.cpp@1:cc2a9eb0bd55, 2012-10-17 (annotated)

Committer:
narshu
Date:
Wed Oct 17 22:25:31 2012 +0000
Revision:
1:cc2a9eb0bd55
Parent:
0:fbfafa6bf5f9 
Commit before publishing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
narshu 0:fbfafa6bf5f9 1 #include "mbed.h"
narshu 0:fbfafa6bf5f9 2 #include "rtos.h"
narshu 0:fbfafa6bf5f9 3 #include "TSH.h"
narshu 0:fbfafa6bf5f9 4 #include "Kalman.h"
narshu 0:fbfafa6bf5f9 5 #include "globals.h"
narshu 0:fbfafa6bf5f9 6 #include "motors.h"
narshu 0:fbfafa6bf5f9 7 #include "math.h"
narshu 0:fbfafa6bf5f9 8 #include "system.h"
narshu 0:fbfafa6bf5f9 9 #include "geometryfuncs.h"
narshu 1:cc2a9eb0bd55 10 #include "motion.h"
narshu 1:cc2a9eb0bd55 11 #include "ai.h"
narshu 1:cc2a9eb0bd55 12 #include "ui.h"
narshu 0:fbfafa6bf5f9 13
narshu 0:fbfafa6bf5f9 14 //#include <iostream>
narshu 0:fbfafa6bf5f9 15
narshu 0:fbfafa6bf5f9 16 //Interface declaration
narshu 0:fbfafa6bf5f9 17 Serial pc(USBTX, USBRX); // tx, rx
narshu 0:fbfafa6bf5f9 18
narshu 1:cc2a9eb0bd55 19 bool Colour = 1; // 1 for red, 0 for blue
narshu 1:cc2a9eb0bd55 20 pos beaconpos[] = {{3000, 1000},{0,0}, {0,2000}}; //predefined red start
narshu 0:fbfafa6bf5f9 21
narshu 1:cc2a9eb0bd55 22 DigitalIn StartTrig(p11);
narshu 1:cc2a9eb0bd55 23 DigitalIn ColourToggle(p19); //high for red, low for blue(purple)
narshu 1:cc2a9eb0bd55 24 Ticker StopTicker;
narshu 0:fbfafa6bf5f9 25
narshu 1:cc2a9eb0bd55 26 TSI2C i2c(p28, p27);
narshu 1:cc2a9eb0bd55 27 Motors motors(i2c);
narshu 1:cc2a9eb0bd55 28 UI ui;
narshu 1:cc2a9eb0bd55 29 Kalman kalman(motors,ui,p10,p21,p22,p23,p24,p25,p26,p5,p6,p7,p8,p9);
narshu 1:cc2a9eb0bd55 30 AI ai;
narshu 1:cc2a9eb0bd55 31 Motion motion(motors, ai, kalman);
narshu 0:fbfafa6bf5f9 32
narshu 0:fbfafa6bf5f9 33
narshu 0:fbfafa6bf5f9 34 void vMotorThread(void const *argument);
narshu 0:fbfafa6bf5f9 35 void vPrintState(void const *argument);
narshu 0:fbfafa6bf5f9 36 void motion_thread(void const *argument);
narshu 1:cc2a9eb0bd55 37 void vStop (void);
narshu 0:fbfafa6bf5f9 38
narshu 1:cc2a9eb0bd55 39 //bool flag_terminate = false;
narshu 0:fbfafa6bf5f9 40
narshu 0:fbfafa6bf5f9 41 float temp = 0;
narshu 0:fbfafa6bf5f9 42
narshu 0:fbfafa6bf5f9 43 //Main loop
narshu 0:fbfafa6bf5f9 44 int main() {
narshu 1:cc2a9eb0bd55 45 ai.flag_manOverride = true;
narshu 1:cc2a9eb0bd55 46 // no motor motions till we pull the trig
narshu 1:cc2a9eb0bd55 47 ai.flag_motorStop = true;
narshu 1:cc2a9eb0bd55 48
narshu 1:cc2a9eb0bd55 49 Colour = ColourToggle;
narshu 1:cc2a9eb0bd55 50 OLED3 = Colour;
narshu 1:cc2a9eb0bd55 51 // re-defines beacon positions by the toggle switch
narshu 1:cc2a9eb0bd55 52 kalman.statelock.lock();
narshu 1:cc2a9eb0bd55 53 if (true) {
narshu 1:cc2a9eb0bd55 54 beaconpos[0].x = 3000;
narshu 1:cc2a9eb0bd55 55 beaconpos[0].y = 1000;
narshu 1:cc2a9eb0bd55 56 beaconpos[1].x = 0;
narshu 1:cc2a9eb0bd55 57 beaconpos[1].y = 0;
narshu 1:cc2a9eb0bd55 58 beaconpos[2].x = 0;
narshu 1:cc2a9eb0bd55 59 beaconpos[2].y = 2000;
narshu 1:cc2a9eb0bd55 60 //beaconpos[] = {{3000, 1000},{0,0}, {0,2000}};
narshu 1:cc2a9eb0bd55 61 } else {
narshu 1:cc2a9eb0bd55 62 beaconpos[0].x = 0;
narshu 1:cc2a9eb0bd55 63 beaconpos[0].y = 1000;
narshu 1:cc2a9eb0bd55 64 beaconpos[1].x = 3000;
narshu 1:cc2a9eb0bd55 65 beaconpos[1].y = 0;
narshu 1:cc2a9eb0bd55 66 beaconpos[2].x = 3000;
narshu 1:cc2a9eb0bd55 67 beaconpos[2].y = 2000;
narshu 1:cc2a9eb0bd55 68 //beaconpos[] = {{0, 1000},{3000,0}, {3000,2000}};
narshu 1:cc2a9eb0bd55 69 }
narshu 1:cc2a9eb0bd55 70 kalman.statelock.unlock();
narshu 0:fbfafa6bf5f9 71 pc.baud(115200);
narshu 1:cc2a9eb0bd55 72
narshu 1:cc2a9eb0bd55 73
narshu 1:cc2a9eb0bd55 74 //Init kalman, this should be done in the mid of the arena before the game starts
narshu 1:cc2a9eb0bd55 75 kalman.KalmanInit();
narshu 0:fbfafa6bf5f9 76
narshu 0:fbfafa6bf5f9 77 //Thread tMotorThread(vMotorThread,NULL,osPriorityNormal,256);
narshu 1:cc2a9eb0bd55 78 //Thread tUpdateState(vPrintState,NULL,osPriorityNormal,1024);
narshu 0:fbfafa6bf5f9 79
narshu 0:fbfafa6bf5f9 80 //measure cpu usage. output updated once per second to symbol cpupercent
narshu 0:fbfafa6bf5f9 81 //Thread mCPUthread(measureCPUidle, NULL, osPriorityIdle, 1024); //check if stack overflow with such a small staack
narshu 0:fbfafa6bf5f9 82
narshu 1:cc2a9eb0bd55 83 pc.printf("We got to main! ;D\r\n");
narshu 0:fbfafa6bf5f9 84
narshu 1:cc2a9eb0bd55 85 if (Colour)
narshu 1:cc2a9eb0bd55 86 printf("I'm in Red \n\r");
narshu 1:cc2a9eb0bd55 87 else
narshu 1:cc2a9eb0bd55 88 printf("I'm in Blue \n\r");
narshu 0:fbfafa6bf5f9 89
narshu 0:fbfafa6bf5f9 90 //REMEMBERT TO PUT PULL UP RESISTORS ON I2C!!!!!!!!!!!!!!
narshu 0:fbfafa6bf5f9 91 while (1) {
narshu 1:cc2a9eb0bd55 92
narshu 1:cc2a9eb0bd55 93 osThreadSetPriority (osThreadGetId(), osPriorityIdle);
narshu 1:cc2a9eb0bd55 94
narshu 1:cc2a9eb0bd55 95 Timer timer;
narshu 1:cc2a9eb0bd55 96 ui.regid(10, 1);
narshu 1:cc2a9eb0bd55 97
narshu 1:cc2a9eb0bd55 98 while (1) {
narshu 1:cc2a9eb0bd55 99 timer.reset();
narshu 1:cc2a9eb0bd55 100 timer.start();
narshu 1:cc2a9eb0bd55 101 nopwait(1000);
narshu 1:cc2a9eb0bd55 102
narshu 1:cc2a9eb0bd55 103 ui.updateval(10, timer.read_us());
narshu 1:cc2a9eb0bd55 104 }
narshu 1:cc2a9eb0bd55 105
narshu 0:fbfafa6bf5f9 106 // do nothing
narshu 1:cc2a9eb0bd55 107 //Thread::wait(osWaitForever);
narshu 1:cc2a9eb0bd55 108 }
narshu 1:cc2a9eb0bd55 109 }
narshu 1:cc2a9eb0bd55 110
narshu 1:cc2a9eb0bd55 111 void AI::ai_thread () {
narshu 1:cc2a9eb0bd55 112 //Thread::wait(1000);
narshu 1:cc2a9eb0bd55 113
narshu 1:cc2a9eb0bd55 114 printf("Waiting for the trigger pull ....\r\n");
narshu 1:cc2a9eb0bd55 115
narshu 1:cc2a9eb0bd55 116 // wait for the start triger
narshu 1:cc2a9eb0bd55 117 while (!StartTrig) {
narshu 1:cc2a9eb0bd55 118 Thread::wait(10);
narshu 1:cc2a9eb0bd55 119 };
narshu 1:cc2a9eb0bd55 120
narshu 1:cc2a9eb0bd55 121
narshu 1:cc2a9eb0bd55 122 //printf("GO! \r\n");
narshu 1:cc2a9eb0bd55 123 kalman.KalmanReset();
narshu 1:cc2a9eb0bd55 124 Thread::wait(100);
narshu 1:cc2a9eb0bd55 125
narshu 1:cc2a9eb0bd55 126
narshu 1:cc2a9eb0bd55 127 // attach a 87 seconds stop timer
narshu 1:cc2a9eb0bd55 128 StopTicker.attach(&vStop, 87);
narshu 1:cc2a9eb0bd55 129
narshu 1:cc2a9eb0bd55 130
narshu 1:cc2a9eb0bd55 131
narshu 1:cc2a9eb0bd55 132
narshu 1:cc2a9eb0bd55 133 // starts motors
narshu 1:cc2a9eb0bd55 134 ai.flag_motorStop = false;
narshu 1:cc2a9eb0bd55 135
narshu 1:cc2a9eb0bd55 136 // ai.flag_manOverride = true;
narshu 1:cc2a9eb0bd55 137 // motors.setSpeed(20);
narshu 1:cc2a9eb0bd55 138 // Thread::wait(5000);
narshu 1:cc2a9eb0bd55 139 ai.flag_manOverride = false;
narshu 1:cc2a9eb0bd55 140
narshu 1:cc2a9eb0bd55 141
narshu 1:cc2a9eb0bd55 142
narshu 1:cc2a9eb0bd55 143 // if (Colour) {
narshu 1:cc2a9eb0bd55 144 // strat 1 RED ==================================
narshu 1:cc2a9eb0bd55 145
narshu 1:cc2a9eb0bd55 146 // run out to black line
narshu 1:cc2a9eb0bd55 147 settarget(640,300,PI/2,true,Colour, MOVE_SPEED);
narshu 1:cc2a9eb0bd55 148 Thread::signal_wait(0x01);
narshu 1:cc2a9eb0bd55 149
narshu 1:cc2a9eb0bd55 150 /*
narshu 1:cc2a9eb0bd55 151 //prep to move block
narshu 1:cc2a9eb0bd55 152 settarget(640, 860,PI,true,Colour, MOVE_SPEED);
narshu 1:cc2a9eb0bd55 153 Thread::signal_wait(0x01);
narshu 1:cc2a9eb0bd55 154
narshu 1:cc2a9eb0bd55 155 //move block to ship
narshu 1:cc2a9eb0bd55 156 settarget(173, 860,PI,true,Colour, MOVE_SPEED);
narshu 1:cc2a9eb0bd55 157 Thread::signal_wait(0x01);
narshu 1:cc2a9eb0bd55 158
narshu 1:cc2a9eb0bd55 159 //back out
narshu 1:cc2a9eb0bd55 160 settarget(640,860,PI/2,false,Colour, MOVE_SPEED);
narshu 1:cc2a9eb0bd55 161 Thread::signal_wait(0x01);
narshu 1:cc2a9eb0bd55 162 */
narshu 1:cc2a9eb0bd55 163
narshu 1:cc2a9eb0bd55 164 // intermediate
narshu 1:cc2a9eb0bd55 165 settarget(640,1600,-PI/2,true,Colour, MOVE_SPEED);
narshu 1:cc2a9eb0bd55 166 Thread::signal_wait(0x01);
narshu 1:cc2a9eb0bd55 167
narshu 1:cc2a9eb0bd55 168 //push first button
narshu 1:cc2a9eb0bd55 169 settarget(640,2200,-PI/2,false,Colour, MOVE_SPEED);
narshu 1:cc2a9eb0bd55 170 //Thread::signal_wait(0x01,10000);
narshu 1:cc2a9eb0bd55 171 Thread::wait(5000);
narshu 1:cc2a9eb0bd55 172
narshu 1:cc2a9eb0bd55 173
narshu 1:cc2a9eb0bd55 174 //back out from button
narshu 1:cc2a9eb0bd55 175 settarget(640,1500,-PI/2,true,Colour, MOVE_SPEED);
narshu 1:cc2a9eb0bd55 176 Thread::signal_wait(0x01);
narshu 1:cc2a9eb0bd55 177
narshu 1:cc2a9eb0bd55 178 // go get middle bullion
narshu 1:cc2a9eb0bd55 179 settarget(1500,1320,0,true,Colour, MOVE_SPEED);
narshu 1:cc2a9eb0bd55 180 Thread::signal_wait(0x01);
narshu 1:cc2a9eb0bd55 181
narshu 1:cc2a9eb0bd55 182
narshu 1:cc2a9eb0bd55 183 // go to second button
narshu 1:cc2a9eb0bd55 184 settarget(1920,1700,-PI/2,true,Colour, MOVE_SPEED);
narshu 1:cc2a9eb0bd55 185 Thread::signal_wait(0x01);
narshu 1:cc2a9eb0bd55 186
narshu 1:cc2a9eb0bd55 187
narshu 1:cc2a9eb0bd55 188 // push button
narshu 1:cc2a9eb0bd55 189 settarget(1920,2200,-PI/2,false,Colour, MOVE_SPEED);
narshu 1:cc2a9eb0bd55 190 Thread::wait(5000);
narshu 1:cc2a9eb0bd55 191
narshu 1:cc2a9eb0bd55 192 // back off
narshu 1:cc2a9eb0bd55 193 settarget(1920,1700,-PI/2,true,Colour, MOVE_SPEED);
narshu 1:cc2a9eb0bd55 194 Thread::signal_wait(0x01);
narshu 1:cc2a9eb0bd55 195
narshu 1:cc2a9eb0bd55 196 // go somewhere
narshu 1:cc2a9eb0bd55 197 settarget(2400,1000,-PI/2,true,Colour, MOVE_SPEED);
narshu 1:cc2a9eb0bd55 198 Thread::signal_wait(0x01);
narshu 1:cc2a9eb0bd55 199
narshu 1:cc2a9eb0bd55 200 //go to opponents captain area
narshu 1:cc2a9eb0bd55 201 settarget(2200,280,PI,true,Colour, MOVE_SPEED);
narshu 1:cc2a9eb0bd55 202 Thread::signal_wait(0x01);
narshu 1:cc2a9eb0bd55 203
narshu 1:cc2a9eb0bd55 204 //back to own captain area
narshu 1:cc2a9eb0bd55 205 settarget(260,275,PI,true,Colour, MOVE_SPEED);
narshu 1:cc2a9eb0bd55 206 Thread::signal_wait(0x01);
narshu 1:cc2a9eb0bd55 207
narshu 1:cc2a9eb0bd55 208
narshu 1:cc2a9eb0bd55 209
narshu 1:cc2a9eb0bd55 210 //back to black line
narshu 1:cc2a9eb0bd55 211 settarget(640,300,0,false,Colour, MOVE_SPEED);
narshu 1:cc2a9eb0bd55 212 Thread::signal_wait(0x01);
narshu 1:cc2a9eb0bd55 213
narshu 1:cc2a9eb0bd55 214
narshu 1:cc2a9eb0bd55 215 //grab CD
narshu 1:cc2a9eb0bd55 216 settarget(1000,525,PI,true,Colour, MOVE_SPEED);
narshu 1:cc2a9eb0bd55 217 Thread::signal_wait(0x01);
narshu 1:cc2a9eb0bd55 218
narshu 1:cc2a9eb0bd55 219
narshu 1:cc2a9eb0bd55 220 //back to own captain area
narshu 1:cc2a9eb0bd55 221 settarget(260,275,PI,true,Colour, MOVE_SPEED);
narshu 1:cc2a9eb0bd55 222 Thread::signal_wait(0x01);
narshu 1:cc2a9eb0bd55 223
narshu 1:cc2a9eb0bd55 224
narshu 1:cc2a9eb0bd55 225
narshu 1:cc2a9eb0bd55 226 //to opponent's totem
narshu 1:cc2a9eb0bd55 227 settarget(1750,850,0,false,Colour, MOVE_SPEED);
narshu 1:cc2a9eb0bd55 228 Thread::signal_wait(0x01);
narshu 1:cc2a9eb0bd55 229
narshu 1:cc2a9eb0bd55 230
narshu 1:cc2a9eb0bd55 231
narshu 1:cc2a9eb0bd55 232 //collect their CDs
narshu 1:cc2a9eb0bd55 233 settarget(2000,750,PI,true,Colour, MOVE_SPEED);
narshu 1:cc2a9eb0bd55 234 Thread::signal_wait(0x01);
narshu 1:cc2a9eb0bd55 235
narshu 1:cc2a9eb0bd55 236
narshu 1:cc2a9eb0bd55 237
narshu 1:cc2a9eb0bd55 238 //collect more
narshu 1:cc2a9eb0bd55 239 settarget(2190,1155,0,true,Colour, MOVE_SPEED);
narshu 1:cc2a9eb0bd55 240 Thread::signal_wait(0x01);
narshu 1:cc2a9eb0bd55 241
narshu 1:cc2a9eb0bd55 242 //go to opponents captain area
narshu 1:cc2a9eb0bd55 243 settarget(2200,280,PI,true,Colour, MOVE_SPEED);
narshu 1:cc2a9eb0bd55 244 Thread::signal_wait(0x01);
narshu 1:cc2a9eb0bd55 245
narshu 1:cc2a9eb0bd55 246
narshu 1:cc2a9eb0bd55 247
narshu 1:cc2a9eb0bd55 248 //go back to ship
narshu 1:cc2a9eb0bd55 249 settarget(210,900,PI,true,Colour, MOVE_SPEED);
narshu 1:cc2a9eb0bd55 250 Thread::signal_wait(0x01);
narshu 1:cc2a9eb0bd55 251
narshu 1:cc2a9eb0bd55 252
narshu 1:cc2a9eb0bd55 253
narshu 1:cc2a9eb0bd55 254 //END
narshu 1:cc2a9eb0bd55 255
narshu 1:cc2a9eb0bd55 256 // terminate thread, stopps motors permanently
narshu 1:cc2a9eb0bd55 257 ai.flag_terminate = true;
narshu 1:cc2a9eb0bd55 258 while (true) {
narshu 0:fbfafa6bf5f9 259 Thread::wait(osWaitForever);
narshu 0:fbfafa6bf5f9 260 }
narshu 1:cc2a9eb0bd55 261
narshu 1:cc2a9eb0bd55 262
narshu 1:cc2a9eb0bd55 263 // end of strat 1 ===========================
narshu 0:fbfafa6bf5f9 264 }
narshu 0:fbfafa6bf5f9 265
narshu 0:fbfafa6bf5f9 266
narshu 0:fbfafa6bf5f9 267 void vMotorThread(void const *argument) {
narshu 0:fbfafa6bf5f9 268 motors.resetEncoders();
narshu 0:fbfafa6bf5f9 269 while (1) {
narshu 0:fbfafa6bf5f9 270 motors.setSpeed(20,20);
narshu 0:fbfafa6bf5f9 271 Thread::wait(2000);
narshu 0:fbfafa6bf5f9 272 motors.stop();
narshu 0:fbfafa6bf5f9 273 Thread::wait(5000);
narshu 0:fbfafa6bf5f9 274 motors.setSpeed(-20,-20);
narshu 0:fbfafa6bf5f9 275 Thread::wait(2000);
narshu 0:fbfafa6bf5f9 276 motors.stop();
narshu 0:fbfafa6bf5f9 277 Thread::wait(5000);
narshu 0:fbfafa6bf5f9 278 motors.setSpeed(-20,20);
narshu 0:fbfafa6bf5f9 279 Thread::wait(2000);
narshu 0:fbfafa6bf5f9 280 motors.stop();
narshu 0:fbfafa6bf5f9 281 Thread::wait(5000);
narshu 0:fbfafa6bf5f9 282 motors.setSpeed(20,-20);
narshu 0:fbfafa6bf5f9 283 Thread::wait(2000);
narshu 0:fbfafa6bf5f9 284 motors.stop();
narshu 0:fbfafa6bf5f9 285 Thread::wait(5000);
narshu 0:fbfafa6bf5f9 286 }
narshu 0:fbfafa6bf5f9 287 }
narshu 0:fbfafa6bf5f9 288
narshu 0:fbfafa6bf5f9 289
narshu 0:fbfafa6bf5f9 290 void vPrintState(void const *argument) {
narshu 0:fbfafa6bf5f9 291 float state[3];
narshu 0:fbfafa6bf5f9 292 float SonarMeasures[3];
narshu 0:fbfafa6bf5f9 293 float IRMeasures[3];
narshu 0:fbfafa6bf5f9 294
narshu 0:fbfafa6bf5f9 295
narshu 0:fbfafa6bf5f9 296 while (1) {
narshu 0:fbfafa6bf5f9 297 kalman.statelock.lock();
narshu 0:fbfafa6bf5f9 298 state[0] = kalman.X(0);
narshu 0:fbfafa6bf5f9 299 state[1] = kalman.X(1);
narshu 0:fbfafa6bf5f9 300 state[2] = kalman.X(2);
narshu 0:fbfafa6bf5f9 301 SonarMeasures[0] = kalman.SonarMeasures[0];
narshu 0:fbfafa6bf5f9 302 SonarMeasures[1] = kalman.SonarMeasures[1];
narshu 0:fbfafa6bf5f9 303 SonarMeasures[2] = kalman.SonarMeasures[2];
narshu 0:fbfafa6bf5f9 304 IRMeasures[0] = kalman.IRMeasures[0];
narshu 0:fbfafa6bf5f9 305 IRMeasures[1] = kalman.IRMeasures[1];
narshu 0:fbfafa6bf5f9 306 IRMeasures[2] = kalman.IRMeasures[2];
narshu 0:fbfafa6bf5f9 307 kalman.statelock.unlock();
narshu 0:fbfafa6bf5f9 308 pc.printf("\r\n");
narshu 1:cc2a9eb0bd55 309 pc.printf("current: %0.4f %0.4f %0.4f \r\n", state[0], state[1],state[2]*180/PI);
narshu 0:fbfafa6bf5f9 310 pc.printf("Sonar: %0.4f %0.4f %0.4f \r\n",SonarMeasures[0],SonarMeasures[1],SonarMeasures[2]);
narshu 0:fbfafa6bf5f9 311 pc.printf("IR : %0.4f %0.4f %0.4f \r\n",IRMeasures[0]*180/PI,IRMeasures[1]*180/PI,IRMeasures[2]*180/PI);
narshu 0:fbfafa6bf5f9 312 Thread::wait(100);
narshu 0:fbfafa6bf5f9 313 }
narshu 0:fbfafa6bf5f9 314 }
narshu 0:fbfafa6bf5f9 315
narshu 1:cc2a9eb0bd55 316 void vStop (void) {
narshu 1:cc2a9eb0bd55 317 // while (true) {
narshu 1:cc2a9eb0bd55 318 motors.coastStop();
narshu 1:cc2a9eb0bd55 319 ai.flag_motorStop = true;
narshu 1:cc2a9eb0bd55 320 // terminate thread, stopps motors permanently
narshu 1:cc2a9eb0bd55 321 ai.flag_terminate = true;
narshu 1:cc2a9eb0bd55 322 // };
narshu 0:fbfafa6bf5f9 323 }