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@56:456d454d9ced, 2015-03-29 (annotated)

Committer:
maximbolduc
Date:
Sun Mar 29 16:03:18 2015 +0000
Revision:
56:456d454d9ced
Parent:
54:2405c62c1049
Child:
57:0299098b2d0e
more settings for dynamic bias

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 56:456d454d9ced 5 #ifndef AUTOSTEER_H
maximbolduc 56:456d454d9ced 6 #define AUTOSTEER_H
maximbolduc 56:456d454d9ced 7
maximbolduc 56:456d454d9ced 8 #include <string.h>
maximbolduc 56:456d454d9ced 9
maximbolduc 56:456d454d9ced 10 using namespace std;
maximbolduc 56:456d454d9ced 11
maximbolduc 47:d3123bb4f673 12 double m_Tcenter[5] = {0,0,0,0,0};
maximbolduc 47:d3123bb4f673 13 double mphaseadv[5]= {0,0,0,0,0};
maximbolduc 47:d3123bb4f673 14 double morder[5]= {0,0,0,0,0};
maximbolduc 47:d3123bb4f673 15 int order;
maximbolduc 47:d3123bb4f673 16 double B0[5]= {0,0,0,0,0};
maximbolduc 47:d3123bb4f673 17 double B1[5]= {0,0,0,0,0};
maximbolduc 47:d3123bb4f673 18 double B2[5]= {0,0,0,0,0};
maximbolduc 47:d3123bb4f673 19 double B3[5]= {0,0,0,0,0};
maximbolduc 47:d3123bb4f673 20 double A_1[5]= {0,0,0,0,0};
maximbolduc 47:d3123bb4f673 21 double A_2[5]= {0,0,0,0,0};
maximbolduc 47:d3123bb4f673 22 double A_3[5]= {0,0,0,0,0};
maximbolduc 47:d3123bb4f673 23
maximbolduc 47:d3123bb4f673 24 double mx[5]= {0,0,0,0,0};
maximbolduc 47:d3123bb4f673 25 double my[5]= {0,0,0,0,0};
maximbolduc 47:d3123bb4f673 26 double mz[5]= {0,0,0,0,0};
maximbolduc 47:d3123bb4f673 27 int Err_aPort = 0;
maximbolduc 47:d3123bb4f673 28
maximbolduc 47:d3123bb4f673 29 double OutputValue = 0;
maximbolduc 47:d3123bb4f673 30 double OutputTime = 0;
maximbolduc 47:d3123bb4f673 31 double LastOutputValue = 0;
maximbolduc 47:d3123bb4f673 32
maximbolduc 47:d3123bb4f673 33 double SpeedN = 1;
maximbolduc 47:d3123bb4f673 34 int porder = 0;
maximbolduc 47:d3123bb4f673 35
maximbolduc 47:d3123bb4f673 36 void SetDigitalFilter(double phaseadv, double _Tcenter, int filternumber)
maximbolduc 47:d3123bb4f673 37 {
maximbolduc 47:d3123bb4f673 38 m_Tcenter[filternumber] = _Tcenter;
maximbolduc 47:d3123bb4f673 39 mphaseadv[filternumber] = phaseadv;
maximbolduc 47:d3123bb4f673 40 morder[filternumber] = porder;
maximbolduc 47:d3123bb4f673 41 _Tcenter = _Tcenter / 2 / PI;
maximbolduc 47:d3123bb4f673 42 order = porder;
maximbolduc 47:d3123bb4f673 43
maximbolduc 47:d3123bb4f673 44 double T1T2ratio = (1 + sin(phaseadv * PI / 180)) / (1 - sin(phaseadv * PI / 180));
maximbolduc 47:d3123bb4f673 45 double _T1 = sqrt(T1T2ratio) * _Tcenter;
maximbolduc 47:d3123bb4f673 46 double _T2 =_T1 / T1T2ratio;
maximbolduc 47:d3123bb4f673 47 double _T = 0.2;
maximbolduc 47:d3123bb4f673 48 double _K = (1 + 2 * _T1 / _T);
maximbolduc 47:d3123bb4f673 49 double _L = (1 - 2 * _T1 / _T);
maximbolduc 47:d3123bb4f673 50 double _M = (1 + 2 * _T2 / _T);
maximbolduc 47:d3123bb4f673 51 double _N = (1 - 2 * _T2 / _T);
maximbolduc 47:d3123bb4f673 52 // order = 2;
maximbolduc 47:d3123bb4f673 53 //version 1,
maximbolduc 47:d3123bb4f673 54 switch (order) {
maximbolduc 47:d3123bb4f673 55 case 3:
maximbolduc 47:d3123bb4f673 56 B0[filternumber] = pow(_K, 3) / pow(_M, 3);
maximbolduc 47:d3123bb4f673 57 B1[filternumber] = 3 * pow(_K, 2) * _L / pow(_M, 3);
maximbolduc 47:d3123bb4f673 58 B2[filternumber] = 3 * _K * pow(_L, 2) / pow(_M, 3);
maximbolduc 47:d3123bb4f673 59 B3[filternumber] = pow(_L, 3) / pow(_M, 3);
maximbolduc 47:d3123bb4f673 60
maximbolduc 47:d3123bb4f673 61 A_1[filternumber] = 3 * _N / _M;
maximbolduc 47:d3123bb4f673 62 A_2[filternumber] = 3 * pow(_N, 2) / pow(_M, 2);
maximbolduc 47:d3123bb4f673 63 A_3[filternumber] = pow(_N, 3) / pow(_M, 3);
maximbolduc 47:d3123bb4f673 64 break;
maximbolduc 47:d3123bb4f673 65 case 2:
maximbolduc 47:d3123bb4f673 66 B0[filternumber] = pow(_K, 2) / pow(_M, 2);
maximbolduc 47:d3123bb4f673 67 B1[filternumber] = 2 * _K * _L / pow(_M, 2);
maximbolduc 47:d3123bb4f673 68 B2[filternumber] = pow(_L, 2) / pow(_M, 2);
maximbolduc 47:d3123bb4f673 69 B3[filternumber] = 0;
maximbolduc 47:d3123bb4f673 70
maximbolduc 47:d3123bb4f673 71 A_1[filternumber] = 2 * _N / _M;
maximbolduc 47:d3123bb4f673 72 A_2[filternumber] = pow(_N, 2) / pow(_M, 2);
maximbolduc 47:d3123bb4f673 73 A_3[filternumber] = 0;
maximbolduc 47:d3123bb4f673 74 break;
maximbolduc 47:d3123bb4f673 75 case 1:
maximbolduc 47:d3123bb4f673 76 case 0:
maximbolduc 47:d3123bb4f673 77 B0[filternumber] = _K / _M;
maximbolduc 47:d3123bb4f673 78 B1[filternumber] = _L / _M;
maximbolduc 47:d3123bb4f673 79 B2[filternumber] = 0;
maximbolduc 47:d3123bb4f673 80 B3[filternumber] = 0;
maximbolduc 47:d3123bb4f673 81
maximbolduc 47:d3123bb4f673 82 A_1[filternumber] = _N / _M;
maximbolduc 47:d3123bb4f673 83 A_2[filternumber] = 0;
maximbolduc 47:d3123bb4f673 84 A_3[filternumber] = 0;
maximbolduc 47:d3123bb4f673 85 break;
maximbolduc 47:d3123bb4f673 86 }
maximbolduc 47:d3123bb4f673 87 }
maximbolduc 47:d3123bb4f673 88 //double d = 0;
maximbolduc 47:d3123bb4f673 89
maximbolduc 56:456d454d9ced 90 std::string Steer(int ActiveTime,int value)
maximbolduc 47:d3123bb4f673 91 {
maximbolduc 47:d3123bb4f673 92 string sRet = "";
maximbolduc 47:d3123bb4f673 93 if (value < 0) value = 0;
maximbolduc 47:d3123bb4f673 94 if ((value > 255)) value = 255;
maximbolduc 47:d3123bb4f673 95 OutputValue = value;
maximbolduc 47:d3123bb4f673 96 OutputTime = ActiveTime;
maximbolduc 47:d3123bb4f673 97 sRet = "$ASTEER," + itos(OutputValue) + "," + itos(ActiveTime) + "\r\n";
maximbolduc 47:d3123bb4f673 98 LastOutputValue = OutputValue;
maximbolduc 54:2405c62c1049 99
maximbolduc 47:d3123bb4f673 100 return (sRet);
maximbolduc 47:d3123bb4f673 101 }
maximbolduc 47:d3123bb4f673 102
maximbolduc 56:456d454d9ced 103 std::string ControlSteerFilter(int ActiveTime, double distAUTOL, double speed, double FilterGain, double min, double max,double SCALE)
maximbolduc 47:d3123bb4f673 104 {
maximbolduc 47:d3123bb4f673 105 string sRet = "";
maximbolduc 47:d3123bb4f673 106 int N = 3;
maximbolduc 47:d3123bb4f673 107 double y = 0;
maximbolduc 47:d3123bb4f673 108
maximbolduc 47:d3123bb4f673 109 mx[N - 3] = mx[N - 2];
maximbolduc 47:d3123bb4f673 110 mx[N - 2] = mx[N - 1];
maximbolduc 47:d3123bb4f673 111 mx[N - 1] = mx[N];
maximbolduc 47:d3123bb4f673 112 if ( FilterGain > 0 ) {
maximbolduc 47:d3123bb4f673 113 if ( abs(distAUTOL) > 0 ) {
maximbolduc 47:d3123bb4f673 114 mx[N] = distAUTOL * FilterGain;
maximbolduc 47:d3123bb4f673 115
maximbolduc 47:d3123bb4f673 116 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 117 mz[N] = my[N];
maximbolduc 47:d3123bb4f673 118
maximbolduc 47:d3123bb4f673 119 my[N - 3] = my[N - 2];
maximbolduc 47:d3123bb4f673 120 my[N - 2] = my[N - 1];
maximbolduc 47:d3123bb4f673 121 my[N - 1] = my[N];
maximbolduc 47:d3123bb4f673 122
maximbolduc 47:d3123bb4f673 123 mz[N - 3] = mz[N - 2];
maximbolduc 47:d3123bb4f673 124 mz[N - 2] = mz[N - 1];
maximbolduc 47:d3123bb4f673 125 mz[N - 1] = mz[N];
maximbolduc 47:d3123bb4f673 126 }
maximbolduc 47:d3123bb4f673 127 }
maximbolduc 56:456d454d9ced 128 SCALE = SCALE / 10;
maximbolduc 47:d3123bb4f673 129 double y1 = (double)mz[N]; // y1 used to preserve value of mz[N]; // mz is now the output
maximbolduc 47:d3123bb4f673 130 //modify scale depending on distance!
maximbolduc 54:2405c62c1049 131 double mscale = 0.1 * abs(distAUTOL) + SCALE;
maximbolduc 56:456d454d9ced 132 if ( mscale > 5* SCALE )
maximbolduc 56:456d454d9ced 133 {
maximbolduc 56:456d454d9ced 134 mscale = 5*SCALE;
maximbolduc 56:456d454d9ced 135 }
maximbolduc 47:d3123bb4f673 136 y = (double)(y1 * mscale); // scale it here if neccesary
maximbolduc 47:d3123bb4f673 137 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 138
maximbolduc 56:456d454d9ced 139 // y = y * mscale;
maximbolduc 47:d3123bb4f673 140
maximbolduc 47:d3123bb4f673 141 if ( y < -max ) {
maximbolduc 47:d3123bb4f673 142 y = -max;
maximbolduc 47:d3123bb4f673 143 } else if ( y > max) {
maximbolduc 47:d3123bb4f673 144 y = max;
maximbolduc 47:d3123bb4f673 145 }
maximbolduc 47:d3123bb4f673 146 y = 127 + y;
maximbolduc 47:d3123bb4f673 147 if (y <= 127) y = y - (min / 2.0);
maximbolduc 47:d3123bb4f673 148 if (y >= 127) y = y + (min / 2.0);
maximbolduc 47:d3123bb4f673 149
maximbolduc 47:d3123bb4f673 150 if (y >= 255) y = 255;
maximbolduc 47:d3123bb4f673 151 if (y <= 0) y = 0;
maximbolduc 47:d3123bb4f673 152
maximbolduc 47:d3123bb4f673 153 int value = (int)y;
maximbolduc 47:d3123bb4f673 154 if (speed > 1.0 ) {
maximbolduc 47:d3123bb4f673 155 sRet= Steer( ActiveTime, value);
maximbolduc 47:d3123bb4f673 156 } else {
maximbolduc 47:d3123bb4f673 157 sRet = Steer( ActiveTime, 127 );
maximbolduc 47:d3123bb4f673 158 }
maximbolduc 47:d3123bb4f673 159
maximbolduc 47:d3123bb4f673 160 return (sRet);
maximbolduc 56:456d454d9ced 161 }
maximbolduc 56:456d454d9ced 162 #endif