TrekkingPhoenix / Mbed 2 deprecated Trekking_Controller_refac

Refactoring Ironcup 2020

Dependencies:   mbed mbed-rtos MotionSensor EthernetInterface

Control/PIDController.cpp@0:8f5db5085df7, 2020-09-21 (annotated)

Committer:
starling
Date:
Mon Sep 21 21:42:07 2020 +0000
Revision:
0:8f5db5085df7
12 mar 2020

Who changed what in which revision?

UserRevisionLine numberNew contents of line
starling 0:8f5db5085df7 1 #include "PIDController.hpp"
starling 0:8f5db5085df7 2
starling 0:8f5db5085df7 3 PIDController::PIDController(Motor* motor, PwmOut* servo, FXAS21002* gyro) {
starling 0:8f5db5085df7 4 this->motor = motor;
starling 0:8f5db5085df7 5 this->servo = servo;
starling 0:8f5db5085df7 6 this->gyro = gyro;
starling 0:8f5db5085df7 7
starling 0:8f5db5085df7 8 initializeController();
starling 0:8f5db5085df7 9
starling 0:8f5db5085df7 10 target_angle = 0;
starling 0:8f5db5085df7 11 saved_velocity = 0;
starling 0:8f5db5085df7 12 gyro_reference = 0;
starling 0:8f5db5085df7 13
starling 0:8f5db5085df7 14 interrupt_time = 0.02;
starling 0:8f5db5085df7 15 Ts = interrupt_time*1000;
starling 0:8f5db5085df7 16 }
starling 0:8f5db5085df7 17
starling 0:8f5db5085df7 18 void PIDController::initializeController() {
starling 0:8f5db5085df7 19 P = INITIAL_P;
starling 0:8f5db5085df7 20 I = INITIAL_I;
starling 0:8f5db5085df7 21 D = INITIAL_D;
starling 0:8f5db5085df7 22 N = INITIAL_N;
starling 0:8f5db5085df7 23
starling 0:8f5db5085df7 24 for (int i = 0; i < 2; i++)
starling 0:8f5db5085df7 25 {
starling 0:8f5db5085df7 26 e[i] = 0;
starling 0:8f5db5085df7 27 ui[i] = 0;
starling 0:8f5db5085df7 28 ud[i] = 0;
starling 0:8f5db5085df7 29 }
starling 0:8f5db5085df7 30 }
starling 0:8f5db5085df7 31
starling 0:8f5db5085df7 32 void PIDController::PIDTrigger() {
starling 0:8f5db5085df7 33 printf("PIDCntroller \r\n");
starling 0:8f5db5085df7 34 PIDController::RunPID();
starling 0:8f5db5085df7 35 }
starling 0:8f5db5085df7 36
starling 0:8f5db5085df7 37 void PIDController::PIDAttach() {
starling 0:8f5db5085df7 38
starling 0:8f5db5085df7 39 pid_trigger.attach(std::bind(&PIDController::PIDTrigger, this), 1.0);
starling 0:8f5db5085df7 40 }
starling 0:8f5db5085df7 41
starling 0:8f5db5085df7 42 void PIDController::PIDDetach() {
starling 0:8f5db5085df7 43 pid_trigger.detach();
starling 0:8f5db5085df7 44 }
starling 0:8f5db5085df7 45
starling 0:8f5db5085df7 46 void PIDController::setInterruptTime(float interrupt_time) {
starling 0:8f5db5085df7 47 this->interrupt_time = interrupt_time;
starling 0:8f5db5085df7 48 }
starling 0:8f5db5085df7 49
starling 0:8f5db5085df7 50 void PIDController::setPIDConstants(float P, float I, float D, float N) {
starling 0:8f5db5085df7 51 this->P = P;
starling 0:8f5db5085df7 52 this->I = I;
starling 0:8f5db5085df7 53 this->D = D;
starling 0:8f5db5085df7 54 this->N = N;
starling 0:8f5db5085df7 55 }
starling 0:8f5db5085df7 56
starling 0:8f5db5085df7 57 void PIDController::setTargetAngle(float target_angle) {
starling 0:8f5db5085df7 58 this->target_angle = target_angle;
starling 0:8f5db5085df7 59 if (target_angle > PI)
starling 0:8f5db5085df7 60 this->target_angle = target_angle - 2 * PI;
starling 0:8f5db5085df7 61 else if (target_angle < -PI)
starling 0:8f5db5085df7 62 this->target_angle = target_angle + 2 * PI;
starling 0:8f5db5085df7 63 }
starling 0:8f5db5085df7 64
starling 0:8f5db5085df7 65 void PIDController::addTargetAngle(float add_angle) {
starling 0:8f5db5085df7 66 this->target_angle = this->target_angle + add_angle;
starling 0:8f5db5085df7 67 }
starling 0:8f5db5085df7 68
starling 0:8f5db5085df7 69 void PIDController::Control() {
starling 0:8f5db5085df7 70 setServoPWM(u*100/(PI/8), *servo);
starling 0:8f5db5085df7 71 if(abs(e[0]) >= ERROR_THRESH && motor->getVelocity() > MINIMUM_CURVE_VELOCITY) {
starling 0:8f5db5085df7 72 saved_velocity = motor->getVelocity();
starling 0:8f5db5085df7 73 motor->setVelocity(MINIMUM_CURVE_VELOCITY);
starling 0:8f5db5085df7 74 }
starling 0:8f5db5085df7 75 else if(abs(e[0]) < ERROR_THRESH && motor->getVelocity() != saved_velocity) {
starling 0:8f5db5085df7 76 motor->setVelocity(saved_velocity);
starling 0:8f5db5085df7 77 }
starling 0:8f5db5085df7 78 }
starling 0:8f5db5085df7 79 /* PID controller for angular position */
starling 0:8f5db5085df7 80 void PIDController::RunPID() {
starling 0:8f5db5085df7 81 float feedback = gyro->get_angle() - gyro_reference;
starling 0:8f5db5085df7 82 e[0]= e[0] - 2*PI;
starling 0:8f5db5085df7 83 if(e[0] < -PI)
starling 0:8f5db5085df7 84 e[0] = e[0] + 2*PI;
starling 0:8f5db5085df7 85
starling 0:8f5db5085df7 86 /* Proportinal Part */
starling 0:8f5db5085df7 87 up[0] = e[0]*P;
starling 0:8f5db5085df7 88
starling 0:8f5db5085df7 89 /* Integral Part */
starling 0:8f5db5085df7 90 ui[1] = ui[0];
starling 0:8f5db5085df7 91 if(abs(u) < PI/8){
starling 0:8f5db5085df7 92 ui[0] = (P*I*Ts)*e[1] + ui[1];
starling 0:8f5db5085df7 93 }
starling 0:8f5db5085df7 94 else if(u > 0)
starling 0:8f5db5085df7 95 ui[0] = PI/8 - up[0];
starling 0:8f5db5085df7 96 else if(u < 0)
starling 0:8f5db5085df7 97 ui[0] = -PI/8 - up[0];
starling 0:8f5db5085df7 98
starling 0:8f5db5085df7 99 /* Derivative Part */
starling 0:8f5db5085df7 100 ud[1] = ud[0];
starling 0:8f5db5085df7 101 ud[0] = P*D*N*(e[0] - e[1]) - ud[1]*(N*Ts -1);
starling 0:8f5db5085df7 102
starling 0:8f5db5085df7 103 /** Sum of all parts **/
starling 0:8f5db5085df7 104 u = up[0] + ud[0] + ui[0];
starling 0:8f5db5085df7 105
starling 0:8f5db5085df7 106 Control();
starling 0:8f5db5085df7 107 }
starling 0:8f5db5085df7 108
starling 0:8f5db5085df7 109 void PIDController::setGyroReference(float gyro_ref) {
starling 0:8f5db5085df7 110 this->gyro_reference = gyro_ref;
starling 0:8f5db5085df7 111 }