Jesse Kaiser / Mbed 2 deprecated EMGcontrol_XY_Table

4 directional EMG control of the XY table. Made during my bachelor end assignment.

Dependencies:   C12832_lcd HIDScope mbed-dsp mbed

main.cpp@43:42bfab67c4a5, 2015-06-04 (annotated)

Committer:
jessekaiser
Date:
Thu Jun 04 14:34:04 2015 +0000
Revision:
43:42bfab67c4a5
Parent:
42:e67627d11789
Child:
44:d5aa53e4778c
Aansturing motor 2 emg signalen;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jessekaiser 0:3acdd563582f 1 #include "mbed.h"
jessekaiser 10:7f94cd65c910 2 #include "C12832_lcd.h"
jessekaiser 23:4d050e85e863 3 #include "arm_math.h"
jessekaiser 23:4d050e85e863 4 #include "HIDScope.h"
jessekaiser 0:3acdd563582f 5
jessekaiser 40:0cfd96cb25fa 6 #define K_Gain 14 //Gain of the filtered EMG signal
jessekaiser 40:0cfd96cb25fa 7 #define Damp 5 //Deceleration of the motor
jessekaiser 34:025b324d15d6 8 #define Mass 1 // Mass value
jessekaiser 34:025b324d15d6 9 #define dt 0.002 //Sample frequency
jessekaiser 40:0cfd96cb25fa 10 #define MAX_bi 0.09 //Can be used for normalisation of the EMG signal of the biceps
jessekaiser 40:0cfd96cb25fa 11 #define MAX_tri 0.09
jessekaiser 40:0cfd96cb25fa 12 #define MIN_freq 500 //The motor turns off below this frequency
jessekaiser 41:a666a531d52e 13 #define EMG_tresh 0.02
jessekaiser 31:372ff8d49430 14
jessekaiser 23:4d050e85e863 15 //Motor control
jessekaiser 0:3acdd563582f 16 DigitalOut Dir(p21);
jessekaiser 0:3acdd563582f 17 PwmOut Step(p22);
jessekaiser 23:4d050e85e863 18
jessekaiser 23:4d050e85e863 19 //Signal to and from computer
jessekaiser 23:4d050e85e863 20 Serial pc(USBTX, USBRX);
jessekaiser 23:4d050e85e863 21
jessekaiser 24:c4c5d30a3938 22 DigitalOut Enable(p25);
jessekaiser 23:4d050e85e863 23
jessekaiser 23:4d050e85e863 24 //Microstepping
jessekaiser 0:3acdd563582f 25 DigitalOut MS1(p27);
jessekaiser 0:3acdd563582f 26 DigitalOut MS2(p28);
jessekaiser 0:3acdd563582f 27 DigitalOut MS3(p29);
jessekaiser 23:4d050e85e863 28
jessekaiser 23:4d050e85e863 29 //Potmeter and EMG
jessekaiser 10:7f94cd65c910 30 AnalogIn Pot1(p19);
jessekaiser 31:372ff8d49430 31 AnalogIn Pot2(p20);
jessekaiser 38:c592354f5080 32
jessekaiser 38:c592354f5080 33 AnalogIn emg1(p17); //EMG bordje bovenop, biceps
jessekaiser 38:c592354f5080 34 AnalogIn emg2(p15); //triceps
jessekaiser 38:c592354f5080 35 HIDScope scope(2);
jessekaiser 29:83f005c637be 36 Ticker scopeTimer;
jessekaiser 25:144eb5822aa7 37
jessekaiser 23:4d050e85e863 38 //lcd
jessekaiser 30:0a8f849e0292 39 C12832_LCD lcd;
jessekaiser 0:3acdd563582f 40
jessekaiser 23:4d050e85e863 41 //Variables for motor control
jessekaiser 40:0cfd96cb25fa 42 float setpoint = 4400; //Frequentie setpint
jessekaiser 41:a666a531d52e 43 float step_freq1 = 1;
jessekaiser 41:a666a531d52e 44 float step_freq2 = 1;
jessekaiser 23:4d050e85e863 45
jessekaiser 23:4d050e85e863 46 //EMG filter
jessekaiser 23:4d050e85e863 47 arm_biquad_casd_df1_inst_f32 lowpass_biceps;
jessekaiser 38:c592354f5080 48 arm_biquad_casd_df1_inst_f32 lowpass_triceps;
jessekaiser 38:c592354f5080 49 //lowpass filter settings: Fc = 2 Hz, Fs = 500 Hz, Gain = -3 dB
jessekaiser 34:025b324d15d6 50 float lowpass_const[] = {0.00015514839749793376, 0.00031029679499586753, 0.00015514839749793376, 1.9644602512795832, -0.9650808448695751};
jessekaiser 23:4d050e85e863 51 arm_biquad_casd_df1_inst_f32 highnotch_biceps;
jessekaiser 38:c592354f5080 52 arm_biquad_casd_df1_inst_f32 highnotch_triceps;
jessekaiser 35:b0737ee24b43 53 //highpass filter settings: Fc = 20 Hz, Fs = 500 Hz, notch Fc = 50, Fs = 500 Hz
jessekaiser 31:372ff8d49430 54 float highnotch_const[] = {0.8370879899975344, -1.6741759799950688, 0.8370879899975344, 1.6474576182593796, -0.7008943417307579, 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, 1.1429772843080923, -0.41279762014290533};
jessekaiser 23:4d050e85e863 55
jessekaiser 23:4d050e85e863 56 //state values
jessekaiser 23:4d050e85e863 57 float lowpass_biceps_states[4];
jessekaiser 23:4d050e85e863 58 float highnotch_biceps_states[8];
jessekaiser 38:c592354f5080 59 float lowpass_triceps_states[4];
jessekaiser 38:c592354f5080 60 float highnotch_triceps_states[8];
jessekaiser 23:4d050e85e863 61
jessekaiser 23:4d050e85e863 62 //global variabels
jessekaiser 23:4d050e85e863 63 float filtered_biceps;
jessekaiser 38:c592354f5080 64 float filtered_triceps;
jessekaiser 41:a666a531d52e 65 float speed_old1;
jessekaiser 41:a666a531d52e 66 float speed_old2;
jessekaiser 41:a666a531d52e 67 float acc1;
jessekaiser 41:a666a531d52e 68 float acc2;
jessekaiser 38:c592354f5080 69 float force1;
jessekaiser 38:c592354f5080 70 float force2;
jessekaiser 41:a666a531d52e 71 float speed1;
jessekaiser 41:a666a531d52e 72 float speed2;
jessekaiser 41:a666a531d52e 73 float damping1;
jessekaiser 41:a666a531d52e 74 float damping2;
jessekaiser 32:46b18f465600 75
jessekaiser 23:4d050e85e863 76 void looper_emg()
jessekaiser 23:4d050e85e863 77 {
jessekaiser 36:d6e0a835bb2d 78
jessekaiser 23:4d050e85e863 79
jessekaiser 23:4d050e85e863 80 float emg_value1_f32;
jessekaiser 38:c592354f5080 81 emg_value1_f32 = emg1.read();
jessekaiser 38:c592354f5080 82
jessekaiser 38:c592354f5080 83 float emg_value2_f32;
jessekaiser 38:c592354f5080 84 emg_value2_f32 = emg2.read();
jessekaiser 23:4d050e85e863 85
jessekaiser 23:4d050e85e863 86 //process emg biceps
jessekaiser 23:4d050e85e863 87 arm_biquad_cascade_df1_f32(&highnotch_biceps, &emg_value1_f32, &filtered_biceps, 1 );
jessekaiser 31:372ff8d49430 88 filtered_biceps = fabs(filtered_biceps);
jessekaiser 23:4d050e85e863 89 arm_biquad_cascade_df1_f32(&lowpass_biceps, &filtered_biceps, &filtered_biceps, 1 );
jessekaiser 36:d6e0a835bb2d 90
jessekaiser 38:c592354f5080 91 //process emg triceps
jessekaiser 38:c592354f5080 92 arm_biquad_cascade_df1_f32(&highnotch_triceps, &emg_value2_f32, &filtered_triceps, 1 );
jessekaiser 38:c592354f5080 93 filtered_triceps = fabs(filtered_triceps);
jessekaiser 38:c592354f5080 94 arm_biquad_cascade_df1_f32(&lowpass_triceps, &filtered_triceps, &filtered_triceps, 1 );
jessekaiser 23:4d050e85e863 95
jessekaiser 23:4d050e85e863 96 /*send value to PC. */
jessekaiser 36:d6e0a835bb2d 97 scope.set(0,filtered_biceps); //Filtered EMG signal
jessekaiser 38:c592354f5080 98 scope.set(1,filtered_triceps);
jessekaiser 23:4d050e85e863 99 }
jessekaiser 26:b88ff19ff5dc 100
jessekaiser 31:372ff8d49430 101 void looper_motor()
jessekaiser 31:372ff8d49430 102 {
jessekaiser 41:a666a531d52e 103 //Vooruit
jessekaiser 41:a666a531d52e 104 force1 = K_Gain*(filtered_biceps/MAX_bi);
jessekaiser 41:a666a531d52e 105 force1 = force1 - damping1;
jessekaiser 41:a666a531d52e 106 acc1 = force1/Mass;
jessekaiser 41:a666a531d52e 107 speed1 = speed_old1 + (acc1 * dt);
jessekaiser 41:a666a531d52e 108 damping1 = speed1 * Damp;
jessekaiser 41:a666a531d52e 109 step_freq1 = (setpoint*speed1);
jessekaiser 41:a666a531d52e 110 speed_old1 = speed1;
jessekaiser 42:e67627d11789 111
jessekaiser 43:42bfab67c4a5 112 //Achteruit triceps
jessekaiser 41:a666a531d52e 113 force2 = K_Gain*(filtered_triceps/MAX_tri);
jessekaiser 41:a666a531d52e 114 force2 = force2 - damping2;
jessekaiser 41:a666a531d52e 115 acc2 = force2/Mass;
jessekaiser 41:a666a531d52e 116 speed2 = speed_old2 + (acc2 * dt);
jessekaiser 41:a666a531d52e 117 damping2 = speed2 * Damp;
jessekaiser 41:a666a531d52e 118 step_freq2 = (setpoint*speed2);
jessekaiser 41:a666a531d52e 119 speed_old2 = speed2;
jessekaiser 41:a666a531d52e 120 if (filtered_biceps > filtered_triceps) {
jessekaiser 41:a666a531d52e 121 Dir = 0;
jessekaiser 43:42bfab67c4a5 122 speed2 = 0.01;
jessekaiser 43:42bfab67c4a5 123 speed_old2 = 0.01;
jessekaiser 41:a666a531d52e 124 Step.period(1.0/step_freq1);
jessekaiser 42:e67627d11789 125 } if (filtered_triceps > filtered_biceps) {
jessekaiser 41:a666a531d52e 126 Dir = 1;
jessekaiser 43:42bfab67c4a5 127 speed1 = 0.01;
jessekaiser 43:42bfab67c4a5 128 speed_old1 = 0.01;
jessekaiser 41:a666a531d52e 129 Step.period(1.0/step_freq2);
jessekaiser 41:a666a531d52e 130 }
jessekaiser 41:a666a531d52e 131 //Speed limit
jessekaiser 42:e67627d11789 132 if (speed1 > 1) {
jessekaiser 41:a666a531d52e 133 speed1 = 1;
jessekaiser 42:e67627d11789 134 step_freq1 = setpoint;
jessekaiser 42:e67627d11789 135 }
jessekaiser 43:42bfab67c4a5 136 if (speed2 > 1) {
jessekaiser 41:a666a531d52e 137 speed2 = 1;
jessekaiser 43:42bfab67c4a5 138 step_freq2 = setpoint;
jessekaiser 41:a666a531d52e 139 }
jessekaiser 41:a666a531d52e 140 //EMG treshold
jessekaiser 41:a666a531d52e 141 if (filtered_biceps < EMG_tresh && filtered_triceps < EMG_tresh) {
jessekaiser 43:42bfab67c4a5 142 Enable = 1; //Enable = 1 turns the motor off.
jessekaiser 41:a666a531d52e 143 } else {
jessekaiser 41:a666a531d52e 144 Enable = 0;
jessekaiser 38:c592354f5080 145 }
jessekaiser 31:372ff8d49430 146
jessekaiser 41:a666a531d52e 147 }
jessekaiser 41:a666a531d52e 148
jessekaiser 41:a666a531d52e 149 int main()
jessekaiser 41:a666a531d52e 150 {
jessekaiser 41:a666a531d52e 151 // Attach the HIDScope::send method from the scope object to the timer at 500Hz. Hier wordt de sample freq aangegeven.
jessekaiser 41:a666a531d52e 152 scopeTimer.attach_us(&scope, &HIDScope::send, 2e3);
jessekaiser 36:d6e0a835bb2d 153
jessekaiser 41:a666a531d52e 154 Ticker emgtimer;
jessekaiser 41:a666a531d52e 155 arm_biquad_cascade_df1_init_f32(&lowpass_biceps, 1 , lowpass_const, lowpass_biceps_states);
jessekaiser 41:a666a531d52e 156 arm_biquad_cascade_df1_init_f32(&highnotch_biceps, 2 , highnotch_const, highnotch_biceps_states);
jessekaiser 41:a666a531d52e 157 arm_biquad_cascade_df1_init_f32(&lowpass_triceps, 1 , lowpass_const, lowpass_triceps_states);
jessekaiser 41:a666a531d52e 158 arm_biquad_cascade_df1_init_f32(&highnotch_triceps, 2 , highnotch_const, highnotch_triceps_states);
jessekaiser 41:a666a531d52e 159 emgtimer.attach(looper_emg, 0.002);
jessekaiser 23:4d050e85e863 160
jessekaiser 41:a666a531d52e 161 Ticker looptimer;
jessekaiser 41:a666a531d52e 162 looptimer.attach(looper_motor, 0.01); //Uitzoeken waarom deze frequentie!
jessekaiser 23:4d050e85e863 163
jessekaiser 41:a666a531d52e 164 //Microstepping control
jessekaiser 41:a666a531d52e 165 MS1 = 1;
jessekaiser 41:a666a531d52e 166 MS2 = 0;
jessekaiser 41:a666a531d52e 167 MS3 = 0;
jessekaiser 41:a666a531d52e 168 Step.write(0.5); // Duty cycle van 50%
jessekaiser 38:c592354f5080 169
jessekaiser 41:a666a531d52e 170 while (1) {
jessekaiser 41:a666a531d52e 171
jessekaiser 41:a666a531d52e 172 //lcd.printf("Bi %.2f ,Tri %.2f \n", filtered_biceps, filtered_triceps); //snelheid meting op lcd
jessekaiser 41:a666a531d52e 173 lcd.printf("1 %.0f, 2 %.0f \n", step_freq1, step_freq2);
jessekaiser 41:a666a531d52e 174 wait(0.01);
jessekaiser 0:3acdd563582f 175 }
jessekaiser 41:a666a531d52e 176 }