Diff: main.cpp

Revision:

24:672abc3f02b7

Parent:

23:917179f72762

Child:

25:df780572cfc8

--- a/main.cpp	Fri Oct 06 12:03:26 2017 +0000
+++ b/main.cpp	Fri Oct 06 12:34:54 2017 +0000
@@ -43,7 +43,7 @@
 
 // MOTOR CONTROL PART    
 bool m1_direction = false;
-int posRevRange = 2; // describes the ends of the position range in complete motor output shaft revolutions in both directions
+int posRevRange = 5; // describes the ends of the position range in complete motor output shaft revolutions in both directions
 const float maxAngle = 2*3.14*posRevRange; // max angle in radians
 const float Ts = 0.1;
 
@@ -64,16 +64,18 @@
 // Initializing position and integral errors
 float posError = 0;
 float integralError = 0;
-float totalError = posError + integralError;
+float derivativeError = 0;
+float e_prev = 0;
+float totalError = posError + integralError + derivativeError;
 
 // readEncoder reads counts and revs and logs results to serial window
-void getError(float &posError, float &integralError){
+void getError(float &posError, float &integralError, float &derivativeError){
     counts = Encoder.getPulses();
     double motor1Position = 2*3.14*(counts/(131*64.0f));
     pc.printf("%0.2f revolutions \r\n", motor1Position);
     posError = getReferencePosition() - motor1Position;
-    
     integralError = integralError + Ts*posError;
+    derivativeError = derivativeError - e_prev;
     totalError = posError + integralError;
     pc.printf("Error is %0.2f \r \n", totalError);
     }