Diff: main.cpp

Revision:

10:d5369a546201

Parent:

9:2435c48d2032

Child:

11:126ae33919b3

--- a/main.cpp	Thu Nov 02 10:48:44 2017 +0000
+++ b/main.cpp	Thu Nov 02 11:22:08 2017 +0000
@@ -36,9 +36,13 @@
 float potmeterwaarde2;
 int maxwaarde, maxwaarde2;
 float refP, refP2;
-float kp, Proportional, kd, VelocidyError, Dervative, ki, Integrator, motorValue
+float kp, Proportional, kd, VelocityError, Derivative, ki, Integrator, motorValue;
 float kp2, Proportional2, kd2, VelocityError2, Derivative2, ki2, Integrator2, motorValue2;
-float Huidigepositie, error, 
+float Huidigepositie;
+float Aerror;
+float Huidigepositie2;
+float Aerror2;
+float motorVal, motorVal2;
 
 float GetReferencePosition() 
 {
@@ -66,13 +70,13 @@
      Derivative = kd*VelocityError;
     e_prev = error;
     
-     ki = 0.00005;                           // kind of scaled.
+     ki = 0.0005;                           // kind of scaled.
     e_int = e_int+Ts*error;
      Integrator = ki*e_int;
     
     
-     motorValue = Proportional + Integrator + Derivative;
-    return motorValue;
+     motorVal = Proportional + Integrator + Derivative;
+    return motorVal;
 }
 
 float FeedBackControl2(float error2, float &e_prev2, float &e_int2)   // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
@@ -85,13 +89,13 @@
      Derivative2 = kd2*VelocityError2;
     e_prev2 = error2;
     
-     ki2 = 0.00005;                           // kind of scaled.
+     ki2 = 0.0005;                           // kind of scaled.
     e_int2 = e_int2+Ts*error2;
      Integrator2 = ki2*e_int2;
     
     
-     motorValue2 = Proportional2 + Integrator2 + Derivative2;
-    return motorValue2;
+     motorVal2 = Proportional2 + Integrator2 + Derivative2;
+    return motorVal2;
 }
 
 
@@ -120,11 +124,11 @@
 {
     if (motorValue2 >= 0)
     {
-        M2D = 0;
+        M2D = 1;
     }
     else 
     {
-        M2D = 1;
+        M2D = 0;
     }
 
     if  (fabs(motorValue2) > 1)    
@@ -139,34 +143,34 @@
 
 float Encoder ()
 {
-    float Huidigepositie = motor1.getPosition ();
+     Huidigepositie = motor1.getPosition ();
     return Huidigepositie;             // huidige positie = current position
 }
 
 float Encoder2 ()
 {
-    float Huidigepositie2 = motor2.getPosition ();
+     Huidigepositie2 = motor2.getPosition ();
     return Huidigepositie2;             // huidige positie = current position
 }
 
 void MeasureAndControl(void)
 {
     // hier the control of the control system
-    float refP = GetReferencePosition(); 
-    float Huidigepositie = Encoder(); 
-    float error = (refP - Huidigepositie);// make an error
-    float motorValue = FeedBackControl(error, e_prev, e_int);
+     refP = GetReferencePosition(); 
+     Huidigepositie = Encoder(); 
+     Aerror = (refP - Huidigepositie);// make an error
+     motorValue = FeedBackControl(Aerror, e_prev, e_int);
     SetMotor1(motorValue);
 
     // hier the control of the control system
-    float refP2 = GetReferencePosition2(); 
-    float Huidigepositie2 = Encoder2(); 
-    float error2 = (refP2 - Huidigepositie2);// make an error
-    float motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
+     refP2 = GetReferencePosition2(); 
+     Huidigepositie2 = Encoder2(); 
+     Aerror2 = (refP2 - Huidigepositie2);// make an error
+     motorValue2 = FeedBackControl2(Aerror2, e_prev2, e_int2);
     SetMotor2(motorValue2);
     
     pc.baud(115200);
-    pc.printf("potmeter = %f, refP = %f, error = %f, motorvalue = %f \r\n, potmeter2 = %f, refP2 = %f, error2 = %f, motorvalue2 = %f \r\n", Potmeterwaarde, refP, error, motorValue, potmeterwaarde2, refP2, error2, motorValue2);
+    pc.printf("potmeter = %f, refP = %f, huidigepositie = %f, error = %f, motorvalue = %f \r\n, potmeter2 = %f, refP2 = %f, huidigepositie2 = %f, error2 = %f, motorvalue2 = %f \r\n", Potmeterwaarde, refP, Huidigepositie, Aerror, motorValue, potmeterwaarde2, refP2, Huidigepositie2, Aerror2, motorValue2);
 }