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