Code for controlling the Magna table
Dependencies: HIDScope MODSERIAL mbed-dsp mbed
main.cpp@5:19f8766b63da, 2016-01-13 (annotated)
- Committer:
- Technical_Muffin
- Date:
- Wed Jan 13 09:09:22 2016 +0000
- Revision:
- 5:19f8766b63da
- Parent:
- 4:61bdf601e7b0
- Child:
- 6:d03e4fa3a2a5
Changed direction values, for more natural movement.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Technical_Muffin | 0:34751b6a7dc9 | 1 | /*Code originally by Jesse Kaiser, s1355783 for control of the 2DOF Planar Table |
Technical_Muffin | 0:34751b6a7dc9 | 2 | Some variables are also numbered at the end. The numbers stands for the muscle that controls it. |
Technical_Muffin | 2:083d74325bfb | 3 | Biceps = 1 = lower right arm(wrist flexors) |
Technical_Muffin | 2:083d74325bfb | 4 | Triceps = 2 = upper right arm(wrist extensors) |
Technical_Muffin | 3:3d5ad874add0 | 5 | Pectoralis Major = 3 = upper left arm(wrist extensors) |
Technical_Muffin | 3:3d5ad874add0 | 6 | Deltoid posterior = 4 = lower left arm(wrist flexors) |
Technical_Muffin | 2:083d74325bfb | 7 | The "x" and "y" at the end of variables stand for the X-movement or Y-movement respectivly. |
Technical_Muffin | 2:083d74325bfb | 8 | The code has been revised to work with the new board and also has a secondary way of controlling it using a joystick |
Technical_Muffin | 0:34751b6a7dc9 | 9 | */ |
Technical_Muffin | 0:34751b6a7dc9 | 10 | |
Technical_Muffin | 0:34751b6a7dc9 | 11 | #include "mbed.h" |
Technical_Muffin | 0:34751b6a7dc9 | 12 | #include "MODSERIAL.h" |
Technical_Muffin | 0:34751b6a7dc9 | 13 | #include "arm_math.h" |
Technical_Muffin | 0:34751b6a7dc9 | 14 | #include "HIDScope.h" |
Technical_Muffin | 0:34751b6a7dc9 | 15 | |
Technical_Muffin | 0:34751b6a7dc9 | 16 | #define P_Gain 0.99 |
Technical_Muffin | 0:34751b6a7dc9 | 17 | #define K_Gain 150 //Gain of the filtered EMG signal |
Technical_Muffin | 0:34751b6a7dc9 | 18 | #define Damp 5 //Deceleration of the motor |
Technical_Muffin | 0:34751b6a7dc9 | 19 | #define Mass 1 // Mass value |
Technical_Muffin | 0:34751b6a7dc9 | 20 | #define dt 0.01 //Sample frequency |
Technical_Muffin | 4:61bdf601e7b0 | 21 | #define EMG_tresh1 0.015 |
Technical_Muffin | 4:61bdf601e7b0 | 22 | #define EMG_tresh2 0.015 |
Technical_Muffin | 4:61bdf601e7b0 | 23 | #define EMG_tresh3 0.015 |
Technical_Muffin | 4:61bdf601e7b0 | 24 | #define EMG_tresh4 0.015 |
Technical_Muffin | 0:34751b6a7dc9 | 25 | #define H_Gain 5 |
Technical_Muffin | 0:34751b6a7dc9 | 26 | #define Pt_x 0.80 |
Technical_Muffin | 0:34751b6a7dc9 | 27 | #define Pt_y 0.50 |
Technical_Muffin | 0:34751b6a7dc9 | 28 | #define error_tresh 0.02 |
Technical_Muffin | 0:34751b6a7dc9 | 29 | |
Technical_Muffin | 0:34751b6a7dc9 | 30 | MODSERIAL pc(USBTX,USBRX); |
Technical_Muffin | 0:34751b6a7dc9 | 31 | //joystick control |
Technical_Muffin | 0:34751b6a7dc9 | 32 | AnalogIn X_control(A1); |
Technical_Muffin | 0:34751b6a7dc9 | 33 | AnalogIn Y_control(A0); |
Technical_Muffin | 0:34751b6a7dc9 | 34 | //Motor control 1 |
Technical_Muffin | 0:34751b6a7dc9 | 35 | DigitalOut Diry(D12); |
Technical_Muffin | 0:34751b6a7dc9 | 36 | PwmOut Stepy(PTA1); |
Technical_Muffin | 0:34751b6a7dc9 | 37 | DigitalOut Enabley(PTC3); |
Technical_Muffin | 0:34751b6a7dc9 | 38 | |
Technical_Muffin | 0:34751b6a7dc9 | 39 | //motor control 2 |
Technical_Muffin | 0:34751b6a7dc9 | 40 | DigitalOut Dirx(PTC17); |
Technical_Muffin | 0:34751b6a7dc9 | 41 | PwmOut Stepx(PTD1); |
Technical_Muffin | 0:34751b6a7dc9 | 42 | DigitalOut Enablex(D0); |
Technical_Muffin | 0:34751b6a7dc9 | 43 | |
Technical_Muffin | 0:34751b6a7dc9 | 44 | //Microstepping 1 |
Technical_Muffin | 0:34751b6a7dc9 | 45 | DigitalOut MS11(D11); |
Technical_Muffin | 0:34751b6a7dc9 | 46 | DigitalOut MS21(D10); |
Technical_Muffin | 0:34751b6a7dc9 | 47 | DigitalOut MS31(D9); |
Technical_Muffin | 0:34751b6a7dc9 | 48 | //Microstepping 2 |
Technical_Muffin | 0:34751b6a7dc9 | 49 | DigitalOut MS12(PTC2); |
Technical_Muffin | 0:34751b6a7dc9 | 50 | DigitalOut MS22(PTA2); |
Technical_Muffin | 0:34751b6a7dc9 | 51 | DigitalOut MS32(PTB23); |
Technical_Muffin | 0:34751b6a7dc9 | 52 | |
Technical_Muffin | 0:34751b6a7dc9 | 53 | DigitalOut Ledr(LED1); |
Technical_Muffin | 0:34751b6a7dc9 | 54 | DigitalOut Ledg(LED2); |
Technical_Muffin | 0:34751b6a7dc9 | 55 | DigitalOut Ledb(LED3); |
Technical_Muffin | 0:34751b6a7dc9 | 56 | |
Technical_Muffin | 0:34751b6a7dc9 | 57 | //EMG inputs |
Technical_Muffin | 4:61bdf601e7b0 | 58 | AnalogIn emg1(A2); //biceps or wrist flexors bottom emg board |
Technical_Muffin | 4:61bdf601e7b0 | 59 | AnalogIn emg2(A3); //triceps or wirst extensors |
Technical_Muffin | 4:61bdf601e7b0 | 60 | AnalogIn emg3(A4); //Pectoralis major or wrist extensors |
Technical_Muffin | 4:61bdf601e7b0 | 61 | AnalogIn emg4(A5); //Deltoid or wrist flexors top emg board |
Technical_Muffin | 0:34751b6a7dc9 | 62 | |
Technical_Muffin | 0:34751b6a7dc9 | 63 | HIDScope scope(4); |
Technical_Muffin | 0:34751b6a7dc9 | 64 | Ticker scopeTimer; |
Technical_Muffin | 0:34751b6a7dc9 | 65 | Ticker emgtimer; |
Technical_Muffin | 0:34751b6a7dc9 | 66 | Ticker looptimer1; |
Technical_Muffin | 0:34751b6a7dc9 | 67 | Ticker looptimer2; |
Technical_Muffin | 0:34751b6a7dc9 | 68 | |
Technical_Muffin | 0:34751b6a7dc9 | 69 | //Variables for motor control |
Technical_Muffin | 0:34751b6a7dc9 | 70 | float setpoint = 3200; //Frequentie setpoint |
Technical_Muffin | 0:34751b6a7dc9 | 71 | float step_freq1 = 1; |
Technical_Muffin | 0:34751b6a7dc9 | 72 | float step_freq2 = 1; |
Technical_Muffin | 0:34751b6a7dc9 | 73 | |
Technical_Muffin | 0:34751b6a7dc9 | 74 | //EMG filter |
Technical_Muffin | 0:34751b6a7dc9 | 75 | arm_biquad_casd_df1_inst_f32 lowpass1_biceps; |
Technical_Muffin | 0:34751b6a7dc9 | 76 | arm_biquad_casd_df1_inst_f32 lowpass1_triceps; |
Technical_Muffin | 0:34751b6a7dc9 | 77 | arm_biquad_casd_df1_inst_f32 lowpass1_pect; |
Technical_Muffin | 0:34751b6a7dc9 | 78 | arm_biquad_casd_df1_inst_f32 lowpass1_deltoid; |
Technical_Muffin | 0:34751b6a7dc9 | 79 | |
Technical_Muffin | 0:34751b6a7dc9 | 80 | arm_biquad_casd_df1_inst_f32 lowpass2_biceps; |
Technical_Muffin | 0:34751b6a7dc9 | 81 | arm_biquad_casd_df1_inst_f32 lowpass2_triceps; |
Technical_Muffin | 0:34751b6a7dc9 | 82 | arm_biquad_casd_df1_inst_f32 lowpass2_pect; |
Technical_Muffin | 0:34751b6a7dc9 | 83 | arm_biquad_casd_df1_inst_f32 lowpass2_deltoid; |
Technical_Muffin | 0:34751b6a7dc9 | 84 | |
Technical_Muffin | 4:61bdf601e7b0 | 85 | arm_biquad_casd_df1_inst_f32 highpass_biceps; |
Technical_Muffin | 4:61bdf601e7b0 | 86 | arm_biquad_casd_df1_inst_f32 highpass_triceps; |
Technical_Muffin | 4:61bdf601e7b0 | 87 | arm_biquad_casd_df1_inst_f32 highpass_pect; |
Technical_Muffin | 4:61bdf601e7b0 | 88 | arm_biquad_casd_df1_inst_f32 highpass_deltoid; |
Technical_Muffin | 4:61bdf601e7b0 | 89 | |
Technical_Muffin | 1:c8ad338ba312 | 90 | //used as extra filter for wrist motion |
Technical_Muffin | 1:c8ad338ba312 | 91 | arm_biquad_casd_df1_inst_f32 bandstop_biceps; |
Technical_Muffin | 1:c8ad338ba312 | 92 | arm_biquad_casd_df1_inst_f32 bandstop_triceps; |
Technical_Muffin | 1:c8ad338ba312 | 93 | arm_biquad_casd_df1_inst_f32 bandstop_pect; |
Technical_Muffin | 1:c8ad338ba312 | 94 | arm_biquad_casd_df1_inst_f32 bandstop_deltoid; |
Technical_Muffin | 5:19f8766b63da | 95 | /* |
Technical_Muffin | 5:19f8766b63da | 96 | //200hz sampling test |
Technical_Muffin | 5:19f8766b63da | 97 | |
Technical_Muffin | 5:19f8766b63da | 98 | //lowpass 1 filter settings: Fc = 90 Hz, Fs = 200 Hz, Gain = -3 dB |
Technical_Muffin | 5:19f8766b63da | 99 | //lowpass 2 filter settings: Fc = 0.8 Hz, Fs = 200 Hz, Gain = -3 dB |
Technical_Muffin | 5:19f8766b63da | 100 | float lowpass1_const[] = {0.800592403464570, 1.60118480692914,0.800592403464570,-1.56101807580072,-0.641351538057563}; |
Technical_Muffin | 5:19f8766b63da | 101 | float lowpass2_const[] = {0.000155148423475699, 0.000310296846951398, 0.000155148423475699,1.96446058020523,-0.965081173899135}; |
Technical_Muffin | 5:19f8766b63da | 102 | //highpass filter settings: Fc = 10 Hz, Fs = 200 Hz |
Technical_Muffin | 5:19f8766b63da | 103 | float highpass_const[] = {0.800592403464570, -1.60118480692914, 0.800592403464570,1.56101807580072 ,-0.641351538057563}; |
Technical_Muffin | 5:19f8766b63da | 104 | //bandstop filter settings Fc=[45Hz 55Hz], Fs= 200Hz |
Technical_Muffin | 5:19f8766b63da | 105 | float bandstop_const[] = {0.800592403464570, -9.80442924324572e-17, 0.800592403464570,-0.201669174120189,-0.800844265795519 , 1,-1.22464679914735e-16 , 1 ,0.201669174120189,-0.800844265795519}; |
Technical_Muffin | 5:19f8766b63da | 106 | */ |
Technical_Muffin | 1:c8ad338ba312 | 107 | |
Technical_Muffin | 1:c8ad338ba312 | 108 | //lowpass 1 filter settings: Fc = 49 Hz, Fs = 100 Hz, Gain = -3 dB |
Technical_Muffin | 1:c8ad338ba312 | 109 | //lowpass 2 filter settings: Fc = 0.8 Hz, Fs = 100 Hz, Gain = -3 dB |
Technical_Muffin | 1:c8ad338ba312 | 110 | float lowpass1_const[] = {0.956543225556877,1.91308645111375,0.956543225556877,-1.91119706742607,-0.914975834801434}; |
Technical_Muffin | 1:c8ad338ba312 | 111 | float lowpass2_const[] = {0.000609854718717299,0.00121970943743460,0.000609854718717299,1.92894226325203,-0.931381682126903}; |
Technical_Muffin | 1:c8ad338ba312 | 112 | //highpass filter settings: Fc = 20 Hz, Fs = 100 Hz |
Technical_Muffin | 1:c8ad338ba312 | 113 | float highpass_const[] = {0.391335772501769,-0.782671545003538,0.391335772501769,0.369527377351241,-0.195815712655833}; |
Technical_Muffin | 1:c8ad338ba312 | 114 | //bandstop filter settings Fc=[29Hz 36Hz], Fs= 100Hz |
Technical_Muffin | 1:c8ad338ba312 | 115 | float bandstop_const[] = {0.732022476556623, 0.681064744276583,0.732022476556623,-0.535944148680739,-0.713976335521464,1,0.930387749130681,1,-1.04147956553087,-0.752398361226849}; |
Technical_Muffin | 4:61bdf601e7b0 | 116 | |
Technical_Muffin | 1:c8ad338ba312 | 117 | /* |
Technical_Muffin | 1:c8ad338ba312 | 118 | //values are usable for triceps and biceps continuous motion, not for wrist motion. |
Technical_Muffin | 0:34751b6a7dc9 | 119 | //lowpass 1 filter settings: Fc = 45 Hz, Fs = 100 Hz, Gain = -3 dB |
Technical_Muffin | 0:34751b6a7dc9 | 120 | //lowpass 2 filter settings: Fc = 0.3 Hz, Fs = 100 Hz, Gain = -3 dB |
Technical_Muffin | 0:34751b6a7dc9 | 121 | float lowpass1_const[] = {0.800592403464570,1.60118480692914,0.800592403464570,-1.56101807580072,-0.641351538057563}; |
Technical_Muffin | 0:34751b6a7dc9 | 122 | float lowpass2_const[] = {8.76555487540147e-05, 0.000175311097508029, 8.76555487540147e-05 , 1.97334424978130, -0.973694871976315}; |
Technical_Muffin | 0:34751b6a7dc9 | 123 | //highpass filter settings: Fc = 20 Hz, Fs = 100 Hz |
Technical_Muffin | 0:34751b6a7dc9 | 124 | |
Technical_Muffin | 0:34751b6a7dc9 | 125 | float highpass_const[] = {0.391335772501769 ,-0.782671545003538, 0.391335772501769,0.369527377351241, -0.195815712655833}; |
Technical_Muffin | 0:34751b6a7dc9 | 126 | |
Technical_Muffin | 1:c8ad338ba312 | 127 | */ |
Technical_Muffin | 0:34751b6a7dc9 | 128 | /* |
Technical_Muffin | 0:34751b6a7dc9 | 129 | //suffer from high wire motion influence and dont react to continuous tension only rapid activation |
Technical_Muffin | 0:34751b6a7dc9 | 130 | |
Technical_Muffin | 0:34751b6a7dc9 | 131 | //lowpass 1 filter settings: Fc = 20 Hz, Fs = 100 Hz, Gain = -3 dB |
Technical_Muffin | 0:34751b6a7dc9 | 132 | //lowpass 2 filter settings: Fc = 0.3 Hz, Fs = 100 Hz, Gain = -3 dB |
Technical_Muffin | 0:34751b6a7dc9 | 133 | float lowpass1_const[] = {0.206572083826148,0.413144167652296,0.206572083826148,0.369527377351241,-0.195815712655833}; |
Technical_Muffin | 0:34751b6a7dc9 | 134 | float lowpass2_const[] = {0.0000876555487540065, 0.000175311097508013, 0.0000876555487540065, 1.97334424978130, -0.973694871976315}; |
Technical_Muffin | 0:34751b6a7dc9 | 135 | //highpass filter settings: Fc = 3 Hz, Fs = 100 Hz |
Technical_Muffin | 0:34751b6a7dc9 | 136 | |
Technical_Muffin | 0:34751b6a7dc9 | 137 | float highpass_const[] = {0.875183092438135,-1.75036618487627,0.875183092438135,1.73472576880928,-0.766006600943264}; |
Technical_Muffin | 0:34751b6a7dc9 | 138 | */ |
Technical_Muffin | 0:34751b6a7dc9 | 139 | |
Technical_Muffin | 0:34751b6a7dc9 | 140 | /* |
Technical_Muffin | 0:34751b6a7dc9 | 141 | Previous coefficients testing to see if new variants work |
Technical_Muffin | 0:34751b6a7dc9 | 142 | lowpass 1 filter settings: Fc = 45 Hz, Fs = 100 Hz, Gain = -3 dB |
Technical_Muffin | 0:34751b6a7dc9 | 143 | lowpass 2 filter settings: Fc = 2 Hz, Fs = 100 Hz, Gain = -3 dB |
Technical_Muffin | 0:34751b6a7dc9 | 144 | float lowpass1_const[] = {0.800592403464570, 1.60118480692914, 0.800592403464570, -1.56101807580072, -0.641351538057563}; |
Technical_Muffin | 0:34751b6a7dc9 | 145 | float lowpass2_const[] = {0.0000876555487540065, 0.000175311097508013, 0.0000876555487540065, 1.97334424978130, -0.973694871976315}; |
Technical_Muffin | 0:34751b6a7dc9 | 146 | //highpass filter settings: Fc = 20 Hz, Fs = 100 Hz |
Technical_Muffin | 0:34751b6a7dc9 | 147 | |
Technical_Muffin | 0:34751b6a7dc9 | 148 | float highpass_const[] = {0.391335772501769, -0.782671545003537,0.391335772501769,0.369527377351241,-0.195815712655833 }; |
Technical_Muffin | 0:34751b6a7dc9 | 149 | */ |
Technical_Muffin | 0:34751b6a7dc9 | 150 | |
Technical_Muffin | 0:34751b6a7dc9 | 151 | //state values |
Technical_Muffin | 0:34751b6a7dc9 | 152 | float lowpass1_biceps_states[4]; |
Technical_Muffin | 0:34751b6a7dc9 | 153 | float lowpass2_biceps_states[4]; |
Technical_Muffin | 0:34751b6a7dc9 | 154 | float highpass_biceps_states[4]; |
Technical_Muffin | 1:c8ad338ba312 | 155 | float bandstop_biceps_states[8]; |
Technical_Muffin | 1:c8ad338ba312 | 156 | |
Technical_Muffin | 0:34751b6a7dc9 | 157 | float lowpass1_triceps_states[4]; |
Technical_Muffin | 0:34751b6a7dc9 | 158 | float lowpass2_triceps_states[4]; |
Technical_Muffin | 0:34751b6a7dc9 | 159 | float highpass_triceps_states[4]; |
Technical_Muffin | 1:c8ad338ba312 | 160 | float bandstop_triceps_states[8]; |
Technical_Muffin | 1:c8ad338ba312 | 161 | |
Technical_Muffin | 0:34751b6a7dc9 | 162 | float lowpass1_pect_states[4]; |
Technical_Muffin | 0:34751b6a7dc9 | 163 | float lowpass2_pect_states[4]; |
Technical_Muffin | 0:34751b6a7dc9 | 164 | float highpass_pect_states[4]; |
Technical_Muffin | 1:c8ad338ba312 | 165 | float bandstop_pect_states[8]; |
Technical_Muffin | 1:c8ad338ba312 | 166 | |
Technical_Muffin | 0:34751b6a7dc9 | 167 | float lowpass1_deltoid_states[4]; |
Technical_Muffin | 0:34751b6a7dc9 | 168 | float lowpass2_deltoid_states[4]; |
Technical_Muffin | 0:34751b6a7dc9 | 169 | float highpass_deltoid_states[4]; |
Technical_Muffin | 1:c8ad338ba312 | 170 | float bandstop_deltoid_states[8]; |
Technical_Muffin | 1:c8ad338ba312 | 171 | |
Technical_Muffin | 0:34751b6a7dc9 | 172 | |
Technical_Muffin | 4:61bdf601e7b0 | 173 | //global variables |
Technical_Muffin | 0:34751b6a7dc9 | 174 | float filtered_biceps, filtered_triceps, filtered_pect, filtered_deltoid; |
Technical_Muffin | 0:34751b6a7dc9 | 175 | float speed_old1, speed_old2, speed_old3, speed_old4; |
Technical_Muffin | 0:34751b6a7dc9 | 176 | float acc1, acc2, acc3, acc4; |
Technical_Muffin | 0:34751b6a7dc9 | 177 | float force1, force2, force3, force4; |
Technical_Muffin | 0:34751b6a7dc9 | 178 | float speed1, speed2, speed3, speed4; |
Technical_Muffin | 0:34751b6a7dc9 | 179 | float damping1, damping2, damping3, damping4; |
Technical_Muffin | 0:34751b6a7dc9 | 180 | float emg_x, emg_y; |
Technical_Muffin | 0:34751b6a7dc9 | 181 | float cx = 0; |
Technical_Muffin | 0:34751b6a7dc9 | 182 | float cy = 0; |
Technical_Muffin | 0:34751b6a7dc9 | 183 | float errorx = 0.2; |
Technical_Muffin | 0:34751b6a7dc9 | 184 | float errory = 0.2; |
Technical_Muffin | 0:34751b6a7dc9 | 185 | float Ps_x = 0; |
Technical_Muffin | 0:34751b6a7dc9 | 186 | float Ps_y = 0; |
Technical_Muffin | 0:34751b6a7dc9 | 187 | float hstep_freqx = 1; |
Technical_Muffin | 0:34751b6a7dc9 | 188 | float hstep_freqy = 1; |
Technical_Muffin | 0:34751b6a7dc9 | 189 | float emg_y_abs = 0; |
Technical_Muffin | 0:34751b6a7dc9 | 190 | float emg_x_abs = 0; |
Technical_Muffin | 0:34751b6a7dc9 | 191 | |
Technical_Muffin | 0:34751b6a7dc9 | 192 | void looper_emg() |
Technical_Muffin | 0:34751b6a7dc9 | 193 | { |
Technical_Muffin | 0:34751b6a7dc9 | 194 | float emg_value1_f32, emg_value2_f32, emg_value3_f32, emg_value4_f32; |
Technical_Muffin | 0:34751b6a7dc9 | 195 | emg_value1_f32 = emg1.read(); |
Technical_Muffin | 0:34751b6a7dc9 | 196 | emg_value2_f32 = emg2.read(); |
Technical_Muffin | 0:34751b6a7dc9 | 197 | emg_value3_f32 = emg3.read(); |
Technical_Muffin | 0:34751b6a7dc9 | 198 | emg_value4_f32 = emg4.read(); |
Technical_Muffin | 0:34751b6a7dc9 | 199 | |
Technical_Muffin | 4:61bdf601e7b0 | 200 | //Biquad process emg biceps |
Technical_Muffin | 1:c8ad338ba312 | 201 | arm_biquad_cascade_df1_f32(&bandstop_biceps, &emg_value1_f32, &filtered_biceps, 1 );//used for wrist motion |
Technical_Muffin | 1:c8ad338ba312 | 202 | arm_biquad_cascade_df1_f32(&lowpass1_biceps, &filtered_biceps, &filtered_biceps, 1 ); |
Technical_Muffin | 0:34751b6a7dc9 | 203 | arm_biquad_cascade_df1_f32(&highpass_biceps, &filtered_biceps, &filtered_biceps, 1 ); |
Technical_Muffin | 0:34751b6a7dc9 | 204 | filtered_biceps = fabs(filtered_biceps); //Rectifier, The Gain is already implemented. |
Technical_Muffin | 0:34751b6a7dc9 | 205 | arm_biquad_cascade_df1_f32(&lowpass2_biceps, &filtered_biceps, &filtered_biceps, 1 ); //low pass filter |
Technical_Muffin | 0:34751b6a7dc9 | 206 | |
Technical_Muffin | 4:61bdf601e7b0 | 207 | //Biquad process emg triceps |
Technical_Muffin | 1:c8ad338ba312 | 208 | arm_biquad_cascade_df1_f32(&bandstop_triceps, &emg_value2_f32, &filtered_triceps, 1 ); //used for wrist motion |
Technical_Muffin | 1:c8ad338ba312 | 209 | arm_biquad_cascade_df1_f32(&lowpass1_triceps, &filtered_triceps, &filtered_triceps, 1 ); |
Technical_Muffin | 0:34751b6a7dc9 | 210 | arm_biquad_cascade_df1_f32(&highpass_triceps, &filtered_triceps, &filtered_triceps, 1 ); |
Technical_Muffin | 0:34751b6a7dc9 | 211 | filtered_triceps = fabs(filtered_triceps); |
Technical_Muffin | 0:34751b6a7dc9 | 212 | arm_biquad_cascade_df1_f32(&lowpass2_triceps, &filtered_triceps, &filtered_triceps, 1 ); |
Technical_Muffin | 0:34751b6a7dc9 | 213 | |
Technical_Muffin | 4:61bdf601e7b0 | 214 | //Biquad process emg pectoralis major |
Technical_Muffin | 1:c8ad338ba312 | 215 | arm_biquad_cascade_df1_f32(&bandstop_pect, &emg_value3_f32, &filtered_pect, 1 );//used for wrist motion |
Technical_Muffin | 1:c8ad338ba312 | 216 | arm_biquad_cascade_df1_f32(&lowpass1_pect, &filtered_pect, &filtered_pect, 1 ); |
Technical_Muffin | 0:34751b6a7dc9 | 217 | arm_biquad_cascade_df1_f32(&highpass_pect, &filtered_pect, &filtered_pect, 1 ); |
Technical_Muffin | 1:c8ad338ba312 | 218 | filtered_pect = fabs(filtered_pect); |
Technical_Muffin | 1:c8ad338ba312 | 219 | arm_biquad_cascade_df1_f32(&lowpass2_pect, &filtered_pect, &filtered_pect, 1 ); |
Technical_Muffin | 0:34751b6a7dc9 | 220 | |
Technical_Muffin | 4:61bdf601e7b0 | 221 | //Biquad process emg deltoid |
Technical_Muffin | 1:c8ad338ba312 | 222 | arm_biquad_cascade_df1_f32(&bandstop_deltoid, &emg_value4_f32, &filtered_deltoid, 1 );//used for wrist motion |
Technical_Muffin | 1:c8ad338ba312 | 223 | arm_biquad_cascade_df1_f32(&lowpass1_deltoid, &filtered_deltoid, &filtered_deltoid, 1 ); |
Technical_Muffin | 0:34751b6a7dc9 | 224 | arm_biquad_cascade_df1_f32(&highpass_deltoid, &filtered_deltoid, &filtered_deltoid, 1 ); |
Technical_Muffin | 1:c8ad338ba312 | 225 | filtered_deltoid = fabs(filtered_deltoid); |
Technical_Muffin | 1:c8ad338ba312 | 226 | arm_biquad_cascade_df1_f32(&lowpass2_deltoid, &filtered_deltoid, &filtered_deltoid, 1 ); |
Technical_Muffin | 1:c8ad338ba312 | 227 | |
Technical_Muffin | 4:61bdf601e7b0 | 228 | // send value to PC for control |
Technical_Muffin | 0:34751b6a7dc9 | 229 | |
Technical_Muffin | 3:3d5ad874add0 | 230 | scope.set(0,filtered_biceps); //Filtered EMG signals |
Technical_Muffin | 3:3d5ad874add0 | 231 | scope.set(1,filtered_triceps); |
Technical_Muffin | 3:3d5ad874add0 | 232 | scope.set(2,filtered_pect); |
Technical_Muffin | 3:3d5ad874add0 | 233 | scope.set(3,filtered_deltoid); |
Technical_Muffin | 0:34751b6a7dc9 | 234 | scope.send(); |
Technical_Muffin | 0:34751b6a7dc9 | 235 | |
Technical_Muffin | 0:34751b6a7dc9 | 236 | } |
Technical_Muffin | 0:34751b6a7dc9 | 237 | |
Technical_Muffin | 0:34751b6a7dc9 | 238 | |
Technical_Muffin | 0:34751b6a7dc9 | 239 | void looper_motory() |
Technical_Muffin | 0:34751b6a7dc9 | 240 | { |
Technical_Muffin | 0:34751b6a7dc9 | 241 | |
Technical_Muffin | 4:61bdf601e7b0 | 242 | emg_y = (filtered_biceps - filtered_triceps); |
Technical_Muffin | 4:61bdf601e7b0 | 243 | /*emg_y_abs = fabs(emg_y); |
Technical_Muffin | 0:34751b6a7dc9 | 244 | force1 = emg_y_abs*K_Gain; |
Technical_Muffin | 0:34751b6a7dc9 | 245 | force1 = force1 - damping1; |
Technical_Muffin | 0:34751b6a7dc9 | 246 | acc1 = force1/Mass; |
Technical_Muffin | 0:34751b6a7dc9 | 247 | speed1 = speed_old1 + (acc1 * dt); |
Technical_Muffin | 0:34751b6a7dc9 | 248 | damping1 = speed1 * Damp; |
Technical_Muffin | 0:34751b6a7dc9 | 249 | step_freq1 = setpoint * speed1;*/ |
Technical_Muffin | 0:34751b6a7dc9 | 250 | // Stepy.period(/*1.0/step_freq1*/0.000625); |
Technical_Muffin | 0:34751b6a7dc9 | 251 | //speed_old1 = speed1; |
Technical_Muffin | 4:61bdf601e7b0 | 252 | |
Technical_Muffin | 4:61bdf601e7b0 | 253 | if (emg_y > 0) { |
Technical_Muffin | 5:19f8766b63da | 254 | Diry = 0; |
Technical_Muffin | 4:61bdf601e7b0 | 255 | } |
Technical_Muffin | 4:61bdf601e7b0 | 256 | if (emg_y < 0) { |
Technical_Muffin | 5:19f8766b63da | 257 | Diry = 1; |
Technical_Muffin | 4:61bdf601e7b0 | 258 | } |
Technical_Muffin | 4:61bdf601e7b0 | 259 | /*//Speed limit |
Technical_Muffin | 4:61bdf601e7b0 | 260 | if (speed2 > 1) { |
Technical_Muffin | 4:61bdf601e7b0 | 261 | speed2 = 1; |
Technical_Muffin | 4:61bdf601e7b0 | 262 | step_freq2 = setpoint; |
Technical_Muffin | 4:61bdf601e7b0 | 263 | }*/ |
Technical_Muffin | 4:61bdf601e7b0 | 264 | //EMG treshold |
Technical_Muffin | 4:61bdf601e7b0 | 265 | if (filtered_biceps < EMG_tresh1 && filtered_triceps < EMG_tresh2) { |
Technical_Muffin | 4:61bdf601e7b0 | 266 | Enabley = 1; //Enable = 1 turns the motor off. |
Technical_Muffin | 4:61bdf601e7b0 | 267 | } else { |
Technical_Muffin | 4:61bdf601e7b0 | 268 | Enabley = 0; |
Technical_Muffin | 4:61bdf601e7b0 | 269 | } |
Technical_Muffin | 1:c8ad338ba312 | 270 | |
Technical_Muffin | 1:c8ad338ba312 | 271 | //0.06 for biceps and triceps movement, 0.04 for wrist lower and upper motion right arm lower2 connections |
Technical_Muffin | 4:61bdf601e7b0 | 272 | /* if (filtered_triceps > 0.04) {//upward cart movement |
Technical_Muffin | 0:34751b6a7dc9 | 273 | Diry.write(1); |
Technical_Muffin | 0:34751b6a7dc9 | 274 | Enabley.write(0); //Enable = 1 turns the motor off. |
Technical_Muffin | 0:34751b6a7dc9 | 275 | Ledr.write(0); |
Technical_Muffin | 0:34751b6a7dc9 | 276 | Ledb.write(1); |
Technical_Muffin | 0:34751b6a7dc9 | 277 | } |
Technical_Muffin | 1:c8ad338ba312 | 278 | else if (filtered_biceps > 0.04) {// downward cart movement |
Technical_Muffin | 0:34751b6a7dc9 | 279 | Diry.write(0); |
Technical_Muffin | 0:34751b6a7dc9 | 280 | Enabley.write(0); //Enable = 1 turns the motor off. |
Technical_Muffin | 0:34751b6a7dc9 | 281 | Ledr.write(1); |
Technical_Muffin | 0:34751b6a7dc9 | 282 | Ledb.write(0); |
Technical_Muffin | 0:34751b6a7dc9 | 283 | } |
Technical_Muffin | 1:c8ad338ba312 | 284 | else if (filtered_triceps < 0.015 && filtered_biceps < 0.015) {//0.04 for biceps and triceps stop, 0.015, 0.015 for wrist lower and upper stop |
Technical_Muffin | 0:34751b6a7dc9 | 285 | Enabley.write(1); |
Technical_Muffin | 0:34751b6a7dc9 | 286 | } |
Technical_Muffin | 0:34751b6a7dc9 | 287 | else{} |
Technical_Muffin | 0:34751b6a7dc9 | 288 | //Speed limit |
Technical_Muffin | 4:61bdf601e7b0 | 289 | if (speed1 > 1) { |
Technical_Muffin | 0:34751b6a7dc9 | 290 | speed1 = 1; |
Technical_Muffin | 0:34751b6a7dc9 | 291 | step_freq1 = setpoint; |
Technical_Muffin | 0:34751b6a7dc9 | 292 | }*/ |
Technical_Muffin | 0:34751b6a7dc9 | 293 | |
Technical_Muffin | 0:34751b6a7dc9 | 294 | } |
Technical_Muffin | 1:c8ad338ba312 | 295 | |
Technical_Muffin | 0:34751b6a7dc9 | 296 | void looper_motorx() |
Technical_Muffin | 0:34751b6a7dc9 | 297 | { |
Technical_Muffin | 0:34751b6a7dc9 | 298 | |
Technical_Muffin | 3:3d5ad874add0 | 299 | emg_x = (filtered_pect - filtered_deltoid); |
Technical_Muffin | 3:3d5ad874add0 | 300 | /*emg_x_abs = fabs(emg_x); |
Technical_Muffin | 0:34751b6a7dc9 | 301 | force2 = emg_x_abs*K_Gain; |
Technical_Muffin | 0:34751b6a7dc9 | 302 | force2 = force2 - damping2; |
Technical_Muffin | 0:34751b6a7dc9 | 303 | acc2 = force2/Mass; |
Technical_Muffin | 0:34751b6a7dc9 | 304 | speed2 = speed_old2 + (acc2 * dt); |
Technical_Muffin | 0:34751b6a7dc9 | 305 | damping2 = speed2 * Damp; |
Technical_Muffin | 0:34751b6a7dc9 | 306 | step_freq2 = setpoint * speed2; |
Technical_Muffin | 0:34751b6a7dc9 | 307 | Stepx.period(1.0/step_freq2); |
Technical_Muffin | 0:34751b6a7dc9 | 308 | speed_old2 = speed2; |
Technical_Muffin | 3:3d5ad874add0 | 309 | */ |
Technical_Muffin | 0:34751b6a7dc9 | 310 | if (emg_x > 0) { |
Technical_Muffin | 5:19f8766b63da | 311 | Dirx = 0; |
Technical_Muffin | 0:34751b6a7dc9 | 312 | } |
Technical_Muffin | 0:34751b6a7dc9 | 313 | if (emg_x < 0) { |
Technical_Muffin | 5:19f8766b63da | 314 | Dirx = 1; |
Technical_Muffin | 0:34751b6a7dc9 | 315 | } |
Technical_Muffin | 3:3d5ad874add0 | 316 | /*//Speed limit |
Technical_Muffin | 0:34751b6a7dc9 | 317 | if (speed2 > 1) { |
Technical_Muffin | 0:34751b6a7dc9 | 318 | speed2 = 1; |
Technical_Muffin | 0:34751b6a7dc9 | 319 | step_freq2 = setpoint; |
Technical_Muffin | 3:3d5ad874add0 | 320 | }*/ |
Technical_Muffin | 0:34751b6a7dc9 | 321 | //EMG treshold |
Technical_Muffin | 0:34751b6a7dc9 | 322 | if (filtered_pect < EMG_tresh3 && filtered_deltoid < EMG_tresh4) { |
Technical_Muffin | 0:34751b6a7dc9 | 323 | Enablex = 1; //Enable = 1 turns the motor off. |
Technical_Muffin | 0:34751b6a7dc9 | 324 | } else { |
Technical_Muffin | 0:34751b6a7dc9 | 325 | Enablex = 0; |
Technical_Muffin | 0:34751b6a7dc9 | 326 | } |
Technical_Muffin | 3:3d5ad874add0 | 327 | /* |
Technical_Muffin | 1:c8ad338ba312 | 328 | //0.06 for biceps and triceps movement, 0.03 for wrist lower and upper motion left arm upper 2 connections |
Technical_Muffin | 1:c8ad338ba312 | 329 | if (filtered_pect > 0.03) {//right cart movement |
Technical_Muffin | 1:c8ad338ba312 | 330 | Dirx.write(1); |
Technical_Muffin | 1:c8ad338ba312 | 331 | Enablex.write(0); //Enable = 1 turns the motor off. |
Technical_Muffin | 1:c8ad338ba312 | 332 | Ledr.write(0); |
Technical_Muffin | 1:c8ad338ba312 | 333 | Ledb.write(1); |
Technical_Muffin | 1:c8ad338ba312 | 334 | } |
Technical_Muffin | 1:c8ad338ba312 | 335 | else if (filtered_deltoid > 0.03) {//left cart movement |
Technical_Muffin | 1:c8ad338ba312 | 336 | Dirx.write(0); |
Technical_Muffin | 1:c8ad338ba312 | 337 | Enablex.write(0); //Enable = 1 turns the motor off. |
Technical_Muffin | 1:c8ad338ba312 | 338 | Ledr.write(1); |
Technical_Muffin | 1:c8ad338ba312 | 339 | Ledb.write(0); |
Technical_Muffin | 1:c8ad338ba312 | 340 | } |
Technical_Muffin | 1:c8ad338ba312 | 341 | else if (filtered_pect < 0.01 && filtered_deltoid < 0.01) {//0.04 for biceps and triceps stop, 0.01 for wrist lower and upper stop |
Technical_Muffin | 1:c8ad338ba312 | 342 | Enablex.write(1); |
Technical_Muffin | 1:c8ad338ba312 | 343 | } |
Technical_Muffin | 4:61bdf601e7b0 | 344 | else{}*/ |
Technical_Muffin | 4:61bdf601e7b0 | 345 | } |
Technical_Muffin | 0:34751b6a7dc9 | 346 | |
Technical_Muffin | 0:34751b6a7dc9 | 347 | int main() |
Technical_Muffin | 0:34751b6a7dc9 | 348 | { |
Technical_Muffin | 0:34751b6a7dc9 | 349 | |
Technical_Muffin | 0:34751b6a7dc9 | 350 | //scopeTimer.attach_us(&scopeSend, 2e3); |
Technical_Muffin | 0:34751b6a7dc9 | 351 | pc.baud(115200); |
Technical_Muffin | 0:34751b6a7dc9 | 352 | Ledr=1; |
Technical_Muffin | 0:34751b6a7dc9 | 353 | Ledg=1; |
Technical_Muffin | 0:34751b6a7dc9 | 354 | Ledb=1; |
Technical_Muffin | 0:34751b6a7dc9 | 355 | |
Technical_Muffin | 0:34751b6a7dc9 | 356 | MS11=1;//higher microstepping in combination with a higher step frequency reduces the vibration significantly |
Technical_Muffin | 0:34751b6a7dc9 | 357 | MS21=1;//it is now in 16th step mode, which seems to work really well and smooth without causing major vibrations |
Technical_Muffin | 0:34751b6a7dc9 | 358 | MS31=1; |
Technical_Muffin | 0:34751b6a7dc9 | 359 | MS12=1; |
Technical_Muffin | 0:34751b6a7dc9 | 360 | MS22=1; |
Technical_Muffin | 0:34751b6a7dc9 | 361 | MS32=1; |
Technical_Muffin | 1:c8ad338ba312 | 362 | Stepy.write(0.5f); |
Technical_Muffin | 0:34751b6a7dc9 | 363 | Enabley.write(1); |
Technical_Muffin | 0:34751b6a7dc9 | 364 | Stepx.write(0.5f); |
Technical_Muffin | 0:34751b6a7dc9 | 365 | Enablex.write(1); |
Technical_Muffin | 0:34751b6a7dc9 | 366 | Stepy.period(0.000625);//use period change for speed adjustments |
Technical_Muffin | 0:34751b6a7dc9 | 367 | Stepx.period(0.000625);//frequency of 1600 Hz |
Technical_Muffin | 0:34751b6a7dc9 | 368 | |
Technical_Muffin | 2:083d74325bfb | 369 | |
Technical_Muffin | 2:083d74325bfb | 370 | /*//code for controlling the mechanism with a joystick |
Technical_Muffin | 0:34751b6a7dc9 | 371 | float Left_value = 0.6; |
Technical_Muffin | 0:34751b6a7dc9 | 372 | float Right_value = 0.9; |
Technical_Muffin | 0:34751b6a7dc9 | 373 | float Up_value = 0.6; |
Technical_Muffin | 0:34751b6a7dc9 | 374 | float Down_value = 0.9; |
Technical_Muffin | 2:083d74325bfb | 375 | |
Technical_Muffin | 2:083d74325bfb | 376 | while(1){ |
Technical_Muffin | 0:34751b6a7dc9 | 377 | if (X_control.read() < Left_value){ |
Technical_Muffin | 0:34751b6a7dc9 | 378 | Dirx.write(0); |
Technical_Muffin | 0:34751b6a7dc9 | 379 | Enablex.write(0); |
Technical_Muffin | 0:34751b6a7dc9 | 380 | } |
Technical_Muffin | 0:34751b6a7dc9 | 381 | else if (X_control.read() > Right_value){ |
Technical_Muffin | 0:34751b6a7dc9 | 382 | Dirx.write(1); |
Technical_Muffin | 0:34751b6a7dc9 | 383 | Enablex.write(0); |
Technical_Muffin | 0:34751b6a7dc9 | 384 | } |
Technical_Muffin | 0:34751b6a7dc9 | 385 | else{ |
Technical_Muffin | 0:34751b6a7dc9 | 386 | Enablex.write(1); |
Technical_Muffin | 0:34751b6a7dc9 | 387 | } |
Technical_Muffin | 0:34751b6a7dc9 | 388 | if (Y_control.read() < Up_value){ |
Technical_Muffin | 0:34751b6a7dc9 | 389 | Diry.write(0); |
Technical_Muffin | 0:34751b6a7dc9 | 390 | Enabley.write(0); |
Technical_Muffin | 0:34751b6a7dc9 | 391 | } |
Technical_Muffin | 0:34751b6a7dc9 | 392 | else if (Y_control.read() > Down_value){ |
Technical_Muffin | 0:34751b6a7dc9 | 393 | Diry.write(1); |
Technical_Muffin | 0:34751b6a7dc9 | 394 | Enabley.write(0); |
Technical_Muffin | 0:34751b6a7dc9 | 395 | } |
Technical_Muffin | 0:34751b6a7dc9 | 396 | else{ |
Technical_Muffin | 0:34751b6a7dc9 | 397 | Enabley.write(1); |
Technical_Muffin | 0:34751b6a7dc9 | 398 | } |
Technical_Muffin | 0:34751b6a7dc9 | 399 | pc.printf("X value is %f and Y value is %f\n", X_control.read(), Y_control.read()); |
Technical_Muffin | 0:34751b6a7dc9 | 400 | }*/ |
Technical_Muffin | 0:34751b6a7dc9 | 401 | |
Technical_Muffin | 0:34751b6a7dc9 | 402 | //biceps |
Technical_Muffin | 0:34751b6a7dc9 | 403 | arm_biquad_cascade_df1_init_f32(&lowpass1_biceps, 1 , lowpass1_const, lowpass1_biceps_states); |
Technical_Muffin | 0:34751b6a7dc9 | 404 | arm_biquad_cascade_df1_init_f32(&lowpass2_biceps, 1 , lowpass2_const, lowpass2_biceps_states); |
Technical_Muffin | 0:34751b6a7dc9 | 405 | arm_biquad_cascade_df1_init_f32(&highpass_biceps, 1 , highpass_const, highpass_biceps_states); |
Technical_Muffin | 1:c8ad338ba312 | 406 | arm_biquad_cascade_df1_init_f32(&bandstop_biceps, 2 , bandstop_const, bandstop_biceps_states); |
Technical_Muffin | 0:34751b6a7dc9 | 407 | //triceps |
Technical_Muffin | 0:34751b6a7dc9 | 408 | arm_biquad_cascade_df1_init_f32(&lowpass1_triceps, 1 , lowpass1_const, lowpass1_triceps_states); |
Technical_Muffin | 0:34751b6a7dc9 | 409 | arm_biquad_cascade_df1_init_f32(&lowpass2_triceps, 1 , lowpass2_const, lowpass2_triceps_states); |
Technical_Muffin | 1:c8ad338ba312 | 410 | arm_biquad_cascade_df1_init_f32(&highpass_triceps, 1 , highpass_const, highpass_triceps_states); |
Technical_Muffin | 1:c8ad338ba312 | 411 | arm_biquad_cascade_df1_init_f32(&bandstop_triceps, 2 , bandstop_const, bandstop_triceps_states); |
Technical_Muffin | 1:c8ad338ba312 | 412 | //pectoralis major |
Technical_Muffin | 0:34751b6a7dc9 | 413 | arm_biquad_cascade_df1_init_f32(&lowpass1_pect, 1 , lowpass1_const, lowpass1_pect_states); |
Technical_Muffin | 0:34751b6a7dc9 | 414 | arm_biquad_cascade_df1_init_f32(&lowpass2_pect, 1 , lowpass2_const, lowpass2_pect_states); |
Technical_Muffin | 0:34751b6a7dc9 | 415 | arm_biquad_cascade_df1_init_f32(&highpass_pect, 1 , highpass_const, highpass_pect_states); |
Technical_Muffin | 1:c8ad338ba312 | 416 | arm_biquad_cascade_df1_init_f32(&bandstop_pect, 2 , bandstop_const, bandstop_pect_states); |
Technical_Muffin | 1:c8ad338ba312 | 417 | |
Technical_Muffin | 0:34751b6a7dc9 | 418 | //deltoid |
Technical_Muffin | 0:34751b6a7dc9 | 419 | arm_biquad_cascade_df1_init_f32(&lowpass1_deltoid, 1 , lowpass1_const, lowpass1_deltoid_states); |
Technical_Muffin | 0:34751b6a7dc9 | 420 | arm_biquad_cascade_df1_init_f32(&lowpass2_deltoid, 1 , lowpass2_const, lowpass2_deltoid_states); |
Technical_Muffin | 1:c8ad338ba312 | 421 | arm_biquad_cascade_df1_init_f32(&highpass_deltoid, 1 , highpass_const, highpass_deltoid_states); |
Technical_Muffin | 1:c8ad338ba312 | 422 | arm_biquad_cascade_df1_init_f32(&bandstop_deltoid, 2 , bandstop_const, bandstop_deltoid_states); |
Technical_Muffin | 1:c8ad338ba312 | 423 | |
Technical_Muffin | 0:34751b6a7dc9 | 424 | emgtimer.attach(looper_emg, 0.01); |
Technical_Muffin | 0:34751b6a7dc9 | 425 | |
Technical_Muffin | 1:c8ad338ba312 | 426 | looptimer1.attach(looper_motorx, 0.01); //X-Spindle motor |
Technical_Muffin | 0:34751b6a7dc9 | 427 | |
Technical_Muffin | 5:19f8766b63da | 428 | looptimer2.attach(looper_motory, 0.01); //Y-Spindle motor |
Technical_Muffin | 0:34751b6a7dc9 | 429 | |
Technical_Muffin | 0:34751b6a7dc9 | 430 | while(1){}; |
Technical_Muffin | 0:34751b6a7dc9 | 431 | } |