Diff: main.cpp

Revision:

16:e51ddfaf2e7a

Parent:

15:fb0bbce41a0f

Child:

17:8a0c720733b8

--- a/main.cpp	Mon Oct 21 10:05:12 2019 +0000
+++ b/main.cpp	Mon Oct 21 12:43:43 2019 +0000
@@ -48,6 +48,7 @@
 float motor2error;
 float Kp=0.27;
 float Ki=0.35;
+float Kd=0.1;
 float u_p1;
 float u_p2;
 float u_i1;
@@ -92,7 +93,7 @@
 
 float PID_controller1(float motor1error){
     static float error_integral1=0;
-    //static float e_prev=e;
+    static float e_prev1=motor1error;
     
     //Proportional part:
     u_p1=Kp*motor1error;
@@ -101,8 +102,13 @@
     error_integral1=error_integral1+ei1*Ts;
     u_i1=Ki*error_integral1;
     
+    //Derivative part
+    float error_derivative1=(motor1error-e_prev1)/Ts;
+    float u_d1=Kd*error_derivative1;
+    e_prev1=motor1error;
+    
     // Sum and limit
-    up1= u_p1+u_i1;
+    up1= u_p1+u_i1+u_d1;
     if (up1>1){
         controlsignal1=1;}
     else if (up1<-1){
@@ -120,6 +126,7 @@
 
 float PID_controller2(float motor2error){
     static float error_integral2=0;
+    static float e_prev2=motor2error;
     
     //Proportional part:
     u_p2=Kp*motor2error;
@@ -128,8 +135,13 @@
     error_integral2=error_integral2+ei2*Ts;
     u_i2=Ki*error_integral2;
     
+    //Derivative part
+    float error_derivative2=(motor2error-e_prev2)/Ts;
+    float u_d2=Kd*error_derivative2;
+    e_prev2=motor2error;
+    
     // Sum and limit
-    up2= u_p2+u_i2;
+    up2= u_p2+u_i2+u_d2;
     if (up2>1){
         controlsignal2=1;}
     else if (up2<-1){
@@ -165,11 +177,19 @@
     // refangle= pi2/3.0f*sin(5.0f*t)*motor1Toggle;
     //t+=0.01;
     }
+
+//void RKI(){
+    
+//    RKI magic
+    
+//    motor1ref=??;
+//    motor2ref=??;
+//    }    
     
 void motorControl()
 {
     button1.fall(&togglehoek);
-    
+//    RKI()
     motor1angle = counts1 * factorin / gearratio; // Angle of motor shaft in rad
     omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
     motor1error=motor1ref-motor1angle;