Diff: main.cpp

Revision:

1:c8ad338ba312

Parent:

0:34751b6a7dc9

Child:

2:083d74325bfb

diff -r 34751b6a7dc9 -r c8ad338ba312 main.cpp
--- a/main.cpp	Thu Jan 07 08:54:02 2016 +0000
+++ b/main.cpp	Thu Jan 07 12:47:35 2016 +0000
@@ -1,9 +1,9 @@
 /*Code originally by Jesse Kaiser, s1355783 for control of the 2DOF Planar Table
 Some variables are also numbered at the end. The numbers stands for the muscle that controls it.
-Biceps =            1
-Triceps =           2
-Pectoralis Major =  3
-Deltoid posterior = 4
+Biceps =            1 = lower right arm
+Triceps =           2 = upper right arm
+Pectoralis Major =  3 = upper left arm
+Deltoid posterior = 4 = lower left arm
 The "x" and "y" at the end of variables stand for the X-Spindle or Y-Spindle respectivly.
 The code has been revised to work with the new board and also has a secondary way of controlling it
 */
@@ -82,6 +82,24 @@
 arm_biquad_casd_df1_inst_f32 lowpass2_pect;
 arm_biquad_casd_df1_inst_f32 lowpass2_deltoid;
 
+//used as extra filter for wrist motion
+arm_biquad_casd_df1_inst_f32 bandstop_biceps;
+arm_biquad_casd_df1_inst_f32 bandstop_triceps;
+arm_biquad_casd_df1_inst_f32 bandstop_pect;
+arm_biquad_casd_df1_inst_f32 bandstop_deltoid;
+
+//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
+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
 //lowpass 2 filter settings: Fc = 0.3 Hz, Fs = 100 Hz, Gain = -3 dB
 float lowpass1_const[] = {0.800592403464570,1.60118480692914,0.800592403464570,-1.56101807580072,-0.641351538057563};
@@ -90,7 +108,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
 
@@ -122,15 +140,23 @@
 float lowpass1_biceps_states[4];
 float lowpass2_biceps_states[4];
 float highpass_biceps_states[4];
+float bandstop_biceps_states[8];
+
 float lowpass1_triceps_states[4];
 float lowpass2_triceps_states[4];
 float highpass_triceps_states[4];
+float bandstop_triceps_states[8];
+
 float lowpass1_pect_states[4];
 float lowpass2_pect_states[4];
 float highpass_pect_states[4];
+float bandstop_pect_states[8];
+
 float lowpass1_deltoid_states[4];
 float lowpass2_deltoid_states[4];
 float highpass_deltoid_states[4];
+float bandstop_deltoid_states[8];
+
 
 //global variabels
 float filtered_biceps, filtered_triceps, filtered_pect, filtered_deltoid;
@@ -160,37 +186,41 @@
     emg_value4_f32 = emg4.read();
     
     //process emg biceps
-    arm_biquad_cascade_df1_f32(&lowpass1_biceps, &emg_value1_f32, &filtered_biceps, 1 );
+    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
-   arm_biquad_cascade_df1_f32(&lowpass1_triceps, &emg_value2_f32, &filtered_triceps, 1 );
+   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
-    arm_biquad_cascade_df1_f32(&lowpass1_pect, &emg_value3_f32, &filtered_pect, 1 );
+    //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 );
+    filtered_pect = fabs(filtered_pect);
+    arm_biquad_cascade_df1_f32(&lowpass2_pect, &filtered_pect, &filtered_pect, 1 );
 
     //process emg deltoid
-    arm_biquad_cascade_df1_f32(&lowpass1_deltoid, &emg_value4_f32, &filtered_deltoid, 1 );
+    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 );
- */
+    filtered_deltoid = fabs(filtered_deltoid);
+    arm_biquad_cascade_df1_f32(&lowpass2_deltoid, &filtered_deltoid, &filtered_deltoid, 1 );
+ 
 // send value to PC.
 
-    scope.set(0,filtered_biceps); //Filtered EMG signal
-    scope.set(1,filtered_triceps);
+    scope.set(0,filtered_pect); //Filtered EMG signal
+    scope.set(1,filtered_deltoid);
     /*scope.set(2,filtered_pect);
     scope.set(3,filtered_deltoid);*/
