Laurence B / Mbed 2 deprecated VolledigeScript

alles ingevoegd

Dependencies:   FastPWM HIDScope MODSERIAL QEI mbed biquadFilter

Fork of deelPID by Laurence B

main.cpp@8:fa268d163bbd, 2018-11-03 (annotated)

Committer:
laurencebr
Date:
Sat Nov 03 09:51:42 2018 +0000
Revision:
8:fa268d163bbd
Parent:
7:de221f894d3b 
Final version voor presentatie

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 8:fa268d163bbd 3 //#include "MODSERIAL.h"
laurencebr 0:2aa29a0824df 4 #include "QEI.h"
laurencebr 8:fa268d163bbd 5 //#include "HIDScope.h"
laurencebr 3:53fb8bd0a448 6 #include "BiQuad.h"
laurencebr 0:2aa29a0824df 7
laurencebr 5:8e326d07f125 8 DigitalIn ButtonCal(D2); //Button 1
laurencebr 8:fa268d163bbd 9 DigitalIn ButtonDemo(D1);; //Button 2
laurencebr 0:2aa29a0824df 10 DigitalOut gpo(D0);
laurencebr 5:8e326d07f125 11 DigitalOut ledred(LED_RED);
laurencebr 5:8e326d07f125 12 DigitalOut ledblue(LED_BLUE);
laurencebr 5:8e326d07f125 13 DigitalOut ledgreen(LED_GREEN);
laurencebr 8:fa268d163bbd 14 //AnalogIn pot1(A4); //POORTEN VERANDEREN
laurencebr 5:8e326d07f125 15 //AnalogIn pot2(A3); //Beneden is I control op 0 gezet. //POORTEN veranderen
laurencebr 8:fa268d163bbd 16 //AnalogIn pot3(A5); //POORTEN VERANDEREN
laurencebr 0:2aa29a0824df 17 QEI encoder1(D10, D11, NC, 32);
laurencebr 0:2aa29a0824df 18 QEI encoder2(D12, D13, NC, 32);
laurencebr 0:2aa29a0824df 19 FastPWM Motor1PWM(D6);
laurencebr 0:2aa29a0824df 20 DigitalOut Motor1Direction(D7);
laurencebr 2:58ec7347245e 21 FastPWM Motor2PWM(D5);
laurencebr 2:58ec7347245e 22 DigitalOut Motor2Direction(D4);
laurencebr 1:23834862b877 23 //EMG
laurencebr 6:405ec2bba6d0 24
laurencebr 8:fa268d163bbd 25
laurencebr 1:23834862b877 26 AnalogIn a0(A0);
laurencebr 1:23834862b877 27 AnalogIn a1(A1);
laurencebr 1:23834862b877 28 AnalogIn a2(A2);
laurencebr 5:8e326d07f125 29 AnalogIn a3(A3);
laurencebr 8:fa268d163bbd 30
laurencebr 6:405ec2bba6d0 31
laurencebr 8:fa268d163bbd 32 /*AnalogIn Ppot(A0);
laurencebr 6:405ec2bba6d0 33 AnalogIn Ipot(A1);
laurencebr 6:405ec2bba6d0 34 AnalogIn Dpot(A2);
laurencebr 8:fa268d163bbd 35 */
laurencebr 7:de221f894d3b 36
laurencebr 6:405ec2bba6d0 37
laurencebr 8:fa268d163bbd 38 //MODSERIAL pc(USBTX, USBRX);
laurencebr 0:2aa29a0824df 39
laurencebr 1:23834862b877 40 //HIDSCOPE //Oppassen waar we HIDSCOPE aanroepen want nu voor meerdere dingen
laurencebr 8:fa268d163bbd 41 //HIDScope scope(6);
laurencebr 8:fa268d163bbd 42 //Ticker scopeTimer;
laurencebr 0:2aa29a0824df 43
laurencebr 1:23834862b877 44 Ticker EMGTicker;
laurencebr 1:23834862b877 45
laurencebr 1:23834862b877 46 // BiQuad filters
laurencebr 1:23834862b877 47 //BiQuad Chains
laurencebr 1:23834862b877 48 BiQuadChain bqc1;
laurencebr 1:23834862b877 49 BiQuadChain bqc2;
laurencebr 1:23834862b877 50 BiQuadChain bqc3;
laurencebr 1:23834862b877 51 BiQuadChain bqc4;
laurencebr 1:23834862b877 52
laurencebr 1:23834862b877 53 //High pass filters 20 Hz
laurencebr 6:405ec2bba6d0 54 BiQuad HP_emg1(1,-2,1,-1.647459981076977,0.700896781188403);
laurencebr 6:405ec2bba6d0 55 BiQuad HP_emg2(1,-2,1,-1.647459981076977,0.700896781188403);
laurencebr 6:405ec2bba6d0 56 BiQuad HP_emg3(1,-2,1,-1.647459981076977,0.700896781188403);
laurencebr 6:405ec2bba6d0 57 BiQuad HP_emg4(1,-2,1,-1.647459981076977,0.700896781188403);
laurencebr 1:23834862b877 58
laurencebr 1:23834862b877 59 //Notch filters 50 Hz
laurencebr 6:405ec2bba6d0 60 BiQuad Notch_emg1(1,-1.619184463611223,1,-1.560333397539390,0.927307768331003);
laurencebr 6:405ec2bba6d0 61 BiQuad Notch_emg2(1,-1.619184463611223,1,-1.560333397539390,0.927307768331003);
laurencebr 6:405ec2bba6d0 62 BiQuad Notch_emg3(1,-1.619184463611223,1,-1.560333397539390,0.927307768331003);
laurencebr 6:405ec2bba6d0 63 BiQuad Notch_emg4(1,-1.619184463611223,1,-1.560333397539390,0.927307768331003);
laurencebr 6:405ec2bba6d0 64
laurencebr 6:405ec2bba6d0 65 //LP 98
laurencebr 6:405ec2bba6d0 66 BiQuad LP1(1,2,1,1.911197067426073,0.914975834801434);
laurencebr 6:405ec2bba6d0 67 BiQuad LP2(1,2,1,1.911197067426073,0.914975834801434);
laurencebr 6:405ec2bba6d0 68 BiQuad LP3(1,2,1,1.911197067426073,0.914975834801434);
laurencebr 6:405ec2bba6d0 69 BiQuad LP4(1,2,1,1.911197067426073,0.914975834801434);
laurencebr 6:405ec2bba6d0 70
laurencebr 7:de221f894d3b 71 // lowpass derivative
laurencebr 7:de221f894d3b 72 BiQuad LPD1(1,2,1,-1.734725768809275,0.766006600943264);
laurencebr 7:de221f894d3b 73 BiQuad LPD2(1,2,1,-1.734725768809275,0.766006600943264);
laurencebr 0:2aa29a0824df 74
laurencebr 0:2aa29a0824df 75 ///Variables
laurencebr 0:2aa29a0824df 76
laurencebr 0:2aa29a0824df 77 double q1Error;
laurencebr 0:2aa29a0824df 78 double q2Error;
laurencebr 0:2aa29a0824df 79 double PrevErrorq1[100];
laurencebr 0:2aa29a0824df 80 double PrevErrorq2[100];
laurencebr 0:2aa29a0824df 81 int n = 100; // Zelfde waarde als PrevErrorarray
laurencebr 0:2aa29a0824df 82 double q1Ref = 1.0; //VOOR TESTEN
laurencebr 0:2aa29a0824df 83 double q2Ref;
laurencebr 3:53fb8bd0a448 84 double xRef = 40.0;
laurencebr 3:53fb8bd0a448 85 double yRef = 40.0;
laurencebr 0:2aa29a0824df 86 double q1Pos;
laurencebr 0:2aa29a0824df 87 double q2Pos;
laurencebr 0:2aa29a0824df 88 double PIDerrorq1;
laurencebr 0:2aa29a0824df 89 double PIDerrorq2;
laurencebr 0:2aa29a0824df 90 double IntegralErrorq1 = 0.0;
laurencebr 0:2aa29a0824df 91 double IntegralErrorq2 = 0.0;
laurencebr 0:2aa29a0824df 92 double DerivativeErrorq1 = 0.0;
laurencebr 0:2aa29a0824df 93 double DerivativeErrorq2 = 0.0;
laurencebr 4:93c4e826d11d 94 double GainP1 = 3.0;
laurencebr 4:93c4e826d11d 95 double GainI1 = 0.0;
laurencebr 4:93c4e826d11d 96 double GainD1 = 0.235;
laurencebr 4:93c4e826d11d 97 double GainP2 = 3.0;
laurencebr 4:93c4e826d11d 98 double GainI2 = 0.0;
laurencebr 4:93c4e826d11d 99 double GainD2 = 0.235;
laurencebr 6:405ec2bba6d0 100 double TickerPeriod = 1.0/500.0;
laurencebr 1:23834862b877 101 // Global variables EMG
laurencebr 1:23834862b877 102 double EMGdata1;
laurencebr 1:23834862b877 103 double EMGdata2;
laurencebr 1:23834862b877 104 double EMGdata3;
laurencebr 1:23834862b877 105 double EMGdata4;
laurencebr 3:53fb8bd0a448 106 int count = 0;
laurencebr 1:23834862b877 107 double EMG_filt1;
laurencebr 1:23834862b877 108 double EMG_filt2;
laurencebr 1:23834862b877 109 double EMG_filt3;
laurencebr 1:23834862b877 110 double EMG_filt4;
laurencebr 3:53fb8bd0a448 111
laurencebr 3:53fb8bd0a448 112 double EMG_max1 = 10000.0;
laurencebr 3:53fb8bd0a448 113 double EMG_max2 = 10000.0;
laurencebr 3:53fb8bd0a448 114 double EMG_max3 = 10000.0;
laurencebr 3:53fb8bd0a448 115 double EMG_max4 = 10000.0;
laurencebr 3:53fb8bd0a448 116
laurencebr 3:53fb8bd0a448 117 double unit_vX;
laurencebr 1:23834862b877 118 double unit_vY;
laurencebr 0:2aa29a0824df 119
laurencebr 0:2aa29a0824df 120 Ticker Kikker;
laurencebr 1:23834862b877 121 int counter = 0;
laurencebr 0:2aa29a0824df 122 int countstep = 0;
laurencebr 0:2aa29a0824df 123
laurencebr 3:53fb8bd0a448 124 //Demo variabelen
laurencebr 6:405ec2bba6d0 125 double vxMax = 1.0;
laurencebr 6:405ec2bba6d0 126 double vyMax = 1.0;
laurencebr 3:53fb8bd0a448 127 int DemoStage = 0;
laurencebr 3:53fb8bd0a448 128
laurencebr 5:8e326d07f125 129 //States
laurencebr 5:8e326d07f125 130 enum states {WaitModusState, EMGCalibrationState, NormalOperatingModusState, DemoModusState};
laurencebr 8:fa268d163bbd 131 states State = WaitModusState;
laurencebr 3:53fb8bd0a448 132
laurencebr 0:2aa29a0824df 133
laurencebr 5:8e326d07f125 134 //Calibration Time
laurencebr 5:8e326d07f125 135
laurencebr 5:8e326d07f125 136 int countcalibration = 0;
laurencebr 5:8e326d07f125 137 double CalibrationTime = 20.0; //Tijd om te calibreren seconden
laurencebr 5:8e326d07f125 138
laurencebr 1:23834862b877 139 //EMGDINGEN
laurencebr 1:23834862b877 140
laurencebr 1:23834862b877 141 void ReadEMG()
laurencebr 1:23834862b877 142 {
laurencebr 1:23834862b877 143 EMGdata1 = a0.read(); // store values in the scope
laurencebr 1:23834862b877 144 EMGdata2 = a1.read();
laurencebr 1:23834862b877 145 EMGdata3 = a2.read();
laurencebr 5:8e326d07f125 146 EMGdata4 = a3.read();
laurencebr 8:fa268d163bbd 147
laurencebr 1:23834862b877 148 }
laurencebr 1:23834862b877 149
laurencebr 1:23834862b877 150 // EMG High pass filters
laurencebr 1:23834862b877 151 double EMG_HP1(double EMGdata) //data 1
laurencebr 1:23834862b877 152 {
laurencebr 1:23834862b877 153 double HP_abs_EMGdata = bqc1.step(EMGdata);
laurencebr 1:23834862b877 154
laurencebr 1:23834862b877 155 return fabs(HP_abs_EMGdata);
laurencebr 1:23834862b877 156 }
laurencebr 1:23834862b877 157
laurencebr 1:23834862b877 158 double EMG_HP2(double EMGdata) //data 2
laurencebr 1:23834862b877 159 {
laurencebr 1:23834862b877 160 double HP_abs_EMGdata = bqc2.step(EMGdata);
laurencebr 1:23834862b877 161
laurencebr 1:23834862b877 162 return fabs(HP_abs_EMGdata);
laurencebr 1:23834862b877 163 }
laurencebr 1:23834862b877 164
laurencebr 1:23834862b877 165 double EMG_HP3(double EMGdata) //data 3
laurencebr 1:23834862b877 166 {
laurencebr 1:23834862b877 167 double HP_abs_EMGdata = bqc3.step(EMGdata);
laurencebr 1:23834862b877 168
laurencebr 1:23834862b877 169 return fabs(HP_abs_EMGdata);
laurencebr 1:23834862b877 170 }
laurencebr 1:23834862b877 171
laurencebr 1:23834862b877 172 double EMG_HP4(double EMGdata) // data 4
laurencebr 1:23834862b877 173 {
laurencebr 1:23834862b877 174 double HP_abs_EMGdata = bqc4.step(EMGdata);
laurencebr 1:23834862b877 175
laurencebr 1:23834862b877 176 return fabs(HP_abs_EMGdata);
laurencebr 1:23834862b877 177 }
laurencebr 1:23834862b877 178
laurencebr 1:23834862b877 179 // EMG moving filter
laurencebr 1:23834862b877 180 double EMG_mean (double EMGarray[100])
laurencebr 1:23834862b877 181 {
laurencebr 1:23834862b877 182 double sum = 0.0;
laurencebr 1:23834862b877 183
laurencebr 1:23834862b877 184 for(int j=0; j<100; j++)
laurencebr 1:23834862b877 185 {
laurencebr 1:23834862b877 186 sum += EMGarray[j];
laurencebr 1:23834862b877 187 }
laurencebr 1:23834862b877 188
laurencebr 1:23834862b877 189 double EMG_filt = sum / 100.0;
laurencebr 1:23834862b877 190
laurencebr 1:23834862b877 191 return EMG_filt;
laurencebr 1:23834862b877 192 }
laurencebr 1:23834862b877 193
laurencebr 1:23834862b877 194 // Filtered signal to output between -1 and 1
laurencebr 1:23834862b877 195 double filt2kin (double EMG_filt1, double EMG_filt2, double max1, double max2)
laurencebr 1:23834862b877 196 {
laurencebr 1:23834862b877 197 double EMG_scaled1 = EMG_filt1 / max1;
laurencebr 1:23834862b877 198 double EMG_scaled2 = EMG_filt2 / max2;
laurencebr 1:23834862b877 199
laurencebr 1:23834862b877 200 double kin_input = EMG_scaled2 - EMG_scaled1;
laurencebr 1:23834862b877 201
laurencebr 1:23834862b877 202 if (kin_input > 1.0) {
laurencebr 1:23834862b877 203 kin_input = 1.0;
laurencebr 1:23834862b877 204 }
laurencebr 1:23834862b877 205 if (kin_input < -1.0) {
laurencebr 1:23834862b877 206 kin_input = -1.0;
laurencebr 1:23834862b877 207 }
laurencebr 1:23834862b877 208
laurencebr 1:23834862b877 209 return kin_input;
laurencebr 1:23834862b877 210 }
laurencebr 1:23834862b877 211
laurencebr 1:23834862b877 212 void EMG ()
laurencebr 1:23834862b877 213 {
laurencebr 1:23834862b877 214 ReadEMG();
laurencebr 1:23834862b877 215
laurencebr 1:23834862b877 216 double HP_abs_EMGdata1 = EMG_HP1(EMGdata1);
laurencebr 1:23834862b877 217 double HP_abs_EMGdata2 = EMG_HP2(EMGdata2);
laurencebr 1:23834862b877 218 double HP_abs_EMGdata3 = EMG_HP3(EMGdata3);
laurencebr 1:23834862b877 219 double HP_abs_EMGdata4 = EMG_HP4(EMGdata4);
laurencebr 1:23834862b877 220
laurencebr 1:23834862b877 221 static double EMG_array1[100];
laurencebr 1:23834862b877 222 static double EMG_array2[100];
laurencebr 1:23834862b877 223 static double EMG_array3[100];
laurencebr 1:23834862b877 224 static double EMG_array4[100];
laurencebr 1:23834862b877 225
laurencebr 1:23834862b877 226 // Fill array 1
laurencebr 1:23834862b877 227 for(int i = 100-1; i >= 1; i--)
laurencebr 1:23834862b877 228 {
laurencebr 1:23834862b877 229 EMG_array1[i] = EMG_array1[i-1];
laurencebr 1:23834862b877 230 }
laurencebr 1:23834862b877 231 EMG_array1[0] = HP_abs_EMGdata1;
laurencebr 1:23834862b877 232
laurencebr 1:23834862b877 233 // Fill array 2
laurencebr 1:23834862b877 234 for(int i = 100-1; i >= 1; i--)
laurencebr 1:23834862b877 235 {
laurencebr 1:23834862b877 236 EMG_array2[i] = EMG_array2[i-1];
laurencebr 1:23834862b877 237 }
laurencebr 1:23834862b877 238 EMG_array2[0] = HP_abs_EMGdata2;
laurencebr 1:23834862b877 239
laurencebr 1:23834862b877 240 // Fill array 3
laurencebr 1:23834862b877 241 for(int i = 100-1; i >= 1; i--)
laurencebr 1:23834862b877 242 {
laurencebr 1:23834862b877 243 EMG_array3[i] = EMG_array3[i-1];
laurencebr 1:23834862b877 244 }
laurencebr 1:23834862b877 245 EMG_array3[0] = HP_abs_EMGdata3;
laurencebr 1:23834862b877 246
laurencebr 1:23834862b877 247 // Fill array 4
laurencebr 1:23834862b877 248 for(int i = 100-1; i >= 1; i--)
laurencebr 1:23834862b877 249 {
laurencebr 1:23834862b877 250 EMG_array4[i] = EMG_array4[i-1];
laurencebr 1:23834862b877 251 }
laurencebr 1:23834862b877 252 EMG_array4[0] = HP_abs_EMGdata4;
laurencebr 1:23834862b877 253
laurencebr 1:23834862b877 254
laurencebr 1:23834862b877 255 EMG_filt1 = EMG_mean(EMG_array1); //global maken
laurencebr 1:23834862b877 256 EMG_filt2 = EMG_mean(EMG_array2);
laurencebr 1:23834862b877 257 EMG_filt3 = EMG_mean(EMG_array3);
laurencebr 1:23834862b877 258 EMG_filt4 = EMG_mean(EMG_array4);
laurencebr 1:23834862b877 259
laurencebr 1:23834862b877 260
laurencebr 1:23834862b877 261
laurencebr 1:23834862b877 262 unit_vX = filt2kin (EMG_filt1, EMG_filt2, EMG_max1, EMG_max2);
laurencebr 1:23834862b877 263 unit_vY = filt2kin (EMG_filt3, EMG_filt4, EMG_max3, EMG_max4);
laurencebr 5:8e326d07f125 264 if (fabs(unit_vX)<0.1)
laurencebr 5:8e326d07f125 265 {
laurencebr 5:8e326d07f125 266 unit_vX = 0.0;
laurencebr 5:8e326d07f125 267 }
laurencebr 5:8e326d07f125 268 if (fabs(unit_vY)<0.1)
laurencebr 5:8e326d07f125 269 {
laurencebr 5:8e326d07f125 270 unit_vY = 0.0;
laurencebr 5:8e326d07f125 271 }
laurencebr 5:8e326d07f125 272
laurencebr 1:23834862b877 273
laurencebr 8:fa268d163bbd 274 /* scope.set(0, EMG_filt1);
laurencebr 6:405ec2bba6d0 275 scope.set(1, EMG_filt2);
laurencebr 6:405ec2bba6d0 276 scope.set(2, unit_vX);
laurencebr 6:405ec2bba6d0 277 scope.set(3, EMG_filt3);
laurencebr 6:405ec2bba6d0 278 scope.set(4, EMG_filt4);
laurencebr 6:405ec2bba6d0 279 scope.set(5, unit_vY);
laurencebr 8:fa268d163bbd 280 */
laurencebr 1:23834862b877 281
laurencebr 1:23834862b877 282 count++;
laurencebr 1:23834862b877 283
laurencebr 1:23834862b877 284 if (count == 100)
laurencebr 1:23834862b877 285 {
laurencebr 8:fa268d163bbd 286 //pc.printf("xRef = %1f, yRef = %1f, q2Ref = %1f, q1Ref = %1f \n\r", xRef, yRef, q2Ref, q1Ref);
laurencebr 1:23834862b877 287 count = 0;
laurencebr 1:23834862b877 288 }
laurencebr 1:23834862b877 289 }
laurencebr 1:23834862b877 290
laurencebr 1:23834862b877 291
laurencebr 0:2aa29a0824df 292
laurencebr 1:23834862b877 293 //PIDCONTROLLLLLLLLLL + INVERSE KINEMATIKS
laurencebr 1:23834862b877 294
laurencebr 1:23834862b877 295
laurencebr 1:23834862b877 296 //InverseKinematics
laurencebr 1:23834862b877 297
laurencebr 3:53fb8bd0a448 298 void inverse()
laurencebr 0:2aa29a0824df 299 {
laurencebr 1:23834862b877 300 double L1 = 40.0; // %define length of arm 1 attached to gear
laurencebr 3:53fb8bd0a448 301 double L3 = sqrt(pow(xRef,2.0)+pow(yRef,2.0));
laurencebr 3:53fb8bd0a448 302 double q3 = atan(yRef/xRef);
laurencebr 1:23834862b877 303 double q4 = 2.0*asin(0.5*L3/L1);
laurencebr 3:53fb8bd0a448 304 q1Ref = (0.5*3.1416-0.5*q4+q3)*9.0;
laurencebr 3:53fb8bd0a448 305 q2Ref = (3.1416-q1Ref/9.0-q4)*4.0;
laurencebr 1:23834862b877 306 }
laurencebr 0:2aa29a0824df 307
laurencebr 0:2aa29a0824df 308
laurencebr 1:23834862b877 309 void InverseKinematics ()// Kinematics function, takes imput between 1 and -1
laurencebr 0:2aa29a0824df 310 {
laurencebr 1:23834862b877 311
laurencebr 6:405ec2bba6d0 312 double V_max = 2.5; //Maximum velocity in either direction
laurencebr 1:23834862b877 313 // CM/s
laurencebr 1:23834862b877 314
laurencebr 1:23834862b877 315 double deltaX = TickerPeriod*V_max*unit_vX; // imput to delta
laurencebr 1:23834862b877 316 double deltaY = TickerPeriod*V_max*unit_vY;
laurencebr 1:23834862b877 317
laurencebr 3:53fb8bd0a448 318 xRef += deltaX;
laurencebr 3:53fb8bd0a448 319 yRef += deltaY;
laurencebr 1:23834862b877 320
laurencebr 3:53fb8bd0a448 321 inverse();
laurencebr 1:23834862b877 322
laurencebr 0:2aa29a0824df 323 }
laurencebr 0:2aa29a0824df 324
laurencebr 0:2aa29a0824df 325
laurencebr 0:2aa29a0824df 326 void ReadCurrentPosition() //Update the coordinates of the end-effector q1Pos, q2Pos
laurencebr 0:2aa29a0824df 327 {
laurencebr 3:53fb8bd0a448 328 q1Pos = -encoder1.getPulses()/32/131.25*2*3.1416 + 3.1416/2.0*9.0; //Position motor 1 in rad
laurencebr 0:2aa29a0824df 329 q2Pos = encoder2.getPulses()/32/131.25*2*3.1416; //Position motor 2 in rad
laurencebr 0:2aa29a0824df 330 }
laurencebr 0:2aa29a0824df 331
laurencebr 0:2aa29a0824df 332 void UpdateError() //Update the q1Error and q2Error values based on q1Ref, q2Ref, q1Pos, q2Pos
laurencebr 0:2aa29a0824df 333 {
laurencebr 0:2aa29a0824df 334 q1Error = q1Ref - q1Pos;
laurencebr 0:2aa29a0824df 335 q2Error = q2Ref - q2Pos;
laurencebr 0:2aa29a0824df 336
laurencebr 0:2aa29a0824df 337 //Update PrevErrorq1 and PrevErrorq2
laurencebr 0:2aa29a0824df 338
laurencebr 0:2aa29a0824df 339 for (int i = 0; i <= n-2 ; i++)
laurencebr 0:2aa29a0824df 340 {
laurencebr 0:2aa29a0824df 341 PrevErrorq1[i] = PrevErrorq1[i+1];
laurencebr 0:2aa29a0824df 342 PrevErrorq2[i] = PrevErrorq2[i+1];
laurencebr 0:2aa29a0824df 343 }
laurencebr 0:2aa29a0824df 344
laurencebr 0:2aa29a0824df 345 PrevErrorq1[n-1] = q1Error;
laurencebr 0:2aa29a0824df 346 PrevErrorq2[n-1] = q2Error;
laurencebr 0:2aa29a0824df 347 }
laurencebr 0:2aa29a0824df 348
laurencebr 0:2aa29a0824df 349 void PIDControl(){
laurencebr 0:2aa29a0824df 350 // Update PIDerrorq1 and PIDerrorq2 based on the gains and the values q1Error and q2Error
laurencebr 0:2aa29a0824df 351
laurencebr 0:2aa29a0824df 352
laurencebr 0:2aa29a0824df 353 // PID control motor 1
laurencebr 0:2aa29a0824df 354 //P-Control
laurencebr 0:2aa29a0824df 355 double P_error1 = GainP1 * q1Error;
laurencebr 0:2aa29a0824df 356
laurencebr 0:2aa29a0824df 357 //I-Control
laurencebr 0:2aa29a0824df 358 IntegralErrorq1 = IntegralErrorq1 + q1Error * TickerPeriod;
laurencebr 0:2aa29a0824df 359 double I_error1 = GainI1 * IntegralErrorq1;
laurencebr 0:2aa29a0824df 360
laurencebr 0:2aa29a0824df 361 //D-Control
laurencebr 0:2aa29a0824df 362 //First smoothen the error signal
laurencebr 7:de221f894d3b 363
laurencebr 0:2aa29a0824df 364 double MovAvq1_1 = 0.0;
laurencebr 0:2aa29a0824df 365 double MovAvq1_2 = 0.0;
laurencebr 7:de221f894d3b 366
laurencebr 0:2aa29a0824df 367 for (int i=0; i<=n-2; i++){ // Creates two mov. av. with one element shifted
laurencebr 0:2aa29a0824df 368 MovAvq1_1 = MovAvq1_1 + PrevErrorq1[i]/((double) (n-1));
laurencebr 0:2aa29a0824df 369 MovAvq1_2 = MovAvq1_2 + PrevErrorq1[i+1]/((double)(n-1));
laurencebr 0:2aa29a0824df 370 }
laurencebr 7:de221f894d3b 371 DerivativeErrorq1 = (MovAvq1_2 - MovAvq1_1)/TickerPeriod;
laurencebr 0:2aa29a0824df 372 double D_error1 = GainD1 * DerivativeErrorq1;
laurencebr 7:de221f894d3b 373
laurencebr 0:2aa29a0824df 374 // Derivative error sum over all time?
laurencebr 7:de221f894d3b 375
laurencebr 7:de221f894d3b 376
laurencebr 0:2aa29a0824df 377 PIDerrorq1 = P_error1 + I_error1 + D_error1;
laurencebr 0:2aa29a0824df 378
laurencebr 0:2aa29a0824df 379 // PID control motor 2
laurencebr 0:2aa29a0824df 380 //P-Control
laurencebr 0:2aa29a0824df 381 double P_error2 = GainP2 * q2Error;
laurencebr 0:2aa29a0824df 382
laurencebr 0:2aa29a0824df 383 //I-Control
laurencebr 0:2aa29a0824df 384 IntegralErrorq2 = IntegralErrorq2 + q2Error * TickerPeriod;
laurencebr 0:2aa29a0824df 385 double I_error2 = GainI2 * IntegralErrorq2;
laurencebr 0:2aa29a0824df 386
laurencebr 0:2aa29a0824df 387 //D-Control
laurencebr 0:2aa29a0824df 388 //First smoothen the error signal
laurencebr 7:de221f894d3b 389
laurencebr 0:2aa29a0824df 390 double MovAvq2_1 = 0.0;
laurencebr 0:2aa29a0824df 391 double MovAvq2_2 = 0.0;
laurencebr 0:2aa29a0824df 392 for (int i=0; i<=n-2; i++){ // Creates two mov. av. with one element shifted
laurencebr 0:2aa29a0824df 393 MovAvq2_1 = MovAvq2_1 + PrevErrorq2[i]/((double)(n-1));
laurencebr 0:2aa29a0824df 394 MovAvq2_2 = MovAvq2_2 + PrevErrorq2[i+1]/((double)(n-1));
laurencebr 0:2aa29a0824df 395 }
laurencebr 0:2aa29a0824df 396 DerivativeErrorq2 = (MovAvq2_2 - MovAvq2_1)/TickerPeriod;
laurencebr 0:2aa29a0824df 397 double D_error2 = GainD2 * DerivativeErrorq2;
laurencebr 7:de221f894d3b 398
laurencebr 7:de221f894d3b 399
laurencebr 0:2aa29a0824df 400 // Derivative error sum over all time?
laurencebr 0:2aa29a0824df 401
laurencebr 0:2aa29a0824df 402 PIDerrorq2 = P_error2 + I_error2 + D_error2;
laurencebr 0:2aa29a0824df 403 }
laurencebr 0:2aa29a0824df 404
laurencebr 0:2aa29a0824df 405
laurencebr 0:2aa29a0824df 406 void MotorControl()
laurencebr 0:2aa29a0824df 407 {
laurencebr 0:2aa29a0824df 408 //Motor 1
laurencebr 0:2aa29a0824df 409 //Keep signal within bounds
laurencebr 4:93c4e826d11d 410 if (PIDerrorq1 > 1.0){ //tijdelijk 0.6, hoort 1.0 te zijn
laurencebr 4:93c4e826d11d 411 PIDerrorq1 = 1.0;
laurencebr 0:2aa29a0824df 412 }
laurencebr 4:93c4e826d11d 413 else if (PIDerrorq1 < -1.0){
laurencebr 4:93c4e826d11d 414 PIDerrorq1 = -1.0;
laurencebr 0:2aa29a0824df 415 }
laurencebr 0:2aa29a0824df 416 //Direction
laurencebr 0:2aa29a0824df 417 if (PIDerrorq1 <= 0){
laurencebr 3:53fb8bd0a448 418 Motor1Direction = 1;
laurencebr 0:2aa29a0824df 419 Motor1PWM.write(-PIDerrorq1); //write Duty cycle
laurencebr 0:2aa29a0824df 420 }
laurencebr 0:2aa29a0824df 421 if (PIDerrorq1 >= 0){
laurencebr 3:53fb8bd0a448 422 Motor1Direction = 0;
laurencebr 0:2aa29a0824df 423 Motor1PWM.write(PIDerrorq1); //write Duty cycle
laurencebr 0:2aa29a0824df 424 }
laurencebr 0:2aa29a0824df 425
laurencebr 0:2aa29a0824df 426 //Motor 2
laurencebr 0:2aa29a0824df 427 //Keep signal within bounds
laurencebr 4:93c4e826d11d 428 if (PIDerrorq2 > 1.0){
laurencebr 4:93c4e826d11d 429 PIDerrorq2 = 1.0;
laurencebr 0:2aa29a0824df 430 }
laurencebr 4:93c4e826d11d 431 else if (PIDerrorq2 < -1.0){
laurencebr 4:93c4e826d11d 432 PIDerrorq2 = -1.0;
laurencebr 0:2aa29a0824df 433 }
laurencebr 0:2aa29a0824df 434 //Direction
laurencebr 0:2aa29a0824df 435
laurencebr 0:2aa29a0824df 436 if (PIDerrorq2 <= 0){
laurencebr 3:53fb8bd0a448 437 Motor2Direction = 1;
laurencebr 0:2aa29a0824df 438 Motor2PWM.write(-PIDerrorq2); //write Duty cycle
laurencebr 0:2aa29a0824df 439 }
laurencebr 0:2aa29a0824df 440 if (PIDerrorq2 >= 0){
laurencebr 3:53fb8bd0a448 441 Motor2Direction = 0;
laurencebr 0:2aa29a0824df 442 Motor2PWM.write(PIDerrorq2); //write Duty cycle
laurencebr 0:2aa29a0824df 443 }
laurencebr 0:2aa29a0824df 444 }
laurencebr 0:2aa29a0824df 445
laurencebr 3:53fb8bd0a448 446
laurencebr 3:53fb8bd0a448 447
laurencebr 3:53fb8bd0a448 448 void DemonstrationPath()
laurencebr 3:53fb8bd0a448 449 {
laurencebr 3:53fb8bd0a448 450
laurencebr 3:53fb8bd0a448 451 // Also think about how to move from whatever position to (40,5)
laurencebr 3:53fb8bd0a448 452 if (DemoStage == 0) //From (40,40) to (40,-5)
laurencebr 3:53fb8bd0a448 453 {
laurencebr 3:53fb8bd0a448 454 if (yRef >0)
laurencebr 3:53fb8bd0a448 455 {
laurencebr 3:53fb8bd0a448 456 yRef = yRef - vyMax * TickerPeriod;
laurencebr 3:53fb8bd0a448 457 }
laurencebr 3:53fb8bd0a448 458 else
laurencebr 3:53fb8bd0a448 459 {
laurencebr 3:53fb8bd0a448 460 DemoStage = 1;
laurencebr 3:53fb8bd0a448 461 }
laurencebr 3:53fb8bd0a448 462 }
laurencebr 3:53fb8bd0a448 463 else if (DemoStage == 1) //From (40,-5) to (65,-5)
laurencebr 3:53fb8bd0a448 464 {
laurencebr 3:53fb8bd0a448 465 if (xRef > 30)
laurencebr 3:53fb8bd0a448 466 {
laurencebr 3:53fb8bd0a448 467 xRef = xRef - vxMax * TickerPeriod;
laurencebr 3:53fb8bd0a448 468 }
laurencebr 3:53fb8bd0a448 469 else
laurencebr 3:53fb8bd0a448 470 {
laurencebr 3:53fb8bd0a448 471 DemoStage = 2;
laurencebr 3:53fb8bd0a448 472 }
laurencebr 3:53fb8bd0a448 473 }
laurencebr 3:53fb8bd0a448 474 else if (DemoStage == 2)
laurencebr 3:53fb8bd0a448 475 {
laurencebr 3:53fb8bd0a448 476 if (yRef < 10) //From (65,-5) to (65, 10)
laurencebr 3:53fb8bd0a448 477 {
laurencebr 3:53fb8bd0a448 478 yRef = yRef + TickerPeriod;
laurencebr 3:53fb8bd0a448 479 }
laurencebr 3:53fb8bd0a448 480 else
laurencebr 3:53fb8bd0a448 481 {
laurencebr 3:53fb8bd0a448 482 DemoStage = 3;
laurencebr 3:53fb8bd0a448 483 }
laurencebr 3:53fb8bd0a448 484 }
laurencebr 3:53fb8bd0a448 485 else if (DemoStage == 3) //From (65,10) to (40,10)
laurencebr 3:53fb8bd0a448 486 {
laurencebr 3:53fb8bd0a448 487 if (xRef < 40)
laurencebr 3:53fb8bd0a448 488 {
laurencebr 3:53fb8bd0a448 489 xRef = xRef + vxMax * TickerPeriod;
laurencebr 3:53fb8bd0a448 490 }
laurencebr 3:53fb8bd0a448 491 else
laurencebr 3:53fb8bd0a448 492 {
laurencebr 3:53fb8bd0a448 493 DemoStage = 0; // Repeat moving in the square pattern
laurencebr 3:53fb8bd0a448 494 }
laurencebr 3:53fb8bd0a448 495 }
laurencebr 3:53fb8bd0a448 496 }
laurencebr 6:405ec2bba6d0 497 void TestPath()
laurencebr 7:de221f894d3b 498 {
laurencebr 7:de221f894d3b 499 if (DemoStage == 0) //From (40,40) to (40,-5)
laurencebr 6:405ec2bba6d0 500 {
laurencebr 7:de221f894d3b 501 if (yRef > 20.0)
laurencebr 6:405ec2bba6d0 502 {
laurencebr 6:405ec2bba6d0 503 yRef = yRef - vyMax * TickerPeriod;
laurencebr 6:405ec2bba6d0 504 }
laurencebr 6:405ec2bba6d0 505 else
laurencebr 6:405ec2bba6d0 506 {
laurencebr 6:405ec2bba6d0 507 DemoStage = 1;
laurencebr 6:405ec2bba6d0 508 }
laurencebr 6:405ec2bba6d0 509 }
laurencebr 7:de221f894d3b 510
laurencebr 6:405ec2bba6d0 511 else if (DemoStage == 1)
laurencebr 6:405ec2bba6d0 512 {
laurencebr 7:de221f894d3b 513 if (yRef < 35.0) //From (65,-5) to (65, 10)
laurencebr 6:405ec2bba6d0 514 {
laurencebr 7:de221f894d3b 515 yRef = yRef + TickerPeriod;
laurencebr 6:405ec2bba6d0 516 }
laurencebr 6:405ec2bba6d0 517 else
laurencebr 6:405ec2bba6d0 518 {
laurencebr 6:405ec2bba6d0 519 DemoStage = 0;
laurencebr 6:405ec2bba6d0 520 }
laurencebr 6:405ec2bba6d0 521 }
laurencebr 7:de221f894d3b 522
laurencebr 6:405ec2bba6d0 523 }
laurencebr 3:53fb8bd0a448 524
laurencebr 3:53fb8bd0a448 525
laurencebr 3:53fb8bd0a448 526
laurencebr 7:de221f894d3b 527
laurencebr 7:de221f894d3b 528
laurencebr 5:8e326d07f125 529 void CalibrationButton()
laurencebr 5:8e326d07f125 530 {
laurencebr 5:8e326d07f125 531 ledred = 1;
laurencebr 5:8e326d07f125 532 ledgreen = 1;
laurencebr 5:8e326d07f125 533 ledblue = 0;
laurencebr 5:8e326d07f125 534
laurencebr 5:8e326d07f125 535 EMG_max1 = 0.0001;
laurencebr 5:8e326d07f125 536 EMG_max2 = 0.0001;
laurencebr 5:8e326d07f125 537 EMG_max3 = 0.0001;
laurencebr 5:8e326d07f125 538 EMG_max4 = 0.0001;
laurencebr 5:8e326d07f125 539
laurencebr 5:8e326d07f125 540 State = EMGCalibrationState;
laurencebr 5:8e326d07f125 541
laurencebr 5:8e326d07f125 542
laurencebr 5:8e326d07f125 543 }
laurencebr 5:8e326d07f125 544
laurencebr 5:8e326d07f125 545 void EMGCalibration()
laurencebr 5:8e326d07f125 546 {
laurencebr 5:8e326d07f125 547 if (0.95*EMG_filt1>EMG_max1)
laurencebr 5:8e326d07f125 548 {
laurencebr 5:8e326d07f125 549 EMG_max1 = 0.95*EMG_filt1;
laurencebr 5:8e326d07f125 550 }
laurencebr 5:8e326d07f125 551 if (0.95*EMG_filt2>EMG_max2)
laurencebr 5:8e326d07f125 552
laurencebr 5:8e326d07f125 553 {
laurencebr 5:8e326d07f125 554 EMG_max2 = 0.95*EMG_filt2;
laurencebr 5:8e326d07f125 555 }
laurencebr 5:8e326d07f125 556
laurencebr 5:8e326d07f125 557 if (0.95*EMG_filt3>EMG_max3)
laurencebr 5:8e326d07f125 558 {
laurencebr 5:8e326d07f125 559 EMG_max3 = 0.95*EMG_filt3;
laurencebr 5:8e326d07f125 560 }
laurencebr 5:8e326d07f125 561 if (0.95*EMG_filt4>EMG_max4)
laurencebr 5:8e326d07f125 562 {
laurencebr 5:8e326d07f125 563 EMG_max4 = 0.95*EMG_filt4;
laurencebr 5:8e326d07f125 564 }
laurencebr 5:8e326d07f125 565
laurencebr 5:8e326d07f125 566 }
laurencebr 5:8e326d07f125 567
laurencebr 5:8e326d07f125 568
laurencebr 5:8e326d07f125 569
laurencebr 3:53fb8bd0a448 570 void CalibrationModus()
laurencebr 3:53fb8bd0a448 571 {
laurencebr 5:8e326d07f125 572
laurencebr 5:8e326d07f125 573 EMG();
laurencebr 5:8e326d07f125 574 EMGCalibration();
laurencebr 5:8e326d07f125 575
laurencebr 5:8e326d07f125 576 countcalibration++;
laurencebr 8:fa268d163bbd 577 //pc.printf("countcal = %i", countcalibration);
laurencebr 5:8e326d07f125 578 if (countcalibration >= (int)(CalibrationTime*1.0/TickerPeriod))
laurencebr 5:8e326d07f125 579 {
laurencebr 5:8e326d07f125 580 State = NormalOperatingModusState;
laurencebr 5:8e326d07f125 581 countcalibration = 0;
laurencebr 5:8e326d07f125 582 }
laurencebr 5:8e326d07f125 583
laurencebr 3:53fb8bd0a448 584 }
laurencebr 3:53fb8bd0a448 585
laurencebr 3:53fb8bd0a448 586 void DemoModus() // The ticker should call this function (in the switch statement)
laurencebr 3:53fb8bd0a448 587 {
laurencebr 5:8e326d07f125 588
laurencebr 8:fa268d163bbd 589 /*GainP1 = 40.0;//Ppot.read()*100.0;
laurencebr 7:de221f894d3b 590 GainI1 = 17.0;//Ipot.read()*20.0;
laurencebr 7:de221f894d3b 591 GainD1 = 2.0;//Dpot.read()*20.0;
laurencebr 7:de221f894d3b 592 GainP2 = Ppot.read()*100.0;
laurencebr 7:de221f894d3b 593 GainI2 = Ipot.read()*20.0;
laurencebr 7:de221f894d3b 594 GainD2 = Dpot.read()*20.0;
laurencebr 8:fa268d163bbd 595 */
laurencebr 5:8e326d07f125 596
laurencebr 8:fa268d163bbd 597 DemonstrationPath();
laurencebr 8:fa268d163bbd 598 //TestPath();
laurencebr 3:53fb8bd0a448 599 inverse();
laurencebr 3:53fb8bd0a448 600 ReadCurrentPosition();
laurencebr 3:53fb8bd0a448 601 UpdateError();
laurencebr 3:53fb8bd0a448 602 PIDControl();
laurencebr 3:53fb8bd0a448 603 MotorControl();
laurencebr 3:53fb8bd0a448 604
laurencebr 5:8e326d07f125 605 //scope.set(0, q1Pos);
laurencebr 5:8e326d07f125 606 // scope.set(1, q1Ref);
laurencebr 5:8e326d07f125 607 //scope.set(2, q2Pos);
laurencebr 5:8e326d07f125 608 //scope.set(3, q2Ref);
laurencebr 5:8e326d07f125 609
laurencebr 3:53fb8bd0a448 610 count++;
laurencebr 3:53fb8bd0a448 611 if (count ==400)
laurencebr 3:53fb8bd0a448 612 {
laurencebr 8:fa268d163bbd 613 //pc.printf("GainP = %lf, GainI = %lf, GainD = %lf, q1Pos = %lf, q2Pos = %lf, q1Ref = %lf, q2Ref = %lf, xRef = %lf, yRef = %lf \n\r", GainP2, GainI2, GainD2, q1Pos, q2Pos, q1Ref, q2Ref, xRef, yRef);
laurencebr 3:53fb8bd0a448 614 count = 0;
laurencebr 3:53fb8bd0a448 615 }
laurencebr 3:53fb8bd0a448 616 }
laurencebr 3:53fb8bd0a448 617
laurencebr 3:53fb8bd0a448 618
laurencebr 0:2aa29a0824df 619 void NormalOperatingModus()
laurencebr 0:2aa29a0824df 620 {
laurencebr 5:8e326d07f125 621 ledred = 1;
laurencebr 5:8e326d07f125 622 ledgreen = 0;
laurencebr 5:8e326d07f125 623 ledblue = 1;
laurencebr 5:8e326d07f125 624
laurencebr 3:53fb8bd0a448 625 EMG();
laurencebr 0:2aa29a0824df 626 InverseKinematics();
laurencebr 0:2aa29a0824df 627 ReadCurrentPosition();
laurencebr 0:2aa29a0824df 628 UpdateError();
laurencebr 0:2aa29a0824df 629 PIDControl();
laurencebr 0:2aa29a0824df 630 MotorControl();
laurencebr 0:2aa29a0824df 631
laurencebr 3:53fb8bd0a448 632 //scope.set(0, q1Pos);
laurencebr 3:53fb8bd0a448 633 //scope.set(1, q1Ref);
laurencebr 0:2aa29a0824df 634
laurencebr 4:93c4e826d11d 635 //GainP1 = pot3.read() * 10;
laurencebr 4:93c4e826d11d 636 //GainI1 = pot2.read() * 10;
laurencebr 4:93c4e826d11d 637 //GainD1 = pot1.read() ;
laurencebr 0:2aa29a0824df 638
laurencebr 0:2aa29a0824df 639 countstep++;
laurencebr 1:23834862b877 640 counter++;
laurencebr 1:23834862b877 641 if (counter == 400) // print 1x per seconde waardes.
laurencebr 0:2aa29a0824df 642 {
laurencebr 6:405ec2bba6d0 643 //pc.printf("xRef = %lf, q1Pos = %lf, q1Ref = %1f \n\r", xRef, q1Pos, q1Ref);
laurencebr 1:23834862b877 644 counter = 0;
laurencebr 0:2aa29a0824df 645 }
laurencebr 0:2aa29a0824df 646 if (countstep == 4000)
laurencebr 0:2aa29a0824df 647 {
laurencebr 0:2aa29a0824df 648 q1Ref = !q1Ref;
laurencebr 0:2aa29a0824df 649 countstep = 0;
laurencebr 0:2aa29a0824df 650 }
laurencebr 0:2aa29a0824df 651
laurencebr 0:2aa29a0824df 652
laurencebr 0:2aa29a0824df 653 }
laurencebr 3:53fb8bd0a448 654
laurencebr 5:8e326d07f125 655
laurencebr 0:2aa29a0824df 656
laurencebr 5:8e326d07f125 657 void StateMachine()
laurencebr 3:53fb8bd0a448 658 {
laurencebr 5:8e326d07f125 659
laurencebr 5:8e326d07f125 660
laurencebr 5:8e326d07f125 661 if (ButtonCal.read() == 0)
laurencebr 5:8e326d07f125 662 {
laurencebr 5:8e326d07f125 663 CalibrationButton();
laurencebr 8:fa268d163bbd 664 // pc.printf("print iets");
laurencebr 8:fa268d163bbd 665 Motor1PWM.write(0.0);
laurencebr 8:fa268d163bbd 666 Motor2PWM.write(0.0);
laurencebr 8:fa268d163bbd 667 }
laurencebr 8:fa268d163bbd 668 if (ButtonDemo.read() == 0)
laurencebr 8:fa268d163bbd 669 {
laurencebr 8:fa268d163bbd 670 State = DemoModusState;
laurencebr 8:fa268d163bbd 671
laurencebr 8:fa268d163bbd 672 ledred = 1;
laurencebr 8:fa268d163bbd 673 ledgreen = 0;
laurencebr 8:fa268d163bbd 674 ledblue = 0;
laurencebr 5:8e326d07f125 675 }
laurencebr 5:8e326d07f125 676
laurencebr 3:53fb8bd0a448 677 switch(State)
laurencebr 3:53fb8bd0a448 678 {
laurencebr 5:8e326d07f125 679 case WaitModusState: //aanmaken
laurencebr 3:53fb8bd0a448 680 EMG();
laurencebr 8:fa268d163bbd 681 // pc.printf("Wait \n\r");
laurencebr 3:53fb8bd0a448 682 break;
laurencebr 5:8e326d07f125 683 case EMGCalibrationState:
laurencebr 5:8e326d07f125 684 CalibrationModus();
laurencebr 5:8e326d07f125 685 //pc.printf("EMG CAL \n\r");
laurencebr 3:53fb8bd0a448 686 break;
laurencebr 5:8e326d07f125 687 case NormalOperatingModusState:
laurencebr 3:53fb8bd0a448 688 NormalOperatingModus();
laurencebr 5:8e326d07f125 689 //pc.printf("NOMS \n\r");
laurencebr 3:53fb8bd0a448 690 break;
laurencebr 5:8e326d07f125 691 case DemoModusState:
laurencebr 5:8e326d07f125 692 DemoModus();
laurencebr 7:de221f894d3b 693 //pc.printf("Demo \n\r");
laurencebr 3:53fb8bd0a448 694 break;
laurencebr 3:53fb8bd0a448 695 default :
laurencebr 3:53fb8bd0a448 696 }
laurencebr 8:fa268d163bbd 697
laurencebr 3:53fb8bd0a448 698 }
laurencebr 5:8e326d07f125 699
laurencebr 3:53fb8bd0a448 700
laurencebr 0:2aa29a0824df 701
laurencebr 0:2aa29a0824df 702 int main()
laurencebr 0:2aa29a0824df 703 {
laurencebr 8:fa268d163bbd 704 //pc.baud(115200);
laurencebr 1:23834862b877 705
laurencebr 5:8e326d07f125 706 /*
laurencebr 5:8e326d07f125 707 GainP1 = pot3.read() * 10;
laurencebr 5:8e326d07f125 708 GainI1 = pot2.read() * 10;
laurencebr 5:8e326d07f125 709 GainD1 = pot1.read();
laurencebr 5:8e326d07f125 710 */
laurencebr 6:405ec2bba6d0 711
laurencebr 8:fa268d163bbd 712 //pc.printf("GainP = %lf, GainI = %lf, GainP = %lf, nog 10 seconden \n\r", GainP1, GainI1, GainD1);
laurencebr 3:53fb8bd0a448 713 wait(7.0);
laurencebr 8:fa268d163bbd 714 //pc.printf("nog 3 seconden \n\r");
laurencebr 3:53fb8bd0a448 715 wait(3.0);
laurencebr 6:405ec2bba6d0 716
laurencebr 3:53fb8bd0a448 717
laurencebr 1:23834862b877 718 //BiQuad chains
laurencebr 6:405ec2bba6d0 719 bqc1.add( &HP_emg1 ).add( &Notch_emg1 );//.add( &LP1);
laurencebr 6:405ec2bba6d0 720 bqc2.add( &HP_emg2 ).add( &Notch_emg2 );//.add( &LP2);
laurencebr 6:405ec2bba6d0 721 bqc3.add( &HP_emg3 ).add( &Notch_emg3 );//.add( &LP3);
laurencebr 6:405ec2bba6d0 722 bqc4.add( &HP_emg4 ).add( &Notch_emg4 );//.add( &LP4);
laurencebr 1:23834862b877 723
laurencebr 0:2aa29a0824df 724 //Initialize array errors to 0
laurencebr 7:de221f894d3b 725 for (int i = 0; i <= 99; i++){
laurencebr 7:de221f894d3b 726 PrevErrorq1[i] = 0;
laurencebr 0:2aa29a0824df 727 PrevErrorq2[i] = 0;
laurencebr 0:2aa29a0824df 728 }
laurencebr 0:2aa29a0824df 729
laurencebr 7:de221f894d3b 730 double frequency_pwm = 16700.0; //16.7 kHz PWM
laurencebr 8:fa268d163bbd 731 Motor1PWM.period(1.0/frequency_pwm); // T = 1/f
laurencebr 8:fa268d163bbd 732 Motor2PWM.period(1.0/frequency_pwm); // T = 1/f
laurencebr 0:2aa29a0824df 733
laurencebr 5:8e326d07f125 734 //Emg Calibratie button
laurencebr 5:8e326d07f125 735 //ButtonCal.fall(&CalibrationButton);
laurencebr 5:8e326d07f125 736
laurencebr 3:53fb8bd0a448 737
laurencebr 5:8e326d07f125 738
laurencebr 5:8e326d07f125 739 Kikker.attach(StateMachine, TickerPeriod);
laurencebr 5:8e326d07f125 740 // scopeTimer.attach_us(&scope, &HIDScope::send, 2e4);
laurencebr 0:2aa29a0824df 741
laurencebr 1:23834862b877 742 //Onderstaande stond in EMG deel, kijken hoe en wat met tickers!!
laurencebr 1:23834862b877 743 // Attach the HIDScope::send method from the scope object to the timer at 50Hz
laurencebr 8:fa268d163bbd 744 //scopeTimer.attach_us(&scope, &HIDScope::send, 5e3);
laurencebr 1:23834862b877 745 //EMGTicker.attach_us(EMG_filtering, 5e3);
laurencebr 1:23834862b877 746 //.
laurencebr 1:23834862b877 747
laurencebr 0:2aa29a0824df 748 while(true);
laurencebr 0:2aa29a0824df 749 {}
laurencebr 0:2aa29a0824df 750 }
laurencebr 0:2aa29a0824df 751
laurencebr 0:2aa29a0824df 752
laurencebr 0:2aa29a0824df 753
laurencebr 0:2aa29a0824df 754