Diff: main.cpp

Revision:

2:7e5ca3715fb6

Parent:

1:570c0d599b9e

--- a/main.cpp	Fri Oct 20 09:32:21 2017 +0000
+++ b/main.cpp	Thu Oct 26 08:19:53 2017 +0000
@@ -2,7 +2,7 @@
 #include "encoder.h"
 #include "Servo.h"
 #include "MODSERIAL.h"
-
+#include "FastPWM.h"
 
 
 
@@ -14,7 +14,7 @@
 Ticker MyControllerTicker1;
 Ticker MyControllerTicker2;
 const double RAD_PER_PULSE = 0.00074799825*2;
-const float CONTROLLER_TS = 0.01;
+const float CONTROLLER_TS = 0.02;
 const double PI = 3.14159265358979323846;
 //------------------------------------------------------------------------------
 //------------------------------------------------------------------------------
@@ -115,24 +115,54 @@
 
 
 //------------------------------------------------------------------------------
+//------------------------------PID_Controller-----------------------------------
+//------------------------------------------------------------------------------
+
+double PID(float error, const float Kp, const float Ki, const float Kd, const float Ts, const float N, double &v1, double &v2) {
+    const double a1 = -4 / (N*Ts+2); 
+    const double a2 = -(N*Ts-2) / ( N*Ts+2);
+    const double b0 = (4*Kp + 4*Kd*N +2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2)) / (2*N*Ts + 4);
+    const double b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N) / (N*Ts + 2);
+    const double b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2)) / (2*N*Ts + 4);
+    
+    double v = error - a1*v1 - a2*v2;
+    double u = b0*v + b1*v1 + b2*v2;
+    v2=v1;
+    v1=v;
+    return u;
+    
+}
+
+//------------------------------------------------------------------------------
+//------------------------------------------------------------------------------
+//------------------------------------------------------------------------------
+
+
+
+
+
+//------------------------------------------------------------------------------
 //--------------------------------Motor1----------------------------------------
 //------------------------------------------------------------------------------
-PwmOut motor1(D5);
+FastPWM motor1(D5);
 DigitalOut motor1DirectionPin(D4);
 DigitalIn ENC2A(D12);
 DigitalIn ENC2B(D13);
 Encoder encoder1(D13,D12);
-const float MOTOR1_KP = 50.0;
-const float MOTOR1_KI = 30.0;
-double m1_err_int = 0;
+const float MOTOR1_KP = 40.0;
+const float MOTOR1_KI = 0.0;
+const float MOTOR1_KD = 15.0;
+double M1_v1 = 0.0;
+double M1_v2 = 0.0;
 const double motor1_gain = 2*PI;
+const float M1_N = 0.5;
 
 
 void motor1_control(){
     float reference_alpha = getreferencebeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2);
     float position_alpha = RAD_PER_PULSE * encoder1.getPosition();
     float error_alpha = reference_alpha-position_alpha;
-    float magnitude1 = PI_controller(error_alpha, MOTOR1_KP, MOTOR1_KI, CONTROLLER_TS, m1_err_int)/ motor1_gain;
+    float magnitude1 = PID(error_alpha, MOTOR1_KP, MOTOR1_KI, MOTOR1_KD, CONTROLLER_TS, M1_N, M1_v1, M1_v2) / motor1_gain;
     motor1 = fabs(magnitude1);
     pc.printf("err_a = %f  alpha = %f   mag1 = %f\r\n", error_alpha, reference_alpha, magnitude1);
     pc.printf("\r\n");
@@ -156,22 +186,26 @@
 //------------------------------------------------------------------------------
 //--------------------------------Motor2----------------------------------------
 //------------------------------------------------------------------------------
-PwmOut motor2(D6);
+FastPWM motor2(D6);
 DigitalOut motor2DirectionPin(D7);
 DigitalIn ENC1A(D10);
 DigitalIn ENC1B(D11);
 Encoder encoder2(D10,D11);
-const float MOTOR2_KP = 50.0;
-const float MOTOR2_KI = 30.0;
+const float MOTOR2_KP = 60.0;
+const float MOTOR2_KI = 0.0;
+const float MOTOR2_KD = 15.0;
 double m2_err_int = 0;
 const double motor2_gain = 2*PI;
+const float M2_N = 0.5;
+double M2_v1 = 0.0;
+double M2_v2 = 0.0;
 
 
 void motor2_control(){
     float reference_beta = getreferencealpha(max_rangex, max_rangey, x_offset, y_offset, alpha_offset, L1, L2);
     float position_beta = RAD_PER_PULSE * -encoder2.getPosition();
     float error_beta = reference_beta-position_beta;
-    float magnitude2 = PI_controller(error_beta, MOTOR2_KP, MOTOR2_KI, CONTROLLER_TS, m2_err_int)/ motor2_gain;
+    float magnitude2 = PID(error_beta, MOTOR2_KP, MOTOR2_KI, MOTOR2_KD, CONTROLLER_TS, M2_N, M1_v1, M1_v2) / motor2_gain;
     motor2 = fabs(magnitude2);
     pc.printf("err2 = %f  beta = %f   mag2 = %f\r\n", error_beta, reference_beta, magnitude2);
     pc.printf("\r\n");
@@ -193,8 +227,8 @@
 
 int main(){
     pc.baud(115200);
-    motor1.period(0.0002f);
-    motor2.period(0.0002f);
+    motor1.period(0.0001f);
+    motor2.period(0.0001f);
     MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS); 
     MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS);
     But1.rise(&servo_control);