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?
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 | 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 | } |