Revision 49:665bdca0f2cd, committed 2013-04-12

Comitter:

madcowswe

Date:

Fri Apr 12 21:07:00 2013 +0000

Parent:

48:254b124cef02

Child:

50:937e860f4621

Commit message:

Kalman online phase estimation works but is SOOO hacked together

Changed in this revision

--- a/Processes/AI/ai.cpp	Fri Apr 12 17:03:53 2013 +0000
+++ b/Processes/AI/ai.cpp	Fri Apr 12 21:07:00 2013 +0000
@@ -23,16 +23,16 @@
 */
 
     current_waypoint[0].x = 0.5;
-    current_waypoint[0].y = 1.85;
+    current_waypoint[0].y = 0.5;
     current_waypoint[0].theta = 0.0;
     current_waypoint[0].pos_threshold = 0.05;
     current_waypoint[0].angle_threshold = 0.05*PI;
 
-    current_waypoint[1].x = 1.2;
-    current_waypoint[1].y = 0.18;
+    current_waypoint[1].x = 2.5;
+    current_waypoint[1].y = 0.5;
     current_waypoint[1].theta = 0;
-    current_waypoint[1].pos_threshold = 0.01;
-    current_waypoint[1].angle_threshold = 0.00001;
+    current_waypoint[1].pos_threshold = 0.05;
+    current_waypoint[1].angle_threshold = 0.05*PI;
 
     current_waypoint[2].x = -999;
 /*
@@ -52,6 +52,8 @@
     secondwp->angle_threshold = 0.00001;
 */    
     motion::setNewWaypoint(current_waypoint);
+    
+    int currwptidx = 0;
     while(1)
     {
         Thread::wait(50);
@@ -60,12 +62,8 @@
         if (motion::checkWaypointStatus())
         {
             
-            if ((current_waypoint+1)->x != -999)
-            { 
-                motion::clearWaypointReached();
-                current_waypoint++;
-                motion::setNewWaypoint(current_waypoint);
-            }
+            motion::setNewWaypoint(&current_waypoint[++currwptidx % 2]);
+            motion::clearWaypointReached();
         }
         motion::waypoint_flag_mutex.unlock();
     }

--- a/Processes/Kalman/Kalman.cpp	Fri Apr 12 17:03:53 2013 +0000
+++ b/Processes/Kalman/Kalman.cpp	Fri Apr 12 21:07:00 2013 +0000
@@ -30,7 +30,7 @@
 Mutex statelock;
 
 float RawReadings[maxmeasure+1];
-int sensorseenflags = 0;
+volatile int sensorseenflags = 0;
 
 bool Kalman_inited = 0;
 
@@ -109,6 +109,8 @@
             frombrefoffset += constrainAngle(IR_Offsets[i] - IR_Offsets[refbeacon]);
         }
     }
+    
+    printf("Used IR info from %d beacons\r\n", cnt);
 
     X(3,0) = constrainAngle(IR_Offsets[refbeacon] + frombrefoffset/cnt);
 
@@ -123,7 +125,7 @@
     P = 0.02*0.02,  0,          0,          0,
         0,          0.02*0.02,  0,          0,
         0,          0,          0.4*0.4,    -0.4*0.4,
-        0,          0,          -0.4*0.4,   0.4*0.4;
+        0,          0,          -0.4*0.4,   0.4*0.4 + 0.05*0.05;
         
     statelock.unlock();
 
@@ -143,8 +145,9 @@
 void predictloop(void const*)
 {
 
-    OLED4 = !Printing::registerID(0, 3);
-    OLED4 = !Printing::registerID(1, 4);
+    OLED4 = !Printing::registerID(0, 3) || OLED4;
+    OLED4 = !Printing::registerID(1, 4) || OLED4;
+    OLED4 = !Printing::registerID(8, 1) || OLED4;
 
     float lastleft = 0;
     float lastright = 0;
@@ -222,6 +225,8 @@
         //Update Printing
         float statecpy[] = {X(0,0), X(1,0), X(2,0)};
         Printing::updateval(0, statecpy, 3);
+        
+        Printing::updateval(8, X(3,0));
 
         float Pcpy[] = {P(0,0), P(0,1), P(1,0), P(1,1)};
         Printing::updateval(1, Pcpy, 4);
@@ -248,6 +253,7 @@
 void runupdate(measurement_t type, float value, float variance)
 {
     sensorseenflags |= 1<<type;
+    RawReadings[type] = value;
 
     if (Kalman_inited) {
         measurmentdata* measured = (measurmentdata*)measureMQ.alloc();
@@ -364,7 +370,7 @@
                 }
 
                 Matrix<float, 4, 1> PHt (P * trans(H));
-                S = (H * PHt)(0,0) + variance;
+                S = (H * PHt)(0,0) + variance*10; //TODO: Temp Hack!
 
                 OLED3 = 0;
                 if (aborton2stddev && Y*Y > 4 * S) {

--- a/globals.h	Fri Apr 12 17:03:53 2013 +0000
+++ b/globals.h	Fri Apr 12 21:07:00 2013 +0000
@@ -13,12 +13,12 @@
 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.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 fwdvarperunit = 0.001; //1 std dev = 7% //TODO: measrue this!!
+const float varperang = 0.0005; //around 3 degree stddev per 180 turn //TODO: measrue this!!
+const float xyvarpertime = 0.0001; //(very poorly) accounts for hitting things
+const float angvarpertime = 0.001;  
 
-const float MOTORCONTROLLER_FILTER_K = 0.8;// TODO: tune this
+const float MOTORCONTROLLER_FILTER_K = 0.5;// TODO: tune this
 const float MOTOR_MAX_POWER = 0.5f;
 
 /*

--- a/main.cpp	Fri Apr 12 17:03:53 2013 +0000
+++ b/main.cpp	Fri Apr 12 21:07:00 2013 +0000
@@ -75,8 +75,8 @@
     
     Thread updatethread(Kalman::updateloop, NULL, osPriorityNormal, 2084);
     
-    //Ticker motorcontroltestticker;
-    //motorcontroltestticker.attach(MotorControl::motor_control_isr, 0.01);
+    Ticker motorcontroltestticker;
+    motorcontroltestticker.attach(MotorControl::motor_control_isr, 0.05);
 
     // ai layer thread
     Thread aithread(AI::ailayer, NULL, osPriorityNormal, 2048);