Diff: main.cpp

Revision:

4:61bdf601e7b0

Parent:

3:3d5ad874add0

Child:

5:19f8766b63da

--- a/main.cpp	Fri Jan 08 11:33:34 2016 +0000
+++ b/main.cpp	Fri Jan 08 11:55:49 2016 +0000
@@ -18,10 +18,10 @@
 #define Damp        5    //Deceleration of the motor
 #define Mass        1    // Mass value
 #define dt          0.01 //Sample frequency
-#define EMG_tresh1   0.01
-#define EMG_tresh2   0.01
-#define EMG_tresh3   0.01
-#define EMG_tresh4   0.01
+#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
@@ -55,10 +55,10 @@
 DigitalOut Ledb(LED3);
 
 //EMG inputs
-AnalogIn emg1(A2); //biceps bottom emg board
-AnalogIn emg2(A3); //triceps
-AnalogIn emg3(A4); //Pectoralis major
-AnalogIn emg4(A5); //Deltoid top emg board
+AnalogIn emg1(A2); //biceps or wrist flexors bottom emg board
+AnalogIn emg2(A3); //triceps or wirst extensors
+AnalogIn emg3(A4); //Pectoralis major or wrist extensors
+AnalogIn emg4(A5); //Deltoid or wrist flexors top emg board
 
 HIDScope scope(4);
 Ticker   scopeTimer;
@@ -82,6 +82,11 @@
 arm_biquad_casd_df1_inst_f32 lowpass2_pect;
 arm_biquad_casd_df1_inst_f32 lowpass2_deltoid;
 
+arm_biquad_casd_df1_inst_f32 highpass_biceps;
+arm_biquad_casd_df1_inst_f32 highpass_triceps;
+arm_biquad_casd_df1_inst_f32 highpass_pect;
+arm_biquad_casd_df1_inst_f32 highpass_deltoid;
+
 //used as extra filter for wrist motion
 arm_biquad_casd_df1_inst_f32 bandstop_biceps;
 arm_biquad_casd_df1_inst_f32 bandstop_triceps;
@@ -93,11 +98,10 @@
 float lowpass1_const[] = {0.956543225556877,1.91308645111375,0.956543225556877,-1.91119706742607,-0.914975834801434};
 float lowpass2_const[] = {0.000609854718717299,0.00121970943743460,0.000609854718717299,1.92894226325203,-0.931381682126903};
 //highpass filter settings: Fc = 20 Hz, Fs = 100 Hz
-
 float highpass_const[] = {0.391335772501769,-0.782671545003538,0.391335772501769,0.369527377351241,-0.195815712655833};
-
 //bandstop filter settings Fc=[29Hz 36Hz], Fs= 100Hz
 float bandstop_const[] = {0.732022476556623, 0.681064744276583,0.732022476556623,-0.535944148680739,-0.713976335521464,1,0.930387749130681,1,-1.04147956553087,-0.752398361226849};
+
 /*
 //values are usable for triceps and biceps continuous motion, not for wrist motion.
 //lowpass 1 filter settings: Fc = 45 Hz, Fs = 100 Hz, Gain = -3 dB
@@ -131,10 +135,6 @@
 
 float highpass_const[] = {0.391335772501769, -0.782671545003537,0.391335772501769,0.369527377351241,-0.195815712655833 };
 */
-arm_biquad_casd_df1_inst_f32 highpass_biceps;
-arm_biquad_casd_df1_inst_f32 highpass_triceps;
-arm_biquad_casd_df1_inst_f32 highpass_pect;
-arm_biquad_casd_df1_inst_f32 highpass_deltoid;
 
 //state values
 float lowpass1_biceps_states[4];
@@ -158,7 +158,7 @@
 float bandstop_deltoid_states[8];
 
 
-//global variabels
+//global variables
 float filtered_biceps, filtered_triceps, filtered_pect, filtered_deltoid;
 float speed_old1, speed_old2, speed_old3, speed_old4;
 float acc1, acc2, acc3, acc4;
@@ -185,40 +185,38 @@
     emg_value3_f32 = emg3.read();
     emg_value4_f32 = emg4.read();
     
