Diff: PROJECT_main.cpp

Revision:

6:14051758db6f

Parent:

5:e5ca53305b87

Child:

7:ca1ade91bd14

--- a/PROJECT_main.cpp	Mon Nov 03 11:17:57 2014 +0000
+++ b/PROJECT_main.cpp	Mon Nov 03 13:05:31 2014 +0000
@@ -94,6 +94,7 @@
 float batjeset = 0; //een if statement wordt "true" wanneer batje voor een bepaalde tijd de juiste hoek heeft behouden
 float balhit = 0;  //balhit wordt 1 wanneer arm een bepaalde hoek heeft afgelegd
 float controlerror = 0;
+float previouserror = 0;
 float pwm = 0;
 
 float omrekenfactor1 = 0.0028035714; // 6.28/(32*70)
@@ -104,12 +105,19 @@
 
 float Kp1 = 1.10; //DEZE KP1 EN KP1 ZIJN NOG NIET DEFINITIEF
 float Ki1 = 0.20; //Kp en Ki van motor1, voor de slag
+float Kd1 = 0.0;
+
+float Kp3 = 0.09; //Kp en Ki van motor1, voor de return
+float Ki3 = 0.05;
+float Kd3 = 0.0;
 
 float Kp2 = 0.30; //Kp en Ki van motor2, voor in het positie brengen en voor de return
 float Ki2 = 0.20;
+float Kd2 = 0.0;
 
-float Kp3 = 0.09; //Kp en Ki van motor1, voor de return
-float Ki3 = 0.05;
+float Kp4 = 0.30; //Kp en Ki van motor2, voor de return
+float Ki4 = 0.20;
+float Kd4 = 0.0;
 
 volatile bool looptimerflag; //voor motorcontrol TSAMP
 
@@ -526,7 +534,9 @@
         //regelaar motor2, bepaalt positie
         controlerror = setpoint2 - motor2.getPosition()*omrekenfactor2;
         integral = integral + controlerror*TSAMP;
-        pwm = Kp2*controlerror + Ki2*integral;
+        derivative = (controlerror - previouserror)/TSAMP;
+        pwm = Kp2*controlerror + Ki2*integral + Kd2*derivative;
+        previouserror = controlerror; 
 
         keep_in_range(&pwm, -1,1);
         pwm_motor2.write(abs(pwm));
@@ -545,7 +555,7 @@
             }
         } else {
             pwm_motor2.write(0);
-            batjeset = integral = 0;
+            batjeset = integral = derivative = previouserror = 0;
             wait(1);
             goto motor1control;
         }
@@ -559,10 +569,12 @@
         if (balhit == 0) {  //regelaar motor1, bepaalt snelheid
             controlerror = setpoint1 - motor1.getSpeed()*omrekenfactor1;
             integral = integral + controlerror*TSAMP;
-            pwm = Kp1*controlerror + Ki1*integral;
+            derivative = (controlerror - previouserror)/TSAMP;
+            pwm = Kp1*controlerror + Ki1*integral + Kd1*derivative;
+            previouserror = controlerror; 
         } else {            //regelaar motor1, bepaalt positie
             pwm_motor1.write(0);
-            balhit = integral = 0;
+            balhit = integral = derivative = previouserror = 0;
             wait(1); // wait voordat arm weer naar beginpositie terugkeert
             goto resetpositionmotor1;
         }
@@ -591,7 +603,9 @@
         //regelaar motor1, bepaalt positie
         controlerror = -1*motor1.getPosition()*omrekenfactor1;
         integral = integral + controlerror*TSAMP;
-        pwm = Kp3*controlerror + Ki3*integral;
+        derivative = (controlerror - previouserror)/TSAMP;
+        pwm = Kp3*controlerror + Ki3*integral + Kd3*derivative;
+        previouserror = controlerror; 
 
         keep_in_range(&pwm, -1,1);
         if(pwm > 0) {
@@ -611,7 +625,7 @@
             }
         } else {
             pwm_motor1.write(0);
-            batjeset = integral = 0;
+            batjeset = integral = derivative = previouserror = 0;
             wait(1);
             goto resetpositionmotor2;
         }
@@ -625,7 +639,9 @@
         //regelaar motor2, bepaalt positie
         controlerror = -1*motor2.getPosition()*omrekenfactor2;
         integral = integral + controlerror*TSAMP;
-        pwm = Kp2*controlerror + Ki2*integral;
+        derivative = (controlerror - previouserror)/TSAMP;
+        pwm = Kp4*controlerror + Ki4*integral + Kd4*derivative;
+        previouserror = controlerror; 
 
         keep_in_range(&pwm, -1,1);
 
@@ -646,7 +662,7 @@
             }
         } else {
             pwm_motor2.write(0);
-            batjeset = integral = 0;
+            batjeset = integral = derivative = previouserror = 0;
             wait(1);
             direction = force = 0;
             goto directionchoice;