Laurence B / Mbed 2 deprecated VolledigeScript

alles ingevoegd

Dependencies:   FastPWM HIDScope MODSERIAL QEI mbed biquadFilter

Fork of deelPID by Laurence B

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?

UserRevisionLine numberNew 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