Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

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?

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