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

Files at this revision

API Documentation at this revision

Revision 44:d5aa53e4778c, committed 2015-06-04

Comitter:
jessekaiser
Date:
Thu Jun 04 14:36:52 2015 +0000
Parent:
43:42bfab67c4a5
Child:
45:f5d74c7f8fbf
Commit message:
Working Test Setup!

Changed in this revision

HIDScope.lib Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed-dsp.lib Show diff for this revision Revisions of this file 
--- a/HIDScope.lib	Thu Jun 04 14:34:04 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/tomlankhorst/code/HIDScope/#e44574634162

--- a/main.cpp	Thu Jun 04 14:34:04 2015 +0000
+++ b/main.cpp	Thu Jun 04 14:36:52 2015 +0000
@@ -1,176 +1,43 @@
 #include "mbed.h"
 #include "C12832_lcd.h"
-#include "arm_math.h"
-#include "HIDScope.h"
 
-#define K_Gain      14      //Gain of the filtered EMG signal
-#define Damp        5       //Deceleration of the motor
-#define Mass        1       // Mass value
-#define dt          0.002   //Sample frequency
-#define MAX_bi      0.09    //Can be used for normalisation of the EMG signal of the biceps
-#define MAX_tri     0.09
-#define MIN_freq    500     //The motor turns off below this frequency
-#define EMG_tresh   0.02
+#define P_GAIN 0.998
 
-//Motor control
 DigitalOut Dir(p21);
 PwmOut Step(p22);
-
-//Signal to and from computer
-Serial pc(USBTX, USBRX);
-
-DigitalOut Enable(p25);
-
-//Microstepping
+DigitalOut Enable(p14);
 DigitalOut MS1(p27);
 DigitalOut MS2(p28);
 DigitalOut MS3(p29);
-
-//Potmeter and EMG
 AnalogIn Pot1(p19);
 AnalogIn Pot2(p20);
-
-AnalogIn emg1(p17); //EMG bordje bovenop, biceps
-AnalogIn emg2(p15); //triceps
-HIDScope scope(2);
-Ticker   scopeTimer;
-
-//lcd
 C12832_LCD lcd;
 
