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@32:46b18f465600, 2015-05-22 (annotated)

Committer:
jessekaiser
Date:
Fri May 22 08:35:28 2015 +0000
Revision:
32:46b18f465600
Parent:
31:372ff8d49430
Child:
33:3c9f8c1e9adf
Proportionele aansturing werkt. Na deze commit wordt ongebruikte code verwijderd.

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 31:372ff8d49430 6 #define P_Gain 0.995
jessekaiser 32:46b18f465600 7 #define K_Gain 65 //Gain of the filtered EMG signal
jessekaiser 32:46b18f465600 8 #define Damp 2 //Deceleration of the motr
jessekaiser 31:372ff8d49430 9 #define tres_bi 0.05 //Biceps emg treshold
jessekaiser 31:372ff8d49430 10 #define Mass 1 // Mass value
jessekaiser 31:372ff8d49430 11 #define dt 0.002 //Sample frequency
jessekaiser 31:372ff8d49430 12
jessekaiser 23:4d050e85e863 13 //Motor control
jessekaiser 0:3acdd563582f 14 DigitalOut Dir(p21);
jessekaiser 0:3acdd563582f 15 PwmOut Step(p22);
jessekaiser 23:4d050e85e863 16
jessekaiser 23:4d050e85e863 17 //Signal to and from computer
jessekaiser 23:4d050e85e863 18 Serial pc(USBTX, USBRX);
jessekaiser 23:4d050e85e863 19
jessekaiser 24:c4c5d30a3938 20 DigitalOut Enable(p25);
jessekaiser 23:4d050e85e863 21
jessekaiser 23:4d050e85e863 22 //Microstepping
jessekaiser 0:3acdd563582f 23 DigitalOut MS1(p27);
jessekaiser 0:3acdd563582f 24 DigitalOut MS2(p28);
jessekaiser 0:3acdd563582f 25 DigitalOut MS3(p29);
jessekaiser 23:4d050e85e863 26
jessekaiser 23:4d050e85e863 27 //Potmeter and EMG
jessekaiser 10:7f94cd65c910 28 AnalogIn Pot1(p19);
jessekaiser 31:372ff8d49430 29 AnalogIn Pot2(p20);
jessekaiser 25:144eb5822aa7 30 AnalogIn emg0(p17);
jessekaiser 28:593929bbdb98 31 HIDScope scope(2);
jessekaiser 29:83f005c637be 32 Ticker scopeTimer;
jessekaiser 25:144eb5822aa7 33
jessekaiser 23:4d050e85e863 34 //lcd
jessekaiser 30:0a8f849e0292 35 C12832_LCD lcd;
jessekaiser 0:3acdd563582f 36
jessekaiser 23:4d050e85e863 37 //Variables for motor control
jessekaiser 32:46b18f465600 38 float setpoint = 9000; //Frequentie
jessekaiser 23:4d050e85e863 39 float step_freq = 1;
jessekaiser 23:4d050e85e863 40
jessekaiser 23:4d050e85e863 41
jessekaiser 23:4d050e85e863 42 // Filters
jessekaiser 23:4d050e85e863 43 arm_biquad_casd_df1_inst_f32 lowpass_pot;
jessekaiser 23:4d050e85e863 44 arm_biquad_casd_df1_inst_f32 lowpass_step;
jessekaiser 23:4d050e85e863 45
jessekaiser 29:83f005c637be 46 //lowpass filter settings: Fc = 1 Hz, Fs = 100 Hz, Gain = -3 dB onepole-lp
jessekaiser 27:c7b1851c9bb7 47 float lowpass_const[] = {0.0201, 0.0402 , 0.0201, 1.5610, -0.6414};
jessekaiser 31:372ff8d49430 48 //Lowpass filter potmeter: Fc = 0.5 Hz, Fs = 500 Hz,
jessekaiser 27:c7b1851c9bb7 49 //float lowpass_const[] = {0.000009825916403675327, 0.000019651832807350654, 0.000009825916403675327, 1.991114207740345, -0.9911535114059596};
jessekaiser 23:4d050e85e863 50
jessekaiser 23:4d050e85e863 51 //EMG filter
jessekaiser 23:4d050e85e863 52 arm_biquad_casd_df1_inst_f32 lowpass_biceps;
jessekaiser 23:4d050e85e863 53 //lowpass filter settings biceps: Fc = 2 Hz, Fs = 500 Hz, Gain = -3 dB
jessekaiser 23:4d050e85e863 54 float lowpass2_const[] = {0.00015514839749793376, 0.00031029679499586753, 0.00015514839749793376, 1.9644602512795832, -0.9650808448695751};
jessekaiser 23:4d050e85e863 55 arm_biquad_casd_df1_inst_f32 highnotch_biceps;
jessekaiser 31:372ff8d49430 56 //highpass filter settings: Fc = 20 Hz, Fs = 500 Hz, Gain = -3 dB, notch Fc = 50, Fs =500Hz, Gain = -3 dB
jessekaiser 31:372ff8d49430 57 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 58
jessekaiser 23:4d050e85e863 59
jessekaiser 23:4d050e85e863 60 //state values
jessekaiser 23:4d050e85e863 61 float lowpass_biceps_states[4];
jessekaiser 23:4d050e85e863 62 float highnotch_biceps_states[8];
jessekaiser 23:4d050e85e863 63 float lowpass_pot_states[4];
jessekaiser 23:4d050e85e863 64
jessekaiser 23:4d050e85e863 65 //global variabels
jessekaiser 23:4d050e85e863 66 float filtered_biceps;
jessekaiser 29:83f005c637be 67 float filtered_average_bi;
jessekaiser 32:46b18f465600 68 float filt_avg_bi_old;
jessekaiser 23:4d050e85e863 69 float filtered_pot;
jessekaiser 23:4d050e85e863 70 float pot_value1_f32;
jessekaiser 31:372ff8d49430 71 float speed_old;
jessekaiser 31:372ff8d49430 72 float acc;
jessekaiser 31:372ff8d49430 73 float force;
jessekaiser 32:46b18f465600 74 float speed;
jessekaiser 32:46b18f465600 75 float speed_old;
jessekaiser 32:46b18f465600 76 float D;
jessekaiser 32:46b18f465600 77
jessekaiser 23:4d050e85e863 78
jessekaiser 29:83f005c637be 79 void average_biceps(float filtered_biceps,float *average)
jessekaiser 29:83f005c637be 80 {
jessekaiser 29:83f005c637be 81 static float total=0;
jessekaiser 29:83f005c637be 82 static float number=0;
jessekaiser 29:83f005c637be 83 total = total + filtered_biceps;
jessekaiser 29:83f005c637be 84 number = number + 1;
jessekaiser 29:83f005c637be 85 if ( number == 50) {
jessekaiser 29:83f005c637be 86 *average = total/50;
jessekaiser 29:83f005c637be 87 total = 0;
jessekaiser 29:83f005c637be 88 number = 0;
jessekaiser 29:83f005c637be 89 }
jessekaiser 29:83f005c637be 90 }
jessekaiser 24:c4c5d30a3938 91
jessekaiser 23:4d050e85e863 92 void looper_emg()
jessekaiser 23:4d050e85e863 93 {
jessekaiser 23:4d050e85e863 94 /*variable to store value in*/
jessekaiser 25:144eb5822aa7 95 volatile uint16_t emg_value1;
jessekaiser 23:4d050e85e863 96
jessekaiser 23:4d050e85e863 97 float emg_value1_f32;
jessekaiser 31:372ff8d49430 98
jessekaiser 23:4d050e85e863 99
jessekaiser 23:4d050e85e863 100 /*put raw emg value both in red and in emg_value*/
jessekaiser 25:144eb5822aa7 101 emg_value1 = emg0.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V)
jessekaiser 23:4d050e85e863 102 emg_value1_f32 = emg0.read();
jessekaiser 23:4d050e85e863 103
jessekaiser 23:4d050e85e863 104 //process emg biceps
jessekaiser 23:4d050e85e863 105 arm_biquad_cascade_df1_f32(&highnotch_biceps, &emg_value1_f32, &filtered_biceps, 1 );
jessekaiser 31:372ff8d49430 106 filtered_biceps = fabs(filtered_biceps);
jessekaiser 23:4d050e85e863 107 arm_biquad_cascade_df1_f32(&lowpass_biceps, &filtered_biceps, &filtered_biceps, 1 );
jessekaiser 29:83f005c637be 108 average_biceps(filtered_biceps,&filtered_average_bi);
jessekaiser 23:4d050e85e863 109
jessekaiser 23:4d050e85e863 110 /*send value to PC. */
jessekaiser 29:83f005c637be 111 scope.set(0,filtered_average_bi); //Raw EMG signal biceps
jessekaiser 29:83f005c637be 112 scope.set(1,filtered_biceps); //Filtered signal
jessekaiser 31:372ff8d49430 113
jessekaiser 23:4d050e85e863 114 }
jessekaiser 31:372ff8d49430 115 /*void looper_pot()
jessekaiser 23:4d050e85e863 116 {
jessekaiser 23:4d050e85e863 117
jessekaiser 23:4d050e85e863 118 pot_value1_f32 = Pot1.read() - 0.500;
jessekaiser 23:4d050e85e863 119
jessekaiser 23:4d050e85e863 120 //process input
jessekaiser 23:4d050e85e863 121 arm_biquad_cascade_df1_f32(&lowpass_pot, &pot_value1_f32, &filtered_pot, 1 );
jessekaiser 31:372ff8d49430 122
jessekaiser 31:372ff8d49430 123 }*/
jessekaiser 23:4d050e85e863 124
jessekaiser 27:c7b1851c9bb7 125
jessekaiser 31:372ff8d49430 126 /*void looper_motor()
jessekaiser 23:4d050e85e863 127 {
jessekaiser 23:4d050e85e863 128 float new_step_freq;
jessekaiser 30:0a8f849e0292 129 float speed;
jessekaiser 31:372ff8d49430 130
jessekaiser 30:0a8f849e0292 131 speed = 0.02*filtered_average_bi + 0.02*filt_avg_bi_old + 0.96*speed_old; //Value between 0 and 1
jessekaiser 30:0a8f849e0292 132 new_step_freq = (setpoint*speed);
jessekaiser 23:4d050e85e863 133 step_freq = abs(new_step_freq); //Gives the PWM frequenty to the motor.
jessekaiser 30:0a8f849e0292 134 speed_old = speed;
jessekaiser 30:0a8f849e0292 135 filt_avg_bi_old = filtered_average_bi;
jessekaiser 26:b88ff19ff5dc 136
jessekaiser 30:0a8f849e0292 137 if (step_freq < 500 || step_freq > 8000) {
jessekaiser 26:b88ff19ff5dc 138 Enable = 1;
jessekaiser 26:b88ff19ff5dc 139 } else {
jessekaiser 26:b88ff19ff5dc 140 Enable = 0;
jessekaiser 26:b88ff19ff5dc 141 }
jessekaiser 31:372ff8d49430 142 Step.period(1.0/(100 + step_freq)); //Step_freq is het aantal Hz.
jessekaiser 31:372ff8d49430 143
jessekaiser 31:372ff8d49430 144 }*/
jessekaiser 31:372ff8d49430 145 //Motor accelereren met EMG treshold
jessekaiser 31:372ff8d49430 146 /*void looper_motor()
jessekaiser 31:372ff8d49430 147 {
jessekaiser 31:372ff8d49430 148 float new_step_freq;
jessekaiser 31:372ff8d49430 149 Dir = 0;
jessekaiser 31:372ff8d49430 150
jessekaiser 31:372ff8d49430 151 if (filtered_average_bi > tres_bi) {
jessekaiser 31:372ff8d49430 152 Enable = 0;
jessekaiser 31:372ff8d49430 153 new_step_freq = ((1-P)*setpoint) + (P*step_freq);
jessekaiser 31:372ff8d49430 154 step_freq = new_step_freq;
jessekaiser 31:372ff8d49430 155 Step.period(1.0/step_freq);
jessekaiser 31:372ff8d49430 156 } else {
jessekaiser 31:372ff8d49430 157 Enable = 1;
jessekaiser 31:372ff8d49430 158 step_freq = 1;
jessekaiser 31:372ff8d49430 159 }
jessekaiser 31:372ff8d49430 160
jessekaiser 31:372ff8d49430 161 }*/
jessekaiser 26:b88ff19ff5dc 162
jessekaiser 31:372ff8d49430 163 void looper_motor()
jessekaiser 31:372ff8d49430 164 {
jessekaiser 31:372ff8d49430 165 Dir = 0;
jessekaiser 31:372ff8d49430 166 force = K_Gain*filtered_biceps;
jessekaiser 31:372ff8d49430 167 force = force - D;
jessekaiser 31:372ff8d49430 168 acc = force/Mass;
jessekaiser 32:46b18f465600 169 speed = speed_old + (acc * dt);
jessekaiser 32:46b18f465600 170 D = speed * Damp;
jessekaiser 32:46b18f465600 171 step_freq = (setpoint*speed);
jessekaiser 31:372ff8d49430 172 Step.period(1.0/step_freq);
jessekaiser 32:46b18f465600 173 speed_old = speed;
jessekaiser 31:372ff8d49430 174
jessekaiser 31:372ff8d49430 175 if (step_freq < 800) {
jessekaiser 31:372ff8d49430 176 Enable = 1;
jessekaiser 31:372ff8d49430 177 } else {
jessekaiser 31:372ff8d49430 178 Enable = 0;
jessekaiser 31:372ff8d49430 179 }
jessekaiser 23:4d050e85e863 180 }
jessekaiser 31:372ff8d49430 181
jessekaiser 31:372ff8d49430 182
jessekaiser 0:3acdd563582f 183 int main()
jessekaiser 0:3acdd563582f 184 {
jessekaiser 31:372ff8d49430 185 // Attach the HIDScope::send method from the scope object to the timer at 50Hz. Hier wordt de sample freq aangegeven.
jessekaiser 31:372ff8d49430 186 scopeTimer.attach_us(&scope, &HIDScope::send, 2e3);
jessekaiser 31:372ff8d49430 187
jessekaiser 31:372ff8d49430 188 /* Ticker log_timer;
jessekaiser 31:372ff8d49430 189 //set up filters. Use external array for constants
jessekaiser 31:372ff8d49430 190 arm_biquad_cascade_df1_init_f32(&lowpass_pot, 1 , lowpass_const, lowpass_pot_states);
jessekaiser 31:372ff8d49430 191 log_timer.attach(looper_pot, 0.01);*/
jessekaiser 26:b88ff19ff5dc 192
jessekaiser 28:593929bbdb98 193 Ticker emgtimer;
jessekaiser 25:144eb5822aa7 194 arm_biquad_cascade_df1_init_f32(&lowpass_biceps, 1 , lowpass_const, lowpass_biceps_states);
jessekaiser 25:144eb5822aa7 195 arm_biquad_cascade_df1_init_f32(&highnotch_biceps, 2 , highnotch_const, highnotch_biceps_states);
jessekaiser 30:0a8f849e0292 196 emgtimer.attach(looper_emg, 0.002);
jessekaiser 23:4d050e85e863 197
jessekaiser 30:0a8f849e0292 198 Ticker looptimer;
jessekaiser 31:372ff8d49430 199 looptimer.attach(looper_motor, 0.01);
jessekaiser 23:4d050e85e863 200
jessekaiser 4:e4341e3524dc 201 MS1 = 1;
jessekaiser 23:4d050e85e863 202 MS2 = 0;
jessekaiser 2:92a63245d11c 203 MS3 = 0;
jessekaiser 27:c7b1851c9bb7 204 //Step.period(1/step_freq);
jessekaiser 9:649229691351 205 Step.write(0.5); // Duty cycle van 50%
jessekaiser 23:4d050e85e863 206
jessekaiser 0:3acdd563582f 207 while (1) {
jessekaiser 23:4d050e85e863 208
jessekaiser 30:0a8f849e0292 209 lcd.printf("Freq %.0f Hz \n", step_freq); //snelheid meting op lcd
jessekaiser 29:83f005c637be 210 //pc.printf(" %.4f \n", Pot1.read());
jessekaiser 26:b88ff19ff5dc 211 //lcd.printf("filt %.3f raw %.3f \n", filtered_biceps, emg0.read());
jessekaiser 32:46b18f465600 212 //pc.printf("speed %.0f Hz p1 %.4f \n", step_freq, pot_value1_f32); //snelheid meting op lcd
jessekaiser 28:593929bbdb98 213 wait(0.01);
jessekaiser 23:4d050e85e863 214
jessekaiser 10:7f94cd65c910 215
jessekaiser 0:3acdd563582f 216 }
jessekaiser 26:b88ff19ff5dc 217 }