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@31:372ff8d49430, 2015-05-21 (annotated)

Committer:
jessekaiser
Date:
Thu May 21 15:38:12 2015 +0000
Revision:
31:372ff8d49430
Parent:
30:0a8f849e0292
Child:
32:46b18f465600
EMG proportional control werkt!!!!!

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