Module 9 Super Team / PIDController1

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of PIDController by Kevin Hetterscheid

Files at this revision

API Documentation at this revision

Revision 19:2d3112fb0ab7, committed 2015-11-03

Comitter:
AeroKev
Date:
Tue Nov 03 10:15:24 2015 +0000
Parent:
18:12a542fa857e
Commit message:
Last Commit

Changed in this revision

HIDScope.lib Show diff for this revision Revisions of this file
MODSERIAL.lib Show diff for this revision Revisions of this file
PinDetect.lib Show diff for this revision Revisions of this file
QEI.lib Show diff for this revision Revisions of this file
biquadFilter.lib Show diff for this revision Revisions of this file
pidControl.cpp Show annotated file Show diff for this revision Revisions of this file 
--- a/HIDScope.lib	Fri Oct 30 10:41:02 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://developer.mbed.org/teams/Module-9-Super-Team/code/HIDScope/#20f3db57ccec

--- a/MODSERIAL.lib	Fri Oct 30 10:41:02 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/Sissors/code/MODSERIAL/#6ffa97119f4f

--- a/PinDetect.lib	Fri Oct 30 10:41:02 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/AjK/code/PinDetect/#cb3afc45028b

--- a/QEI.lib	Fri Oct 30 10:41:02 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://developer.mbed.org/users/aberk/code/QEI/#5c2ad81551aa

--- a/biquadFilter.lib	Fri Oct 30 10:41:02 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/tomlankhorst/code/biquadFilter/#e3bf917ae0a3

--- 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);