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@0:009e84d7af32, 2018-10-19 (annotated)

Committer:
brass_phoenix
Date:
Fri Oct 19 10:57:39 2018 +0000
Revision:
0:009e84d7af32
Child:
3:f1067b5bb5af
+ Created the 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 0:009e84d7af32 14 low_pass_filter = BiQuad(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
brass_phoenix 0:009e84d7af32 15 }
brass_phoenix 0:009e84d7af32 16
brass_phoenix 0:009e84d7af32 17 void PID::set_period(double period) {
brass_phoenix 0:009e84d7af32 18 pid_period = period;
brass_phoenix 0:009e84d7af32 19 }
brass_phoenix 0:009e84d7af32 20
brass_phoenix 0:009e84d7af32 21 void PID::set_k_values(double Kp, double Ki, double Kd) {
brass_phoenix 0:009e84d7af32 22 this->Kp = Kp;
brass_phoenix 0:009e84d7af32 23 this->Ki = Ki;
brass_phoenix 0:009e84d7af32 24 this->Kd = Kd;
brass_phoenix 0:009e84d7af32 25 }
brass_phoenix 0:009e84d7af32 26
brass_phoenix 0:009e84d7af32 27 double PID::update(double error) {
brass_phoenix 0:009e84d7af32 28 if (first_update) {
brass_phoenix 0:009e84d7af32 29 // Make sure the previous error has a sensible value (not 0).
brass_phoenix 0:009e84d7af32 30 error_previous = error;
brass_phoenix 0:009e84d7af32 31 first_update = false;
brass_phoenix 0:009e84d7af32 32 }
brass_phoenix 0:009e84d7af32 33
brass_phoenix 0:009e84d7af32 34 // Proportional part
brass_phoenix 0:009e84d7af32 35 double u_p = Kp * error;
brass_phoenix 0:009e84d7af32 36
brass_phoenix 0:009e84d7af32 37 // Integral part
brass_phoenix 0:009e84d7af32 38 error_integral += error * pid_period;
brass_phoenix 0:009e84d7af32 39 double u_i = Ki * error_integral;
brass_phoenix 0:009e84d7af32 40
brass_phoenix 0:009e84d7af32 41 // Derivative part
brass_phoenix 0:009e84d7af32 42 double error_der = (error-error_previous)/pid_period;
brass_phoenix 0:009e84d7af32 43 double filtered_error_der = low_pass_filter.step(error_der);
brass_phoenix 0:009e84d7af32 44 double u_d = Kd * filtered_error_der;
brass_phoenix 0:009e84d7af32 45 error_previous = error;
brass_phoenix 0:009e84d7af32 46
brass_phoenix 0:009e84d7af32 47 // Add all components and return.
brass_phoenix 0:009e84d7af32 48 return u_p + u_d + u_i;
brass_phoenix 0:009e84d7af32 49 }