Diff: main.cpp

Revision:

66:c094d1868b30

Parent:

65:821683c7eb98

Child:

67:fba5b64bb295

diff -r 821683c7eb98 -r c094d1868b30 main.cpp
--- a/main.cpp	Thu Jun 18 11:24:51 2015 +0000
+++ b/main.cpp	Thu Jun 18 11:50:07 2015 +0000
@@ -13,10 +13,10 @@
 //#include "HIDScope.h"
 
 #define P_Gain      0.995
-#define K_Gain      50    //Gain of the filtered EMG signal
-#define Damp        5       //Deceleration of the motor
-#define Mass        1       // Mass value
-#define dt          0.01   //Sample frequency
+#define K_Gain      25   //Gain of the filtered EMG signal
+#define Damp        5    //Deceleration of the motor
+#define Mass        1    // Mass value
+#define dt          0.01 //Sample frequency
 #define MAX_bi      0.40 //Can be used for normalisation of the EMG signal of the biceps
 #define MAX_tri     0.60
 #define MAX_pect    0.48
@@ -25,9 +25,9 @@
 #define EMG_tresh2   0.02
 #define EMG_tresh3   0.01
 #define EMG_tresh4   0.01
-#define H_Gain  2
-#define Pt_x    44
-#define Pt_y    10
+#define H_Gain  3
+#define Pt_x    0.88
+#define Pt_y    0.25
 
 //Motor control
 DigitalOut Dirx(p21);
@@ -157,11 +157,11 @@
     speed_old1 = speed1;
 
 
-    if (emg_y > 0 || Posy < 0.20) {
+    if (emg_y > 0) {
         Diry = 1;
     }
 
-    if (emg_y < 0 || Posy > 0.70) {
+    if (emg_y < 0) {
         Diry = 0;
     }
     //Speed limit
@@ -190,10 +190,10 @@
     Stepx.period(1.0/step_freq2);
     speed_old2 = speed2;
 
-    if (emg_x > 0 || Posx < 0.30) {
+    if (emg_x > 0) {
         Dirx = 0;
     }
-    if (emg_x < 0 || Posx > 0.86) {
+    if (emg_x < 0) {
         Dirx = 1;
     }
     //Speed limit
@@ -214,106 +214,106 @@
 {
     // 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);
-
-    /*while(1) {
-
-        Diry = 1;
-        Dirx = 1;
-        Stepx.period(1/1500);
-        wait(0.01);
-        Stepy.period(1/1500);
-        wait(0.01);
-    }*/
+    
     Enablex = 1;
     Enabley = 1;
-    wait(1);
+    wait(1); 
     lcd.printf("Start homing");
     wait(2);
     lcd.cls();
     wait(1);
     Enablex = 0;
     Enabley = 0;
-    while(errorx > 2 && errory > 2) {
-        
+    while(errorx > 0.03 || errory > 0.03) {
+        lcd.printf("%.2f %.2f  \n", Ps_x, Ps_y);
 
-        Ps_x = Posx.read()* 50;
-        Ps_y = Posy.read()* 40;
-        lcd.printf("%.1f %.1f  \n", Ps_x, Ps_y);
+        Ps_x = Posx.read();
+        Ps_y = Posy.read();
 
-        if (Ps_x < 44) {
+        if (Ps_x < 0.88) {
             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 > 10) {
+        }
+        if (Ps_y > 0.25) {
             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);
-        } 
-
+        }
+     
     }
     lcd.cls();
     wait(1);
     lcd.printf("Done");
+    wait(1);
+    lcd.cls();
+    wait(1);
     Enablex = 1;
     Enabley = 1;
     wait(3);
-    /*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);
+    lcd.printf("Start EMG Control");
+    wait(2);
+    lcd.cls();
+    wait(1);
+    Enablex = 0;
+    Enabley = 0; 
+    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);
 
-      Ticker looptimer1;
-      looptimer1.attach(looper_motorx, 0.01); //X-Spindle motor, why this freq?
+     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 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;
-      MS3 = 0;
-      Stepx.write(0.5); // Duty cycle of 50%
-      Stepy.write(0.5);
+     //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);
 
 
-      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("%.2f %.2f \n", force1, force2);
-          //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 \n", force1, force2);
+         //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);
 
-      }*/
+     }
 }