Diff: pidControl.cpp

Revision:

19:2d3112fb0ab7

Parent:

18:12a542fa857e

--- a/pidControl.cpp	Fri Oct 30 10:41:02 2015 +0000
+++ b/pidControl.cpp	Tue Nov 03 10:15:24 2015 +0000
@@ -1,26 +1,26 @@
 #include "mbed.h"
 #include "QEI.h"
-#include "HIDScope.h"
-#include "MODSERIAL.h"
-#include "PinDetect.h"
 #include "pidControl.h"
-#include "biquadFilter.h"
 #include "compute.h"
 
-Serial pc(USBTX, USBRX);
-
-PinName m1_enc_a = D12;
-PinName m1_enc_b = D13;
-PinName m1_pwm = D6;
-PinName m1_dir = D7;
+PinName     m1_enc_a =  D12;
+PinName     m1_enc_b =  D13;
+PinName     m1_pwm =    D6;
+PinName     m1_dir =    D7;
 
-PinName m2_enc_a = D11;
-PinName m2_enc_b = D10;
-PinName m2_pwm = D5;
-PinName m2_dir = D4;
+PinName     m2_enc_a =  D11;
+PinName     m2_enc_b =  D10;
+PinName     m2_pwm =    D5;
+PinName     m2_dir =    D4;
 
-PinName m2_pot = A0;
-PinName m1_pot = A1;
+// PWM Speed Control：
+DigitalOut  dir1(m1_dir);
+PwmOut      pwm1(m1_pwm);
+DigitalOut  dir2(m2_dir);
+PwmOut      pwm2(m2_pwm);
+
+QEI         enc1(m1_enc_a, m1_enc_b, NC, 1);
+QEI         enc2(m2_enc_a, m2_enc_b, NC, 1);
 
 const double motor1_kp = 0.4f, motor1_ki = 0.005f, motor1_kd = 0.04f;
 const double motor2_kp = 0.4f, motor2_ki = 0.005f, motor2_kd = 0.04f;
@@ -28,37 +28,19 @@
 const double motor1_push_kp = 0.9f, motor1_push_ki = 0.009f, motor1_push_kd = 0.05f;
 const double motor2_push_kp = 0.9f, motor2_push_ki = 0.009f, motor2_push_kd = 0.05f;
 
-const double m1_f_a1 = 1.0, m1_f_a2 = 2.0, m1_f_b0 = 1.0, m1_f_b1 = 3.0, m1_f_b2 = 4.0;
-const double m2_f_a1 = 1.0, m2_f_a2 = 2.0, m2_f_b0 = 1.0, m2_f_b1 = 3.0, m2_f_b2 = 4.0;
+int     sigPerRev = 4192;
+float   tickRate = 0.01f;
+float   graphTickRate = 0.01f;
 
-int sigPerRev = 4192;
-float tickRate = 0.01f;
-float graphTickRate = 0.01f;
-
-double offsetA, offsetB;
-
-AnalogIn pot1(m1_pot);
-AnalogIn pot2(m2_pot);
+double  offsetA, offsetB;
+bool    pushing;
 
-// PWM Speed Control：
-DigitalOut dir1(m1_dir);
-PwmOut pwm1(m1_pwm);
-DigitalOut dir2(m2_dir);
-PwmOut pwm2(m2_pwm);
-
-QEI enc1(m1_enc_a, m1_enc_b, NC, 1);
-QEI enc2(m2_enc_a, m2_enc_b, NC, 1);
+float   currentRotation1 = 0, currentRotation2 = 0;
+float   desiredRotation1 = 0, desiredRotation2 = 0;
+double  error1 = 0, error2 = 0;
 
-float currentRotation1 = 0, currentRotation2 = 0;
-float desiredRotation1 = 0, desiredRotation2 = 0;
-double error1 = 0, error2 = 0;
-
-double m1_error_integral = 0, m2_error_integral = 0;
-double m1_error_derivative = 0, m2_error_derivative = 0;
-biquadFilter m1_filter(m1_f_a1, m1_f_a2, m1_f_b0, m1_f_b1, m1_f_b2);
-biquadFilter m2_filter(m2_f_a1, m2_f_a2, m2_f_b0, m2_f_b1, m2_f_b2);
-
-bool pushing;
+double  m1_error_integral = 0, m2_error_integral = 0;
+double  m1_error_derivative = 0, m2_error_derivative = 0;
 
 /*
 * Calculates the number of radians given a number of pulses of an encoder
@@ -75,12 +57,11 @@
 * Calculates the result of the PID Controller
 */
 double pid_control(double error, double kp, double ki, double ts, double &error_integral,
-                   double kd, double previous_error, double &error_derivative, biquadFilter filter)
+                   double kd, double previous_error, double &error_derivative)
 {
     error_integral = error_integral + ts * error;
     error_derivative = (error - previous_error) / ts;
-    // error_derivative = filter.step(error_derivative);
-
+    
     double result = kp * error + ki * error_integral + kd * error_derivative;
     return result;
 }
@@ -103,8 +84,6 @@
 */
 void PID_init()
 {
-    pc.printf("Initializing PID Controller...\r\n");
-
     /* Setting the pulse width modulation */
     pwm1.period_ms(0.01);
     pwm1.pulsewidth_ms(0.005);
@@ -114,7 +93,6 @@
     /* Resetting the encoders */
     enc1.reset();
     enc2.reset();
-    pc.printf("Encoders reset\r\n");
 
     /*  
     * Calibrate the first motor
@@ -247,12 +225,12 @@
             control2 = 0;
         }
         else {
-            control1 = pid_control(error1, motor1_push_kp, motor1_push_ki, tickRate, m1_error_integral, motor1_push_kd, previous_error1, m1_error_derivative, m1_filter);
-            control2 = pid_control(error2, motor2_push_kp, motor2_push_ki, tickRate, m2_error_integral, motor2_push_kd, previous_error2, m2_error_derivative, m2_filter);
+            control1 = pid_control(error1, motor1_push_kp, motor1_push_ki, tickRate, m1_error_integral, motor1_push_kd, previous_error1, m1_error_derivative);
+            control2 = pid_control(error2, motor2_push_kp, motor2_push_ki, tickRate, m2_error_integral, motor2_push_kd, previous_error2, m2_error_derivative);
         }
     } else {
-        control1 = pid_control(error1, motor1_kp, motor1_ki, tickRate, m1_error_integral, motor1_kd, previous_error1, m1_error_derivative, m1_filter);
-        control2 = pid_control(error2, motor2_kp, motor2_ki, tickRate, m2_error_integral, motor2_kd, previous_error2, m2_error_derivative, m2_filter);
+        control1 = pid_control(error1, motor1_kp, motor1_ki, tickRate, m1_error_integral, motor1_kd, previous_error1, m1_error_derivative);
+        control2 = pid_control(error2, motor2_kp, motor2_ki, tickRate, m2_error_integral, motor2_kd, previous_error2, m2_error_derivative);
     }
     
     int d1 = getPDirection(control1,1);