Revision 32:ada943ecaceb, committed 2013-04-10

Comitter:

madcowswe

Date:

Wed Apr 10 18:04:47 2013 +0000

Parent:

30:00e1493b44f0

Child:

33:e3f633620816

Commit message:

Made calibration more robust

Changed in this revision

--- a/Processes/Kalman/Kalman.cpp	Wed Apr 10 14:34:07 2013 +0000
+++ b/Processes/Kalman/Kalman.cpp	Wed Apr 10 18:04:47 2013 +0000
@@ -29,6 +29,7 @@
 Mutex statelock;
 
 float RawReadings[maxmeasure+1];
+int sensorseenflags = 0;
 float IRphaseOffset;
 
 bool Kalman_inited = 0;
@@ -51,6 +52,9 @@
     
     //WARNING: HARDCODED! TODO: fix it so it works for both sides!
     
+    printf("waiting for all sonar, and at least 1 IR\r\n");
+    while( ((sensorseenflags & 0x7)^0x7) || !(sensorseenflags & 0x7<<3) );
+    
     //solve for our position (assume perfect bias)
     const float d = beaconpos[2].y - beaconpos[1].y;
     const float i = beaconpos[2].y - beaconpos[0].y;
@@ -77,20 +81,36 @@
     IRMeasuresloc[2] = RawReadings[IR2];
     printf("IR 0: %0.4f, 1: %0.4f, 2: %0.4f \r\n", IRMeasuresloc[0]*180/PI, IRMeasuresloc[1]*180/PI, IRMeasuresloc[2]*180/PI);
 
-    float IR_Offsets[3];
-    float fromb0offset = 0;
+    float IR_Offsets[3] = {0};
+    float frombrefoffset = 0;
+    int refbeacon = 0;
+    
+    for (int i = 0; i < 3; i++){
+        if (sensorseenflags & 1<<(3+i)){
+            refbeacon = i;
+            break;
+        }
+    }
+    
+    printf("refbeacon is %d\r\n", refbeacon);
+    
+    int cnt = 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);
-
-        //printf("Angle %d : %f \n\r",i,angle_est*180/PI );
-        IR_Offsets[i] = constrainAngle(IRMeasuresloc[i] - angle_est);
-
-        fromb0offset += constrainAngle(IR_Offsets[i] - IR_Offsets[0]);
+        if (sensorseenflags & 1<<(3+i)){
+            cnt++;
+            
+            //Compute IR offset
+            float angle_est = atan2(beaconpos[i].y - y_coor,beaconpos[i].x - x_coor);
+    
+            //printf("Angle %d : %f \n\r",i,angle_est*180/PI );
+            IR_Offsets[i] = constrainAngle(IRMeasuresloc[i] - angle_est);
+    
+            frombrefoffset += constrainAngle(IR_Offsets[i] - IR_Offsets[refbeacon]);
+        }
     }
 
-    IRphaseOffset = constrainAngle(IR_Offsets[0] + fromb0offset/3);
+    IRphaseOffset = constrainAngle(IR_Offsets[refbeacon] + frombrefoffset/cnt);
 
     //debug
     printf("Offsets IR: %0.4f\r\n",IRphaseOffset*180/PI);
