verslag
Dependencies: Encoder HIDScope MODSERIAL- mbed-dsp mbed
Fork of PROJECT_FINAL by
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;