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@14:e5fc69651b1d, 2018-11-05 (annotated)

Committer:
MAHCSnijders
Date:
Mon Nov 05 16:03:11 2018 +0000
Revision:
14:e5fc69651b1d
Parent:
4:5353c5d0d2ed 
Fixed constant r

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 4:5353c5d0d2ed 32 void PID::clear_state() {
brass_phoenix 4:5353c5d0d2ed 33 first_update = true;
brass_phoenix 4:5353c5d0d2ed 34 error_previous = 0;
brass_phoenix 4:5353c5d0d2ed 35 error_integral = 0;
brass_phoenix 4:5353c5d0d2ed 36
brass_phoenix 4:5353c5d0d2ed 37 // Cleart the low pass filter.
brass_phoenix 4:5353c5d0d2ed 38 wz[0] = 0;
brass_phoenix 4:5353c5d0d2ed 39 wz[1] = 0;
brass_phoenix 4:5353c5d0d2ed 40 }
brass_phoenix 4:5353c5d0d2ed 41
brass_phoenix 0:009e84d7af32 42 double PID::update(double error) {
brass_phoenix 0:009e84d7af32 43 if (first_update) {
brass_phoenix 0:009e84d7af32 44 // Make sure the previous error has a sensible value (not 0).
brass_phoenix 0:009e84d7af32 45 error_previous = error;
brass_phoenix 0:009e84d7af32 46 first_update = false;
brass_phoenix 0:009e84d7af32 47 }
brass_phoenix 0:009e84d7af32 48
brass_phoenix 0:009e84d7af32 49 // Proportional part
brass_phoenix 0:009e84d7af32 50 double u_p = Kp * error;
brass_phoenix 0:009e84d7af32 51
brass_phoenix 0:009e84d7af32 52 // Integral part
brass_phoenix 0:009e84d7af32 53 error_integral += error * pid_period;
brass_phoenix 0:009e84d7af32 54 double u_i = Ki * error_integral;
brass_phoenix 0:009e84d7af32 55
MAHCSnijders 14:e5fc69651b1d 56 // Derivative part (needs filter)
brass_phoenix 0:009e84d7af32 57 double error_der = (error-error_previous)/pid_period;
brass_phoenix 3:f1067b5bb5af 58 double filtered_error_der = biquad_step(error_der);
brass_phoenix 0:009e84d7af32 59 double u_d = Kd * filtered_error_der;
brass_phoenix 0:009e84d7af32 60 error_previous = error;
brass_phoenix 0:009e84d7af32 61
brass_phoenix 0:009e84d7af32 62 // Add all components and return.
brass_phoenix 0:009e84d7af32 63 return u_p + u_d + u_i;
brass_phoenix 3:f1067b5bb5af 64 }
brass_phoenix 3:f1067b5bb5af 65
brass_phoenix 3:f1067b5bb5af 66 double PID::biquad_step(double x) {
brass_phoenix 3:f1067b5bb5af 67
brass_phoenix 3:f1067b5bb5af 68 double y,w;
brass_phoenix 3:f1067b5bb5af 69
brass_phoenix 3:f1067b5bb5af 70 /* Direct form II */
brass_phoenix 3:f1067b5bb5af 71 w = x - A[0]*wz[0] - A[1]*wz[1];
brass_phoenix 3:f1067b5bb5af 72 y = B[0]*w + B[1]*wz[0] + B[2]*wz[1];
brass_phoenix 3:f1067b5bb5af 73
brass_phoenix 3:f1067b5bb5af 74 /* Shift */
brass_phoenix 3:f1067b5bb5af 75 wz[1] = wz[0];
brass_phoenix 3:f1067b5bb5af 76 wz[0] = w;
brass_phoenix 3:f1067b5bb5af 77
brass_phoenix 3:f1067b5bb5af 78 return y;
brass_phoenix 3:f1067b5bb5af 79 }