Diff: main.cpp

Revision:

6:d03e4fa3a2a5

Parent:

5:19f8766b63da

Child:

7:20757784f5bf

--- a/main.cpp	Wed Jan 13 09:09:22 2016 +0000
+++ b/main.cpp	Wed Jan 13 09:15:12 2016 +0000
@@ -13,19 +13,10 @@
 #include "arm_math.h"
 #include "HIDScope.h"
 
-#define P_Gain      0.99
-#define K_Gain      150   //Gain of the filtered EMG signal
-#define Damp        5    //Deceleration of the motor
-#define Mass        1    // Mass value
-#define dt          0.01 //Sample frequency
 #define EMG_tresh1   0.015
 #define EMG_tresh2   0.015
 #define EMG_tresh3   0.015
 #define EMG_tresh4   0.015
-#define H_Gain  5
-#define Pt_x    0.80
-#define Pt_y    0.50
-#define error_tresh 0.02
 
 MODSERIAL pc(USBTX,USBRX);
 //joystick control
@@ -92,18 +83,6 @@
 arm_biquad_casd_df1_inst_f32 bandstop_triceps;
 arm_biquad_casd_df1_inst_f32 bandstop_pect;
 arm_biquad_casd_df1_inst_f32 bandstop_deltoid;
-/*
-//200hz sampling test
-
-//lowpass 1 filter settings: Fc = 90 Hz, Fs = 200 Hz, Gain = -3 dB
-//lowpass 2 filter settings: Fc = 0.8 Hz, Fs = 200 Hz, Gain = -3 dB
-float lowpass1_const[] = {0.800592403464570, 1.60118480692914,0.800592403464570,-1.56101807580072,-0.641351538057563};
-float lowpass2_const[] = {0.000155148423475699,  0.000310296846951398,    0.000155148423475699,1.96446058020523,-0.965081173899135};
-//highpass filter settings: Fc = 10 Hz, Fs = 200 Hz
-float highpass_const[] = {0.800592403464570, -1.60118480692914,   0.800592403464570,1.56101807580072 ,-0.641351538057563};
-//bandstop filter settings Fc=[45Hz 55Hz], Fs= 200Hz
-float bandstop_const[] = {0.800592403464570, -9.80442924324572e-17,   0.800592403464570,-0.201669174120189,-0.800844265795519 , 1,-1.22464679914735e-16 ,  1 ,0.201669174120189,-0.800844265795519};
-*/
 
 //lowpass 1 filter settings: Fc = 49 Hz, Fs = 100 Hz, Gain = -3 dB
 //lowpass 2 filter settings: Fc = 0.8 Hz, Fs = 100 Hz, Gain = -3 dB
@@ -125,28 +104,7 @@
 float highpass_const[] = {0.391335772501769 ,-0.782671545003538,  0.391335772501769,0.369527377351241,  -0.195815712655833};
 
 */
-/*
-//suffer from high wire motion influence and dont react to continuous tension only rapid activation
 
-//lowpass 1 filter settings: Fc = 20 Hz, Fs = 100 Hz, Gain = -3 dB
-//lowpass 2 filter settings: Fc = 0.3 Hz, Fs = 100 Hz, Gain = -3 dB
-float lowpass1_const[] = {0.206572083826148,0.413144167652296,0.206572083826148,0.369527377351241,-0.195815712655833};
-float lowpass2_const[] = {0.0000876555487540065, 0.000175311097508013, 0.0000876555487540065, 1.97334424978130, -0.973694871976315};
-//highpass filter settings: Fc = 3 Hz, Fs = 100 Hz
-
-float highpass_const[] = {0.875183092438135,-1.75036618487627,0.875183092438135,1.73472576880928,-0.766006600943264};
-*/
-
-/*
-Previous coefficients testing to see if new variants work
-lowpass 1 filter settings: Fc = 45 Hz, Fs = 100 Hz, Gain = -3 dB
-lowpass 2 filter settings: Fc = 2 Hz, Fs = 100 Hz, Gain = -3 dB
-float lowpass1_const[] = {0.800592403464570, 1.60118480692914, 0.800592403464570, -1.56101807580072, -0.641351538057563};
-float lowpass2_const[] = {0.0000876555487540065, 0.000175311097508013, 0.0000876555487540065, 1.97334424978130, -0.973694871976315};
-//highpass filter settings: Fc = 20 Hz, Fs = 100 Hz
-
-float highpass_const[] = {0.391335772501769, -0.782671545003537,0.391335772501769,0.369527377351241,-0.195815712655833 };
-*/
 
 //state values
 float lowpass1_biceps_states[4];
