Revision 62:509622cce1c6, committed 2015-06-17

Comitter:

jessekaiser

Date:

Wed Jun 17 14:54:59 2015 +0000

Parent:

61:99f3ae6e053b

Child:

63:e336c16c8957

Commit message:

Homing werkt!

Changed in this revision

--- a/main.cpp	Wed Jun 17 10:09:51 2015 +0000
+++ b/main.cpp	Wed Jun 17 14:54:59 2015 +0000
@@ -12,6 +12,7 @@
 #include "arm_math.h"
 //#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
@@ -24,7 +25,7 @@
 #define EMG_tresh2   0.02
 #define EMG_tresh3   0.01
 #define EMG_tresh4   0.01
-#define H_Gain  5
+#define H_Gain  2
 #define Pt_x    0.83
 #define Pt_y    0.25
 
@@ -105,6 +106,8 @@
 float errory = 0.2;
 float Ps_x = 0;
 float Ps_y = 0;
+float hstep_freqx = 1;
+float hstep_freqy = 1;
 
 void looper_emg()
 {
@@ -211,35 +214,64 @@
 {
     // 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);
+    //while(1) {
     MS1 = 1;
     MS2 = 0;
     MS3 = 0;
+    //Diry = 1;
+    //Dirx = 1;
     Stepx.write(0.5); // Duty cycle of 50%
     Stepy.write(0.5);
+    //Stepx.period(1/1500);
+    //Stepy.period(1/1500); 
+    //wait(0.01);
+    //}
+    //Enablex = 1;
+    //Enabley = 1;
+   
+    lcd.printf("Start homing");
+    wait(2);
+    lcd.cls();
+    wait(1);
+    Enablex = 0;
+    Enabley = 0;
+    while(errorx > 0.05 && errory > 0.05) {
+        lcd.printf("%.2f %.2f  \n", Ps_x, Ps_y);
 
-    while(errorx > 0.05 || errory > 0.05) {
-        lcd.printf("%.2f %.2f \n", errorx, errory);
         Ps_x = Posx.read();
         Ps_y = Posy.read();
+
         if (Ps_x < 0.83) {
             Dirx = 0;
             errorx = Pt_x - Ps_x;
             cx = errorx * H_Gain;
-            Stepx.period(1.0/(setpoint*cx));
+            
+            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) {
             Diry = 0;
             errory = Ps_y - Pt_y;
             cy = errory * H_Gain;
-            Stepy.period(1.0/(setpoint*cy));
+            
+            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);
         }
-        wait(0.01);
+     
     }
-   
-    lcd.printf("Done"); 
-    Enablex = 1; 
+    lcd.cls();
+    wait(1);
+    lcd.printf("Done");
+    Enablex = 1;
     Enabley = 1;
-    /* Ticker emgtimer;    //biceps
+    wai(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