Diff: autosteer.h

Revision:

54:2405c62c1049

Parent:

47:d3123bb4f673

Child:

56:456d454d9ced

--- a/autosteer.h	Sat Mar 14 18:27:39 2015 +0000
+++ b/autosteer.h	Sat Mar 14 19:47:37 2015 +0000
@@ -83,37 +83,21 @@
 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];
@@ -122,7 +106,6 @@
         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];
 
@@ -136,21 +119,12 @@
         }
     }
     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;
-
+    double mscale = 0.1 * 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;
+    y = y * mscale;
 
     if ( y < -max ) {
         y = -max;
@@ -158,16 +132,13 @@
         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 {
@@ -175,207 +146,4 @@
     }
 
     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 <string.h>
-#include <math.h>
-#include <stdlib.h>
-
-#ifndef PI
-#define PI 3.14159265359
-#endif
-
-double m_Tcenter[10];
-double mphaseadv[10];
-double morder[10];
-int order;
-double B0[10];
-double B1[10];
-double B2[10];
-double B3[10];
-double A_1[10];
-double A_2[10];
-double A_3[10];
-
-double mx[10];
-double my[10];
-double mz[10];
-int Err_aPort = 0;
-
-double OutputValue = 0;
-double OutputTime = 0;
-double LastOutputValue = 0;
-
-double SpeedN = 1;
-int porder = 2;
-
-std::string itos(int n)
-{
-    const int max_size = std::numeric_limits<int>::digits10 + 1 + 1;
-    char buffer[max_size] = {0};
-    sprintf(buffer, "%d", n);
-    return std::string(buffer);
-}
-
-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,bool steer, 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];
-    mx[N] = distAUTOL * FilterGain;
-
-    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];
-    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]);
-
-    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;
-    if (abs(distAUTOL) > 0.5) mscale = scale * 1.5;
-    if (abs(distAUTOL) > 1.5) mscale = scale * 1.5;
-
-    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) {
-        sRet= Steer( ActiveTime, value);
-    //}
-
-    return (sRet);
-}*/
\ No newline at end of file
+}
\ No newline at end of file