![](/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@35:63c890ac71ff, 2018-11-05 (annotated)
- Committer:
- MarijkeZondag
- Date:
- Mon Nov 05 16:45:17 2018 +0000
- Revision:
- 35:63c890ac71ff
- Parent:
- 34:b8b18ba0c336
- Child:
- 36:650a9245bc44
Code mooi gemaakt + comments
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> |
MarijkeZondag | 28:5e54cd4525de | 6 | #include "QEI.h" |
vsluiter | 0:c8f15874531b | 7 | |
MarijkeZondag | 22:5d956c93b3ae | 8 | //ATTENTION: set mBed to version 151 |
MarijkeZondag | 22:5d956c93b3ae | 9 | // set QEI to version 0, (gebruiken wij (nog) niet, is voor encoder) |
MarijkeZondag | 22:5d956c93b3ae | 10 | // set MODSERIAL to version 44 |
MarijkeZondag | 22:5d956c93b3ae | 11 | // set HIDScope to version 7 |
MarijkeZondag | 22:5d956c93b3ae | 12 | // set biquadFilter to version 7 |
MarijkeZondag | 10:39ec51206c8b | 13 | |
MarijkeZondag | 22:5d956c93b3ae | 14 | AnalogIn emg0_in (A0); //First raw EMG signal input |
MarijkeZondag | 22:5d956c93b3ae | 15 | AnalogIn emg1_in (A1); //Second raw EMG signal input |
MarijkeZondag | 22:5d956c93b3ae | 16 | AnalogIn emg2_in (A2); //Third raw EMG signal input |
MarijkeZondag | 22:5d956c93b3ae | 17 | |
MarijkeZondag | 26:ac5656aa35c7 | 18 | InterruptIn button1 (D10); |
MarijkeZondag | 16:5f7196ddc77b | 19 | InterruptIn button2 (D11); |
MarijkeZondag | 6:f4bbb73f3989 | 20 | |
MarijkeZondag | 28:5e54cd4525de | 21 | DigitalOut directionpin1 (D4); |
MarijkeZondag | 28:5e54cd4525de | 22 | DigitalOut directionpin2 (D7); |
MarijkeZondag | 26:ac5656aa35c7 | 23 | |
MarijkeZondag | 28:5e54cd4525de | 24 | PwmOut pwmpin1 (D5); |
MarijkeZondag | 28:5e54cd4525de | 25 | PwmOut pwmpin2 (D6); |
MarijkeZondag | 22:5d956c93b3ae | 26 | |
MarijkeZondag | 17:741798018c0d | 27 | DigitalOut ledr (LED_RED); |
MarijkeZondag | 17:741798018c0d | 28 | DigitalOut ledb (LED_BLUE); |
MarijkeZondag | 17:741798018c0d | 29 | DigitalOut ledg (LED_GREEN); |
MarijkeZondag | 13:a3d4b4daf5b4 | 30 | |
MarijkeZondag | 9:c722418997b5 | 31 | |
MarijkeZondag | 35:63c890ac71ff | 32 | MODSERIAL pc(USBTX, USBRX); //Serial communication to see if the code works step by step, turn on if hidscope is off |
MarijkeZondag | 28:5e54cd4525de | 33 | QEI encoder2 (D9, D8, NC, 8400,QEI::X4_ENCODING); |
MarijkeZondag | 28:5e54cd4525de | 34 | QEI encoder1 (D12, D13, NC, 8400,QEI::X4_ENCODING); |
MarijkeZondag | 6:f4bbb73f3989 | 35 | |
MarijkeZondag | 35:63c890ac71ff | 36 | //HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered |
vsluiter | 0:c8f15874531b | 37 | |
MarijkeZondag | 22:5d956c93b3ae | 38 | //Tickers |
MarijkeZondag | 28:5e54cd4525de | 39 | Ticker func_tick; |
MarijkeZondag | 28:5e54cd4525de | 40 | Ticker movag_tick; |
MarijkeZondag | 30:f04a35f2a06d | 41 | Ticker emg_tick; |
MarijkeZondag | 30:f04a35f2a06d | 42 | Ticker print_tick; |
MarijkeZondag | 30:f04a35f2a06d | 43 | |
MarijkeZondag | 9:c722418997b5 | 44 | |
MarijkeZondag | 9:c722418997b5 | 45 | //Global variables |
MarijkeZondag | 35:63c890ac71ff | 46 | const float T = 0.002f; //Ticker period EMG, engine control |
MarijkeZondag | 35:63c890ac71ff | 47 | const float T2 = 0.2f; //Ticker print function |
MarijkeZondag | 10:39ec51206c8b | 48 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 49 | //EMG filter |
MarijkeZondag | 22:5d956c93b3ae | 50 | double emg0_filt, emg1_filt, emg2_filt; //Variables for filtered EMG data channel 0, 1 and 2 |
MarijkeZondag | 22:5d956c93b3ae | 51 | double emg0_raw, emg1_raw, emg2_raw; |
MarijkeZondag | 22:5d956c93b3ae | 52 | double emg0_filt_x, emg1_filt_x, emg2_filt_x; |
MarijkeZondag | 35:63c890ac71ff | 53 | const int windowsize = 150; //Size of the array over which the moving average (MovAg) is calculated |
MarijkeZondag | 13:a3d4b4daf5b4 | 54 | double sum, sum1, sum2, sum3; //variables used to sum elements in array |
MarijkeZondag | 22:5d956c93b3ae | 55 | double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize]; //Empty arrays to calculate MoveAg |
MarijkeZondag | 35:63c890ac71ff | 56 | double movAg0, movAg1, movAg2; //Outcome of MovAg |
MarijkeZondag | 13:a3d4b4daf5b4 | 57 | |
MarijkeZondag | 22:5d956c93b3ae | 58 | //Calibration variables |
MarijkeZondag | 22:5d956c93b3ae | 59 | int x = -1; //Start switch, colour LED is blue. |
MarijkeZondag | 35:63c890ac71ff | 60 | int emg_cal = 0; //If emg_cal is set to 1, motors can begin to work in this code |
MarijkeZondag | 35:63c890ac71ff | 61 | const int sizeCal = 1500; //Size of the dataset used for calibration |
MarijkeZondag | 35:63c890ac71ff | 62 | double StoreCal0[sizeCal], StoreCal1[sizeCal], StoreCal2[sizeCal]; //Arrays to put the dataset of the calibration in |
MarijkeZondag | 35:63c890ac71ff | 63 | double Mean0,Mean1,Mean2; //Average of maximum contraction |
MarijkeZondag | 22:5d956c93b3ae | 64 | double Threshold0, Threshold1, Threshold2; |
MarijkeZondag | 13:a3d4b4daf5b4 | 65 | |
MarijkeZondag | 35:63c890ac71ff | 66 | //Biquad //Variables for the biquad band filters |
MarijkeZondag | 22:5d956c93b3ae | 67 | BiQuadChain emg0filter; |
MarijkeZondag | 10:39ec51206c8b | 68 | BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 69 | BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 70 | BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); |
MarijkeZondag | 35:63c890ac71ff | 71 | BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); //Notch filter biquad coefficients |
MarijkeZondag | 10:39ec51206c8b | 72 | |
MarijkeZondag | 22:5d956c93b3ae | 73 | BiQuadChain emg1filter; |
MarijkeZondag | 10:39ec51206c8b | 74 | BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 75 | BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 76 | BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); |
MarijkeZondag | 35:63c890ac71ff | 77 | BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 78 | |
MarijkeZondag | 22:5d956c93b3ae | 79 | BiQuadChain emg2filter; |
MarijkeZondag | 10:39ec51206c8b | 80 | BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 81 | BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 82 | BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 ); |
MarijkeZondag | 35:63c890ac71ff | 83 | BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); |
MarijkeZondag | 10:39ec51206c8b | 84 | |
Marle | 25:bbef09ff226b | 85 | |
MarijkeZondag | 26:ac5656aa35c7 | 86 | //Variables PID controller |
MarijkeZondag | 35:63c890ac71ff | 87 | double PI = 3.14159; //Pi value |
Marle | 25:bbef09ff226b | 88 | |
MarijkeZondag | 35:63c890ac71ff | 89 | double Kp1 = 20.0; //Proportional gain motor 1 |
MarijkeZondag | 35:63c890ac71ff | 90 | double Ki1 = 1.02; //Integrative term motor 1 |
MarijkeZondag | 35:63c890ac71ff | 91 | double Kd1 = 1.0; //Differential term motor 1 |
MarijkeZondag | 35:63c890ac71ff | 92 | double encoder_radians1=0; //Inital encoder value motor 1 |
MarijkeZondag | 35:63c890ac71ff | 93 | double err_integral1 = 0; //Initial error integral value motor 1 |
MarijkeZondag | 35:63c890ac71ff | 94 | double err_prev1, err_prev2; //Variables called previous error motor 1 and motor 2 |
MarijkeZondag | 35:63c890ac71ff | 95 | double err1, err2; //Variables called error motor 1 and motor 2 |
MarijkeZondag | 35:63c890ac71ff | 96 | BiQuad LowPassFilterDer1( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 ); //Low_pass differential term Sample frequency 500 Hz, cutoff 20Hz low pass |
MarijkeZondag | 35:63c890ac71ff | 97 | |
MarijkeZondag | 35:63c890ac71ff | 98 | double Kp2 = 20.0; / //Motor 2 |
MarijkeZondag | 26:ac5656aa35c7 | 99 | double Ki2 = 1.02; |
MarijkeZondag | 28:5e54cd4525de | 100 | double Kd2 = 1.0; |
MarijkeZondag | 26:ac5656aa35c7 | 101 | double encoder_radians2=0; |
MarijkeZondag | 28:5e54cd4525de | 102 | double err_integral2 = 0; |
MarijkeZondag | 28:5e54cd4525de | 103 | double u1, u2; |
MarijkeZondag | 30:f04a35f2a06d | 104 | BiQuad LowPassFilterDer2( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 ); |
MarijkeZondag | 26:ac5656aa35c7 | 105 | |
MarijkeZondag | 32:56a8bd82e971 | 106 | // Inverse Kinematica variables |
MarijkeZondag | 35:63c890ac71ff | 107 | //const double L1 = 0.208; // Height table to joint 1 |
MarijkeZondag | 35:63c890ac71ff | 108 | //const double L2 = 0.288; // Height table to joint 2 |
MarijkeZondag | 35:63c890ac71ff | 109 | const double L3 = 0.212; // Length arm |
MarijkeZondag | 35:63c890ac71ff | 110 | //const double L4 = 0.089; // Distance backside base to joint 1 |
MarijkeZondag | 35:63c890ac71ff | 111 | //const double L5 = 0.030; // Distance from joint 1 to joint 2 |
MarijkeZondag | 35:63c890ac71ff | 112 | const double r_trans = 0.035; // Calculate translation to shaft rotation |
MarijkeZondag | 32:56a8bd82e971 | 113 | |
MarijkeZondag | 32:56a8bd82e971 | 114 | // Variërende variabelen inverse kinematics: |
MarijkeZondag | 35:63c890ac71ff | 115 | double q1ref = 0.0; // Current motor angle of joint 1 as determined out of reference signal |
MarijkeZondag | 35:63c890ac71ff | 116 | double q2ref = 0.0; // Current motor angle of joint 2 as determined out of reference signal |
MarijkeZondag | 35:63c890ac71ff | 117 | double v_x; // Desired velocity of end effector in x direction --> Determined by EMG signals |
MarijkeZondag | 35:63c890ac71ff | 118 | double v_y; // Desired velocity of end effector in y direction --> Determined by EMG signals |
MarijkeZondag | 32:56a8bd82e971 | 119 | |
MarijkeZondag | 35:63c890ac71ff | 120 | //double Lq1; // Translational distance due to motor rotation joint 1 |
MarijkeZondag | 35:63c890ac71ff | 121 | //double Cq2; // Joint angle of the system (corrected for gear ratio 1:5) |
MarijkeZondag | 32:56a8bd82e971 | 122 | |
MarijkeZondag | 35:63c890ac71ff | 123 | double q1_dot=0.0; // Required angular velocity of motor 1 to reach v_des |
MarijkeZondag | 35:63c890ac71ff | 124 | double q2_dot=0.0; // Required angular velocity of motor 2 to reach v_des |
MarijkeZondag | 32:56a8bd82e971 | 125 | |
MarijkeZondag | 35:63c890ac71ff | 126 | double q1_ii=0.0; // Reference signal for motorangle q1ref |
MarijkeZondag | 35:63c890ac71ff | 127 | double q2_ii=0.0; // Reference signal for motorangle q2ref |
MarijkeZondag | 32:56a8bd82e971 | 128 | |
MarijkeZondag | 32:56a8bd82e971 | 129 | double q1_motor; |
MarijkeZondag | 32:56a8bd82e971 | 130 | double q2_motor; |
MarijkeZondag | 28:5e54cd4525de | 131 | |
MarijkeZondag | 26:ac5656aa35c7 | 132 | //--------------Functions----------------------------------------------------------------------------------------------------------------------------// |
MarijkeZondag | 26:ac5656aa35c7 | 133 | |
MarijkeZondag | 26:ac5656aa35c7 | 134 | |
MarijkeZondag | 35:63c890ac71ff | 135 | //------------------ Filter EMG + Calibration EMG --------------------------------------------------------------------------------------------------// |
MarijkeZondag | 26:ac5656aa35c7 | 136 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 137 | void switch_to_calibrate() |
MarijkeZondag | 13:a3d4b4daf5b4 | 138 | { |
MarijkeZondag | 35:63c890ac71ff | 139 | //Every time function gets called, x increases. Every button press --> new calibration state. |
MarijkeZondag | 35:63c890ac71ff | 140 | //Starts with x = -1. So when function gets called 1 time, x = 0. In the end, x = 4 will reset to -1. |
MarijkeZondag | 35:63c890ac71ff | 141 | |
MarijkeZondag | 35:63c890ac71ff | 142 | x++; |
MarijkeZondag | 35:63c890ac71ff | 143 | |
MarijkeZondag | 22:5d956c93b3ae | 144 | |
MarijkeZondag | 35:63c890ac71ff | 145 | if(x==0) //If x = 0, led is red |
MarijkeZondag | 13:a3d4b4daf5b4 | 146 | { |
MarijkeZondag | 17:741798018c0d | 147 | ledr = 0; |
MarijkeZondag | 17:741798018c0d | 148 | ledb = 1; |
MarijkeZondag | 17:741798018c0d | 149 | ledg = 1; |
MarijkeZondag | 13:a3d4b4daf5b4 | 150 | } |
MarijkeZondag | 35:63c890ac71ff | 151 | else if (x==1) //If x = 1, led is blue |
MarijkeZondag | 13:a3d4b4daf5b4 | 152 | { |
MarijkeZondag | 17:741798018c0d | 153 | ledr = 1; |
MarijkeZondag | 17:741798018c0d | 154 | ledb = 0; |
MarijkeZondag | 17:741798018c0d | 155 | ledg = 1; |
MarijkeZondag | 13:a3d4b4daf5b4 | 156 | } |
MarijkeZondag | 35:63c890ac71ff | 157 | else if (x==2) //If x = 2, led is green |
MarijkeZondag | 13:a3d4b4daf5b4 | 158 | { |
MarijkeZondag | 17:741798018c0d | 159 | ledr = 1; |
MarijkeZondag | 17:741798018c0d | 160 | ledb = 1; |
MarijkeZondag | 17:741798018c0d | 161 | ledg = 0; |
MarijkeZondag | 13:a3d4b4daf5b4 | 162 | } |
MarijkeZondag | 35:63c890ac71ff | 163 | else //If x = 3 or 4, led is white |
MarijkeZondag | 13:a3d4b4daf5b4 | 164 | { |
MarijkeZondag | 17:741798018c0d | 165 | ledr = 0; |
MarijkeZondag | 17:741798018c0d | 166 | ledb = 0; |
MarijkeZondag | 17:741798018c0d | 167 | ledg = 0; |
MarijkeZondag | 13:a3d4b4daf5b4 | 168 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 169 | |
MarijkeZondag | 35:63c890ac71ff | 170 | if(x==4) //Reset back to x = -1 |
MarijkeZondag | 13:a3d4b4daf5b4 | 171 | { |
MarijkeZondag | 16:5f7196ddc77b | 172 | x = -1; |
MarijkeZondag | 35:63c890ac71ff | 173 | emg_cal=0; //Reset, motors off |
MarijkeZondag | 13:a3d4b4daf5b4 | 174 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 175 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 176 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 177 | |
MarijkeZondag | 13:a3d4b4daf5b4 | 178 | void calibrate(void) |
MarijkeZondag | 12:eaed305a76c3 | 179 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 180 | switch(x) |
MarijkeZondag | 13:a3d4b4daf5b4 | 181 | { |
MarijkeZondag | 35:63c890ac71ff | 182 | case 0: //If calibration state 0: |
MarijkeZondag | 13:a3d4b4daf5b4 | 183 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 184 | sum = 0.0; |
MarijkeZondag | 35:63c890ac71ff | 185 | for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 0 |
MarijkeZondag | 13:a3d4b4daf5b4 | 186 | { |
MarijkeZondag | 22:5d956c93b3ae | 187 | StoreCal0[j] = emg0_filt; |
MarijkeZondag | 13:a3d4b4daf5b4 | 188 | sum+=StoreCal0[j]; |
MarijkeZondag | 35:63c890ac71ff | 189 | wait(0.001f); |
MarijkeZondag | 13:a3d4b4daf5b4 | 190 | } |
MarijkeZondag | 35:63c890ac71ff | 191 | Mean0 = sum/sizeCal; //Calculate mean of the datapoints in the calibration set |
MarijkeZondag | 35:63c890ac71ff | 192 | Threshold0 = Mean0*0.5; //Threshold calculation calve = 0.8*mean |
MarijkeZondag | 35:63c890ac71ff | 193 | break; //Stop. Threshold is calculated. |
MarijkeZondag | 13:a3d4b4daf5b4 | 194 | } |
MarijkeZondag | 35:63c890ac71ff | 195 | case 1: //If calibration state 1: |
MarijkeZondag | 13:a3d4b4daf5b4 | 196 | { |
MarijkeZondag | 22:5d956c93b3ae | 197 | sum = 0.0; |
MarijkeZondag | 35:63c890ac71ff | 198 | for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 1 |
MarijkeZondag | 13:a3d4b4daf5b4 | 199 | { |
MarijkeZondag | 22:5d956c93b3ae | 200 | StoreCal1[j] = emg1_filt; |
MarijkeZondag | 13:a3d4b4daf5b4 | 201 | sum+=StoreCal1[j]; |
MarijkeZondag | 21:1da43fdbd254 | 202 | wait(0.001f); |
MarijkeZondag | 13:a3d4b4daf5b4 | 203 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 204 | Mean1 = sum/sizeCal; |
MarijkeZondag | 23:97a976a8f0fc | 205 | Threshold1 = Mean1/2; |
MarijkeZondag | 13:a3d4b4daf5b4 | 206 | break; |
MarijkeZondag | 35:63c890ac71ff | 207 | } |
MarijkeZondag | 35:63c890ac71ff | 208 | case 2: //If calibration state 2: |
MarijkeZondag | 13:a3d4b4daf5b4 | 209 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 210 | sum = 0.0; |
MarijkeZondag | 35:63c890ac71ff | 211 | for(int j = 0; j<=sizeCal-1; j++) //Array filled with datapoints from the EMGfilter signal of muscle 2 |
MarijkeZondag | 13:a3d4b4daf5b4 | 212 | { |
MarijkeZondag | 23:97a976a8f0fc | 213 | StoreCal2[j] = emg2_filt; |
MarijkeZondag | 13:a3d4b4daf5b4 | 214 | sum+=StoreCal2[j]; |
MarijkeZondag | 21:1da43fdbd254 | 215 | wait(0.001f); |
MarijkeZondag | 13:a3d4b4daf5b4 | 216 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 217 | Mean2 = sum/sizeCal; |
MarijkeZondag | 23:97a976a8f0fc | 218 | Threshold2 = Mean2/2; |
MarijkeZondag | 13:a3d4b4daf5b4 | 219 | break; |
MarijkeZondag | 13:a3d4b4daf5b4 | 220 | } |
MarijkeZondag | 35:63c890ac71ff | 221 | case 3: //EMG is calibrated, robot can be set to Home position. |
MarijkeZondag | 13:a3d4b4daf5b4 | 222 | { |
MarijkeZondag | 35:63c890ac71ff | 223 | emg_cal = 1; //This is the setting for which the motors can begin turning in this code |
MarijkeZondag | 27:fa493551be99 | 224 | |
MarijkeZondag | 21:1da43fdbd254 | 225 | wait(0.001f); |
MarijkeZondag | 13:a3d4b4daf5b4 | 226 | break; |
MarijkeZondag | 13:a3d4b4daf5b4 | 227 | } |
MarijkeZondag | 35:63c890ac71ff | 228 | default: //Ensures nothing happens if x is not 0,1 or 2 |
MarijkeZondag | 13:a3d4b4daf5b4 | 229 | { |
MarijkeZondag | 13:a3d4b4daf5b4 | 230 | break; |
MarijkeZondag | 13:a3d4b4daf5b4 | 231 | } |
MarijkeZondag | 13:a3d4b4daf5b4 | 232 | } |
MarijkeZondag | 12:eaed305a76c3 | 233 | } |
MarijkeZondag | 28:5e54cd4525de | 234 | |
MarijkeZondag | 28:5e54cd4525de | 235 | void EMGFilter0() |
MarijkeZondag | 28:5e54cd4525de | 236 | { |
MarijkeZondag | 35:63c890ac71ff | 237 | emg0_raw = emg0_in.read(); //Give name to raw EMG0 data |
MarijkeZondag | 35:63c890ac71ff | 238 | emg0_filt_x = emg0filter.step(emg0_raw); //Use biquad chain to filter raw EMG data |
MarijkeZondag | 35:63c890ac71ff | 239 | emg0_filt = abs(emg0_filt_x); //Rectifier |
MarijkeZondag | 28:5e54cd4525de | 240 | } |
MarijkeZondag | 28:5e54cd4525de | 241 | |
MarijkeZondag | 28:5e54cd4525de | 242 | void EMGFilter1() |
MarijkeZondag | 28:5e54cd4525de | 243 | { |
MarijkeZondag | 35:63c890ac71ff | 244 | emg1_raw = emg1_in.read(); //Give name to raw EMG1 data |
MarijkeZondag | 35:63c890ac71ff | 245 | emg1_filt_x = emg1filter.step(emg1_raw); //Use biquad chain to filter raw EMG data |
MarijkeZondag | 35:63c890ac71ff | 246 | emg1_filt = abs(emg1_filt_x); //Rectifier |
MarijkeZondag | 28:5e54cd4525de | 247 | } |
MarijkeZondag | 28:5e54cd4525de | 248 | |
MarijkeZondag | 28:5e54cd4525de | 249 | void EMGFilter2() |
MarijkeZondag | 28:5e54cd4525de | 250 | { |
MarijkeZondag | 35:63c890ac71ff | 251 | emg2_raw = emg2_in.read(); //Give name to raw EMG1 data |
MarijkeZondag | 35:63c890ac71ff | 252 | emg2_filt_x = emg2filter.step(emg2_raw); //Use biquad chain to filter raw EMG data |
MarijkeZondag | 35:63c890ac71ff | 253 | emg2_filt = abs(emg2_filt_x); //Rectifier |
MarijkeZondag | 28:5e54cd4525de | 254 | } |
MarijkeZondag | 28:5e54cd4525de | 255 | |
MarijkeZondag | 35:63c890ac71ff | 256 | void MovAg() //Calculate moving average (MovAg) |
MarijkeZondag | 28:5e54cd4525de | 257 | { |
MarijkeZondag | 35:63c890ac71ff | 258 | for (int i = windowsize-1; i>=0; i--) //Make arrays for the last datapoints of the filtered signals |
MarijkeZondag | 28:5e54cd4525de | 259 | { |
MarijkeZondag | 35:63c890ac71ff | 260 | StoreArray0[i] = StoreArray0[i-1]; //Shifts the i'th element one place to the right, this makes it "rolling or moving" average. |
MarijkeZondag | 28:5e54cd4525de | 261 | StoreArray1[i] = StoreArray1[i-1]; |
MarijkeZondag | 28:5e54cd4525de | 262 | StoreArray2[i] = StoreArray2[i-1]; |
MarijkeZondag | 28:5e54cd4525de | 263 | } |
MarijkeZondag | 28:5e54cd4525de | 264 | |
MarijkeZondag | 35:63c890ac71ff | 265 | StoreArray0[0] = emg0_filt; //Stores the latest datapoint of the filtered signal in the first element of the array |
MarijkeZondag | 28:5e54cd4525de | 266 | StoreArray1[0] = emg1_filt; |
MarijkeZondag | 28:5e54cd4525de | 267 | StoreArray2[0] = emg2_filt; |
MarijkeZondag | 28:5e54cd4525de | 268 | |
MarijkeZondag | 28:5e54cd4525de | 269 | sum1 = 0.0; |
MarijkeZondag | 28:5e54cd4525de | 270 | sum2 = 0.0; |
MarijkeZondag | 28:5e54cd4525de | 271 | sum3 = 0.0; |
MarijkeZondag | 28:5e54cd4525de | 272 | |
MarijkeZondag | 35:63c890ac71ff | 273 | for(int a = 0; a<= windowsize-1; a++) //Sums the elements in the arrays |
MarijkeZondag | 28:5e54cd4525de | 274 | { |
MarijkeZondag | 28:5e54cd4525de | 275 | sum1 += StoreArray0[a]; |
MarijkeZondag | 28:5e54cd4525de | 276 | sum2 += StoreArray1[a]; |
MarijkeZondag | 28:5e54cd4525de | 277 | sum3 += StoreArray2[a]; |
MarijkeZondag | 28:5e54cd4525de | 278 | } |
MarijkeZondag | 28:5e54cd4525de | 279 | |
MarijkeZondag | 35:63c890ac71ff | 280 | movAg0 = sum1/windowsize; //calculates an average in the array |
MarijkeZondag | 28:5e54cd4525de | 281 | movAg1 = sum2/windowsize; |
MarijkeZondag | 28:5e54cd4525de | 282 | movAg2 = sum3/windowsize; |
MarijkeZondag | 28:5e54cd4525de | 283 | } |
MarijkeZondag | 28:5e54cd4525de | 284 | |
MarijkeZondag | 35:63c890ac71ff | 285 | void emg_filtered() //Call all filter functions |
MarijkeZondag | 28:5e54cd4525de | 286 | { |
MarijkeZondag | 28:5e54cd4525de | 287 | EMGFilter0(); |
MarijkeZondag | 28:5e54cd4525de | 288 | EMGFilter1(); |
MarijkeZondag | 28:5e54cd4525de | 289 | EMGFilter2(); |
MarijkeZondag | 28:5e54cd4525de | 290 | } |
MarijkeZondag | 28:5e54cd4525de | 291 | |
MarijkeZondag | 35:63c890ac71ff | 292 | //---------PID controller 1 + 2 + motor control 1 & 2-----------------------------------------------------------------------------------------------// |
MarijkeZondag | 28:5e54cd4525de | 293 | void PID_control1() |
MarijkeZondag | 28:5e54cd4525de | 294 | { |
MarijkeZondag | 28:5e54cd4525de | 295 | // Proportional part: |
MarijkeZondag | 35:63c890ac71ff | 296 | double u_k1 = Kp1 * err1; //Proportional gain times calculated error |
MarijkeZondag | 28:5e54cd4525de | 297 | |
MarijkeZondag | 28:5e54cd4525de | 298 | //Integral part |
MarijkeZondag | 35:63c890ac71ff | 299 | err_integral1 = err_integral1 + err1 * T; //Adds the error*T |
MarijkeZondag | 35:63c890ac71ff | 300 | double u_i1 = Ki1 * err_integral1; //Integral term times the integral |
MarijkeZondag | 28:5e54cd4525de | 301 | |
MarijkeZondag | 28:5e54cd4525de | 302 | // Derivative part |
MarijkeZondag | 35:63c890ac71ff | 303 | double err_derivative1 = (err1 - err_prev1)/T; //error - previous error /T |
MarijkeZondag | 35:63c890ac71ff | 304 | double filtered_err_derivative1 = LowPassFilterDer1.step(err_derivative1); //Filter the derivative term for stabilization |
MarijkeZondag | 35:63c890ac71ff | 305 | double u_d1 = Kd1 * filtered_err_derivative1; //Derivative term times the derivative error |
MarijkeZondag | 35:63c890ac71ff | 306 | err_prev1 = err1; //Sets the current error to previous error (remember) |
MarijkeZondag | 28:5e54cd4525de | 307 | |
MarijkeZondag | 35:63c890ac71ff | 308 | // Sum all parts and return it |
MarijkeZondag | 35:63c890ac71ff | 309 | u1 = u_k1 + u_i1 + u_d1; |
MarijkeZondag | 28:5e54cd4525de | 310 | } |
MarijkeZondag | 28:5e54cd4525de | 311 | |
MarijkeZondag | 28:5e54cd4525de | 312 | void PID_control2() |
MarijkeZondag | 28:5e54cd4525de | 313 | { |
MarijkeZondag | 28:5e54cd4525de | 314 | // Proportional part: |
MarijkeZondag | 28:5e54cd4525de | 315 | double u_k2 = Kp2 * err2; |
MarijkeZondag | 28:5e54cd4525de | 316 | |
MarijkeZondag | 28:5e54cd4525de | 317 | //Integral part |
MarijkeZondag | 28:5e54cd4525de | 318 | err_integral2 = err_integral2 + err2 * T; |
MarijkeZondag | 28:5e54cd4525de | 319 | double u_i2 = Ki2 * err_integral2; |
MarijkeZondag | 28:5e54cd4525de | 320 | |
MarijkeZondag | 28:5e54cd4525de | 321 | // Derivative part |
MarijkeZondag | 28:5e54cd4525de | 322 | double err_derivative2 = (err2 - err_prev2)/T; |
MarijkeZondag | 28:5e54cd4525de | 323 | double filtered_err_derivative2 = LowPassFilterDer2.step(err_derivative2); |
MarijkeZondag | 28:5e54cd4525de | 324 | double u_d2 = Kd2 * filtered_err_derivative2; |
MarijkeZondag | 28:5e54cd4525de | 325 | err_prev2 = err2; |
MarijkeZondag | 28:5e54cd4525de | 326 | |
MarijkeZondag | 35:63c890ac71ff | 327 | // Sum all parts and return it |
MarijkeZondag | 28:5e54cd4525de | 328 | u2 = u_k2 + u_i2 + u_d2; |
MarijkeZondag | 28:5e54cd4525de | 329 | } |
MarijkeZondag | 35:63c890ac71ff | 330 | void engine_control1() //Engine 1 is translational engine, connected with left side pins |
MarijkeZondag | 28:5e54cd4525de | 331 | { |
MarijkeZondag | 28:5e54cd4525de | 332 | encoder_radians1 = (double)encoder1.getPulses()*(2.0*PI)/8400.0; |
MarijkeZondag | 35:63c890ac71ff | 333 | err1 = q1_motor - encoder_radians1; //Calculate error between desired angle 1 and current angle 1 |
MarijkeZondag | 35:63c890ac71ff | 334 | PID_control1(); //PID 1 controller function call |
MarijkeZondag | 35:63c890ac71ff | 335 | pwmpin1 = fabs(u1); //Motor 1 speed set |
MarijkeZondag | 35:63c890ac71ff | 336 | directionpin1.write(u1<0); //Direction motor 1 set |
MarijkeZondag | 28:5e54cd4525de | 337 | } |
MarijkeZondag | 28:5e54cd4525de | 338 | |
MarijkeZondag | 35:63c890ac71ff | 339 | void engine_control2() //Engine 2 is rotational engine, connected with right side wires |
MarijkeZondag | 28:5e54cd4525de | 340 | { |
MarijkeZondag | 28:5e54cd4525de | 341 | encoder_radians2 = (float)encoder2.getPulses()*(2.0*PI)/8400.0; |
MarijkeZondag | 35:63c890ac71ff | 342 | err2 = q2_motor - encoder_radians2; //Calculate error between desired angle 2 and current angle 2 |
MarijkeZondag | 35:63c890ac71ff | 343 | PID_control2(); //PID 2 controller function call |
MarijkeZondag | 35:63c890ac71ff | 344 | pwmpin2 = fabs(u2); //Motor 2 speed set |
MarijkeZondag | 35:63c890ac71ff | 345 | directionpin2.write(u2>0); //Direction motor 2 set |
MarijkeZondag | 28:5e54cd4525de | 346 | } |
MarijkeZondag | 28:5e54cd4525de | 347 | |
Esmee | 33:976be2825a23 | 348 | |
MarijkeZondag | 35:63c890ac71ff | 349 | //------------------ Inversed Kinematics -----------------------------------------------------------------------------------------------------------// |
MarijkeZondag | 8:895d941a5910 | 350 | |
Marle | 25:bbef09ff226b | 351 | void inverse_kinematics() |
Esmee | 34:b8b18ba0c336 | 352 | { |
Marle | 25:bbef09ff226b | 353 | |
MarijkeZondag | 35:63c890ac71ff | 354 | q1_dot = (v_x*cos(q2ref) + v_y*sin(q2ref))/cos(q2ref); //Calculate desired angular velocity of motor 1 |
MarijkeZondag | 35:63c890ac71ff | 355 | q2_dot = v_y/(L3*cos(q2ref)); //Calculate desired angular velocity of motor 2 |
MarijkeZondag | 35:63c890ac71ff | 356 | |
MarijkeZondag | 35:63c890ac71ff | 357 | q1_ii = q1ref + q1_dot*T; //Adds the desired angle of motor 1 to the reference angle |
MarijkeZondag | 35:63c890ac71ff | 358 | q2_ii = q2ref + q2_dot*T; //Adds the desired angle of motor 2 to the reference angle |
Marle | 25:bbef09ff226b | 359 | |
MarijkeZondag | 35:63c890ac71ff | 360 | q1ref = q1_ii; //Makes new qref |
MarijkeZondag | 35:63c890ac71ff | 361 | q2ref = q2_ii; //Makes new qref |
MarijkeZondag | 32:56a8bd82e971 | 362 | |
MarijkeZondag | 35:63c890ac71ff | 363 | q1_motor = -q1ref/r_trans; //Sets the angle at which motor 1 needs to go, with ratio rotation/translation |
MarijkeZondag | 35:63c890ac71ff | 364 | q2_motor = q2ref*5.0; //Sets the angle at which motor 2 needs to go, scaled by 5 |
MarijkeZondag | 28:5e54cd4525de | 365 | |
MarijkeZondag | 35:63c890ac71ff | 366 | engine_control1(); //Call engine_control 1 function |
MarijkeZondag | 35:63c890ac71ff | 367 | engine_control2(); //Call engine_control 2 function |
Esmee | 33:976be2825a23 | 368 | |
Marle | 25:bbef09ff226b | 369 | } |
Marle | 25:bbef09ff226b | 370 | |
Marle | 25:bbef09ff226b | 371 | void v_des_calculate_qref() |
MarijkeZondag | 23:97a976a8f0fc | 372 | { |
MarijkeZondag | 35:63c890ac71ff | 373 | while(emg_cal==1) //After calibration is finished, emg_cal will be 1. Otherwise 0. |
MarijkeZondag | 16:5f7196ddc77b | 374 | { |
MarijkeZondag | 35:63c890ac71ff | 375 | if(movAg1>Threshold1 && movAg0<Threshold0) //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is off (movAg0) |
MarijkeZondag | 16:5f7196ddc77b | 376 | { |
MarijkeZondag | 35:63c890ac71ff | 377 | v_x = 0.05; //Movement in +x direction |
MarijkeZondag | 28:5e54cd4525de | 378 | v_y = 0.0; |
MarijkeZondag | 28:5e54cd4525de | 379 | |
MarijkeZondag | 35:63c890ac71ff | 380 | ledr = 0; //Led is red |
Marle | 25:bbef09ff226b | 381 | ledb = 1; |
Marle | 25:bbef09ff226b | 382 | ledg = 1; |
MarijkeZondag | 16:5f7196ddc77b | 383 | } |
MarijkeZondag | 35:63c890ac71ff | 384 | else if(movAg2>Threshold2 && movAg0<Threshold0) //If the filtered EMG signal of muscle 2 is higher than the threshold and the switch is off (movAg0) |
MarijkeZondag | 16:5f7196ddc77b | 385 | { |
MarijkeZondag | 35:63c890ac71ff | 386 | v_y = 0.05; //Movement in +y direction |
MarijkeZondag | 28:5e54cd4525de | 387 | v_x = 0.0; |
MarijkeZondag | 28:5e54cd4525de | 388 | |
MarijkeZondag | 35:63c890ac71ff | 389 | ledr = 1; //Led is green |
Marle | 25:bbef09ff226b | 390 | ledb = 1; |
Marle | 25:bbef09ff226b | 391 | ledg = 0; |
MarijkeZondag | 24:6d63ad38fef7 | 392 | } |
Marle | 25:bbef09ff226b | 393 | |
MarijkeZondag | 35:63c890ac71ff | 394 | else if(movAg0>Threshold0 && movAg1>Threshold1) //If the filtered EMG signal of muscle 1 is higher than the threshold and the switch is on (movAg0) |
MarijkeZondag | 24:6d63ad38fef7 | 395 | { |
MarijkeZondag | 35:63c890ac71ff | 396 | v_y = 0.0; //Movement in -x direction |
Esmee | 33:976be2825a23 | 397 | v_x = -0.05; |
MarijkeZondag | 28:5e54cd4525de | 398 | |
MarijkeZondag | 35:63c890ac71ff | 399 | ledr = 0; //Led is purple |
Marle | 25:bbef09ff226b | 400 | ledb = 0; |
MarijkeZondag | 30:f04a35f2a06d | 401 | ledg = 1; |
MarijkeZondag | 30:f04a35f2a06d | 402 | } |
MarijkeZondag | 28:5e54cd4525de | 403 | |
MarijkeZondag | 35:63c890ac71ff | 404 | else if(movAg0>Threshold0 && movAg2>Threshold2) //If the filtered EMG signal of muscle 2 is higher than the threshold and the switch is on (movAg0) |
MarijkeZondag | 30:f04a35f2a06d | 405 | { |
MarijkeZondag | 35:63c890ac71ff | 406 | v_y = -0.05; //Movement in -y direction |
MarijkeZondag | 30:f04a35f2a06d | 407 | v_x = 0.0; |
MarijkeZondag | 30:f04a35f2a06d | 408 | |
MarijkeZondag | 35:63c890ac71ff | 409 | ledr = 1; //Led is blue |
MarijkeZondag | 30:f04a35f2a06d | 410 | ledb = 0; |
MarijkeZondag | 30:f04a35f2a06d | 411 | ledg = 1; |
MarijkeZondag | 30:f04a35f2a06d | 412 | } |
MarijkeZondag | 35:63c890ac71ff | 413 | else //If not higher than any threshold, motors will not turn at all |
Marle | 25:bbef09ff226b | 414 | { |
Marle | 25:bbef09ff226b | 415 | v_x = 0; |
Marle | 25:bbef09ff226b | 416 | v_y = 0; |
MarijkeZondag | 28:5e54cd4525de | 417 | |
MarijkeZondag | 35:63c890ac71ff | 418 | ledr = 0; //Led is white |
MarijkeZondag | 24:6d63ad38fef7 | 419 | ledb = 0; |
MarijkeZondag | 24:6d63ad38fef7 | 420 | ledg = 0; |
MarijkeZondag | 24:6d63ad38fef7 | 421 | } |
MarijkeZondag | 27:fa493551be99 | 422 | |
MarijkeZondag | 35:63c890ac71ff | 423 | inverse_kinematics(); //Call inverse kinematics function |
MarijkeZondag | 27:fa493551be99 | 424 | |
MarijkeZondag | 28:5e54cd4525de | 425 | break; |
MarijkeZondag | 23:97a976a8f0fc | 426 | } |
MarijkeZondag | 26:ac5656aa35c7 | 427 | } |
MarijkeZondag | 26:ac5656aa35c7 | 428 | |
MarijkeZondag | 35:63c890ac71ff | 429 | //------------------ Start main function -----------------------------------------------------------------------------------------------------------// |
MarijkeZondag | 26:ac5656aa35c7 | 430 | |
MarijkeZondag | 23:97a976a8f0fc | 431 | int main() |
MarijkeZondag | 23:97a976a8f0fc | 432 | { |
MarijkeZondag | 35:63c890ac71ff | 433 | pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz |
MarijkeZondag | 23:97a976a8f0fc | 434 | |
MarijkeZondag | 35:63c890ac71ff | 435 | emg0filter.add( &emg0band1 ).add( &emg0band2 ).add( &emg0band3 ).add( ¬ch1 ); //Attach biquad elements to chain |
MarijkeZondag | 23:97a976a8f0fc | 436 | emg1filter.add( &emg1band1 ).add( &emg1band2 ).add( &emg1band3 ).add( ¬ch2 ); |
MarijkeZondag | 23:97a976a8f0fc | 437 | emg2filter.add( &emg2band1 ).add( &emg2band2 ).add( &emg2band3 ).add( ¬ch3 ); |
MarijkeZondag | 10:39ec51206c8b | 438 | |
MarijkeZondag | 35:63c890ac71ff | 439 | emg_tick.attach(&emg_filtered,T); //EMG signals filtered + moving average every T sec. |
MarijkeZondag | 28:5e54cd4525de | 440 | movag_tick.attach(&MovAg,T); |
MarijkeZondag | 35:63c890ac71ff | 441 | func_tick.attach(&v_des_calculate_qref,T); //v_des determined every T |
MarijkeZondag | 28:5e54cd4525de | 442 | |
MarijkeZondag | 35:63c890ac71ff | 443 | button1.rise(switch_to_calibrate); //Switch state of calibration (which muscle) |
MarijkeZondag | 35:63c890ac71ff | 444 | button2.rise(calibrate); //Calibrate threshold for 3 muscles |
MarijkeZondag | 28:5e54cd4525de | 445 | |
MarijkeZondag | 23:97a976a8f0fc | 446 | while(true) |
MarijkeZondag | 23:97a976a8f0fc | 447 | { |
MarijkeZondag | 30:f04a35f2a06d | 448 | ; |
MarijkeZondag | 23:97a976a8f0fc | 449 | } |
vsluiter | 0:c8f15874531b | 450 | } |