Revision 1:cbd32e9800fb, committed 2018-04-22

Comitter:

KDrainEE

Date:

Sun Apr 22 13:22:06 2018 +0000

Branch:

test

Parent:

0:8bf709d3440b

Commit message:

branch test

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kalman.h	Sun Apr 22 13:22:06 2018 +0000
@@ -0,0 +1,39 @@
+
+class Kalman{
+    public:
+        void statePriori();
+        void statePostPriori();
+        void kCompute(float observation, float variance, float angle);
+    
+    private:
+        float mean;
+        float covariance;
+        float dt;
+        float PredictionVariance;
+};
+
+float[2] Kalman::statePriori(float speed, float angle){
+    float returns[2];
+    returns[1] = this->mean + this->dt*speed*sin(angle);
+    returns[2] = this->covariance * pow(this->dt*speed*cos(angle),2) + PredictionVariance;
+    return returns;
+}
+
+void Kalman::statePostPriori(float observation, float measVariance, float angle, float covariance_hat, float mean_hat){
+    float h = observation*cos(angle);
+    float s = 1/(pow(h,2)*covariance_hat + measVariance);
+    float k = covariance_hat*h*s;
+    float z = observation*sin(angle);
+    this->mean = mean_hat+k*(z-h*mean_hat);
+    this->covariance = (1-k*h)*covariance_hat;
+}
+
+float Kalman::kCompute(float speed, float turnAngle, float observation[2], float variance[2], float angle[2]){
+   float[2] predictions = statePriori(speed, turnAngle);
+   statePostPriori(observation[0], variance[0], angle[0], predictions[1], predictions[0]);
+   statePostPriori(observation[0], variance[0], angle[0], this->covariance, this->mean);
+   return this->mean;
+}
+   
+   
+

--- a/main.cpp	Wed Apr 18 14:58:02 2018 +0000
+++ b/main.cpp	Sun Apr 22 13:22:06 2018 +0000
@@ -8,9 +8,7 @@
 
 #include "mbed.h"
 #include <iostream>
-#include <cmath>
-
-using std::pow;
+//figuring out how to branch and shit
 
 #define TI 0.001f
 
@@ -18,10 +16,10 @@
 #define MINM 0.0f
 #define MAXM 0.53f
 #define KPM 0.15f //0.1414f
-//#define KI 19.7408f
+#define KI 19.7408f
 
-//#define KPS 2.0E-2f //Original 2.0e-2
-//#define KD 1.0e-4f
+#define KPS 2.0E-2f //Original 2.0e-2
+#define KD 1.0e-4f
 #define SET 0.0f
 #define MINS 0.05f
 #define MAXS 0.1f
@@ -30,6 +28,14 @@
 #define STEER_FREQ 0.02f //50 Hz
 #define STEERUPDATEPERIOD 0.05
 
+AnalogIn cameraIn1(PTB0);
+DigitalOut si(PTA12);
+DigitalOut clk(PTD4);
+
+int sensorVal = 0;
+float PixArray[128];
+int maxVal = 9;
+
 AnalogIn _right(PTB1); //Right sensor
 AnalogIn _left(PTB3); //Left sensor
 AnalogIn speed(PTB2);   //tachometer
@@ -46,6 +52,8 @@
 
 InterruptIn navRt(PTD2);
 InterruptIn navLft(PTD3);
+bool lTrig = false;
+bool rTrig = false;
 
 volatile int rightCount;
 volatile int leftCount;
@@ -55,14 +63,12 @@
 float Setpoint = 0.0;
 float spHolder;
 float errSum = 0.0;
-float Kpm = 0.141;
-
 
 float fbPrev = 0.0f;
 float Kps = 2.0E-2; //0.013 for setpoint = 0.0/0.05
 float Kd = 1.0e-4;
 
-
+float Kpm = 0.141;
 
 inline void applyBrake()
 {  
@@ -156,28 +162,27 @@
 void incL()
 {      
     leftCount++;
+    lTrig = true;
 }
 
 void incR()
 {
     rightCount++;
+    rTrig = true;
 }
 
-void controller()
+void steer()
 {
-    //steering
     float L = _left.read();
     float R = _right.read();
-    float velocity = speed.read();
-//    if (L == 0.0 && R == 0.0)
-//    {
-//        //off track
-//        Setpoint = 0.0;
-//    }           
+    if (L == 0.0 && R == 0.0)
+    {
+        //off track
+        Setpoint = 0.0;
+    }           
     float fb = L - R; 
     float e = SET - fb;
-    Kps = 0.001/Setpoint;
-    if (Kps > 0.03f) Kps = 0.03f;
+    Kps = 
     float Controlleroutput = Kps * e - (Kd * (fb - fbPrev)/TI)+ BIAS;//subtract derivative of error?? 
     if (Controlleroutput > MAXS) Controlleroutput = MAXS;
     else if (Controlleroutput < MINS) Controlleroutput = MINS;
@@ -187,31 +192,32 @@
         servoSig.write(Controlleroutput);
     }
     fbPrev = fb;
-    data[0] = L;
-    data[1] = R;
+    data[0] = _right.read();
+    data[1] = _left.read();
     data[2] = Controlleroutput;
     data[3] = e;
     
-    //driving
-    float error = Setpoint-velocity;
+}
+void drive()
+{
+    float error = Setpoint-speed.read();
     errSum +=(error * TI);
-
-    float Ki = 19.7408;
-    if (Setpoint < 0.15){
-        Ki = 4.9352;
-    } else if(Setpoint >= 0.15 && Setpoint < 0.3){
-        Ki = 9.8704;
-    }     
-    float iTerm = Ki*errSum;
+    float iTerm = KI*errSum;
     if(iTerm > MAXM) iTerm = MAXM;
     if(iTerm < MINM) iTerm = MINM; 
-    float output = Kpm*error + iTerm;
+    float output = KPM*error + iTerm;
     if(output > MAXM) output = MAXM;
     if(output < MINM) output = MINM;        
 
     gateDrive.write(output);
     data[4] = gateDrive.read();
-    data[5] = speed.read();    
+    data[5] = speed.read(); 
+}
+
+void cb()
+{
+    steer();
+    drive();
 }
 
 int main()
@@ -224,16 +230,25 @@
     gateDrive.write(Setpoint);
 
     ctrlTimer.start();
-    control.attach(&controller, TI);
+    control.attach(&cb, TI);
     
     rightCount = 0;
     leftCount = 0; 
-
+    
+  
     navRt.fall(&incR);
     navLft.fall(&incL);
     
     while(1) {
         myled = !myled;
+        if(lTrig){
+            bt.putc('l');
+            lTrig = false;
+        }
+        if(rTrig){
+            bt.putc('r');
+            rTrig = false;
+        }
         //bt.printf("%f ",data[0]);
 //        bt.printf("%f ", data[1]);
 //        bt.printf("%f ", data[2]);

--- a/mbed.bld	Wed Apr 18 14:58:02 2018 +0000
+++ b/mbed.bld	Sun Apr 22 13:22:06 2018 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/7130f322cb7e
\ No newline at end of file
+http://mbed.org/users/mbed_official/code/mbed/builds/994bdf8177cb
\ No newline at end of file