-    //process emg biceps
+    //Biquad process emg biceps
     arm_biquad_cascade_df1_f32(&bandstop_biceps, &emg_value1_f32, &filtered_biceps, 1 );//used for wrist motion
     arm_biquad_cascade_df1_f32(&lowpass1_biceps, &filtered_biceps, &filtered_biceps, 1 );
     arm_biquad_cascade_df1_f32(&highpass_biceps, &filtered_biceps, &filtered_biceps, 1 );
     filtered_biceps = fabs(filtered_biceps);                                                //Rectifier, The Gain is already implemented.
     arm_biquad_cascade_df1_f32(&lowpass2_biceps, &filtered_biceps, &filtered_biceps, 1 );    //low pass filter
 
-    //process emg triceps
+    //Biquad process emg triceps
    arm_biquad_cascade_df1_f32(&bandstop_triceps, &emg_value2_f32, &filtered_triceps, 1 );    //used for wrist motion
    arm_biquad_cascade_df1_f32(&lowpass1_triceps, &filtered_triceps, &filtered_triceps, 1 );
    arm_biquad_cascade_df1_f32(&highpass_triceps, &filtered_triceps, &filtered_triceps, 1 );
    filtered_triceps = fabs(filtered_triceps);
    arm_biquad_cascade_df1_f32(&lowpass2_triceps, &filtered_triceps, &filtered_triceps, 1 );
 
-    //process emg pectoralis major
+    //Biquad process emg pectoralis major
     arm_biquad_cascade_df1_f32(&bandstop_pect, &emg_value3_f32, &filtered_pect, 1 );//used for wrist motion
     arm_biquad_cascade_df1_f32(&lowpass1_pect, &filtered_pect, &filtered_pect, 1 );
     arm_biquad_cascade_df1_f32(&highpass_pect, &filtered_pect, &filtered_pect, 1 );
     filtered_pect = fabs(filtered_pect);
     arm_biquad_cascade_df1_f32(&lowpass2_pect, &filtered_pect, &filtered_pect, 1 );
 
-    //process emg deltoid
+    //Biquad process emg deltoid
     arm_biquad_cascade_df1_f32(&bandstop_deltoid, &emg_value4_f32, &filtered_deltoid, 1 );//used for wrist motion
     arm_biquad_cascade_df1_f32(&lowpass1_deltoid, &filtered_deltoid, &filtered_deltoid, 1 );
     arm_biquad_cascade_df1_f32(&highpass_deltoid, &filtered_deltoid, &filtered_deltoid, 1 );
     filtered_deltoid = fabs(filtered_deltoid);
     arm_biquad_cascade_df1_f32(&lowpass2_deltoid, &filtered_deltoid, &filtered_deltoid, 1 );
  
-// send value to PC.
+// send value to PC for control
 
     scope.set(0,filtered_biceps); //Filtered EMG signals
     scope.set(1,filtered_triceps);
-    /*scope.set(2,filtered_pect);
-    scope.set(3,filtered_deltoid);*/
     scope.set(2,filtered_pect);
     scope.set(3,filtered_deltoid);
     scope.send();
@@ -229,8 +227,8 @@
 void looper_motory()
 {
 
-    /*emg_y = (filtered_biceps - filtered_triceps);
-    emg_y_abs = fabs(emg_y);
+    emg_y = (filtered_biceps - filtered_triceps);
+    /*emg_y_abs = fabs(emg_y);
     force1 = emg_y_abs*K_Gain;
     force1 = force1 - damping1;
     acc1 = force1/Mass;
@@ -239,9 +237,27 @@
     step_freq1 = setpoint * speed1;*/
    // Stepy.period(/*1.0/step_freq1*/0.000625);
     //speed_old1 = speed1;
+
+    if (emg_y > 0) {
+        Diry = 1;
+    }
+    if (emg_y < 0) {
+        Diry = 0;
+    }
+    /*//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
+ /*   if (filtered_triceps > 0.04) {//upward cart movement
         Diry.write(1);
         Enabley.write(0); //Enable = 1 turns the motor off.
         Ledr.write(0);
@@ -258,7 +274,7 @@
     }
     else{}
     //Speed limit
-    /*if (speed1 > 1) {
+    if (speed1 > 1) {
         speed1 = 1;
         step_freq1 = setpoint;
     }*/
@@ -313,8 +329,8 @@
     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{}
-}*/
+    else{}*/
+}
 
 int main()
 {