Diff: main.cpp

Revision:

21:60102b9dd590

Parent:

20:71c39665276e

--- a/main.cpp	Fri Nov 03 11:30:23 2017 +0000
+++ b/main.cpp	Fri Nov 03 11:57:00 2017 +0000
@@ -80,7 +80,7 @@
     
     int maxwaarde = 4096;                   // = 64x64
     refP = (((0.5*pi) - q1)/(2*pi))*maxwaarde;
-    refP2 = ((- q1 - q2)/(2*pi))*maxwaarde;    //Get reference positions was eerst 0.5*pi
+    refP2 = ((q1 + q2)/(2*pi))*maxwaarde;    //Get reference positions was eerst 0.5*pi
 }
 
 void SetpointRobot()
@@ -125,7 +125,7 @@
     
 float FeedBackControl(float error, float &e_prev, float &e_int)   // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
 {
-    float kp = 0.0015;                               // kind of scaled.
+    float kp = 0.015;                               // kind of scaled.
     float Proportional= kp*error;
     
     float kd = 0.0008;                           // kind of scaled. 
@@ -228,7 +228,7 @@
     float error = (refP - Huidigepositie);// make an error
     float motorValue = FeedBackControl(error, e_prev, e_int);
     SetMotor1(motorValue);
-    pc.printf("refP = %f, Huidigepositie = %f, error = %f, motorValue = %f \r\n", refP, Huidigepositie, error, motorValue);
+    pc.printf("Rsx = %f, Rsy = %f, refP = %f, Huidigepositie = %f, error = %f, motorValue = %f \r\n", Rsx, Rsy, refP, Huidigepositie, error, motorValue);
 
     // hier the control of the control system
     //float refP2 = GetReferencePosition2();