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