Diff: main.cpp

Revision:

41:a26acd346c2f

Parent:

40:a68cc1a1a1e7

Child:

42:854d8cc26bbb

--- a/main.cpp	Fri Mar 06 16:31:19 2015 +0000
+++ b/main.cpp	Fri Mar 06 21:19:23 2015 +0000
@@ -157,127 +157,6 @@
     return yaw_angle;
 }
 
-/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-//PID
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-/*double outMin_ = 0.0;
-double outMax_ = 510.0;
-double inMin_ = 0.0;
-double inMax_ = 10.0;
-double prevControllerOutput_ = 5.0;
-double accError_ = 0.0;
-double prevProcessVariable_ = 5.0;
-double pParam_;
-double iParam_;
-double dParam_;
-double tSample_ = 0.2;
-double Kc_ ;
-double tauR_ ;
-double tauD_ ;
-bool inAuto = true;
-double bias_ = 0.0;
-bool usingFeedForward = true;
-double processVariable_ = 5.0;
-
-void setTunings(float Kc, float tauI, float tauD)
-{
-    //Verify that the tunings make sense.
-    if (Kc == 0.0 || tauI < 0.0 || tauD < 0.0) {
-        return;
-    }
-    //Store raw values to hand back to user on request.
-    pParam_ = Kc;
-    iParam_ = tauI;
-    dParam_ = tauD;
-    float tempTauR;
-    if (tauI == 0.0) {
-        tempTauR = 0.0;
-    } else {
-        tempTauR = (1.0 / tauI) * tSample_;
-    }
-    //For "bumpless transfer" we need to rescale the accumulated error.
-    if (inAuto) {
-        if (tempTauR == 0.0) {
-            accError_ = 0.0;
-        } else {
-            accError_ *= (Kc_ * tauR_) / (Kc * tempTauR);
-        }
-    }
-    Kc_   = Kc;
-    tauR_ = tempTauR;
-    tauD_ = tauD / tSample_;
-
-}
-
-void reset(void)
-{
-    double     outSpan_ = outMax_ - outMin_;
-    double     inSpan_ = inMax_ - inMin_;
-    float scaledBias = 0.0;
-
-    //  if (usingFeedForward) {
-    scaledBias = (bias_ - outMin_) / outSpan_;
-
-    prevControllerOutput_ = scaledBias;
-    prevProcessVariable_  = (processVariable_ - inMin_) / inSpan_;
-
-    //Clear any error in the integral.
-    accError_ = 0;
-}
-
-float compute(double setPoint_)
-{
-    double     outSpan_ = outMax_ - outMin_;
-    double     inSpan_ = inMax_ - inMin_;
-    //Pull in the input and setpoint, and scale them into percent span.
-    float scaledPV = (processVariable_ - inMin_) / inSpan_;
-
-    if (scaledPV > 1.0) {
-        scaledPV = 1.0;
-    } else if (scaledPV < 0.0) {
-        scaledPV = 0.0;
-    }
-
-    float scaledSP = (setPoint_ - inMin_) / inSpan_;
-    if (scaledSP > 1.0) {
-        scaledSP = 1;
-    } else if (scaledSP < 0.0) {
-        scaledSP = 0;
-    }
-    float error = scaledSP - scaledPV;
-
-    //Check and see if the output is pegged at a limit and only
-    //integrate if it is not. This is to prevent reset-windup.
-    if (!(prevControllerOutput_ >= 1 && error > 0) && !(prevControllerOutput_ <= 0 && error < 0)) {
-        accError_ += error;
-    }
-
-    //Compute the current slope of the input signal.
-    float dMeas = (scaledPV - prevProcessVariable_) / tSample_;
-    float scaledBias = 0.0;
-
-    if (usingFeedForward) {
-        scaledBias = (bias_ - outMin_) / outSpan_;
-    }
-
-    //Perform the PID calculation.
-    double controllerOutput_ = scaledBias + Kc_ * (error + (tauR_ * accError_) - (tauD_ * dMeas));
-    //Make sure the computed output is within output constraints.
-    if (controllerOutput_ < 0.0) {
-        controllerOutput_ = 0.0;
-    } else if (controllerOutput_ > 1.0) {
-        controllerOutput_ = 1.0;
-    }
-
-    //Remember this output for the windup check next time.
-    prevControllerOutput_ = controllerOutput_;
-    //Remember the input for the derivative calculation next time.
-    prevProcessVariable_  = scaledPV;
-
-    //Scale the output from percent span back out to a real world number.
-    return ((controllerOutput_ * outSpan_) + outMin_);
-}*/
-
 double get_roll()
 {
     double roll_angle = 0;
@@ -573,20 +452,20 @@
     } else if ( forward_sign == 1 ) {
         error = error;
     }
