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@47:d3123bb4f673, 2015-03-14 (annotated)

Committer:
maximbolduc
Date:
Sat Mar 14 01:28:37 2015 +0000
Revision:
47:d3123bb4f673
Parent:
41:a26acd346c2f
Child:
54:2405c62c1049
splitted into classes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maximbolduc 47:d3123bb4f673 1 #ifndef PI
maximbolduc 47:d3123bb4f673 2 #define PI 3.14159265359
maximbolduc 47:d3123bb4f673 3 #endif
maximbolduc 47:d3123bb4f673 4
maximbolduc 47:d3123bb4f673 5 double m_Tcenter[5] = {0,0,0,0,0};
maximbolduc 47:d3123bb4f673 6 double mphaseadv[5]= {0,0,0,0,0};
maximbolduc 47:d3123bb4f673 7 double morder[5]= {0,0,0,0,0};
maximbolduc 47:d3123bb4f673 8 int order;
maximbolduc 47:d3123bb4f673 9 double B0[5]= {0,0,0,0,0};
maximbolduc 47:d3123bb4f673 10 double B1[5]= {0,0,0,0,0};
maximbolduc 47:d3123bb4f673 11 double B2[5]= {0,0,0,0,0};
maximbolduc 47:d3123bb4f673 12 double B3[5]= {0,0,0,0,0};
maximbolduc 47:d3123bb4f673 13 double A_1[5]= {0,0,0,0,0};
maximbolduc 47:d3123bb4f673 14 double A_2[5]= {0,0,0,0,0};
maximbolduc 47:d3123bb4f673 15 double A_3[5]= {0,0,0,0,0};
maximbolduc 47:d3123bb4f673 16
maximbolduc 47:d3123bb4f673 17 double mx[5]= {0,0,0,0,0};
maximbolduc 47:d3123bb4f673 18 double my[5]= {0,0,0,0,0};
maximbolduc 47:d3123bb4f673 19 double mz[5]= {0,0,0,0,0};
maximbolduc 47:d3123bb4f673 20 int Err_aPort = 0;
maximbolduc 47:d3123bb4f673 21
maximbolduc 47:d3123bb4f673 22 double OutputValue = 0;
maximbolduc 47:d3123bb4f673 23 double OutputTime = 0;
maximbolduc 47:d3123bb4f673 24 double LastOutputValue = 0;
maximbolduc 47:d3123bb4f673 25
maximbolduc 47:d3123bb4f673 26 double SpeedN = 1;
maximbolduc 47:d3123bb4f673 27 int porder = 0;
maximbolduc 47:d3123bb4f673 28
maximbolduc 47:d3123bb4f673 29 void SetDigitalFilter(double phaseadv, double _Tcenter, int filternumber)
maximbolduc 47:d3123bb4f673 30 {
maximbolduc 47:d3123bb4f673 31 m_Tcenter[filternumber] = _Tcenter;
maximbolduc 47:d3123bb4f673 32 mphaseadv[filternumber] = phaseadv;
maximbolduc 47:d3123bb4f673 33 morder[filternumber] = porder;
maximbolduc 47:d3123bb4f673 34 _Tcenter = _Tcenter / 2 / PI;
maximbolduc 47:d3123bb4f673 35 order = porder;
maximbolduc 47:d3123bb4f673 36
maximbolduc 47:d3123bb4f673 37 double T1T2ratio = (1 + sin(phaseadv * PI / 180)) / (1 - sin(phaseadv * PI / 180));
maximbolduc 47:d3123bb4f673 38 double _T1 = sqrt(T1T2ratio) * _Tcenter;
maximbolduc 47:d3123bb4f673 39 double _T2 =_T1 / T1T2ratio;
maximbolduc 47:d3123bb4f673 40 double _T = 0.2;
maximbolduc 47:d3123bb4f673 41 double _K = (1 + 2 * _T1 / _T);
maximbolduc 47:d3123bb4f673 42 double _L = (1 - 2 * _T1 / _T);
maximbolduc 47:d3123bb4f673 43 double _M = (1 + 2 * _T2 / _T);
maximbolduc 47:d3123bb4f673 44 double _N = (1 - 2 * _T2 / _T);
maximbolduc 47:d3123bb4f673 45 // order = 2;
maximbolduc 47:d3123bb4f673 46 //version 1,
maximbolduc 47:d3123bb4f673 47 switch (order) {
maximbolduc 47:d3123bb4f673 48 case 3:
maximbolduc 47:d3123bb4f673 49 B0[filternumber] = pow(_K, 3) / pow(_M, 3);
maximbolduc 47:d3123bb4f673 50 B1[filternumber] = 3 * pow(_K, 2) * _L / pow(_M, 3);
maximbolduc 47:d3123bb4f673 51 B2[filternumber] = 3 * _K * pow(_L, 2) / pow(_M, 3);
maximbolduc 47:d3123bb4f673 52 B3[filternumber] = pow(_L, 3) / pow(_M, 3);
maximbolduc 47:d3123bb4f673 53
maximbolduc 47:d3123bb4f673 54 A_1[filternumber] = 3 * _N / _M;
maximbolduc 47:d3123bb4f673 55 A_2[filternumber] = 3 * pow(_N, 2) / pow(_M, 2);
maximbolduc 47:d3123bb4f673 56 A_3[filternumber] = pow(_N, 3) / pow(_M, 3);
maximbolduc 47:d3123bb4f673 57 break;
maximbolduc 47:d3123bb4f673 58 case 2:
maximbolduc 47:d3123bb4f673 59 B0[filternumber] = pow(_K, 2) / pow(_M, 2);
maximbolduc 47:d3123bb4f673 60 B1[filternumber] = 2 * _K * _L / pow(_M, 2);
maximbolduc 47:d3123bb4f673 61 B2[filternumber] = pow(_L, 2) / pow(_M, 2);
maximbolduc 47:d3123bb4f673 62 B3[filternumber] = 0;
maximbolduc 47:d3123bb4f673 63
maximbolduc 47:d3123bb4f673 64 A_1[filternumber] = 2 * _N / _M;
maximbolduc 47:d3123bb4f673 65 A_2[filternumber] = pow(_N, 2) / pow(_M, 2);
maximbolduc 47:d3123bb4f673 66 A_3[filternumber] = 0;
maximbolduc 47:d3123bb4f673 67 break;
maximbolduc 47:d3123bb4f673 68 case 1:
maximbolduc 47:d3123bb4f673 69 case 0:
maximbolduc 47:d3123bb4f673 70 B0[filternumber] = _K / _M;
maximbolduc 47:d3123bb4f673 71 B1[filternumber] = _L / _M;
maximbolduc 47:d3123bb4f673 72 B2[filternumber] = 0;
maximbolduc 47:d3123bb4f673 73 B3[filternumber] = 0;
maximbolduc 47:d3123bb4f673 74
maximbolduc 47:d3123bb4f673 75 A_1[filternumber] = _N / _M;
maximbolduc 47:d3123bb4f673 76 A_2[filternumber] = 0;
maximbolduc 47:d3123bb4f673 77 A_3[filternumber] = 0;
maximbolduc 47:d3123bb4f673 78 break;
maximbolduc 47:d3123bb4f673 79 }
maximbolduc 47:d3123bb4f673 80 }
maximbolduc 47:d3123bb4f673 81 //double d = 0;
maximbolduc 47:d3123bb4f673 82
maximbolduc 47:d3123bb4f673 83 string Steer(int ActiveTime,int value)
maximbolduc 47:d3123bb4f673 84 {
maximbolduc 47:d3123bb4f673 85 string sRet = "";
maximbolduc 47:d3123bb4f673 86 //f ((Err_aPort == 0)) {
maximbolduc 47:d3123bb4f673 87 // if (ActiveTime > 300) ActiveTime = 300;
maximbolduc 47:d3123bb4f673 88 if (value < 0) value = 0;
maximbolduc 47:d3123bb4f673 89 if ((value > 255)) value = 255;
maximbolduc 47:d3123bb4f673 90 OutputValue = value;
maximbolduc 47:d3123bb4f673 91 OutputTime = ActiveTime;
maximbolduc 47:d3123bb4f673 92
maximbolduc 47:d3123bb4f673 93 // d = //= DateTime.Now - autosteer.LastCommunication;
maximbolduc 47:d3123bb4f673 94
maximbolduc 47:d3123bb4f673 95 //no need to send repeated 127=do nothing
maximbolduc 47:d3123bb4f673 96 //if ((OutputValue != 127) || (LastOutputValue != OutputValue)) { // || (d.read()-LastCommunication > 2)) {
maximbolduc 47:d3123bb4f673 97 sRet = "$ASTEER," + itos(OutputValue) + "," + itos(ActiveTime) + "\r\n";
maximbolduc 47:d3123bb4f673 98 LastOutputValue = OutputValue;
maximbolduc 47:d3123bb4f673 99 // autosteer.Timer1counter = 0;
maximbolduc 47:d3123bb4f673 100 // autosteer.LastCommunication = DateTime.Now;
maximbolduc 47:d3123bb4f673 101 //}
maximbolduc 47:d3123bb4f673 102 // == }
maximbolduc 47:d3123bb4f673 103 return (sRet);
maximbolduc 47:d3123bb4f673 104 }
maximbolduc 47:d3123bb4f673 105
maximbolduc 47:d3123bb4f673 106 string ControlSteerFilter(int ActiveTime, double distAUTOL, double speed, double FilterGain, double min, double max,double SCALE)
maximbolduc 47:d3123bb4f673 107 {
maximbolduc 47:d3123bb4f673 108 string sRet = "";
maximbolduc 47:d3123bb4f673 109
maximbolduc 47:d3123bb4f673 110 int N = 3;
maximbolduc 47:d3123bb4f673 111 double y = 0;
maximbolduc 47:d3123bb4f673 112 // if (B0[0] == 9999.0) {
maximbolduc 47:d3123bb4f673 113 // SetDigitalFilter(47, 4.2 / 2 / PI, 0);
maximbolduc 47:d3123bb4f673 114 // }
maximbolduc 47:d3123bb4f673 115 // if (distAUTOL == 5000) distAUTOL = 0; //not set
maximbolduc 47:d3123bb4f673 116 // if (speed < 1) steer=false;
maximbolduc 47:d3123bb4f673 117
maximbolduc 47:d3123bb4f673 118 mx[N - 3] = mx[N - 2];
maximbolduc 47:d3123bb4f673 119 mx[N - 2] = mx[N - 1];
maximbolduc 47:d3123bb4f673 120 mx[N - 1] = mx[N];
maximbolduc 47:d3123bb4f673 121 if ( FilterGain > 0 ) {
maximbolduc 47:d3123bb4f673 122 if ( abs(distAUTOL) > 0 ) {
maximbolduc 47:d3123bb4f673 123 mx[N] = distAUTOL * FilterGain;
maximbolduc 47:d3123bb4f673 124
maximbolduc 47:d3123bb4f673 125
maximbolduc 47:d3123bb4f673 126 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 47:d3123bb4f673 127 mz[N] = my[N];
maximbolduc 47:d3123bb4f673 128
maximbolduc 47:d3123bb4f673 129 my[N - 3] = my[N - 2];
maximbolduc 47:d3123bb4f673 130 my[N - 2] = my[N - 1];
maximbolduc 47:d3123bb4f673 131 my[N - 1] = my[N];
maximbolduc 47:d3123bb4f673 132
maximbolduc 47:d3123bb4f673 133 mz[N - 3] = mz[N - 2];
maximbolduc 47:d3123bb4f673 134 mz[N - 2] = mz[N - 1];
maximbolduc 47:d3123bb4f673 135 mz[N - 1] = mz[N];
maximbolduc 47:d3123bb4f673 136 }
maximbolduc 47:d3123bb4f673 137 }
maximbolduc 47:d3123bb4f673 138 double y1 = (double)mz[N]; // y1 used to preserve value of mz[N]; // mz is now the output
maximbolduc 47:d3123bb4f673 139 //pc.printf("%f\r\n",y1);
maximbolduc 47:d3123bb4f673 140
maximbolduc 47:d3123bb4f673 141 //modify scale depending on distance!
maximbolduc 47:d3123bb4f673 142 double mscale = SCALE;
maximbolduc 47:d3123bb4f673 143 //if (abs(distAUTOL) > 0.5) mscale = SCALE * 1.5;
maximbolduc 47:d3123bb4f673 144 //if (abs(distAUTOL) > 1.5) mscale = SCALE * 1.5;
maximbolduc 47:d3123bb4f673 145 mscale = 0.85 * abs(distAUTOL) + SCALE;
maximbolduc 47:d3123bb4f673 146
maximbolduc 47:d3123bb4f673 147 y = (double)(y1 * mscale); // scale it here if neccesary
maximbolduc 47:d3123bb4f673 148 // pc.printf("%f\r\n",y);
maximbolduc 47:d3123bb4f673 149
maximbolduc 47:d3123bb4f673 150 // y = (double) (y * 10 / speed) ; //added March12, 10 km/h being most typical speed
maximbolduc 47:d3123bb4f673 151 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=
maximbolduc 47:d3123bb4f673 152
maximbolduc 47:d3123bb4f673 153 y = y * SCALE;
maximbolduc 47:d3123bb4f673 154
maximbolduc 47:d3123bb4f673 155 if ( y < -max ) {
maximbolduc 47:d3123bb4f673 156 y = -max;
maximbolduc 47:d3123bb4f673 157 } else if ( y > max) {
maximbolduc 47:d3123bb4f673 158 y = max;
maximbolduc 47:d3123bb4f673 159 }
maximbolduc 47:d3123bb4f673 160 y = 127 + y;
maximbolduc 47:d3123bb4f673 161
maximbolduc 47:d3123bb4f673 162 if (y <= 127) y = y - (min / 2.0);
maximbolduc 47:d3123bb4f673 163 if (y >= 127) y = y + (min / 2.0);
maximbolduc 47:d3123bb4f673 164
maximbolduc 47:d3123bb4f673 165 // y = y + 127;
maximbolduc 47:d3123bb4f673 166 if (y >= 255) y = 255;
maximbolduc 47:d3123bb4f673 167 if (y <= 0) y = 0;
maximbolduc 47:d3123bb4f673 168
maximbolduc 47:d3123bb4f673 169 int value = (int)y;
maximbolduc 47:d3123bb4f673 170
maximbolduc 47:d3123bb4f673 171 if (speed > 1.0 ) {
maximbolduc 47:d3123bb4f673 172 sRet= Steer( ActiveTime, value);
maximbolduc 47:d3123bb4f673 173 } else {
maximbolduc 47:d3123bb4f673 174 sRet = Steer( ActiveTime, 127 );
maximbolduc 47:d3123bb4f673 175 }
maximbolduc 47:d3123bb4f673 176
maximbolduc 47:d3123bb4f673 177 return (sRet);
maximbolduc 47:d3123bb4f673 178 }
maximbolduc 47:d3123bb4f673 179
maximbolduc 47:d3123bb4f673 180
maximbolduc 47:d3123bb4f673 181
maximbolduc 47:d3123bb4f673 182
maximbolduc 40:a68cc1a1a1e7 183 //Maybe you rather use the routine I currently use in FarmerGPS? It uses a lookahead and simply distance to AB-line.
maximbolduc 40:a68cc1a1a1e7 184 //No heading error at all.
maximbolduc 40:a68cc1a1a1e7 185
maximbolduc 40:a68cc1a1a1e7 186 //ControlSteerFilter is the main routine. ActiveTime in ms, typically under 200ms, distAUTOL is distance to AB line at lookahead position
maximbolduc 47:d3123bb4f673 187 /*#include "mbed.h"
maximbolduc 40:a68cc1a1a1e7 188 #include <string.h>
maximbolduc 40:a68cc1a1a1e7 189 #include <math.h>
maximbolduc 40:a68cc1a1a1e7 190 #include <stdlib.h>
maximbolduc 41:a26acd346c2f 191
maximbolduc 41:a26acd346c2f 192 #ifndef PI
maximbolduc 40:a68cc1a1a1e7 193 #define PI 3.14159265359
maximbolduc 41:a26acd346c2f 194 #endif
maximbolduc 40:a68cc1a1a1e7 195
maximbolduc 40:a68cc1a1a1e7 196 double m_Tcenter[10];
maximbolduc 40:a68cc1a1a1e7 197 double mphaseadv[10];
maximbolduc 40:a68cc1a1a1e7 198 double morder[10];
maximbolduc 40:a68cc1a1a1e7 199 int order;
maximbolduc 40:a68cc1a1a1e7 200 double B0[10];
maximbolduc 40:a68cc1a1a1e7 201 double B1[10];
maximbolduc 40:a68cc1a1a1e7 202 double B2[10];
maximbolduc 40:a68cc1a1a1e7 203 double B3[10];
maximbolduc 40:a68cc1a1a1e7 204 double A_1[10];
maximbolduc 40:a68cc1a1a1e7 205 double A_2[10];
maximbolduc 40:a68cc1a1a1e7 206 double A_3[10];
maximbolduc 40:a68cc1a1a1e7 207
maximbolduc 40:a68cc1a1a1e7 208 double mx[10];
maximbolduc 40:a68cc1a1a1e7 209 double my[10];
maximbolduc 40:a68cc1a1a1e7 210 double mz[10];
maximbolduc 40:a68cc1a1a1e7 211 int Err_aPort = 0;
maximbolduc 40:a68cc1a1a1e7 212
maximbolduc 40:a68cc1a1a1e7 213 double OutputValue = 0;
maximbolduc 40:a68cc1a1a1e7 214 double OutputTime = 0;
maximbolduc 40:a68cc1a1a1e7 215 double LastOutputValue = 0;
maximbolduc 40:a68cc1a1a1e7 216
maximbolduc 40:a68cc1a1a1e7 217 double SpeedN = 1;
maximbolduc 41:a26acd346c2f 218 int porder = 2;
maximbolduc 40:a68cc1a1a1e7 219
maximbolduc 40:a68cc1a1a1e7 220 std::string itos(int n)
maximbolduc 40:a68cc1a1a1e7 221 {
maximbolduc 47:d3123bb4f673 222 const int max_size = std::numeric_limits<int>::digits10 + 1 + 1;
maximbolduc 40:a68cc1a1a1e7 223 char buffer[max_size] = {0};
maximbolduc 40:a68cc1a1a1e7 224 sprintf(buffer, "%d", n);
maximbolduc 40:a68cc1a1a1e7 225 return std::string(buffer);
maximbolduc 40:a68cc1a1a1e7 226 }
maximbolduc 34:c2bc9f9be7ff 227
maximbolduc 41:a26acd346c2f 228 void SetDigitalFilter(double phaseadv, double _Tcenter, int filternumber)
maximbolduc 40:a68cc1a1a1e7 229 {
maximbolduc 40:a68cc1a1a1e7 230 m_Tcenter[filternumber] = _Tcenter;
maximbolduc 40:a68cc1a1a1e7 231 mphaseadv[filternumber] = phaseadv;
maximbolduc 40:a68cc1a1a1e7 232 morder[filternumber] = porder;
maximbolduc 40:a68cc1a1a1e7 233 _Tcenter = _Tcenter / 2 / PI;
maximbolduc 41:a26acd346c2f 234 order = porder;
maximbolduc 40:a68cc1a1a1e7 235
maximbolduc 40:a68cc1a1a1e7 236 double T1T2ratio = (1 + sin(phaseadv * PI / 180)) / (1 - sin(phaseadv * PI / 180));
maximbolduc 40:a68cc1a1a1e7 237 double _T1 = sqrt(T1T2ratio) * _Tcenter;
maximbolduc 41:a26acd346c2f 238 double _T2 =_T1 / T1T2ratio;
maximbolduc 40:a68cc1a1a1e7 239 double _T = 0.2;
maximbolduc 40:a68cc1a1a1e7 240 double _K = (1 + 2 * _T1 / _T);
maximbolduc 40:a68cc1a1a1e7 241 double _L = (1 - 2 * _T1 / _T);
maximbolduc 40:a68cc1a1a1e7 242 double _M = (1 + 2 * _T2 / _T);
maximbolduc 40:a68cc1a1a1e7 243 double _N = (1 - 2 * _T2 / _T);
maximbolduc 41:a26acd346c2f 244 order = 2;
maximbolduc 40:a68cc1a1a1e7 245 //version 1,
maximbolduc 40:a68cc1a1a1e7 246 switch (order) {
maximbolduc 40:a68cc1a1a1e7 247 case 3:
maximbolduc 40:a68cc1a1a1e7 248 B0[filternumber] = pow(_K, 3) / pow(_M, 3);
maximbolduc 40:a68cc1a1a1e7 249 B1[filternumber] = 3 * pow(_K, 2) * _L / pow(_M, 3);
maximbolduc 40:a68cc1a1a1e7 250 B2[filternumber] = 3 * _K * pow(_L, 2) / pow(_M, 3);
maximbolduc 40:a68cc1a1a1e7 251 B3[filternumber] = pow(_L, 3) / pow(_M, 3);
maximbolduc 40:a68cc1a1a1e7 252
maximbolduc 40:a68cc1a1a1e7 253 A_1[filternumber] = 3 * _N / _M;
maximbolduc 40:a68cc1a1a1e7 254 A_2[filternumber] = 3 * pow(_N, 2) / pow(_M, 2);
maximbolduc 40:a68cc1a1a1e7 255 A_3[filternumber] = pow(_N, 3) / pow(_M, 3);
maximbolduc 40:a68cc1a1a1e7 256 break;
maximbolduc 40:a68cc1a1a1e7 257 case 2:
maximbolduc 40:a68cc1a1a1e7 258 B0[filternumber] = pow(_K, 2) / pow(_M, 2);
maximbolduc 40:a68cc1a1a1e7 259 B1[filternumber] = 2 * _K * _L / pow(_M, 2);
maximbolduc 40:a68cc1a1a1e7 260 B2[filternumber] = pow(_L, 2) / pow(_M, 2);
maximbolduc 40:a68cc1a1a1e7 261 B3[filternumber] = 0;
maximbolduc 34:c2bc9f9be7ff 262
maximbolduc 40:a68cc1a1a1e7 263 A_1[filternumber] = 2 * _N / _M;
maximbolduc 40:a68cc1a1a1e7 264 A_2[filternumber] = pow(_N, 2) / pow(_M, 2);
maximbolduc 40:a68cc1a1a1e7 265 A_3[filternumber] = 0;
maximbolduc 40:a68cc1a1a1e7 266 break;
maximbolduc 40:a68cc1a1a1e7 267 case 1:
maximbolduc 40:a68cc1a1a1e7 268 case 0:
maximbolduc 40:a68cc1a1a1e7 269 B0[filternumber] = _K / _M;
maximbolduc 40:a68cc1a1a1e7 270 B1[filternumber] = _L / _M;
maximbolduc 40:a68cc1a1a1e7 271 B2[filternumber] = 0;
maximbolduc 40:a68cc1a1a1e7 272 B3[filternumber] = 0;
maximbolduc 40:a68cc1a1a1e7 273
maximbolduc 40:a68cc1a1a1e7 274 A_1[filternumber] = _N / _M;
maximbolduc 40:a68cc1a1a1e7 275 A_2[filternumber] = 0;
maximbolduc 40:a68cc1a1a1e7 276 A_3[filternumber] = 0;
maximbolduc 40:a68cc1a1a1e7 277 break;
maximbolduc 40:a68cc1a1a1e7 278 }
maximbolduc 40:a68cc1a1a1e7 279 }
maximbolduc 40:a68cc1a1a1e7 280 //double d = 0;
maximbolduc 40:a68cc1a1a1e7 281
maximbolduc 41:a26acd346c2f 282 string Steer(int ActiveTime,int value)
maximbolduc 40:a68cc1a1a1e7 283 {
maximbolduc 40:a68cc1a1a1e7 284 string sRet = "";
maximbolduc 41:a26acd346c2f 285 //f ((Err_aPort == 0)) {
maximbolduc 40:a68cc1a1a1e7 286 // if (ActiveTime > 300) ActiveTime = 300;
maximbolduc 41:a26acd346c2f 287 // if (value < 0) value = 0;
maximbolduc 41:a26acd346c2f 288 // if ((value > 255)) value = 255;
maximbolduc 40:a68cc1a1a1e7 289 OutputValue = value;
maximbolduc 40:a68cc1a1a1e7 290 OutputTime = ActiveTime;
maximbolduc 40:a68cc1a1a1e7 291
maximbolduc 40:a68cc1a1a1e7 292 // d = //= DateTime.Now - autosteer.LastCommunication;
maximbolduc 40:a68cc1a1a1e7 293
maximbolduc 40:a68cc1a1a1e7 294 //no need to send repeated 127=do nothing
maximbolduc 41:a26acd346c2f 295 // if ((OutputValue != 127) || (LastOutputValue != OutputValue)) { // || (d.read()-LastCommunication > 2)) {
maximbolduc 40:a68cc1a1a1e7 296 sRet = "$ASTEER," + itos(OutputValue) + "," + itos(ActiveTime) + "\r\n";
maximbolduc 40:a68cc1a1a1e7 297 LastOutputValue = OutputValue;
maximbolduc 40:a68cc1a1a1e7 298 // autosteer.Timer1counter = 0;
maximbolduc 40:a68cc1a1a1e7 299 // autosteer.LastCommunication = DateTime.Now;
maximbolduc 40:a68cc1a1a1e7 300 }
maximbolduc 41:a26acd346c2f 301 // == }
maximbolduc 40:a68cc1a1a1e7 302 return (sRet);
maximbolduc 40:a68cc1a1a1e7 303 }
maximbolduc 40:a68cc1a1a1e7 304
maximbolduc 41:a26acd346c2f 305 string ControlSteerFilter(int ActiveTime, double distAUTOL, double speed,bool steer, double FilterGain, double min, double max,double SCALE)
maximbolduc 40:a68cc1a1a1e7 306 {
maximbolduc 40:a68cc1a1a1e7 307 string sRet = "";
maximbolduc 34:c2bc9f9be7ff 308
maximbolduc 40:a68cc1a1a1e7 309 int N = 3;
maximbolduc 41:a26acd346c2f 310 double y = 0;
maximbolduc 40:a68cc1a1a1e7 311 if (B0[0] == 9999.0) {
maximbolduc 41:a26acd346c2f 312 SetDigitalFilter(47, 4.2 / 2 / PI, 0);
maximbolduc 40:a68cc1a1a1e7 313 }
maximbolduc 41:a26acd346c2f 314 // if (distAUTOL == 5000) distAUTOL = 0; //not set
maximbolduc 40:a68cc1a1a1e7 315 if (speed < 1) steer=false;
maximbolduc 40:a68cc1a1a1e7 316
maximbolduc 40:a68cc1a1a1e7 317 mx[N - 3] = mx[N - 2];
maximbolduc 40:a68cc1a1a1e7 318 mx[N - 2] = mx[N - 1];
maximbolduc 40:a68cc1a1a1e7 319 mx[N - 1] = mx[N];
maximbolduc 40:a68cc1a1a1e7 320 mx[N] = distAUTOL * FilterGain;
maximbolduc 40:a68cc1a1a1e7 321
maximbolduc 40:a68cc1a1a1e7 322 my[N - 3] = my[N - 2];
maximbolduc 40:a68cc1a1a1e7 323 my[N - 2] = my[N - 1];
maximbolduc 40:a68cc1a1a1e7 324 my[N - 1] = my[N];
maximbolduc 40:a68cc1a1a1e7 325
maximbolduc 40:a68cc1a1a1e7 326 mz[N - 3] = mz[N - 2];
maximbolduc 40:a68cc1a1a1e7 327 mz[N - 2] = mz[N - 1];
maximbolduc 40:a68cc1a1a1e7 328 mz[N - 1] = mz[N];
maximbolduc 40:a68cc1a1a1e7 329 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 41:a26acd346c2f 330 mz[N] = my[N];
maximbolduc 41:a26acd346c2f 331 pc.printf("$%f\r\n\0",my[N]);
maximbolduc 40:a68cc1a1a1e7 332
maximbolduc 41:a26acd346c2f 333 double y1 = (double)mz[N]; // y1 used to preserve value of mz[N]; // mz is now the output
maximbolduc 41:a26acd346c2f 334 pc.printf("$%f\r\n\0",y1);
maximbolduc 40:a68cc1a1a1e7 335
maximbolduc 40:a68cc1a1a1e7 336 //modify scale depending on distance!
maximbolduc 41:a26acd346c2f 337 double mscale = SCALE;
maximbolduc 40:a68cc1a1a1e7 338 if (abs(distAUTOL) > 0.5) mscale = scale * 1.5;
maximbolduc 40:a68cc1a1a1e7 339 if (abs(distAUTOL) > 1.5) mscale = scale * 1.5;
maximbolduc 40:a68cc1a1a1e7 340
maximbolduc 41:a26acd346c2f 341 y = (double)(y1 * mscale); // scale it here if neccesary
maximbolduc 41:a26acd346c2f 342 // y = (double) (y * 10 / speed) ; //added March12, 10 km/h being most typical speed
maximbolduc 41:a26acd346c2f 343 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=
maximbolduc 41:a26acd346c2f 344 pc.printf("$%f\r\n\0",y);
maximbolduc 41:a26acd346c2f 345
maximbolduc 41:a26acd346c2f 346 y = y * SCALE;
maximbolduc 41:a26acd346c2f 347 //pc.printf("$%f\r\n\0",y0);
maximbolduc 41:a26acd346c2f 348
maximbolduc 47:d3123bb4f673 349 // if ( y < -max ) {
maximbolduc 47:d3123bb4f673 350 // y = -max;
maximbolduc 47:d3123bb4f673 351 // } else if ( y > max) {
maximbolduc 47:d3123bb4f673 352 // y = max;
maximbolduc 47:d3123bb4f673 353 //}
maximbolduc 41:a26acd346c2f 354
maximbolduc 41:a26acd346c2f 355 y = 127.0 + y;
maximbolduc 41:a26acd346c2f 356
maximbolduc 41:a26acd346c2f 357 if (y <= 127.0) y = y - (min / 2.0);
maximbolduc 41:a26acd346c2f 358 if (y >= 127.0) y = y + (min / 2.0);
maximbolduc 41:a26acd346c2f 359
maximbolduc 41:a26acd346c2f 360 if ( y < - max)
maximbolduc 41:a26acd346c2f 361 {
maximbolduc 41:a26acd346c2f 362 y = -max;
maximbolduc 41:a26acd346c2f 363 }
maximbolduc 41:a26acd346c2f 364 else if ( y > max )
maximbolduc 41:a26acd346c2f 365 {
maximbolduc 41:a26acd346c2f 366 y=max;
maximbolduc 41:a26acd346c2f 367 }
maximbolduc 41:a26acd346c2f 368 // if ( y <= 127 ) y = y - (max / 2);
maximbolduc 41:a26acd346c2f 369 // if ( y >= 127 ) y = y + (max / 2);
maximbolduc 41:a26acd346c2f 370
maximbolduc 41:a26acd346c2f 371
maximbolduc 40:a68cc1a1a1e7 372 if (y >= 255) y = 255;
maximbolduc 40:a68cc1a1a1e7 373 if (y <= 0) y = 0;
maximbolduc 41:a26acd346c2f 374
maximbolduc 40:a68cc1a1a1e7 375 int value = (int)y;
maximbolduc 41:a26acd346c2f 376 // if (steer) {
maximbolduc 40:a68cc1a1a1e7 377 sRet= Steer( ActiveTime, value);
maximbolduc 41:a26acd346c2f 378 //}
maximbolduc 41:a26acd346c2f 379
maximbolduc 40:a68cc1a1a1e7 380 return (sRet);
maximbolduc 47:d3123bb4f673 381 }*/