Diff: main.cpp

Revision:

3:4abcd40682fa

Parent:

2:22f0ce494b16

Child:

4:0844ab0d7d93

--- a/main.cpp	Thu Oct 08 12:35:56 2015 +0000
+++ b/main.cpp	Thu Oct 08 13:10:32 2015 +0000
@@ -16,8 +16,11 @@
 const int cw = 1;
 const int ccw = 0;
 
-// Controller gain proportional
+// Controller gain proportional and intergrator
 const double motor1_Kp = 5; // more or les random number.
+const double motor1_Ki = 1;
+const double M1_timestep = 0.01; // reason ticker works with 100 Hz.
+double motor1_error_integraal = 0; // initial value of integral error
 
 // calculating pulses to rotations in degree.
 const double Offset = 3450 ;//8400 counts is aangegeven op de motor. 3500 is almost 1 cirkel with a bit of overshoot 3450 looks good. about 10 - 20 counts oveshoot. for moter 1(tape)! Motor 2 almost the same(nice)
@@ -28,13 +31,15 @@
 
 
 // Reusable P controller
-double P(double error, double Kp)
+double PI(double error, const double Kp, const double Ki, double Ts, double &e_int)
 {
     double error_normalised_degree = error / resolution; // calculate how much degree the motor has to turn.
     double error_normalised_rotation = error_normalised_degree / 360; //calculate how much the rotaions the motor has to turn
     
-    double P_output = Kp * error_normalised_rotation;
-    return P_output;
+    e_int = e_int + Ts * error_normalised_rotation;
+    
+    double PI_output = Kp * error_normalised_rotation + Ki * e_int;
+    return PI_output;
 }
 // Next task, measure the error and apply the output to the plant
 void motor1_Controller()
@@ -43,7 +48,7 @@
     double position = wheel.getPulses();
     double error = reference - position;
 
-    double output = P( error, motor1_Kp );
+    double output = PI( error, motor1_Kp, motor1_Ki, M1_timestep, motor1_error_integraal );
 
 
 
@@ -67,7 +72,7 @@
 { 
 
  double kijken = wheel.getPulses();
-pc.printf("ref %.0f count%.0f\n",movement,kijken);
+pc.printf("ref %.0f count%.0f int err %.2f \n",movement,kijken, motor1_error_integraal);
     
     }