Biorobotics 7 / BioroboticsMotorControl

PID motor controll for the biorobotics project.

Dependencies:   FastPWM QEI

Dependents:   PID_example Motor_calibration Demo_mode Demo_mode ... more

pid.cpp@3:f1067b5bb5af, 2018-10-29 (annotated)

Committer:
brass_phoenix
Date:
Mon Oct 29 15:27:56 2018 +0000
Revision:
3:f1067b5bb5af
Parent:
0:009e84d7af32
Child:
4:5353c5d0d2ed
* No longer dependant on BiQuad library.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
brass_phoenix 0:009e84d7af32 1 #include "pid.h"
brass_phoenix 0:009e84d7af32 2
brass_phoenix 0:009e84d7af32 3 PID::PID() {
brass_phoenix 0:009e84d7af32 4 Kp = 0;
brass_phoenix 0:009e84d7af32 5 Ki = 0;
brass_phoenix 0:009e84d7af32 6 Kd = 0;
brass_phoenix 0:009e84d7af32 7 error_integral = 0;
brass_phoenix 0:009e84d7af32 8 error_previous = 0;
brass_phoenix 0:009e84d7af32 9 pid_period = 0;
brass_phoenix 0:009e84d7af32 10 first_update = true;
brass_phoenix 0:009e84d7af32 11
brass_phoenix 0:009e84d7af32 12 // Setup the low pass filter.
brass_phoenix 0:009e84d7af32 13 // The values are from the slides (PES, lecture 5)
brass_phoenix 3:f1067b5bb5af 14 B[0] = 0.0640; // b0
brass_phoenix 3:f1067b5bb5af 15 B[1] = 0.1279; // b1
brass_phoenix 3:f1067b5bb5af 16 B[2] = 0.0640; // b2
brass_phoenix 3:f1067b5bb5af 17 // a0 = 1
brass_phoenix 3:f1067b5bb5af 18 A[0] = -1.1683; // a1
brass_phoenix 3:f1067b5bb5af 19 A[1] = 0.4241; // a2
brass_phoenix 0:009e84d7af32 20 }
brass_phoenix 0:009e84d7af32 21
brass_phoenix 0:009e84d7af32 22 void PID::set_period(double period) {
brass_phoenix 0:009e84d7af32 23 pid_period = period;
brass_phoenix 0:009e84d7af32 24 }
brass_phoenix 0:009e84d7af32 25
brass_phoenix 0:009e84d7af32 26 void PID::set_k_values(double Kp, double Ki, double Kd) {
brass_phoenix 0:009e84d7af32 27 this->Kp = Kp;
brass_phoenix 0:009e84d7af32 28 this->Ki = Ki;
brass_phoenix 0:009e84d7af32 29 this->Kd = Kd;
brass_phoenix 0:009e84d7af32 30 }
brass_phoenix 0:009e84d7af32 31
brass_phoenix 0:009e84d7af32 32 double PID::update(double error) {
brass_phoenix 0:009e84d7af32 33 if (first_update) {
brass_phoenix 0:009e84d7af32 34 // Make sure the previous error has a sensible value (not 0).
brass_phoenix 0:009e84d7af32 35 error_previous = error;
brass_phoenix 0:009e84d7af32 36 first_update = false;
brass_phoenix 0:009e84d7af32 37 }
brass_phoenix 0:009e84d7af32 38
brass_phoenix 0:009e84d7af32 39 // Proportional part
brass_phoenix 0:009e84d7af32 40 double u_p = Kp * error;
brass_phoenix 0:009e84d7af32 41
brass_phoenix 0:009e84d7af32 42 // Integral part
brass_phoenix 0:009e84d7af32 43 error_integral += error * pid_period;
brass_phoenix 0:009e84d7af32 44 double u_i = Ki * error_integral;
brass_phoenix 0:009e84d7af32 45
brass_phoenix 0:009e84d7af32 46 // Derivative part
brass_phoenix 0:009e84d7af32 47 double error_der = (error-error_previous)/pid_period;
brass_phoenix 3:f1067b5bb5af 48 double filtered_error_der = biquad_step(error_der);
brass_phoenix 0:009e84d7af32 49 double u_d = Kd * filtered_error_der;
brass_phoenix 0:009e84d7af32 50 error_previous = error;
brass_phoenix 0:009e84d7af32 51
brass_phoenix 0:009e84d7af32 52 // Add all components and return.
brass_phoenix 0:009e84d7af32 53 return u_p + u_d + u_i;
brass_phoenix 3:f1067b5bb5af 54 }
brass_phoenix 3:f1067b5bb5af 55
brass_phoenix 3:f1067b5bb5af 56 double PID::biquad_step(double x) {
brass_phoenix 3:f1067b5bb5af 57
brass_phoenix 3:f1067b5bb5af 58 double y,w;
brass_phoenix 3:f1067b5bb5af 59
brass_phoenix 3:f1067b5bb5af 60 /* Direct form II */
brass_phoenix 3:f1067b5bb5af 61 w = x - A[0]*wz[0] - A[1]*wz[1];
brass_phoenix 3:f1067b5bb5af 62 y = B[0]*w + B[1]*wz[0] + B[2]*wz[1];
brass_phoenix 3:f1067b5bb5af 63
brass_phoenix 3:f1067b5bb5af 64 /* Shift */
brass_phoenix 3:f1067b5bb5af 65 wz[1] = wz[0];
brass_phoenix 3:f1067b5bb5af 66 wz[0] = w;
brass_phoenix 3:f1067b5bb5af 67
brass_phoenix 3:f1067b5bb5af 68 return y;
brass_phoenix 3:f1067b5bb5af 69 }