Revision 8:834e6a98f058, committed 2018-10-23

Comitter:

Wimboo

Date:

Tue Oct 23 12:21:43 2018 +0000

Parent:

7:9e0ded88fe60

Commit message:

Met Y1 en Y2 tussen 0.5 en 1;

Changed in this revision

diff -r 9e0ded88fe60 -r 834e6a98f058 main.cpp
--- a/main.cpp	Mon Oct 22 10:00:09 2018 +0000
+++ b/main.cpp	Tue Oct 23 12:21:43 2018 +0000
@@ -51,7 +51,8 @@
 float f3;     
 float Output_Last1;      // Remember previous position
 float Output_Last2;      // Remember previous position
-float Y;                // Value that is outputted to motor control
+float Y1;                // Value that is outputted to motor control
+float Y2;
 float P_Last = 0;       // Starting position
 const float Max_Speed = 400;      //Max speed of the motor
       
@@ -113,16 +114,20 @@
     e3 = G - P;
     Output_Last1 = Output1;
     Output1 = Kp * (e3 - e2) + Output_Last1 +Ki * e3 + Kd * (e3 - 2*e2 + e1);
-    Y = Output1;
+    Y1 = Output1;
      
     if (Output1 >= 1){
-        Y = 1;
+        Y1 = 1;
     }
     else if (Output1 <= -1){
-        Y = -1;
-    }    
-    P = P_Last + Y * Timestep * Max_Speed;
-
+        Y1 = -1;
+    }   
+    if (Y1 >= 0) {
+        Y1 = 0.5f * Y1 + 0.5f;
+    } 
+    else {
+    Y1 = 0.5f + 0.5f * Y1;
+    }
     scope.set(0,Output1);
     scope.set(1,P);
     scope.send();
@@ -135,15 +140,20 @@
     f3 = G - P;
     Output_Last2 = Output2;
     Output2 = Kp * (f3 - f2) + Output_Last2 +Ki * f3 + Kd * (f3 - 2*f2 + f1);
-    Y = Output2;
+    Y2 = Output2;
      
     if (Output2 >= 1){
-        Y = 1;
+        Y2 = 1;
     }
     else if (Output2 <= -1){
-        Y = -1;
+        Y2 = -1;
     }    
-    P = P_Last + Y * Timestep * Max_Speed;
+    if (Y2 >= 0) {
+        Y2 = 0.5f * Y2 + 0.5f;
+    } 
+    else {
+    Y2 = 0.5f + 0.5f * Y2;
+    }
 
     scope.set(0,Output2);
     scope.set(1,P);