@@ -220,6 +240,8 @@
 
 void runupdate(measurement_t type, float value, float variance)
 {
+    sensorseenflags |= 1<<type;
+
     if (!Kalman_inited) {
         RawReadings[type] = value;
     } else {
@@ -233,7 +255,7 @@
         measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
         if (measured) {
             measured->mtype = type;
-            measured->value = value;
+            measured->value = RawReadings[type];
             measured->variance = variance;
 
             osStatus putret = measureMQ.put(measured);
@@ -343,7 +365,7 @@
                 }
 
                 Matrix<float, 3, 1> PH (P * trans(H));
-                S = (H * PH)(0,0) + variance;
+                S = (H * PH)(0,0) + variance*10; //TODO: temp hack
 
                 if (aborton2stddev && Y*Y > 4 * S) {
                     statelock.unlock();

--- a/Processes/Printing/Printing.cpp	Wed Apr 10 14:34:07 2013 +0000
+++ b/Processes/Printing/Printing.cpp	Wed Apr 10 18:04:47 2013 +0000
@@ -61,11 +61,13 @@
     Serial pc(USBTX, USBRX);
     pc.baud(115200);
     
-    char sync[] = "ABCD";
-    cout.write(sync, 4);
-    cout << std::endl;
+    while(true){
     
-    while(true){
+        //send sync symbol
+        char sync[] = "ABCD";
+        cout.write(sync, 4);
+        cout << std::endl;
+    
         // Send number of packets
         char numtosend = 0;
         for (unsigned int v = newdataflags; v; numtosend++){v &= v - 1;}        

--- a/globals.cpp	Wed Apr 10 14:34:07 2013 +0000
+++ b/globals.cpp	Wed Apr 10 18:04:47 2013 +0000
@@ -2,5 +2,5 @@
 #include "globals.h"
 
 //Store global objects here
-pos beaconpos[] = {{0,1}, {3,0}, {3,2}};
+pos beaconpos[] = {{-0.040,1}, {3.040,-0.040}, {3.040,2.040}};
 Waypoint* AI::current_waypoint;
\ No newline at end of file

--- a/globals.h	Wed Apr 10 14:34:07 2013 +0000
+++ b/globals.h	Wed Apr 10 18:04:47 2013 +0000
@@ -10,13 +10,13 @@
 
 const float ENCODER_M_PER_TICK = 0.00084;
 const float ENCODER_WHEELBASE = 0.068;
-const float TURRET_FWD_PLACEMENT = 0.042;
+const float TURRET_FWD_PLACEMENT = -0.042;
 
 //Robot movement constants
 const float fwdvarperunit = 0.01; //1 std dev = 7% //TODO: measrue this!!
-const float varperang = 0.01; //around 1 degree stddev per 180 turn //TODO: measrue this!!
-const float xyvarpertime = 0.0005; //(very poorly) accounts for hitting things
-const float angvarpertime = 0.001;
+const float varperang = 0.0002; //around 1 degree stddev per 180 turn //TODO: measrue this!!
+const float xyvarpertime = 0;//0.0005; //(very poorly) accounts for hitting things
+const float angvarpertime = 0;//0.001;
 
 const float MOTORCONTROLLER_FILTER_K = 0.5;// TODO: tune this
 const float MOTOR_MAX_POWER = 0.4f;
@@ -60,6 +60,7 @@
 
 const PinName P_SERIAL_RX       = p14;
 const PinName P_DISTANCE_SENSOR = p15;
+const PinName P_FWD_DISTANCE_SENSOR = p16;
 
 const PinName P_COLOR_SENSOR_IN = p20;
 

--- a/main.cpp	Wed Apr 10 14:34:07 2013 +0000
+++ b/main.cpp	Wed Apr 10 18:04:47 2013 +0000
@@ -79,18 +79,17 @@
     Kalman::KalmanInit();
     
     Thread predictthread(Kalman::predictloop, NULL, osPriorityNormal, 2084);//512); //temp 2k
-    
     Kalman::start_predict_ticker(&predictthread);
     
-    Ticker motorcontroltestticker;
-    motorcontroltestticker.attach(MotorControl::motor_control_isr, 0.05);
+    Thread updatethread(Kalman::updateloop, NULL, osPriorityNormal, 2084);
+    
+    //Ticker motorcontroltestticker;
+    //motorcontroltestticker.attach(MotorControl::motor_control_isr, 0.05);
     
     // motion layer periodic callback
     RtosTimer motion_timer(motion::motionlayer, osTimerPeriodic);
     motion_timer.start(50);
 
-   
-    Thread::wait(3500);
     Thread printingThread(Printing::printingloop, NULL, osPriorityLow, 2048);
 
     //measureCPUidle(); //repurpose thread for idle measurement