Het complete motorscript met alle gewenste functies
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of Motor_EMG_Definitief by
main.cpp@10:5c5f2ea11045, 2015-10-30 (annotated)
- Committer:
- Margreeth95
- Date:
- Fri Oct 30 09:17:33 2015 +0000
- Revision:
- 10:5c5f2ea11045
- Parent:
- 9:350a82a7745a
- Child:
- 11:65d053a1b511
Definitieve code, laatste functie ingevoegd voor knipperen lampje
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
laura94 | 0:5816557b2064 | 1 | //--------------------------------------------------------------------------------------------------------------------------// |
laura94 | 0:5816557b2064 | 2 | // Motorscript voor 2 motoren voor de "SJOEL ROBOT", Groep 7 |
laura94 | 0:5816557b2064 | 3 | //--------------------------------------------------------------------------------------------------------------------------// |
laura94 | 0:5816557b2064 | 4 | // Libraries |
laura94 | 0:5816557b2064 | 5 | //--------------------------------------------------------------------------------------------------------------------------// |
laura94 | 0:5816557b2064 | 6 | #include "mbed.h" |
laura94 | 0:5816557b2064 | 7 | #include "MODSERIAL.h" |
laura94 | 0:5816557b2064 | 8 | #include "HIDScope.h" |
laura94 | 0:5816557b2064 | 9 | #include "QEI.h" |
laura94 | 0:5816557b2064 | 10 | #include "biquadFilter.h" |
laura94 | 0:5816557b2064 | 11 | |
laura94 | 0:5816557b2064 | 12 | //--------------------------------------------------------------------------------------------------------------------------// |
laura94 | 0:5816557b2064 | 13 | // Constanten/Inputs/Outputs |
laura94 | 0:5816557b2064 | 14 | //--------------------------------------------------------------------------------------------------------------------------// |
laura94 | 0:5816557b2064 | 15 | MODSERIAL pc(USBTX, USBRX); // To/From PC |
laura94 | 0:5816557b2064 | 16 | QEI Encoder2(D3, D2, NC, 32); // Encoder Motor 2 |
laura94 | 0:5816557b2064 | 17 | QEI Encoder1(D13,D12,NC, 32); // Encoder Motor 1 |
Rvs94 | 1:3f49c8818619 | 18 | HIDScope scope(6); // Scope, 4 channels |
laura94 | 0:5816557b2064 | 19 | |
laura94 | 0:5816557b2064 | 20 | // LEDs |
laura94 | 0:5816557b2064 | 21 | DigitalOut LedR(LED_RED); |
laura94 | 0:5816557b2064 | 22 | DigitalOut LedG(LED_GREEN); |
laura94 | 0:5816557b2064 | 23 | DigitalOut LedB(LED_BLUE); |
laura94 | 0:5816557b2064 | 24 | |
laura94 | 0:5816557b2064 | 25 | // Motor |
laura94 | 0:5816557b2064 | 26 | DigitalOut motor1direction(D7); // Motor 1, Direction & Speed |
laura94 | 0:5816557b2064 | 27 | PwmOut motor1speed(D6); |
laura94 | 0:5816557b2064 | 28 | DigitalOut motor2direction(D4); // Motor 2, Direction & Speed |
laura94 | 0:5816557b2064 | 29 | PwmOut motor2speed(D5); |
laura94 | 0:5816557b2064 | 30 | |
laura94 | 0:5816557b2064 | 31 | //EMG |
laura94 | 0:5816557b2064 | 32 | AnalogIn EMG_left(A0); //Analog input |
laura94 | 0:5816557b2064 | 33 | AnalogIn EMG_right(A1); |
laura94 | 0:5816557b2064 | 34 | |
laura94 | 0:5816557b2064 | 35 | // Tickers |
laura94 | 0:5816557b2064 | 36 | Ticker ScopeTime; |
laura94 | 0:5816557b2064 | 37 | Ticker myControllerTicker2; |
laura94 | 0:5816557b2064 | 38 | Ticker myControllerTicker1; |
laura94 | 0:5816557b2064 | 39 | Ticker SampleEMGLeft; |
laura94 | 0:5816557b2064 | 40 | Ticker SampleEMGRight; |
laura94 | 0:5816557b2064 | 41 | Ticker ScopeTimer; |
laura94 | 0:5816557b2064 | 42 | Ticker serial; |
laura94 | 0:5816557b2064 | 43 | Ticker MovingAverageLeft; |
laura94 | 0:5816557b2064 | 44 | Ticker MovingAverageRight; |
laura94 | 0:5816557b2064 | 45 | |
laura94 | 0:5816557b2064 | 46 | // Constants |
laura94 | 0:5816557b2064 | 47 | double reference2, reference1; |
laura94 | 0:5816557b2064 | 48 | double position2 = 0, position1 = 0; |
laura94 | 0:5816557b2064 | 49 | double m2_ref = 0, m1_ref = 0; |
laura94 | 0:5816557b2064 | 50 | int count = 0; |
Margreeth95 | 9:350a82a7745a | 51 | double Grens2 = 60, Grens1 = 60; |
Rvs94 | 4:92a3b38a5302 | 52 | double Stapgrootte = 1; |
laura94 | 0:5816557b2064 | 53 | |
laura94 | 0:5816557b2064 | 54 | // Declaring variables |
laura94 | 0:5816557b2064 | 55 | double EMG_L_f_v1 = 0, EMG_L_f_v2 = 0; |
laura94 | 0:5816557b2064 | 56 | double EMG_L_fh=0; |
laura94 | 0:5816557b2064 | 57 | double EMG_left_value; |
laura94 | 0:5816557b2064 | 58 | double EMG_left_f1; |
laura94 | 0:5816557b2064 | 59 | double EMG_left_f2; |
laura94 | 0:5816557b2064 | 60 | double EMG_left_f3; |
laura94 | 0:5816557b2064 | 61 | double EMG_left_abs; |
laura94 | 0:5816557b2064 | 62 | |
laura94 | 0:5816557b2064 | 63 | double EMG_right_value; |
laura94 | 0:5816557b2064 | 64 | double EMG_right_f1; |
laura94 | 0:5816557b2064 | 65 | double EMG_right_f2; |
laura94 | 0:5816557b2064 | 66 | double EMG_right_f3; |
laura94 | 0:5816557b2064 | 67 | double EMG_right_abs; |
laura94 | 0:5816557b2064 | 68 | double Threshold1; |
laura94 | 0:5816557b2064 | 69 | double Threshold3; |
laura94 | 0:5816557b2064 | 70 | |
laura94 | 0:5816557b2064 | 71 | int N = 50; |
laura94 | 0:5816557b2064 | 72 | double MAF_left[50]; |
laura94 | 0:5816557b2064 | 73 | double EMG_left_MAF; |
laura94 | 0:5816557b2064 | 74 | double MAF_right[50]; |
laura94 | 0:5816557b2064 | 75 | double EMG_right_MAF; |
laura94 | 0:5816557b2064 | 76 | |
laura94 | 0:5816557b2064 | 77 | |
laura94 | 0:5816557b2064 | 78 | //Sample time (motor-step) |
Rvs94 | 2:7873c44b0568 | 79 | const double m2_Ts = 0.001, m1_Ts = 0.001; |
laura94 | 0:5816557b2064 | 80 | |
laura94 | 0:5816557b2064 | 81 | //Controller gain Motor 2 & 1 |
laura94 | 6:d1d747050fcc | 82 | const double m2_Kp = 0.1523/5,m2_Ki = 0.08585/5, m2_Kd = 0.004534/5; |
laura94 | 6:d1d747050fcc | 83 | const double m1_Kp = 0.1523/5,m1_Ki = 0.08585/5, m1_Kd = 0.004534/5; |
laura94 | 0:5816557b2064 | 84 | double m2_err_int = 0, m2_prev_err = 0; |
laura94 | 0:5816557b2064 | 85 | double m1_err_int = 0, m1_prev_err = 0; |
laura94 | 0:5816557b2064 | 86 | |
laura94 | 0:5816557b2064 | 87 | // coëfficiënten |
Rvs94 | 1:3f49c8818619 | 88 | const double BiGainEMG_H1 = 0.796821; |
laura94 | 0:5816557b2064 | 89 | const double EMGH1_a1 = -1.47500228332, EMGH1_a2 = 0.55273994299, EMGH1_b0 = 1.0*BiGainEMG_H1, EMGH1_b1 = -1.99922446977*BiGainEMG_H1, EMGH1_b2 = 1.0*BiGainEMG_H1; //coefficients for high-pass filter |
laura94 | 0:5816557b2064 | 90 | |
laura94 | 0:5816557b2064 | 91 | const double BiGainEMG_L1= 0.001041; |
laura94 | 0:5816557b2064 | 92 | const double EMGL1_a1 = -1.87506717001, EMGL1_a2 = 0.87923101463, EMGL1_b0 = 1.0*BiGainEMG_L1, EMGL1_b1 = 2.00000000000*BiGainEMG_L1, EMGL1_b2 = 1.0*BiGainEMG_L1; // coefficients for low-pass filter |
laura94 | 0:5816557b2064 | 93 | |
laura94 | 0:5816557b2064 | 94 | const double BiGainEMG_N1 = 1.0; |
laura94 | 0:5816557b2064 | 95 | const double EMGN1_a1 = -1.58174308681, EMGN1_a2 = 0.96540248979, EMGN1_b0 = 1.0*BiGainEMG_N1, EMGN1_b1 = -1.61816176147*BiGainEMG_N1, EMGN1_b2 = 1.0*BiGainEMG_N1; //coefficients for notch filter |
laura94 | 0:5816557b2064 | 96 | |
laura94 | 0:5816557b2064 | 97 | // Creating the filters |
laura94 | 0:5816557b2064 | 98 | biquadFilter EMG_highpass1 (EMGH1_a1, EMGH1_a2, EMGH1_b0, EMGH1_b1, EMGH1_b2); // creates the high pass filter |
laura94 | 0:5816557b2064 | 99 | biquadFilter EMG_lowpass1 (EMGL1_a1, EMGL1_a2, EMGL1_b0, EMGL1_b1, EMGL1_b2); // creates the low pass filter |
laura94 | 0:5816557b2064 | 100 | biquadFilter EMG_notch1 (EMGN1_a1, EMGN1_a2, EMGN1_b0, EMGN1_b1, EMGN1_b2); // creates the notch filter |
laura94 | 0:5816557b2064 | 101 | |
laura94 | 0:5816557b2064 | 102 | biquadFilter EMG_highpass1R (EMGH1_a1, EMGH1_a2, EMGH1_b0, EMGH1_b1, EMGH1_b2); // creates the high pass filter |
laura94 | 0:5816557b2064 | 103 | biquadFilter EMG_lowpass1R (EMGL1_a1, EMGL1_a2, EMGL1_b0, EMGL1_b1, EMGL1_b2); // creates the low pass filter |
laura94 | 0:5816557b2064 | 104 | biquadFilter EMG_notch1R (EMGN1_a1, EMGN1_a2, EMGN1_b0, EMGN1_b1, EMGN1_b2); // creates the notch filter |
laura94 | 0:5816557b2064 | 105 | |
laura94 | 0:5816557b2064 | 106 | |
laura94 | 0:5816557b2064 | 107 | //--------------------------------------------------------------------------------------------------------------------------// |
laura94 | 0:5816557b2064 | 108 | // General Functions |
laura94 | 0:5816557b2064 | 109 | //--------------------------------------------------------------------------------------------------------------------------// |
laura94 | 0:5816557b2064 | 110 | |
Margreeth95 | 10:5c5f2ea11045 | 111 | //Blinking led |
Margreeth95 | 10:5c5f2ea11045 | 112 | void BlinkLed(DigitalOut LED, int n, float Time) |
Margreeth95 | 10:5c5f2ea11045 | 113 | { |
Margreeth95 | 10:5c5f2ea11045 | 114 | for(int i=1; i<=n;i++) |
Margreeth95 | 10:5c5f2ea11045 | 115 | { |
Margreeth95 | 10:5c5f2ea11045 | 116 | LED.write(0); |
Margreeth95 | 10:5c5f2ea11045 | 117 | wait(Time); |
Margreeth95 | 10:5c5f2ea11045 | 118 | LED.write(1); |
Margreeth95 | 10:5c5f2ea11045 | 119 | wait(Time); |
Margreeth95 | 10:5c5f2ea11045 | 120 | } |
Margreeth95 | 10:5c5f2ea11045 | 121 | LED.write(1); |
Margreeth95 | 10:5c5f2ea11045 | 122 | } |
Margreeth95 | 10:5c5f2ea11045 | 123 | |
laura94 | 0:5816557b2064 | 124 | //HIDScope |
laura94 | 0:5816557b2064 | 125 | void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt |
laura94 | 0:5816557b2064 | 126 | { |
Rvs94 | 1:3f49c8818619 | 127 | scope.set(0, position2); |
Rvs94 | 1:3f49c8818619 | 128 | scope.set(1, position1); |
Rvs94 | 1:3f49c8818619 | 129 | scope.set(2, EMG_left_MAF); |
Rvs94 | 1:3f49c8818619 | 130 | scope.set(3, EMG_right_MAF); |
Rvs94 | 1:3f49c8818619 | 131 | scope.set(4, count); |
laura94 | 0:5816557b2064 | 132 | scope.send(); |
laura94 | 0:5816557b2064 | 133 | |
laura94 | 0:5816557b2064 | 134 | } |
laura94 | 0:5816557b2064 | 135 | |
laura94 | 0:5816557b2064 | 136 | // Reusable PID controller |
laura94 | 0:5816557b2064 | 137 | double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev) |
laura94 | 0:5816557b2064 | 138 | { |
laura94 | 0:5816557b2064 | 139 | // Derivative |
laura94 | 0:5816557b2064 | 140 | double e_der = (e-e_prev)/Ts; |
laura94 | 0:5816557b2064 | 141 | e_prev = e; |
laura94 | 0:5816557b2064 | 142 | // Integral |
laura94 | 0:5816557b2064 | 143 | e_int = e_int + Ts*e; |
laura94 | 0:5816557b2064 | 144 | // PID |
laura94 | 0:5816557b2064 | 145 | return Kp * e + Ki*e_int + Kd*e_der; |
laura94 | 0:5816557b2064 | 146 | } |
laura94 | 0:5816557b2064 | 147 | |
laura94 | 0:5816557b2064 | 148 | //--------------------------------------------------------------------------------------------------------------------------// |
laura94 | 0:5816557b2064 | 149 | //EMG functions |
laura94 | 0:5816557b2064 | 150 | //--------------------------------------------------------------------------------------------------------------------------// |
laura94 | 0:5816557b2064 | 151 | |
laura94 | 0:5816557b2064 | 152 | // EMG filtering function |
laura94 | 0:5816557b2064 | 153 | void EMGfilterLeft() |
laura94 | 0:5816557b2064 | 154 | { |
laura94 | 0:5816557b2064 | 155 | EMG_left_value = EMG_left.read(); |
laura94 | 0:5816557b2064 | 156 | EMG_left_f1 = EMG_highpass1.step(EMG_left_value); |
laura94 | 0:5816557b2064 | 157 | EMG_left_abs = fabs(EMG_left_f1); |
laura94 | 0:5816557b2064 | 158 | EMG_left_f2 = EMG_lowpass1.step(EMG_left_abs); |
laura94 | 0:5816557b2064 | 159 | EMG_left_f3 = EMG_notch1.step(EMG_left_f2); |
laura94 | 0:5816557b2064 | 160 | |
laura94 | 0:5816557b2064 | 161 | } |
laura94 | 0:5816557b2064 | 162 | |
laura94 | 0:5816557b2064 | 163 | void EMGfilterRight() |
laura94 | 0:5816557b2064 | 164 | { |
laura94 | 0:5816557b2064 | 165 | EMG_right_value = EMG_right.read(); |
laura94 | 0:5816557b2064 | 166 | EMG_right_f1 = EMG_highpass1R.step(EMG_right_value); |
laura94 | 0:5816557b2064 | 167 | EMG_right_abs = fabs(EMG_right_f1); |
laura94 | 0:5816557b2064 | 168 | EMG_right_f2 = EMG_lowpass1R.step(EMG_right_abs); |
laura94 | 0:5816557b2064 | 169 | EMG_right_f3 = EMG_notch1R.step(EMG_right_f2); |
laura94 | 0:5816557b2064 | 170 | } |
laura94 | 0:5816557b2064 | 171 | |
laura94 | 0:5816557b2064 | 172 | // Movingaverage Filter |
laura94 | 0:5816557b2064 | 173 | void MovingAverageFilterLeft() |
laura94 | 0:5816557b2064 | 174 | { |
laura94 | 0:5816557b2064 | 175 | EMG_left_MAF = (MAF_left[0]+MAF_left[1]+MAF_left[2]+MAF_left[3]+MAF_left[4]+MAF_left[5]+MAF_left[6]+MAF_left[7]+MAF_left[8]+MAF_left[9]+MAF_left[10]+MAF_left[11]+MAF_left[12]+MAF_left[13]+MAF_left[14]+MAF_left[15]+MAF_left[16]+MAF_left[17]+MAF_left[18]+MAF_left[19]+MAF_left[20]+MAF_left[21]+MAF_left[22]+MAF_left[23]+MAF_left[24]+MAF_left[25]+MAF_left[26]+MAF_left[27]+MAF_left[28]+MAF_left[29]+MAF_left[30]+MAF_left[31]+MAF_left[32]+MAF_left[33]+MAF_left[34]+MAF_left[35]+MAF_left[36]+MAF_left[37]+MAF_left[38]+MAF_left[39]+MAF_left[40]+MAF_left[41]+MAF_left[42]+MAF_left[43]+MAF_left[44]+MAF_left[45]+MAF_left[46]+MAF_left[47]+MAF_left[48]+MAF_left[49])/N; |
laura94 | 0:5816557b2064 | 176 | MAF_left[49] = MAF_left[48], MAF_left[48] = MAF_left[47], MAF_left[47] = MAF_left[46], MAF_left[46] = MAF_left[45], MAF_left[45] = MAF_left[44], MAF_left[44] = MAF_left[43], MAF_left[43] = MAF_left[42], MAF_left[42] = MAF_left[41], MAF_left[41] = MAF_left[40], MAF_left[40] = MAF_left[39], MAF_left[39] = MAF_left[38], MAF_left[38] = MAF_left[37], MAF_left[37] = MAF_left[36], MAF_left[36] = MAF_left[35], MAF_left[35] = MAF_left[34], MAF_left[34] = MAF_left[33], MAF_left[33] = MAF_left[32], MAF_left[32] = MAF_left[31], MAF_left[31] = MAF_left[30], MAF_left[30] = MAF_left[29], MAF_left[29] = MAF_left[28], MAF_left[28] = MAF_left[27], MAF_left[27] = MAF_left[26], MAF_left[26] = MAF_left[25]; |
laura94 | 0:5816557b2064 | 177 | MAF_left[25] = MAF_left[24], MAF_left[24] = MAF_left[23], MAF_left[23] = MAF_left[22], MAF_left[22] = MAF_left[21], MAF_left[21] = MAF_left[20], MAF_left[20] = MAF_left[19], MAF_left[19] = MAF_left[18], MAF_left[18] = MAF_left[17], MAF_left[17] = MAF_left[16], MAF_left[16] = MAF_left[15], MAF_left[15] = MAF_left[14], MAF_left[14] = MAF_left[13], MAF_left[13] = MAF_left[12], MAF_left[12] = MAF_left[11], MAF_left[11] = MAF_left[10], MAF_left[10] = MAF_left[9], MAF_left[9] = MAF_left[8], MAF_left[8] = MAF_left[7], MAF_left[7] = MAF_left[6], MAF_left[6] = MAF_left[5], MAF_left[5] = MAF_left[4], MAF_left[4] = MAF_left[3], MAF_left[3] = MAF_left[2], MAF_left[2] = MAF_left[1], MAF_left[1] = MAF_left[0]; |
laura94 | 0:5816557b2064 | 178 | MAF_left[0] = EMG_left_f3; |
laura94 | 0:5816557b2064 | 179 | } |
laura94 | 0:5816557b2064 | 180 | |
laura94 | 0:5816557b2064 | 181 | void MovingAverageFilterRight() |
laura94 | 0:5816557b2064 | 182 | { |
laura94 | 0:5816557b2064 | 183 | EMG_right_MAF = (MAF_right[0]+MAF_right[1]+MAF_right[2]+MAF_right[3]+MAF_right[4]+MAF_right[5]+MAF_right[6]+MAF_right[7]+MAF_right[8]+MAF_right[9]+MAF_right[10]+MAF_right[11]+MAF_right[12]+MAF_right[13]+MAF_right[14]+MAF_right[15]+MAF_right[16]+MAF_right[17]+MAF_right[18]+MAF_right[19]+MAF_right[20]+MAF_right[21]+MAF_right[22]+MAF_right[23]+MAF_right[24]+MAF_right[25]+MAF_right[26]+MAF_right[27]+MAF_right[28]+MAF_right[29]+MAF_right[30]+MAF_right[31]+MAF_right[32]+MAF_right[33]+MAF_right[34]+MAF_right[35]+MAF_right[36]+MAF_right[37]+MAF_right[38]+MAF_right[39]+MAF_right[40]+MAF_right[41]+MAF_right[42]+MAF_right[43]+MAF_right[44]+MAF_right[45]+MAF_right[46]+MAF_right[47]+MAF_right[48]+MAF_right[49])/N; |
laura94 | 0:5816557b2064 | 184 | MAF_right[49] = MAF_right[48], MAF_right[48] = MAF_right[47], MAF_right[47] = MAF_right[46], MAF_right[46] = MAF_right[45], MAF_right[45] = MAF_right[44], MAF_right[44] = MAF_right[43], MAF_right[43] = MAF_right[42], MAF_right[42] = MAF_right[41], MAF_right[41] = MAF_right[40], MAF_right[40] = MAF_right[39], MAF_right[39] = MAF_right[38], MAF_right[38] = MAF_right[37], MAF_right[37] = MAF_right[36], MAF_right[36] = MAF_right[35], MAF_right[35] = MAF_right[34], MAF_right[34] = MAF_right[33], MAF_right[33] = MAF_right[32], MAF_right[32] = MAF_right[31], MAF_right[31] = MAF_right[30], MAF_right[30] = MAF_right[29], MAF_right[29] = MAF_right[28], MAF_right[28] = MAF_right[27], MAF_right[27] = MAF_right[26], MAF_right[26] = MAF_right[25]; |
laura94 | 0:5816557b2064 | 185 | MAF_right[25] = MAF_right[24], MAF_right[24] = MAF_right[23], MAF_right[23] = MAF_right[22], MAF_right[22] = MAF_right[21], MAF_right[21] = MAF_right[20], MAF_right[20] = MAF_right[19], MAF_right[19] = MAF_right[18], MAF_right[18] = MAF_right[17], MAF_right[17] = MAF_right[16], MAF_right[16] = MAF_right[15], MAF_right[15] = MAF_right[14], MAF_right[14] = MAF_right[13], MAF_right[13] = MAF_right[12], MAF_right[12] = MAF_right[11], MAF_right[11] = MAF_right[10], MAF_right[10] = MAF_right[9], MAF_right[9] = MAF_right[8], MAF_right[8] = MAF_right[7], MAF_right[7] = MAF_right[6], MAF_right[6] = MAF_right[5], MAF_right[5] = MAF_right[4], MAF_right[4] = MAF_right[3], MAF_right[3] = MAF_right[2], MAF_right[2] = MAF_right[1], MAF_right[1] = MAF_right[0]; |
laura94 | 0:5816557b2064 | 186 | MAF_right[0] = EMG_right_f3; |
laura94 | 0:5816557b2064 | 187 | } |
laura94 | 0:5816557b2064 | 188 | |
laura94 | 0:5816557b2064 | 189 | //--------------------------------------------------------------------------------------------------------------------------// |
laura94 | 0:5816557b2064 | 190 | // Motor control functions |
laura94 | 0:5816557b2064 | 191 | //--------------------------------------------------------------------------------------------------------------------------// |
laura94 | 0:5816557b2064 | 192 | |
laura94 | 0:5816557b2064 | 193 | // Motor2 control |
laura94 | 0:5816557b2064 | 194 | void motor2_Controller() |
laura94 | 0:5816557b2064 | 195 | { |
laura94 | 0:5816557b2064 | 196 | // Setpoint motor 2 |
laura94 | 0:5816557b2064 | 197 | reference2 = m2_ref; // Reference in degrees |
Rvs94 | 1:3f49c8818619 | 198 | position2 = 360.0*Encoder2.getPulses()/(32.0*131.0); // Position in degrees |
laura94 | 0:5816557b2064 | 199 | // Speed control |
laura94 | 0:5816557b2064 | 200 | double m2_P1 = PID( reference2 - position2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err); |
laura94 | 0:5816557b2064 | 201 | double m2_P2 = m2_P1; |
laura94 | 0:5816557b2064 | 202 | motor2speed = abs(m2_P2); |
laura94 | 0:5816557b2064 | 203 | // Direction control |
laura94 | 0:5816557b2064 | 204 | if(m2_P2 > 0) |
laura94 | 0:5816557b2064 | 205 | { |
laura94 | 0:5816557b2064 | 206 | motor2direction = 0; |
laura94 | 0:5816557b2064 | 207 | } |
laura94 | 0:5816557b2064 | 208 | else |
laura94 | 0:5816557b2064 | 209 | { |
laura94 | 0:5816557b2064 | 210 | motor2direction = 1; |
laura94 | 0:5816557b2064 | 211 | } |
laura94 | 0:5816557b2064 | 212 | } |
laura94 | 0:5816557b2064 | 213 | |
laura94 | 0:5816557b2064 | 214 | // Motor1 control |
laura94 | 0:5816557b2064 | 215 | void motor1_Controller() |
laura94 | 0:5816557b2064 | 216 | { |
laura94 | 0:5816557b2064 | 217 | // Setpoint Motor 1 |
laura94 | 0:5816557b2064 | 218 | reference1 = m1_ref; // Reference in degrees |
Rvs94 | 1:3f49c8818619 | 219 | position1 = 360.0*Encoder1.getPulses()/(32.0*131.0); // Position in degrees |
laura94 | 0:5816557b2064 | 220 | // Speed control |
laura94 | 0:5816557b2064 | 221 | double m1_P1 = PID( reference1 - position1, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int, m1_prev_err); |
laura94 | 0:5816557b2064 | 222 | double m1_P2 = m1_P1; |
laura94 | 0:5816557b2064 | 223 | motor1speed = abs(m1_P2); |
laura94 | 0:5816557b2064 | 224 | // Direction control |
laura94 | 0:5816557b2064 | 225 | if(m1_P2 > 0) |
laura94 | 0:5816557b2064 | 226 | { |
laura94 | 0:5816557b2064 | 227 | motor1direction = 1; |
laura94 | 0:5816557b2064 | 228 | } |
laura94 | 0:5816557b2064 | 229 | else |
laura94 | 0:5816557b2064 | 230 | { |
laura94 | 0:5816557b2064 | 231 | motor1direction = 0; |
laura94 | 0:5816557b2064 | 232 | } |
laura94 | 0:5816557b2064 | 233 | } |
laura94 | 0:5816557b2064 | 234 | |
laura94 | 0:5816557b2064 | 235 | //--------------------------------------------------------------------------------------------------------------------------// |
laura94 | 0:5816557b2064 | 236 | // Main function |
laura94 | 0:5816557b2064 | 237 | //--------------------------------------------------------------------------------------------------------------------------// |
laura94 | 0:5816557b2064 | 238 | int main() |
laura94 | 0:5816557b2064 | 239 | { |
laura94 | 0:5816557b2064 | 240 | //--------------------------------------------------------------------------------------------------------------------------// |
laura94 | 0:5816557b2064 | 241 | // Initalizing |
laura94 | 0:5816557b2064 | 242 | //--------------------------------------------------------------------------------------------------------------------------// |
laura94 | 0:5816557b2064 | 243 | //LEDs OFF |
laura94 | 0:5816557b2064 | 244 | LedR = LedB = LedG = 1; |
laura94 | 0:5816557b2064 | 245 | |
laura94 | 0:5816557b2064 | 246 | //PC connection & check |
laura94 | 0:5816557b2064 | 247 | pc.baud(115200); |
laura94 | 0:5816557b2064 | 248 | pc.printf("Tot aan loop werkt\n"); |
laura94 | 0:5816557b2064 | 249 | |
laura94 | 0:5816557b2064 | 250 | // Tickers |
laura94 | 0:5816557b2064 | 251 | ScopeTime.attach(&ScopeSend, 0.01f); // 100 Hz, Scope |
Rvs94 | 2:7873c44b0568 | 252 | myControllerTicker2.attach(&motor2_Controller, m2_Ts ); // 100 Hz, Motor 2 |
Rvs94 | 2:7873c44b0568 | 253 | myControllerTicker1.attach(&motor1_Controller, m1_Ts ); // 100 Hz, Motor 1 |
laura94 | 0:5816557b2064 | 254 | SampleEMGLeft.attach(&EMGfilterLeft, 0.01f); |
laura94 | 0:5816557b2064 | 255 | SampleEMGRight.attach(&EMGfilterRight, 0.01f); |
laura94 | 0:5816557b2064 | 256 | MovingAverageLeft.attach(&MovingAverageFilterLeft, 0.01f); |
laura94 | 0:5816557b2064 | 257 | MovingAverageRight.attach(&MovingAverageFilterRight, 0.01f); |
Rvs94 | 1:3f49c8818619 | 258 | //--------------------------------------------------------------------------------------------------------------------------// |
Rvs94 | 1:3f49c8818619 | 259 | // Determing Threshold |
Rvs94 | 1:3f49c8818619 | 260 | //--------------------------------------------------------------------------------------------------------------------------// |
laura94 | 0:5816557b2064 | 261 | wait(20); |
laura94 | 0:5816557b2064 | 262 | |
Margreeth95 | 10:5c5f2ea11045 | 263 | BlinkLed(LedG, 2, 0.2); //bepaling threshold linkerarm |
Rvs94 | 1:3f49c8818619 | 264 | LedG.write(0); |
Rvs94 | 1:3f49c8818619 | 265 | wait(2); |
Rvs94 | 7:111e13da9bc0 | 266 | Threshold1 = 0.5*EMG_left_MAF; |
Rvs94 | 1:3f49c8818619 | 267 | LedG.write(1); |
Margreeth95 | 10:5c5f2ea11045 | 268 | |
Margreeth95 | 10:5c5f2ea11045 | 269 | BlinkLed(LedR, 1, 2); |
Rvs94 | 1:3f49c8818619 | 270 | |
Margreeth95 | 10:5c5f2ea11045 | 271 | BlinkLed(LedB, 2, 0.2); //bepaling threshold rechterarm |
Rvs94 | 1:3f49c8818619 | 272 | LedB.write(0); |
Rvs94 | 1:3f49c8818619 | 273 | wait(2); |
Rvs94 | 7:111e13da9bc0 | 274 | Threshold3 = 0.5*EMG_right_MAF; |
Rvs94 | 1:3f49c8818619 | 275 | LedB.write(1); |
Margreeth95 | 10:5c5f2ea11045 | 276 | |
Margreeth95 | 10:5c5f2ea11045 | 277 | pc.printf("Threshold links = %f, Threshold rechts = %f/n", Threshold1, Threshold3); |
Margreeth95 | 10:5c5f2ea11045 | 278 | BlinkLed(LedR, 1, 2); |
Margreeth95 | 10:5c5f2ea11045 | 279 | LedR.write(1); |
laura94 | 0:5816557b2064 | 280 | //--------------------------------------------------------------------------------------------------------------------------// |
laura94 | 0:5816557b2064 | 281 | // Control Program |
laura94 | 0:5816557b2064 | 282 | //--------------------------------------------------------------------------------------------------------------------------// |
laura94 | 0:5816557b2064 | 283 | while(true) |
laura94 | 0:5816557b2064 | 284 | { |
laura94 | 0:5816557b2064 | 285 | //char c = pc.getc(); |
laura94 | 0:5816557b2064 | 286 | // 1 Program UP |
Rvs94 | 1:3f49c8818619 | 287 | if ((EMG_right_MAF >= Threshold3) && (EMG_left_MAF >= Threshold1)) //if(c == 'e') // |
laura94 | 0:5816557b2064 | 288 | { |
laura94 | 0:5816557b2064 | 289 | count = count + 1; |
laura94 | 0:5816557b2064 | 290 | if(count > 2) |
laura94 | 0:5816557b2064 | 291 | { |
laura94 | 0:5816557b2064 | 292 | count = 2; |
laura94 | 0:5816557b2064 | 293 | } |
Rvs94 | 1:3f49c8818619 | 294 | wait(2); |
laura94 | 0:5816557b2064 | 295 | } |
laura94 | 0:5816557b2064 | 296 | // 1 Program DOWN |
laura94 | 0:5816557b2064 | 297 | // if(c == 'd') // Hoe gaat dit aangestuurd worden? |
laura94 | 0:5816557b2064 | 298 | // { |
laura94 | 0:5816557b2064 | 299 | // count = count - 1; |
laura94 | 0:5816557b2064 | 300 | // if(count < 0) |
laura94 | 0:5816557b2064 | 301 | // { |
laura94 | 0:5816557b2064 | 302 | // count = 0; |
laura94 | 0:5816557b2064 | 303 | // } |
laura94 | 0:5816557b2064 | 304 | // } |
laura94 | 0:5816557b2064 | 305 | // PROGRAM 0: Motor 2 control and indirect control of motor 1, Green LED |
laura94 | 0:5816557b2064 | 306 | if(count == 0) |
laura94 | 0:5816557b2064 | 307 | { |
laura94 | 0:5816557b2064 | 308 | LedR = LedB = 1; |
laura94 | 0:5816557b2064 | 309 | LedG = 0; |
Margreeth95 | 10:5c5f2ea11045 | 310 | |
Rvs94 | 3:86ca263a83dc | 311 | if ((EMG_right_MAF >= Threshold3) && (EMG_left_MAF <= Threshold1)) //if(c == 'r') // |
laura94 | 0:5816557b2064 | 312 | { |
laura94 | 0:5816557b2064 | 313 | m2_ref = m2_ref + Stapgrootte; |
Rvs94 | 4:92a3b38a5302 | 314 | m1_ref = m1_ref + Stapgrootte; |
laura94 | 0:5816557b2064 | 315 | if (m2_ref > Grens2) |
laura94 | 0:5816557b2064 | 316 | { |
laura94 | 0:5816557b2064 | 317 | m2_ref = Grens2; |
Rvs94 | 4:92a3b38a5302 | 318 | m1_ref = Grens1; |
laura94 | 0:5816557b2064 | 319 | } |
Rvs94 | 4:92a3b38a5302 | 320 | wait(0.05); |
laura94 | 0:5816557b2064 | 321 | } |
Rvs94 | 3:86ca263a83dc | 322 | if((EMG_right_MAF < Threshold3) && (EMG_left_MAF > Threshold1)) //if (c == 'f') // |
laura94 | 0:5816557b2064 | 323 | { |
laura94 | 0:5816557b2064 | 324 | m2_ref = m2_ref - Stapgrootte; |
Rvs94 | 4:92a3b38a5302 | 325 | m1_ref = m1_ref - Stapgrootte; |
laura94 | 0:5816557b2064 | 326 | if (m2_ref < -1*Grens2) |
laura94 | 0:5816557b2064 | 327 | { |
laura94 | 0:5816557b2064 | 328 | m2_ref = -1*Grens2; |
Rvs94 | 4:92a3b38a5302 | 329 | m1_ref = -1*Grens1; |
laura94 | 0:5816557b2064 | 330 | } |
Rvs94 | 4:92a3b38a5302 | 331 | wait(0.05); |
laura94 | 0:5816557b2064 | 332 | } |
Rvs94 | 1:3f49c8818619 | 333 | } |
laura94 | 0:5816557b2064 | 334 | // PROGRAM 1: Motor 1 control, Red LED |
laura94 | 0:5816557b2064 | 335 | if(count == 1) |
laura94 | 0:5816557b2064 | 336 | { |
laura94 | 0:5816557b2064 | 337 | LedG = LedB = 1; |
laura94 | 0:5816557b2064 | 338 | LedR = 0; |
Rvs94 | 7:111e13da9bc0 | 339 | //Linker grenzen |
Rvs94 | 8:86a13efe98c8 | 340 | if ((EMG_right_MAF <= Threshold3) && (EMG_left_MAF >= Threshold1) && (m2_ref < 0)) // if(c == 't') // Draait naar links |
laura94 | 0:5816557b2064 | 341 | { |
laura94 | 0:5816557b2064 | 342 | m1_ref = m1_ref + Stapgrootte; |
Rvs94 | 1:3f49c8818619 | 343 | |
Margreeth95 | 9:350a82a7745a | 344 | if (m1_ref > m2_ref + 35 ) |
laura94 | 0:5816557b2064 | 345 | { |
Margreeth95 | 9:350a82a7745a | 346 | m1_ref = m2_ref + 35 ; |
laura94 | 0:5816557b2064 | 347 | } |
Rvs94 | 4:92a3b38a5302 | 348 | wait(0.05); |
laura94 | 0:5816557b2064 | 349 | } |
Rvs94 | 8:86a13efe98c8 | 350 | if ((EMG_left_MAF < Threshold1) && (EMG_right_MAF > Threshold3) && (m2_ref < 0)) //if(c == 'g') // Draait naar rechts |
Rvs94 | 7:111e13da9bc0 | 351 | { |
Rvs94 | 7:111e13da9bc0 | 352 | m1_ref = m1_ref - Stapgrootte; |
Rvs94 | 7:111e13da9bc0 | 353 | |
Margreeth95 | 9:350a82a7745a | 354 | if (m1_ref < -1*Grens1 ) |
Rvs94 | 7:111e13da9bc0 | 355 | { |
Margreeth95 | 9:350a82a7745a | 356 | m1_ref = -1*Grens1; |
Rvs94 | 7:111e13da9bc0 | 357 | } |
Rvs94 | 7:111e13da9bc0 | 358 | wait(0.05); |
Rvs94 | 7:111e13da9bc0 | 359 | } |
Rvs94 | 7:111e13da9bc0 | 360 | //Rechter grenzen |
Rvs94 | 8:86a13efe98c8 | 361 | if ((EMG_right_MAF <= Threshold3) && (EMG_left_MAF >= Threshold1) && (m2_ref >= 0)) // if(c == 't') // Draait naar links |
Rvs94 | 8:86a13efe98c8 | 362 | { |
Rvs94 | 7:111e13da9bc0 | 363 | m1_ref = m1_ref + Stapgrootte; |
Rvs94 | 7:111e13da9bc0 | 364 | |
Rvs94 | 8:86a13efe98c8 | 365 | if (m1_ref > Grens1 ) |
Rvs94 | 7:111e13da9bc0 | 366 | { |
Rvs94 | 8:86a13efe98c8 | 367 | m1_ref = Grens1; |
Rvs94 | 7:111e13da9bc0 | 368 | } |
Rvs94 | 7:111e13da9bc0 | 369 | wait(0.05); |
Rvs94 | 7:111e13da9bc0 | 370 | } |
Rvs94 | 8:86a13efe98c8 | 371 | if ((EMG_left_MAF < Threshold1) && (EMG_right_MAF > Threshold3) && (m2_ref >= 0)) //if(c == 'g') // Draait naar rechts |
Rvs94 | 8:86a13efe98c8 | 372 | { |
laura94 | 0:5816557b2064 | 373 | m1_ref = m1_ref - Stapgrootte; |
Rvs94 | 1:3f49c8818619 | 374 | |
Margreeth95 | 9:350a82a7745a | 375 | if (m1_ref < m2_ref - 35 ) |
laura94 | 0:5816557b2064 | 376 | { |
Margreeth95 | 9:350a82a7745a | 377 | m1_ref = m2_ref - 35 ; |
laura94 | 0:5816557b2064 | 378 | } |
Rvs94 | 4:92a3b38a5302 | 379 | wait(0.05); |
laura94 | 0:5816557b2064 | 380 | } |
laura94 | 0:5816557b2064 | 381 | } |
laura94 | 0:5816557b2064 | 382 | // PROGRAM 2: Firing mechanism & Reset, Blue LED |
laura94 | 0:5816557b2064 | 383 | if(count == 2) |
laura94 | 0:5816557b2064 | 384 | { |
Margreeth95 | 10:5c5f2ea11045 | 385 | LedG = LedR = 1; |
Margreeth95 | 10:5c5f2ea11045 | 386 | pc.printf("Please press the button to shoot the disc\n"); |
Margreeth95 | 10:5c5f2ea11045 | 387 | BlinkLed(LedB, 25, 0.1); |
Margreeth95 | 10:5c5f2ea11045 | 388 | |
laura94 | 0:5816557b2064 | 389 | //VUUUUR!! (To Do) |
Margreeth95 | 10:5c5f2ea11045 | 390 | while (m2_ref != 0) // positiearm terug naar startpositie |
Margreeth95 | 10:5c5f2ea11045 | 391 | { |
Margreeth95 | 10:5c5f2ea11045 | 392 | if(m2_ref < 0) |
Margreeth95 | 10:5c5f2ea11045 | 393 | { |
Margreeth95 | 10:5c5f2ea11045 | 394 | m2_ref = m2_ref+Stapgrootte; |
Margreeth95 | 10:5c5f2ea11045 | 395 | wait(0.05); |
Margreeth95 | 10:5c5f2ea11045 | 396 | } |
Margreeth95 | 10:5c5f2ea11045 | 397 | if(m2_ref > 0) |
Margreeth95 | 10:5c5f2ea11045 | 398 | { |
Margreeth95 | 10:5c5f2ea11045 | 399 | m2_ref = m2_ref-Stapgrootte; |
Margreeth95 | 10:5c5f2ea11045 | 400 | wait(0.05); |
Margreeth95 | 10:5c5f2ea11045 | 401 | } |
Margreeth95 | 10:5c5f2ea11045 | 402 | } |
Margreeth95 | 10:5c5f2ea11045 | 403 | |
Margreeth95 | 10:5c5f2ea11045 | 404 | while (m1_ref != 0) //werparm terug naar startpositie |
Margreeth95 | 10:5c5f2ea11045 | 405 | { |
Margreeth95 | 10:5c5f2ea11045 | 406 | if(m1_ref<0) |
Margreeth95 | 10:5c5f2ea11045 | 407 | { |
Margreeth95 | 10:5c5f2ea11045 | 408 | m1_ref = m1_ref+Stapgrootte; |
Margreeth95 | 10:5c5f2ea11045 | 409 | wait(0.05); |
Margreeth95 | 10:5c5f2ea11045 | 410 | } |
Margreeth95 | 10:5c5f2ea11045 | 411 | |
Margreeth95 | 10:5c5f2ea11045 | 412 | if(m1_ref>0) |
Margreeth95 | 10:5c5f2ea11045 | 413 | { |
Margreeth95 | 10:5c5f2ea11045 | 414 | m1_ref = m1_ref-Stapgrootte; |
Margreeth95 | 10:5c5f2ea11045 | 415 | wait(0.05); |
Margreeth95 | 10:5c5f2ea11045 | 416 | } |
Margreeth95 | 10:5c5f2ea11045 | 417 | } |
Margreeth95 | 10:5c5f2ea11045 | 418 | |
Margreeth95 | 10:5c5f2ea11045 | 419 | if((m2_ref == 0) && (m1_ref == 0)) // terug naar program 0 |
Margreeth95 | 10:5c5f2ea11045 | 420 | { |
Margreeth95 | 10:5c5f2ea11045 | 421 | count = 0; |
Margreeth95 | 10:5c5f2ea11045 | 422 | } |
laura94 | 0:5816557b2064 | 423 | } |
Rvs94 | 1:3f49c8818619 | 424 | |
laura94 | 0:5816557b2064 | 425 | } |
laura94 | 0:5816557b2064 | 426 | } |