Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

main.cpp@26:0995f61cb7b8, 2012-10-17 (annotated)

Committer:
narshu
Date:
Wed Oct 17 22:22:47 2012 +0000
Revision:
26:0995f61cb7b8
Parent:
25:143b19c1fb05 
Eurobot 2012 Primary;

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