Diff: main.cpp

Revision:

14:e55e80c1bd9a

Parent:

13:d1f37c9038de

--- a/main.cpp	Sun Apr 22 21:47:24 2018 +0000
+++ b/main.cpp	Tue Apr 24 00:36:08 2018 +0000
@@ -9,6 +9,7 @@
 #include "mbed.h"
 #include <iostream>
 #include "lsc.h"
+#include "kalman.h"
 
 #define TI 0.001f
 
@@ -66,7 +67,8 @@
 volatile int rightCount;
 volatile int leftCount;
 
-float data[6];
+int data[6];
+float temp;
 
 float Setpoint = 0.0;
 float spHolder;
@@ -234,9 +236,9 @@
     rTrig = true;
 }
 
+Kalman ekf;
 void steer()
 {
-    
     float L = _left.read();
     float R = _right.read();
     if (L == 0.0 && R == 0.0)
@@ -244,9 +246,25 @@
         //off track
         Setpoint = 0.0;
     }           
-//    float fb = L - R; 
-    float fb = (float)camMax/64;
+    float mfb = L - R; 
+    data[0] = mfb;
+    float cfb = (float)camMax/64;
+    data[1] = cfb;
+    data[0] = 64-camMax;
+    //      data[0] = camMax;
+    float observations[2] = {mfb,cfb};
+    float variances[2] = {0.19,1.5e-6};
+    //float angles[2] = {inductorAngles[64-camMax],cameraAngles[64-camMax]};
+    
+    data[2] = angles[0];
+    data[3] = angles[1];
+    //EKF
+    float fb = ekf.kCompute(6.7475*speed.read(),1800*servoSig.read() - 135.0,observations,variances,angles);
+   // float fb = L - R;
+//    temp = L - R;
+    data[4] = fb;
     float e = SET - fb;
+    
     Kps = 0.005/speed.read();
     if (Kps > 0.02) Kps = 0.02;
     Kd = 1.0e-3/speed.read();
@@ -300,15 +318,10 @@
     servoSig.period(STEER_FREQ);
     gateDrive.period(.00005f);
     gateDrive.write(Setpoint);
-   
-
     ctrlTimer.start();
-    control.attach(&cb, TI);
-    
+    control.attach(&cb, TI);   
     rightCount = 0;
-    leftCount = 0; 
-    
-  
+    leftCount = 0;  
     navRt.fall(&incR);
     navLft.fall(&incL);
     
@@ -316,16 +329,8 @@
     stopButton.fall(&stopState);
     waitState();
     while(1) {
-        
-//        if(lTrig){
-//            bt.putc('l');
-//            lTrig = false;
-//        }
-//        if(rTrig){
-//            bt.putc('r');
-//            rTrig = false;
-//        }
-        bt.printf("%i\r\n", camMax);
+        bt.printf("mfb: %f, cfb: %f, angle0: %f, angle1: %f, fb: %f\r\n", data[0], data[1], data[2], data[3], data[4]);
+        //bt.printf("%i, %f \r\n", camMax, temp);
 
     }
 }
\ No newline at end of file