Diff: main.cpp

Revision:

69:a1ba54587b35

Parent:

68:2b778b6da923

Child:

70:e84629c7dfed

--- a/main.cpp	Fri Jun 19 09:19:43 2015 +0000
+++ b/main.cpp	Fri Jun 19 11:25:22 2015 +0000
@@ -12,7 +12,7 @@
 #include "arm_math.h"
 //#include "HIDScope.h"
 
-#define P_Gain      0.995
+#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
@@ -25,9 +25,10 @@
 #define EMG_tresh2   0.01
 #define EMG_tresh3   0.01
 #define EMG_tresh4   0.01
-#define H_Gain  3
-#define Pt_x    0.88
-#define Pt_y    0.25
+#define H_Gain  3.5
+#define Pt_x    0.50
+#define Pt_y    0.50
+#define error_tresh 0.03
 
 //Motor control
 DigitalOut Dirx(p21);
@@ -65,8 +66,7 @@
 float setpoint = 2000; //Frequentie setpoint
 float step_freq1 = 1;
 float step_freq2 = 1;
-float step_freq3 = 1;
-float step_freq4 = 1;
+
 
 //EMG filter
 arm_biquad_casd_df1_inst_f32 lowpass_biceps;
@@ -108,6 +108,8 @@
 float Ps_y = 0;
 float hstep_freqx = 1;
 float hstep_freqy = 1;
+float emg_y_abs = 0;
+float emg_x_abs = 0;
 
 void looper_emg()
 {
@@ -145,8 +147,8 @@
 }
 
 void looper_motory()
-{   float emg_y_abs;
-    
+{
+
     emg_y = (filtered_biceps - filtered_triceps);
     emg_y_abs = fabs(emg_y);
     force1 = emg_y_abs*K_Gain;
@@ -158,7 +160,6 @@
     Stepy.period(1.0/step_freq1);
     speed_old1 = speed1;
 
-
     if (emg_y > 0) {
         Diry = 1;
     }
@@ -180,9 +181,9 @@
 
 }
 
-void looper_motorx()
+/*void looper_motorx()
 {
-    float emg_x_abs;
+
     emg_x = (filtered_pect - filtered_deltoid);
     emg_x_abs = fabs(emg_x);
     force2 = emg_x_abs*K_Gain;
@@ -212,88 +213,87 @@
         Enablex = 0;
     }
 
-}
+}*/
 
 int main()
 {
     // Attach the HIDScope::send method from the scope object to the timer at 500Hz. Hier wordt de sample freq aangegeven.
     // scopeTimer.attach_us(&scope, &HIDScope::send, 2e3);
-    
+/*
     MS1 = 1;
     MS2 = 0;
     MS3 = 0;
-    
+
     Stepx.write(0.5); // Duty cycle of 50%
     Stepy.write(0.5);
-    
+
     Enablex = 1;
     Enabley = 1;
-    wait(1); 
+    wait(1);
     lcd.printf("Start homing");
     wait(2);
     lcd.cls();
     wait(1);
     Enablex = 0;
     Enabley = 0;
-    while(errorx > 0.03 || errory > 0.03) {
-        lcd.printf("%.0f %.2f  \n", hstep_freqx, hstep_freqy );
+    while(errorx > error_tresh || errory > error_tresh) {
 
         Ps_x = Posx.read();
         Ps_y = Posy.read();
         errorx = fabs(Pt_x - Ps_x);
         errory = fabs(Ps_y - Pt_y);
-
-        if (Ps_x < 0.88 && errorx > 0.03) {
+        lcd.printf("%.2f %.2f \n", errorx, errory);
+        
+        
+        if (Ps_x < 0.50 && errorx > error_tresh) {
             Dirx = 0;
             //errorx = Pt_x - Ps_x;
             cx = errorx * H_Gain;
-            
+
             float hnew_step_freqx;
             hnew_step_freqx = ((1-P_Gain)*setpoint*cx) + (P_Gain*hstep_freqx);
             hstep_freqx = hnew_step_freqx;
             Stepx.period(1.0/hstep_freqx);
             wait(0.01);
         }
-        if (Ps_y > 0.25 && errory > 0.03) {
+        if (Ps_y > 0.50 && errory > error_tresh) {
             Diry = 0;
             //errory = Ps_y - Pt_y;
             cy = errory * H_Gain;
-            
+
             float hnew_step_freqy;
             hnew_step_freqy = ((1-P_Gain)*setpoint*cy) + (P_Gain*hstep_freqy);
             hstep_freqy = hnew_step_freqy;
             Stepy.period(1.0/hstep_freqy);
             wait(0.01);
         }
-        
-        if (Ps_x > 0.88 && errorx > 0.03) {
+
+        if (Ps_x > 0.50 && errorx > error_tresh) {
             Dirx = 1;
             //errorx = Pt_x - Ps_x;
             cx = errorx * H_Gain;
-            
+
             float hnew_step_freqx;
             hnew_step_freqx = ((1-P_Gain)*setpoint*cx) + (P_Gain*hstep_freqx);
             hstep_freqx = hnew_step_freqx;
             Stepx.period(1.0/hstep_freqx);
             wait(0.01);
         }
-        if (Ps_y < 0.25 && errory > 0.03) {
+        if (Ps_y < 0.50 && errory > error_tresh) {
             Diry = 1;
             //errory = Ps_y - Pt_y;
             cy = errory * H_Gain;
-            
+
             float hnew_step_freqy;
             hnew_step_freqy = ((1-P_Gain)*setpoint*cy) + (P_Gain*hstep_freqy);
             hstep_freqy = hnew_step_freqy;
             Stepy.period(1.0/hstep_freqy);
             wait(0.01);
         }
-     
+
     }
-    lcd.cls();
-    wait(1);
     lcd.printf("Done");
-    wait(1);
+    wait(2);
     lcd.cls();
     wait(1);
     Enablex = 1;
@@ -302,47 +302,48 @@
     lcd.printf("Start EMG Control");
     wait(2);
     lcd.cls();
-    wait(1); 
+    wait(1);
     Enablex = 0;
-    Enabley = 0; 
+    Enabley = 0;*/
+
+    MS1 = 1;
+    MS2 = 0;
+    MS3 = 0;
+    Stepx.write(0.5); // Duty cycle of 50%
+    Stepy.write(0.5);
+
     Ticker emgtimer;    //biceps
-     arm_biquad_cascade_df1_init_f32(&lowpass_biceps, 1 , lowpass_const, lowpass_biceps_states);
-     arm_biquad_cascade_df1_init_f32(&highnotch_biceps, 2 , highnotch_const, highnotch_biceps_states);
-     //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
-     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
-     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.01);
+    arm_biquad_cascade_df1_init_f32(&lowpass_biceps, 1 , lowpass_const, lowpass_biceps_states);
+    arm_biquad_cascade_df1_init_f32(&highnotch_biceps, 2 , highnotch_const, highnotch_biceps_states);
+    //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
+    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
+    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.01);
 
