This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Diff: Processes/Kalman/Kalman.cpp
- Revision:
- 17:6263e90bf3ba
- Parent:
- 16:52250d8d8fce
- Child:
- 19:4b993a9a156e
--- a/Processes/Kalman/Kalman.cpp Sun Apr 07 16:50:36 2013 +0000 +++ b/Processes/Kalman/Kalman.cpp Sun Apr 07 17:51:59 2013 +0000 @@ -38,46 +38,44 @@ //Note: this init function assumes that the robot faces east, theta=0, in the +x direction void KalmanInit() { - + //Solving for sonar bias is done by entering the following into wolfram alpha //(a-f)^2 = x^2 + y^2; (b-f)^2 = (x-3)^2 + y^2; (c-f)^2 = (x-1.5)^2+(y-2)^2: solve for x,y,f //where a, b, c are the measured distances, and f is the bias - + SensorOffsets[SONAR0] = sonartimebias; SensorOffsets[SONAR1] = sonartimebias; SensorOffsets[SONAR2] = sonartimebias; - + //solve for our position (assume perfect bias) - float x_coor = - - + const float d = beaconpos[0].y - beaconpos[1].y; + const float i = beaconpos[0].y - beaconpos[2].y; + const float j = beaconpos[0].x - beaconpos[2].x; + + float y_coor = (r1*r1-r2*r2+d*d)/2d; + float x_coor = (r1*r1-r3*r3+i*i+j*j)/(2*j) - (i*y_coor)/j; + //IR float IRMeasuresloc[3]; - IRMeasuresloc[0] = RawReadings[IR0]; IRMeasuresloc[1] = RawReadings[IR1]; IRMeasuresloc[2] = RawReadings[IR2]; //printf("0: %0.4f, 1: %0.4f, 2: %0.4f \n\r", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI); - IR_Offset = 0; + float IR_Offsets[3]; + float fromb0offset = 0; for (int i = 0; i < 3; i++) { //Compute IR offset float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor); - if (!Colour) - angle_est -= PI; + //printf("Angle %d : %f \n\r",i,angle_est*180/PI ); + IR_Offset[i] = constrainAngle(IRMeasuresloc[i] - angle_est); - // take average offset angle from valid readings - if (IRMeasuresloc[i] != 0) { - // changed to current angle - estimated angle - float angle_temp = IRMeasuresloc[i] - angle_est; - angle_temp -= (floor(angle_temp/(2*PI)))*2*PI; - IR_Offset += angle_temp; - } + fromb0offset += IR_Offsets[i] - IR_Offset[0]; } - IR_Offset /= float(beacon_cnt); + //debug printf("Offsets IR: %0.4f, Sonar: %0.4f \r\n",IR_Offset*180/PI,Sonar_Offset*1000 ); @@ -191,9 +189,9 @@ if (!Kalman_init) RawReadings[type] = value; else { - + RawReadings[type] = value - SensorOffsets[type]; - + measurmentdata* measured = (measurmentdata*)measureMQ.alloc(); if (measured) { measured->mtype = type;