Diff: main.cpp

Revision:

11:0793a78109a2

Parent:

10:80fe931a71e4

Child:

12:69ab81cf5b7d

--- a/main.cpp	Tue Sep 29 13:37:22 2015 +0000
+++ b/main.cpp	Tue Sep 29 14:01:47 2015 +0000
@@ -31,21 +31,23 @@
     
 }
 // Controller gain
-const double motor1_Kp = 0.05;
-
+const double m2_Kp = 0.07,m2_Ki = 0.005;
+const double m2_Ts = 0.01;
+double m2_err_int = 0;
 
-// Reusable P controller
-double P( double error, const double Kp ) 
+// Reusable PI controller
+double P( double error, const double Kp, const double Ki, double Ts, double &e_int) 
 {
-    return Kp * error;
+    e_int = e_int + Ts*error;
+    return Kp * error + Ki*e_int;
 }
 
 // Next task, measure the error and apply the output to the plant
-void motor1_Controller() 
+void motor2_Controller() 
 {
     reference = potmeter2.read()*360;
     position = Encoder.getPulses()*360/(0.5*128*131); // Aantal Degs
-    double P2 = P( reference - position, motor1_Kp );
+    double P2 = P( reference - position, m2_Kp, m2_Ki, m2_Ts, m2_err_int);
     motor2speed = abs(P2);
     if(P2 > 0)
     {  
@@ -64,7 +66,7 @@
     pc.printf("Tot aan loop werkt\n");
     
     ScopeTime.attach_us(&ScopeSend, 10e4);
-    myControllerTicker.attach( &motor1_Controller, 0.01f ); // 100 Hz
+    myControllerTicker.attach( &motor2_Controller, 0.01f ); // 100 Hz
     while(true)
     {
        pc.printf("position = %f aantal degs = %f \n",reference,position);