Revision 53:f783b3192dbb, committed 2015-06-10

Comitter:

jessekaiser

Date:

Wed Jun 10 08:33:52 2015 +0000

Parent:

52:b75248e62240

Child:

54:abb7f76a0473

Commit message:

Endconcept 5. After this control of 1 motor at a time

Changed in this revision

--- a/main.cpp	Tue Jun 09 13:16:50 2015 +0000
+++ b/main.cpp	Wed Jun 10 08:33:52 2015 +0000
@@ -56,7 +56,7 @@
 C12832_LCD lcd;
 
 //Variables for motor control
-float setpoint = 1500; //Frequentie setpint
+float setpoint = 1000; //Frequentie setpint
 float step_freq1 = 1;
 float step_freq2 = 1;
 float step_freq3 = 1;
@@ -88,66 +88,12 @@
 
 //global variabels
 float filtered_biceps, filtered_triceps, filtered_pect, filtered_deltoid;
-float average_bi, average_tri, average_pect, average_delt;
 float speed_old1, speed_old2, speed_old3, speed_old4;
 float acc1, acc2, acc3, acc4;
 float force1, force2, force3, force4;
 float speed1, speed2, speed3, speed4;
 float damping1, damping2, damping3, damping4;
 
-
-void average_biceps(float filtered_biceps,float *average)
-{
-    static float total=0;
-    static float number=0;
-    total = total + filtered_biceps;
-    number = number + 1;
-    if ( number == 50) {
-        *average = total/50;
-        total = 0;
-        number = 0;
-    }
-}
-
-void average_triceps(float filtered_triceps,float *average)
-{
-    static float total=0;
-    static float number=0;
-    total = total + filtered_triceps;
-    number = number + 1;
-    if ( number == 50) {
-        *average = total/50;
-        total = 0;
-        number = 0;
-    }
-}
-
-void average_pectoralis(float filtered_pect,float *average)
-{
-    static float total=0;
-    static float number=0;
-    total = total + filtered_pect;
-    number = number + 1;
-    if ( number == 50) {
-        *average = total/50;
-        total = 0;
-        number = 0;
-    }
-}
-
-void average_deltoid(float filtered_deltoid ,float *average)
-{
-    static float total=0;
-    static float number=0;
-    total = total + filtered_deltoid;
-    number = number + 1;
-    if ( number == 50) {
-        *average = total/50;
-        total = 0;
-        number = 0;
-    }
-}
-
 void looper_emg()
 {
     float emg_value1_f32, emg_value2_f32, emg_value3_f32, emg_value4_f32;
@@ -160,22 +106,22 @@
     arm_biquad_cascade_df1_f32(&highnotch_biceps, &emg_value1_f32, &filtered_biceps, 1 );   //High pass and notch filter
     filtered_biceps = fabs(filtered_biceps);                                                //Rectifier
     arm_biquad_cascade_df1_f32(&lowpass_biceps, &filtered_biceps, &filtered_biceps, 1 );    //low pass filter
-    average_biceps(filtered_biceps,&average_bi);
+
     //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 );
-     average_triceps(filtered_triceps,&average_tri);
+
     //process emg pectoralis major
     arm_biquad_cascade_df1_f32(&highnotch_pect, &emg_value3_f32, &filtered_pect, 1 );
     filtered_pect = fabs(filtered_pect);
     arm_biquad_cascade_df1_f32(&lowpass_pect, &filtered_pect, &filtered_pect, 1 );
-     average_pectoralis(filtered_pect,&average_pect);
+
     //process emg deltoid
     arm_biquad_cascade_df1_f32(&highnotch_deltoid, &emg_value4_f32, &filtered_deltoid, 1 );
     filtered_deltoid = fabs(filtered_deltoid);
     arm_biquad_cascade_df1_f32(&lowpass_deltoid, &filtered_deltoid, &filtered_deltoid, 1 );
-     average_deltoid(filtered_deltoid,&average_delt);
+
     /*send value to PC. */
     scope.set(0,filtered_biceps); //Filtered EMG signal
     scope.set(1,filtered_triceps);
@@ -186,7 +132,7 @@
 void looper_motory()
 {
     //Forward
-    force1 = K_Gain*(average_bi/MAX_bi);
+    force1 = K_Gain*(filtered_biceps/MAX_bi);
     force1 = force1 - damping1;
     acc1 = force1/Mass;
     speed1 = speed_old1 + (acc1 * dt);
@@ -195,7 +141,7 @@
     speed_old1 = speed1;
 
     //Achteruit triceps
-    force2 = K_Gain*(average_tri/MAX_tri);
+    force2 = K_Gain*(filtered_triceps/MAX_tri);
     force2 = force2 - damping2;
     acc2 = force2/Mass;
     speed2 = speed_old2 + (acc2 * dt);
@@ -239,7 +185,7 @@
 void looper_motorx()
 {
     //To the left
-    force3 = K_Gain*(average_pect/MAX_pect);
+    force3 = K_Gain*(filtered_pect/MAX_pect);
     force3 = force3 - damping3;
     acc3 = force3/Mass;
     speed3 = speed_old3 + (acc3 * dt);
@@ -248,13 +194,14 @@
     speed_old3 = speed3;
 
     //To the right
-    force4 = K_Gain*(average_delt/MAX_delt);
+    force4 = K_Gain*(filtered_deltoid/MAX_delt);
     force4 = force4 - damping4;
     acc4 = force4/Mass;
     speed4 = speed_old4 + (acc4 * dt);
     damping4 = speed4 * Damp;
     step_freq4 = (setpoint*speed4);
     speed_old4 = speed4;
+    
     if (filtered_pect > filtered_deltoid) {
         Dirx = 0;
         speed4 = 0.01;
@@ -305,7 +252,7 @@
     //deltoid
     arm_biquad_cascade_df1_init_f32(&lowpass_deltoid, 1 , lowpass_const, lowpass_deltoid_states);
     arm_biquad_cascade_df1_init_f32(&highnotch_deltoid, 2 , highnotch_const, highnotch_deltoid_states);
-    emgtimer.attach(looper_emg, 0.002);
+    emgtimer.attach(looper_emg, 0.01);
 
     Ticker looptimer1;
     looptimer1.attach(looper_motorx, 0.01); //X-Spindle motor, why this freq?