K K / Mbed 2 deprecated revised_XY_table_code

Code for controlling the Magna table

Dependencies:   HIDScope MODSERIAL mbed-dsp mbed

main.cpp@8:69b7085f5343, 2016-01-24 (annotated)

Committer:
Technical_Muffin
Date:
Sun Jan 24 10:47:09 2016 +0000
Revision:
8:69b7085f5343
Parent:
7:20757784f5bf 
Commented Magna table code

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 7:20757784f5bf 15 //factors proportional control
Technical_Muffin 7:20757784f5bf 16 #define K_Gain 150 //Gain of the filtered EMG signal
Technical_Muffin 7:20757784f5bf 17 #define Damp 5 //Deceleration of the motor
Technical_Muffin 7:20757784f5bf 18 #define Mass 1 // Mass value
Technical_Muffin 7:20757784f5bf 19 #define dt 0.01 //Sample frequency
Technical_Muffin 8:69b7085f5343 20 //threshold values to determine when the motor needs to stop
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
Technical_Muffin 8:69b7085f5343 26 //button for switching between proportional and precision speed control
Technical_Muffin 7:20757784f5bf 27 InterruptIn button1(PTC6);
Technical_Muffin 7:20757784f5bf 28
Technical_Muffin 0:34751b6a7dc9 29 MODSERIAL pc(USBTX,USBRX);
Technical_Muffin 0:34751b6a7dc9 30 //joystick control
Technical_Muffin 0:34751b6a7dc9 31 AnalogIn X_control(A1);
Technical_Muffin 0:34751b6a7dc9 32 AnalogIn Y_control(A0);
Technical_Muffin 8:69b7085f5343 33
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 8:69b7085f5343 52 //initializing lights for testing and notification purposes
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 8:69b7085f5343 65 Ticker emgtimer;
Technical_Muffin 8:69b7085f5343 66 Ticker looptimer1;
Technical_Muffin 8:69b7085f5343 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 8:69b7085f5343 74 //EMG filters
Technical_Muffin 8:69b7085f5343 75 //Lowpass1
Technical_Muffin 0:34751b6a7dc9 76 arm_biquad_casd_df1_inst_f32 lowpass1_biceps;
Technical_Muffin 0:34751b6a7dc9 77 arm_biquad_casd_df1_inst_f32 lowpass1_triceps;
Technical_Muffin 0:34751b6a7dc9 78 arm_biquad_casd_df1_inst_f32 lowpass1_pect;
Technical_Muffin 0:34751b6a7dc9 79 arm_biquad_casd_df1_inst_f32 lowpass1_deltoid;
Technical_Muffin 8:69b7085f5343 80 //lowpass2
Technical_Muffin 0:34751b6a7dc9 81 arm_biquad_casd_df1_inst_f32 lowpass2_biceps;
Technical_Muffin 0:34751b6a7dc9 82 arm_biquad_casd_df1_inst_f32 lowpass2_triceps;
Technical_Muffin 0:34751b6a7dc9 83 arm_biquad_casd_df1_inst_f32 lowpass2_pect;
Technical_Muffin 0:34751b6a7dc9 84 arm_biquad_casd_df1_inst_f32 lowpass2_deltoid;
Technical_Muffin 8:69b7085f5343 85 //highpass
Technical_Muffin 4:61bdf601e7b0 86 arm_biquad_casd_df1_inst_f32 highpass_biceps;
Technical_Muffin 4:61bdf601e7b0 87 arm_biquad_casd_df1_inst_f32 highpass_triceps;
Technical_Muffin 4:61bdf601e7b0 88 arm_biquad_casd_df1_inst_f32 highpass_pect;
Technical_Muffin 4:61bdf601e7b0 89 arm_biquad_casd_df1_inst_f32 highpass_deltoid;
Technical_Muffin 1:c8ad338ba312 90 //used as extra filter for wrist motion
Technical_Muffin 8:69b7085f5343 91 //bandstop
Technical_Muffin 1:c8ad338ba312 92 arm_biquad_casd_df1_inst_f32 bandstop_biceps;
Technical_Muffin 1:c8ad338ba312 93 arm_biquad_casd_df1_inst_f32 bandstop_triceps;
Technical_Muffin 1:c8ad338ba312 94 arm_biquad_casd_df1_inst_f32 bandstop_pect;
Technical_Muffin 1:c8ad338ba312 95 arm_biquad_casd_df1_inst_f32 bandstop_deltoid;
Technical_Muffin 1:c8ad338ba312 96
Technical_Muffin 1:c8ad338ba312 97 //lowpass 1 filter settings: Fc = 49 Hz, Fs = 100 Hz, Gain = -3 dB
Technical_Muffin 1:c8ad338ba312 98 //lowpass 2 filter settings: Fc = 0.8 Hz, Fs = 100 Hz, Gain = -3 dB
Technical_Muffin 1:c8ad338ba312 99 float lowpass1_const[] = {0.956543225556877,1.91308645111375,0.956543225556877,-1.91119706742607,-0.914975834801434};
Technical_Muffin 1:c8ad338ba312 100 float lowpass2_const[] = {0.000609854718717299,0.00121970943743460,0.000609854718717299,1.92894226325203,-0.931381682126903};
Technical_Muffin 1:c8ad338ba312 101 //highpass filter settings: Fc = 20 Hz, Fs = 100 Hz
Technical_Muffin 1:c8ad338ba312 102 float highpass_const[] = {0.391335772501769,-0.782671545003538,0.391335772501769,0.369527377351241,-0.195815712655833};
Technical_Muffin 1:c8ad338ba312 103 //bandstop filter settings Fc=[29Hz 36Hz], Fs= 100Hz
Technical_Muffin 1:c8ad338ba312 104 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 105
Technical_Muffin 1:c8ad338ba312 106 /*
Technical_Muffin 1:c8ad338ba312 107 //values are usable for triceps and biceps continuous motion, not for wrist motion.
Technical_Muffin 0:34751b6a7dc9 108 //lowpass 1 filter settings: Fc = 45 Hz, Fs = 100 Hz, Gain = -3 dB
Technical_Muffin 0:34751b6a7dc9 109 //lowpass 2 filter settings: Fc = 0.3 Hz, Fs = 100 Hz, Gain = -3 dB
Technical_Muffin 0:34751b6a7dc9 110 float lowpass1_const[] = {0.800592403464570,1.60118480692914,0.800592403464570,-1.56101807580072,-0.641351538057563};
Technical_Muffin 0:34751b6a7dc9 111 float lowpass2_const[] = {8.76555487540147e-05, 0.000175311097508029, 8.76555487540147e-05 , 1.97334424978130, -0.973694871976315};
Technical_Muffin 0:34751b6a7dc9 112 //highpass filter settings: Fc = 20 Hz, Fs = 100 Hz
Technical_Muffin 0:34751b6a7dc9 113
Technical_Muffin 0:34751b6a7dc9 114 float highpass_const[] = {0.391335772501769 ,-0.782671545003538, 0.391335772501769,0.369527377351241, -0.195815712655833};
Technical_Muffin 0:34751b6a7dc9 115
Technical_Muffin 1:c8ad338ba312 116 */
Technical_Muffin 0:34751b6a7dc9 117
Technical_Muffin 0:34751b6a7dc9 118
Technical_Muffin 8:69b7085f5343 119 //state values for filter initialization
Technical_Muffin 0:34751b6a7dc9 120 float lowpass1_biceps_states[4];
Technical_Muffin 0:34751b6a7dc9 121 float lowpass2_biceps_states[4];
Technical_Muffin 0:34751b6a7dc9 122 float highpass_biceps_states[4];
Technical_Muffin 1:c8ad338ba312 123 float bandstop_biceps_states[8];
Technical_Muffin 1:c8ad338ba312 124
Technical_Muffin 0:34751b6a7dc9 125 float lowpass1_triceps_states[4];
Technical_Muffin 0:34751b6a7dc9 126 float lowpass2_triceps_states[4];
Technical_Muffin 0:34751b6a7dc9 127 float highpass_triceps_states[4];
Technical_Muffin 1:c8ad338ba312 128 float bandstop_triceps_states[8];
Technical_Muffin 1:c8ad338ba312 129
Technical_Muffin 0:34751b6a7dc9 130 float lowpass1_pect_states[4];
Technical_Muffin 0:34751b6a7dc9 131 float lowpass2_pect_states[4];
Technical_Muffin 0:34751b6a7dc9 132 float highpass_pect_states[4];
Technical_Muffin 1:c8ad338ba312 133 float bandstop_pect_states[8];
Technical_Muffin 1:c8ad338ba312 134
Technical_Muffin 0:34751b6a7dc9 135 float lowpass1_deltoid_states[4];
Technical_Muffin 0:34751b6a7dc9 136 float lowpass2_deltoid_states[4];
Technical_Muffin 0:34751b6a7dc9 137 float highpass_deltoid_states[4];
Technical_Muffin 1:c8ad338ba312 138 float bandstop_deltoid_states[8];
Technical_Muffin 1:c8ad338ba312 139
Technical_Muffin 0:34751b6a7dc9 140
Technical_Muffin 4:61bdf601e7b0 141 //global variables
Technical_Muffin 0:34751b6a7dc9 142 float filtered_biceps, filtered_triceps, filtered_pect, filtered_deltoid;
Technical_Muffin 0:34751b6a7dc9 143 float speed_old1, speed_old2, speed_old3, speed_old4;
Technical_Muffin 0:34751b6a7dc9 144 float acc1, acc2, acc3, acc4;
Technical_Muffin 0:34751b6a7dc9 145 float force1, force2, force3, force4;
Technical_Muffin 0:34751b6a7dc9 146 float speed1, speed2, speed3, speed4;
Technical_Muffin 0:34751b6a7dc9 147 float damping1, damping2, damping3, damping4;
Technical_Muffin 0:34751b6a7dc9 148 float emg_x, emg_y;
Technical_Muffin 0:34751b6a7dc9 149 float emg_y_abs = 0;
Technical_Muffin 0:34751b6a7dc9 150 float emg_x_abs = 0;
Technical_Muffin 7:20757784f5bf 151 bool proportional = 1;
Technical_Muffin 8:69b7085f5343 152 int count = 0;
Technical_Muffin 0:34751b6a7dc9 153
Technical_Muffin 8:69b7085f5343 154 void looper_emg()//EMG filtering function
Technical_Muffin 0:34751b6a7dc9 155 {
Technical_Muffin 0:34751b6a7dc9 156 float emg_value1_f32, emg_value2_f32, emg_value3_f32, emg_value4_f32;
Technical_Muffin 8:69b7085f5343 157 emg_value1_f32 = emg1.read();//read out analog inputs for EMG value
Technical_Muffin 0:34751b6a7dc9 158 emg_value2_f32 = emg2.read();
Technical_Muffin 8:69b7085f5343 159 emg_value3_f32 = emg1.read();
Technical_Muffin 8:69b7085f5343 160 emg_value4_f32 = emg2.read();
Technical_Muffin 0:34751b6a7dc9 161
Technical_Muffin 4:61bdf601e7b0 162 //Biquad process emg biceps
Technical_Muffin 8:69b7085f5343 163 arm_biquad_cascade_df1_f32(&bandstop_biceps, &emg_value1_f32, &filtered_biceps, 1 );//bandstop filter used for wrist motion
Technical_Muffin 8:69b7085f5343 164 arm_biquad_cascade_df1_f32(&lowpass1_biceps, &filtered_biceps, &filtered_biceps, 1 );//first lowpass filter
Technical_Muffin 8:69b7085f5343 165 arm_biquad_cascade_df1_f32(&highpass_biceps, &filtered_biceps, &filtered_biceps, 1 );//highpass filter
Technical_Muffin 0:34751b6a7dc9 166 filtered_biceps = fabs(filtered_biceps); //Rectifier, The Gain is already implemented.
Technical_Muffin 8:69b7085f5343 167 arm_biquad_cascade_df1_f32(&lowpass2_biceps, &filtered_biceps, &filtered_biceps, 1 ); //second low pass filter
Technical_Muffin 0:34751b6a7dc9 168
Technical_Muffin 4:61bdf601e7b0 169 //Biquad process emg triceps
Technical_Muffin 1:c8ad338ba312 170 arm_biquad_cascade_df1_f32(&bandstop_triceps, &emg_value2_f32, &filtered_triceps, 1 ); //used for wrist motion
Technical_Muffin 1:c8ad338ba312 171 arm_biquad_cascade_df1_f32(&lowpass1_triceps, &filtered_triceps, &filtered_triceps, 1 );
Technical_Muffin 0:34751b6a7dc9 172 arm_biquad_cascade_df1_f32(&highpass_triceps, &filtered_triceps, &filtered_triceps, 1 );
Technical_Muffin 0:34751b6a7dc9 173 filtered_triceps = fabs(filtered_triceps);
Technical_Muffin 0:34751b6a7dc9 174 arm_biquad_cascade_df1_f32(&lowpass2_triceps, &filtered_triceps, &filtered_triceps, 1 );
Technical_Muffin 0:34751b6a7dc9 175
Technical_Muffin 4:61bdf601e7b0 176 //Biquad process emg pectoralis major
Technical_Muffin 1:c8ad338ba312 177 arm_biquad_cascade_df1_f32(&bandstop_pect, &emg_value3_f32, &filtered_pect, 1 );//used for wrist motion
Technical_Muffin 1:c8ad338ba312 178 arm_biquad_cascade_df1_f32(&lowpass1_pect, &filtered_pect, &filtered_pect, 1 );
Technical_Muffin 0:34751b6a7dc9 179 arm_biquad_cascade_df1_f32(&highpass_pect, &filtered_pect, &filtered_pect, 1 );
Technical_Muffin 1:c8ad338ba312 180 filtered_pect = fabs(filtered_pect);
Technical_Muffin 1:c8ad338ba312 181 arm_biquad_cascade_df1_f32(&lowpass2_pect, &filtered_pect, &filtered_pect, 1 );
Technical_Muffin 0:34751b6a7dc9 182
Technical_Muffin 4:61bdf601e7b0 183 //Biquad process emg deltoid
Technical_Muffin 1:c8ad338ba312 184 arm_biquad_cascade_df1_f32(&bandstop_deltoid, &emg_value4_f32, &filtered_deltoid, 1 );//used for wrist motion
Technical_Muffin 1:c8ad338ba312 185 arm_biquad_cascade_df1_f32(&lowpass1_deltoid, &filtered_deltoid, &filtered_deltoid, 1 );
Technical_Muffin 0:34751b6a7dc9 186 arm_biquad_cascade_df1_f32(&highpass_deltoid, &filtered_deltoid, &filtered_deltoid, 1 );
Technical_Muffin 1:c8ad338ba312 187 filtered_deltoid = fabs(filtered_deltoid);
Technical_Muffin 1:c8ad338ba312 188 arm_biquad_cascade_df1_f32(&lowpass2_deltoid, &filtered_deltoid, &filtered_deltoid, 1 );
Technical_Muffin 1:c8ad338ba312 189
Technical_Muffin 4:61bdf601e7b0 190 // send value to PC for control
Technical_Muffin 0:34751b6a7dc9 191
Technical_Muffin 3:3d5ad874add0 192 scope.set(0,filtered_biceps); //Filtered EMG signals
Technical_Muffin 3:3d5ad874add0 193 scope.set(1,filtered_triceps);
Technical_Muffin 3:3d5ad874add0 194 scope.set(2,filtered_pect);
Technical_Muffin 3:3d5ad874add0 195 scope.set(3,filtered_deltoid);
Technical_Muffin 0:34751b6a7dc9 196 scope.send();
Technical_Muffin 0:34751b6a7dc9 197
Technical_Muffin 0:34751b6a7dc9 198 }
Technical_Muffin 0:34751b6a7dc9 199
Technical_Muffin 8:69b7085f5343 200 //y motor control
Technical_Muffin 0:34751b6a7dc9 201 void looper_motory()
Technical_Muffin 0:34751b6a7dc9 202 {
Technical_Muffin 8:69b7085f5343 203 //determine which direction the motor will rotate
Technical_Muffin 4:61bdf601e7b0 204 emg_y = (filtered_biceps - filtered_triceps);
Technical_Muffin 8:69b7085f5343 205 //switch between proportional EMG control and pre-set speed control
Technical_Muffin 8:69b7085f5343 206 if(proportional==0)
Technical_Muffin 7:20757784f5bf 207 {
Technical_Muffin 8:69b7085f5343 208 //proportional control explained in the report
Technical_Muffin 8:69b7085f5343 209 Stepy.write(0.5);
Technical_Muffin 8:69b7085f5343 210 Ledr=0;//red light to make it clear which control method is used
Technical_Muffin 7:20757784f5bf 211 emg_y_abs = fabs(emg_y);
Technical_Muffin 7:20757784f5bf 212 force1 = emg_y_abs*K_Gain;
Technical_Muffin 7:20757784f5bf 213 force1 = force1 - damping1;
Technical_Muffin 7:20757784f5bf 214 acc1 = force1/Mass;
Technical_Muffin 7:20757784f5bf 215 speed1 = speed_old1 + (acc1 * dt);
Technical_Muffin 7:20757784f5bf 216 damping1 = speed1 * Damp;
Technical_Muffin 7:20757784f5bf 217 step_freq1 = setpoint * speed1;
Technical_Muffin 7:20757784f5bf 218 Stepy.period(1.0/step_freq1);
Technical_Muffin 7:20757784f5bf 219 speed_old1 = speed1;
Technical_Muffin 7:20757784f5bf 220
Technical_Muffin 7:20757784f5bf 221 //Speed limit
Technical_Muffin 7:20757784f5bf 222 if (speed1 > 1) {
Technical_Muffin 7:20757784f5bf 223 speed1 = 1;
Technical_Muffin 7:20757784f5bf 224 }
Technical_Muffin 8:69b7085f5343 225 }
Technical_Muffin 8:69b7085f5343 226 else{
Technical_Muffin 7:20757784f5bf 227 //precision control
Technical_Muffin 8:69b7085f5343 228 Ledr=1;
Technical_Muffin 7:20757784f5bf 229 Stepy.period(0.000625);//frequency of 1600 Hz
Technical_Muffin 7:20757784f5bf 230 }
Technical_Muffin 8:69b7085f5343 231
Technical_Muffin 6:d03e4fa3a2a5 232 if (emg_y > 0) {//downward movement
Technical_Muffin 5:19f8766b63da 233 Diry = 0;
Technical_Muffin 4:61bdf601e7b0 234 }
Technical_Muffin 6:d03e4fa3a2a5 235 if (emg_y < 0) {//upward movement
Technical_Muffin 5:19f8766b63da 236 Diry = 1;
Technical_Muffin 4:61bdf601e7b0 237 }
Technical_Muffin 6:d03e4fa3a2a5 238
Technical_Muffin 8:69b7085f5343 239 //EMG treshold, determine if signal is strong enough to start the motors
Technical_Muffin 4:61bdf601e7b0 240 if (filtered_biceps < EMG_tresh1 && filtered_triceps < EMG_tresh2) {
Technical_Muffin 4:61bdf601e7b0 241 Enabley = 1; //Enable = 1 turns the motor off.
Technical_Muffin 8:69b7085f5343 242 }
Technical_Muffin 8:69b7085f5343 243 else {
Technical_Muffin 8:69b7085f5343 244 Enabley = 0;//Enable = 0 turns the motor on.
Technical_Muffin 4:61bdf601e7b0 245 }
Technical_Muffin 0:34751b6a7dc9 246 }
Technical_Muffin 8:69b7085f5343 247 //same setup as with the y motor
Technical_Muffin 0:34751b6a7dc9 248 void looper_motorx()
Technical_Muffin 0:34751b6a7dc9 249 {
Technical_Muffin 3:3d5ad874add0 250 emg_x = (filtered_pect - filtered_deltoid);
Technical_Muffin 8:69b7085f5343 251 if(proportional==0)
Technical_Muffin 7:20757784f5bf 252 {
Technical_Muffin 7:20757784f5bf 253 //proportional control
Technical_Muffin 8:69b7085f5343 254 Stepx.write(0.5);
Technical_Muffin 7:20757784f5bf 255 Ledr=0;
Technical_Muffin 7:20757784f5bf 256 emg_x_abs = fabs(emg_x);
Technical_Muffin 7:20757784f5bf 257 force2 = emg_x_abs*K_Gain;
Technical_Muffin 7:20757784f5bf 258 force2 = force2 - damping2;
Technical_Muffin 7:20757784f5bf 259 acc2 = force2/Mass;
Technical_Muffin 7:20757784f5bf 260 speed2 = speed_old2 + (acc2 * dt);
Technical_Muffin 7:20757784f5bf 261 damping2 = speed2 * Damp;
Technical_Muffin 7:20757784f5bf 262 step_freq2 = setpoint * speed2;
Technical_Muffin 7:20757784f5bf 263 Stepx.period(1.0/step_freq2);
Technical_Muffin 7:20757784f5bf 264 speed_old2 = speed2;
Technical_Muffin 7:20757784f5bf 265
Technical_Muffin 7:20757784f5bf 266 //speed limit
Technical_Muffin 7:20757784f5bf 267 if (speed2 > 1) {
Technical_Muffin 7:20757784f5bf 268 speed2 = 1;
Technical_Muffin 7:20757784f5bf 269 }
Technical_Muffin 8:69b7085f5343 270
Technical_Muffin 8:69b7085f5343 271 }
Technical_Muffin 8:69b7085f5343 272 else{
Technical_Muffin 7:20757784f5bf 273 //precision control
Technical_Muffin 7:20757784f5bf 274 Ledr=1;
Technical_Muffin 7:20757784f5bf 275 Stepx.period(0.000625);//frequency of 1600 Hz
Technical_Muffin 7:20757784f5bf 276 }
Technical_Muffin 8:69b7085f5343 277
Technical_Muffin 6:d03e4fa3a2a5 278 if (emg_x > 0) {//left movement
Technical_Muffin 5:19f8766b63da 279 Dirx = 0;
Technical_Muffin 0:34751b6a7dc9 280 }
Technical_Muffin 8:69b7085f5343 281 if (emg_x < 0) {//rig1ht movement
Technical_Muffin 5:19f8766b63da 282 Dirx = 1;
Technical_Muffin 0:34751b6a7dc9 283 }
Technical_Muffin 6:d03e4fa3a2a5 284
Technical_Muffin 0:34751b6a7dc9 285 //EMG treshold
Technical_Muffin 0:34751b6a7dc9 286 if (filtered_pect < EMG_tresh3 && filtered_deltoid < EMG_tresh4) {
Technical_Muffin 0:34751b6a7dc9 287 Enablex = 1; //Enable = 1 turns the motor off.
Technical_Muffin 0:34751b6a7dc9 288 } else {
Technical_Muffin 0:34751b6a7dc9 289 Enablex = 0;
Technical_Muffin 0:34751b6a7dc9 290 }
Technical_Muffin 4:61bdf601e7b0 291 }
Technical_Muffin 0:34751b6a7dc9 292
Technical_Muffin 7:20757784f5bf 293 void changecontrol(){
Technical_Muffin 7:20757784f5bf 294 proportional = !proportional;//code for changing speed control
Technical_Muffin 7:20757784f5bf 295 }
Technical_Muffin 7:20757784f5bf 296
Technical_Muffin 7:20757784f5bf 297
Technical_Muffin 0:34751b6a7dc9 298 int main()
Technical_Muffin 0:34751b6a7dc9 299 {
Technical_Muffin 8:69b7085f5343 300 pc.baud(115200);//sent data to pc for testing purposes
Technical_Muffin 8:69b7085f5343 301 Ledr=1;//turn of all the lights
Technical_Muffin 0:34751b6a7dc9 302 Ledg=1;
Technical_Muffin 0:34751b6a7dc9 303 Ledb=1;
Technical_Muffin 0:34751b6a7dc9 304
Technical_Muffin 0:34751b6a7dc9 305 MS11=1;//higher microstepping in combination with a higher step frequency reduces the vibration significantly
Technical_Muffin 0:34751b6a7dc9 306 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 307 MS31=1;
Technical_Muffin 0:34751b6a7dc9 308 MS12=1;
Technical_Muffin 0:34751b6a7dc9 309 MS22=1;
Technical_Muffin 0:34751b6a7dc9 310 MS32=1;
Technical_Muffin 8:69b7085f5343 311 Stepy.write(0.5);//set all motors to half load and disable them.
Technical_Muffin 0:34751b6a7dc9 312 Enabley.write(1);
Technical_Muffin 8:69b7085f5343 313 Stepx.write(0.5);
Technical_Muffin 0:34751b6a7dc9 314 Enablex.write(1);
Technical_Muffin 7:20757784f5bf 315 //Stepy.period(0.000625);//use period change for speed adjustments
Technical_Muffin 7:20757784f5bf 316 //Stepx.period(0.000625);//frequency of 1600 Hz
Technical_Muffin 0:34751b6a7dc9 317
Technical_Muffin 2:083d74325bfb 318
Technical_Muffin 2:083d74325bfb 319 /*//code for controlling the mechanism with a joystick
Technical_Muffin 0:34751b6a7dc9 320 float Left_value = 0.6;
Technical_Muffin 0:34751b6a7dc9 321 float Right_value = 0.9;
Technical_Muffin 0:34751b6a7dc9 322 float Up_value = 0.6;
Technical_Muffin 0:34751b6a7dc9 323 float Down_value = 0.9;
Technical_Muffin 2:083d74325bfb 324
Technical_Muffin 2:083d74325bfb 325 while(1){
Technical_Muffin 0:34751b6a7dc9 326 if (X_control.read() < Left_value){
Technical_Muffin 0:34751b6a7dc9 327 Dirx.write(0);
Technical_Muffin 0:34751b6a7dc9 328 Enablex.write(0);
Technical_Muffin 0:34751b6a7dc9 329 }
Technical_Muffin 0:34751b6a7dc9 330 else if (X_control.read() > Right_value){
Technical_Muffin 0:34751b6a7dc9 331 Dirx.write(1);
Technical_Muffin 0:34751b6a7dc9 332 Enablex.write(0);
Technical_Muffin 0:34751b6a7dc9 333 }
Technical_Muffin 0:34751b6a7dc9 334 else{
Technical_Muffin 0:34751b6a7dc9 335 Enablex.write(1);
Technical_Muffin 0:34751b6a7dc9 336 }
Technical_Muffin 0:34751b6a7dc9 337 if (Y_control.read() < Up_value){
Technical_Muffin 0:34751b6a7dc9 338 Diry.write(0);
Technical_Muffin 0:34751b6a7dc9 339 Enabley.write(0);
Technical_Muffin 0:34751b6a7dc9 340 }
Technical_Muffin 0:34751b6a7dc9 341 else if (Y_control.read() > Down_value){
Technical_Muffin 0:34751b6a7dc9 342 Diry.write(1);
Technical_Muffin 0:34751b6a7dc9 343 Enabley.write(0);
Technical_Muffin 0:34751b6a7dc9 344 }
Technical_Muffin 0:34751b6a7dc9 345 else{
Technical_Muffin 0:34751b6a7dc9 346 Enabley.write(1);
Technical_Muffin 0:34751b6a7dc9 347 }
Technical_Muffin 0:34751b6a7dc9 348 }*/
Technical_Muffin 8:69b7085f5343 349 //initialization of the biquad filters and appointing of names to filters.
Technical_Muffin 0:34751b6a7dc9 350 //biceps
Technical_Muffin 0:34751b6a7dc9 351 arm_biquad_cascade_df1_init_f32(&lowpass1_biceps, 1 , lowpass1_const, lowpass1_biceps_states);
Technical_Muffin 0:34751b6a7dc9 352 arm_biquad_cascade_df1_init_f32(&lowpass2_biceps, 1 , lowpass2_const, lowpass2_biceps_states);
Technical_Muffin 0:34751b6a7dc9 353 arm_biquad_cascade_df1_init_f32(&highpass_biceps, 1 , highpass_const, highpass_biceps_states);
Technical_Muffin 1:c8ad338ba312 354 arm_biquad_cascade_df1_init_f32(&bandstop_biceps, 2 , bandstop_const, bandstop_biceps_states);
Technical_Muffin 0:34751b6a7dc9 355 //triceps
Technical_Muffin 0:34751b6a7dc9 356 arm_biquad_cascade_df1_init_f32(&lowpass1_triceps, 1 , lowpass1_const, lowpass1_triceps_states);
Technical_Muffin 0:34751b6a7dc9 357 arm_biquad_cascade_df1_init_f32(&lowpass2_triceps, 1 , lowpass2_const, lowpass2_triceps_states);
Technical_Muffin 1:c8ad338ba312 358 arm_biquad_cascade_df1_init_f32(&highpass_triceps, 1 , highpass_const, highpass_triceps_states);
Technical_Muffin 1:c8ad338ba312 359 arm_biquad_cascade_df1_init_f32(&bandstop_triceps, 2 , bandstop_const, bandstop_triceps_states);
Technical_Muffin 1:c8ad338ba312 360 //pectoralis major
Technical_Muffin 0:34751b6a7dc9 361 arm_biquad_cascade_df1_init_f32(&lowpass1_pect, 1 , lowpass1_const, lowpass1_pect_states);
Technical_Muffin 0:34751b6a7dc9 362 arm_biquad_cascade_df1_init_f32(&lowpass2_pect, 1 , lowpass2_const, lowpass2_pect_states);
Technical_Muffin 0:34751b6a7dc9 363 arm_biquad_cascade_df1_init_f32(&highpass_pect, 1 , highpass_const, highpass_pect_states);
Technical_Muffin 1:c8ad338ba312 364 arm_biquad_cascade_df1_init_f32(&bandstop_pect, 2 , bandstop_const, bandstop_pect_states);
Technical_Muffin 1:c8ad338ba312 365
Technical_Muffin 0:34751b6a7dc9 366 //deltoid
Technical_Muffin 0:34751b6a7dc9 367 arm_biquad_cascade_df1_init_f32(&lowpass1_deltoid, 1 , lowpass1_const, lowpass1_deltoid_states);
Technical_Muffin 0:34751b6a7dc9 368 arm_biquad_cascade_df1_init_f32(&lowpass2_deltoid, 1 , lowpass2_const, lowpass2_deltoid_states);
Technical_Muffin 1:c8ad338ba312 369 arm_biquad_cascade_df1_init_f32(&highpass_deltoid, 1 , highpass_const, highpass_deltoid_states);
Technical_Muffin 1:c8ad338ba312 370 arm_biquad_cascade_df1_init_f32(&bandstop_deltoid, 2 , bandstop_const, bandstop_deltoid_states);
Technical_Muffin 1:c8ad338ba312 371
Technical_Muffin 8:69b7085f5343 372 emgtimer.attach(looper_emg, 0.01);//emg signal filtering
Technical_Muffin 0:34751b6a7dc9 373
Technical_Muffin 8:69b7085f5343 374 looptimer1.attach(looper_motorx, 0.01); //X motor control
Technical_Muffin 0:34751b6a7dc9 375
Technical_Muffin 8:69b7085f5343 376 looptimer2.attach(looper_motory, 0.01); //Y motor control
Technical_Muffin 0:34751b6a7dc9 377
Technical_Muffin 8:69b7085f5343 378 while(1){//if button is pressed the control mode changes with it.
Technical_Muffin 7:20757784f5bf 379 button1.fall(changecontrol);
Technical_Muffin 7:20757784f5bf 380 };
Technical_Muffin 0:34751b6a7dc9 381 }