-//Variables for motor control
-float setpoint = 4400; //Frequentie setpint
-float step_freq1 = 1;
-float step_freq2 = 1;
-
-//EMG filter
-arm_biquad_casd_df1_inst_f32 lowpass_biceps;
-arm_biquad_casd_df1_inst_f32 lowpass_triceps;
-//lowpass filter settings: Fc = 2 Hz, Fs = 500 Hz, Gain = -3 dB
-float lowpass_const[] = {0.00015514839749793376, 0.00031029679499586753, 0.00015514839749793376, 1.9644602512795832, -0.9650808448695751};
-arm_biquad_casd_df1_inst_f32 highnotch_biceps;
-arm_biquad_casd_df1_inst_f32 highnotch_triceps;
-//highpass filter settings: Fc = 20 Hz, Fs = 500 Hz, notch Fc = 50, Fs = 500 Hz
-float highnotch_const[] = {0.8370879899975344, -1.6741759799950688, 0.8370879899975344, 1.6474576182593796, -0.7008943417307579, 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, 1.1429772843080923, -0.41279762014290533};
-
-//state values
-float lowpass_biceps_states[4];
-float highnotch_biceps_states[8];
-float lowpass_triceps_states[4];
-float highnotch_triceps_states[8];
-
-//global variabels
-float filtered_biceps;
-float filtered_triceps;
-float speed_old1;
-float speed_old2;
-float acc1;
-float acc2;
-float force1;
-float force2;
-float speed1;
-float speed2;
-float damping1;
-float damping2;
-
-void looper_emg()
-{
-
-
-    float emg_value1_f32;
-    emg_value1_f32 = emg1.read();
-
-    float emg_value2_f32;
-    emg_value2_f32 = emg2.read();
-
-    //process emg biceps
-    arm_biquad_cascade_df1_f32(&highnotch_biceps, &emg_value1_f32, &filtered_biceps, 1 );
-    filtered_biceps = fabs(filtered_biceps);
-    arm_biquad_cascade_df1_f32(&lowpass_biceps, &filtered_biceps, &filtered_biceps, 1 );
-
-    //process emg triceps
-    arm_biquad_cascade_df1_f32(&highnotch_triceps, &emg_value2_f32, &filtered_triceps, 1 );
-    filtered_triceps = fabs(filtered_triceps);
-    arm_biquad_cascade_df1_f32(&lowpass_triceps, &filtered_triceps, &filtered_triceps, 1 );
-
-    /*send value to PC. */
-    scope.set(0,filtered_biceps); //Filtered EMG signal
-    scope.set(1,filtered_triceps);
-}
-
-void looper_motor()
-{
-    //Vooruit
-    force1 = K_Gain*(filtered_biceps/MAX_bi);
-    force1 = force1 - damping1;
-    acc1 = force1/Mass;
-    speed1 = speed_old1 + (acc1 * dt);
-    damping1 = speed1 * Damp;
-    step_freq1 = (setpoint*speed1);
-    speed_old1 = speed1;
-    
-    //Achteruit triceps
-    force2 = K_Gain*(filtered_triceps/MAX_tri);
-    force2 = force2 - damping2;
-    acc2 = force2/Mass;
-    speed2 = speed_old2 + (acc2 * dt);
-    damping2 = speed2 * Damp;
-    step_freq2 = (setpoint*speed2);
-    speed_old2 = speed2;
-    if (filtered_biceps > filtered_triceps) {
-        Dir = 0;
-        speed2 = 0.01;
-        speed_old2 = 0.01;
-        Step.period(1.0/step_freq1);
-    } if (filtered_triceps > filtered_biceps) {
-        Dir = 1;
-        speed1 = 0.01;
-        speed_old1 = 0.01;  
-        Step.period(1.0/step_freq2);
-    }
-    //Speed limit
-    if (speed1 > 1) {
-        speed1 = 1;
-        step_freq1 = setpoint;
-    }
-    if (speed2 > 1) {
-        speed2 = 1;
-        step_freq2 = setpoint;
-    }
-    //EMG treshold
-    if (filtered_biceps < EMG_tresh && filtered_triceps < EMG_tresh) {
-        Enable = 1; //Enable = 1 turns the motor off. 
-    } else {
-        Enable = 0;
-    }
-
-}
-
+BusIn Joystick(p12,p13,p14,p15,p16);
+DigitalIn Up(p15);
+DigitalIn Down(p12);
 int main()
 {
-    // Attach the HIDScope::send method from the scope object to the timer at 500Hz. Hier wordt de sample freq aangegeven.
-    scopeTimer.attach_us(&scope, &HIDScope::send, 2e3);
-
-    Ticker emgtimer;
-    arm_biquad_cascade_df1_init_f32(&lowpass_biceps, 1 , lowpass_const, lowpass_biceps_states);
-    arm_biquad_cascade_df1_init_f32(&highnotch_biceps, 2 , highnotch_const, highnotch_biceps_states);
-    arm_biquad_cascade_df1_init_f32(&lowpass_triceps, 1 , lowpass_const, lowpass_triceps_states);
-    arm_biquad_cascade_df1_init_f32(&highnotch_triceps, 2 , highnotch_const, highnotch_triceps_states);
-    emgtimer.attach(looper_emg, 0.002);
-
-    Ticker looptimer;
-    looptimer.attach(looper_motor, 0.01); //Uitzoeken waarom deze frequentie!
+    Enable = 0;
+    float setpoint = 2500; //Frequentie 
+    float step_freq = 1;
+    MS1 = 1;
+    MS2 = 1;
+    MS3 = 0;
+    float p1;
+    Step.period(1./step_freq); // 1 kHz, vanaf 2,5 kHz doet de motor het niet meer.
+    Step.write(0.5); // Duty cycle van 50%
+   // Dir = Pot1; // Dir 1 is naar boven, Dir 0 naar onder.
+    Enable = 1;
+    while (1) {
+        p1 = Pot1.read();
+        Dir = 0;
+        float new_step_freq;
+        new_step_freq = ((1-P_GAIN)*setpoint) + (P_GAIN*step_freq);
+        step_freq = new_step_freq;
+        Step.period(1.0/step_freq);
+        lcd.printf("freq : %.0f \n", step_freq);
+        wait(0.01); //Hier nog ticker inbouwen
 
-    //Microstepping control
-    MS1 = 1;
-    MS2 = 0;
-    MS3 = 0;
-    Step.write(0.5); // Duty cycle van 50%
-
-    while (1) {
-
-        //lcd.printf("Bi %.2f ,Tri %.2f \n", filtered_biceps, filtered_triceps); //snelheid meting op lcd
-        lcd.printf("1 %.0f, 2 %.0f \n", step_freq1, step_freq2);
-        wait(0.01);
     }
-}
+}
\ No newline at end of file

--- a/mbed-dsp.lib	Thu Jun 04 14:34:04 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/teams/mbed-official/code/mbed-dsp/#7a284390b0ce