-  //if ( abs(position_dist) < 0.5 ) {
-        if ( forward_sign == position_sign ) {
-            if ( position_dist > dist_line ) { // && abs(position_dist) <
-               // error = error * (dist_line * filter_g - (position_dist * phase_adv));
-              // error = 0;
-            } else {
-               // error = error * (dist_line * filter_g - (position_dist * phase_adv)*0.8);
-                error = error * dist_line * filter_g;
-            }
-        } else { //
-            error = (error * ((dist_line * filter_g) - (position_dist * phase_adv)))*0.8;//.8 in order to come back less on line than we came on it
+    //if ( abs(position_dist) < 0.5 ) {
+    if ( forward_sign == position_sign ) {
+        if ( position_dist > dist_line ) { // && abs(position_dist) <
+            // error = error * (dist_line * filter_g - (position_dist * phase_adv));
+            // error = 0;
+        } else {
+            // error = error * (dist_line * filter_g - (position_dist * phase_adv)*0.8);
+            error = error * dist_line * filter_g;
         }
+    } else { //
+        error = (error * ((dist_line * filter_g) - (position_dist * phase_adv)))*0.8;//.8 in order to come back less on line than we came on it
+    }
     //} else { //target coming back at 15-20 degrees on the line
-        error = error;// + heading_err  * 2 ;
+    error = error;// + heading_err  * 2 ;
     //}
 
     error = error * scale;
@@ -632,6 +511,201 @@
     /* NOTREACHED */
 }
 
+//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 = 0;
+
+std::string itos(int n)
+{
+    const int max_size = std::numeric_limits<int>::digits10 + 1 /*sign*/ + 1 /*0-terminator*/;
+    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, 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];
+
+    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;
+
+    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;
+    pc.printf("%f\r\n",y);
+
+    //if (steer) {
+        sRet= Steer( ActiveTime, value);
+    //}
+
+    return (sRet);
+}
+
 Point old_position;
 
 //char rmc_cpy[256];
@@ -741,13 +815,18 @@
         looked_ahead.SetX(look_ahead_lat);
         looked_ahead.SetY(look_ahead_lon);
         double filtering = sqrt(111111.0*111111.0 + 111111.0 * cos(decimal_latitude/57.295779513)*111111.0 * cos(decimal_latitude/57.295779513));
-        distance_to_line = dist_Point_to_Line( looked_ahead,line_start,line_end) * isLeft( line_start,line_end,looked_ahead) * filtering;
+        distance_to_line = dist_Point_to_Line( looked_ahead,line_start,line_end) * isLeft( line_start,line_end,looked_ahead) * filtering;/////////////////////////////////////////////////
+             SetDigitalFilter(phaseadv,tcenter, 0 );
 
-        int val = freepilot(position, looked_ahead, diff_angle, distance_to_line, scale,  filterg , phaseadv,tcenter ,filtering);
+        //  int val = freepilot(position, looked_ahead, diff_angle, distance_to_line, scale,  filterg , phaseadv,tcenter ,filtering);
+        string steering = ControlSteerFilter(225, distance_to_line, speed_km , filterg, 20.0, 100.0, scale);
+//pc.printf("%f\r\n",distance_to_line);
+
 
         char command[128];
-        sprintf(command,"$ASTEER,%i,%i\r\n",val,250);    //(int)((((val/2)-127)/scale)+127),500);
+        sprintf(command,"%s\r\n\0",steering.c_str());    //(int)((((val/2)-127)/scale)+127),500);
         pc.puts(command);
+
         process_ASTEER(command);
     }
     string rmc = "";
@@ -916,26 +995,27 @@
     }
     if ( phase && center && scl && avg && ahead ) {
         lookaheadtime = atof(ahead);
-        scale = atof(scl); ///////////////////////////////////////////////////////////////////////////////
+        scale = atof(scl);
         phaseadv = atof(phase);
         avgpos = atof(avg);
         tcenter = atof(center);
         filterg = atof(fg);
 
-        scale = scale * -1;
+        // scale = scale * -1;
+        SetDigitalFilter(phaseadv,tcenter, 0 );
     }
 }
 
 void pc_analyse(char* pc_string)
 {
+    pc.puts(pc_string);
     if (!strncmp(pc_string, "$ASTEER", 7)) {
         //stop sending when already exists
-
     } else if (!strncmp(pc_string, "$BANY",5)) {
         _ID = Config_GetID();
         Config_Save();
     } else if (!strncmp(pc_string, "$GPSBAUD",8)) {
-           string mystring = pc_string;
+        string mystring = pc_string;
         string baud = pc_string;
         if ( mystring.find('*') > 0 ) {
             string baud = mystring.substr(0,mystring.find('*'));
@@ -1214,6 +1294,7 @@
     autosteer_timeout.start();
 
     //setTunings(0.25, 5, 1);
+    SetDigitalFilter(phaseadv,tcenter, 0);
 
     while(1) {
         //JH send version information every 10 seconds to keep Bluetooth alive