Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

main.cpp@24:7a3906c2f5d5, 2012-05-04 (annotated)

Committer:
narshu
Date:
Fri May 04 05:23:45 2012 +0000
Revision:
24:7a3906c2f5d5
Parent:
23:1901cb6d0d95
Child:
25:143b19c1fb05
1st working version with accurate target acquisition.

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 24:7a3906c2f5d5 39 int main() {
narshu 21:15da49f18c63 40 // no motor motions till we pull the trig
narshu 21:15da49f18c63 41 ai.flag_motorStop = true;
narshu 24:7a3906c2f5d5 42 nopwait(1000);
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 24:7a3906c2f5d5 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 24:7a3906c2f5d5 137
narshu 24:7a3906c2f5d5 138
narshu 24:7a3906c2f5d5 139 //REPLACE TICKER!!!!
narshu 23:1901cb6d0d95 140 StopTicker.attach(&vStop, 87);
narshu 22:7ba09c0af0d0 141
narshu 22:7ba09c0af0d0 142
narshu 21:15da49f18c63 143 // starts motors
narshu 21:15da49f18c63 144 ai.flag_motorStop = false;
narshu 23:1901cb6d0d95 145
narshu 23:1901cb6d0d95 146 // no override
narshu 23:1901cb6d0d95 147 ai.flag_manOverride = false;
narshu 23:1901cb6d0d95 148
narshu 23:1901cb6d0d95 149
narshu 23:1901cb6d0d95 150 //if (Colour){
narshu 22:7ba09c0af0d0 151 // strat 1 RED ==================================
narshu 23:1901cb6d0d95 152 ArmsOpen();
narshu 24:7a3906c2f5d5 153 //Thread::wait(500);
narshu 24:7a3906c2f5d5 154
narshu 19:06610e1c0895 155 // goto middle x
narshu 24:7a3906c2f5d5 156 settarget(1500, 250, PI/2, true,Colour, 35);
narshu 19:06610e1c0895 157 Thread::signal_wait(0x01);
narshu 19:06610e1c0895 158 Thread::wait(2000);
narshu 24:7a3906c2f5d5 159
narshu 19:06610e1c0895 160 // to palm tree
narshu 24:7a3906c2f5d5 161 settarget(1500, 1000, PI, true,Colour, 35);
narshu 19:06610e1c0895 162 Thread::signal_wait(0x01);
narshu 19:06610e1c0895 163 Thread::wait(2000);
narshu 22:7ba09c0af0d0 164
narshu 19:06610e1c0895 165 // run over totem
narshu 24:7a3906c2f5d5 166 settarget(800,1050,PI, true,Colour, 60);
narshu 19:06610e1c0895 167 Thread::signal_wait(0x01);
narshu 19:06610e1c0895 168 Thread::wait(2000);
narshu 22:7ba09c0af0d0 169
narshu 23:1901cb6d0d95 170 ArmsClose();
narshu 19:06610e1c0895 171 // back to ship
narshu 24:7a3906c2f5d5 172 settarget(220,780,PI,true,Colour, 35);
narshu 19:06610e1c0895 173 Thread::signal_wait(0x01);
narshu 19:06610e1c0895 174 Thread::wait(2000);
narshu 23:1901cb6d0d95 175 //}
narshu 22:7ba09c0af0d0 176
narshu 23:1901cb6d0d95 177 /*else{
narshu 22:7ba09c0af0d0 178 // strat 1 BLUE ==================================
narshu 22:7ba09c0af0d0 179 // goto middle x
narshu 22:7ba09c0af0d0 180 settarget(3000-1500, 250, PI/2, true);
narshu 22:7ba09c0af0d0 181 Thread::signal_wait(0x01);
narshu 22:7ba09c0af0d0 182 Thread::wait(2000);
narshu 22:7ba09c0af0d0 183
narshu 22:7ba09c0af0d0 184 // to palm tree
narshu 22:7ba09c0af0d0 185 settarget(3000-1500, 1000, 0, true);
narshu 22:7ba09c0af0d0 186 Thread::signal_wait(0x01);
narshu 22:7ba09c0af0d0 187 Thread::wait(2000);
narshu 22:7ba09c0af0d0 188
narshu 22:7ba09c0af0d0 189 // run over totem
narshu 22:7ba09c0af0d0 190 settarget(3000-640,1000,0, true);
narshu 22:7ba09c0af0d0 191 Thread::signal_wait(0x01);
narshu 22:7ba09c0af0d0 192 Thread::wait(2000);
narshu 22:7ba09c0af0d0 193
narshu 22:7ba09c0af0d0 194 // back to ship
narshu 22:7ba09c0af0d0 195 settarget(3000-220,780,0,true);
narshu 22:7ba09c0af0d0 196 Thread::signal_wait(0x01);
narshu 23:1901cb6d0d95 197 Thread::wait(2000);
narshu 23:1901cb6d0d95 198 }
narshu 23:1901cb6d0d95 199 */
narshu 24:7a3906c2f5d5 200 /*
narshu 22:7ba09c0af0d0 201 // going from ship to ship for the remaining secs
narshu 22:7ba09c0af0d0 202 while (true){
narshu 23:1901cb6d0d95 203 // back to home, RED
narshu 23:1901cb6d0d95 204 settarget(500,400,PI,true);
narshu 22:7ba09c0af0d0 205 Thread::signal_wait(0x01);
narshu 22:7ba09c0af0d0 206 Thread::wait(2000);
narshu 19:06610e1c0895 207
narshu 22:7ba09c0af0d0 208 // back to ship, BLUE
narshu 23:1901cb6d0d95 209 settarget(500,1600,0,true);
narshu 22:7ba09c0af0d0 210 Thread::signal_wait(0x01);
narshu 22:7ba09c0af0d0 211 Thread::wait(2000);
narshu 22:7ba09c0af0d0 212 }
narshu 24:7a3906c2f5d5 213 */
narshu 22:7ba09c0af0d0 214
narshu 21:15da49f18c63 215 // terminate thread, stopps motors permanently
narshu 21:15da49f18c63 216 ai.flag_terminate = true;
narshu 22:7ba09c0af0d0 217 while (true) {
narshu 19:06610e1c0895 218 Thread::wait(osWaitForever);
narshu 19:06610e1c0895 219 }
narshu 22:7ba09c0af0d0 220
narshu 22:7ba09c0af0d0 221
narshu 19:06610e1c0895 222 // end of strat 1 ===========================
narshu 9:377560539b74 223 }
narshu 9:377560539b74 224
narshu 0:f3bf6c7e2283 225
narshu 0:f3bf6c7e2283 226 void vMotorThread(void const *argument) {
narshu 0:f3bf6c7e2283 227 motors.resetEncoders();
narshu 0:f3bf6c7e2283 228 while (1) {
narshu 1:bbabbd997d21 229 motors.setSpeed(20,20);
narshu 1:bbabbd997d21 230 Thread::wait(2000);
narshu 1:bbabbd997d21 231 motors.stop();
narshu 1:bbabbd997d21 232 Thread::wait(5000);
narshu 1:bbabbd997d21 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 0:f3bf6c7e2283 237 motors.setSpeed(-20,20);
narshu 0:f3bf6c7e2283 238 Thread::wait(2000);
narshu 0:f3bf6c7e2283 239 motors.stop();
narshu 0:f3bf6c7e2283 240 Thread::wait(5000);
narshu 1:bbabbd997d21 241 motors.setSpeed(20,-20);
narshu 1:bbabbd997d21 242 Thread::wait(2000);
narshu 1:bbabbd997d21 243 motors.stop();
narshu 1:bbabbd997d21 244 Thread::wait(5000);
narshu 0:f3bf6c7e2283 245 }
narshu 0:f3bf6c7e2283 246 }
narshu 0:f3bf6c7e2283 247
narshu 0:f3bf6c7e2283 248
narshu 0:f3bf6c7e2283 249 void vPrintState(void const *argument) {
narshu 0:f3bf6c7e2283 250 float state[3];
narshu 1:bbabbd997d21 251 float SonarMeasures[3];
narshu 1:bbabbd997d21 252 float IRMeasures[3];
narshu 1:bbabbd997d21 253
narshu 22:7ba09c0af0d0 254 Thread::wait(5000);
narshu 0:f3bf6c7e2283 255 while (1) {
narshu 0:f3bf6c7e2283 256 kalman.statelock.lock();
narshu 0:f3bf6c7e2283 257 state[0] = kalman.X(0);
narshu 0:f3bf6c7e2283 258 state[1] = kalman.X(1);
narshu 0:f3bf6c7e2283 259 state[2] = kalman.X(2);
narshu 1:bbabbd997d21 260 SonarMeasures[0] = kalman.SonarMeasures[0];
narshu 1:bbabbd997d21 261 SonarMeasures[1] = kalman.SonarMeasures[1];
narshu 1:bbabbd997d21 262 SonarMeasures[2] = kalman.SonarMeasures[2];
narshu 1:bbabbd997d21 263 IRMeasures[0] = kalman.IRMeasures[0];
narshu 1:bbabbd997d21 264 IRMeasures[1] = kalman.IRMeasures[1];
narshu 1:bbabbd997d21 265 IRMeasures[2] = kalman.IRMeasures[2];
narshu 0:f3bf6c7e2283 266 kalman.statelock.unlock();
narshu 0:f3bf6c7e2283 267 pc.printf("\r\n");
narshu 22:7ba09c0af0d0 268 pc.printf("current: %0.4f %0.4f %0.4f \r\n", state[0]*1000, state[1]*1000,state[2]*180/PI);
narshu 22:7ba09c0af0d0 269 pc.printf("Sonar: %0.4f %0.4f %0.4f \r\n",SonarMeasures[0]*1000,SonarMeasures[1]*1000,SonarMeasures[2]*1000);
narshu 1:bbabbd997d21 270 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 271 Thread::wait(100);
narshu 0:f3bf6c7e2283 272 }
narshu 0:f3bf6c7e2283 273 }
narshu 22:7ba09c0af0d0 274
narshu 22:7ba09c0af0d0 275 void vStop (void) {
narshu 22:7ba09c0af0d0 276 // while (true) {
narshu 23:1901cb6d0d95 277 motors.coastStop();
narshu 22:7ba09c0af0d0 278 ai.flag_motorStop = true;
narshu 22:7ba09c0af0d0 279 // terminate thread, stopps motors permanently
narshu 22:7ba09c0af0d0 280 ai.flag_terminate = true;
narshu 22:7ba09c0af0d0 281 // };
narshu 22:7ba09c0af0d0 282 }