-    scope.set(2,emg1.read());//testing emg signal for cause of variance
-    scope.set(3,emg2.read());
+    scope.set(2,emg3.read());//testing emg signal for cause of variance
+    scope.set(3,emg4.read());
     scope.send();
     
 }
@@ -209,20 +239,21 @@
     step_freq1 = setpoint * speed1;*/
    // Stepy.period(/*1.0/step_freq1*/0.000625);
     //speed_old1 = speed1;
-
-    if (filtered_triceps > 0.06) {
+    
+//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.06) {
+    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.04 && filtered_biceps < 0.04) {
+    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{}
@@ -233,11 +264,11 @@
     }*/
 
 }
-/*
+
 void looper_motorx()
 {
 
-    emg_x = (filtered_pect - filtered_deltoid);
+   /* emg_x = (filtered_pect - filtered_deltoid);
     emg_x_abs = fabs(emg_x);
     force2 = emg_x_abs*K_Gain;
     force2 = force2 - damping2;
@@ -265,8 +296,25 @@
     } 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()
 {
@@ -283,10 +331,8 @@
     MS12=1;
     MS22=1;
     MS32=1;
-/*Diry.write(1);*/
-Stepy.write(0.5f);/*
+Stepy.write(0.5f);
 Enabley.write(1);
-Dirx.write(1);*/
 Stepx.write(0.5f);
 Enablex.write(1);
 Stepy.period(0.000625);//use period change for speed adjustments
@@ -327,21 +373,27 @@
     arm_biquad_cascade_df1_init_f32(&lowpass1_biceps, 1 , lowpass1_const, lowpass1_biceps_states);
     arm_biquad_cascade_df1_init_f32(&lowpass2_biceps, 1 , lowpass2_const, lowpass2_biceps_states);
     arm_biquad_cascade_df1_init_f32(&highpass_biceps, 1 , highpass_const, highpass_biceps_states);
+    arm_biquad_cascade_df1_init_f32(&bandstop_biceps, 2 , bandstop_const, bandstop_biceps_states);
     //triceps
     arm_biquad_cascade_df1_init_f32(&lowpass1_triceps, 1 , lowpass1_const, lowpass1_triceps_states);
     arm_biquad_cascade_df1_init_f32(&lowpass2_triceps, 1 , lowpass2_const, lowpass2_triceps_states);
-   arm_biquad_cascade_df1_init_f32(&highpass_triceps, 1 , highpass_const, highpass_triceps_states);
-    /*//pectoralis major
+    arm_biquad_cascade_df1_init_f32(&highpass_triceps, 1 , highpass_const, highpass_triceps_states);
+    arm_biquad_cascade_df1_init_f32(&bandstop_triceps, 2 , bandstop_const, bandstop_triceps_states);
+    //pectoralis major
     arm_biquad_cascade_df1_init_f32(&lowpass1_pect, 1 , lowpass1_const, lowpass1_pect_states);
     arm_biquad_cascade_df1_init_f32(&lowpass2_pect, 1 , lowpass2_const, lowpass2_pect_states);
     arm_biquad_cascade_df1_init_f32(&highpass_pect, 1 , highpass_const, highpass_pect_states);
+    arm_biquad_cascade_df1_init_f32(&bandstop_pect, 2 , bandstop_const, bandstop_pect_states);
+
     //deltoid
     arm_biquad_cascade_df1_init_f32(&lowpass1_deltoid, 1 , lowpass1_const, lowpass1_deltoid_states);
     arm_biquad_cascade_df1_init_f32(&lowpass2_deltoid, 1 , lowpass2_const, lowpass2_deltoid_states);
-    arm_biquad_cascade_df1_init_f32(&highpass_deltoid, 1 , highpass_const, highpass_deltoid_states);*/
+    arm_biquad_cascade_df1_init_f32(&highpass_deltoid, 1 , highpass_const, highpass_deltoid_states);
+    arm_biquad_cascade_df1_init_f32(&bandstop_deltoid, 2 , bandstop_const, bandstop_deltoid_states);
+
     emgtimer.attach(looper_emg, 0.01);
 
-    //looptimer1.attach(looper_motorx, 0.01); //X-Spindle motor
+    looptimer1.attach(looper_motorx, 0.01); //X-Spindle motor
 
     looptimer2.attach(looper_motory, 0.01); //Y-Spindle motor