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@45:f5d74c7f8fbf, 2015-06-05 (annotated)

Committer:
jessekaiser
Date:
Fri Jun 05 09:22:43 2015 +0000
Revision:
45:f5d74c7f8fbf
Parent:
44:d5aa53e4778c
Child:
46:7a7cb589579a
2 directie aansturingen werkt nu! Ook geen pull out torque kwesties meer.

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