Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

main.cpp@21:15da49f18c63, 2012-05-01 (annotated)

Committer:
narshu
Date:
Tue May 01 16:54:44 2012 +0000
Revision:
21:15da49f18c63
Parent:
19:06610e1c0895
Child:
22:7ba09c0af0d0
Added support for both starting locs (except AI target mirroring)

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 0:f3bf6c7e2283 19 Motors motors;
narshu 2:cffa347bb943 20 UI ui;
narshu 4:7b7334441da9 21 Kalman kalman(motors,ui,p23,p14,p14,p14,p15,p15,p15,p5,p6,p7,p8,p11);
narshu 2:cffa347bb943 22 AI ai;
narshu 10:294b9adbc9d3 23 Motion motion(motors, ai, kalman);
narshu 1:bbabbd997d21 24
narshu 0:f3bf6c7e2283 25 //TODO mutex on kalman state, and on motor commands (i.e. on the i2c bus)
narshu 0:f3bf6c7e2283 26
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 1:bbabbd997d21 31
narshu 0:f3bf6c7e2283 32 //Main loop
narshu 0:f3bf6c7e2283 33 int main() {
narshu 0:f3bf6c7e2283 34 pc.baud(115200);
narshu 21:15da49f18c63 35 // no motor motions till we pull the trig
narshu 21:15da49f18c63 36 ai.flag_motorStop = true;
narshu 21:15da49f18c63 37 //Init kalman, this should be done in the mid of the arena before the game starts
narshu 2:cffa347bb943 38 kalman.KalmanInit();
narshu 1:bbabbd997d21 39
narshu 9:377560539b74 40 //Thread tMotorThread(vMotorThread,NULL,osPriorityNormal,256);
narshu 7:f9c59a3e4155 41 //Thread tUpdateState(vPrintState,NULL,osPriorityNormal,1024);
narshu 21:15da49f18c63 42
narshu 2:cffa347bb943 43
narshu 0:f3bf6c7e2283 44 pc.printf("We got to main! ;D\r\n");
narshu 0:f3bf6c7e2283 45
narshu 0:f3bf6c7e2283 46 //REMEMBERT TO PUT PULL UP RESISTORS ON I2C!!!!!!!!!!!!!!
narshu 0:f3bf6c7e2283 47 while (1) {
narshu 21:15da49f18c63 48 // we use main loop to estimate the cpu usage
narshu 9:377560539b74 49
narshu 9:377560539b74 50 osThreadSetPriority (osThreadGetId(), osPriorityIdle);
narshu 9:377560539b74 51
narshu 9:377560539b74 52 Timer timer;
narshu 9:377560539b74 53 ui.regid(10, 1);
narshu 9:377560539b74 54
narshu 9:377560539b74 55 while(1) {
narshu 9:377560539b74 56 timer.reset();
narshu 9:377560539b74 57 timer.start();
narshu 9:377560539b74 58 nopwait(1000);
narshu 9:377560539b74 59
narshu 9:377560539b74 60 ui.updateval(10, timer.read_us());
narshu 9:377560539b74 61 }
narshu 9:377560539b74 62
narshu 0:f3bf6c7e2283 63 // do nothing
narshu 10:294b9adbc9d3 64 //Thread::wait(osWaitForever);
narshu 0:f3bf6c7e2283 65 }
narshu 0:f3bf6c7e2283 66 }
narshu 0:f3bf6c7e2283 67
narshu 9:377560539b74 68 void AI::ai_thread () {
narshu 9:377560539b74 69 /*
narshu 9:377560539b74 70 //printf("aithreadstart\r\n");
narshu 9:377560539b74 71 Thread::signal_wait(0x01);
narshu 9:377560539b74 72 settarget(660, 400, PI/2, true);
narshu 9:377560539b74 73
narshu 9:377560539b74 74 Thread::signal_wait(0x01);
narshu 9:377560539b74 75 settarget(660, 570, PI, true);
narshu 9:377560539b74 76
narshu 9:377560539b74 77 Thread::signal_wait(0x01);
narshu 9:377560539b74 78 settarget(400, 870, PI, true);
narshu 9:377560539b74 79
narshu 9:377560539b74 80 Thread::signal_wait(0x01);
narshu 9:377560539b74 81 settarget(660, 870, PI, false);
narshu 9:377560539b74 82
narshu 9:377560539b74 83 flag_terminate = true;
narshu 9:377560539b74 84 */
narshu 13:57ea4e520dbd 85
narshu 21:15da49f18c63 86
narshu 21:15da49f18c63 87 //
narshu 21:15da49f18c63 88 // Put some code here so it's started by the pull trigger
narshu 21:15da49f18c63 89 // start a 90s timer here as well
narshu 21:15da49f18c63 90 //
narshu 21:15da49f18c63 91 //while (!Tiggered);
narshu 21:15da49f18c63 92
narshu 21:15da49f18c63 93 // starts motors
narshu 21:15da49f18c63 94 ai.flag_motorStop = false;
narshu 19:06610e1c0895 95 // strat 1 ==================================
narshu 19:06610e1c0895 96 // goto middle x
narshu 19:06610e1c0895 97 settarget(1500, 250, PI/2, true);
narshu 19:06610e1c0895 98 Thread::signal_wait(0x01);
narshu 19:06610e1c0895 99 Thread::wait(2000);
narshu 19:06610e1c0895 100
narshu 19:06610e1c0895 101 // to palm tree
narshu 19:06610e1c0895 102 settarget(1500, 1000, PI, true);
narshu 19:06610e1c0895 103 Thread::signal_wait(0x01);
narshu 19:06610e1c0895 104 Thread::wait(2000);
narshu 19:06610e1c0895 105
narshu 19:06610e1c0895 106 // run over totem
narshu 19:06610e1c0895 107 settarget(640,1000,PI, true);
narshu 19:06610e1c0895 108 Thread::signal_wait(0x01);
narshu 19:06610e1c0895 109 Thread::wait(2000);
narshu 19:06610e1c0895 110
narshu 19:06610e1c0895 111 // back to ship
narshu 19:06610e1c0895 112 settarget(220,780,PI,true);
narshu 19:06610e1c0895 113 Thread::signal_wait(0x01);
narshu 19:06610e1c0895 114 Thread::wait(2000);
narshu 19:06610e1c0895 115
narshu 21:15da49f18c63 116 // terminate thread, stopps motors permanently
narshu 21:15da49f18c63 117 ai.flag_terminate = true;
narshu 19:06610e1c0895 118 while(true){
narshu 19:06610e1c0895 119 Thread::wait(osWaitForever);
narshu 19:06610e1c0895 120 }
narshu 19:06610e1c0895 121
narshu 19:06610e1c0895 122
narshu 19:06610e1c0895 123 // end of strat 1 ===========================
narshu 9:377560539b74 124 }
narshu 9:377560539b74 125
narshu 0:f3bf6c7e2283 126
narshu 0:f3bf6c7e2283 127 void vMotorThread(void const *argument) {
narshu 0:f3bf6c7e2283 128 motors.resetEncoders();
narshu 0:f3bf6c7e2283 129 while (1) {
narshu 1:bbabbd997d21 130 motors.setSpeed(20,20);
narshu 1:bbabbd997d21 131 Thread::wait(2000);
narshu 1:bbabbd997d21 132 motors.stop();
narshu 1:bbabbd997d21 133 Thread::wait(5000);
narshu 1:bbabbd997d21 134 motors.setSpeed(-20,-20);
narshu 0:f3bf6c7e2283 135 Thread::wait(2000);
narshu 0:f3bf6c7e2283 136 motors.stop();
narshu 0:f3bf6c7e2283 137 Thread::wait(5000);
narshu 0:f3bf6c7e2283 138 motors.setSpeed(-20,20);
narshu 0:f3bf6c7e2283 139 Thread::wait(2000);
narshu 0:f3bf6c7e2283 140 motors.stop();
narshu 0:f3bf6c7e2283 141 Thread::wait(5000);
narshu 1:bbabbd997d21 142 motors.setSpeed(20,-20);
narshu 1:bbabbd997d21 143 Thread::wait(2000);
narshu 1:bbabbd997d21 144 motors.stop();
narshu 1:bbabbd997d21 145 Thread::wait(5000);
narshu 0:f3bf6c7e2283 146 }
narshu 0:f3bf6c7e2283 147 }
narshu 0:f3bf6c7e2283 148
narshu 0:f3bf6c7e2283 149
narshu 0:f3bf6c7e2283 150 void vPrintState(void const *argument) {
narshu 0:f3bf6c7e2283 151 float state[3];
narshu 1:bbabbd997d21 152 float SonarMeasures[3];
narshu 1:bbabbd997d21 153 float IRMeasures[3];
narshu 1:bbabbd997d21 154
narshu 0:f3bf6c7e2283 155
narshu 0:f3bf6c7e2283 156 while (1) {
narshu 0:f3bf6c7e2283 157 kalman.statelock.lock();
narshu 0:f3bf6c7e2283 158 state[0] = kalman.X(0);
narshu 0:f3bf6c7e2283 159 state[1] = kalman.X(1);
narshu 0:f3bf6c7e2283 160 state[2] = kalman.X(2);
narshu 1:bbabbd997d21 161 SonarMeasures[0] = kalman.SonarMeasures[0];
narshu 1:bbabbd997d21 162 SonarMeasures[1] = kalman.SonarMeasures[1];
narshu 1:bbabbd997d21 163 SonarMeasures[2] = kalman.SonarMeasures[2];
narshu 1:bbabbd997d21 164 IRMeasures[0] = kalman.IRMeasures[0];
narshu 1:bbabbd997d21 165 IRMeasures[1] = kalman.IRMeasures[1];
narshu 1:bbabbd997d21 166 IRMeasures[2] = kalman.IRMeasures[2];
narshu 0:f3bf6c7e2283 167 kalman.statelock.unlock();
narshu 0:f3bf6c7e2283 168 pc.printf("\r\n");
narshu 0:f3bf6c7e2283 169 pc.printf("current: %0.4f %0.4f %0.4f \r\n", state[0], state[1],state[2]);
narshu 1:bbabbd997d21 170 pc.printf("Sonar: %0.4f %0.4f %0.4f \r\n",SonarMeasures[0],SonarMeasures[1],SonarMeasures[2]);
narshu 1:bbabbd997d21 171 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 172 Thread::wait(100);
narshu 0:f3bf6c7e2283 173 }
narshu 0:f3bf6c7e2283 174 }