Revision 27:7cb3a21d9a2e, committed 2013-04-10

Comitter:

madcowswe

Date:

Wed Apr 10 03:46:23 2013 +0000

Parent:

26:b16f1045108f

Child:

28:664e81033846

Commit message:

Update loop rewritten, not tested not set to run

Changed in this revision

--- a/Communication/coprocserial.cpp	Wed Apr 10 02:01:51 2013 +0000
+++ b/Communication/coprocserial.cpp	Wed Apr 10 03:46:23 2013 +0000
@@ -5,7 +5,7 @@
 Serial coprocserial(NC, P_SERIAL_RX);
 
 //DigitalOut OLED1(LED1);
-DigitalOut OLED2(LED2);
+DigitalOut OLED3(LED3);
 
 // bytes packing
 typedef union {
@@ -52,7 +52,7 @@
         if (++ctr == 12){
             ctr = 0;
             
-            OLED2 = !OLED2;
+            OLED3 = !OLED3;
             buffprintflag = 1;
             
             //runupdate

--- a/Processes/Kalman/Kalman.cpp	Wed Apr 10 02:01:51 2013 +0000
+++ b/Processes/Kalman/Kalman.cpp	Wed Apr 10 03:46:23 2013 +0000
@@ -21,6 +21,7 @@
 
 DigitalOut OLED4(LED4);
 DigitalOut OLED1(LED1);
+DigitalOut OLED2(LED2);
 
 //State variables
 Matrix<float, 3, 1> X;
@@ -95,7 +96,7 @@
     printf("Offsets IR: %0.4f\r\n",IRphaseOffset*180/PI);
 
     statelock.lock();
-    X(0,0) = x_coor;
+    X(0,0) = x_coor-TURRET_FWD_PLACEMENT; //assume facing east
     X(1,0) = y_coor;
     X(2,0) = 0;
     
@@ -248,29 +249,26 @@
     
 
 }
-/*
+
 void Kalman::updateloop(void const*)
 {
 
     //sonar Y chanels
-    ui.regid(2, 1);
-    ui.regid(3, 1);
-    ui.regid(4, 1);
+    OLED4 = !Printing::registerID(2, 1);
+    OLED4 = !Printing::registerID(3, 1);
+    OLED4 = !Printing::registerID(4, 1);
 
     //IR Y chanels
-    ui.regid(5, 1);
-    ui.regid(6, 1);
-    ui.regid(7, 1);
+    OLED4 = !Printing::registerID(5, 1);
+    OLED4 = !Printing::registerID(6, 1);
+    OLED4 = !Printing::registerID(7, 1);
 
-    measurement_t type;
-    float value,variance,rbx,rby,expecdist,Y;
-    float dhdx,dhdy;
     bool aborton2stddev = false;
 
     Matrix<float, 1, 3> H;
 
-    float S;
-    Matrix<float, 3, 3> I3( identity< Matrix<float, 3, 3> >() );
+    float Y,S;
+    const Matrix<float, 3, 3> I3( identity< Matrix<float, 3, 3> >() );
 
 
     while (1) {
@@ -281,66 +279,67 @@
         if (evt.status == osEventMail) {
 
             measurmentdata &measured = *(measurmentdata*)evt.value.p;
-            type = measured.mtype; //Note, may support more measurment types than sonar in the future!
-            value = measured.value;
-            variance = measured.variance;
+            measurement_t type = measured.mtype; //Note, may support more measurment types than sonar in the future!
+            float value = measured.value;
+            float variance = measured.variance;
 
             // don't forget to free the memory
             measureMQ.free(&measured);
 
             if (type <= maxmeasure) {
 
-                if (type <= SONAR3) {
+                if (type <= SONAR2) {
 
-                    InitLock.lock();
-                    float dist = value / 1000.0f - Sonar_Offset; //converting to m from mm,subtract the offset
-                    InitLock.unlock();
-
+                    float dist = value;
                     int sonarid = type;
                     aborton2stddev = true;
 
                     statelock.lock();
-                    //update the current sonar readings
-                    SonarMeasures[sonarid] = dist;
-
-                    rbx = X(0) - beaconpos[sonarid].x/1000.0f;
-                    rby = X(1) - beaconpos[sonarid].y/1000.0f;
-
-                    expecdist = hypot(rbx, rby);//sqrt(rbx*rbx + rby*rby);
+                    
+                    float fp_ct = TURRET_FWD_PLACEMENT*cos(X(2,0));
+                    float fp_st = TURRET_FWD_PLACEMENT*sin(X(2,0));
+                    
+                    float rbx = X(0,0) + fp_ct - beaconpos[sonarid].x;
+                    float rby = X(1,0) + fp_st - beaconpos[sonarid].y;
+                    
+                    float expecdist = hypot(rbx, rby);//sqrt(rbx*rbx + rby*rby);
                     Y = dist - expecdist;
 
                     //send to ui
-                    ui.updateval(sonarid+2, Y);
+                    Printing::updateval(sonarid+2, Y);
 
-                    dhdx = rbx / expecdist;
-                    dhdy = rby / expecdist;
+                    float r_expecdist = 1.0f/expecdist;
 
-                    H = dhdx, dhdy, 0;
+                    float dhdx = rbx * r_expecdist;
+                    float dhdy = rby * r_expecdist;
+                    float dhdt = (fp_ct*rby - fp_st*rbx) * r_expecdist;
 
-                } else if (type <= IR3) {
+                    H = dhdx, dhdy, dhdt;
 
-                    aborton2stddev = false;
+                } else if (type <= IR2) {
+
+                    aborton2stddev = true;
                     int IRidx = type-3;
 
-                    // subtract the IR offset
-                    InitLock.lock();
-                    value -= IR_Offset;
-                    InitLock.unlock();
+                    statelock.lock();
+                    
+                    float fp_ct = TURRET_FWD_PLACEMENT*cos(X(2,0));
+                    float fp_st = TURRET_FWD_PLACEMENT*sin(X(2,0));
 
-                    statelock.lock();
-                    IRMeasures[IRidx] = value;
+                    float brx = beaconpos[IRidx].x - (X(0,0) + fp_ct);
+                    float bry = beaconpos[IRidx].y - (X(1,0) + fp_st);
 
-                    rbx = X(0) - beaconpos[IRidx].x/1000.0f;
-                    rby = X(1) - beaconpos[IRidx].y/1000.0f;
-
-                    float expecang = atan2(-rby, -rbx) - X(2);
-                    Y = rectifyAng(value - expecang);
+                    float expecang = atan2(bry, brx) - X(2,0); //constrainAngle can be called late
+                    Y = constrainAngle(value - expecang);
 
                     //send to ui
-                    ui.updateval(IRidx + 5, Y);
+                    Printing::updateval(IRidx + 5, Y);
 
-                    float dstsq = rbx*rbx + rby*rby;
-                    H = -rby/dstsq, rbx/dstsq, -1;
+                    float r_dstsq = 1.0f/(brx*brx + bry*bry);
+                    float dhdx = -bry*r_dstsq;
+                    float dhdy = brx*r_dstsq;
+                    float dhdt = fp_ct*dhdy - fp_st*dhdx - 1.0f;
+                    H = dhdx, dhdy, dhdt;
                 }
 
                 Matrix<float, 3, 1> PH (P * trans(H));
@@ -354,8 +353,8 @@
                 Matrix<float, 3, 1> K (PH * (1/S));
 
                 //Updating state
-                X += col(K, 0) * Y;
-                X(2) = rectifyAng(X(2));
+                X += K * Y;
+                X(2,0) = constrainAngle(X(2,0));
 
                 P = (I3 - K * H) * P;
 
@@ -372,6 +371,5 @@
 
 }
 
-*/
 
 } //Kalman Namespace

