Laurence B / Mbed 2 deprecated VolledigeScript

alles ingevoegd

Dependencies:   FastPWM HIDScope MODSERIAL QEI mbed biquadFilter

Fork of deelPID by Laurence B

main.cpp@5:8e326d07f125, 2018-10-31 (annotated)

Committer:
laurencebr
Date:
Wed Oct 31 16:08:45 2018 +0000
Revision:
5:8e326d07f125
Parent:
4:93c4e826d11d
Child:
6:405ec2bba6d0
laatste versie 31-10

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