-     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
+    //Ticker looptimer1;
+    //looptimer1.attach(looper_motorx, 0.01); //X-Spindle motor, why this freq?
 
-     //Microstepping control, now configured as half stepping (MS1=1,MS2=0,MS3=0)
-     MS1 = 1;
-     MS2 = 0;
-     MS3 = 0;
-     Stepx.write(0.5); // Duty cycle of 50%
-     Stepy.write(0.5);
+    Ticker looptimer2;
+    looptimer2.attach(looper_motory, 0.01); //Y-Spindle motor
+
+    //Microstepping control, now configured as half stepping (MS1=1,MS2=0,MS3=0)
 
 
-     while (1) {
+
+    while (1) {
 
 
-         //lcd.printf("x %.2f, y %.2f \n", Posx.read(), Posy.read());
-         //lcd.printf("%.2f, %.2f %.2f %.2f \n", filtered_biceps, filtered_triceps, filtered_pect, filtered_deltoid); //Filtered EMG values
-         //lcd.printf("1 %.0f, 2 %.0f \n", step_freq1, step_freq2); //step_freq value of every EMG sensor
-         lcd.printf("%.0f %.2f \n", Stepy.read(), emg_y);
-         //lcd.printf("%.2f, %.2f %.2f %.2f \n", gain_biceps, gain_triceps, gain_pect, gain_deltoid);
-         //lcd.printf("%.2f, %.2f %.2f %.2f \n", norm_biceps, norm_triceps, norm_pect, norm_deltoid);
-         wait(0.01);
+        //lcd.printf("x %.2f, y %.2f \n", Posx.read(), Posy.read());
+        //lcd.printf("%.2f, %.2f %.2f %.2f \n", filtered_biceps, filtered_triceps, filtered_pect, filtered_deltoid); //Filtered EMG values
+        //lcd.printf("1 %.0f, 2 %.0f \n", step_freq1, step_freq2); //step_freq value of every EMG sensor
+        lcd.printf("%.2f %.2f %.2f %.2f  \n", emg_y_abs, step_freq1, filtered_biceps, filtered_triceps);
+        wait(0.01); 
 
-     }
+    }
 }