Diff: autosteer.h

Revision:

47:d3123bb4f673

Parent:

41:a26acd346c2f

Child:

54:2405c62c1049

--- a/autosteer.h	Sat Mar 14 01:01:19 2015 +0000
+++ b/autosteer.h	Sat Mar 14 01:28:37 2015 +0000
@@ -1,8 +1,190 @@
+#ifndef PI
+#define PI 3.14159265359
+#endif
+
+double m_Tcenter[5] = {0,0,0,0,0};
+double mphaseadv[5]= {0,0,0,0,0};
+double morder[5]= {0,0,0,0,0};
+int order;
+double B0[5]= {0,0,0,0,0};
+double B1[5]= {0,0,0,0,0};
+double B2[5]= {0,0,0,0,0};
+double B3[5]= {0,0,0,0,0};
+double A_1[5]= {0,0,0,0,0};
+double A_2[5]= {0,0,0,0,0};
+double A_3[5]= {0,0,0,0,0};
+
+double mx[5]= {0,0,0,0,0};
+double my[5]= {0,0,0,0,0};
+double mz[5]= {0,0,0,0,0};
+int Err_aPort = 0;
+
+double OutputValue = 0;
+double OutputTime = 0;
+double LastOutputValue = 0;
+
+double SpeedN = 1;
+int porder = 0;
+
+void SetDigitalFilter(double phaseadv, double _Tcenter, int filternumber)
+{
+    m_Tcenter[filternumber] = _Tcenter;
+    mphaseadv[filternumber] = phaseadv;
+    morder[filternumber] = porder;
+    _Tcenter = _Tcenter / 2 / PI;
+    order = porder;
+
+    double T1T2ratio = (1 + sin(phaseadv * PI / 180)) / (1 - sin(phaseadv * PI / 180));
+    double _T1 = sqrt(T1T2ratio) * _Tcenter;
+    double _T2 =_T1 / T1T2ratio;
+    double _T = 0.2;
+    double _K = (1 + 2 * _T1 / _T);
+    double _L = (1 - 2 * _T1 / _T);
+    double _M = (1 + 2 * _T2 / _T);
+    double _N = (1 - 2 * _T2 / _T);
+//   order = 2;
+    //version 1,
+    switch (order) {
+        case 3:
+            B0[filternumber] = pow(_K, 3) / pow(_M, 3);
+            B1[filternumber] = 3 * pow(_K, 2) * _L / pow(_M, 3);
+            B2[filternumber] = 3 * _K * pow(_L, 2) / pow(_M, 3);
+            B3[filternumber] = pow(_L, 3) / pow(_M, 3);
+
+            A_1[filternumber] = 3 * _N / _M;
+            A_2[filternumber] = 3 * pow(_N, 2) / pow(_M, 2);
+            A_3[filternumber] = pow(_N, 3) / pow(_M, 3);
+            break;
+        case 2:
+            B0[filternumber] = pow(_K, 2) / pow(_M, 2);
+            B1[filternumber] = 2 * _K * _L / pow(_M, 2);
+            B2[filternumber] = pow(_L, 2) / pow(_M, 2);
+            B3[filternumber] = 0;
+
+            A_1[filternumber] = 2 * _N / _M;
+            A_2[filternumber] = pow(_N, 2) / pow(_M, 2);
+            A_3[filternumber] = 0;
+            break;
+        case 1:
+        case 0:
+            B0[filternumber] = _K / _M;
+            B1[filternumber] = _L / _M;
+            B2[filternumber] = 0;
+            B3[filternumber] = 0;
+
+            A_1[filternumber] = _N / _M;
+            A_2[filternumber] = 0;
+            A_3[filternumber] = 0;
+            break;
+    }
+}
+//double d = 0;
+
+string Steer(int ActiveTime,int value)
+{
+    string sRet = "";
+    //f ((Err_aPort == 0)) {
+    //  if (ActiveTime > 300) ActiveTime = 300;
+    if (value < 0) value = 0;
+    if ((value > 255)) value = 255;
+    OutputValue = value;
+    OutputTime = ActiveTime;
+
+    // d =  //= DateTime.Now - autosteer.LastCommunication;
+
+    //no need to send repeated 127=do nothing
+    //if ((OutputValue != 127) || (LastOutputValue != OutputValue)) { // || (d.read()-LastCommunication > 2)) {
+    sRet = "$ASTEER," + itos(OutputValue) + "," + itos(ActiveTime) + "\r\n";
+    LastOutputValue = OutputValue;
+    //  autosteer.Timer1counter = 0;
+    //  autosteer.LastCommunication = DateTime.Now;
+    //}
+//  == }
+    return (sRet);
+}
+
+string ControlSteerFilter(int ActiveTime,  double distAUTOL, double speed, double FilterGain, double min, double max,double SCALE)
+{
+    string sRet = "";
+
+    int N = 3;
+    double y = 0;
+    //  if (B0[0] == 9999.0) {
+    //     SetDigitalFilter(47, 4.2 / 2 / PI, 0);
+    // }
+    //  if (distAUTOL == 5000) distAUTOL = 0; //not set
+//   if (speed < 1) steer=false;
+
+    mx[N - 3] = mx[N - 2];
+    mx[N - 2] = mx[N - 1];
+    mx[N - 1] = mx[N];
+    if ( FilterGain > 0 ) {
+        if ( abs(distAUTOL) > 0 ) {
+            mx[N] = distAUTOL * FilterGain;
+
+
+            my[N] = -A_1[0] * (double)my[N - 1] - A_2[0] * (double)my[N - 2] - A_3[0] * (double)my[N - 3] + B0[0] * (double)mx[N] + B1[0] * (double)mx[N - 1] + B2[0] * (double)mx[N - 2] + B3[0] * (double)mx[N - 3];
+            mz[N] = my[N];
+
+            my[N - 3] = my[N - 2];
+            my[N - 2] = my[N - 1];
+            my[N - 1] = my[N];
+
+            mz[N - 3] = mz[N - 2];
+            mz[N - 2] = mz[N - 1];
+            mz[N - 1] = mz[N];
+        }
+    }
+    double y1 = (double)mz[N]; // y1 used to preserve value of mz[N];        // mz is now the output
+    //pc.printf("%f\r\n",y1);
+
+    //modify scale depending on distance!
+    double mscale = SCALE;
+    //if (abs(distAUTOL) > 0.5) mscale = SCALE * 1.5;
+    //if (abs(distAUTOL) > 1.5) mscale = SCALE * 1.5;
+    mscale = 0.85 * abs(distAUTOL) + SCALE;
+
+    y = (double)(y1 * mscale);      // scale it here   if neccesary
+    // pc.printf("%f\r\n",y);
+
+    // y = (double) (y * 10 / speed) ; //added March12, 10 km/h being most typical speed
+    y = (double)(y * pow((10.0 / speed), SpeedN)); //SpeedN varies the gain factor with speed  n=0 to 1 or more. n=0 should give  y=
+
+    y = y * SCALE;
+
+    if ( y < -max ) {
+        y = -max;
+    } else if ( y > max) {
+        y = max;
+    }
+    y = 127 + y;
+
+    if (y <= 127) y = y - (min / 2.0);
+    if (y >= 127) y = y + (min / 2.0);
+
+    //  y = y + 127;
+    if (y >= 255) y = 255;
+    if (y <= 0) y = 0;
+
+    int value = (int)y;
+
+    if (speed > 1.0 ) {
+        sRet= Steer( ActiveTime, value);
+    } else {
+        sRet = Steer( ActiveTime, 127 );
+    }
+
+    return (sRet);
+}
+
+
+
+
 //Maybe you rather use the routine I currently use in FarmerGPS? It uses a lookahead and simply distance to AB-line.
 //No heading error at all.
 
 //ControlSteerFilter is the main routine. ActiveTime in ms, typically under 200ms, distAUTOL is distance to AB line at lookahead position
-#include "mbed.h"
+/*#include "mbed.h"
 #include <string.h>
 #include <math.h>
 #include <stdlib.h>
@@ -37,7 +219,7 @@
 
 std::string itos(int n)
 {
-    const int max_size = std::numeric_limits<int>::digits10 + 1 /*sign*/ + 1 /*0-terminator*/;
+    const int max_size = std::numeric_limits<int>::digits10 + 1 + 1;
     char buffer[max_size] = {0};
     sprintf(buffer, "%d", n);
     return std::string(buffer);
@@ -164,11 +346,11 @@
     y = y * SCALE;
 //pc.printf("$%f\r\n\0",y0);
 
-  /*  if ( y < -max ) {
-        y = -max;
-    } else if ( y > max) {
-        y = max;
-    }*/
+  //  if ( y < -max ) {
+     //   y = -max;
+   // } else if ( y > max) {
+      //  y = max;
+    //}
 
     y = 127.0 + y;
 
@@ -196,4 +378,4 @@
     //}
 
     return (sRet);
-}
\ No newline at end of file
+}*/
\ No newline at end of file