Diff: autosteer.h

Revision:

41:a26acd346c2f

Parent:

40:a68cc1a1a1e7

Child:

47:d3123bb4f673

--- a/autosteer.h	Fri Mar 06 16:31:19 2015 +0000
+++ b/autosteer.h	Fri Mar 06 21:19:23 2015 +0000
@@ -6,8 +6,10 @@
 #include <string.h>
 #include <math.h>
 #include <stdlib.h>
+
+#ifndef PI
 #define PI 3.14159265359
-
+#endif
 
 double m_Tcenter[10];
 double mphaseadv[10];
@@ -31,6 +33,7 @@
 double LastOutputValue = 0;
 
 double SpeedN = 1;
+int porder = 2;
 
 std::string itos(int n)
 {
@@ -40,30 +43,23 @@
     return std::string(buffer);
 }
 
-void SetDigitalFilter(int porder, double phaseadv, double _Tcenter, int filternumber)
+void SetDigitalFilter(double phaseadv, double _Tcenter, int filternumber)
 {
-    // try {
     m_Tcenter[filternumber] = _Tcenter;
     mphaseadv[filternumber] = phaseadv;
     morder[filternumber] = porder;
     _Tcenter = _Tcenter / 2 / PI;
-
-    //alogfile = "AUTO-" + equipment.fieldfilename + ".txt";
+    order = porder;
 
-    // double phaseadv = 47;
-    // double _Tcenter = 3.6;
-    order = porder;
     double T1T2ratio = (1 + sin(phaseadv * PI / 180)) / (1 - sin(phaseadv * PI / 180));
     double _T1 = sqrt(T1T2ratio) * _Tcenter;
-    double _T2 =_T1/T1T2ratio;
-
+    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);
-  //  int iFilterCount = 1;
-
+order = 2;
     //version 1,
     switch (order) {
         case 3:
@@ -101,42 +97,39 @@
 }
 //double d = 0;
 
-string Steer( int ActiveTime,int value)
+string Steer(int ActiveTime,int value)
 {
     string sRet = "";
-    if ((Err_aPort == 0)) {
+    //f ((Err_aPort == 0)) {
         //  if (ActiveTime > 300) ActiveTime = 300;
-        if (value < 0) value = 0;
-        if ((value > 255)) value = 255;
+       // 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)) {
+       // 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;
-        } else {
-            //no data expected
         }
-    }
+ //  == }
     return (sRet);
 }
 
-string ControlSteerFilter( int ActiveTime,  double distAUTOL, double lddist, double speed, bool steer, double FilterGain)
+string ControlSteerFilter(int ActiveTime,  double distAUTOL, double speed,bool steer, double FilterGain, double min, double max,double SCALE)
 {
     string sRet = "";
 
     int N = 3;
-    float y = 0;
+    double y = 0;
     if (B0[0] == 9999.0) {
-        SetDigitalFilter(2, 47, 4.2 / 2 / PI,0);
+        SetDigitalFilter(47, 4.2 / 2 / PI, 0);
     }
-    if (distAUTOL == 5000) distAUTOL = 0; //not set
-
+  //  if (distAUTOL == 5000) distAUTOL = 0; //not set
     if (speed < 1) steer=false;
 
     mx[N - 3] = mx[N - 2];
@@ -151,36 +144,56 @@
     mz[N - 3] = mz[N - 2];
     mz[N - 2] = mz[N - 1];
     mz[N - 1] = mz[N];
-
     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];
+pc.printf("$%f\r\n\0",my[N]);
 
-    //if (iFilterCount > 1) {
-   //     mz[N] = -A_1[1] * (double)mz[N - 1] - A_2[1] * (double)mz[N - 2] - A_3[1] * (double)mz[N - 3] + B0[1] * (double)my[N] + B1[1] * (double)my[N - 1] + B2[1] * (double)mz[N - 2] + B3[1] * (double)mz[N - 3];
-  //  } else {
-        mz[N] = my[N];
-  //  }
-
-    //done calculating, shift array values. now done at loop start
-    float y1 = (float)mz[N]; // y1 used to preserve value of mz[N];        // mz is now the output
-    // if (morder[1] == 0) y1 = (float)my[N];
+    double y1 = (double)mz[N]; // y1 used to preserve value of mz[N];        // mz is now the output
+pc.printf("$%f\r\n\0",y1);
 
     //modify scale depending on distance!
-    double mscale = scale;
+    double mscale = SCALE;
     if (abs(distAUTOL) > 0.5) mscale = scale * 1.5;
     if (abs(distAUTOL) > 1.5) mscale = scale * 1.5;
 
-    y = (float)(y1 * mscale);      // scale it here   if neccesary
-    // y = (float) (y * 10 / speed) ; //added March12, 10 km/h being most typical speed
-    y = (float)(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 = 127 + y;
+    y = (double)(y1 * mscale);      // scale it here   if neccesary
+    // 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=
+pc.printf("$%f\r\n\0",y);
+
+    y = y * SCALE;
+//pc.printf("$%f\r\n\0",y0);
+
+  /*  if ( y < -max ) {
+        y = -max;
+    } else if ( y > max) {
+        y = max;
+    }*/
+
+    y = 127.0 + y;
+
+    if (y <= 127.0) y = y - (min / 2.0);
+    if (y >= 127.0) y = y + (min / 2.0);
+
+if ( y < - max)
+{ 
+    y = -max;
+}
+else if ( y > max ) 
+{
+    y=max;
+}
+   // if ( y <= 127 ) y = y - (max / 2);
+   // if ( y >= 127 ) y = y + (max / 2);
+
+
     if (y >= 255) y = 255;
     if (y <= 0) y = 0;
+
     int value = (int)y;
-    if (steer) {
+   // if (steer) {
         sRet= Steer( ActiveTime, value);
-    } else {
-        //   autosteer.LastCommunication = DateTime.Now;
-        //   return (sRet.c_str());
-    }
+    //}
+
     return (sRet);
 }
\ No newline at end of file