Agra-GPS / FreePilot_V2-3

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   FreePilot PinDetect mbed-src

Fork of FreePilot_V2-2 by Agra-GPS

autosteer.h@40:a68cc1a1a1e7, 2015-03-06 (annotated)

Committer:
maximbolduc
Date:
Fri Mar 06 16:31:19 2015 +0000
Revision:
40:a68cc1a1a1e7
Parent:
34:c2bc9f9be7ff
Child:
41:a26acd346c2f
farmerGPS autosteer routine

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maximbolduc 40:a68cc1a1a1e7 1 //Maybe you rather use the routine I currently use in FarmerGPS? It uses a lookahead and simply distance to AB-line.
maximbolduc 40:a68cc1a1a1e7 2 //No heading error at all.
maximbolduc 40:a68cc1a1a1e7 3
maximbolduc 40:a68cc1a1a1e7 4 //ControlSteerFilter is the main routine. ActiveTime in ms, typically under 200ms, distAUTOL is distance to AB line at lookahead position
maximbolduc 40:a68cc1a1a1e7 5 #include "mbed.h"
maximbolduc 40:a68cc1a1a1e7 6 #include <string.h>
maximbolduc 40:a68cc1a1a1e7 7 #include <math.h>
maximbolduc 40:a68cc1a1a1e7 8 #include <stdlib.h>
maximbolduc 40:a68cc1a1a1e7 9 #define PI 3.14159265359
maximbolduc 40:a68cc1a1a1e7 10
maximbolduc 40:a68cc1a1a1e7 11
maximbolduc 40:a68cc1a1a1e7 12 double m_Tcenter[10];
maximbolduc 40:a68cc1a1a1e7 13 double mphaseadv[10];
maximbolduc 40:a68cc1a1a1e7 14 double morder[10];
maximbolduc 40:a68cc1a1a1e7 15 int order;
maximbolduc 40:a68cc1a1a1e7 16 double B0[10];
maximbolduc 40:a68cc1a1a1e7 17 double B1[10];
maximbolduc 40:a68cc1a1a1e7 18 double B2[10];
maximbolduc 40:a68cc1a1a1e7 19 double B3[10];
maximbolduc 40:a68cc1a1a1e7 20 double A_1[10];
maximbolduc 40:a68cc1a1a1e7 21 double A_2[10];
maximbolduc 40:a68cc1a1a1e7 22 double A_3[10];
maximbolduc 40:a68cc1a1a1e7 23
maximbolduc 40:a68cc1a1a1e7 24 double mx[10];
maximbolduc 40:a68cc1a1a1e7 25 double my[10];
maximbolduc 40:a68cc1a1a1e7 26 double mz[10];
maximbolduc 40:a68cc1a1a1e7 27 int Err_aPort = 0;
maximbolduc 40:a68cc1a1a1e7 28
maximbolduc 40:a68cc1a1a1e7 29 double OutputValue = 0;
maximbolduc 40:a68cc1a1a1e7 30 double OutputTime = 0;
maximbolduc 40:a68cc1a1a1e7 31 double LastOutputValue = 0;
maximbolduc 40:a68cc1a1a1e7 32
maximbolduc 40:a68cc1a1a1e7 33 double SpeedN = 1;
maximbolduc 40:a68cc1a1a1e7 34
maximbolduc 40:a68cc1a1a1e7 35 std::string itos(int n)
maximbolduc 40:a68cc1a1a1e7 36 {
maximbolduc 40:a68cc1a1a1e7 37 const int max_size = std::numeric_limits<int>::digits10 + 1 /*sign*/ + 1 /*0-terminator*/;
maximbolduc 40:a68cc1a1a1e7 38 char buffer[max_size] = {0};
maximbolduc 40:a68cc1a1a1e7 39 sprintf(buffer, "%d", n);
maximbolduc 40:a68cc1a1a1e7 40 return std::string(buffer);
maximbolduc 40:a68cc1a1a1e7 41 }
maximbolduc 34:c2bc9f9be7ff 42
maximbolduc 40:a68cc1a1a1e7 43 void SetDigitalFilter(int porder, double phaseadv, double _Tcenter, int filternumber)
maximbolduc 40:a68cc1a1a1e7 44 {
maximbolduc 40:a68cc1a1a1e7 45 // try {
maximbolduc 40:a68cc1a1a1e7 46 m_Tcenter[filternumber] = _Tcenter;
maximbolduc 40:a68cc1a1a1e7 47 mphaseadv[filternumber] = phaseadv;
maximbolduc 40:a68cc1a1a1e7 48 morder[filternumber] = porder;
maximbolduc 40:a68cc1a1a1e7 49 _Tcenter = _Tcenter / 2 / PI;
maximbolduc 40:a68cc1a1a1e7 50
maximbolduc 40:a68cc1a1a1e7 51 //alogfile = "AUTO-" + equipment.fieldfilename + ".txt";
maximbolduc 40:a68cc1a1a1e7 52
maximbolduc 40:a68cc1a1a1e7 53 // double phaseadv = 47;
maximbolduc 40:a68cc1a1a1e7 54 // double _Tcenter = 3.6;
maximbolduc 40:a68cc1a1a1e7 55 order = porder;
maximbolduc 40:a68cc1a1a1e7 56 double T1T2ratio = (1 + sin(phaseadv * PI / 180)) / (1 - sin(phaseadv * PI / 180));
maximbolduc 40:a68cc1a1a1e7 57 double _T1 = sqrt(T1T2ratio) * _Tcenter;
maximbolduc 40:a68cc1a1a1e7 58 double _T2 =_T1/T1T2ratio;
maximbolduc 40:a68cc1a1a1e7 59
maximbolduc 40:a68cc1a1a1e7 60 double _T = 0.2;
maximbolduc 40:a68cc1a1a1e7 61 double _K = (1 + 2 * _T1 / _T);
maximbolduc 40:a68cc1a1a1e7 62 double _L = (1 - 2 * _T1 / _T);
maximbolduc 40:a68cc1a1a1e7 63 double _M = (1 + 2 * _T2 / _T);
maximbolduc 40:a68cc1a1a1e7 64 double _N = (1 - 2 * _T2 / _T);
maximbolduc 40:a68cc1a1a1e7 65 // int iFilterCount = 1;
maximbolduc 40:a68cc1a1a1e7 66
maximbolduc 40:a68cc1a1a1e7 67 //version 1,
maximbolduc 40:a68cc1a1a1e7 68 switch (order) {
maximbolduc 40:a68cc1a1a1e7 69 case 3:
maximbolduc 40:a68cc1a1a1e7 70 B0[filternumber] = pow(_K, 3) / pow(_M, 3);
maximbolduc 40:a68cc1a1a1e7 71 B1[filternumber] = 3 * pow(_K, 2) * _L / pow(_M, 3);
maximbolduc 40:a68cc1a1a1e7 72 B2[filternumber] = 3 * _K * pow(_L, 2) / pow(_M, 3);
maximbolduc 40:a68cc1a1a1e7 73 B3[filternumber] = pow(_L, 3) / pow(_M, 3);
maximbolduc 40:a68cc1a1a1e7 74
maximbolduc 40:a68cc1a1a1e7 75 A_1[filternumber] = 3 * _N / _M;
maximbolduc 40:a68cc1a1a1e7 76 A_2[filternumber] = 3 * pow(_N, 2) / pow(_M, 2);
maximbolduc 40:a68cc1a1a1e7 77 A_3[filternumber] = pow(_N, 3) / pow(_M, 3);
maximbolduc 40:a68cc1a1a1e7 78 break;
maximbolduc 40:a68cc1a1a1e7 79 case 2:
maximbolduc 40:a68cc1a1a1e7 80 B0[filternumber] = pow(_K, 2) / pow(_M, 2);
maximbolduc 40:a68cc1a1a1e7 81 B1[filternumber] = 2 * _K * _L / pow(_M, 2);
maximbolduc 40:a68cc1a1a1e7 82 B2[filternumber] = pow(_L, 2) / pow(_M, 2);
maximbolduc 40:a68cc1a1a1e7 83 B3[filternumber] = 0;
maximbolduc 34:c2bc9f9be7ff 84
maximbolduc 40:a68cc1a1a1e7 85 A_1[filternumber] = 2 * _N / _M;
maximbolduc 40:a68cc1a1a1e7 86 A_2[filternumber] = pow(_N, 2) / pow(_M, 2);
maximbolduc 40:a68cc1a1a1e7 87 A_3[filternumber] = 0;
maximbolduc 40:a68cc1a1a1e7 88 break;
maximbolduc 40:a68cc1a1a1e7 89 case 1:
maximbolduc 40:a68cc1a1a1e7 90 case 0:
maximbolduc 40:a68cc1a1a1e7 91 B0[filternumber] = _K / _M;
maximbolduc 40:a68cc1a1a1e7 92 B1[filternumber] = _L / _M;
maximbolduc 40:a68cc1a1a1e7 93 B2[filternumber] = 0;
maximbolduc 40:a68cc1a1a1e7 94 B3[filternumber] = 0;
maximbolduc 40:a68cc1a1a1e7 95
maximbolduc 40:a68cc1a1a1e7 96 A_1[filternumber] = _N / _M;
maximbolduc 40:a68cc1a1a1e7 97 A_2[filternumber] = 0;
maximbolduc 40:a68cc1a1a1e7 98 A_3[filternumber] = 0;
maximbolduc 40:a68cc1a1a1e7 99 break;
maximbolduc 40:a68cc1a1a1e7 100 }
maximbolduc 40:a68cc1a1a1e7 101 }
maximbolduc 40:a68cc1a1a1e7 102 //double d = 0;
maximbolduc 40:a68cc1a1a1e7 103
maximbolduc 40:a68cc1a1a1e7 104 string Steer( int ActiveTime,int value)
maximbolduc 40:a68cc1a1a1e7 105 {
maximbolduc 40:a68cc1a1a1e7 106 string sRet = "";
maximbolduc 40:a68cc1a1a1e7 107 if ((Err_aPort == 0)) {
maximbolduc 40:a68cc1a1a1e7 108 // if (ActiveTime > 300) ActiveTime = 300;
maximbolduc 40:a68cc1a1a1e7 109 if (value < 0) value = 0;
maximbolduc 40:a68cc1a1a1e7 110 if ((value > 255)) value = 255;
maximbolduc 40:a68cc1a1a1e7 111 OutputValue = value;
maximbolduc 40:a68cc1a1a1e7 112 OutputTime = ActiveTime;
maximbolduc 40:a68cc1a1a1e7 113
maximbolduc 40:a68cc1a1a1e7 114 // d = //= DateTime.Now - autosteer.LastCommunication;
maximbolduc 40:a68cc1a1a1e7 115
maximbolduc 40:a68cc1a1a1e7 116 //no need to send repeated 127=do nothing
maximbolduc 40:a68cc1a1a1e7 117 if ((OutputValue != 127) || (LastOutputValue != OutputValue)) { // || (d.read()-LastCommunication > 2)) {
maximbolduc 40:a68cc1a1a1e7 118 sRet = "$ASTEER," + itos(OutputValue) + "," + itos(ActiveTime) + "\r\n";
maximbolduc 40:a68cc1a1a1e7 119 LastOutputValue = OutputValue;
maximbolduc 40:a68cc1a1a1e7 120 // autosteer.Timer1counter = 0;
maximbolduc 40:a68cc1a1a1e7 121 // autosteer.LastCommunication = DateTime.Now;
maximbolduc 40:a68cc1a1a1e7 122 } else {
maximbolduc 40:a68cc1a1a1e7 123 //no data expected
maximbolduc 40:a68cc1a1a1e7 124 }
maximbolduc 40:a68cc1a1a1e7 125 }
maximbolduc 40:a68cc1a1a1e7 126 return (sRet);
maximbolduc 40:a68cc1a1a1e7 127 }
maximbolduc 40:a68cc1a1a1e7 128
maximbolduc 40:a68cc1a1a1e7 129 string ControlSteerFilter( int ActiveTime, double distAUTOL, double lddist, double speed, bool steer, double FilterGain)
maximbolduc 40:a68cc1a1a1e7 130 {
maximbolduc 40:a68cc1a1a1e7 131 string sRet = "";
maximbolduc 34:c2bc9f9be7ff 132
maximbolduc 40:a68cc1a1a1e7 133 int N = 3;
maximbolduc 40:a68cc1a1a1e7 134 float y = 0;
maximbolduc 40:a68cc1a1a1e7 135 if (B0[0] == 9999.0) {
maximbolduc 40:a68cc1a1a1e7 136 SetDigitalFilter(2, 47, 4.2 / 2 / PI,0);
maximbolduc 40:a68cc1a1a1e7 137 }
maximbolduc 40:a68cc1a1a1e7 138 if (distAUTOL == 5000) distAUTOL = 0; //not set
maximbolduc 40:a68cc1a1a1e7 139
maximbolduc 40:a68cc1a1a1e7 140 if (speed < 1) steer=false;
maximbolduc 40:a68cc1a1a1e7 141
maximbolduc 40:a68cc1a1a1e7 142 mx[N - 3] = mx[N - 2];
maximbolduc 40:a68cc1a1a1e7 143 mx[N - 2] = mx[N - 1];
maximbolduc 40:a68cc1a1a1e7 144 mx[N - 1] = mx[N];
maximbolduc 40:a68cc1a1a1e7 145 mx[N] = distAUTOL * FilterGain;
maximbolduc 40:a68cc1a1a1e7 146
maximbolduc 40:a68cc1a1a1e7 147 my[N - 3] = my[N - 2];
maximbolduc 40:a68cc1a1a1e7 148 my[N - 2] = my[N - 1];
maximbolduc 40:a68cc1a1a1e7 149 my[N - 1] = my[N];
maximbolduc 40:a68cc1a1a1e7 150
maximbolduc 40:a68cc1a1a1e7 151 mz[N - 3] = mz[N - 2];
maximbolduc 40:a68cc1a1a1e7 152 mz[N - 2] = mz[N - 1];
maximbolduc 40:a68cc1a1a1e7 153 mz[N - 1] = mz[N];
maximbolduc 40:a68cc1a1a1e7 154
maximbolduc 40:a68cc1a1a1e7 155 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];
maximbolduc 40:a68cc1a1a1e7 156
maximbolduc 40:a68cc1a1a1e7 157 //if (iFilterCount > 1) {
maximbolduc 40:a68cc1a1a1e7 158 // 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];
maximbolduc 40:a68cc1a1a1e7 159 // } else {
maximbolduc 40:a68cc1a1a1e7 160 mz[N] = my[N];
maximbolduc 40:a68cc1a1a1e7 161 // }
maximbolduc 40:a68cc1a1a1e7 162
maximbolduc 40:a68cc1a1a1e7 163 //done calculating, shift array values. now done at loop start
maximbolduc 40:a68cc1a1a1e7 164 float y1 = (float)mz[N]; // y1 used to preserve value of mz[N]; // mz is now the output
maximbolduc 40:a68cc1a1a1e7 165 // if (morder[1] == 0) y1 = (float)my[N];
maximbolduc 40:a68cc1a1a1e7 166
maximbolduc 40:a68cc1a1a1e7 167 //modify scale depending on distance!
maximbolduc 40:a68cc1a1a1e7 168 double mscale = scale;
maximbolduc 40:a68cc1a1a1e7 169 if (abs(distAUTOL) > 0.5) mscale = scale * 1.5;
maximbolduc 40:a68cc1a1a1e7 170 if (abs(distAUTOL) > 1.5) mscale = scale * 1.5;
maximbolduc 40:a68cc1a1a1e7 171
maximbolduc 40:a68cc1a1a1e7 172 y = (float)(y1 * mscale); // scale it here if neccesary
maximbolduc 40:a68cc1a1a1e7 173 // y = (float) (y * 10 / speed) ; //added March12, 10 km/h being most typical speed
maximbolduc 40:a68cc1a1a1e7 174 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=
maximbolduc 40:a68cc1a1a1e7 175 y = 127 + y;
maximbolduc 40:a68cc1a1a1e7 176 if (y >= 255) y = 255;
maximbolduc 40:a68cc1a1a1e7 177 if (y <= 0) y = 0;
maximbolduc 40:a68cc1a1a1e7 178 int value = (int)y;
maximbolduc 40:a68cc1a1a1e7 179 if (steer) {
maximbolduc 40:a68cc1a1a1e7 180 sRet= Steer( ActiveTime, value);
maximbolduc 40:a68cc1a1a1e7 181 } else {
maximbolduc 40:a68cc1a1a1e7 182 // autosteer.LastCommunication = DateTime.Now;
maximbolduc 40:a68cc1a1a1e7 183 // return (sRet.c_str());
maximbolduc 40:a68cc1a1a1e7 184 }
maximbolduc 40:a68cc1a1a1e7 185 return (sRet);
maximbolduc 40:a68cc1a1a1e7 186 }