Revision 51:75a8c7191555, committed 2015-06-09

Comitter:

jessekaiser

Date:

Tue Jun 09 13:06:45 2015 +0000

Parent:

50:5e08b74bf023

Child:

52:b75248e62240

Commit message:

Eindconcept 3. De y-motor works for a short time, than stops.

Changed in this revision

--- a/main.cpp	Tue Jun 09 09:46:30 2015 +0000
+++ b/main.cpp	Tue Jun 09 13:06:45 2015 +0000
@@ -4,7 +4,7 @@
 Triceps =           2
 Pectoralis Major =  3
 Deltoid =           4
-The "x" and "y" at the end of variables stand for the X-Spindle or Y-Spindle respectivly. 
+The "x" and "y" at the end of variables stand for the X-Spindle or Y-Spindle respectivly.
 */
 
 #include "mbed.h"
@@ -16,12 +16,12 @@
 #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 MAX_pect    0.09
-#define MAX_delt    0.09
+#define MAX_bi      0.04    //Can be used for normalisation of the EMG signal of the biceps
+#define MAX_tri     0.04
+#define MAX_pect    0.04
+#define MAX_delt    0.04
 #define MIN_freq    500     //The motor turns off below this frequency
-#define EMG_tresh   0.02
+#define EMG_tresh   0.01
 
 //Motor control
 DigitalOut Dirx(p21);
@@ -33,7 +33,7 @@
 Serial pc(USBTX, USBRX);
 
 DigitalOut Enablex(p26); //Connected to green led
-DigitalOut Enabley(p25); //Connected to blue led 
+DigitalOut Enabley(p25); //Connected to blue led
 
 //Microstepping
 DigitalOut MS1(p27);
@@ -60,7 +60,7 @@
 float step_freq1 = 1;
 float step_freq2 = 1;
 float step_freq3 = 1;
-float step_freq4 = 1; 
+float step_freq4 = 1;
 
 //EMG filter
 arm_biquad_casd_df1_inst_f32 lowpass_biceps;
@@ -126,12 +126,12 @@
     scope.set(0,filtered_biceps); //Filtered EMG signal
     scope.set(1,filtered_triceps);
     scope.set(2,filtered_pect);
-    scope.set(3,filtered_deltoid); 
+    scope.set(3,filtered_deltoid);
 }
 
 void looper_motory()
 {
-    //Vooruit
+    //Forward
     force1 = K_Gain*(filtered_biceps/MAX_bi);
     force1 = force1 - damping1;
     acc1 = force1/Mass;
@@ -139,7 +139,7 @@
     damping1 = speed1 * Damp;
     step_freq1 = (setpoint*speed1);
     speed_old1 = speed1;
-    
+
     //Achteruit triceps
     force2 = K_Gain*(filtered_triceps/MAX_tri);
     force2 = force2 - damping2;
@@ -149,14 +149,15 @@
     step_freq2 = (setpoint*speed2);
     speed_old2 = speed2;
     if (filtered_biceps > filtered_triceps) {
-        Diry = 0;
+        Diry = 1;
         speed2 = 0.01;
         speed_old2 = 0.01;
         Stepy.period(1.0/step_freq1);
-    } if (filtered_triceps > filtered_biceps) {
-        Diry = 1;
+    }
+    if (filtered_triceps > filtered_biceps) {
+        Diry = 0;
         speed1 = 0.01;
-        speed_old1 = 0.01;  
+        speed_old1 = 0.01;
         Stepy.period(1.0/step_freq2);
     }
     //Speed limit
@@ -170,7 +171,7 @@
     }
     //EMG treshold
     if (filtered_biceps < EMG_tresh && filtered_triceps < EMG_tresh) {
-        Enabley = 1; //Enable = 1 turns the motor off. 
+        Enabley = 1; //Enable = 1 turns the motor off.
         speed1 = 0.01;
         speed_old1 = 0.01;
         speed2 = 0.01;
@@ -191,7 +192,7 @@
     damping3 = speed3 * Damp;
     step_freq3 = (setpoint*speed3);
     speed_old3 = speed3;
-    
+
     //To the right
     force4 = K_Gain*(filtered_deltoid/MAX_delt);
     force4 = force4 - damping4;
@@ -205,10 +206,11 @@
         speed4 = 0.01;
         speed_old4 = 0.01;
         Stepx.period(1.0/step_freq3);
-    } if (filtered_deltoid > filtered_pect) {
+    }
+    if (filtered_deltoid > filtered_pect) {
         Dirx = 1;
         speed3 = 0.01;
-        speed_old3 = 0.01;  
+        speed_old3 = 0.01;
         Stepx.period(1.0/step_freq4);
     }
     //Speed limit
@@ -222,7 +224,7 @@
     }
     //EMG treshold
     if (filtered_pect < EMG_tresh && filtered_deltoid < EMG_tresh) {
-        Enablex = 1; //Enable = 1 turns the motor off. 
+        Enablex = 1; //Enable = 1 turns the motor off.
         speed3 = 0.01;
         speed_old3 = 0.01;
         speed4 = 0.01;
@@ -243,7 +245,7 @@
     //triceps
     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);
-    //pectoralis major   
+    //pectoralis major
     arm_biquad_cascade_df1_init_f32(&lowpass_pect, 1 , lowpass_const, lowpass_pect_states);
     arm_biquad_cascade_df1_init_f32(&highnotch_pect, 2 , highnotch_const, highnotch_pect_states);
     //deltoid
@@ -253,10 +255,10 @@
 
     Ticker looptimer1;
     looptimer1.attach(looper_motorx, 0.01); //X-Spindle motor, why this freq?
-    
+
     Ticker looptimer2;
     looptimer2.attach(looper_motory, 0.01); //Y-Spindle motor
-    
+
     //Microstepping control, now configured as half stepping (MS1=1,MS2=0,MS3=0)
     MS1 = 1;
     MS2 = 0;
@@ -266,8 +268,8 @@
 
     while (1) {
 
-        //lcd.printf("Bi %.2f ,Tri %.2f \n", filtered_biceps, filtered_triceps); Filtered EMG values 
-        lcd.printf("1 %.0f, 2 %.0f, 3 %.0f, 4 %.0f \n", step_freq1, step_freq2, step_freq3, step_freq4); //step_freq value of every EMG sensor
-        wait(0.01);
+        //lcd.printf("Bi %.2f ,Tri %.2f \n", filtered_biceps, filtered_triceps); Filtered EMG values
+        //lcd.printf("1 %.0f, 2 %.0f, 3 %.0f, 4 %.0f \n", step_freq1, step_freq2, step_freq3, step_freq4); //step_freq value of every EMG sensor
+        //wait(0.01);
     }
 }