Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

main.cpp@9:377560539b74, 2012-04-28 (annotated)

Committer:
narshu
Date:
Sat Apr 28 17:21:24 2012 +0000
Revision:
9:377560539b74
Parent:
7:f9c59a3e4155
Child:
10:294b9adbc9d3
Restructured project to have a single shared lib; Also raised the RF baud rate

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 2:cffa347bb943 10 #include "ai.h"
narshu 2:cffa347bb943 11 #include "ui.h"
narshu 0:f3bf6c7e2283 12
narshu 0:f3bf6c7e2283 13 //#include <iostream>
narshu 0:f3bf6c7e2283 14
narshu 0:f3bf6c7e2283 15 //Interface declaration
narshu 0:f3bf6c7e2283 16 Serial pc(USBTX, USBRX); // tx, rx
narshu 0:f3bf6c7e2283 17
narshu 0:f3bf6c7e2283 18 Motors motors;
narshu 2:cffa347bb943 19 UI ui;
narshu 0:f3bf6c7e2283 20
narshu 0:f3bf6c7e2283 21
narshu 4:7b7334441da9 22 Kalman kalman(motors,ui,p23,p14,p14,p14,p15,p15,p15,p5,p6,p7,p8,p11);
narshu 2:cffa347bb943 23 AI ai;
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 9:377560539b74 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 0:f3bf6c7e2283 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 9:377560539b74 92
narshu 9:377560539b74 93 while (1) {
narshu 9:377560539b74 94
narshu 9:377560539b74 95 // goes to the mid
narshu 9:377560539b74 96 Thread::signal_wait(0x01);
narshu 9:377560539b74 97 settarget(1500, 1000, PI/2, true);
narshu 9:377560539b74 98
narshu 9:377560539b74 99 // left roll
narshu 9:377560539b74 100 Thread::signal_wait(0x01);
narshu 9:377560539b74 101 settarget(500, 1500, PI/2, true);
narshu 9:377560539b74 102
narshu 9:377560539b74 103 // mid
narshu 9:377560539b74 104 Thread::signal_wait(0x01);
narshu 9:377560539b74 105 settarget(1500, 1000, PI/2, true);
narshu 9:377560539b74 106
narshu 9:377560539b74 107 // map
narshu 9:377560539b74 108 Thread::signal_wait(0x01);
narshu 9:377560539b74 109 settarget(1500, 1500, PI/2, true);
narshu 9:377560539b74 110
narshu 9:377560539b74 111 // mid
narshu 9:377560539b74 112 Thread::signal_wait(0x01);
narshu 9:377560539b74 113 settarget(1500, 1000, -PI/2, true);
narshu 9:377560539b74 114
narshu 9:377560539b74 115 // home
narshu 9:377560539b74 116 Thread::signal_wait(0x01);
narshu 9:377560539b74 117 settarget(500, 500, 0, true);
narshu 9:377560539b74 118
narshu 9:377560539b74 119 }
narshu 9:377560539b74 120
narshu 9:377560539b74 121 Thread::signal_wait(0x01);
narshu 9:377560539b74 122 flag_terminate = true;
narshu 9:377560539b74 123 //OLED3 = true;
narshu 9:377560539b74 124
narshu 9:377560539b74 125 while (true) {
narshu 9:377560539b74 126 Thread::wait(osWaitForever);
narshu 9:377560539b74 127 }
narshu 9:377560539b74 128 }
narshu 9:377560539b74 129
narshu 0:f3bf6c7e2283 130
narshu 0:f3bf6c7e2283 131 void vMotorThread(void const *argument) {
narshu 0:f3bf6c7e2283 132 motors.resetEncoders();
narshu 0:f3bf6c7e2283 133 while (1) {
narshu 1:bbabbd997d21 134 motors.setSpeed(20,20);
narshu 1:bbabbd997d21 135 Thread::wait(2000);
narshu 1:bbabbd997d21 136 motors.stop();
narshu 1:bbabbd997d21 137 Thread::wait(5000);
narshu 1:bbabbd997d21 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 0:f3bf6c7e2283 142 motors.setSpeed(-20,20);
narshu 0:f3bf6c7e2283 143 Thread::wait(2000);
narshu 0:f3bf6c7e2283 144 motors.stop();
narshu 0:f3bf6c7e2283 145 Thread::wait(5000);
narshu 1:bbabbd997d21 146 motors.setSpeed(20,-20);
narshu 1:bbabbd997d21 147 Thread::wait(2000);
narshu 1:bbabbd997d21 148 motors.stop();
narshu 1:bbabbd997d21 149 Thread::wait(5000);
narshu 0:f3bf6c7e2283 150 }
narshu 0:f3bf6c7e2283 151 }
narshu 0:f3bf6c7e2283 152
narshu 0:f3bf6c7e2283 153
narshu 0:f3bf6c7e2283 154
narshu 0:f3bf6c7e2283 155 void vPrintState(void const *argument) {
narshu 0:f3bf6c7e2283 156 float state[3];
narshu 1:bbabbd997d21 157 float SonarMeasures[3];
narshu 1:bbabbd997d21 158 float IRMeasures[3];
narshu 1:bbabbd997d21 159
narshu 0:f3bf6c7e2283 160
narshu 0:f3bf6c7e2283 161 while (1) {
narshu 0:f3bf6c7e2283 162 kalman.statelock.lock();
narshu 0:f3bf6c7e2283 163 state[0] = kalman.X(0);
narshu 0:f3bf6c7e2283 164 state[1] = kalman.X(1);
narshu 0:f3bf6c7e2283 165 state[2] = kalman.X(2);
narshu 1:bbabbd997d21 166 SonarMeasures[0] = kalman.SonarMeasures[0];
narshu 1:bbabbd997d21 167 SonarMeasures[1] = kalman.SonarMeasures[1];
narshu 1:bbabbd997d21 168 SonarMeasures[2] = kalman.SonarMeasures[2];
narshu 1:bbabbd997d21 169 IRMeasures[0] = kalman.IRMeasures[0];
narshu 1:bbabbd997d21 170 IRMeasures[1] = kalman.IRMeasures[1];
narshu 1:bbabbd997d21 171 IRMeasures[2] = kalman.IRMeasures[2];
narshu 0:f3bf6c7e2283 172 kalman.statelock.unlock();
narshu 0:f3bf6c7e2283 173 pc.printf("\r\n");
narshu 0:f3bf6c7e2283 174 pc.printf("current: %0.4f %0.4f %0.4f \r\n", state[0], state[1],state[2]);
narshu 1:bbabbd997d21 175 pc.printf("Sonar: %0.4f %0.4f %0.4f \r\n",SonarMeasures[0],SonarMeasures[1],SonarMeasures[2]);
narshu 1:bbabbd997d21 176 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 177 Thread::wait(100);
narshu 0:f3bf6c7e2283 178 }
narshu 0:f3bf6c7e2283 179 }
narshu 0:f3bf6c7e2283 180
narshu 0:f3bf6c7e2283 181 // motion control thread ------------------------
narshu 0:f3bf6c7e2283 182 void motion_thread(void const *argument) {
narshu 0:f3bf6c7e2283 183 motors.resetEncoders();
narshu 2:cffa347bb943 184 //motors.setSpeed(MOVE_SPEED/2,MOVE_SPEED/2);
narshu 2:cffa347bb943 185 //Thread::wait(1000);
narshu 0:f3bf6c7e2283 186 motors.stop();
narshu 2:cffa347bb943 187 ai.thr_AI.signal_set(0x01);
narshu 0:f3bf6c7e2283 188
narshu 0:f3bf6c7e2283 189 float currX, currY,currTheta;
narshu 0:f3bf6c7e2283 190 float speedL,speedR;
narshu 0:f3bf6c7e2283 191 float diffDir,diffSpeed;
narshu 0:f3bf6c7e2283 192 float lastdiffSpeed = 0;
narshu 0:f3bf6c7e2283 193
narshu 0:f3bf6c7e2283 194 while (1) {
narshu 2:cffa347bb943 195 if (ai.flag_terminate) {
narshu 0:f3bf6c7e2283 196 terminate();
narshu 0:f3bf6c7e2283 197 }
narshu 0:f3bf6c7e2283 198
narshu 0:f3bf6c7e2283 199 // get kalman localization estimate ------------------------
narshu 0:f3bf6c7e2283 200 kalman.statelock.lock();
narshu 0:f3bf6c7e2283 201 currX = kalman.X(0)*1000.0f;
narshu 0:f3bf6c7e2283 202 currY = kalman.X(1)*1000.0f;
narshu 0:f3bf6c7e2283 203 currTheta = kalman.X(2);
narshu 0:f3bf6c7e2283 204 kalman.statelock.unlock();
narshu 0:f3bf6c7e2283 205
narshu 0:f3bf6c7e2283 206
narshu 0:f3bf6c7e2283 207 // check if target reached ----------------------------------
narshu 2:cffa347bb943 208 if ( ( abs(currX - ai.target.x) < POSITION_TOR )
narshu 2:cffa347bb943 209 &&( abs(currY - ai.target.y) < POSITION_TOR )
narshu 0:f3bf6c7e2283 210 ) {
narshu 0:f3bf6c7e2283 211
narshu 2:cffa347bb943 212 diffDir = rectifyAng(currTheta - ai.target.theta);
narshu 0:f3bf6c7e2283 213 diffSpeed = diffDir / PI;
narshu 0:f3bf6c7e2283 214
narshu 0:f3bf6c7e2283 215 if (abs(diffDir) > ANGLE_TOR) {
narshu 0:f3bf6c7e2283 216 if (abs(diffSpeed) < 0.5) {
narshu 0:f3bf6c7e2283 217 diffSpeed = 0.5*diffSpeed/abs(diffSpeed);
narshu 0:f3bf6c7e2283 218 }
narshu 0:f3bf6c7e2283 219 motors.setSpeed( int(diffSpeed*MOVE_SPEED), -int(diffSpeed*MOVE_SPEED));
narshu 0:f3bf6c7e2283 220
narshu 0:f3bf6c7e2283 221
narshu 0:f3bf6c7e2283 222 } else {
narshu 0:f3bf6c7e2283 223 motors.stop();
narshu 0:f3bf6c7e2283 224 Thread::wait(4000);
narshu 2:cffa347bb943 225 ai.thr_AI.signal_set(0x01);
narshu 0:f3bf6c7e2283 226 }
narshu 0:f3bf6c7e2283 227 }
narshu 0:f3bf6c7e2283 228
narshu 0:f3bf6c7e2283 229 // adjust motion to reach target ----------------------------
narshu 0:f3bf6c7e2283 230 else {
narshu 0:f3bf6c7e2283 231
narshu 0:f3bf6c7e2283 232 // calc direction to target
narshu 2:cffa347bb943 233 float targetDir = atan2(ai.target.y - currY, ai.target.x - currX);
narshu 2:cffa347bb943 234 if (!ai.target.facing)
narshu 2:cffa347bb943 235 targetDir = PI - targetDir;
narshu 0:f3bf6c7e2283 236
narshu 0:f3bf6c7e2283 237
narshu 0:f3bf6c7e2283 238 diffDir = rectifyAng(currTheta - targetDir);
narshu 0:f3bf6c7e2283 239 diffSpeed = diffDir / PI;
narshu 0:f3bf6c7e2283 240
narshu 0:f3bf6c7e2283 241 if (abs(diffDir) > ANGLE_TOR*2) {
narshu 0:f3bf6c7e2283 242 if (abs(diffSpeed) < 0.5) {
narshu 0:f3bf6c7e2283 243 diffSpeed = 0.5*diffSpeed/abs(diffSpeed);
narshu 0:f3bf6c7e2283 244 }
narshu 0:f3bf6c7e2283 245 motors.setSpeed( int(diffSpeed*MOVE_SPEED), -int(diffSpeed*MOVE_SPEED));
narshu 0:f3bf6c7e2283 246 } else {
narshu 0:f3bf6c7e2283 247
narshu 0:f3bf6c7e2283 248
narshu 0:f3bf6c7e2283 249 if (abs(diffSpeed-lastdiffSpeed) > MAX_STEP_RATIO ) {
narshu 0:f3bf6c7e2283 250 if (diffSpeed-lastdiffSpeed >= 0) {
narshu 0:f3bf6c7e2283 251 diffSpeed = lastdiffSpeed + MAX_STEP_RATIO;
narshu 0:f3bf6c7e2283 252 } else {
narshu 0:f3bf6c7e2283 253 diffSpeed = lastdiffSpeed - MAX_STEP_RATIO;
narshu 0:f3bf6c7e2283 254 }
narshu 0:f3bf6c7e2283 255 }
narshu 0:f3bf6c7e2283 256 lastdiffSpeed = diffSpeed;
narshu 0:f3bf6c7e2283 257
narshu 0:f3bf6c7e2283 258 // calculte the motor speeds
narshu 0:f3bf6c7e2283 259 if (diffSpeed <= 0) {
narshu 0:f3bf6c7e2283 260 speedL = (1-2*abs(diffSpeed))*MOVE_SPEED;
narshu 0:f3bf6c7e2283 261 speedR = MOVE_SPEED;
narshu 0:f3bf6c7e2283 262
narshu 0:f3bf6c7e2283 263 } else {
narshu 0:f3bf6c7e2283 264 speedR = (1-2*abs(diffSpeed))*MOVE_SPEED;
narshu 0:f3bf6c7e2283 265 speedL = MOVE_SPEED;
narshu 0:f3bf6c7e2283 266 }
narshu 2:cffa347bb943 267 if (ai.target.facing)
narshu 2:cffa347bb943 268 motors.setSpeed( int(speedL), int(speedR));
narshu 2:cffa347bb943 269 else
narshu 2:cffa347bb943 270 motors.setSpeed( -int(speedL), -int(speedR));
narshu 0:f3bf6c7e2283 271 }
narshu 0:f3bf6c7e2283 272 }
narshu 0:f3bf6c7e2283 273
narshu 0:f3bf6c7e2283 274 // wait
narshu 0:f3bf6c7e2283 275 Thread::wait(MOTION_UPDATE_PERIOD);
narshu 0:f3bf6c7e2283 276 }
narshu 1:bbabbd997d21 277 }