--- a/Processes/Kalman/Kalman.h	Wed Apr 10 02:01:51 2013 +0000
+++ b/Processes/Kalman/Kalman.h	Wed Apr 10 03:46:23 2013 +0000
@@ -7,6 +7,13 @@
 namespace Kalman
 {
 
+typedef struct State 
+{
+    float x;
+    float y;
+    float theta;
+} State;
+
 //Accessor function to get the state as one consistent struct
 State getState();
 

--- a/Processes/Motion/motion.cpp	Wed Apr 10 02:01:51 2013 +0000
+++ b/Processes/Motion/motion.cpp	Wed Apr 10 03:46:23 2013 +0000
@@ -7,6 +7,7 @@
 
 #include "motion.h"
 #include "supportfuncs.h"
+#include "Kalman.h"
 
 namespace motion
 {
@@ -20,6 +21,7 @@
     Waypoint target_waypoint = *AI::current_waypoint;
     
     // get current state from Kalman
+    using Kalman::State;
     State current_state = Kalman::getState();
     
     float delta_x = target_waypoint.x - current_state.x;

--- a/globals.h	Wed Apr 10 02:01:51 2013 +0000
+++ b/globals.h	Wed Apr 10 03:46:23 2013 +0000
@@ -8,8 +8,8 @@
 
 #define ENABLE_GLOBAL_ENCODERS
 
-const float ENCODER_M_PER_TICK = 0.00084; //TODO: measure this!!
-const float ENCODER_WHEELBASE = 0.068; //TODO: measrue this!!
+const float ENCODER_M_PER_TICK = 0.00084;
+const float ENCODER_WHEELBASE = 0.068;
 const float TURRET_FWD_PLACEMENT = 0.042;
 
 //Robot movement constants
@@ -98,13 +98,6 @@
 
 const float PI = 3.14159265359;
 
-typedef struct State 
-{
-    float x;
-    float y;
-    float theta;
-} State;
-
 typedef struct Waypoint
 {
     float x;