K K / Mbed 2 deprecated revised_XY_table_code

Code for controlling the Magna table

Dependencies:   HIDScope MODSERIAL mbed-dsp mbed

main.cpp@0:34751b6a7dc9, 2016-01-07 (annotated)

Committer:
Technical_Muffin
Date:
Thu Jan 07 08:54:02 2016 +0000
Revision:
0:34751b6a7dc9
Child:
1:c8ad338ba312
Current state of the actinarm movement code, it works with 2 biceps muscles to control 1 motor. Wrist motion doesn't work. And it needs continuous activation not rapid.

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 0:34751b6a7dc9 3 Biceps = 1
Technical_Muffin 0:34751b6a7dc9 4 Triceps = 2
Technical_Muffin 0:34751b6a7dc9 5 Pectoralis Major = 3
Technical_Muffin 0:34751b6a7dc9 6 Deltoid posterior = 4
Technical_Muffin 0:34751b6a7dc9 7 The "x" and "y" at the end of variables stand for the X-Spindle or Y-Spindle respectivly.
Technical_Muffin 0:34751b6a7dc9 8 The code has been revised to work with the new board and also has a secondary way of controlling it
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 0:34751b6a7dc9 21 #define EMG_tresh1 0.02
Technical_Muffin 0:34751b6a7dc9 22 #define EMG_tresh2 0.02
Technical_Muffin 0:34751b6a7dc9 23 #define EMG_tresh3 0.02
Technical_Muffin 0:34751b6a7dc9 24 #define EMG_tresh4 0.02
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 0:34751b6a7dc9 58 AnalogIn emg1(A2); //biceps bottom emg board
Technical_Muffin 0:34751b6a7dc9 59 AnalogIn emg2(A3); //triceps
Technical_Muffin 0:34751b6a7dc9 60 AnalogIn emg3(A4); //Pectoralis major
Technical_Muffin 0:34751b6a7dc9 61 AnalogIn emg4(A5); //Deltoid 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 0:34751b6a7dc9 85 //lowpass 1 filter settings: Fc = 45 Hz, Fs = 100 Hz, Gain = -3 dB
Technical_Muffin 0:34751b6a7dc9 86 //lowpass 2 filter settings: Fc = 0.3 Hz, Fs = 100 Hz, Gain = -3 dB
Technical_Muffin 0:34751b6a7dc9 87 float lowpass1_const[] = {0.800592403464570,1.60118480692914,0.800592403464570,-1.56101807580072,-0.641351538057563};
Technical_Muffin 0:34751b6a7dc9 88 float lowpass2_const[] = {8.76555487540147e-05, 0.000175311097508029, 8.76555487540147e-05 , 1.97334424978130, -0.973694871976315};
Technical_Muffin 0:34751b6a7dc9 89 //highpass filter settings: Fc = 20 Hz, Fs = 100 Hz
Technical_Muffin 0:34751b6a7dc9 90
Technical_Muffin 0:34751b6a7dc9 91 float highpass_const[] = {0.391335772501769 ,-0.782671545003538, 0.391335772501769,0.369527377351241, -0.195815712655833};
Technical_Muffin 0:34751b6a7dc9 92
Technical_Muffin 0:34751b6a7dc9 93
Technical_Muffin 0:34751b6a7dc9 94 /*
Technical_Muffin 0:34751b6a7dc9 95 //suffer from high wire motion influence and dont react to continuous tension only rapid activation
Technical_Muffin 0:34751b6a7dc9 96
Technical_Muffin 0:34751b6a7dc9 97 //lowpass 1 filter settings: Fc = 20 Hz, Fs = 100 Hz, Gain = -3 dB
Technical_Muffin 0:34751b6a7dc9 98 //lowpass 2 filter settings: Fc = 0.3 Hz, Fs = 100 Hz, Gain = -3 dB
Technical_Muffin 0:34751b6a7dc9 99 float lowpass1_const[] = {0.206572083826148,0.413144167652296,0.206572083826148,0.369527377351241,-0.195815712655833};
Technical_Muffin 0:34751b6a7dc9 100 float lowpass2_const[] = {0.0000876555487540065, 0.000175311097508013, 0.0000876555487540065, 1.97334424978130, -0.973694871976315};
Technical_Muffin 0:34751b6a7dc9 101 //highpass filter settings: Fc = 3 Hz, Fs = 100 Hz
Technical_Muffin 0:34751b6a7dc9 102
Technical_Muffin 0:34751b6a7dc9 103 float highpass_const[] = {0.875183092438135,-1.75036618487627,0.875183092438135,1.73472576880928,-0.766006600943264};
Technical_Muffin 0:34751b6a7dc9 104 */
Technical_Muffin 0:34751b6a7dc9 105
Technical_Muffin 0:34751b6a7dc9 106 /*
Technical_Muffin 0:34751b6a7dc9 107 Previous coefficients testing to see if new variants work
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 = 2 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[] = {0.0000876555487540065, 0.000175311097508013, 0.0000876555487540065, 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.782671545003537,0.391335772501769,0.369527377351241,-0.195815712655833 };
Technical_Muffin 0:34751b6a7dc9 115 */
Technical_Muffin 0:34751b6a7dc9 116 arm_biquad_casd_df1_inst_f32 highpass_biceps;
Technical_Muffin 0:34751b6a7dc9 117 arm_biquad_casd_df1_inst_f32 highpass_triceps;
Technical_Muffin 0:34751b6a7dc9 118 arm_biquad_casd_df1_inst_f32 highpass_pect;
Technical_Muffin 0:34751b6a7dc9 119 arm_biquad_casd_df1_inst_f32 highpass_deltoid;
Technical_Muffin 0:34751b6a7dc9 120
Technical_Muffin 0:34751b6a7dc9 121 //state values
Technical_Muffin 0:34751b6a7dc9 122 float lowpass1_biceps_states[4];
Technical_Muffin 0:34751b6a7dc9 123 float lowpass2_biceps_states[4];
Technical_Muffin 0:34751b6a7dc9 124 float highpass_biceps_states[4];
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 0:34751b6a7dc9 128 float lowpass1_pect_states[4];
Technical_Muffin 0:34751b6a7dc9 129 float lowpass2_pect_states[4];
Technical_Muffin 0:34751b6a7dc9 130 float highpass_pect_states[4];
Technical_Muffin 0:34751b6a7dc9 131 float lowpass1_deltoid_states[4];
Technical_Muffin 0:34751b6a7dc9 132 float lowpass2_deltoid_states[4];
Technical_Muffin 0:34751b6a7dc9 133 float highpass_deltoid_states[4];
Technical_Muffin 0:34751b6a7dc9 134
Technical_Muffin 0:34751b6a7dc9 135 //global variabels
Technical_Muffin 0:34751b6a7dc9 136 float filtered_biceps, filtered_triceps, filtered_pect, filtered_deltoid;
Technical_Muffin 0:34751b6a7dc9 137 float speed_old1, speed_old2, speed_old3, speed_old4;
Technical_Muffin 0:34751b6a7dc9 138 float acc1, acc2, acc3, acc4;
Technical_Muffin 0:34751b6a7dc9 139 float force1, force2, force3, force4;
Technical_Muffin 0:34751b6a7dc9 140 float speed1, speed2, speed3, speed4;
Technical_Muffin 0:34751b6a7dc9 141 float damping1, damping2, damping3, damping4;
Technical_Muffin 0:34751b6a7dc9 142 float emg_x, emg_y;
Technical_Muffin 0:34751b6a7dc9 143 float cx = 0;
Technical_Muffin 0:34751b6a7dc9 144 float cy = 0;
Technical_Muffin 0:34751b6a7dc9 145 float errorx = 0.2;
Technical_Muffin 0:34751b6a7dc9 146 float errory = 0.2;
Technical_Muffin 0:34751b6a7dc9 147 float Ps_x = 0;
Technical_Muffin 0:34751b6a7dc9 148 float Ps_y = 0;
Technical_Muffin 0:34751b6a7dc9 149 float hstep_freqx = 1;
Technical_Muffin 0:34751b6a7dc9 150 float hstep_freqy = 1;
Technical_Muffin 0:34751b6a7dc9 151 float emg_y_abs = 0;
Technical_Muffin 0:34751b6a7dc9 152 float emg_x_abs = 0;
Technical_Muffin 0:34751b6a7dc9 153
Technical_Muffin 0:34751b6a7dc9 154 void looper_emg()
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 0:34751b6a7dc9 157 emg_value1_f32 = emg1.read();
Technical_Muffin 0:34751b6a7dc9 158 emg_value2_f32 = emg2.read();
Technical_Muffin 0:34751b6a7dc9 159 emg_value3_f32 = emg3.read();
Technical_Muffin 0:34751b6a7dc9 160 emg_value4_f32 = emg4.read();
Technical_Muffin 0:34751b6a7dc9 161
Technical_Muffin 0:34751b6a7dc9 162 //process emg biceps
Technical_Muffin 0:34751b6a7dc9 163 arm_biquad_cascade_df1_f32(&lowpass1_biceps, &emg_value1_f32, &filtered_biceps, 1 );
Technical_Muffin 0:34751b6a7dc9 164 arm_biquad_cascade_df1_f32(&highpass_biceps, &filtered_biceps, &filtered_biceps, 1 );
Technical_Muffin 0:34751b6a7dc9 165 filtered_biceps = fabs(filtered_biceps); //Rectifier, The Gain is already implemented.
Technical_Muffin 0:34751b6a7dc9 166 arm_biquad_cascade_df1_f32(&lowpass2_biceps, &filtered_biceps, &filtered_biceps, 1 ); //low pass filter
Technical_Muffin 0:34751b6a7dc9 167
Technical_Muffin 0:34751b6a7dc9 168 //process emg triceps
Technical_Muffin 0:34751b6a7dc9 169 arm_biquad_cascade_df1_f32(&lowpass1_triceps, &emg_value2_f32, &filtered_triceps, 1 );
Technical_Muffin 0:34751b6a7dc9 170 arm_biquad_cascade_df1_f32(&highpass_triceps, &filtered_triceps, &filtered_triceps, 1 );
Technical_Muffin 0:34751b6a7dc9 171 filtered_triceps = fabs(filtered_triceps);
Technical_Muffin 0:34751b6a7dc9 172 arm_biquad_cascade_df1_f32(&lowpass2_triceps, &filtered_triceps, &filtered_triceps, 1 );
Technical_Muffin 0:34751b6a7dc9 173
Technical_Muffin 0:34751b6a7dc9 174 /* //process emg pectoralis major
Technical_Muffin 0:34751b6a7dc9 175 arm_biquad_cascade_df1_f32(&lowpass1_pect, &emg_value3_f32, &filtered_pect, 1 );
Technical_Muffin 0:34751b6a7dc9 176 arm_biquad_cascade_df1_f32(&highpass_pect, &filtered_pect, &filtered_pect, 1 );
Technical_Muffin 0:34751b6a7dc9 177 //filtered_pect = fabs(filtered_pect);
Technical_Muffin 0:34751b6a7dc9 178 //arm_biquad_cascade_df1_f32(&lowpass2_pect, &filtered_pect, &filtered_pect, 1 );
Technical_Muffin 0:34751b6a7dc9 179
Technical_Muffin 0:34751b6a7dc9 180 //process emg deltoid
Technical_Muffin 0:34751b6a7dc9 181 arm_biquad_cascade_df1_f32(&lowpass1_deltoid, &emg_value4_f32, &filtered_deltoid, 1 );
Technical_Muffin 0:34751b6a7dc9 182 arm_biquad_cascade_df1_f32(&highpass_deltoid, &filtered_deltoid, &filtered_deltoid, 1 );
Technical_Muffin 0:34751b6a7dc9 183 //filtered_deltoid = fabs(filtered_deltoid);
Technical_Muffin 0:34751b6a7dc9 184 //arm_biquad_cascade_df1_f32(&lowpass2_deltoid, &filtered_deltoid, &filtered_deltoid, 1 );
Technical_Muffin 0:34751b6a7dc9 185 */
Technical_Muffin 0:34751b6a7dc9 186 // send value to PC.
Technical_Muffin 0:34751b6a7dc9 187
Technical_Muffin 0:34751b6a7dc9 188 scope.set(0,filtered_biceps); //Filtered EMG signal
Technical_Muffin 0:34751b6a7dc9 189 scope.set(1,filtered_triceps);
Technical_Muffin 0:34751b6a7dc9 190 /*scope.set(2,filtered_pect);
Technical_Muffin 0:34751b6a7dc9 191 scope.set(3,filtered_deltoid);*/
Technical_Muffin 0:34751b6a7dc9 192 scope.set(2,emg1.read());//testing emg signal for cause of variance
Technical_Muffin 0:34751b6a7dc9 193 scope.set(3,emg2.read());
Technical_Muffin 0:34751b6a7dc9 194 scope.send();
Technical_Muffin 0:34751b6a7dc9 195
Technical_Muffin 0:34751b6a7dc9 196 }
Technical_Muffin 0:34751b6a7dc9 197
Technical_Muffin 0:34751b6a7dc9 198
Technical_Muffin 0:34751b6a7dc9 199 void looper_motory()
Technical_Muffin 0:34751b6a7dc9 200 {
Technical_Muffin 0:34751b6a7dc9 201
Technical_Muffin 0:34751b6a7dc9 202 /*emg_y = (filtered_biceps - filtered_triceps);
Technical_Muffin 0:34751b6a7dc9 203 emg_y_abs = fabs(emg_y);
Technical_Muffin 0:34751b6a7dc9 204 force1 = emg_y_abs*K_Gain;
Technical_Muffin 0:34751b6a7dc9 205 force1 = force1 - damping1;
Technical_Muffin 0:34751b6a7dc9 206 acc1 = force1/Mass;
Technical_Muffin 0:34751b6a7dc9 207 speed1 = speed_old1 + (acc1 * dt);
Technical_Muffin 0:34751b6a7dc9 208 damping1 = speed1 * Damp;
Technical_Muffin 0:34751b6a7dc9 209 step_freq1 = setpoint * speed1;*/
Technical_Muffin 0:34751b6a7dc9 210 // Stepy.period(/*1.0/step_freq1*/0.000625);
Technical_Muffin 0:34751b6a7dc9 211 //speed_old1 = speed1;
Technical_Muffin 0:34751b6a7dc9 212
Technical_Muffin 0:34751b6a7dc9 213 if (filtered_triceps > 0.06) {
Technical_Muffin 0:34751b6a7dc9 214 Diry.write(1);
Technical_Muffin 0:34751b6a7dc9 215 Enabley.write(0); //Enable = 1 turns the motor off.
Technical_Muffin 0:34751b6a7dc9 216 Ledr.write(0);
Technical_Muffin 0:34751b6a7dc9 217 Ledb.write(1);
Technical_Muffin 0:34751b6a7dc9 218 }
Technical_Muffin 0:34751b6a7dc9 219 else if (filtered_biceps > 0.06) {
Technical_Muffin 0:34751b6a7dc9 220 Diry.write(0);
Technical_Muffin 0:34751b6a7dc9 221 Enabley.write(0); //Enable = 1 turns the motor off.
Technical_Muffin 0:34751b6a7dc9 222 Ledr.write(1);
Technical_Muffin 0:34751b6a7dc9 223 Ledb.write(0);
Technical_Muffin 0:34751b6a7dc9 224 }
Technical_Muffin 0:34751b6a7dc9 225 else if (filtered_triceps < 0.04 && filtered_biceps < 0.04) {
Technical_Muffin 0:34751b6a7dc9 226 Enabley.write(1);
Technical_Muffin 0:34751b6a7dc9 227 }
Technical_Muffin 0:34751b6a7dc9 228 else{}
Technical_Muffin 0:34751b6a7dc9 229 //Speed limit
Technical_Muffin 0:34751b6a7dc9 230 /*if (speed1 > 1) {
Technical_Muffin 0:34751b6a7dc9 231 speed1 = 1;
Technical_Muffin 0:34751b6a7dc9 232 step_freq1 = setpoint;
Technical_Muffin 0:34751b6a7dc9 233 }*/
Technical_Muffin 0:34751b6a7dc9 234
Technical_Muffin 0:34751b6a7dc9 235 }
Technical_Muffin 0:34751b6a7dc9 236 /*
Technical_Muffin 0:34751b6a7dc9 237 void looper_motorx()
Technical_Muffin 0:34751b6a7dc9 238 {
Technical_Muffin 0:34751b6a7dc9 239
Technical_Muffin 0:34751b6a7dc9 240 emg_x = (filtered_pect - filtered_deltoid);
Technical_Muffin 0:34751b6a7dc9 241 emg_x_abs = fabs(emg_x);
Technical_Muffin 0:34751b6a7dc9 242 force2 = emg_x_abs*K_Gain;
Technical_Muffin 0:34751b6a7dc9 243 force2 = force2 - damping2;
Technical_Muffin 0:34751b6a7dc9 244 acc2 = force2/Mass;
Technical_Muffin 0:34751b6a7dc9 245 speed2 = speed_old2 + (acc2 * dt);
Technical_Muffin 0:34751b6a7dc9 246 damping2 = speed2 * Damp;
Technical_Muffin 0:34751b6a7dc9 247 step_freq2 = setpoint * speed2;
Technical_Muffin 0:34751b6a7dc9 248 Stepx.period(1.0/step_freq2);
Technical_Muffin 0:34751b6a7dc9 249 speed_old2 = speed2;
Technical_Muffin 0:34751b6a7dc9 250
Technical_Muffin 0:34751b6a7dc9 251 if (emg_x > 0) {
Technical_Muffin 0:34751b6a7dc9 252 Dirx = 1;
Technical_Muffin 0:34751b6a7dc9 253 }
Technical_Muffin 0:34751b6a7dc9 254 if (emg_x < 0) {
Technical_Muffin 0:34751b6a7dc9 255 Dirx = 0;
Technical_Muffin 0:34751b6a7dc9 256 }
Technical_Muffin 0:34751b6a7dc9 257 //Speed limit
Technical_Muffin 0:34751b6a7dc9 258 if (speed2 > 1) {
Technical_Muffin 0:34751b6a7dc9 259 speed2 = 1;
Technical_Muffin 0:34751b6a7dc9 260 step_freq2 = setpoint;
Technical_Muffin 0:34751b6a7dc9 261 }
Technical_Muffin 0:34751b6a7dc9 262 //EMG treshold
Technical_Muffin 0:34751b6a7dc9 263 if (filtered_pect < EMG_tresh3 && filtered_deltoid < EMG_tresh4) {
Technical_Muffin 0:34751b6a7dc9 264 Enablex = 1; //Enable = 1 turns the motor off.
Technical_Muffin 0:34751b6a7dc9 265 } else {
Technical_Muffin 0:34751b6a7dc9 266 Enablex = 0;
Technical_Muffin 0:34751b6a7dc9 267 }
Technical_Muffin 0:34751b6a7dc9 268
Technical_Muffin 0:34751b6a7dc9 269 }*/
Technical_Muffin 0:34751b6a7dc9 270
Technical_Muffin 0:34751b6a7dc9 271 int main()
Technical_Muffin 0:34751b6a7dc9 272 {
Technical_Muffin 0:34751b6a7dc9 273
Technical_Muffin 0:34751b6a7dc9 274 //scopeTimer.attach_us(&scopeSend, 2e3);
Technical_Muffin 0:34751b6a7dc9 275 pc.baud(115200);
Technical_Muffin 0:34751b6a7dc9 276 Ledr=1;
Technical_Muffin 0:34751b6a7dc9 277 Ledg=1;
Technical_Muffin 0:34751b6a7dc9 278 Ledb=1;
Technical_Muffin 0:34751b6a7dc9 279
Technical_Muffin 0:34751b6a7dc9 280 MS11=1;//higher microstepping in combination with a higher step frequency reduces the vibration significantly
Technical_Muffin 0:34751b6a7dc9 281 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 282 MS31=1;
Technical_Muffin 0:34751b6a7dc9 283 MS12=1;
Technical_Muffin 0:34751b6a7dc9 284 MS22=1;
Technical_Muffin 0:34751b6a7dc9 285 MS32=1;
Technical_Muffin 0:34751b6a7dc9 286 /*Diry.write(1);*/
Technical_Muffin 0:34751b6a7dc9 287 Stepy.write(0.5f);/*
Technical_Muffin 0:34751b6a7dc9 288 Enabley.write(1);
Technical_Muffin 0:34751b6a7dc9 289 Dirx.write(1);*/
Technical_Muffin 0:34751b6a7dc9 290 Stepx.write(0.5f);
Technical_Muffin 0:34751b6a7dc9 291 Enablex.write(1);
Technical_Muffin 0:34751b6a7dc9 292 Stepy.period(0.000625);//use period change for speed adjustments
Technical_Muffin 0:34751b6a7dc9 293 Stepx.period(0.000625);//frequency of 1600 Hz
Technical_Muffin 0:34751b6a7dc9 294
Technical_Muffin 0:34751b6a7dc9 295 float Left_value = 0.6;
Technical_Muffin 0:34751b6a7dc9 296 float Right_value = 0.9;
Technical_Muffin 0:34751b6a7dc9 297 float Up_value = 0.6;
Technical_Muffin 0:34751b6a7dc9 298 float Down_value = 0.9;
Technical_Muffin 0:34751b6a7dc9 299 /*
Technical_Muffin 0:34751b6a7dc9 300 while(1){//code for controlling the mechanism with a joystick
Technical_Muffin 0:34751b6a7dc9 301 if (X_control.read() < Left_value){
Technical_Muffin 0:34751b6a7dc9 302 Dirx.write(0);
Technical_Muffin 0:34751b6a7dc9 303 Enablex.write(0);
Technical_Muffin 0:34751b6a7dc9 304 }
Technical_Muffin 0:34751b6a7dc9 305 else if (X_control.read() > Right_value){
Technical_Muffin 0:34751b6a7dc9 306 Dirx.write(1);
Technical_Muffin 0:34751b6a7dc9 307 Enablex.write(0);
Technical_Muffin 0:34751b6a7dc9 308 }
Technical_Muffin 0:34751b6a7dc9 309 else{
Technical_Muffin 0:34751b6a7dc9 310 Enablex.write(1);
Technical_Muffin 0:34751b6a7dc9 311 }
Technical_Muffin 0:34751b6a7dc9 312 if (Y_control.read() < Up_value){
Technical_Muffin 0:34751b6a7dc9 313 Diry.write(0);
Technical_Muffin 0:34751b6a7dc9 314 Enabley.write(0);
Technical_Muffin 0:34751b6a7dc9 315 }
Technical_Muffin 0:34751b6a7dc9 316 else if (Y_control.read() > Down_value){
Technical_Muffin 0:34751b6a7dc9 317 Diry.write(1);
Technical_Muffin 0:34751b6a7dc9 318 Enabley.write(0);
Technical_Muffin 0:34751b6a7dc9 319 }
Technical_Muffin 0:34751b6a7dc9 320 else{
Technical_Muffin 0:34751b6a7dc9 321 Enabley.write(1);
Technical_Muffin 0:34751b6a7dc9 322 }
Technical_Muffin 0:34751b6a7dc9 323 pc.printf("X value is %f and Y value is %f\n", X_control.read(), Y_control.read());
Technical_Muffin 0:34751b6a7dc9 324 }*/
Technical_Muffin 0:34751b6a7dc9 325
Technical_Muffin 0:34751b6a7dc9 326 //biceps
Technical_Muffin 0:34751b6a7dc9 327 arm_biquad_cascade_df1_init_f32(&lowpass1_biceps, 1 , lowpass1_const, lowpass1_biceps_states);
Technical_Muffin 0:34751b6a7dc9 328 arm_biquad_cascade_df1_init_f32(&lowpass2_biceps, 1 , lowpass2_const, lowpass2_biceps_states);
Technical_Muffin 0:34751b6a7dc9 329 arm_biquad_cascade_df1_init_f32(&highpass_biceps, 1 , highpass_const, highpass_biceps_states);
Technical_Muffin 0:34751b6a7dc9 330 //triceps
Technical_Muffin 0:34751b6a7dc9 331 arm_biquad_cascade_df1_init_f32(&lowpass1_triceps, 1 , lowpass1_const, lowpass1_triceps_states);
Technical_Muffin 0:34751b6a7dc9 332 arm_biquad_cascade_df1_init_f32(&lowpass2_triceps, 1 , lowpass2_const, lowpass2_triceps_states);
Technical_Muffin 0:34751b6a7dc9 333 arm_biquad_cascade_df1_init_f32(&highpass_triceps, 1 , highpass_const, highpass_triceps_states);
Technical_Muffin 0:34751b6a7dc9 334 /*//pectoralis major
Technical_Muffin 0:34751b6a7dc9 335 arm_biquad_cascade_df1_init_f32(&lowpass1_pect, 1 , lowpass1_const, lowpass1_pect_states);
Technical_Muffin 0:34751b6a7dc9 336 arm_biquad_cascade_df1_init_f32(&lowpass2_pect, 1 , lowpass2_const, lowpass2_pect_states);
Technical_Muffin 0:34751b6a7dc9 337 arm_biquad_cascade_df1_init_f32(&highpass_pect, 1 , highpass_const, highpass_pect_states);
Technical_Muffin 0:34751b6a7dc9 338 //deltoid
Technical_Muffin 0:34751b6a7dc9 339 arm_biquad_cascade_df1_init_f32(&lowpass1_deltoid, 1 , lowpass1_const, lowpass1_deltoid_states);
Technical_Muffin 0:34751b6a7dc9 340 arm_biquad_cascade_df1_init_f32(&lowpass2_deltoid, 1 , lowpass2_const, lowpass2_deltoid_states);
Technical_Muffin 0:34751b6a7dc9 341 arm_biquad_cascade_df1_init_f32(&highpass_deltoid, 1 , highpass_const, highpass_deltoid_states);*/
Technical_Muffin 0:34751b6a7dc9 342 emgtimer.attach(looper_emg, 0.01);
Technical_Muffin 0:34751b6a7dc9 343
Technical_Muffin 0:34751b6a7dc9 344 //looptimer1.attach(looper_motorx, 0.01); //X-Spindle motor
Technical_Muffin 0:34751b6a7dc9 345
Technical_Muffin 0:34751b6a7dc9 346 looptimer2.attach(looper_motory, 0.01); //Y-Spindle motor
Technical_Muffin 0:34751b6a7dc9 347
Technical_Muffin 0:34751b6a7dc9 348 while(1){};
Technical_Muffin 0:34751b6a7dc9 349 }