Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

main.cpp@7:f9c59a3e4155, 2012-04-27 (annotated)

Committer:
narshu
Date:
Fri Apr 27 16:37:26 2012 +0000
Revision:
7:f9c59a3e4155
Parent:
4:7b7334441da9
Child:
9:377560539b74
Serial problem fixed, working on UI

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 1:bbabbd997d21 43 Thread tMotorThread(vMotorThread,NULL,osPriorityNormal,256);
narshu 7:f9c59a3e4155 44 //Thread tUpdateState(vPrintState,NULL,osPriorityNormal,1024);
narshu 2:cffa347bb943 45
narshu 1:bbabbd997d21 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 0:f3bf6c7e2283 56 // do nothing
narshu 0:f3bf6c7e2283 57 Thread::wait(osWaitForever);
narshu 0:f3bf6c7e2283 58 }
narshu 0:f3bf6c7e2283 59 }
narshu 0:f3bf6c7e2283 60
narshu 0:f3bf6c7e2283 61
narshu 0:f3bf6c7e2283 62 void vMotorThread(void const *argument) {
narshu 0:f3bf6c7e2283 63 motors.resetEncoders();
narshu 0:f3bf6c7e2283 64 while (1) {
narshu 1:bbabbd997d21 65 motors.setSpeed(20,20);
narshu 1:bbabbd997d21 66 Thread::wait(2000);
narshu 1:bbabbd997d21 67 motors.stop();
narshu 1:bbabbd997d21 68 Thread::wait(5000);
narshu 1:bbabbd997d21 69 motors.setSpeed(-20,-20);
narshu 0:f3bf6c7e2283 70 Thread::wait(2000);
narshu 0:f3bf6c7e2283 71 motors.stop();
narshu 0:f3bf6c7e2283 72 Thread::wait(5000);
narshu 0:f3bf6c7e2283 73 motors.setSpeed(-20,20);
narshu 0:f3bf6c7e2283 74 Thread::wait(2000);
narshu 0:f3bf6c7e2283 75 motors.stop();
narshu 0:f3bf6c7e2283 76 Thread::wait(5000);
narshu 1:bbabbd997d21 77 motors.setSpeed(20,-20);
narshu 1:bbabbd997d21 78 Thread::wait(2000);
narshu 1:bbabbd997d21 79 motors.stop();
narshu 1:bbabbd997d21 80 Thread::wait(5000);
narshu 0:f3bf6c7e2283 81 }
narshu 0:f3bf6c7e2283 82 }
narshu 0:f3bf6c7e2283 83
narshu 0:f3bf6c7e2283 84
narshu 0:f3bf6c7e2283 85
narshu 0:f3bf6c7e2283 86 void vPrintState(void const *argument) {
narshu 0:f3bf6c7e2283 87 float state[3];
narshu 1:bbabbd997d21 88 float SonarMeasures[3];
narshu 1:bbabbd997d21 89 float IRMeasures[3];
narshu 1:bbabbd997d21 90
narshu 0:f3bf6c7e2283 91
narshu 0:f3bf6c7e2283 92 while (1) {
narshu 0:f3bf6c7e2283 93 kalman.statelock.lock();
narshu 0:f3bf6c7e2283 94 state[0] = kalman.X(0);
narshu 0:f3bf6c7e2283 95 state[1] = kalman.X(1);
narshu 0:f3bf6c7e2283 96 state[2] = kalman.X(2);
narshu 1:bbabbd997d21 97 SonarMeasures[0] = kalman.SonarMeasures[0];
narshu 1:bbabbd997d21 98 SonarMeasures[1] = kalman.SonarMeasures[1];
narshu 1:bbabbd997d21 99 SonarMeasures[2] = kalman.SonarMeasures[2];
narshu 1:bbabbd997d21 100 IRMeasures[0] = kalman.IRMeasures[0];
narshu 1:bbabbd997d21 101 IRMeasures[1] = kalman.IRMeasures[1];
narshu 1:bbabbd997d21 102 IRMeasures[2] = kalman.IRMeasures[2];
narshu 0:f3bf6c7e2283 103 kalman.statelock.unlock();
narshu 0:f3bf6c7e2283 104 pc.printf("\r\n");
narshu 0:f3bf6c7e2283 105 pc.printf("current: %0.4f %0.4f %0.4f \r\n", state[0], state[1],state[2]);
narshu 1:bbabbd997d21 106 pc.printf("Sonar: %0.4f %0.4f %0.4f \r\n",SonarMeasures[0],SonarMeasures[1],SonarMeasures[2]);
narshu 1:bbabbd997d21 107 pc.printf("IR : %0.4f %0.4f %0.4f \r\n",IRMeasures[0]*180/PI,IRMeasures[1]*180/PI,IRMeasures[2]*180/PI);
narshu 2:cffa347bb943 108 pc.printf("Angle_Offset: %0.4f \r\n",kalman.ir.angleOffset*180/PI);
narshu 1:bbabbd997d21 109 Thread::wait(100);
narshu 0:f3bf6c7e2283 110 }
narshu 0:f3bf6c7e2283 111 }
narshu 0:f3bf6c7e2283 112
narshu 0:f3bf6c7e2283 113 // motion control thread ------------------------
narshu 0:f3bf6c7e2283 114 void motion_thread(void const *argument) {
narshu 0:f3bf6c7e2283 115 motors.resetEncoders();
narshu 2:cffa347bb943 116 //motors.setSpeed(MOVE_SPEED/2,MOVE_SPEED/2);
narshu 2:cffa347bb943 117 //Thread::wait(1000);
narshu 0:f3bf6c7e2283 118 motors.stop();
narshu 2:cffa347bb943 119 ai.thr_AI.signal_set(0x01);
narshu 0:f3bf6c7e2283 120
narshu 0:f3bf6c7e2283 121 float currX, currY,currTheta;
narshu 0:f3bf6c7e2283 122 float speedL,speedR;
narshu 0:f3bf6c7e2283 123 float diffDir,diffSpeed;
narshu 0:f3bf6c7e2283 124 float lastdiffSpeed = 0;
narshu 0:f3bf6c7e2283 125
narshu 0:f3bf6c7e2283 126 while (1) {
narshu 2:cffa347bb943 127 if (ai.flag_terminate) {
narshu 0:f3bf6c7e2283 128 terminate();
narshu 0:f3bf6c7e2283 129 }
narshu 0:f3bf6c7e2283 130
narshu 0:f3bf6c7e2283 131 // get kalman localization estimate ------------------------
narshu 0:f3bf6c7e2283 132 kalman.statelock.lock();
narshu 0:f3bf6c7e2283 133 currX = kalman.X(0)*1000.0f;
narshu 0:f3bf6c7e2283 134 currY = kalman.X(1)*1000.0f;
narshu 0:f3bf6c7e2283 135 currTheta = kalman.X(2);
narshu 0:f3bf6c7e2283 136 kalman.statelock.unlock();
narshu 0:f3bf6c7e2283 137
narshu 0:f3bf6c7e2283 138
narshu 0:f3bf6c7e2283 139 // check if target reached ----------------------------------
narshu 2:cffa347bb943 140 if ( ( abs(currX - ai.target.x) < POSITION_TOR )
narshu 2:cffa347bb943 141 &&( abs(currY - ai.target.y) < POSITION_TOR )
narshu 0:f3bf6c7e2283 142 ) {
narshu 0:f3bf6c7e2283 143
narshu 2:cffa347bb943 144 diffDir = rectifyAng(currTheta - ai.target.theta);
narshu 0:f3bf6c7e2283 145 diffSpeed = diffDir / PI;
narshu 0:f3bf6c7e2283 146
narshu 0:f3bf6c7e2283 147 if (abs(diffDir) > ANGLE_TOR) {
narshu 0:f3bf6c7e2283 148 if (abs(diffSpeed) < 0.5) {
narshu 0:f3bf6c7e2283 149 diffSpeed = 0.5*diffSpeed/abs(diffSpeed);
narshu 0:f3bf6c7e2283 150 }
narshu 0:f3bf6c7e2283 151 motors.setSpeed( int(diffSpeed*MOVE_SPEED), -int(diffSpeed*MOVE_SPEED));
narshu 0:f3bf6c7e2283 152
narshu 0:f3bf6c7e2283 153
narshu 0:f3bf6c7e2283 154 } else {
narshu 0:f3bf6c7e2283 155 motors.stop();
narshu 0:f3bf6c7e2283 156 Thread::wait(4000);
narshu 2:cffa347bb943 157 ai.thr_AI.signal_set(0x01);
narshu 0:f3bf6c7e2283 158 }
narshu 0:f3bf6c7e2283 159 }
narshu 0:f3bf6c7e2283 160
narshu 0:f3bf6c7e2283 161 // adjust motion to reach target ----------------------------
narshu 0:f3bf6c7e2283 162 else {
narshu 0:f3bf6c7e2283 163
narshu 0:f3bf6c7e2283 164 // calc direction to target
narshu 2:cffa347bb943 165 float targetDir = atan2(ai.target.y - currY, ai.target.x - currX);
narshu 2:cffa347bb943 166 if (!ai.target.facing)
narshu 2:cffa347bb943 167 targetDir = PI - targetDir;
narshu 0:f3bf6c7e2283 168
narshu 0:f3bf6c7e2283 169
narshu 0:f3bf6c7e2283 170 diffDir = rectifyAng(currTheta - targetDir);
narshu 0:f3bf6c7e2283 171 diffSpeed = diffDir / PI;
narshu 0:f3bf6c7e2283 172
narshu 0:f3bf6c7e2283 173 if (abs(diffDir) > ANGLE_TOR*2) {
narshu 0:f3bf6c7e2283 174 if (abs(diffSpeed) < 0.5) {
narshu 0:f3bf6c7e2283 175 diffSpeed = 0.5*diffSpeed/abs(diffSpeed);
narshu 0:f3bf6c7e2283 176 }
narshu 0:f3bf6c7e2283 177 motors.setSpeed( int(diffSpeed*MOVE_SPEED), -int(diffSpeed*MOVE_SPEED));
narshu 0:f3bf6c7e2283 178 } else {
narshu 0:f3bf6c7e2283 179
narshu 0:f3bf6c7e2283 180
narshu 0:f3bf6c7e2283 181 if (abs(diffSpeed-lastdiffSpeed) > MAX_STEP_RATIO ) {
narshu 0:f3bf6c7e2283 182 if (diffSpeed-lastdiffSpeed >= 0) {
narshu 0:f3bf6c7e2283 183 diffSpeed = lastdiffSpeed + MAX_STEP_RATIO;
narshu 0:f3bf6c7e2283 184 } else {
narshu 0:f3bf6c7e2283 185 diffSpeed = lastdiffSpeed - MAX_STEP_RATIO;
narshu 0:f3bf6c7e2283 186 }
narshu 0:f3bf6c7e2283 187 }
narshu 0:f3bf6c7e2283 188 lastdiffSpeed = diffSpeed;
narshu 0:f3bf6c7e2283 189
narshu 0:f3bf6c7e2283 190 // calculte the motor speeds
narshu 0:f3bf6c7e2283 191 if (diffSpeed <= 0) {
narshu 0:f3bf6c7e2283 192 speedL = (1-2*abs(diffSpeed))*MOVE_SPEED;
narshu 0:f3bf6c7e2283 193 speedR = MOVE_SPEED;
narshu 0:f3bf6c7e2283 194
narshu 0:f3bf6c7e2283 195 } else {
narshu 0:f3bf6c7e2283 196 speedR = (1-2*abs(diffSpeed))*MOVE_SPEED;
narshu 0:f3bf6c7e2283 197 speedL = MOVE_SPEED;
narshu 0:f3bf6c7e2283 198 }
narshu 2:cffa347bb943 199 if (ai.target.facing)
narshu 2:cffa347bb943 200 motors.setSpeed( int(speedL), int(speedR));
narshu 2:cffa347bb943 201 else
narshu 2:cffa347bb943 202 motors.setSpeed( -int(speedL), -int(speedR));
narshu 0:f3bf6c7e2283 203 }
narshu 0:f3bf6c7e2283 204 }
narshu 0:f3bf6c7e2283 205
narshu 0:f3bf6c7e2283 206 // wait
narshu 0:f3bf6c7e2283 207 Thread::wait(MOTION_UPDATE_PERIOD);
narshu 0:f3bf6c7e2283 208 }
narshu 1:bbabbd997d21 209 }