K K / Mbed 2 deprecated revised_XY_table_code

Code for controlling the Magna table

Dependencies:   HIDScope MODSERIAL mbed-dsp mbed

main.cpp@4:61bdf601e7b0, 2016-01-08 (annotated)

Committer:
Technical_Muffin
Date:
Fri Jan 08 11:55:49 2016 +0000
Revision:
4:61bdf601e7b0
Parent:
3:3d5ad874add0
Child:
5:19f8766b63da
EMG control by comparative signal calculation, not using speed control as signal is probably to small and inconsistent

Who changed what in which revision?

UserRevisionLine numberNew 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 1:c8ad338ba312 95
Technical_Muffin 1:c8ad338ba312 96 //lowpass 1 filter settings: Fc = 49 Hz, Fs = 100 Hz, Gain = -3 dB
Technical_Muffin 1:c8ad338ba312 97 //lowpass 2 filter settings: Fc = 0.8 Hz, Fs = 100 Hz, Gain = -3 dB
Technical_Muffin 1:c8ad338ba312 98 float lowpass1_const[] = {0.956543225556877,1.91308645111375,0.956543225556877,-1.91119706742607,-0.914975834801434};
Technical_Muffin 1:c8ad338ba312 99 float lowpass2_const[] = {0.000609854718717299,0.00121970943743460,0.000609854718717299,1.92894226325203,-0.931381682126903};
Technical_Muffin 1:c8ad338ba312 100 //highpass filter settings: Fc = 20 Hz, Fs = 100 Hz
Technical_Muffin 1:c8ad338ba312 101 float highpass_const[] = {0.391335772501769,-0.782671545003538,0.391335772501769,0.369527377351241,-0.195815712655833};
Technical_Muffin 1:c8ad338ba312 102 //bandstop filter settings Fc=[29Hz 36Hz], Fs= 100Hz
Technical_Muffin 1:c8ad338ba312 103 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 104
Technical_Muffin 1:c8ad338ba312 105 /*
Technical_Muffin 1:c8ad338ba312 106 //values are usable for triceps and biceps continuous motion, not for wrist motion.
Technical_Muffin 0:34751b6a7dc9 107 //lowpass 1 filter settings: Fc = 45 Hz, Fs = 100 Hz, Gain = -3 dB
Technical_Muffin 0:34751b6a7dc9 108 //lowpass 2 filter settings: Fc = 0.3 Hz, Fs = 100 Hz, Gain = -3 dB
Technical_Muffin 0:34751b6a7dc9 109 float lowpass1_const[] = {0.800592403464570,1.60118480692914,0.800592403464570,-1.56101807580072,-0.641351538057563};
Technical_Muffin 0:34751b6a7dc9 110 float lowpass2_const[] = {8.76555487540147e-05, 0.000175311097508029, 8.76555487540147e-05 , 1.97334424978130, -0.973694871976315};
Technical_Muffin 0:34751b6a7dc9 111 //highpass filter settings: Fc = 20 Hz, Fs = 100 Hz
Technical_Muffin 0:34751b6a7dc9 112
Technical_Muffin 0:34751b6a7dc9 113 float highpass_const[] = {0.391335772501769 ,-0.782671545003538, 0.391335772501769,0.369527377351241, -0.195815712655833};
Technical_Muffin 0:34751b6a7dc9 114
Technical_Muffin 1:c8ad338ba312 115 */
Technical_Muffin 0:34751b6a7dc9 116 /*
Technical_Muffin 0:34751b6a7dc9 117 //suffer from high wire motion influence and dont react to continuous tension only rapid activation
Technical_Muffin 0:34751b6a7dc9 118
Technical_Muffin 0:34751b6a7dc9 119 //lowpass 1 filter settings: Fc = 20 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.206572083826148,0.413144167652296,0.206572083826148,0.369527377351241,-0.195815712655833};
Technical_Muffin 0:34751b6a7dc9 122 float lowpass2_const[] = {0.0000876555487540065, 0.000175311097508013, 0.0000876555487540065, 1.97334424978130, -0.973694871976315};
Technical_Muffin 0:34751b6a7dc9 123 //highpass filter settings: Fc = 3 Hz, Fs = 100 Hz
Technical_Muffin 0:34751b6a7dc9 124
Technical_Muffin 0:34751b6a7dc9 125 float highpass_const[] = {0.875183092438135,-1.75036618487627,0.875183092438135,1.73472576880928,-0.766006600943264};
Technical_Muffin 0:34751b6a7dc9 126 */
Technical_Muffin 0:34751b6a7dc9 127
Technical_Muffin 0:34751b6a7dc9 128 /*
Technical_Muffin 0:34751b6a7dc9 129 Previous coefficients testing to see if new variants work
Technical_Muffin 0:34751b6a7dc9 130 lowpass 1 filter settings: Fc = 45 Hz, Fs = 100 Hz, Gain = -3 dB
Technical_Muffin 0:34751b6a7dc9 131 lowpass 2 filter settings: Fc = 2 Hz, Fs = 100 Hz, Gain = -3 dB
Technical_Muffin 0:34751b6a7dc9 132 float lowpass1_const[] = {0.800592403464570, 1.60118480692914, 0.800592403464570, -1.56101807580072, -0.641351538057563};
Technical_Muffin 0:34751b6a7dc9 133 float lowpass2_const[] = {0.0000876555487540065, 0.000175311097508013, 0.0000876555487540065, 1.97334424978130, -0.973694871976315};
Technical_Muffin 0:34751b6a7dc9 134 //highpass filter settings: Fc = 20 Hz, Fs = 100 Hz
Technical_Muffin 0:34751b6a7dc9 135
Technical_Muffin 0:34751b6a7dc9 136 float highpass_const[] = {0.391335772501769, -0.782671545003537,0.391335772501769,0.369527377351241,-0.195815712655833 };
Technical_Muffin 0:34751b6a7dc9 137 */
Technical_Muffin 0:34751b6a7dc9 138
Technical_Muffin 0:34751b6a7dc9 139 //state values
Technical_Muffin 0:34751b6a7dc9 140 float lowpass1_biceps_states[4];
Technical_Muffin 0:34751b6a7dc9 141 float lowpass2_biceps_states[4];
Technical_Muffin 0:34751b6a7dc9 142 float highpass_biceps_states[4];
Technical_Muffin 1:c8ad338ba312 143 float bandstop_biceps_states[8];
Technical_Muffin 1:c8ad338ba312 144
Technical_Muffin 0:34751b6a7dc9 145 float lowpass1_triceps_states[4];
Technical_Muffin 0:34751b6a7dc9 146 float lowpass2_triceps_states[4];
Technical_Muffin 0:34751b6a7dc9 147 float highpass_triceps_states[4];
Technical_Muffin 1:c8ad338ba312 148 float bandstop_triceps_states[8];
Technical_Muffin 1:c8ad338ba312 149
Technical_Muffin 0:34751b6a7dc9 150 float lowpass1_pect_states[4];
Technical_Muffin 0:34751b6a7dc9 151 float lowpass2_pect_states[4];
Technical_Muffin 0:34751b6a7dc9 152 float highpass_pect_states[4];
Technical_Muffin 1:c8ad338ba312 153 float bandstop_pect_states[8];
Technical_Muffin 1:c8ad338ba312 154
Technical_Muffin 0:34751b6a7dc9 155 float lowpass1_deltoid_states[4];
Technical_Muffin 0:34751b6a7dc9 156 float lowpass2_deltoid_states[4];
Technical_Muffin 0:34751b6a7dc9 157 float highpass_deltoid_states[4];
Technical_Muffin 1:c8ad338ba312 158 float bandstop_deltoid_states[8];
Technical_Muffin 1:c8ad338ba312 159
Technical_Muffin 0:34751b6a7dc9 160
Technical_Muffin 4:61bdf601e7b0 161 //global variables
Technical_Muffin 0:34751b6a7dc9 162 float filtered_biceps, filtered_triceps, filtered_pect, filtered_deltoid;
Technical_Muffin 0:34751b6a7dc9 163 float speed_old1, speed_old2, speed_old3, speed_old4;
Technical_Muffin 0:34751b6a7dc9 164 float acc1, acc2, acc3, acc4;
Technical_Muffin 0:34751b6a7dc9 165 float force1, force2, force3, force4;
Technical_Muffin 0:34751b6a7dc9 166 float speed1, speed2, speed3, speed4;
Technical_Muffin 0:34751b6a7dc9 167 float damping1, damping2, damping3, damping4;
Technical_Muffin 0:34751b6a7dc9 168 float emg_x, emg_y;
Technical_Muffin 0:34751b6a7dc9 169 float cx = 0;
Technical_Muffin 0:34751b6a7dc9 170 float cy = 0;
Technical_Muffin 0:34751b6a7dc9 171 float errorx = 0.2;
Technical_Muffin 0:34751b6a7dc9 172 float errory = 0.2;
Technical_Muffin 0:34751b6a7dc9 173 float Ps_x = 0;
Technical_Muffin 0:34751b6a7dc9 174 float Ps_y = 0;
Technical_Muffin 0:34751b6a7dc9 175 float hstep_freqx = 1;
Technical_Muffin 0:34751b6a7dc9 176 float hstep_freqy = 1;
Technical_Muffin 0:34751b6a7dc9 177 float emg_y_abs = 0;
Technical_Muffin 0:34751b6a7dc9 178 float emg_x_abs = 0;
Technical_Muffin 0:34751b6a7dc9 179
Technical_Muffin 0:34751b6a7dc9 180 void looper_emg()
Technical_Muffin 0:34751b6a7dc9 181 {
Technical_Muffin 0:34751b6a7dc9 182 float emg_value1_f32, emg_value2_f32, emg_value3_f32, emg_value4_f32;
Technical_Muffin 0:34751b6a7dc9 183 emg_value1_f32 = emg1.read();
Technical_Muffin 0:34751b6a7dc9 184 emg_value2_f32 = emg2.read();
Technical_Muffin 0:34751b6a7dc9 185 emg_value3_f32 = emg3.read();
Technical_Muffin 0:34751b6a7dc9 186 emg_value4_f32 = emg4.read();
Technical_Muffin 0:34751b6a7dc9 187
Technical_Muffin 4:61bdf601e7b0 188 //Biquad process emg biceps
Technical_Muffin 1:c8ad338ba312 189 arm_biquad_cascade_df1_f32(&bandstop_biceps, &emg_value1_f32, &filtered_biceps, 1 );//used for wrist motion
Technical_Muffin 1:c8ad338ba312 190 arm_biquad_cascade_df1_f32(&lowpass1_biceps, &filtered_biceps, &filtered_biceps, 1 );
Technical_Muffin 0:34751b6a7dc9 191 arm_biquad_cascade_df1_f32(&highpass_biceps, &filtered_biceps, &filtered_biceps, 1 );
Technical_Muffin 0:34751b6a7dc9 192 filtered_biceps = fabs(filtered_biceps); //Rectifier, The Gain is already implemented.
Technical_Muffin 0:34751b6a7dc9 193 arm_biquad_cascade_df1_f32(&lowpass2_biceps, &filtered_biceps, &filtered_biceps, 1 ); //low pass filter
Technical_Muffin 0:34751b6a7dc9 194
Technical_Muffin 4:61bdf601e7b0 195 //Biquad process emg triceps
Technical_Muffin 1:c8ad338ba312 196 arm_biquad_cascade_df1_f32(&bandstop_triceps, &emg_value2_f32, &filtered_triceps, 1 ); //used for wrist motion
Technical_Muffin 1:c8ad338ba312 197 arm_biquad_cascade_df1_f32(&lowpass1_triceps, &filtered_triceps, &filtered_triceps, 1 );
Technical_Muffin 0:34751b6a7dc9 198 arm_biquad_cascade_df1_f32(&highpass_triceps, &filtered_triceps, &filtered_triceps, 1 );
Technical_Muffin 0:34751b6a7dc9 199 filtered_triceps = fabs(filtered_triceps);
Technical_Muffin 0:34751b6a7dc9 200 arm_biquad_cascade_df1_f32(&lowpass2_triceps, &filtered_triceps, &filtered_triceps, 1 );
Technical_Muffin 0:34751b6a7dc9 201
Technical_Muffin 4:61bdf601e7b0 202 //Biquad process emg pectoralis major
Technical_Muffin 1:c8ad338ba312 203 arm_biquad_cascade_df1_f32(&bandstop_pect, &emg_value3_f32, &filtered_pect, 1 );//used for wrist motion
Technical_Muffin 1:c8ad338ba312 204 arm_biquad_cascade_df1_f32(&lowpass1_pect, &filtered_pect, &filtered_pect, 1 );
Technical_Muffin 0:34751b6a7dc9 205 arm_biquad_cascade_df1_f32(&highpass_pect, &filtered_pect, &filtered_pect, 1 );
Technical_Muffin 1:c8ad338ba312 206 filtered_pect = fabs(filtered_pect);
Technical_Muffin 1:c8ad338ba312 207 arm_biquad_cascade_df1_f32(&lowpass2_pect, &filtered_pect, &filtered_pect, 1 );
Technical_Muffin 0:34751b6a7dc9 208
Technical_Muffin 4:61bdf601e7b0 209 //Biquad process emg deltoid
Technical_Muffin 1:c8ad338ba312 210 arm_biquad_cascade_df1_f32(&bandstop_deltoid, &emg_value4_f32, &filtered_deltoid, 1 );//used for wrist motion
Technical_Muffin 1:c8ad338ba312 211 arm_biquad_cascade_df1_f32(&lowpass1_deltoid, &filtered_deltoid, &filtered_deltoid, 1 );
Technical_Muffin 0:34751b6a7dc9 212 arm_biquad_cascade_df1_f32(&highpass_deltoid, &filtered_deltoid, &filtered_deltoid, 1 );
Technical_Muffin 1:c8ad338ba312 213 filtered_deltoid = fabs(filtered_deltoid);
Technical_Muffin 1:c8ad338ba312 214 arm_biquad_cascade_df1_f32(&lowpass2_deltoid, &filtered_deltoid, &filtered_deltoid, 1 );
Technical_Muffin 1:c8ad338ba312 215
Technical_Muffin 4:61bdf601e7b0 216 // send value to PC for control
Technical_Muffin 0:34751b6a7dc9 217
Technical_Muffin 3:3d5ad874add0 218 scope.set(0,filtered_biceps); //Filtered EMG signals
Technical_Muffin 3:3d5ad874add0 219 scope.set(1,filtered_triceps);
Technical_Muffin 3:3d5ad874add0 220 scope.set(2,filtered_pect);
Technical_Muffin 3:3d5ad874add0 221 scope.set(3,filtered_deltoid);
Technical_Muffin 0:34751b6a7dc9 222 scope.send();
Technical_Muffin 0:34751b6a7dc9 223
Technical_Muffin 0:34751b6a7dc9 224 }
Technical_Muffin 0:34751b6a7dc9 225
Technical_Muffin 0:34751b6a7dc9 226
Technical_Muffin 0:34751b6a7dc9 227 void looper_motory()
Technical_Muffin 0:34751b6a7dc9 228 {
Technical_Muffin 0:34751b6a7dc9 229
Technical_Muffin 4:61bdf601e7b0 230 emg_y = (filtered_biceps - filtered_triceps);
Technical_Muffin 4:61bdf601e7b0 231 /*emg_y_abs = fabs(emg_y);
Technical_Muffin 0:34751b6a7dc9 232 force1 = emg_y_abs*K_Gain;
Technical_Muffin 0:34751b6a7dc9 233 force1 = force1 - damping1;
Technical_Muffin 0:34751b6a7dc9 234 acc1 = force1/Mass;
Technical_Muffin 0:34751b6a7dc9 235 speed1 = speed_old1 + (acc1 * dt);
Technical_Muffin 0:34751b6a7dc9 236 damping1 = speed1 * Damp;
Technical_Muffin 0:34751b6a7dc9 237 step_freq1 = setpoint * speed1;*/
Technical_Muffin 0:34751b6a7dc9 238 // Stepy.period(/*1.0/step_freq1*/0.000625);
Technical_Muffin 0:34751b6a7dc9 239 //speed_old1 = speed1;
Technical_Muffin 4:61bdf601e7b0 240
Technical_Muffin 4:61bdf601e7b0 241 if (emg_y > 0) {
Technical_Muffin 4:61bdf601e7b0 242 Diry = 1;
Technical_Muffin 4:61bdf601e7b0 243 }
Technical_Muffin 4:61bdf601e7b0 244 if (emg_y < 0) {
Technical_Muffin 4:61bdf601e7b0 245 Diry = 0;
Technical_Muffin 4:61bdf601e7b0 246 }
Technical_Muffin 4:61bdf601e7b0 247 /*//Speed limit
Technical_Muffin 4:61bdf601e7b0 248 if (speed2 > 1) {
Technical_Muffin 4:61bdf601e7b0 249 speed2 = 1;
Technical_Muffin 4:61bdf601e7b0 250 step_freq2 = setpoint;
Technical_Muffin 4:61bdf601e7b0 251 }*/
Technical_Muffin 4:61bdf601e7b0 252 //EMG treshold
Technical_Muffin 4:61bdf601e7b0 253 if (filtered_biceps < EMG_tresh1 && filtered_triceps < EMG_tresh2) {
Technical_Muffin 4:61bdf601e7b0 254 Enabley = 1; //Enable = 1 turns the motor off.
Technical_Muffin 4:61bdf601e7b0 255 } else {
Technical_Muffin 4:61bdf601e7b0 256 Enabley = 0;
Technical_Muffin 4:61bdf601e7b0 257 }
Technical_Muffin 1:c8ad338ba312 258
Technical_Muffin 1:c8ad338ba312 259 //0.06 for biceps and triceps movement, 0.04 for wrist lower and upper motion right arm lower2 connections
Technical_Muffin 4:61bdf601e7b0 260 /* if (filtered_triceps > 0.04) {//upward cart movement
Technical_Muffin 0:34751b6a7dc9 261 Diry.write(1);
Technical_Muffin 0:34751b6a7dc9 262 Enabley.write(0); //Enable = 1 turns the motor off.
Technical_Muffin 0:34751b6a7dc9 263 Ledr.write(0);
Technical_Muffin 0:34751b6a7dc9 264 Ledb.write(1);
Technical_Muffin 0:34751b6a7dc9 265 }
Technical_Muffin 1:c8ad338ba312 266 else if (filtered_biceps > 0.04) {// downward cart movement
Technical_Muffin 0:34751b6a7dc9 267 Diry.write(0);
Technical_Muffin 0:34751b6a7dc9 268 Enabley.write(0); //Enable = 1 turns the motor off.
Technical_Muffin 0:34751b6a7dc9 269 Ledr.write(1);
Technical_Muffin 0:34751b6a7dc9 270 Ledb.write(0);
Technical_Muffin 0:34751b6a7dc9 271 }
Technical_Muffin 1:c8ad338ba312 272 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 273 Enabley.write(1);
Technical_Muffin 0:34751b6a7dc9 274 }
Technical_Muffin 0:34751b6a7dc9 275 else{}
Technical_Muffin 0:34751b6a7dc9 276 //Speed limit
Technical_Muffin 4:61bdf601e7b0 277 if (speed1 > 1) {
Technical_Muffin 0:34751b6a7dc9 278 speed1 = 1;
Technical_Muffin 0:34751b6a7dc9 279 step_freq1 = setpoint;
Technical_Muffin 0:34751b6a7dc9 280 }*/
Technical_Muffin 0:34751b6a7dc9 281
Technical_Muffin 0:34751b6a7dc9 282 }
Technical_Muffin 1:c8ad338ba312 283
Technical_Muffin 0:34751b6a7dc9 284 void looper_motorx()
Technical_Muffin 0:34751b6a7dc9 285 {
Technical_Muffin 0:34751b6a7dc9 286
Technical_Muffin 3:3d5ad874add0 287 emg_x = (filtered_pect - filtered_deltoid);
Technical_Muffin 3:3d5ad874add0 288 /*emg_x_abs = fabs(emg_x);
Technical_Muffin 0:34751b6a7dc9 289 force2 = emg_x_abs*K_Gain;
Technical_Muffin 0:34751b6a7dc9 290 force2 = force2 - damping2;
Technical_Muffin 0:34751b6a7dc9 291 acc2 = force2/Mass;
Technical_Muffin 0:34751b6a7dc9 292 speed2 = speed_old2 + (acc2 * dt);
Technical_Muffin 0:34751b6a7dc9 293 damping2 = speed2 * Damp;
Technical_Muffin 0:34751b6a7dc9 294 step_freq2 = setpoint * speed2;
Technical_Muffin 0:34751b6a7dc9 295 Stepx.period(1.0/step_freq2);
Technical_Muffin 0:34751b6a7dc9 296 speed_old2 = speed2;
Technical_Muffin 3:3d5ad874add0 297 */
Technical_Muffin 0:34751b6a7dc9 298 if (emg_x > 0) {
Technical_Muffin 0:34751b6a7dc9 299 Dirx = 1;
Technical_Muffin 0:34751b6a7dc9 300 }
Technical_Muffin 0:34751b6a7dc9 301 if (emg_x < 0) {
Technical_Muffin 0:34751b6a7dc9 302 Dirx = 0;
Technical_Muffin 0:34751b6a7dc9 303 }
Technical_Muffin 3:3d5ad874add0 304 /*//Speed limit
Technical_Muffin 0:34751b6a7dc9 305 if (speed2 > 1) {
Technical_Muffin 0:34751b6a7dc9 306 speed2 = 1;
Technical_Muffin 0:34751b6a7dc9 307 step_freq2 = setpoint;
Technical_Muffin 3:3d5ad874add0 308 }*/
Technical_Muffin 0:34751b6a7dc9 309 //EMG treshold
Technical_Muffin 0:34751b6a7dc9 310 if (filtered_pect < EMG_tresh3 && filtered_deltoid < EMG_tresh4) {
Technical_Muffin 0:34751b6a7dc9 311 Enablex = 1; //Enable = 1 turns the motor off.
Technical_Muffin 0:34751b6a7dc9 312 } else {
Technical_Muffin 0:34751b6a7dc9 313 Enablex = 0;
Technical_Muffin 0:34751b6a7dc9 314 }
Technical_Muffin 3:3d5ad874add0 315 /*
Technical_Muffin 1:c8ad338ba312 316 //0.06 for biceps and triceps movement, 0.03 for wrist lower and upper motion left arm upper 2 connections
Technical_Muffin 1:c8ad338ba312 317 if (filtered_pect > 0.03) {//right cart movement
Technical_Muffin 1:c8ad338ba312 318 Dirx.write(1);
Technical_Muffin 1:c8ad338ba312 319 Enablex.write(0); //Enable = 1 turns the motor off.
Technical_Muffin 1:c8ad338ba312 320 Ledr.write(0);
Technical_Muffin 1:c8ad338ba312 321 Ledb.write(1);
Technical_Muffin 1:c8ad338ba312 322 }
Technical_Muffin 1:c8ad338ba312 323 else if (filtered_deltoid > 0.03) {//left cart movement
Technical_Muffin 1:c8ad338ba312 324 Dirx.write(0);
Technical_Muffin 1:c8ad338ba312 325 Enablex.write(0); //Enable = 1 turns the motor off.
Technical_Muffin 1:c8ad338ba312 326 Ledr.write(1);
Technical_Muffin 1:c8ad338ba312 327 Ledb.write(0);
Technical_Muffin 1:c8ad338ba312 328 }
Technical_Muffin 1:c8ad338ba312 329 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 330 Enablex.write(1);
Technical_Muffin 1:c8ad338ba312 331 }
Technical_Muffin 4:61bdf601e7b0 332 else{}*/
Technical_Muffin 4:61bdf601e7b0 333 }
Technical_Muffin 0:34751b6a7dc9 334
Technical_Muffin 0:34751b6a7dc9 335 int main()
Technical_Muffin 0:34751b6a7dc9 336 {
Technical_Muffin 0:34751b6a7dc9 337
Technical_Muffin 0:34751b6a7dc9 338 //scopeTimer.attach_us(&scopeSend, 2e3);
Technical_Muffin 0:34751b6a7dc9 339 pc.baud(115200);
Technical_Muffin 0:34751b6a7dc9 340 Ledr=1;
Technical_Muffin 0:34751b6a7dc9 341 Ledg=1;
Technical_Muffin 0:34751b6a7dc9 342 Ledb=1;
Technical_Muffin 0:34751b6a7dc9 343
Technical_Muffin 0:34751b6a7dc9 344 MS11=1;//higher microstepping in combination with a higher step frequency reduces the vibration significantly
Technical_Muffin 0:34751b6a7dc9 345 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 346 MS31=1;
Technical_Muffin 0:34751b6a7dc9 347 MS12=1;
Technical_Muffin 0:34751b6a7dc9 348 MS22=1;
Technical_Muffin 0:34751b6a7dc9 349 MS32=1;
Technical_Muffin 1:c8ad338ba312 350 Stepy.write(0.5f);
Technical_Muffin 0:34751b6a7dc9 351 Enabley.write(1);
Technical_Muffin 0:34751b6a7dc9 352 Stepx.write(0.5f);
Technical_Muffin 0:34751b6a7dc9 353 Enablex.write(1);
Technical_Muffin 0:34751b6a7dc9 354 Stepy.period(0.000625);//use period change for speed adjustments
Technical_Muffin 0:34751b6a7dc9 355 Stepx.period(0.000625);//frequency of 1600 Hz
Technical_Muffin 0:34751b6a7dc9 356
Technical_Muffin 2:083d74325bfb 357
Technical_Muffin 2:083d74325bfb 358 /*//code for controlling the mechanism with a joystick
Technical_Muffin 0:34751b6a7dc9 359 float Left_value = 0.6;
Technical_Muffin 0:34751b6a7dc9 360 float Right_value = 0.9;
Technical_Muffin 0:34751b6a7dc9 361 float Up_value = 0.6;
Technical_Muffin 0:34751b6a7dc9 362 float Down_value = 0.9;
Technical_Muffin 2:083d74325bfb 363
Technical_Muffin 2:083d74325bfb 364 while(1){
Technical_Muffin 0:34751b6a7dc9 365 if (X_control.read() < Left_value){
Technical_Muffin 0:34751b6a7dc9 366 Dirx.write(0);
Technical_Muffin 0:34751b6a7dc9 367 Enablex.write(0);
Technical_Muffin 0:34751b6a7dc9 368 }
Technical_Muffin 0:34751b6a7dc9 369 else if (X_control.read() > Right_value){
Technical_Muffin 0:34751b6a7dc9 370 Dirx.write(1);
Technical_Muffin 0:34751b6a7dc9 371 Enablex.write(0);
Technical_Muffin 0:34751b6a7dc9 372 }
Technical_Muffin 0:34751b6a7dc9 373 else{
Technical_Muffin 0:34751b6a7dc9 374 Enablex.write(1);
Technical_Muffin 0:34751b6a7dc9 375 }
Technical_Muffin 0:34751b6a7dc9 376 if (Y_control.read() < Up_value){
Technical_Muffin 0:34751b6a7dc9 377 Diry.write(0);
Technical_Muffin 0:34751b6a7dc9 378 Enabley.write(0);
Technical_Muffin 0:34751b6a7dc9 379 }
Technical_Muffin 0:34751b6a7dc9 380 else if (Y_control.read() > Down_value){
Technical_Muffin 0:34751b6a7dc9 381 Diry.write(1);
Technical_Muffin 0:34751b6a7dc9 382 Enabley.write(0);
Technical_Muffin 0:34751b6a7dc9 383 }
Technical_Muffin 0:34751b6a7dc9 384 else{
Technical_Muffin 0:34751b6a7dc9 385 Enabley.write(1);
Technical_Muffin 0:34751b6a7dc9 386 }
Technical_Muffin 0:34751b6a7dc9 387 pc.printf("X value is %f and Y value is %f\n", X_control.read(), Y_control.read());
Technical_Muffin 0:34751b6a7dc9 388 }*/
Technical_Muffin 0:34751b6a7dc9 389
Technical_Muffin 0:34751b6a7dc9 390 //biceps
Technical_Muffin 0:34751b6a7dc9 391 arm_biquad_cascade_df1_init_f32(&lowpass1_biceps, 1 , lowpass1_const, lowpass1_biceps_states);
Technical_Muffin 0:34751b6a7dc9 392 arm_biquad_cascade_df1_init_f32(&lowpass2_biceps, 1 , lowpass2_const, lowpass2_biceps_states);
Technical_Muffin 0:34751b6a7dc9 393 arm_biquad_cascade_df1_init_f32(&highpass_biceps, 1 , highpass_const, highpass_biceps_states);
Technical_Muffin 1:c8ad338ba312 394 arm_biquad_cascade_df1_init_f32(&bandstop_biceps, 2 , bandstop_const, bandstop_biceps_states);
Technical_Muffin 0:34751b6a7dc9 395 //triceps
Technical_Muffin 0:34751b6a7dc9 396 arm_biquad_cascade_df1_init_f32(&lowpass1_triceps, 1 , lowpass1_const, lowpass1_triceps_states);
Technical_Muffin 0:34751b6a7dc9 397 arm_biquad_cascade_df1_init_f32(&lowpass2_triceps, 1 , lowpass2_const, lowpass2_triceps_states);
Technical_Muffin 1:c8ad338ba312 398 arm_biquad_cascade_df1_init_f32(&highpass_triceps, 1 , highpass_const, highpass_triceps_states);
Technical_Muffin 1:c8ad338ba312 399 arm_biquad_cascade_df1_init_f32(&bandstop_triceps, 2 , bandstop_const, bandstop_triceps_states);
Technical_Muffin 1:c8ad338ba312 400 //pectoralis major
Technical_Muffin 0:34751b6a7dc9 401 arm_biquad_cascade_df1_init_f32(&lowpass1_pect, 1 , lowpass1_const, lowpass1_pect_states);
Technical_Muffin 0:34751b6a7dc9 402 arm_biquad_cascade_df1_init_f32(&lowpass2_pect, 1 , lowpass2_const, lowpass2_pect_states);
Technical_Muffin 0:34751b6a7dc9 403 arm_biquad_cascade_df1_init_f32(&highpass_pect, 1 , highpass_const, highpass_pect_states);
Technical_Muffin 1:c8ad338ba312 404 arm_biquad_cascade_df1_init_f32(&bandstop_pect, 2 , bandstop_const, bandstop_pect_states);
Technical_Muffin 1:c8ad338ba312 405
Technical_Muffin 0:34751b6a7dc9 406 //deltoid
Technical_Muffin 0:34751b6a7dc9 407 arm_biquad_cascade_df1_init_f32(&lowpass1_deltoid, 1 , lowpass1_const, lowpass1_deltoid_states);
Technical_Muffin 0:34751b6a7dc9 408 arm_biquad_cascade_df1_init_f32(&lowpass2_deltoid, 1 , lowpass2_const, lowpass2_deltoid_states);
Technical_Muffin 1:c8ad338ba312 409 arm_biquad_cascade_df1_init_f32(&highpass_deltoid, 1 , highpass_const, highpass_deltoid_states);
Technical_Muffin 1:c8ad338ba312 410 arm_biquad_cascade_df1_init_f32(&bandstop_deltoid, 2 , bandstop_const, bandstop_deltoid_states);
Technical_Muffin 1:c8ad338ba312 411
Technical_Muffin 0:34751b6a7dc9 412 emgtimer.attach(looper_emg, 0.01);
Technical_Muffin 0:34751b6a7dc9 413
Technical_Muffin 1:c8ad338ba312 414 looptimer1.attach(looper_motorx, 0.01); //X-Spindle motor
Technical_Muffin 0:34751b6a7dc9 415
Technical_Muffin 3:3d5ad874add0 416 //looptimer2.attach(looper_motory, 0.01); //Y-Spindle motor
Technical_Muffin 0:34751b6a7dc9 417
Technical_Muffin 0:34751b6a7dc9 418 while(1){};
Technical_Muffin 0:34751b6a7dc9 419 }