![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
final version
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of Project_script_union_final by
main.cpp@26:ac5656aa35c7, 2018-10-31 (annotated)
- Committer:
- MarijkeZondag
- Date:
- Wed Oct 31 10:36:01 2018 +0000
- Revision:
- 26:ac5656aa35c7
- Parent:
- 25:bbef09ff226b
- Child:
- 27:fa493551be99
Alles samengevoegd
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
vsluiter | 0:c8f15874531b | 1 | #include "mbed.h" |
vsluiter | 0:c8f15874531b | 2 | #include "MODSERIAL.h" |
MarijkeZondag | 10:39ec51206c8b | 3 | #include "BiQuad.h" |
MarijkeZondag | 10:39ec51206c8b | 4 | #include "HIDScope.h" |
MarijkeZondag | 10:39ec51206c8b | 5 | #include <math.h> |
vsluiter | 0:c8f15874531b | 6 | |
MarijkeZondag | 22:5d956c93b3ae | 7 | //ATTENTION: set mBed to version 151 |
MarijkeZondag | 22:5d956c93b3ae | 8 | // set QEI to version 0, (gebruiken wij (nog) niet, is voor encoder) |
MarijkeZondag | 22:5d956c93b3ae | 9 | // set MODSERIAL to version 44 |
MarijkeZondag | 22:5d956c93b3ae | 10 | // set HIDScope to version 7 |
MarijkeZondag | 22:5d956c93b3ae | 11 | // set biquadFilter to version 7 |
MarijkeZondag | 10:39ec51206c8b | 12 | |
MarijkeZondag | 22:5d956c93b3ae | 13 | AnalogIn emg0_in (A0); //First raw EMG signal input |
MarijkeZondag | 22:5d956c93b3ae | 14 | AnalogIn emg1_in (A1); //Second raw EMG signal input |
MarijkeZondag | 22:5d956c93b3ae | 15 | AnalogIn emg2_in (A2); //Third raw EMG signal input |
MarijkeZondag | 22:5d956c93b3ae | 16 | |
MarijkeZondag | 26:ac5656aa35c7 | 17 | InterruptIn encoderA1 (D9); |
MarijkeZondag | 26:ac5656aa35c7 | 18 | InterruptIn encoderB1 (D8); |
MarijkeZondag | 26:ac5656aa35c7 | 19 | InterruptIn encoderA2 (D12); |
MarijkeZondag | 26:ac5656aa35c7 | 20 | InterruptIn encoderB2 (D13); |
MarijkeZondag | 26:ac5656aa35c7 | 21 | |
MarijkeZondag | 26:ac5656aa35c7 | 22 | InterruptIn button1 (D10); |
MarijkeZondag | 16:5f7196ddc77b | 23 | InterruptIn button2 (D11); |
MarijkeZondag | 6:f4bbb73f3989 | 24 | |
MarijkeZondag | 23:97a976a8f0fc | 25 | DigitalOut directionpin1 (D7); |
MarijkeZondag | 23:97a976a8f0fc | 26 | DigitalOut directionpin2 (D4); |
MarijkeZondag | 26:ac5656aa35c7 | 27 | |
MarijkeZondag | 23:97a976a8f0fc | 28 | PwmOut pwmpin1 (D6); |
MarijkeZondag | 23:97a976a8f0fc | 29 | PwmOut pwmpin2 (D5); |
MarijkeZondag | 22:5d956c93b3ae | 30 | |
MarijkeZondag | 17:741798018c0d | 31 | DigitalOut ledr (LED_RED); |
MarijkeZondag | 17:741798018c0d | 32 | DigitalOut ledb (LED_BLUE); |
MarijkeZondag | 17:741798018c0d | 33 | DigitalOut ledg (LED_GREEN); |
MarijkeZondag | 13:a3d4b4daf5b4 | 34 | |
MarijkeZondag | 9:c722418997b5 | 35 | |
MarijkeZondag | 23:97a976a8f0fc | 36 | MODSERIAL pc(USBTX, USBRX); //Serial communication to see if the code works step by step, turn on if hidscope is off |
MarijkeZondag | 6:f4bbb73f3989 | 37 | |
MarijkeZondag | 23:97a976a8f0fc | 38 | //HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered |
vsluiter | 0:c8f15874531b | 39 | |
MarijkeZondag | 22:5d956c93b3ae | 40 | //Tickers |
Marle | 25:bbef09ff226b | 41 | Ticker ticker; |
MarijkeZondag | 9:c722418997b5 | 42 | |
MarijkeZondag | 9:c722418997b5 | 43 | //Global variables |
MarijkeZondag | 22:5d956c93b3ae | 44 | const float T = 0.002f; //Ticker period |
MarijkeZondag | 10:39ec51206c8b | 45 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 46 | //EMG filter |
MarijkeZondag | 22:5d956c93b3ae | 47 | double emg0_filt, emg1_filt, emg2_filt; //Variables for filtered EMG data channel 0, 1 and 2 |
MarijkeZondag | 22:5d956c93b3ae | 48 | double emg0_raw, emg1_raw, emg2_raw; |
MarijkeZondag | 22:5d956c93b3ae | 49 | double emg0_filt_x, emg1_filt_x, emg2_filt_x; |
MarijkeZondag | 22:5d956c93b3ae | 50 | const int windowsize = 150; //Size of the array over which the moving average (MovAg) is calculated. (random number) |
MarijkeZondag | 13:a3d4b4daf5b4 | 51 | double sum, sum1, sum2, sum3; //variables used to sum elements in array |
MarijkeZondag | 22:5d956c93b3ae | 52 | double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize]; //Empty arrays to calculate MoveAg |
MarijkeZondag | 22:5d956c93b3ae | 53 | double movAg0, movAg1, movAg2; //outcome of MovAg (moet dit een array zijn??) |
MarijkeZondag | 13:a3d4b4daf5b4 | 54 | |
MarijkeZondag | 22:5d956c93b3ae | 55 | //Calibration variables |
MarijkeZondag | 22:5d956c93b3ae | 56 | int x = -1; //Start switch, colour LED is blue. |
MarijkeZondag | 22:5d956c93b3ae | 57 | int emg_cal = 0; //if emg_cal is set to 1, motors can begin to work in this code (!!) |
MarijkeZondag | 22:5d956c93b3ae | 58 | const int sizeCal = 1500; //size of the dataset used for calibration, eerst 2000 |
MarijkeZondag | 22:5d956c93b3ae | 59 | double StoreCal0[sizeCal], StoreCal1[sizeCal], StoreCal2[sizeCal]; //arrays to put the dataset of the calibration in |
MarijkeZondag | 22:5d956c93b3ae | 60 | double Mean0,Mean1,Mean2; //average of maximum tightening |
MarijkeZondag | 22:5d956c93b3ae | 61 | double Threshold0, Threshold1, Threshold2; |
MarijkeZondag | 13:a3d4b4daf5b4 | 62 | |
MarijkeZondag | 22:5d956c93b3ae | 63 | //Biquad //Variables for the biquad band filters (alle 3 dezelfde maar je kan niet 3x 'emg0band' aanroepen ofzo) |
MarijkeZondag | 22:5d956c93b3ae | 64 | BiQuadChain emg0filter; |
MarijkeZondag | 10:39ec51206c8b | 65 | BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 66 | BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 67 | BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); |
MarijkeZondag | 22:5d956c93b3ae | 68 | BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter biquad coefficients |
MarijkeZondag | 10:39ec51206c8b | 69 | |
MarijkeZondag | 22:5d956c93b3ae | 70 | BiQuadChain emg1filter; |
MarijkeZondag | 10:39ec51206c8b | 71 | BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 72 | BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 73 | BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); |
MarijkeZondag | 22:5d956c93b3ae | 74 | BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter |
MarijkeZondag | 10:39ec51206c8b | 75 | |
MarijkeZondag | 22:5d956c93b3ae | 76 | BiQuadChain emg2filter; |
MarijkeZondag | 10:39ec51206c8b | 77 | BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 78 | BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 79 | BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); |
MarijkeZondag | 14:fa09dae67390 | 80 | BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter |
MarijkeZondag | 10:39ec51206c8b | 81 | |
MarijkeZondag | 26:ac5656aa35c7 | 82 | // Inverse Kinematica variables |
Marle | 25:bbef09ff226b | 83 | const double L1 = 0.208; // Hoogte van tafel tot joint 1 |
Marle | 25:bbef09ff226b | 84 | const double L2 = 0.288; // Hoogte van tafel tot joint 2 |
Marle | 25:bbef09ff226b | 85 | const double L3 = 0.212; // Lengte van de arm |
Marle | 25:bbef09ff226b | 86 | const double L4 = 0.089; // Afstand van achterkant base tot joint 1 |
Marle | 25:bbef09ff226b | 87 | const double L5 = 0.030; // Afstand van joint 1 naar joint 2 |
Marle | 25:bbef09ff226b | 88 | const double r_trans = 0.035; // Kan gebruikt worden om om te rekenen van translation naar shaft rotation |
Marle | 25:bbef09ff226b | 89 | |
Marle | 25:bbef09ff226b | 90 | // Variërende variabelen inverse kinematics: |
Marle | 25:bbef09ff226b | 91 | double q1ref = 0; // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is |
Marle | 25:bbef09ff226b | 92 | double q2ref = 0; // Huidige motorhoek van joint 2 zoals bepaald uit referentiesignaal --> checken of het goede type is |
Marle | 25:bbef09ff226b | 93 | double v_x; // Desired velocity end effector in x direction --> Determined by EMG signals |
Marle | 25:bbef09ff226b | 94 | double v_y; // Desired velocity end effector in y direction --> Determined by EMG signals |
Marle | 25:bbef09ff226b | 95 | |
Marle | 25:bbef09ff226b | 96 | double Lq1; // Translatieafstand als gevolg van motor rotation joint 1 |
Marle | 25:bbef09ff226b | 97 | double Cq2; // Joint angle of the system (corrected for gear ratio 1:5) |
Marle | 25:bbef09ff226b | 98 | |
Marle | 25:bbef09ff226b | 99 | double q1_dot; // Benodigde hoeksnelheid van motor 1 om v_des te bereiken |
Marle | 25:bbef09ff226b | 100 | double q2_dot; // Benodigde hoeksnelheid van motor 2 om v_des te bereiken |
Marle | 25:bbef09ff226b | 101 | |
Marle | 25:bbef09ff226b | 102 | double q1_ii; // Reference signal for motorangle q1ref |
Marle | 25:bbef09ff226b | 103 | double q2_ii; // Reference signal for motorangle q2ref |
Marle | 25:bbef09ff226b | 104 | |
MarijkeZondag | 26:ac5656aa35c7 | 105 | //Variables PID controller |
MarijkeZondag | 26:ac5656aa35c7 | 106 | double PI = 3.14159; |
MarijkeZondag | 26:ac5656aa35c7 | 107 | double Kp1 = 17.5; //Motor 1 |
MarijkeZondag | 26:ac5656aa35c7 | 108 | double Ki1 = 1.02; |
MarijkeZondag | 26:ac5656aa35c7 | 109 | double Kd1 = 23.2; |
MarijkeZondag | 26:ac5656aa35c7 | 110 | double encoder1 = 0; |
MarijkeZondag | 26:ac5656aa35c7 | 111 | double encoder_radians1=0; |
Marle | 25:bbef09ff226b | 112 | |
MarijkeZondag | 26:ac5656aa35c7 | 113 | double Kp2 = 17.5; //Motor 2 |
MarijkeZondag | 26:ac5656aa35c7 | 114 | double Ki2 = 1.02; |
MarijkeZondag | 26:ac5656aa35c7 | 115 | double Kd2 = 23.2; |
MarijkeZondag | 26:ac5656aa35c7 | 116 | double encoder2 = 0; |
MarijkeZondag | 26:ac5656aa35c7 | 117 | double encoder_radians2=0; |
MarijkeZondag | 26:ac5656aa35c7 | 118 | |
MarijkeZondag | 26:ac5656aa35c7 | 119 | |
MarijkeZondag | 26:ac5656aa35c7 | 120 | //--------------Functions----------------------------------------------------------------------------------------------------------------------------// |
MarijkeZondag | 26:ac5656aa35c7 | 121 | |
MarijkeZondag | 26:ac5656aa35c7 | 122 | |
MarijkeZondag | 26:ac5656aa35c7 | 123 | //------------------ Encoder motor 1 --------------------------------// |
MarijkeZondag | 26:ac5656aa35c7 | 124 | |
MarijkeZondag | 26:ac5656aa35c7 | 125 | void encoderA1_rise() |
MarijkeZondag | 26:ac5656aa35c7 | 126 | { |
MarijkeZondag | 26:ac5656aa35c7 | 127 | if(encoderB1==false) |
MarijkeZondag | 26:ac5656aa35c7 | 128 | { |
MarijkeZondag | 26:ac5656aa35c7 | 129 | encoder1++; |
MarijkeZondag | 26:ac5656aa35c7 | 130 | } |
MarijkeZondag | 26:ac5656aa35c7 | 131 | else |
MarijkeZondag | 26:ac5656aa35c7 | 132 | { |
MarijkeZondag | 26:ac5656aa35c7 | 133 | encoder1--; |
MarijkeZondag | 26:ac5656aa35c7 | 134 | } |
MarijkeZondag | 26:ac5656aa35c7 | 135 | } |
MarijkeZondag | 26:ac5656aa35c7 | 136 | |
MarijkeZondag | 26:ac5656aa35c7 | 137 | void encoderA1_fall() |
MarijkeZondag | 26:ac5656aa35c7 | 138 | { |
MarijkeZondag | 26:ac5656aa35c7 | 139 | if(encoderB1==true) |
MarijkeZondag | 26:ac5656aa35c7 | 140 | { |
MarijkeZondag | 26:ac5656aa35c7 | 141 | encoder1++; |
MarijkeZondag | 26:ac5656aa35c7 | 142 | } |
MarijkeZondag | 26:ac5656aa35c7 | 143 | else |
MarijkeZondag | 26:ac5656aa35c7 | 144 | { |
MarijkeZondag | 26:ac5656aa35c7 | 145 | encoder1--; |
MarijkeZondag | 26:ac5656aa35c7 | 146 | } |
MarijkeZondag | 26:ac5656aa35c7 | 147 | } |
MarijkeZondag | 26:ac5656aa35c7 | 148 | |
MarijkeZondag | 26:ac5656aa35c7 | 149 | void encoderB1_rise() |
MarijkeZondag | 26:ac5656aa35c7 | 150 | { |
MarijkeZondag | 26:ac5656aa35c7 | 151 | if(encoderA1==true) |
MarijkeZondag | 26:ac5656aa35c7 | 152 | { |
MarijkeZondag | 26:ac5656aa35c7 | 153 | encoder1++; |
MarijkeZondag | 26:ac5656aa35c7 | 154 | } |
MarijkeZondag | 26:ac5656aa35c7 | 155 | else |
MarijkeZondag | 26:ac5656aa35c7 | 156 | { |
MarijkeZondag | 26:ac5656aa35c7 | 157 | encoder1--; |
MarijkeZondag | 26:ac5656aa35c7 | 158 | } |
MarijkeZondag | 26:ac5656aa35c7 | 159 | } |
MarijkeZondag | 26:ac5656aa35c7 | 160 | |
MarijkeZondag | 26:ac5656aa35c7 | 161 | void encoderB1_fall() |
MarijkeZondag | 26:ac5656aa35c7 | 162 | { |
MarijkeZondag | 26:ac5656aa35c7 | 163 | if(encoderA1==false) |
MarijkeZondag | 26:ac5656aa35c7 | 164 | { |
MarijkeZondag | 26:ac5656aa35c7 | 165 | encoder1++; |
MarijkeZondag | 26:ac5656aa35c7 | 166 | } |
MarijkeZondag | 26:ac5656aa35c7 | 167 | else |
MarijkeZondag | 26:ac5656aa35c7 | 168 | { |
MarijkeZondag | 26:ac5656aa35c7 | 169 | encoder1--; |
MarijkeZondag | 26:ac5656aa35c7 | 170 | } |
MarijkeZondag | 26:ac5656aa35c7 | 171 | } |
MarijkeZondag | 26:ac5656aa35c7 | 172 | |
MarijkeZondag | 26:ac5656aa35c7 | 173 | void encoder_count1() |
MarijkeZondag | 26:ac5656aa35c7 | 174 | { |
MarijkeZondag | 26:ac5656aa35c7 | 175 | encoderA1.rise(&encoderA1_rise); |
MarijkeZondag | 26:ac5656aa35c7 | 176 | encoderA1.fall(&encoderA1_fall); |
MarijkeZondag | 26:ac5656aa35c7 | 177 | encoderB1.rise(&encoderB1_rise); |
MarijkeZondag | 26:ac5656aa35c7 | 178 | encoderB1.fall(&encoderB1_fall); |
MarijkeZondag | 26:ac5656aa35c7 | 179 | } |
MarijkeZondag | 26:ac5656aa35c7 | 180 | |
MarijkeZondag | 26:ac5656aa35c7 | 181 | //------------------ Encoder motor 2 --------------------------------// |
MarijkeZondag | 26:ac5656aa35c7 | 182 | |
MarijkeZondag | 26:ac5656aa35c7 | 183 | void encoderA2_rise() |
MarijkeZondag | 26:ac5656aa35c7 | 184 | { |
MarijkeZondag | 26:ac5656aa35c7 | 185 | if(encoderB2==false) |
MarijkeZondag | 26:ac5656aa35c7 | 186 | { |
MarijkeZondag | 26:ac5656aa35c7 | 187 | encoder2++; |
MarijkeZondag | 26:ac5656aa35c7 | 188 | } |
MarijkeZondag | 26:ac5656aa35c7 | 189 | else |
MarijkeZondag | 26:ac5656aa35c7 | 190 | { |
MarijkeZondag | 26:ac5656aa35c7 | 191 | encoder2--; |
MarijkeZondag | 26:ac5656aa35c7 | 192 | } |
MarijkeZondag | 26:ac5656aa35c7 | 193 | } |
MarijkeZondag | 12:eaed305a76c3 | 194 | |
MarijkeZondag | 26:ac5656aa35c7 | 195 | void encoderA2_fall() |
MarijkeZondag | 26:ac5656aa35c7 | 196 | { |
MarijkeZondag | 26:ac5656aa35c7 | 197 | if(encoderB2==true) |
MarijkeZondag | 26:ac5656aa35c7 | 198 | { |
MarijkeZondag | 26:ac5656aa35c7 | 199 | encoder2++; |
MarijkeZondag | 26:ac5656aa35c7 | 200 | } |
MarijkeZondag | 26:ac5656aa35c7 | 201 | else |
MarijkeZondag | 26:ac5656aa35c7 | 202 | { |
MarijkeZondag | 26:ac5656aa35c7 | 203 | encoder2--; |
MarijkeZondag | 26:ac5656aa35c7 | 204 | } |
MarijkeZondag | 26:ac5656aa35c7 | 205 | } |
MarijkeZondag | 26:ac5656aa35c7 | 206 | |
MarijkeZondag | 26:ac5656aa35c7 | 207 | void encoderB2_rise() |
MarijkeZondag | 26:ac5656aa35c7 | 208 | { |
MarijkeZondag | 26:ac5656aa35c7 | 209 | if(encoderA2==true) |
MarijkeZondag | 26:ac5656aa35c7 | 210 | { |
MarijkeZondag | 26:ac5656aa35c7 | 211 | encoder2++; |
MarijkeZondag | 26:ac5656aa35c7 | 212 | } |
MarijkeZondag | 26:ac5656aa35c7 | 213 | else |
MarijkeZondag | 26:ac5656aa35c7 | 214 | { |
MarijkeZondag | 26:ac5656aa35c7 | 215 | encoder2--; |
MarijkeZondag | 26:ac5656aa35c7 | 216 | } |
MarijkeZondag | 26:ac5656aa35c7 | 217 | } |
MarijkeZondag | 26:ac5656aa35c7 | 218 | |
MarijkeZondag | 26:ac5656aa35c7 | 219 | void encoderB2_fall() |
MarijkeZondag | 26:ac5656aa35c7 | 220 | { |
MarijkeZondag | 26:ac5656aa35c7 | 221 | if(encoderA2==false) |
MarijkeZondag | 26:ac5656aa35c7 | 222 | { |
MarijkeZondag | 26:ac5656aa35c7 | 223 | encoder2++; |
MarijkeZondag | 26:ac5656aa35c7 | 224 | } |
MarijkeZondag | 26:ac5656aa35c7 | 225 | else |
MarijkeZondag | 26:ac5656aa35c7 | 226 | { |
MarijkeZondag | 26:ac5656aa35c7 | 227 | encoder2--; |
MarijkeZondag | 26:ac5656aa35c7 | 228 | } |
MarijkeZondag | 26:ac5656aa35c7 | 229 | } |
MarijkeZondag | 26:ac5656aa35c7 | 230 | |
MarijkeZondag | 26:ac5656aa35c7 | 231 | void encoder_count2() |
MarijkeZondag | 26:ac5656aa35c7 | 232 | { |
MarijkeZondag | 26:ac5656aa35c7 | 233 | encoderA2.rise(&encoderA2_rise); |
MarijkeZondag | 26:ac5656aa35c7 | 234 | encoderA2.fall(&encoderA2_fall); |
MarijkeZondag | 26:ac5656aa35c7 | 235 | encoderB2.rise(&encoderB2_rise); |
MarijkeZondag | 26:ac5656aa35c7 | 236 | encoderB2.fall(&encoderB2_fall); |
MarijkeZondag | 26:ac5656aa35c7 | 237 | } |
MarijkeZondag | 26:ac5656aa35c7 | 238 | |
MarijkeZondag | 26:ac5656aa35c7 | 239 | //------------------ Filter EMG + Calibration EMG --------------------------------// |
MarijkeZondag | 26:ac5656aa35c7 | 240 | |
MarijkeZondag | 26:ac5656aa35c7 | 241 | void EMGFilter0() |
MarijkeZondag | 26:ac5656aa35c7 | 242 | { |
MarijkeZondag | 26:ac5656aa35c7 | 243 | emg0_raw = emg0_in.read(); //give name to raw EMG0 data calve |
MarijkeZondag | 26:ac5656aa35c7 | 244 | emg0_filt_x = emg0filter.step(emg0_raw); //Use biquad chain to filter raw EMG data |
MarijkeZondag | 26:ac5656aa35c7 | 245 | emg0_filt = abs(emg0_filt_x); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch, stel er komt iets raars uit, dan Notch uit de biquad chain halen en aparte chain voor aanmaken. |
MarijkeZondag | 26:ac5656aa35c7 | 246 | } |
MarijkeZondag | 26:ac5656aa35c7 | 247 | |
MarijkeZondag | 26:ac5656aa35c7 | 248 | void EMGFilter1() |
MarijkeZondag | 26:ac5656aa35c7 | 249 | { |
MarijkeZondag | 26:ac5656aa35c7 | 250 | emg1_raw = emg1_in.read(); //give name to raw EMG1 data bicep 1 |
MarijkeZondag | 26:ac5656aa35c7 | 251 | emg1_filt_x = emg1filter.step(emg1_raw); //Use biquad chain to filter raw EMG data |
MarijkeZondag | 26:ac5656aa35c7 | 252 | emg1_filt = abs(emg1_filt_x); //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch. |
MarijkeZondag | 26:ac5656aa35c7 | 253 | } |
MarijkeZondag | 26:ac5656aa35c7 | 254 | |
MarijkeZondag | 26:ac5656aa35c7 | 255 | void EMGFilter2() |
MarijkeZondag | 26:ac5656aa35c7 | 256 | { |
MarijkeZondag | 26:ac5656aa35c7 | 257 | emg2_raw = emg2_in.read(); //Give name to raw EMG1 data bicep 2 |
MarijkeZondag | 26:ac5656aa35c7 | 258 | emg2_filt_x = emg2filter.step(emg2_raw); //Use biquad chain to filter raw EMG data |
MarijkeZondag | 26:ac5656aa35c7 | 259 | emg2_filt = abs(emg2_filt_x); //Rectifier. LET OP: volgorde filter: band-notch-rectifier. |
MarijkeZondag | 26:ac5656aa35c7 | 260 | } |
MarijkeZondag | 26:ac5656aa35c7 | 261 | |
MarijkeZondag | 26:ac5656aa35c7 | 262 | void MovAg() //Calculate moving average (MovAg) |
MarijkeZondag | 26:ac5656aa35c7 | 263 | { |
MarijkeZondag | 26:ac5656aa35c7 | 264 | for (int i = windowsize-1; i>=0; i--) //Make arrays for the last datapoints of the filtered signals |
MarijkeZondag | 26:ac5656aa35c7 | 265 | { |
MarijkeZondag | 26:ac5656aa35c7 | 266 | StoreArray0[i] = StoreArray0[i-1]; //Shifts the i'th element one place to the right, this makes it "rolling or moving" average. |
MarijkeZondag | 26:ac5656aa35c7 | 267 | StoreArray1[i] = StoreArray1[i-1]; |
MarijkeZondag | 26:ac5656aa35c7 | 268 | StoreArray2[i] = StoreArray2[i-1]; |
MarijkeZondag | 26:ac5656aa35c7 | 269 | } |
MarijkeZondag | 26:ac5656aa35c7 | 270 | |
MarijkeZondag | 26:ac5656aa35c7 | 271 | StoreArray0[0] = emg0_filt; //Stores the latest datapoint of the filtered signal in the first element of the array |
MarijkeZondag | 26:ac5656aa35c7 | 272 | StoreArray1[0] = emg1_filt; |
MarijkeZondag | 26:ac5656aa35c7 | 273 | StoreArray2[0] = emg2_filt; |
MarijkeZondag | 26:ac5656aa35c7 | 274 | |
MarijkeZondag | 26:ac5656aa35c7 | 275 | sum1 = 0.0; |
MarijkeZondag | 26:ac5656aa35c7 | 276 | sum2 = 0.0; |
MarijkeZondag | 26:ac5656aa35c7 | 277 | sum3 = 0.0; |
MarijkeZondag | 26:ac5656aa35c7 | 278 | |
MarijkeZondag | 26:ac5656aa35c7 | 279 | for(int a = 0; a<= windowsize-1; a++) //Sums the elements in the arrays |
MarijkeZondag | 26:ac5656aa35c7 | 280 | { |
MarijkeZondag | 26:ac5656aa35c7 | 281 | sum1 += StoreArray0[a]; |
MarijkeZondag | 26:ac5656aa35c7 | 282 | sum2 += StoreArray1[a]; |
MarijkeZondag | 26:ac5656aa35c7 | 283 | sum3 += StoreArray2[a]; |
MarijkeZondag | 26:ac5656aa35c7 | 284 | } |
MarijkeZondag | 26:ac5656aa35c7 | 285 | |
MarijkeZondag | 26:ac5656aa35c7 | 286 | movAg0 = sum1/windowsize; //calculates an average in the array |
MarijkeZondag | 26:ac5656aa35c7 | 287 | movAg1 = sum2/windowsize; |
MarijkeZondag | 26:ac5656aa35c7 | 288 | movAg2 = sum3/windowsize; |
MarijkeZondag | 26:ac5656aa35c7 | 289 | //serial getallen sturen, als het 1 getal is gaat hier wat fout, als het een reeks is dan gaat er bij de input naar HIDscope wat fout. |
MarijkeZondag | 26:ac5656aa35c7 | 290 | } |
MarijkeZondag | 26:ac5656aa35c7 | 291 | |
MarijkeZondag | 26:ac5656aa35c7 | 292 | void emg_filtered() //Call all filter functions |
MarijkeZondag | 26:ac5656aa35c7 | 293 | { |
MarijkeZondag | 26:ac5656aa35c7 | 294 | EMGFilter0(); |
MarijkeZondag | 26:ac5656aa35c7 | 295 | EMGFilter1(); |
MarijkeZondag | 26:ac5656aa35c7 | 296 | EMGFilter2(); |
MarijkeZondag | 26:ac5656aa35c7 | 297 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 298 | void switch_to_calibrate() |
MarijkeZondag | 13:a3d4b4daf5b4 | 299 | { |
MarijkeZondag | 22:5d956c93b3ae | 300 | x++; //Every time function gets called, x increases. Every button press --> new calibration state. |
MarijkeZondag | 22:5d956c93b3ae | 301 | //Starts with x = -1. So when function gets called 1 time, x = 0. In the end, x = 4 will reset to -1. |
MarijkeZondag | 22:5d956c93b3ae | 302 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 303 | if(x==0) //If x = 0, led is red |
MarijkeZondag | 13:a3d4b4daf5b4 | 304 | { |
MarijkeZondag | 17:741798018c0d | 305 | ledr = 0; |
MarijkeZondag | 17:741798018c0d | 306 | ledb = 1; |
MarijkeZondag | 17:741798018c0d | 307 | ledg = 1; |
MarijkeZondag | 13:a3d4b4daf5b4 | 308 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 309 | else if (x==1) //If x = 1, led is blue |
MarijkeZondag | 13:a3d4b4daf5b4 | 310 | { |
MarijkeZondag | 17:741798018c0d | 311 | ledr = 1; |
MarijkeZondag | 17:741798018c0d | 312 | ledb = 0; |
MarijkeZondag | 17:741798018c0d | 313 | ledg = 1; |
MarijkeZondag | 13:a3d4b4daf5b4 | 314 | } |
MarijkeZondag | 22:5d956c93b3ae | 315 | else if (x==2) //If x = 2, led is green |
MarijkeZondag | 13:a3d4b4daf5b4 | 316 | { |
MarijkeZondag | 17:741798018c0d | 317 | ledr = 1; |
MarijkeZondag | 17:741798018c0d | 318 | ledb = 1; |
MarijkeZondag | 17:741798018c0d | 319 | ledg = 0; |
MarijkeZondag | 13:a3d4b4daf5b4 | 320 | } |
MarijkeZondag | 22:5d956c93b3ae | 321 | else //If x = 3 or 4, led is white |
MarijkeZondag | 13:a3d4b4daf5b4 | 322 | { |
MarijkeZondag | 17:741798018c0d | 323 | ledr = 0; |
MarijkeZondag | 17:741798018c0d | 324 | ledb = 0; |
MarijkeZondag | 17:741798018c0d | 325 | ledg = 0; |
MarijkeZondag | 13:a3d4b4daf5b4 | 326 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 327 | |
MarijkeZondag | 22:5d956c93b3ae | 328 | if(x==4) //Reset back to x = -1 |
MarijkeZondag | 13:a3d4b4daf5b4 | 329 | { |
MarijkeZondag | 16:5f7196ddc77b | 330 | x = -1; |
MarijkeZondag | 23:97a976a8f0fc | 331 | emg_cal=0; //reset, motors off |
MarijkeZondag | 13:a3d4b4daf5b4 | 332 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 333 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 334 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 335 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 336 | void calibrate(void) |
MarijkeZondag | 12:eaed305a76c3 | 337 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 338 | switch(x) |
MarijkeZondag | 13:a3d4b4daf5b4 | 339 | { |
MarijkeZondag | 22:5d956c93b3ae | 340 | case 0: //If calibration state 0: |
MarijkeZondag | 13:a3d4b4daf5b4 | 341 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 342 | sum = 0.0; |
MarijkeZondag | 22:5d956c93b3ae | 343 | for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 0 |
MarijkeZondag | 13:a3d4b4daf5b4 | 344 | { |
MarijkeZondag | 22:5d956c93b3ae | 345 | StoreCal0[j] = emg0_filt; |
MarijkeZondag | 13:a3d4b4daf5b4 | 346 | sum+=StoreCal0[j]; |
MarijkeZondag | 22:5d956c93b3ae | 347 | wait(0.001f); //Does there need to be a wait? |
MarijkeZondag | 13:a3d4b4daf5b4 | 348 | } |
MarijkeZondag | 22:5d956c93b3ae | 349 | Mean0 = sum/sizeCal; //Calculate mean of the datapoints in the calibration set (2000 samples) |
MarijkeZondag | 23:97a976a8f0fc | 350 | Threshold0 = Mean0/2; //Threshold calculation = 0.5*mean |
MarijkeZondag | 22:5d956c93b3ae | 351 | break; //Stop. Threshold is calculated, we will use this further in the code |
MarijkeZondag | 13:a3d4b4daf5b4 | 352 | } |
MarijkeZondag | 22:5d956c93b3ae | 353 | case 1: //If calibration state 1: |
MarijkeZondag | 13:a3d4b4daf5b4 | 354 | { |
MarijkeZondag | 22:5d956c93b3ae | 355 | sum = 0.0; |
MarijkeZondag | 22:5d956c93b3ae | 356 | for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 1 |
MarijkeZondag | 13:a3d4b4daf5b4 | 357 | { |
MarijkeZondag | 22:5d956c93b3ae | 358 | StoreCal1[j] = emg1_filt; |
MarijkeZondag | 13:a3d4b4daf5b4 | 359 | sum+=StoreCal1[j]; |
MarijkeZondag | 21:1da43fdbd254 | 360 | wait(0.001f); |
MarijkeZondag | 13:a3d4b4daf5b4 | 361 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 362 | Mean1 = sum/sizeCal; |
MarijkeZondag | 23:97a976a8f0fc | 363 | Threshold1 = Mean1/2; |
MarijkeZondag | 13:a3d4b4daf5b4 | 364 | break; |
MarijkeZondag | 13:a3d4b4daf5b4 | 365 | } |
MarijkeZondag | 22:5d956c93b3ae | 366 | case 2: //If calibration state 2: |
MarijkeZondag | 13:a3d4b4daf5b4 | 367 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 368 | sum = 0.0; |
MarijkeZondag | 22:5d956c93b3ae | 369 | for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 2 |
MarijkeZondag | 13:a3d4b4daf5b4 | 370 | { |
MarijkeZondag | 23:97a976a8f0fc | 371 | StoreCal2[j] = emg2_filt; |
MarijkeZondag | 13:a3d4b4daf5b4 | 372 | sum+=StoreCal2[j]; |
MarijkeZondag | 21:1da43fdbd254 | 373 | wait(0.001f); |
MarijkeZondag | 13:a3d4b4daf5b4 | 374 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 375 | Mean2 = sum/sizeCal; |
MarijkeZondag | 23:97a976a8f0fc | 376 | Threshold2 = Mean2/2; |
MarijkeZondag | 13:a3d4b4daf5b4 | 377 | break; |
MarijkeZondag | 13:a3d4b4daf5b4 | 378 | } |
MarijkeZondag | 22:5d956c93b3ae | 379 | case 3: //EMG is calibrated, robot can be set to Home position. |
MarijkeZondag | 13:a3d4b4daf5b4 | 380 | { |
MarijkeZondag | 22:5d956c93b3ae | 381 | emg_cal = 1; //This is the setting for which the motors can begin turning in this code (!!) |
MarijkeZondag | 21:1da43fdbd254 | 382 | wait(0.001f); |
MarijkeZondag | 13:a3d4b4daf5b4 | 383 | break; |
MarijkeZondag | 13:a3d4b4daf5b4 | 384 | } |
MarijkeZondag | 22:5d956c93b3ae | 385 | default: //Ensures nothing happens if x is not 0,1 or 2. |
MarijkeZondag | 13:a3d4b4daf5b4 | 386 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 387 | break; |
MarijkeZondag | 13:a3d4b4daf5b4 | 388 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 389 | } |
MarijkeZondag | 12:eaed305a76c3 | 390 | } |
MarijkeZondag | 26:ac5656aa35c7 | 391 | /* |
MarijkeZondag | 22:5d956c93b3ae | 392 | void HIDScope_sample() |
MarijkeZondag | 22:5d956c93b3ae | 393 | { |
MarijkeZondag | 26:ac5656aa35c7 | 394 | scope.set(0,emg0_raw); |
MarijkeZondag | 26:ac5656aa35c7 | 395 | scope.set(1,emg0_filt); |
MarijkeZondag | 26:ac5656aa35c7 | 396 | scope.set(1,movAg0); //als moving average werkt |
MarijkeZondag | 26:ac5656aa35c7 | 397 | scope.set(2,emg1_raw); |
MarijkeZondag | 26:ac5656aa35c7 | 398 | scope.set(3,emg1_filt); |
MarijkeZondag | 26:ac5656aa35c7 | 399 | scope.set(3,movAg1); //als moving average werkt |
MarijkeZondag | 26:ac5656aa35c7 | 400 | scope.set(4,emg2_raw); |
MarijkeZondag | 26:ac5656aa35c7 | 401 | scope.set(5,emg2_filt); |
MarijkeZondag | 26:ac5656aa35c7 | 402 | scope.set(5,movAg2); //als moving average werkt |
MarijkeZondag | 22:5d956c93b3ae | 403 | |
MarijkeZondag | 26:ac5656aa35c7 | 404 | scope.send(); //Send data to HIDScope server |
MarijkeZondag | 22:5d956c93b3ae | 405 | } |
MarijkeZondag | 26:ac5656aa35c7 | 406 | */ |
MarijkeZondag | 8:895d941a5910 | 407 | |
MarijkeZondag | 26:ac5656aa35c7 | 408 | //------------------ Inversed Kinematics --------------------------------// |
MarijkeZondag | 8:895d941a5910 | 409 | |
Marle | 25:bbef09ff226b | 410 | void inverse_kinematics() |
Marle | 25:bbef09ff226b | 411 | { |
Marle | 25:bbef09ff226b | 412 | Lq1 = q1ref*r_trans; |
Marle | 25:bbef09ff226b | 413 | Cq2 = q2ref/5.0; |
Marle | 25:bbef09ff226b | 414 | |
Marle | 25:bbef09ff226b | 415 | q1_dot = v_x + (v_y*(L1 + L3*sin(Cq2)))/(L4 + Lq1 + L3*cos(Cq2)); |
Marle | 25:bbef09ff226b | 416 | q2_dot = v_y/(L4 + Lq1 + L3*cos(Cq2)); |
Marle | 25:bbef09ff226b | 417 | |
Marle | 25:bbef09ff226b | 418 | q1_ii = q1ref + q1_dot*T; |
Marle | 25:bbef09ff226b | 419 | q2_ii = q2ref + q2_dot*T; |
Marle | 25:bbef09ff226b | 420 | |
Marle | 25:bbef09ff226b | 421 | q1ref = q1_ii; |
Marle | 25:bbef09ff226b | 422 | q2ref = q2_ii; |
Marle | 25:bbef09ff226b | 423 | } |
Marle | 25:bbef09ff226b | 424 | |
Marle | 25:bbef09ff226b | 425 | void v_des_calculate_qref() |
MarijkeZondag | 23:97a976a8f0fc | 426 | { |
MarijkeZondag | 26:ac5656aa35c7 | 427 | while(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0. |
MarijkeZondag | 16:5f7196ddc77b | 428 | { |
MarijkeZondag | 26:ac5656aa35c7 | 429 | if(movAg1>Threshold1) //If the filtered EMG signal of muscle 1 is higher than the threshold, motor 1 will turn |
MarijkeZondag | 16:5f7196ddc77b | 430 | { |
MarijkeZondag | 26:ac5656aa35c7 | 431 | v_x = 1.0; //beweging in +x direction |
MarijkeZondag | 26:ac5656aa35c7 | 432 | ledr = 0; //red |
Marle | 25:bbef09ff226b | 433 | ledb = 1; |
Marle | 25:bbef09ff226b | 434 | ledg = 1; |
MarijkeZondag | 16:5f7196ddc77b | 435 | } |
MarijkeZondag | 26:ac5656aa35c7 | 436 | else if(movAg2>Threshold2) //If the filtered EMG signal of muscle 2 is higher than the threshold, motor 1 and 2 will turn |
MarijkeZondag | 16:5f7196ddc77b | 437 | { |
MarijkeZondag | 26:ac5656aa35c7 | 438 | v_y = 1.0; //beweging in +y direction |
MarijkeZondag | 26:ac5656aa35c7 | 439 | ledr = 1; //green |
Marle | 25:bbef09ff226b | 440 | ledb = 1; |
Marle | 25:bbef09ff226b | 441 | ledg = 0; |
MarijkeZondag | 24:6d63ad38fef7 | 442 | } |
Marle | 25:bbef09ff226b | 443 | |
MarijkeZondag | 26:ac5656aa35c7 | 444 | else if(movAg0>Threshold0) //If the filtered EMG signal of muscle 0 is higher than the threshold, motor1 will turn in 1 direction |
MarijkeZondag | 24:6d63ad38fef7 | 445 | { |
Marle | 25:bbef09ff226b | 446 | v_x = -v_x; |
Marle | 25:bbef09ff226b | 447 | v_y = -v_y; |
MarijkeZondag | 26:ac5656aa35c7 | 448 | ledr = 1; //Blue |
Marle | 25:bbef09ff226b | 449 | ledb = 0; |
Marle | 25:bbef09ff226b | 450 | ledg = 1; |
Marle | 25:bbef09ff226b | 451 | } |
MarijkeZondag | 26:ac5656aa35c7 | 452 | else //If not higher than the threshold, motors will not turn at all |
Marle | 25:bbef09ff226b | 453 | { |
Marle | 25:bbef09ff226b | 454 | v_x = 0; |
Marle | 25:bbef09ff226b | 455 | v_y = 0; |
MarijkeZondag | 26:ac5656aa35c7 | 456 | ledr = 0; //white |
MarijkeZondag | 24:6d63ad38fef7 | 457 | ledb = 0; |
MarijkeZondag | 24:6d63ad38fef7 | 458 | ledg = 0; |
MarijkeZondag | 24:6d63ad38fef7 | 459 | } |
Marle | 25:bbef09ff226b | 460 | |
MarijkeZondag | 23:97a976a8f0fc | 461 | break; |
MarijkeZondag | 23:97a976a8f0fc | 462 | } |
MarijkeZondag | 26:ac5656aa35c7 | 463 | |
MarijkeZondag | 26:ac5656aa35c7 | 464 | inverse_kinematics(); //Call inverse kinematics function |
MarijkeZondag | 26:ac5656aa35c7 | 465 | } |
MarijkeZondag | 26:ac5656aa35c7 | 466 | |
MarijkeZondag | 26:ac5656aa35c7 | 467 | //---------PID controller motor 1 + start motor 1 -----------------------------------------------------------// |
MarijkeZondag | 26:ac5656aa35c7 | 468 | double PID_controller1(double err1) |
MarijkeZondag | 26:ac5656aa35c7 | 469 | { |
MarijkeZondag | 26:ac5656aa35c7 | 470 | static double err_integral1 = 0; |
MarijkeZondag | 26:ac5656aa35c7 | 471 | static double err_prev1 = err1; // initialization with this value only done once! |
MarijkeZondag | 26:ac5656aa35c7 | 472 | |
MarijkeZondag | 26:ac5656aa35c7 | 473 | static BiQuad LowPassFilter1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); |
MarijkeZondag | 26:ac5656aa35c7 | 474 | |
MarijkeZondag | 26:ac5656aa35c7 | 475 | // Proportional part: |
MarijkeZondag | 26:ac5656aa35c7 | 476 | double u_k1 = Kp1 * err1; |
MarijkeZondag | 26:ac5656aa35c7 | 477 | |
MarijkeZondag | 26:ac5656aa35c7 | 478 | // Integral part |
MarijkeZondag | 26:ac5656aa35c7 | 479 | err_integral1 = err_integral1 + err1 * T; |
MarijkeZondag | 26:ac5656aa35c7 | 480 | double u_i1 = Ki1 * err_integral1; |
MarijkeZondag | 26:ac5656aa35c7 | 481 | |
MarijkeZondag | 26:ac5656aa35c7 | 482 | // Derivative part |
MarijkeZondag | 26:ac5656aa35c7 | 483 | double err_derivative1 = (err1 - err_prev1)/T; |
MarijkeZondag | 26:ac5656aa35c7 | 484 | double filtered_err_derivative1 = LowPassFilter1.step(err_derivative1); |
MarijkeZondag | 26:ac5656aa35c7 | 485 | double u_d1 = Kd1 * filtered_err_derivative1; |
MarijkeZondag | 26:ac5656aa35c7 | 486 | err_prev1 = err1; |
MarijkeZondag | 26:ac5656aa35c7 | 487 | |
MarijkeZondag | 26:ac5656aa35c7 | 488 | // Sum all parts and return it |
MarijkeZondag | 26:ac5656aa35c7 | 489 | return u_k1 + u_i1 + u_d1; |
MarijkeZondag | 26:ac5656aa35c7 | 490 | } |
MarijkeZondag | 26:ac5656aa35c7 | 491 | |
MarijkeZondag | 26:ac5656aa35c7 | 492 | void start_your_engines1(double u1) |
MarijkeZondag | 26:ac5656aa35c7 | 493 | { |
MarijkeZondag | 26:ac5656aa35c7 | 494 | if(encoder1<5250 && encoder1>-5250) //limits rotation, in counts |
MarijkeZondag | 26:ac5656aa35c7 | 495 | { |
MarijkeZondag | 26:ac5656aa35c7 | 496 | pwmpin1 = fabs(u1); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!! |
MarijkeZondag | 26:ac5656aa35c7 | 497 | directionpin1.write(u1 < 0.0f); |
MarijkeZondag | 26:ac5656aa35c7 | 498 | } |
MarijkeZondag | 26:ac5656aa35c7 | 499 | else |
MarijkeZondag | 26:ac5656aa35c7 | 500 | { |
MarijkeZondag | 26:ac5656aa35c7 | 501 | pwmpin1 = 0; |
MarijkeZondag | 26:ac5656aa35c7 | 502 | } |
MarijkeZondag | 26:ac5656aa35c7 | 503 | } |
MarijkeZondag | 26:ac5656aa35c7 | 504 | |
MarijkeZondag | 26:ac5656aa35c7 | 505 | void engine_control1() //Engine 1 is rotational engine, connected with left side pins |
MarijkeZondag | 26:ac5656aa35c7 | 506 | { |
MarijkeZondag | 26:ac5656aa35c7 | 507 | encoder_radians1 = encoder1*(2*PI)/8400; |
MarijkeZondag | 26:ac5656aa35c7 | 508 | double err1 = q1ref - encoder_radians1; |
MarijkeZondag | 26:ac5656aa35c7 | 509 | double u1 = PID_controller1(err1); //PID controller function call |
MarijkeZondag | 26:ac5656aa35c7 | 510 | start_your_engines1(u1); //Call start_your_engines function |
MarijkeZondag | 23:97a976a8f0fc | 511 | } |
MarijkeZondag | 23:97a976a8f0fc | 512 | |
MarijkeZondag | 23:97a976a8f0fc | 513 | |
MarijkeZondag | 26:ac5656aa35c7 | 514 | |
MarijkeZondag | 26:ac5656aa35c7 | 515 | //---------PID controller motor 1 + start motor 1 -----------------------------------------------------------// |
MarijkeZondag | 26:ac5656aa35c7 | 516 | double PID_controller2(double err2) |
MarijkeZondag | 26:ac5656aa35c7 | 517 | { |
MarijkeZondag | 26:ac5656aa35c7 | 518 | static double err_integral2 = 0; |
MarijkeZondag | 26:ac5656aa35c7 | 519 | static double err_prev2 = err2; // initialization with this value only done once! |
MarijkeZondag | 26:ac5656aa35c7 | 520 | |
MarijkeZondag | 26:ac5656aa35c7 | 521 | static BiQuad LowPassFilter2(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); |
MarijkeZondag | 26:ac5656aa35c7 | 522 | |
MarijkeZondag | 26:ac5656aa35c7 | 523 | // Proportional part: |
MarijkeZondag | 26:ac5656aa35c7 | 524 | double u_k2 = Kp2 * err2; |
MarijkeZondag | 26:ac5656aa35c7 | 525 | |
MarijkeZondag | 26:ac5656aa35c7 | 526 | // Integral part |
MarijkeZondag | 26:ac5656aa35c7 | 527 | err_integral2 = err_integral2 + err2 * T; |
MarijkeZondag | 26:ac5656aa35c7 | 528 | double u_i2 = Ki2 * err_integral2; |
MarijkeZondag | 26:ac5656aa35c7 | 529 | |
MarijkeZondag | 26:ac5656aa35c7 | 530 | // Derivative part |
MarijkeZondag | 26:ac5656aa35c7 | 531 | double err_derivative2 = (err2 - err_prev2)/T; |
MarijkeZondag | 26:ac5656aa35c7 | 532 | double filtered_err_derivative2 = LowPassFilter2.step(err_derivative2); |
MarijkeZondag | 26:ac5656aa35c7 | 533 | double u_d2 = Kd2 * filtered_err_derivative2; |
MarijkeZondag | 26:ac5656aa35c7 | 534 | err_prev2 = err2; |
MarijkeZondag | 26:ac5656aa35c7 | 535 | |
MarijkeZondag | 26:ac5656aa35c7 | 536 | // Sum all parts and return it |
MarijkeZondag | 26:ac5656aa35c7 | 537 | return u_k2 + u_i2 + u_d2; |
MarijkeZondag | 26:ac5656aa35c7 | 538 | } |
MarijkeZondag | 26:ac5656aa35c7 | 539 | |
MarijkeZondag | 26:ac5656aa35c7 | 540 | void start_your_engines2(double u2) |
MarijkeZondag | 26:ac5656aa35c7 | 541 | { |
MarijkeZondag | 26:ac5656aa35c7 | 542 | if(encoder2<12600 && encoder2>-1) //limits translation in counts |
MarijkeZondag | 26:ac5656aa35c7 | 543 | { |
MarijkeZondag | 26:ac5656aa35c7 | 544 | pwmpin2 = fabs(u2); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!! |
MarijkeZondag | 26:ac5656aa35c7 | 545 | directionpin2.write(u2 < 0.0f); |
MarijkeZondag | 26:ac5656aa35c7 | 546 | } |
MarijkeZondag | 26:ac5656aa35c7 | 547 | else |
MarijkeZondag | 26:ac5656aa35c7 | 548 | { |
MarijkeZondag | 26:ac5656aa35c7 | 549 | pwmpin2 = 0; |
MarijkeZondag | 26:ac5656aa35c7 | 550 | } |
MarijkeZondag | 26:ac5656aa35c7 | 551 | } |
MarijkeZondag | 26:ac5656aa35c7 | 552 | |
MarijkeZondag | 26:ac5656aa35c7 | 553 | void engine_control2() //Engine 2 is translational engine, connected with right side wires |
MarijkeZondag | 26:ac5656aa35c7 | 554 | { |
MarijkeZondag | 26:ac5656aa35c7 | 555 | encoder_radians2 = encoder2*(2*PI)/8400; |
MarijkeZondag | 26:ac5656aa35c7 | 556 | double err2 = q2ref - encoder_radians2; |
MarijkeZondag | 26:ac5656aa35c7 | 557 | double u2 = PID_controller2(err2); //PID controller function call |
MarijkeZondag | 26:ac5656aa35c7 | 558 | start_your_engines2(u2); //Call start_your_engines function |
MarijkeZondag | 26:ac5656aa35c7 | 559 | } |
MarijkeZondag | 26:ac5656aa35c7 | 560 | |
MarijkeZondag | 26:ac5656aa35c7 | 561 | //------------------ Start main function --------------------------// |
MarijkeZondag | 26:ac5656aa35c7 | 562 | |
MarijkeZondag | 26:ac5656aa35c7 | 563 | |
MarijkeZondag | 23:97a976a8f0fc | 564 | int main() |
MarijkeZondag | 23:97a976a8f0fc | 565 | { |
MarijkeZondag | 23:97a976a8f0fc | 566 | pc.baud(115200); |
MarijkeZondag | 26:ac5656aa35c7 | 567 | pc.printf("Hello World!\r\n"); //Serial communication only works if hidscope is turned off. |
MarijkeZondag | 26:ac5656aa35c7 | 568 | pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz |
MarijkeZondag | 23:97a976a8f0fc | 569 | |
MarijkeZondag | 23:97a976a8f0fc | 570 | emg0filter.add( &emg0band1 ).add( &emg0band2 ).add( &emg0band3 ).add( ¬ch1 ); //attach biquad elements to chain |
MarijkeZondag | 23:97a976a8f0fc | 571 | emg1filter.add( &emg1band1 ).add( &emg1band2 ).add( &emg1band3 ).add( ¬ch2 ); |
MarijkeZondag | 23:97a976a8f0fc | 572 | emg2filter.add( &emg2band1 ).add( &emg2band2 ).add( &emg2band3 ).add( ¬ch3 ); |
MarijkeZondag | 10:39ec51206c8b | 573 | |
MarijkeZondag | 23:97a976a8f0fc | 574 | while(true) |
MarijkeZondag | 23:97a976a8f0fc | 575 | { |
MarijkeZondag | 26:ac5656aa35c7 | 576 | ticker.attach(&emg_filtered,T); //EMG signals filtered every T sec. |
MarijkeZondag | 26:ac5656aa35c7 | 577 | ticker.attach(&MovAg,T); //Moving average calculation every T sec. |
MarijkeZondag | 26:ac5656aa35c7 | 578 | ticker.attach(&v_des_calculate_qref,T); //v_des determined every T |
Marle | 25:bbef09ff226b | 579 | |
MarijkeZondag | 26:ac5656aa35c7 | 580 | // HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec. |
MarijkeZondag | 23:97a976a8f0fc | 581 | |
MarijkeZondag | 26:ac5656aa35c7 | 582 | button1.rise(switch_to_calibrate); //Switch state of calibration (which muscle) |
MarijkeZondag | 26:ac5656aa35c7 | 583 | wait(0.2f); //Wait to avoid bouncing of button |
MarijkeZondag | 26:ac5656aa35c7 | 584 | button2.rise(calibrate); //Calibrate threshold for 3 muscles |
MarijkeZondag | 26:ac5656aa35c7 | 585 | wait(0.2f); //Wait to avoid bouncing of button |
MarijkeZondag | 23:97a976a8f0fc | 586 | |
MarijkeZondag | 26:ac5656aa35c7 | 587 | pc.printf("x is %i\n\r",x); |
MarijkeZondag | 26:ac5656aa35c7 | 588 | pc.printf("Movag0 = %f , Movag1 = %f, Movag2 = %f \n\r",movAg0, movAg1, movAg2); |
MarijkeZondag | 26:ac5656aa35c7 | 589 | pc.printf("Thresh0 = %f , Thresh1 = %f, Thresh2 = %f \n\r",Threshold0, Threshold1, Threshold2); |
MarijkeZondag | 26:ac5656aa35c7 | 590 | //wait(2.0f); |
MarijkeZondag | 23:97a976a8f0fc | 591 | } |
vsluiter | 0:c8f15874531b | 592 | } |