Revision 87:97015f6cb9cc, committed 2015-06-25

Comitter:

jessekaiser

Date:

Thu Jun 25 10:10:35 2015 +0000

Parent:

86:9d781e7aa893

Child:

88:c9146d957102

Commit message:

EMG voor X-motor werkt!

Changed in this revision

--- a/main.cpp	Thu Jun 25 09:35:23 2015 +0000
+++ b/main.cpp	Thu Jun 25 10:10:35 2015 +0000
@@ -34,7 +34,7 @@
 DigitalOut Dirx(p25);
 PwmOut Stepx(p26);
 DigitalOut Diry(p23);
-PwmOut Stepy(p24);
+PwmOut Stepy(p28);
 
 //Signal to and from computer
 Serial pc(USBTX, USBRX);
@@ -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;
@@ -236,16 +236,28 @@
     wait(1);
     Enablex = 0;
     Enabley = 0;
-    while(errorx > error_tresh || errory > error_tresh) {
+    while(errorx > error_tresh) {
 
         Ps_x = Posx.read();
         Ps_y = Posy.read();
         errorx = fabs(Pt_x - Ps_x);
         errory = fabs(Ps_y - Pt_y);
-        pc.printf("%.2f %.2f \n", errorx, Ps_x);
+       
         
-        
-        if (Ps_x < 0.50 && errorx > error_tresh) {
+   
+        if (Ps_x < Pt_x && 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_x > Pt_x && errorx > error_tresh) {
             Dirx = 0;
             //errorx = Pt_x - Ps_x;
             cx = errorx * H_Gain;
@@ -256,41 +268,7 @@
             Stepx.period(1.0/hstep_freqx);
             wait(0.01);
         }
-        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.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.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);
-        }
-
+         pc.printf("%.2f %.2f %.0f %.0f\n", errorx, Ps_x, Dirx, hstep_freqx);
     }
     lcd.printf("Done");
     wait(2);
@@ -305,7 +283,7 @@
     wait(1);
     Enablex = 0;
     Enabley = 0;
-
+*/
     MS1 = 1;
     MS2 = 0;
     MS3 = 0;