@@ -240,114 +198,44 @@
 {
 
     emg_y = (filtered_biceps - filtered_triceps);
-    /*emg_y_abs = fabs(emg_y);
-    force1 = emg_y_abs*K_Gain;
-    force1 = force1 - damping1;
-    acc1 = force1/Mass;
-    speed1 = speed_old1 + (acc1 * dt);
-    damping1 = speed1 * Damp;
-    step_freq1 = setpoint * speed1;*/
-   // Stepy.period(/*1.0/step_freq1*/0.000625);
-    //speed_old1 = speed1;
-
-    if (emg_y > 0) {
+    
+    if (emg_y > 0) {//downward movement
         Diry = 0;
     }
-    if (emg_y < 0) {
+    if (emg_y < 0) {//upward movement
         Diry = 1;
     }
-    /*//Speed limit
-    if (speed2 > 1) {
-        speed2 = 1;
-        step_freq2 = setpoint;
-    }*/
+
     //EMG treshold
     if (filtered_biceps < EMG_tresh1 && filtered_triceps < EMG_tresh2) {
         Enabley = 1; //Enable = 1 turns the motor off.
     } else {
         Enabley = 0;
     }
-    
-//0.06 for biceps and triceps movement, 0.04 for wrist lower and upper motion right arm lower2 connections
- /*   if (filtered_triceps > 0.04) {//upward cart movement
-        Diry.write(1);
-        Enabley.write(0); //Enable = 1 turns the motor off.
-        Ledr.write(0);
-        Ledb.write(1);
-    }
-    else if (filtered_biceps > 0.04) {// downward cart movement 
-        Diry.write(0);
-        Enabley.write(0); //Enable = 1 turns the motor off.
-        Ledr.write(1);
-        Ledb.write(0);
-    }
-    else if (filtered_triceps < 0.015 && filtered_biceps < 0.015) {//0.04 for biceps and triceps stop, 0.015, 0.015 for wrist lower and upper stop
-        Enabley.write(1);
-    }
-    else{}
-    //Speed limit
-    if (speed1 > 1) {
-        speed1 = 1;
-        step_freq1 = setpoint;
-    }*/
-
 }
 
 void looper_motorx()
 {
 
     emg_x = (filtered_pect - filtered_deltoid);
-    /*emg_x_abs = fabs(emg_x);
-    force2 = emg_x_abs*K_Gain;
-    force2 = force2 - damping2;
-    acc2 = force2/Mass;
-    speed2 = speed_old2 + (acc2 * dt);
-    damping2 = speed2 * Damp;
-    step_freq2 = setpoint * speed2;
-    Stepx.period(1.0/step_freq2);
-    speed_old2 = speed2;
-*/
-    if (emg_x > 0) {
+
+    if (emg_x > 0) {//left movement
         Dirx = 0;
     }
-    if (emg_x < 0) {
+    if (emg_x < 0) {//right movement
         Dirx = 1;
     }
-    /*//Speed limit
-    if (speed2 > 1) {
-        speed2 = 1;
-        step_freq2 = setpoint;
-    }*/
+
     //EMG treshold
     if (filtered_pect < EMG_tresh3 && filtered_deltoid < EMG_tresh4) {
         Enablex = 1; //Enable = 1 turns the motor off.
     } else {
         Enablex = 0;
     }
-/*
-//0.06 for biceps and triceps movement, 0.03 for wrist lower and upper motion left arm upper 2 connections
-    if (filtered_pect > 0.03) {//right cart movement
-        Dirx.write(1);
-        Enablex.write(0); //Enable = 1 turns the motor off.
-        Ledr.write(0);
-        Ledb.write(1);
-    }
-    else if (filtered_deltoid > 0.03) {//left cart movement
-        Dirx.write(0);
-        Enablex.write(0); //Enable = 1 turns the motor off.
-        Ledr.write(1);
-        Ledb.write(0);
-    }
-    else if (filtered_pect < 0.01 && filtered_deltoid < 0.01) {//0.04 for biceps and triceps stop, 0.01 for wrist lower and upper stop
-        Enablex.write(1);
-    }
-    else{}*/
 }
 
 int main()
 {
-    
-    //scopeTimer.attach_us(&scopeSend, 2e3);
     pc.baud(115200);
     Ledr=1;
     Ledg=1;