Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

main.cpp@19:06610e1c0895, 2012-04-30 (annotated)

Committer:
narshu
Date:
Mon Apr 30 20:15:43 2012 +0000
Revision:
19:06610e1c0895
Parent:
17:bafcef1c3579
Child:
21:15da49f18c63
Changed move speed and AI targets

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 //NOTE! Recieving data with RF12B now DISABLED due to interferance with rtos!
narshu 0:f3bf6c7e2283 27
narshu 1:bbabbd997d21 28
narshu 0:f3bf6c7e2283 29 void vMotorThread(void const *argument);
narshu 0:f3bf6c7e2283 30 void vPrintState(void const *argument);
narshu 0:f3bf6c7e2283 31 void motion_thread(void const *argument);
narshu 1:bbabbd997d21 32
narshu 2:cffa347bb943 33 //bool flag_terminate = false;
narshu 0:f3bf6c7e2283 34
narshu 0:f3bf6c7e2283 35 float temp = 0;
narshu 0:f3bf6c7e2283 36
narshu 0:f3bf6c7e2283 37 //Main loop
narshu 0:f3bf6c7e2283 38 int main() {
narshu 0:f3bf6c7e2283 39 pc.baud(115200);
narshu 2:cffa347bb943 40 //Init kalman
narshu 2:cffa347bb943 41 kalman.KalmanInit();
narshu 1:bbabbd997d21 42
narshu 9:377560539b74 43 //Thread tMotorThread(vMotorThread,NULL,osPriorityNormal,256);
narshu 7:f9c59a3e4155 44 //Thread tUpdateState(vPrintState,NULL,osPriorityNormal,1024);
narshu 2:cffa347bb943 45
narshu 10:294b9adbc9d3 46 //Thread thr_motion(motion_thread,NULL,osPriorityNormal,1024);
narshu 1:bbabbd997d21 47 //Motion_Thread_Ptr = &thr_motion;
narshu 1:bbabbd997d21 48
narshu 1:bbabbd997d21 49 //measure cpu usage. output updated once per second to symbol cpupercent
narshu 1:bbabbd997d21 50 //Thread mCPUthread(measureCPUidle, NULL, osPriorityIdle, 1024); //check if stack overflow with such a small staack
narshu 2:cffa347bb943 51
narshu 0:f3bf6c7e2283 52 pc.printf("We got to main! ;D\r\n");
narshu 0:f3bf6c7e2283 53
narshu 0:f3bf6c7e2283 54 //REMEMBERT TO PUT PULL UP RESISTORS ON I2C!!!!!!!!!!!!!!
narshu 0:f3bf6c7e2283 55 while (1) {
narshu 9:377560539b74 56
narshu 9:377560539b74 57 osThreadSetPriority (osThreadGetId(), osPriorityIdle);
narshu 9:377560539b74 58
narshu 9:377560539b74 59 Timer timer;
narshu 9:377560539b74 60 ui.regid(10, 1);
narshu 9:377560539b74 61
narshu 9:377560539b74 62 while(1) {
narshu 9:377560539b74 63 timer.reset();
narshu 9:377560539b74 64 timer.start();
narshu 9:377560539b74 65 nopwait(1000);
narshu 9:377560539b74 66
narshu 9:377560539b74 67 ui.updateval(10, timer.read_us());
narshu 9:377560539b74 68 }
narshu 9:377560539b74 69
narshu 0:f3bf6c7e2283 70 // do nothing
narshu 10:294b9adbc9d3 71 //Thread::wait(osWaitForever);
narshu 0:f3bf6c7e2283 72 }
narshu 0:f3bf6c7e2283 73 }
narshu 0:f3bf6c7e2283 74
narshu 9:377560539b74 75 void AI::ai_thread () {
narshu 9:377560539b74 76 /*
narshu 9:377560539b74 77 //printf("aithreadstart\r\n");
narshu 9:377560539b74 78 Thread::signal_wait(0x01);
narshu 9:377560539b74 79 settarget(660, 400, PI/2, true);
narshu 9:377560539b74 80
narshu 9:377560539b74 81 Thread::signal_wait(0x01);
narshu 9:377560539b74 82 settarget(660, 570, PI, true);
narshu 9:377560539b74 83
narshu 9:377560539b74 84 Thread::signal_wait(0x01);
narshu 9:377560539b74 85 settarget(400, 870, PI, true);
narshu 9:377560539b74 86
narshu 9:377560539b74 87 Thread::signal_wait(0x01);
narshu 9:377560539b74 88 settarget(660, 870, PI, false);
narshu 9:377560539b74 89
narshu 9:377560539b74 90 flag_terminate = true;
narshu 9:377560539b74 91 */
narshu 13:57ea4e520dbd 92
narshu 19:06610e1c0895 93 // strat 1 ==================================
narshu 19:06610e1c0895 94 // goto middle x
narshu 19:06610e1c0895 95 settarget(1500, 250, PI/2, true);
narshu 19:06610e1c0895 96 Thread::signal_wait(0x01);
narshu 19:06610e1c0895 97 Thread::wait(2000);
narshu 19:06610e1c0895 98
narshu 19:06610e1c0895 99 // to palm tree
narshu 19:06610e1c0895 100 settarget(1500, 1000, PI, true);
narshu 19:06610e1c0895 101 Thread::signal_wait(0x01);
narshu 19:06610e1c0895 102 Thread::wait(2000);
narshu 19:06610e1c0895 103
narshu 19:06610e1c0895 104 // run over totem
narshu 19:06610e1c0895 105 settarget(640,1000,PI, true);
narshu 19:06610e1c0895 106 Thread::signal_wait(0x01);
narshu 19:06610e1c0895 107 Thread::wait(2000);
narshu 19:06610e1c0895 108
narshu 19:06610e1c0895 109 // back to ship
narshu 19:06610e1c0895 110 settarget(220,780,PI,true);
narshu 19:06610e1c0895 111 Thread::signal_wait(0x01);
narshu 19:06610e1c0895 112 Thread::wait(2000);
narshu 19:06610e1c0895 113
narshu 19:06610e1c0895 114 flag_terminate = true;
narshu 19:06610e1c0895 115 while(true){
narshu 19:06610e1c0895 116 Thread::wait(osWaitForever);
narshu 19:06610e1c0895 117 }
narshu 19:06610e1c0895 118
narshu 19:06610e1c0895 119
narshu 19:06610e1c0895 120 // end of strat 1 ===========================
narshu 19:06610e1c0895 121
narshu 19:06610e1c0895 122
narshu 19:06610e1c0895 123
narshu 19:06610e1c0895 124
narshu 9:377560539b74 125 while (1) {
narshu 17:bafcef1c3579 126
narshu 17:bafcef1c3579 127
narshu 17:bafcef1c3579 128 // goes to the mid
narshu 17:bafcef1c3579 129 settarget(500, 1000, 0, true);
narshu 17:bafcef1c3579 130 Thread::signal_wait(0x01);
narshu 17:bafcef1c3579 131 Thread::wait(2000);
narshu 17:bafcef1c3579 132
narshu 17:bafcef1c3579 133 settarget(2500, 1000, PI, true);
narshu 17:bafcef1c3579 134 Thread::signal_wait(0x01);
narshu 17:bafcef1c3579 135 Thread::wait(2000);
narshu 17:bafcef1c3579 136
narshu 17:bafcef1c3579 137 }
narshu 9:377560539b74 138
narshu 17:bafcef1c3579 139
narshu 17:bafcef1c3579 140
narshu 17:bafcef1c3579 141 // goes to the mid
narshu 17:bafcef1c3579 142 settarget(700, 1500, 0, false);
narshu 17:bafcef1c3579 143 Thread::signal_wait(0x01);
narshu 17:bafcef1c3579 144 Thread::wait(2000);
narshu 17:bafcef1c3579 145 //////////////////////////////////////////////////////
narshu 9:377560539b74 146 // goes to the mid
narshu 13:57ea4e520dbd 147 settarget(1500, 1000, PI/2, true);
narshu 9:377560539b74 148 Thread::signal_wait(0x01);
narshu 13:57ea4e520dbd 149 Thread::wait(2000);
narshu 9:377560539b74 150
narshu 9:377560539b74 151 // left roll
narshu 13:57ea4e520dbd 152 settarget(500, 1500, PI/2, true);
narshu 9:377560539b74 153 Thread::signal_wait(0x01);
narshu 13:57ea4e520dbd 154 Thread::wait(2000);
narshu 13:57ea4e520dbd 155
narshu 9:377560539b74 156 // mid
narshu 13:57ea4e520dbd 157 settarget(1500, 1000, PI/2, true);
narshu 9:377560539b74 158 Thread::signal_wait(0x01);
narshu 13:57ea4e520dbd 159 Thread::wait(2000);
narshu 9:377560539b74 160
narshu 9:377560539b74 161 // map
narshu 13:57ea4e520dbd 162 settarget(1500, 1500, PI/2, true);
narshu 9:377560539b74 163 Thread::signal_wait(0x01);
narshu 13:57ea4e520dbd 164 Thread::wait(2000);
narshu 13:57ea4e520dbd 165
narshu 9:377560539b74 166
narshu 9:377560539b74 167 // mid
narshu 13:57ea4e520dbd 168 settarget(1500, 1000, -PI/2, true);
narshu 9:377560539b74 169 Thread::signal_wait(0x01);
narshu 13:57ea4e520dbd 170 Thread::wait(2000);
narshu 13:57ea4e520dbd 171
narshu 9:377560539b74 172
narshu 9:377560539b74 173 // home
narshu 13:57ea4e520dbd 174 settarget(500, 500, 0, true);
narshu 9:377560539b74 175 Thread::signal_wait(0x01);
narshu 13:57ea4e520dbd 176 Thread::wait(2000);
narshu 13:57ea4e520dbd 177
narshu 13:57ea4e520dbd 178 // oponents base
narshu 13:57ea4e520dbd 179 settarget(2500, 500, 0, true);
narshu 13:57ea4e520dbd 180 Thread::signal_wait(0x01);
narshu 13:57ea4e520dbd 181 Thread::wait(2000);
narshu 13:57ea4e520dbd 182
narshu 13:57ea4e520dbd 183 // oponents ship
narshu 13:57ea4e520dbd 184 settarget(2500, 1500, 0, true);
narshu 13:57ea4e520dbd 185 Thread::signal_wait(0x01);
narshu 13:57ea4e520dbd 186 Thread::wait(2000);
narshu 13:57ea4e520dbd 187
narshu 13:57ea4e520dbd 188
narshu 17:bafcef1c3579 189 //}
narshu 9:377560539b74 190
narshu 9:377560539b74 191 Thread::signal_wait(0x01);
narshu 9:377560539b74 192 flag_terminate = true;
narshu 9:377560539b74 193 //OLED3 = true;
narshu 9:377560539b74 194
narshu 9:377560539b74 195 while (true) {
narshu 9:377560539b74 196 Thread::wait(osWaitForever);
narshu 9:377560539b74 197 }
narshu 9:377560539b74 198 }
narshu 9:377560539b74 199
narshu 0:f3bf6c7e2283 200
narshu 0:f3bf6c7e2283 201 void vMotorThread(void const *argument) {
narshu 0:f3bf6c7e2283 202 motors.resetEncoders();
narshu 0:f3bf6c7e2283 203 while (1) {
narshu 1:bbabbd997d21 204 motors.setSpeed(20,20);
narshu 1:bbabbd997d21 205 Thread::wait(2000);
narshu 1:bbabbd997d21 206 motors.stop();
narshu 1:bbabbd997d21 207 Thread::wait(5000);
narshu 1:bbabbd997d21 208 motors.setSpeed(-20,-20);
narshu 0:f3bf6c7e2283 209 Thread::wait(2000);
narshu 0:f3bf6c7e2283 210 motors.stop();
narshu 0:f3bf6c7e2283 211 Thread::wait(5000);
narshu 0:f3bf6c7e2283 212 motors.setSpeed(-20,20);
narshu 0:f3bf6c7e2283 213 Thread::wait(2000);
narshu 0:f3bf6c7e2283 214 motors.stop();
narshu 0:f3bf6c7e2283 215 Thread::wait(5000);
narshu 1:bbabbd997d21 216 motors.setSpeed(20,-20);
narshu 1:bbabbd997d21 217 Thread::wait(2000);
narshu 1:bbabbd997d21 218 motors.stop();
narshu 1:bbabbd997d21 219 Thread::wait(5000);
narshu 0:f3bf6c7e2283 220 }
narshu 0:f3bf6c7e2283 221 }
narshu 0:f3bf6c7e2283 222
narshu 0:f3bf6c7e2283 223
narshu 0:f3bf6c7e2283 224 void vPrintState(void const *argument) {
narshu 0:f3bf6c7e2283 225 float state[3];
narshu 1:bbabbd997d21 226 float SonarMeasures[3];
narshu 1:bbabbd997d21 227 float IRMeasures[3];
narshu 1:bbabbd997d21 228
narshu 0:f3bf6c7e2283 229
narshu 0:f3bf6c7e2283 230 while (1) {
narshu 0:f3bf6c7e2283 231 kalman.statelock.lock();
narshu 0:f3bf6c7e2283 232 state[0] = kalman.X(0);
narshu 0:f3bf6c7e2283 233 state[1] = kalman.X(1);
narshu 0:f3bf6c7e2283 234 state[2] = kalman.X(2);
narshu 1:bbabbd997d21 235 SonarMeasures[0] = kalman.SonarMeasures[0];
narshu 1:bbabbd997d21 236 SonarMeasures[1] = kalman.SonarMeasures[1];
narshu 1:bbabbd997d21 237 SonarMeasures[2] = kalman.SonarMeasures[2];
narshu 1:bbabbd997d21 238 IRMeasures[0] = kalman.IRMeasures[0];
narshu 1:bbabbd997d21 239 IRMeasures[1] = kalman.IRMeasures[1];
narshu 1:bbabbd997d21 240 IRMeasures[2] = kalman.IRMeasures[2];
narshu 0:f3bf6c7e2283 241 kalman.statelock.unlock();
narshu 0:f3bf6c7e2283 242 pc.printf("\r\n");
narshu 0:f3bf6c7e2283 243 pc.printf("current: %0.4f %0.4f %0.4f \r\n", state[0], state[1],state[2]);
narshu 1:bbabbd997d21 244 pc.printf("Sonar: %0.4f %0.4f %0.4f \r\n",SonarMeasures[0],SonarMeasures[1],SonarMeasures[2]);
narshu 1:bbabbd997d21 245 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 246 Thread::wait(100);
narshu 0:f3bf6c7e2283 247 }
narshu 0:f3bf6c7e2283 248 }