Diff: main.cpp

Revision:

9:2435c48d2032

Parent:

8:42d1d02673ae

Child:

10:d5369a546201

--- a/main.cpp	Thu Nov 02 10:33:03 2017 +0000
+++ b/main.cpp	Thu Nov 02 10:48:44 2017 +0000
@@ -34,58 +34,63 @@
 
 float Potmeterwaarde;
 float potmeterwaarde2;
+int maxwaarde, maxwaarde2;
+float refP, refP2;
+float kp, Proportional, kd, VelocidyError, Dervative, ki, Integrator, motorValue
+float kp2, Proportional2, kd2, VelocityError2, Derivative2, ki2, Integrator2, motorValue2;
+float Huidigepositie, error, 
 
 float GetReferencePosition() 
 {
      Potmeterwaarde = potMeter2.read();
-    int maxwaarde = 4096;                   // = 64x64
-    float refP = Potmeterwaarde*maxwaarde;
+     maxwaarde = 4096;                   // = 64x64
+     refP = Potmeterwaarde*maxwaarde;
     return refP;                            // value between 0 and 4096 
 }
 
 float GetReferencePosition2() 
 {
      potmeterwaarde2 = potmeter1.read();
-    int maxwaarde2 = 4096;                   // = 64x64
-    float refP2 = potmeterwaarde2*maxwaarde2;
+     maxwaarde2 = 4096;                   // = 64x64
+     refP2 = potmeterwaarde2*maxwaarde2;
     return refP2;                            // value between 0 and 4096 
 }
     
 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.001;                             // kind of scaled.
-    float Proportional= kp*error;
+     kp = 0.001;                             // kind of scaled.
+     Proportional= kp*error;
     
-    float kd = 0.0004;                           // kind of scaled. 
-    float VelocityError = (error - e_prev)/Ts; 
-    float Derivative = kd*VelocityError;
+     kd = 0.0004;                           // kind of scaled. 
+     VelocityError = (error - e_prev)/Ts; 
+     Derivative = kd*VelocityError;
     e_prev = error;
     
-    float ki = 0.00005;                           // kind of scaled.
+     ki = 0.00005;                           // kind of scaled.
     e_int = e_int+Ts*error;
-    float Integrator = ki*e_int;
+     Integrator = ki*e_int;
     
     
-    float motorValue = Proportional + Integrator + Derivative;
+     motorValue = Proportional + Integrator + Derivative;
     return motorValue;
 }
 
 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)
 {
-    float kp2 = 0.001;                             // kind of scaled.
-    float Proportional2= kp2*error2;
+     kp2 = 0.001;                             // kind of scaled.
+     Proportional2= kp2*error2;
     
-    float kd2 = 0.0004;                           // kind of scaled. 
-    float VelocityError2 = (error2 - e_prev2)/Ts; 
-    float Derivative2 = kd2*VelocityError2;
+     kd2 = 0.0004;                           // kind of scaled. 
+     VelocityError2 = (error2 - e_prev2)/Ts; 
+     Derivative2 = kd2*VelocityError2;
     e_prev2 = error2;
     
-    float ki2 = 0.00005;                           // kind of scaled.
+     ki2 = 0.00005;                           // kind of scaled.
     e_int2 = e_int2+Ts*error2;
-    float Integrator2 = ki2*e_int2;
+     Integrator2 = ki2*e_int2;
     
     
-    float motorValue2 = Proportional2 + Integrator2 + Derivative2;
+     motorValue2 = Proportional2 + Integrator2 + Derivative2;
     return motorValue2;
 }
 
@@ -161,7 +166,7 @@
     SetMotor2(motorValue2);
     
     pc.baud(115200);
-    pc.printf("potmeter = %f, refP = %f, motorvalue = %f \r\n, potmeter2 = %f, refP2 = %f, motorvalue2 = %f \r\n", Potmeterwaarde, refP, motorValue, potmeterwaarde2, refP2, motorValue2);
+    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);
 }