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?
User | Revision | Line number | New 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 | } |