Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

main.cpp@2:cffa347bb943, 2012-04-26 (annotated)

Committer:
narshu
Date:
Thu Apr 26 21:02:12 2012 +0000
Revision:
2:cffa347bb943
Parent:
1:bbabbd997d21
Child:
4:7b7334441da9
not working

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