alles ingevoegd
Dependencies: FastPWM HIDScope MODSERIAL QEI mbed biquadFilter
Fork of deelPID by
main.cpp@2:58ec7347245e, 2018-10-31 (annotated)
- Committer:
- laurencebr
- Date:
- Wed Oct 31 08:04:10 2018 +0000
- Revision:
- 2:58ec7347245e
- Parent:
- 1:23834862b877
- Child:
- 3:53fb8bd0a448
kabels op goede plek. GainI heeft geen potmeter
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
laurencebr | 0:2aa29a0824df | 1 | #include "mbed.h" |
laurencebr | 0:2aa29a0824df | 2 | #include "FastPWM.h" |
laurencebr | 0:2aa29a0824df | 3 | #include "MODSERIAL.h" |
laurencebr | 0:2aa29a0824df | 4 | #include "QEI.h" |
laurencebr | 0:2aa29a0824df | 5 | #include "HIDScope.h" |
laurencebr | 0:2aa29a0824df | 6 | |
laurencebr | 0:2aa29a0824df | 7 | DigitalOut gpo(D0); |
laurencebr | 0:2aa29a0824df | 8 | DigitalOut led(LED_RED); |
laurencebr | 2:58ec7347245e | 9 | AnalogIn pot1(A4); //POORTEN VERANDEREN |
laurencebr | 2:58ec7347245e | 10 | //AnalogIn pot2(); //Beneden is I control op 0 gezet. //POORTEN veranderen |
laurencebr | 2:58ec7347245e | 11 | AnalogIn pot3(A5); //POORTEN VERANDEREN |
laurencebr | 0:2aa29a0824df | 12 | QEI encoder1(D10, D11, NC, 32); |
laurencebr | 0:2aa29a0824df | 13 | QEI encoder2(D12, D13, NC, 32); |
laurencebr | 0:2aa29a0824df | 14 | FastPWM Motor1PWM(D6); |
laurencebr | 0:2aa29a0824df | 15 | DigitalOut Motor1Direction(D7); |
laurencebr | 2:58ec7347245e | 16 | FastPWM Motor2PWM(D5); |
laurencebr | 2:58ec7347245e | 17 | DigitalOut Motor2Direction(D4); |
laurencebr | 1:23834862b877 | 18 | //EMG |
laurencebr | 1:23834862b877 | 19 | AnalogIn a0(A0); |
laurencebr | 1:23834862b877 | 20 | AnalogIn a1(A1); |
laurencebr | 1:23834862b877 | 21 | AnalogIn a2(A2); |
laurencebr | 1:23834862b877 | 22 | AnalogIn a3(A3); |
laurencebr | 0:2aa29a0824df | 23 | |
laurencebr | 0:2aa29a0824df | 24 | MODSERIAL pc(USBTX, USBRX); |
laurencebr | 0:2aa29a0824df | 25 | |
laurencebr | 1:23834862b877 | 26 | //HIDSCOPE //Oppassen waar we HIDSCOPE aanroepen want nu voor meerdere dingen |
laurencebr | 0:2aa29a0824df | 27 | HIDScope scope(4); |
laurencebr | 0:2aa29a0824df | 28 | Ticker scopeTimer; |
laurencebr | 0:2aa29a0824df | 29 | |
laurencebr | 1:23834862b877 | 30 | Ticker EMGTicker; |
laurencebr | 1:23834862b877 | 31 | |
laurencebr | 1:23834862b877 | 32 | // BiQuad filters |
laurencebr | 1:23834862b877 | 33 | //BiQuad Chains |
laurencebr | 1:23834862b877 | 34 | BiQuadChain bqc1; |
laurencebr | 1:23834862b877 | 35 | BiQuadChain bqc2; |
laurencebr | 1:23834862b877 | 36 | BiQuadChain bqc3; |
laurencebr | 1:23834862b877 | 37 | BiQuadChain bqc4; |
laurencebr | 1:23834862b877 | 38 | |
laurencebr | 1:23834862b877 | 39 | //High pass filters 20 Hz |
laurencebr | 1:23834862b877 | 40 | BiQuad HP_emg1(1,-2,1,-1.142980502539901,0.412801598096189); |
laurencebr | 1:23834862b877 | 41 | BiQuad HP_emg2(1,-2,1,-1.142980502539901,0.412801598096189); |
laurencebr | 1:23834862b877 | 42 | BiQuad HP_emg3(1,-2,1,-1.142980502539901,0.412801598096189); |
laurencebr | 1:23834862b877 | 43 | BiQuad HP_emg4(1,-2,1,-1.142980502539901,0.412801598096189); |
laurencebr | 1:23834862b877 | 44 | |
laurencebr | 1:23834862b877 | 45 | //Notch filters 50 Hz |
laurencebr | 1:23834862b877 | 46 | BiQuad Notch_emg1(1,-1.225251386743515e-16,1,-1.187919512117619e-16,0.939062505817492); |
laurencebr | 1:23834862b877 | 47 | BiQuad Notch_emg2(1,-1.225251386743515e-16,1,-1.187919512117619e-16,0.939062505817492); |
laurencebr | 1:23834862b877 | 48 | BiQuad Notch_emg3(1,-1.225251386743515e-16,1,-1.187919512117619e-16,0.939062505817492); |
laurencebr | 1:23834862b877 | 49 | BiQuad Notch_emg4(1,-1.225251386743515e-16,1,-1.187919512117619e-16,0.939062505817492); |
laurencebr | 0:2aa29a0824df | 50 | |
laurencebr | 0:2aa29a0824df | 51 | ///Variables |
laurencebr | 0:2aa29a0824df | 52 | |
laurencebr | 0:2aa29a0824df | 53 | double q1Error; |
laurencebr | 0:2aa29a0824df | 54 | double q2Error; |
laurencebr | 0:2aa29a0824df | 55 | double PrevErrorq1[100]; |
laurencebr | 0:2aa29a0824df | 56 | double PrevErrorq2[100]; |
laurencebr | 0:2aa29a0824df | 57 | int n = 100; // Zelfde waarde als PrevErrorarray |
laurencebr | 0:2aa29a0824df | 58 | double q1Ref = 1.0; //VOOR TESTEN |
laurencebr | 0:2aa29a0824df | 59 | double q2Ref; |
laurencebr | 0:2aa29a0824df | 60 | double xRef; |
laurencebr | 0:2aa29a0824df | 61 | double yRef; |
laurencebr | 0:2aa29a0824df | 62 | double q1Pos; |
laurencebr | 0:2aa29a0824df | 63 | double q2Pos; |
laurencebr | 0:2aa29a0824df | 64 | double PIDerrorq1; |
laurencebr | 0:2aa29a0824df | 65 | double PIDerrorq2; |
laurencebr | 0:2aa29a0824df | 66 | double IntegralErrorq1 = 0.0; |
laurencebr | 0:2aa29a0824df | 67 | double IntegralErrorq2 = 0.0; |
laurencebr | 0:2aa29a0824df | 68 | double DerivativeErrorq1 = 0.0; |
laurencebr | 0:2aa29a0824df | 69 | double DerivativeErrorq2 = 0.0; |
laurencebr | 0:2aa29a0824df | 70 | double GainP1 = 2.0; |
laurencebr | 0:2aa29a0824df | 71 | double GainI1 = 1.2; |
laurencebr | 0:2aa29a0824df | 72 | double GainD1 = 0.0; |
laurencebr | 0:2aa29a0824df | 73 | double GainP2 = 2.0; |
laurencebr | 0:2aa29a0824df | 74 | double GainI2 = 2.0; |
laurencebr | 0:2aa29a0824df | 75 | double GainD2 = 2.0; |
laurencebr | 0:2aa29a0824df | 76 | double TickerPeriod = 1.0/400.0; |
laurencebr | 1:23834862b877 | 77 | // Global variables EMG |
laurencebr | 1:23834862b877 | 78 | double EMGdata1; |
laurencebr | 1:23834862b877 | 79 | double EMGdata2; |
laurencebr | 1:23834862b877 | 80 | double EMGdata3; |
laurencebr | 1:23834862b877 | 81 | double EMGdata4; |
laurencebr | 1:23834862b877 | 82 | int count; |
laurencebr | 1:23834862b877 | 83 | double EMG_filt1; |
laurencebr | 1:23834862b877 | 84 | double EMG_filt2; |
laurencebr | 1:23834862b877 | 85 | double EMG_filt3; |
laurencebr | 1:23834862b877 | 86 | double EMG_filt4; |
laurencebr | 1:23834862b877 | 87 | double unit_vx; |
laurencebr | 1:23834862b877 | 88 | double unit_vY; |
laurencebr | 0:2aa29a0824df | 89 | |
laurencebr | 0:2aa29a0824df | 90 | Ticker Kikker; |
laurencebr | 1:23834862b877 | 91 | int counter = 0; |
laurencebr | 0:2aa29a0824df | 92 | int countstep = 0; |
laurencebr | 0:2aa29a0824df | 93 | |
laurencebr | 0:2aa29a0824df | 94 | |
laurencebr | 1:23834862b877 | 95 | //EMGDINGEN |
laurencebr | 1:23834862b877 | 96 | |
laurencebr | 1:23834862b877 | 97 | void ReadEMG() |
laurencebr | 1:23834862b877 | 98 | { |
laurencebr | 1:23834862b877 | 99 | EMGdata1 = a0.read(); // store values in the scope |
laurencebr | 1:23834862b877 | 100 | EMGdata2 = a1.read(); |
laurencebr | 1:23834862b877 | 101 | EMGdata3 = a2.read(); |
laurencebr | 1:23834862b877 | 102 | EMGdata4 = a3.read(); |
laurencebr | 1:23834862b877 | 103 | } |
laurencebr | 1:23834862b877 | 104 | |
laurencebr | 1:23834862b877 | 105 | // EMG High pass filters |
laurencebr | 1:23834862b877 | 106 | double EMG_HP1(double EMGdata) //data 1 |
laurencebr | 1:23834862b877 | 107 | { |
laurencebr | 1:23834862b877 | 108 | double HP_abs_EMGdata = bqc1.step(EMGdata); |
laurencebr | 1:23834862b877 | 109 | |
laurencebr | 1:23834862b877 | 110 | return fabs(HP_abs_EMGdata); |
laurencebr | 1:23834862b877 | 111 | } |
laurencebr | 1:23834862b877 | 112 | |
laurencebr | 1:23834862b877 | 113 | double EMG_HP2(double EMGdata) //data 2 |
laurencebr | 1:23834862b877 | 114 | { |
laurencebr | 1:23834862b877 | 115 | double HP_abs_EMGdata = bqc2.step(EMGdata); |
laurencebr | 1:23834862b877 | 116 | |
laurencebr | 1:23834862b877 | 117 | return fabs(HP_abs_EMGdata); |
laurencebr | 1:23834862b877 | 118 | } |
laurencebr | 1:23834862b877 | 119 | |
laurencebr | 1:23834862b877 | 120 | double EMG_HP3(double EMGdata) //data 3 |
laurencebr | 1:23834862b877 | 121 | { |
laurencebr | 1:23834862b877 | 122 | double HP_abs_EMGdata = bqc3.step(EMGdata); |
laurencebr | 1:23834862b877 | 123 | |
laurencebr | 1:23834862b877 | 124 | return fabs(HP_abs_EMGdata); |
laurencebr | 1:23834862b877 | 125 | } |
laurencebr | 1:23834862b877 | 126 | |
laurencebr | 1:23834862b877 | 127 | double EMG_HP4(double EMGdata) // data 4 |
laurencebr | 1:23834862b877 | 128 | { |
laurencebr | 1:23834862b877 | 129 | double HP_abs_EMGdata = bqc4.step(EMGdata); |
laurencebr | 1:23834862b877 | 130 | |
laurencebr | 1:23834862b877 | 131 | return fabs(HP_abs_EMGdata); |
laurencebr | 1:23834862b877 | 132 | } |
laurencebr | 1:23834862b877 | 133 | |
laurencebr | 1:23834862b877 | 134 | // EMG moving filter |
laurencebr | 1:23834862b877 | 135 | double EMG_mean (double EMGarray[100]) |
laurencebr | 1:23834862b877 | 136 | { |
laurencebr | 1:23834862b877 | 137 | double sum = 0.0; |
laurencebr | 1:23834862b877 | 138 | |
laurencebr | 1:23834862b877 | 139 | for(int j=0; j<100; j++) |
laurencebr | 1:23834862b877 | 140 | { |
laurencebr | 1:23834862b877 | 141 | sum += EMGarray[j]; |
laurencebr | 1:23834862b877 | 142 | } |
laurencebr | 1:23834862b877 | 143 | |
laurencebr | 1:23834862b877 | 144 | double EMG_filt = sum / 100.0; |
laurencebr | 1:23834862b877 | 145 | |
laurencebr | 1:23834862b877 | 146 | return EMG_filt; |
laurencebr | 1:23834862b877 | 147 | } |
laurencebr | 1:23834862b877 | 148 | |
laurencebr | 1:23834862b877 | 149 | // Filtered signal to output between -1 and 1 |
laurencebr | 1:23834862b877 | 150 | double filt2kin (double EMG_filt1, double EMG_filt2, double max1, double max2) |
laurencebr | 1:23834862b877 | 151 | { |
laurencebr | 1:23834862b877 | 152 | double EMG_scaled1 = EMG_filt1 / max1; |
laurencebr | 1:23834862b877 | 153 | double EMG_scaled2 = EMG_filt2 / max2; |
laurencebr | 1:23834862b877 | 154 | |
laurencebr | 1:23834862b877 | 155 | double kin_input = EMG_scaled2 - EMG_scaled1; |
laurencebr | 1:23834862b877 | 156 | |
laurencebr | 1:23834862b877 | 157 | if (kin_input > 1.0) { |
laurencebr | 1:23834862b877 | 158 | kin_input = 1.0; |
laurencebr | 1:23834862b877 | 159 | } |
laurencebr | 1:23834862b877 | 160 | if (kin_input < -1.0) { |
laurencebr | 1:23834862b877 | 161 | kin_input = -1.0; |
laurencebr | 1:23834862b877 | 162 | } |
laurencebr | 1:23834862b877 | 163 | |
laurencebr | 1:23834862b877 | 164 | return kin_input; |
laurencebr | 1:23834862b877 | 165 | } |
laurencebr | 1:23834862b877 | 166 | |
laurencebr | 1:23834862b877 | 167 | void EMG () |
laurencebr | 1:23834862b877 | 168 | { |
laurencebr | 1:23834862b877 | 169 | ReadEMG(); |
laurencebr | 1:23834862b877 | 170 | |
laurencebr | 1:23834862b877 | 171 | double HP_abs_EMGdata1 = EMG_HP1(EMGdata1); |
laurencebr | 1:23834862b877 | 172 | double HP_abs_EMGdata2 = EMG_HP2(EMGdata2); |
laurencebr | 1:23834862b877 | 173 | double HP_abs_EMGdata3 = EMG_HP3(EMGdata3); |
laurencebr | 1:23834862b877 | 174 | double HP_abs_EMGdata4 = EMG_HP4(EMGdata4); |
laurencebr | 1:23834862b877 | 175 | |
laurencebr | 1:23834862b877 | 176 | static double EMG_array1[100]; |
laurencebr | 1:23834862b877 | 177 | static double EMG_array2[100]; |
laurencebr | 1:23834862b877 | 178 | static double EMG_array3[100]; |
laurencebr | 1:23834862b877 | 179 | static double EMG_array4[100]; |
laurencebr | 1:23834862b877 | 180 | |
laurencebr | 1:23834862b877 | 181 | // Fill array 1 |
laurencebr | 1:23834862b877 | 182 | for(int i = 100-1; i >= 1; i--) |
laurencebr | 1:23834862b877 | 183 | { |
laurencebr | 1:23834862b877 | 184 | EMG_array1[i] = EMG_array1[i-1]; |
laurencebr | 1:23834862b877 | 185 | } |
laurencebr | 1:23834862b877 | 186 | EMG_array1[0] = HP_abs_EMGdata1; |
laurencebr | 1:23834862b877 | 187 | |
laurencebr | 1:23834862b877 | 188 | // Fill array 2 |
laurencebr | 1:23834862b877 | 189 | for(int i = 100-1; i >= 1; i--) |
laurencebr | 1:23834862b877 | 190 | { |
laurencebr | 1:23834862b877 | 191 | EMG_array2[i] = EMG_array2[i-1]; |
laurencebr | 1:23834862b877 | 192 | } |
laurencebr | 1:23834862b877 | 193 | EMG_array2[0] = HP_abs_EMGdata2; |
laurencebr | 1:23834862b877 | 194 | |
laurencebr | 1:23834862b877 | 195 | // Fill array 3 |
laurencebr | 1:23834862b877 | 196 | for(int i = 100-1; i >= 1; i--) |
laurencebr | 1:23834862b877 | 197 | { |
laurencebr | 1:23834862b877 | 198 | EMG_array3[i] = EMG_array3[i-1]; |
laurencebr | 1:23834862b877 | 199 | } |
laurencebr | 1:23834862b877 | 200 | EMG_array3[0] = HP_abs_EMGdata3; |
laurencebr | 1:23834862b877 | 201 | |
laurencebr | 1:23834862b877 | 202 | // Fill array 4 |
laurencebr | 1:23834862b877 | 203 | for(int i = 100-1; i >= 1; i--) |
laurencebr | 1:23834862b877 | 204 | { |
laurencebr | 1:23834862b877 | 205 | EMG_array4[i] = EMG_array4[i-1]; |
laurencebr | 1:23834862b877 | 206 | } |
laurencebr | 1:23834862b877 | 207 | EMG_array4[0] = HP_abs_EMGdata4; |
laurencebr | 1:23834862b877 | 208 | |
laurencebr | 1:23834862b877 | 209 | |
laurencebr | 1:23834862b877 | 210 | EMG_filt1 = EMG_mean(EMG_array1); //global maken |
laurencebr | 1:23834862b877 | 211 | EMG_filt2 = EMG_mean(EMG_array2); |
laurencebr | 1:23834862b877 | 212 | EMG_filt3 = EMG_mean(EMG_array3); |
laurencebr | 1:23834862b877 | 213 | EMG_filt4 = EMG_mean(EMG_array4); |
laurencebr | 1:23834862b877 | 214 | |
laurencebr | 1:23834862b877 | 215 | |
laurencebr | 1:23834862b877 | 216 | |
laurencebr | 1:23834862b877 | 217 | unit_vX = filt2kin (EMG_filt1, EMG_filt2, EMG_max1, EMG_max2); |
laurencebr | 1:23834862b877 | 218 | unit_vY = filt2kin (EMG_filt3, EMG_filt4, EMG_max3, EMG_max4); |
laurencebr | 1:23834862b877 | 219 | |
laurencebr | 1:23834862b877 | 220 | scope.set(0, unit_Vx); |
laurencebr | 1:23834862b877 | 221 | scope.set(1, unit_Vy); |
laurencebr | 1:23834862b877 | 222 | //scope.set(2, EMG_filt3); |
laurencebr | 1:23834862b877 | 223 | //scope.set(3, EMG_filt4); |
laurencebr | 1:23834862b877 | 224 | |
laurencebr | 1:23834862b877 | 225 | |
laurencebr | 1:23834862b877 | 226 | count++; |
laurencebr | 1:23834862b877 | 227 | |
laurencebr | 1:23834862b877 | 228 | if (count == 100) |
laurencebr | 1:23834862b877 | 229 | { |
laurencebr | 1:23834862b877 | 230 | pc.printf("EMG filt 1 = %lf \t EMG filt 2 = %lf \t EMG filt 3 = %lf \t EMG filt 4 = %lf \n\r", EMG_filt1, EMG_filt2, EMG_filt3, EMG_filt4); |
laurencebr | 1:23834862b877 | 231 | count = 0; |
laurencebr | 1:23834862b877 | 232 | } |
laurencebr | 1:23834862b877 | 233 | } |
laurencebr | 1:23834862b877 | 234 | |
laurencebr | 1:23834862b877 | 235 | |
laurencebr | 0:2aa29a0824df | 236 | |
laurencebr | 1:23834862b877 | 237 | //PIDCONTROLLLLLLLLLL + INVERSE KINEMATIKS |
laurencebr | 1:23834862b877 | 238 | |
laurencebr | 1:23834862b877 | 239 | |
laurencebr | 1:23834862b877 | 240 | //InverseKinematics |
laurencebr | 1:23834862b877 | 241 | |
laurencebr | 1:23834862b877 | 242 | void inverse(double X1, double Y1, double & thetaM1, double & thetaM2) |
laurencebr | 0:2aa29a0824df | 243 | { |
laurencebr | 1:23834862b877 | 244 | double L1 = 40.0; // %define length of arm 1 attached to gear |
laurencebr | 1:23834862b877 | 245 | double L3 = sqrt(pow(X1,2.0)+pow(Y1,2.0)); |
laurencebr | 1:23834862b877 | 246 | double q3 = atan(Y1/X1); |
laurencebr | 1:23834862b877 | 247 | double q4 = 2.0*asin(0.5*L3/L1); |
laurencebr | 1:23834862b877 | 248 | thetaM1 = (0.5*3.1416-0.5*q4+q3)*9.0; |
laurencebr | 1:23834862b877 | 249 | thetaM2 = (3.1416-thetaM1/9.0-q4)*4.0; |
laurencebr | 1:23834862b877 | 250 | } |
laurencebr | 0:2aa29a0824df | 251 | |
laurencebr | 0:2aa29a0824df | 252 | |
laurencebr | 1:23834862b877 | 253 | void InverseKinematics ()// Kinematics function, takes imput between 1 and -1 |
laurencebr | 0:2aa29a0824df | 254 | { |
laurencebr | 1:23834862b877 | 255 | |
laurencebr | 1:23834862b877 | 256 | double V_max = 1.0; //Maximum velocity in either direction |
laurencebr | 1:23834862b877 | 257 | // CM/s |
laurencebr | 1:23834862b877 | 258 | |
laurencebr | 1:23834862b877 | 259 | double deltaX = TickerPeriod*V_max*unit_vX; // imput to delta |
laurencebr | 1:23834862b877 | 260 | double deltaY = TickerPeriod*V_max*unit_vY; |
laurencebr | 1:23834862b877 | 261 | |
laurencebr | 1:23834862b877 | 262 | static double X1; |
laurencebr | 1:23834862b877 | 263 | static double Y1; |
laurencebr | 1:23834862b877 | 264 | X1 += deltaX; |
laurencebr | 1:23834862b877 | 265 | Y1 += deltaY; |
laurencebr | 1:23834862b877 | 266 | |
laurencebr | 1:23834862b877 | 267 | double THETA1; |
laurencebr | 1:23834862b877 | 268 | double THETA2; |
laurencebr | 1:23834862b877 | 269 | |
laurencebr | 1:23834862b877 | 270 | inverse(X1, Y1, THETA1, THETA2); |
laurencebr | 1:23834862b877 | 271 | q1Ref = THETA1; |
laurencebr | 1:23834862b877 | 272 | q2Ref = THETA2; |
laurencebr | 0:2aa29a0824df | 273 | } |
laurencebr | 0:2aa29a0824df | 274 | |
laurencebr | 0:2aa29a0824df | 275 | |
laurencebr | 0:2aa29a0824df | 276 | void ReadCurrentPosition() //Update the coordinates of the end-effector q1Pos, q2Pos |
laurencebr | 0:2aa29a0824df | 277 | { |
laurencebr | 0:2aa29a0824df | 278 | q1Pos = encoder1.getPulses()/32/131.25*2*3.1416; //Position motor 1 in rad |
laurencebr | 0:2aa29a0824df | 279 | q2Pos = encoder2.getPulses()/32/131.25*2*3.1416; //Position motor 2 in rad |
laurencebr | 0:2aa29a0824df | 280 | } |
laurencebr | 0:2aa29a0824df | 281 | |
laurencebr | 0:2aa29a0824df | 282 | void UpdateError() //Update the q1Error and q2Error values based on q1Ref, q2Ref, q1Pos, q2Pos |
laurencebr | 0:2aa29a0824df | 283 | { |
laurencebr | 0:2aa29a0824df | 284 | q1Error = q1Ref - q1Pos; |
laurencebr | 0:2aa29a0824df | 285 | q2Error = q2Ref - q2Pos; |
laurencebr | 0:2aa29a0824df | 286 | |
laurencebr | 0:2aa29a0824df | 287 | //Update PrevErrorq1 and PrevErrorq2 |
laurencebr | 0:2aa29a0824df | 288 | |
laurencebr | 0:2aa29a0824df | 289 | for (int i = 0; i <= n-2 ; i++) |
laurencebr | 0:2aa29a0824df | 290 | { |
laurencebr | 0:2aa29a0824df | 291 | PrevErrorq1[i] = PrevErrorq1[i+1]; |
laurencebr | 0:2aa29a0824df | 292 | PrevErrorq2[i] = PrevErrorq2[i+1]; |
laurencebr | 0:2aa29a0824df | 293 | } |
laurencebr | 0:2aa29a0824df | 294 | |
laurencebr | 0:2aa29a0824df | 295 | PrevErrorq1[n-1] = q1Error; |
laurencebr | 0:2aa29a0824df | 296 | PrevErrorq2[n-1] = q2Error; |
laurencebr | 0:2aa29a0824df | 297 | } |
laurencebr | 0:2aa29a0824df | 298 | |
laurencebr | 0:2aa29a0824df | 299 | void PIDControl(){ |
laurencebr | 0:2aa29a0824df | 300 | // Update PIDerrorq1 and PIDerrorq2 based on the gains and the values q1Error and q2Error |
laurencebr | 0:2aa29a0824df | 301 | |
laurencebr | 0:2aa29a0824df | 302 | |
laurencebr | 0:2aa29a0824df | 303 | // PID control motor 1 |
laurencebr | 0:2aa29a0824df | 304 | //P-Control |
laurencebr | 0:2aa29a0824df | 305 | double P_error1 = GainP1 * q1Error; |
laurencebr | 0:2aa29a0824df | 306 | |
laurencebr | 0:2aa29a0824df | 307 | //I-Control |
laurencebr | 0:2aa29a0824df | 308 | IntegralErrorq1 = IntegralErrorq1 + q1Error * TickerPeriod; |
laurencebr | 0:2aa29a0824df | 309 | double I_error1 = GainI1 * IntegralErrorq1; |
laurencebr | 0:2aa29a0824df | 310 | |
laurencebr | 0:2aa29a0824df | 311 | //D-Control |
laurencebr | 0:2aa29a0824df | 312 | //First smoothen the error signal |
laurencebr | 0:2aa29a0824df | 313 | double MovAvq1_1 = 0.0; |
laurencebr | 0:2aa29a0824df | 314 | double MovAvq1_2 = 0.0; |
laurencebr | 0:2aa29a0824df | 315 | for (int i=0; i<=n-2; i++){ // Creates two mov. av. with one element shifted |
laurencebr | 0:2aa29a0824df | 316 | MovAvq1_1 = MovAvq1_1 + PrevErrorq1[i]/((double) (n-1)); |
laurencebr | 0:2aa29a0824df | 317 | MovAvq1_2 = MovAvq1_2 + PrevErrorq1[i+1]/((double)(n-1)); |
laurencebr | 0:2aa29a0824df | 318 | } |
laurencebr | 0:2aa29a0824df | 319 | DerivativeErrorq1 = (MovAvq1_2 - MovAvq1_1)/TickerPeriod; |
laurencebr | 0:2aa29a0824df | 320 | double D_error1 = GainD1 * DerivativeErrorq1; |
laurencebr | 0:2aa29a0824df | 321 | // Derivative error sum over all time? |
laurencebr | 0:2aa29a0824df | 322 | |
laurencebr | 0:2aa29a0824df | 323 | PIDerrorq1 = P_error1 + I_error1 + D_error1; |
laurencebr | 0:2aa29a0824df | 324 | |
laurencebr | 0:2aa29a0824df | 325 | // PID control motor 2 |
laurencebr | 0:2aa29a0824df | 326 | //P-Control |
laurencebr | 0:2aa29a0824df | 327 | double P_error2 = GainP2 * q2Error; |
laurencebr | 0:2aa29a0824df | 328 | |
laurencebr | 0:2aa29a0824df | 329 | //I-Control |
laurencebr | 0:2aa29a0824df | 330 | IntegralErrorq2 = IntegralErrorq2 + q2Error * TickerPeriod; |
laurencebr | 0:2aa29a0824df | 331 | double I_error2 = GainI2 * IntegralErrorq2; |
laurencebr | 0:2aa29a0824df | 332 | |
laurencebr | 0:2aa29a0824df | 333 | //D-Control |
laurencebr | 0:2aa29a0824df | 334 | //First smoothen the error signal |
laurencebr | 0:2aa29a0824df | 335 | double MovAvq2_1 = 0.0; |
laurencebr | 0:2aa29a0824df | 336 | double MovAvq2_2 = 0.0; |
laurencebr | 0:2aa29a0824df | 337 | for (int i=0; i<=n-2; i++){ // Creates two mov. av. with one element shifted |
laurencebr | 0:2aa29a0824df | 338 | MovAvq2_1 = MovAvq2_1 + PrevErrorq2[i]/((double)(n-1)); |
laurencebr | 0:2aa29a0824df | 339 | MovAvq2_2 = MovAvq2_2 + PrevErrorq2[i+1]/((double)(n-1)); |
laurencebr | 0:2aa29a0824df | 340 | } |
laurencebr | 0:2aa29a0824df | 341 | DerivativeErrorq2 = (MovAvq2_2 - MovAvq2_1)/TickerPeriod; |
laurencebr | 0:2aa29a0824df | 342 | double D_error2 = GainD2 * DerivativeErrorq2; |
laurencebr | 0:2aa29a0824df | 343 | // Derivative error sum over all time? |
laurencebr | 0:2aa29a0824df | 344 | |
laurencebr | 0:2aa29a0824df | 345 | PIDerrorq2 = P_error2 + I_error2 + D_error2; |
laurencebr | 0:2aa29a0824df | 346 | } |
laurencebr | 0:2aa29a0824df | 347 | |
laurencebr | 0:2aa29a0824df | 348 | |
laurencebr | 0:2aa29a0824df | 349 | void MotorControl() |
laurencebr | 0:2aa29a0824df | 350 | { |
laurencebr | 0:2aa29a0824df | 351 | //Motor 1 |
laurencebr | 0:2aa29a0824df | 352 | //Keep signal within bounds |
laurencebr | 0:2aa29a0824df | 353 | if (PIDerrorq1 > 1.0){ |
laurencebr | 0:2aa29a0824df | 354 | PIDerrorq1 = 1.0; |
laurencebr | 0:2aa29a0824df | 355 | } |
laurencebr | 0:2aa29a0824df | 356 | else if (PIDerrorq1 < -1.0){ |
laurencebr | 0:2aa29a0824df | 357 | PIDerrorq1 = -1.0; |
laurencebr | 0:2aa29a0824df | 358 | } |
laurencebr | 0:2aa29a0824df | 359 | //Direction |
laurencebr | 0:2aa29a0824df | 360 | if (PIDerrorq1 <= 0){ |
laurencebr | 0:2aa29a0824df | 361 | Motor1Direction = 0; |
laurencebr | 0:2aa29a0824df | 362 | Motor1PWM.write(-PIDerrorq1); //write Duty cycle |
laurencebr | 0:2aa29a0824df | 363 | } |
laurencebr | 0:2aa29a0824df | 364 | if (PIDerrorq1 >= 0){ |
laurencebr | 0:2aa29a0824df | 365 | Motor1Direction = 1; |
laurencebr | 0:2aa29a0824df | 366 | Motor1PWM.write(PIDerrorq1); //write Duty cycle |
laurencebr | 0:2aa29a0824df | 367 | } |
laurencebr | 0:2aa29a0824df | 368 | |
laurencebr | 0:2aa29a0824df | 369 | //Motor 2 |
laurencebr | 0:2aa29a0824df | 370 | //Keep signal within bounds |
laurencebr | 0:2aa29a0824df | 371 | if (PIDerrorq2 > 1.0){ |
laurencebr | 0:2aa29a0824df | 372 | PIDerrorq2 = 1.0; |
laurencebr | 0:2aa29a0824df | 373 | } |
laurencebr | 0:2aa29a0824df | 374 | else if (PIDerrorq2 < -1.0){ |
laurencebr | 0:2aa29a0824df | 375 | PIDerrorq2 = -1.0; |
laurencebr | 0:2aa29a0824df | 376 | } |
laurencebr | 0:2aa29a0824df | 377 | //Direction |
laurencebr | 0:2aa29a0824df | 378 | |
laurencebr | 0:2aa29a0824df | 379 | if (PIDerrorq2 <= 0){ |
laurencebr | 0:2aa29a0824df | 380 | Motor2Direction = 0; |
laurencebr | 0:2aa29a0824df | 381 | Motor2PWM.write(-PIDerrorq2); //write Duty cycle |
laurencebr | 0:2aa29a0824df | 382 | } |
laurencebr | 0:2aa29a0824df | 383 | if (PIDerrorq2 >= 0){ |
laurencebr | 0:2aa29a0824df | 384 | Motor2Direction = 1; |
laurencebr | 0:2aa29a0824df | 385 | Motor2PWM.write(PIDerrorq2); //write Duty cycle |
laurencebr | 0:2aa29a0824df | 386 | } |
laurencebr | 0:2aa29a0824df | 387 | } |
laurencebr | 0:2aa29a0824df | 388 | |
laurencebr | 0:2aa29a0824df | 389 | void NormalOperatingModus() |
laurencebr | 0:2aa29a0824df | 390 | { |
laurencebr | 1:23834862b877 | 391 | EMG() |
laurencebr | 0:2aa29a0824df | 392 | InverseKinematics(); |
laurencebr | 0:2aa29a0824df | 393 | ReadCurrentPosition(); |
laurencebr | 0:2aa29a0824df | 394 | UpdateError(); |
laurencebr | 0:2aa29a0824df | 395 | PIDControl(); |
laurencebr | 0:2aa29a0824df | 396 | MotorControl(); |
laurencebr | 0:2aa29a0824df | 397 | |
laurencebr | 0:2aa29a0824df | 398 | scope.set(0, q1Pos); |
laurencebr | 0:2aa29a0824df | 399 | scope.set(1, q1Ref); |
laurencebr | 0:2aa29a0824df | 400 | |
laurencebr | 0:2aa29a0824df | 401 | GainP1 = pot3.read() * 10; |
laurencebr | 2:58ec7347245e | 402 | GainI1 = 0; //pot2.read() * 10; |
laurencebr | 0:2aa29a0824df | 403 | GainD1 = pot1.read() ; |
laurencebr | 0:2aa29a0824df | 404 | |
laurencebr | 0:2aa29a0824df | 405 | countstep++; |
laurencebr | 1:23834862b877 | 406 | counter++; |
laurencebr | 1:23834862b877 | 407 | if (counter == 400) // print 1x per seconde waardes. |
laurencebr | 0:2aa29a0824df | 408 | { |
laurencebr | 0:2aa29a0824df | 409 | pc.printf("GainP1 = %1f, GainI1 = %1f, GainD1 = %1f, q1Pos = %lf, q1Ref = %1f \n\r", GainP1, GainI1, GainD1, q1Pos, q1Ref); |
laurencebr | 1:23834862b877 | 410 | counter = 0; |
laurencebr | 0:2aa29a0824df | 411 | } |
laurencebr | 0:2aa29a0824df | 412 | if (countstep == 4000) |
laurencebr | 0:2aa29a0824df | 413 | { |
laurencebr | 0:2aa29a0824df | 414 | q1Ref = !q1Ref; |
laurencebr | 0:2aa29a0824df | 415 | countstep = 0; |
laurencebr | 0:2aa29a0824df | 416 | } |
laurencebr | 0:2aa29a0824df | 417 | |
laurencebr | 0:2aa29a0824df | 418 | |
laurencebr | 0:2aa29a0824df | 419 | } |
laurencebr | 0:2aa29a0824df | 420 | |
laurencebr | 0:2aa29a0824df | 421 | |
laurencebr | 0:2aa29a0824df | 422 | int main() |
laurencebr | 0:2aa29a0824df | 423 | { |
laurencebr | 0:2aa29a0824df | 424 | pc.baud(115200); |
laurencebr | 1:23834862b877 | 425 | |
laurencebr | 1:23834862b877 | 426 | //BiQuad chains |
laurencebr | 1:23834862b877 | 427 | bqc1.add( &HP_emg1 ).add( &Notch_emg1 ); |
laurencebr | 1:23834862b877 | 428 | bqc2.add( &HP_emg2 ).add( &Notch_emg2 ); |
laurencebr | 1:23834862b877 | 429 | bqc3.add( &HP_emg3 ).add( &Notch_emg3 ); |
laurencebr | 1:23834862b877 | 430 | bqc4.add( &HP_emg4 ).add( &Notch_emg4 ); |
laurencebr | 1:23834862b877 | 431 | |
laurencebr | 0:2aa29a0824df | 432 | //Initialize array errors to 0 |
laurencebr | 0:2aa29a0824df | 433 | for (int i = 0; i <= 9; i++){ |
laurencebr | 0:2aa29a0824df | 434 | PrevErrorq2[i] = 0; |
laurencebr | 0:2aa29a0824df | 435 | PrevErrorq2[i] = 0; |
laurencebr | 0:2aa29a0824df | 436 | } |
laurencebr | 0:2aa29a0824df | 437 | |
laurencebr | 0:2aa29a0824df | 438 | int frequency_pwm = 16700; //16.7 kHz PWM |
laurencebr | 0:2aa29a0824df | 439 | Motor1PWM.period(1.0/frequency_pwm); // T = 1/f |
laurencebr | 0:2aa29a0824df | 440 | Motor2PWM.period(1.0/frequency_pwm); // T = 1/f |
laurencebr | 0:2aa29a0824df | 441 | |
laurencebr | 0:2aa29a0824df | 442 | Kikker.attach(NormalOperatingModus, TickerPeriod); |
laurencebr | 0:2aa29a0824df | 443 | scopeTimer.attach_us(&scope, &HIDScope::send, 2e4); |
laurencebr | 0:2aa29a0824df | 444 | |
laurencebr | 1:23834862b877 | 445 | //Onderstaande stond in EMG deel, kijken hoe en wat met tickers!! |
laurencebr | 1:23834862b877 | 446 | // Attach the HIDScope::send method from the scope object to the timer at 50Hz |
laurencebr | 1:23834862b877 | 447 | scopeTimer.attach_us(&scope, &HIDScope::send, 5e3); |
laurencebr | 1:23834862b877 | 448 | //EMGTicker.attach_us(EMG_filtering, 5e3); |
laurencebr | 1:23834862b877 | 449 | //. |
laurencebr | 1:23834862b877 | 450 | |
laurencebr | 0:2aa29a0824df | 451 | while(true); |
laurencebr | 0:2aa29a0824df | 452 | {} |
laurencebr | 0:2aa29a0824df | 453 | } |
laurencebr | 0:2aa29a0824df | 454 | |
laurencebr | 0:2aa29a0824df | 455 | |
laurencebr | 0:2aa29a0824df | 456 | |
laurencebr | 0:2aa29a0824df | 457 |