Diff: main.cpp

Revision:

90:399b877f8a77

Parent:

89:7882984abbe8

Child:

91:dc73a4b07653

--- a/main.cpp	Thu Jun 25 11:03:22 2015 +0000
+++ b/main.cpp	Thu Jun 25 14:52:11 2015 +0000
@@ -25,10 +25,10 @@
 #define EMG_tresh2   0.01
 #define EMG_tresh3   0.01
 #define EMG_tresh4   0.01
-#define H_Gain  3.5
-#define Pt_x    0.50
+#define H_Gain  8
+#define Pt_x    0.70
 #define Pt_y    0.50
-#define error_tresh 0.03
+#define error_tresh 0.02
 
 //Motor control
 DigitalOut Dirx(p25);
@@ -63,7 +63,7 @@
 C12832_LCD lcd;
 
 //Variables for motor control
-float setpoint = 2000; //Frequentie setpoint
+float setpoint = 1750; //Frequentie setpoint
 float step_freq1 = 1;
 float step_freq2 = 1;
 
@@ -217,7 +217,7 @@
 
 int main()
 {
-  /*  // Attach the HIDScope::send method from the scope object to the timer at 500Hz. Hier wordt de sample freq aangegeven.
+    // 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;
@@ -243,9 +243,25 @@
         errorx = fabs(Pt_x - Ps_x);
         errory = fabs(Ps_y - Pt_y);
        
-        
-   
         if (Ps_x < Pt_x && 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;
+            if(hstep_freqx < 2000){
+            Stepx.period(1.0/hstep_freqx);
+            wait(0.01);}
+            else{
+            Stepx.period(1.0/setpoint);
+            wait(0.01);
+            }
+            
+        }
+       
+        if (Ps_x > Pt_x && errorx > error_tresh) {
             Dirx = 1;
             //errorx = Pt_x - Ps_x;
             cx = errorx * H_Gain;
@@ -253,37 +269,26 @@
             float hnew_step_freqx;
             hnew_step_freqx = ((1-P_Gain)*setpoint*cx) + (P_Gain*hstep_freqx);
             hstep_freqx = hnew_step_freqx;
+            if(hstep_freqx < 2000){
             Stepx.period(1.0/hstep_freqx);
+            wait(0.01);}
+            else{
+            Stepx.period(1.0/setpoint);
             wait(0.01);
+            }
         }
-       
-        if (Ps_x > Pt_x && 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);
-        }
-         pc.printf("%.2f %.2f %.0f %.0f\n", errorx, Ps_x, Dirx, hstep_freqx);
+         pc.printf("%.2f %.2f %.1f %.0f \n", errorx, Ps_x, cx, hstep_freqx);
     }
-    lcd.printf("Done");
+    pc.printf("Done");
     wait(2);
-    lcd.cls();
-    wait(1);
     Enablex = 1;
     Enabley = 1;
-    wait(3);
-    lcd.printf("Start EMG Control");
     wait(2);
-    lcd.cls();
-    wait(1);
+    pc.printf("Start EMG Control");
+    wait(2);
     Enablex = 0;
     Enabley = 0;
-*/
+
     MS1 = 1;
     MS2 = 0;
     MS3 = 0;