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@39:191ae0d12bd6, 2015-06-01 (annotated)

Committer:
jessekaiser
Date:
Mon Jun 01 11:34:57 2015 +0000
Revision:
39:191ae0d12bd6
Parent:
38:c592354f5080
Child:
40:0cfd96cb25fa
Poging 2, werkt waarschijnlijk slechter;

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