Eurobot2012_Primary
Dependencies: mbed Eurobot_2012_Primary
main.cpp@1:bbabbd997d21, 2012-04-20 (annotated)
- Committer:
- narshu
- Date:
- Fri Apr 20 21:56:15 2012 +0000
- Revision:
- 1:bbabbd997d21
- Parent:
- 0:f3bf6c7e2283
- Child:
- 2:cffa347bb943
copied everything from secondary;
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 | 0:f3bf6c7e2283 | 10 | |
narshu | 0:f3bf6c7e2283 | 11 | //#include <iostream> |
narshu | 0:f3bf6c7e2283 | 12 | |
narshu | 0:f3bf6c7e2283 | 13 | //Interface declaration |
narshu | 0:f3bf6c7e2283 | 14 | //I2C i2c(p28, p27); // sda, scl |
narshu | 0:f3bf6c7e2283 | 15 | TSI2C i2c(p28, p27); |
narshu | 0:f3bf6c7e2283 | 16 | Serial pc(USBTX, USBRX); // tx, rx |
narshu | 1:bbabbd997d21 | 17 | Serial IRturret(p9, p10); |
narshu | 0:f3bf6c7e2283 | 18 | |
narshu | 0:f3bf6c7e2283 | 19 | DigitalOut OLED1(LED1); |
narshu | 0:f3bf6c7e2283 | 20 | DigitalOut OLED2(LED2); |
narshu | 0:f3bf6c7e2283 | 21 | DigitalOut OLED3(LED3); |
narshu | 0:f3bf6c7e2283 | 22 | DigitalOut OLED4(LED4); |
narshu | 0:f3bf6c7e2283 | 23 | |
narshu | 0:f3bf6c7e2283 | 24 | Motors motors; |
narshu | 0:f3bf6c7e2283 | 25 | Kalman kalman(motors); |
narshu | 0:f3bf6c7e2283 | 26 | |
narshu | 0:f3bf6c7e2283 | 27 | float targetX = 1000, targetY = 1000, targetTheta = 0; |
narshu | 0:f3bf6c7e2283 | 28 | |
narshu | 1:bbabbd997d21 | 29 | // bytes packing/unpacking for IR turret serial comm |
narshu | 0:f3bf6c7e2283 | 30 | union IRValue_t { |
narshu | 1:bbabbd997d21 | 31 | float IR_floats[3]; |
narshu | 1:bbabbd997d21 | 32 | int IR_ints[3]; |
narshu | 1:bbabbd997d21 | 33 | unsigned char IR_chars[12]; |
narshu | 0:f3bf6c7e2283 | 34 | } IRValues; |
narshu | 0:f3bf6c7e2283 | 35 | |
narshu | 1:bbabbd997d21 | 36 | char Alignment_char[4] = {0xFF,0xFE,0xFD,0xFC}; |
narshu | 1:bbabbd997d21 | 37 | int Alignment_ptr = 0; |
narshu | 1:bbabbd997d21 | 38 | bool data_flag = false; |
narshu | 1:bbabbd997d21 | 39 | int buff_pointer = 0; |
narshu | 1:bbabbd997d21 | 40 | bool angleInit = false; |
narshu | 1:bbabbd997d21 | 41 | float angleOffset = 0; |
narshu | 1:bbabbd997d21 | 42 | |
narshu | 1:bbabbd997d21 | 43 | void vIRValueISR (void); |
narshu | 1:bbabbd997d21 | 44 | void vKalmanInit(void); |
narshu | 1:bbabbd997d21 | 45 | |
narshu | 0:f3bf6c7e2283 | 46 | //TODO mutex on kalman state, and on motor commands (i.e. on the i2c bus) |
narshu | 0:f3bf6c7e2283 | 47 | //NOTE! Recieving data with RF12B now DISABLED due to interferance with rtos! |
narshu | 0:f3bf6c7e2283 | 48 | |
narshu | 1:bbabbd997d21 | 49 | |
narshu | 0:f3bf6c7e2283 | 50 | void vMotorThread(void const *argument); |
narshu | 0:f3bf6c7e2283 | 51 | void vPrintState(void const *argument); |
narshu | 0:f3bf6c7e2283 | 52 | void ai_thread (void const *argument); |
narshu | 0:f3bf6c7e2283 | 53 | void motion_thread(void const *argument); |
narshu | 1:bbabbd997d21 | 54 | |
narshu | 0:f3bf6c7e2283 | 55 | |
narshu | 0:f3bf6c7e2283 | 56 | float getAngle (float x, float y); |
narshu | 0:f3bf6c7e2283 | 57 | void getIRValue(void const *argument); |
narshu | 0:f3bf6c7e2283 | 58 | |
narshu | 1:bbabbd997d21 | 59 | // Thread pointers |
narshu | 1:bbabbd997d21 | 60 | Thread *AI_Thread_Ptr; |
narshu | 1:bbabbd997d21 | 61 | Thread *Motion_Thread_Ptr; |
narshu | 0:f3bf6c7e2283 | 62 | |
narshu | 0:f3bf6c7e2283 | 63 | Mutex targetlock; |
narshu | 0:f3bf6c7e2283 | 64 | bool flag_terminate = false; |
narshu | 0:f3bf6c7e2283 | 65 | |
narshu | 0:f3bf6c7e2283 | 66 | float temp = 0; |
narshu | 0:f3bf6c7e2283 | 67 | |
narshu | 0:f3bf6c7e2283 | 68 | //Main loop |
narshu | 0:f3bf6c7e2283 | 69 | int main() { |
narshu | 0:f3bf6c7e2283 | 70 | pc.baud(115200); |
narshu | 1:bbabbd997d21 | 71 | IRturret.baud(115200); |
narshu | 0:f3bf6c7e2283 | 72 | IRturret.format(8,Serial::Odd,1); |
narshu | 1:bbabbd997d21 | 73 | //IRturret.attach(&vIRValueISR,Serial::RxIrq); |
narshu | 1:bbabbd997d21 | 74 | //vKalmanInit(); |
narshu | 1:bbabbd997d21 | 75 | |
narshu | 1:bbabbd997d21 | 76 | Thread tMotorThread(vMotorThread,NULL,osPriorityNormal,256); |
narshu | 0:f3bf6c7e2283 | 77 | Thread tUpdateState(vPrintState,NULL,osPriorityNormal,1024); |
narshu | 0:f3bf6c7e2283 | 78 | |
narshu | 1:bbabbd997d21 | 79 | //Thread thr_AI(ai_thread,NULL,osPriorityNormal,1024); |
narshu | 1:bbabbd997d21 | 80 | //Thread thr_motion(motion_thread,NULL,osPriorityNormal,1024); |
narshu | 1:bbabbd997d21 | 81 | //AI_Thread_Ptr = &thr_AI; |
narshu | 1:bbabbd997d21 | 82 | //Motion_Thread_Ptr = &thr_motion; |
narshu | 1:bbabbd997d21 | 83 | |
narshu | 1:bbabbd997d21 | 84 | //measure cpu usage. output updated once per second to symbol cpupercent |
narshu | 1:bbabbd997d21 | 85 | //Thread mCPUthread(measureCPUidle, NULL, osPriorityIdle, 1024); //check if stack overflow with such a small staack |
narshu | 1:bbabbd997d21 | 86 | |
narshu | 1:bbabbd997d21 | 87 | |
narshu | 0:f3bf6c7e2283 | 88 | pc.printf("We got to main! ;D\r\n"); |
narshu | 0:f3bf6c7e2283 | 89 | |
narshu | 0:f3bf6c7e2283 | 90 | //REMEMBERT TO PUT PULL UP RESISTORS ON I2C!!!!!!!!!!!!!! |
narshu | 0:f3bf6c7e2283 | 91 | while (1) { |
narshu | 0:f3bf6c7e2283 | 92 | // do nothing |
narshu | 0:f3bf6c7e2283 | 93 | Thread::wait(osWaitForever); |
narshu | 0:f3bf6c7e2283 | 94 | } |
narshu | 0:f3bf6c7e2283 | 95 | } |
narshu | 0:f3bf6c7e2283 | 96 | |
narshu | 0:f3bf6c7e2283 | 97 | |
narshu | 0:f3bf6c7e2283 | 98 | void vMotorThread(void const *argument) { |
narshu | 0:f3bf6c7e2283 | 99 | motors.resetEncoders(); |
narshu | 0:f3bf6c7e2283 | 100 | while (1) { |
narshu | 1:bbabbd997d21 | 101 | motors.setSpeed(20,20); |
narshu | 1:bbabbd997d21 | 102 | Thread::wait(2000); |
narshu | 1:bbabbd997d21 | 103 | motors.stop(); |
narshu | 1:bbabbd997d21 | 104 | Thread::wait(5000); |
narshu | 1:bbabbd997d21 | 105 | motors.setSpeed(-20,-20); |
narshu | 0:f3bf6c7e2283 | 106 | Thread::wait(2000); |
narshu | 0:f3bf6c7e2283 | 107 | motors.stop(); |
narshu | 0:f3bf6c7e2283 | 108 | Thread::wait(5000); |
narshu | 0:f3bf6c7e2283 | 109 | motors.setSpeed(-20,20); |
narshu | 0:f3bf6c7e2283 | 110 | Thread::wait(2000); |
narshu | 0:f3bf6c7e2283 | 111 | motors.stop(); |
narshu | 0:f3bf6c7e2283 | 112 | Thread::wait(5000); |
narshu | 1:bbabbd997d21 | 113 | motors.setSpeed(20,-20); |
narshu | 1:bbabbd997d21 | 114 | Thread::wait(2000); |
narshu | 1:bbabbd997d21 | 115 | motors.stop(); |
narshu | 1:bbabbd997d21 | 116 | Thread::wait(5000); |
narshu | 0:f3bf6c7e2283 | 117 | } |
narshu | 0:f3bf6c7e2283 | 118 | } |
narshu | 0:f3bf6c7e2283 | 119 | |
narshu | 0:f3bf6c7e2283 | 120 | |
narshu | 0:f3bf6c7e2283 | 121 | |
narshu | 0:f3bf6c7e2283 | 122 | void vPrintState(void const *argument) { |
narshu | 0:f3bf6c7e2283 | 123 | float state[3]; |
narshu | 1:bbabbd997d21 | 124 | float SonarMeasures[3]; |
narshu | 1:bbabbd997d21 | 125 | float IRMeasures[3]; |
narshu | 1:bbabbd997d21 | 126 | |
narshu | 0:f3bf6c7e2283 | 127 | |
narshu | 0:f3bf6c7e2283 | 128 | while (1) { |
narshu | 0:f3bf6c7e2283 | 129 | kalman.statelock.lock(); |
narshu | 0:f3bf6c7e2283 | 130 | state[0] = kalman.X(0); |
narshu | 0:f3bf6c7e2283 | 131 | state[1] = kalman.X(1); |
narshu | 0:f3bf6c7e2283 | 132 | state[2] = kalman.X(2); |
narshu | 1:bbabbd997d21 | 133 | SonarMeasures[0] = kalman.SonarMeasures[0]; |
narshu | 1:bbabbd997d21 | 134 | SonarMeasures[1] = kalman.SonarMeasures[1]; |
narshu | 1:bbabbd997d21 | 135 | SonarMeasures[2] = kalman.SonarMeasures[2]; |
narshu | 1:bbabbd997d21 | 136 | IRMeasures[0] = kalman.IRMeasures[0]; |
narshu | 1:bbabbd997d21 | 137 | IRMeasures[1] = kalman.IRMeasures[1]; |
narshu | 1:bbabbd997d21 | 138 | IRMeasures[2] = kalman.IRMeasures[2]; |
narshu | 0:f3bf6c7e2283 | 139 | kalman.statelock.unlock(); |
narshu | 0:f3bf6c7e2283 | 140 | pc.printf("\r\n"); |
narshu | 0:f3bf6c7e2283 | 141 | pc.printf("current: %0.4f %0.4f %0.4f \r\n", state[0], state[1],state[2]); |
narshu | 1:bbabbd997d21 | 142 | pc.printf("Sonar: %0.4f %0.4f %0.4f \r\n",SonarMeasures[0],SonarMeasures[1],SonarMeasures[2]); |
narshu | 1:bbabbd997d21 | 143 | 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 | 144 | pc.printf("Angle_Offset: %0.4f \r\n",angleOffset*180/PI); |
narshu | 1:bbabbd997d21 | 145 | Thread::wait(100); |
narshu | 0:f3bf6c7e2283 | 146 | } |
narshu | 0:f3bf6c7e2283 | 147 | } |
narshu | 0:f3bf6c7e2283 | 148 | |
narshu | 0:f3bf6c7e2283 | 149 | |
narshu | 0:f3bf6c7e2283 | 150 | // AI thread ------------------------------------ |
narshu | 0:f3bf6c7e2283 | 151 | void ai_thread (void const *argument) { |
narshu | 0:f3bf6c7e2283 | 152 | // goes to the mid |
narshu | 0:f3bf6c7e2283 | 153 | Thread::signal_wait(0x01); |
narshu | 0:f3bf6c7e2283 | 154 | targetlock.lock(); |
narshu | 0:f3bf6c7e2283 | 155 | targetX = 1500; |
narshu | 0:f3bf6c7e2283 | 156 | targetY = 1000; |
narshu | 0:f3bf6c7e2283 | 157 | targetTheta = PI/2; |
narshu | 0:f3bf6c7e2283 | 158 | targetlock.unlock(); |
narshu | 0:f3bf6c7e2283 | 159 | |
narshu | 0:f3bf6c7e2283 | 160 | // left roll |
narshu | 0:f3bf6c7e2283 | 161 | Thread::signal_wait(0x01); |
narshu | 0:f3bf6c7e2283 | 162 | targetlock.lock(); |
narshu | 0:f3bf6c7e2283 | 163 | targetX = 500; |
narshu | 0:f3bf6c7e2283 | 164 | targetY = 1700; |
narshu | 0:f3bf6c7e2283 | 165 | targetTheta = PI/2; |
narshu | 0:f3bf6c7e2283 | 166 | targetlock.unlock(); |
narshu | 0:f3bf6c7e2283 | 167 | |
narshu | 0:f3bf6c7e2283 | 168 | // mid |
narshu | 0:f3bf6c7e2283 | 169 | Thread::signal_wait(0x01); |
narshu | 0:f3bf6c7e2283 | 170 | targetlock.lock(); |
narshu | 0:f3bf6c7e2283 | 171 | targetX = 1500; |
narshu | 0:f3bf6c7e2283 | 172 | targetY = 1000; |
narshu | 0:f3bf6c7e2283 | 173 | targetTheta = PI/2; |
narshu | 0:f3bf6c7e2283 | 174 | targetlock.unlock(); |
narshu | 0:f3bf6c7e2283 | 175 | |
narshu | 0:f3bf6c7e2283 | 176 | // map |
narshu | 0:f3bf6c7e2283 | 177 | Thread::signal_wait(0x01); |
narshu | 0:f3bf6c7e2283 | 178 | targetlock.lock(); |
narshu | 0:f3bf6c7e2283 | 179 | targetX = 1500; |
narshu | 0:f3bf6c7e2283 | 180 | targetY = 1700; |
narshu | 0:f3bf6c7e2283 | 181 | targetTheta = PI/2; |
narshu | 0:f3bf6c7e2283 | 182 | targetlock.unlock(); |
narshu | 0:f3bf6c7e2283 | 183 | |
narshu | 0:f3bf6c7e2283 | 184 | // mid |
narshu | 0:f3bf6c7e2283 | 185 | Thread::signal_wait(0x01); |
narshu | 0:f3bf6c7e2283 | 186 | targetlock.lock(); |
narshu | 0:f3bf6c7e2283 | 187 | targetX = 1500; |
narshu | 0:f3bf6c7e2283 | 188 | targetY = 1000; |
narshu | 1:bbabbd997d21 | 189 | targetTheta = -PI/2; |
narshu | 0:f3bf6c7e2283 | 190 | targetlock.unlock(); |
narshu | 0:f3bf6c7e2283 | 191 | |
narshu | 0:f3bf6c7e2283 | 192 | // home |
narshu | 0:f3bf6c7e2283 | 193 | Thread::signal_wait(0x01); |
narshu | 0:f3bf6c7e2283 | 194 | targetlock.lock(); |
narshu | 0:f3bf6c7e2283 | 195 | targetX = 500; |
narshu | 0:f3bf6c7e2283 | 196 | targetY = 500; |
narshu | 0:f3bf6c7e2283 | 197 | targetTheta = 0; |
narshu | 0:f3bf6c7e2283 | 198 | targetlock.unlock(); |
narshu | 0:f3bf6c7e2283 | 199 | |
narshu | 0:f3bf6c7e2283 | 200 | Thread::signal_wait(0x01); |
narshu | 0:f3bf6c7e2283 | 201 | flag_terminate = true; |
narshu | 0:f3bf6c7e2283 | 202 | //OLED3 = true; |
narshu | 0:f3bf6c7e2283 | 203 | |
narshu | 0:f3bf6c7e2283 | 204 | while (true) { |
narshu | 0:f3bf6c7e2283 | 205 | Thread::wait(osWaitForever); |
narshu | 0:f3bf6c7e2283 | 206 | } |
narshu | 0:f3bf6c7e2283 | 207 | } |
narshu | 0:f3bf6c7e2283 | 208 | |
narshu | 0:f3bf6c7e2283 | 209 | // motion control thread ------------------------ |
narshu | 0:f3bf6c7e2283 | 210 | void motion_thread(void const *argument) { |
narshu | 0:f3bf6c7e2283 | 211 | motors.resetEncoders(); |
narshu | 1:bbabbd997d21 | 212 | motors.setSpeed(MOVE_SPEED/2,MOVE_SPEED/2); |
narshu | 1:bbabbd997d21 | 213 | Thread::wait(1000); |
narshu | 0:f3bf6c7e2283 | 214 | motors.stop(); |
narshu | 1:bbabbd997d21 | 215 | (*AI_Thread_Ptr).signal_set(0x01); |
narshu | 0:f3bf6c7e2283 | 216 | |
narshu | 0:f3bf6c7e2283 | 217 | |
narshu | 0:f3bf6c7e2283 | 218 | |
narshu | 0:f3bf6c7e2283 | 219 | float currX, currY,currTheta; |
narshu | 0:f3bf6c7e2283 | 220 | float speedL,speedR; |
narshu | 0:f3bf6c7e2283 | 221 | float diffDir,diffSpeed; |
narshu | 0:f3bf6c7e2283 | 222 | float lastdiffSpeed = 0; |
narshu | 0:f3bf6c7e2283 | 223 | |
narshu | 0:f3bf6c7e2283 | 224 | while (1) { |
narshu | 0:f3bf6c7e2283 | 225 | if (flag_terminate) { |
narshu | 0:f3bf6c7e2283 | 226 | terminate(); |
narshu | 0:f3bf6c7e2283 | 227 | } |
narshu | 0:f3bf6c7e2283 | 228 | |
narshu | 0:f3bf6c7e2283 | 229 | // get kalman localization estimate ------------------------ |
narshu | 0:f3bf6c7e2283 | 230 | kalman.statelock.lock(); |
narshu | 0:f3bf6c7e2283 | 231 | currX = kalman.X(0)*1000.0f; |
narshu | 0:f3bf6c7e2283 | 232 | currY = kalman.X(1)*1000.0f; |
narshu | 0:f3bf6c7e2283 | 233 | currTheta = kalman.X(2); |
narshu | 0:f3bf6c7e2283 | 234 | kalman.statelock.unlock(); |
narshu | 0:f3bf6c7e2283 | 235 | |
narshu | 0:f3bf6c7e2283 | 236 | |
narshu | 0:f3bf6c7e2283 | 237 | // check if target reached ---------------------------------- |
narshu | 0:f3bf6c7e2283 | 238 | if ( ( abs(currX - targetX) < POSITION_TOR ) |
narshu | 0:f3bf6c7e2283 | 239 | &&( abs(currY - targetY) < POSITION_TOR ) |
narshu | 0:f3bf6c7e2283 | 240 | ) { |
narshu | 0:f3bf6c7e2283 | 241 | |
narshu | 0:f3bf6c7e2283 | 242 | diffDir = rectifyAng(currTheta - targetTheta); |
narshu | 0:f3bf6c7e2283 | 243 | diffSpeed = diffDir / PI; |
narshu | 0:f3bf6c7e2283 | 244 | |
narshu | 0:f3bf6c7e2283 | 245 | if (abs(diffDir) > ANGLE_TOR) { |
narshu | 0:f3bf6c7e2283 | 246 | if (abs(diffSpeed) < 0.5) { |
narshu | 0:f3bf6c7e2283 | 247 | diffSpeed = 0.5*diffSpeed/abs(diffSpeed); |
narshu | 0:f3bf6c7e2283 | 248 | } |
narshu | 0:f3bf6c7e2283 | 249 | motors.setSpeed( int(diffSpeed*MOVE_SPEED), -int(diffSpeed*MOVE_SPEED)); |
narshu | 0:f3bf6c7e2283 | 250 | |
narshu | 0:f3bf6c7e2283 | 251 | |
narshu | 0:f3bf6c7e2283 | 252 | } else { |
narshu | 0:f3bf6c7e2283 | 253 | motors.stop(); |
narshu | 0:f3bf6c7e2283 | 254 | Thread::wait(4000); |
narshu | 1:bbabbd997d21 | 255 | (*AI_Thread_Ptr).signal_set(0x01); |
narshu | 0:f3bf6c7e2283 | 256 | } |
narshu | 0:f3bf6c7e2283 | 257 | } |
narshu | 0:f3bf6c7e2283 | 258 | |
narshu | 0:f3bf6c7e2283 | 259 | // adjust motion to reach target ---------------------------- |
narshu | 0:f3bf6c7e2283 | 260 | else { |
narshu | 0:f3bf6c7e2283 | 261 | |
narshu | 0:f3bf6c7e2283 | 262 | // calc direction to target |
narshu | 0:f3bf6c7e2283 | 263 | float targetDir = atan2(targetY - currY, targetX - currX); |
narshu | 0:f3bf6c7e2283 | 264 | |
narshu | 0:f3bf6c7e2283 | 265 | |
narshu | 0:f3bf6c7e2283 | 266 | diffDir = rectifyAng(currTheta - targetDir); |
narshu | 0:f3bf6c7e2283 | 267 | diffSpeed = diffDir / PI; |
narshu | 0:f3bf6c7e2283 | 268 | |
narshu | 0:f3bf6c7e2283 | 269 | if (abs(diffDir) > ANGLE_TOR*2) { |
narshu | 0:f3bf6c7e2283 | 270 | if (abs(diffSpeed) < 0.5) { |
narshu | 0:f3bf6c7e2283 | 271 | diffSpeed = 0.5*diffSpeed/abs(diffSpeed); |
narshu | 0:f3bf6c7e2283 | 272 | } |
narshu | 0:f3bf6c7e2283 | 273 | motors.setSpeed( int(diffSpeed*MOVE_SPEED), -int(diffSpeed*MOVE_SPEED)); |
narshu | 0:f3bf6c7e2283 | 274 | } else { |
narshu | 0:f3bf6c7e2283 | 275 | |
narshu | 0:f3bf6c7e2283 | 276 | |
narshu | 0:f3bf6c7e2283 | 277 | if (abs(diffSpeed-lastdiffSpeed) > MAX_STEP_RATIO ) { |
narshu | 0:f3bf6c7e2283 | 278 | if (diffSpeed-lastdiffSpeed >= 0) { |
narshu | 0:f3bf6c7e2283 | 279 | diffSpeed = lastdiffSpeed + MAX_STEP_RATIO; |
narshu | 0:f3bf6c7e2283 | 280 | } else { |
narshu | 0:f3bf6c7e2283 | 281 | diffSpeed = lastdiffSpeed - MAX_STEP_RATIO; |
narshu | 0:f3bf6c7e2283 | 282 | } |
narshu | 0:f3bf6c7e2283 | 283 | } |
narshu | 0:f3bf6c7e2283 | 284 | lastdiffSpeed = diffSpeed; |
narshu | 0:f3bf6c7e2283 | 285 | |
narshu | 0:f3bf6c7e2283 | 286 | // calculte the motor speeds |
narshu | 0:f3bf6c7e2283 | 287 | if (diffSpeed <= 0) { |
narshu | 0:f3bf6c7e2283 | 288 | speedL = (1-2*abs(diffSpeed))*MOVE_SPEED; |
narshu | 0:f3bf6c7e2283 | 289 | speedR = MOVE_SPEED; |
narshu | 0:f3bf6c7e2283 | 290 | |
narshu | 0:f3bf6c7e2283 | 291 | } else { |
narshu | 0:f3bf6c7e2283 | 292 | speedR = (1-2*abs(diffSpeed))*MOVE_SPEED; |
narshu | 0:f3bf6c7e2283 | 293 | speedL = MOVE_SPEED; |
narshu | 0:f3bf6c7e2283 | 294 | } |
narshu | 0:f3bf6c7e2283 | 295 | |
narshu | 0:f3bf6c7e2283 | 296 | motors.setSpeed( int(speedL), int(speedR)); |
narshu | 0:f3bf6c7e2283 | 297 | } |
narshu | 0:f3bf6c7e2283 | 298 | } |
narshu | 0:f3bf6c7e2283 | 299 | |
narshu | 0:f3bf6c7e2283 | 300 | // wait |
narshu | 0:f3bf6c7e2283 | 301 | Thread::wait(MOTION_UPDATE_PERIOD); |
narshu | 0:f3bf6c7e2283 | 302 | } |
narshu | 0:f3bf6c7e2283 | 303 | } |
narshu | 0:f3bf6c7e2283 | 304 | |
narshu | 1:bbabbd997d21 | 305 | void vIRValueISR (void) { |
narshu | 0:f3bf6c7e2283 | 306 | |
narshu | 1:bbabbd997d21 | 307 | OLED3 = !OLED3; |
narshu | 1:bbabbd997d21 | 308 | // A workaround for mbed UART ISR bug |
narshu | 1:bbabbd997d21 | 309 | // Clear the RBR flag to make sure the interrupt doesn't loop |
narshu | 1:bbabbd997d21 | 310 | // UART3 for the port on pins 9/10, UART2 for pins 28/27, and UART1 for pins 13/14. |
narshu | 1:bbabbd997d21 | 311 | // UART0 for USB UART |
narshu | 1:bbabbd997d21 | 312 | unsigned char RBR = LPC_UART1->RBR; |
narshu | 1:bbabbd997d21 | 313 | |
narshu | 1:bbabbd997d21 | 314 | |
narshu | 1:bbabbd997d21 | 315 | if (!data_flag) { // look for alignment bytes |
narshu | 1:bbabbd997d21 | 316 | if (RBR == Alignment_char[Alignment_ptr]) { |
narshu | 1:bbabbd997d21 | 317 | Alignment_ptr ++; |
narshu | 0:f3bf6c7e2283 | 318 | } |
narshu | 1:bbabbd997d21 | 319 | if (Alignment_ptr >= 4) { |
narshu | 1:bbabbd997d21 | 320 | Alignment_ptr = 0; |
narshu | 1:bbabbd997d21 | 321 | data_flag = true; // set the dataflag |
narshu | 1:bbabbd997d21 | 322 | } |
narshu | 1:bbabbd997d21 | 323 | } else { // fetch data bytes |
narshu | 1:bbabbd997d21 | 324 | IRValues.IR_chars[buff_pointer] = RBR; |
narshu | 1:bbabbd997d21 | 325 | buff_pointer ++; |
narshu | 1:bbabbd997d21 | 326 | if (buff_pointer >= 12) { |
narshu | 1:bbabbd997d21 | 327 | buff_pointer = 0; |
narshu | 1:bbabbd997d21 | 328 | data_flag = false; // dessert the dataflag |
narshu | 1:bbabbd997d21 | 329 | if (angleInit) { |
narshu | 1:bbabbd997d21 | 330 | kalman.runupdate(Kalman::measurement_t(IRValues.IR_ints[0]+3),rectifyAng(IRValues.IR_floats[1]+angleOffset),IRValues.IR_floats[2]); |
narshu | 1:bbabbd997d21 | 331 | } else { |
narshu | 1:bbabbd997d21 | 332 | kalman.runupdate(Kalman::measurement_t(IRValues.IR_ints[0]+3),IRValues.IR_floats[1],IRValues.IR_floats[2]); |
narshu | 1:bbabbd997d21 | 333 | } |
narshu | 1:bbabbd997d21 | 334 | } |
narshu | 1:bbabbd997d21 | 335 | |
narshu | 0:f3bf6c7e2283 | 336 | } |
narshu | 0:f3bf6c7e2283 | 337 | } |
narshu | 0:f3bf6c7e2283 | 338 | |
narshu | 1:bbabbd997d21 | 339 | void vKalmanInit(void) { |
narshu | 1:bbabbd997d21 | 340 | float SonarMeasures[3]; |
narshu | 1:bbabbd997d21 | 341 | float IRMeasures[3]; |
narshu | 1:bbabbd997d21 | 342 | int beacon_cnt = 0; |
narshu | 1:bbabbd997d21 | 343 | wait(1); |
narshu | 1:bbabbd997d21 | 344 | IRturret.attach(NULL,Serial::RxIrq); |
narshu | 1:bbabbd997d21 | 345 | kalman.statelock.lock(); |
narshu | 1:bbabbd997d21 | 346 | SonarMeasures[0] = kalman.SonarMeasures[0]*1000.0f; |
narshu | 1:bbabbd997d21 | 347 | SonarMeasures[1] = kalman.SonarMeasures[1]*1000.0f; |
narshu | 1:bbabbd997d21 | 348 | SonarMeasures[2] = kalman.SonarMeasures[2]*1000.0f; |
narshu | 1:bbabbd997d21 | 349 | IRMeasures[0] = kalman.IRMeasures[0]; |
narshu | 1:bbabbd997d21 | 350 | IRMeasures[1] = kalman.IRMeasures[1]; |
narshu | 1:bbabbd997d21 | 351 | IRMeasures[2] = kalman.IRMeasures[2]; |
narshu | 1:bbabbd997d21 | 352 | kalman.statelock.unlock(); |
narshu | 1:bbabbd997d21 | 353 | //printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasures[0]*180/PI, IRMeasures[1]*180/PI, IRMeasures[2]*180/PI); |
narshu | 1:bbabbd997d21 | 354 | float d = beaconpos[2].y - beaconpos[1].y; |
narshu | 1:bbabbd997d21 | 355 | float i = beaconpos[0].y - beaconpos[1].y; |
narshu | 1:bbabbd997d21 | 356 | float j = beaconpos[0].x - beaconpos[1].x; |
narshu | 1:bbabbd997d21 | 357 | float y_coor = (SonarMeasures[1]*SonarMeasures[1]- SonarMeasures[2]*SonarMeasures[2] + d*d) / (2*d); |
narshu | 1:bbabbd997d21 | 358 | float x_coor = (SonarMeasures[1]*SonarMeasures[1] - SonarMeasures[0]*SonarMeasures[0] + i*i + j*j)/(2*j) - i*y_coor/j; |
narshu | 1:bbabbd997d21 | 359 | angleOffset = 0; |
narshu | 1:bbabbd997d21 | 360 | for (int i = 0; i < 3; i++) { |
narshu | 1:bbabbd997d21 | 361 | float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor); |
narshu | 1:bbabbd997d21 | 362 | if (IRMeasures[i] != 0){ |
narshu | 1:bbabbd997d21 | 363 | beacon_cnt ++; |
narshu | 1:bbabbd997d21 | 364 | float angle_temp = angle_est - IRMeasures[i]; |
narshu | 1:bbabbd997d21 | 365 | angle_temp -= (floor(angle_temp/(2*PI)))*2*PI; |
narshu | 1:bbabbd997d21 | 366 | angleOffset += angle_temp; |
narshu | 1:bbabbd997d21 | 367 | } |
narshu | 0:f3bf6c7e2283 | 368 | } |
narshu | 1:bbabbd997d21 | 369 | angleOffset = angleOffset/float(beacon_cnt); |
narshu | 1:bbabbd997d21 | 370 | //printf("\n\r"); |
narshu | 1:bbabbd997d21 | 371 | angleInit = true; |
narshu | 1:bbabbd997d21 | 372 | kalman.statelock.lock(); |
narshu | 1:bbabbd997d21 | 373 | kalman.X(0) = x_coor/1000.0f; |
narshu | 1:bbabbd997d21 | 374 | kalman.X(1) = y_coor/1000.0f; |
narshu | 1:bbabbd997d21 | 375 | kalman.X(2) = 0; |
narshu | 1:bbabbd997d21 | 376 | kalman.statelock.unlock(); |
narshu | 1:bbabbd997d21 | 377 | //printf("x: %0.4f, y: %0.4f, offset: %0.4f \n\r", x_coor, y_coor, angleOffset*180/PI); |
narshu | 1:bbabbd997d21 | 378 | IRturret.attach(&vIRValueISR,Serial::RxIrq); |
narshu | 1:bbabbd997d21 | 379 | } |