Laurence B / Mbed 2 deprecated VolledigeScript

alles ingevoegd

Dependencies:   FastPWM HIDScope MODSERIAL QEI mbed biquadFilter

Fork of deelPID by Laurence B

main.cpp@3:53fb8bd0a448, 2018-10-31 (annotated)

Committer:
laurencebr
Date:
Wed Oct 31 11:47:36 2018 +0000
Revision:
3:53fb8bd0a448
Parent:
2:58ec7347245e
Child:
4:93c4e826d11d
Versie waar robot kan bewegen in Demomodus.

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