Team Virgo v3 / Orion_newPCB_test_LV

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.

Dependencies:   mbed-rtos mbed QEI BNO055 MPU6050_DMP_Nucleo-I2Cdev virgo3_imuHandler_Orion_PCB MAX17048 Servo

Fork of Orion_newPCB_test by Team Virgo v3

01_DriveTrain/pidControl.cpp@6:690db8b5030b, 2016-02-04 (annotated)

Committer:
akashvibhute
Date:
Thu Feb 04 02:11:36 2016 +0000
Revision:
6:690db8b5030b
Parent:
2:761e3c932ce0 
PID tuned, general code cleanup

Who changed what in which revision?

UserRevisionLine numberNew contents of line
akashvibhute 2:761e3c932ce0 1 #include "pidControl.h"
akashvibhute 2:761e3c932ce0 2
akashvibhute 2:761e3c932ce0 3 pidControl::pidControl()
akashvibhute 2:761e3c932ce0 4 {
akashvibhute 2:761e3c932ce0 5 for(int i=0; i<4; i++) PIDFf_gains[i]=0;
akashvibhute 2:761e3c932ce0 6 for(int i=0; i<4; i++) {
akashvibhute 6:690db8b5030b 7 for(int j=0; j<2; j++)
akashvibhute 2:761e3c932ce0 8 PIDFf_limits[i][j]=0;
akashvibhute 2:761e3c932ce0 9 }
akashvibhute 2:761e3c932ce0 10 for(int i=0; i<2; i++) output_limits[i]=0;
akashvibhute 2:761e3c932ce0 11 for(int i=0; i<2; i++) controlError[i] =0;
akashvibhute 2:761e3c932ce0 12 for(int i=0; i<4; i++) PIDFf_terms[i]=0;
akashvibhute 6:690db8b5030b 13 Summ_term=0;
akashvibhute 6:690db8b5030b 14
akashvibhute 6:690db8b5030b 15 //load default control values from config file
akashvibhute 6:690db8b5030b 16 PIDFf_limits[0][0] = (-1.0)*PIDFf_lim_P;
akashvibhute 6:690db8b5030b 17 PIDFf_limits[0][1] = (1.0)*PIDFf_lim_P;
akashvibhute 6:690db8b5030b 18
akashvibhute 6:690db8b5030b 19 PIDFf_limits[1][0] = (-1.0)*PIDFf_lim_I;
akashvibhute 6:690db8b5030b 20 PIDFf_limits[1][1] = (1.0)*PIDFf_lim_I;
akashvibhute 6:690db8b5030b 21
akashvibhute 6:690db8b5030b 22 PIDFf_limits[2][0] = (-1.0)*PIDFf_lim_D;
akashvibhute 6:690db8b5030b 23 PIDFf_limits[2][1] = (1.0)*PIDFf_lim_D;
akashvibhute 6:690db8b5030b 24
akashvibhute 6:690db8b5030b 25 PIDFf_limits[3][0] = (-1.0)*PIDFf_lim_Ff;
akashvibhute 6:690db8b5030b 26 PIDFf_limits[3][1] = (1.0)*PIDFf_lim_Ff;
akashvibhute 6:690db8b5030b 27
akashvibhute 6:690db8b5030b 28 output_limits[0]=PIDFf_range_min;
akashvibhute 6:690db8b5030b 29 output_limits[1]=PIDFf_range_max;
akashvibhute 6:690db8b5030b 30
akashvibhute 6:690db8b5030b 31 PIDFf_gains[0]=PIDFf_kP/1000;
akashvibhute 6:690db8b5030b 32 PIDFf_gains[1]=PIDFf_kI/1000;
akashvibhute 6:690db8b5030b 33 PIDFf_gains[2]=PIDFf_kD/1000;
akashvibhute 6:690db8b5030b 34 PIDFf_gains[3]=PIDFf_kFf/1000;
akashvibhute 2:761e3c932ce0 35 }
akashvibhute 2:761e3c932ce0 36
akashvibhute 2:761e3c932ce0 37 void pidControl::setGains(float kP,float kI, float kD, float kFf)
akashvibhute 2:761e3c932ce0 38 {
akashvibhute 6:690db8b5030b 39 PIDFf_gains[0]=kP/1000;
akashvibhute 6:690db8b5030b 40 PIDFf_gains[1]=kI/1000;
akashvibhute 6:690db8b5030b 41 PIDFf_gains[2]=kD/1000;
akashvibhute 6:690db8b5030b 42 PIDFf_gains[3]=kFf/1000;
akashvibhute 2:761e3c932ce0 43 }
akashvibhute 2:761e3c932ce0 44
akashvibhute 2:761e3c932ce0 45 void pidControl::setTermLimits(float P_min, float P_max, float I_min, float I_max, float D_min, float D_max, float Ff_min, float Ff_max)
akashvibhute 2:761e3c932ce0 46 {
akashvibhute 2:761e3c932ce0 47 PIDFf_limits[0][0] = P_min;
akashvibhute 2:761e3c932ce0 48 PIDFf_limits[0][1] = P_max;
akashvibhute 2:761e3c932ce0 49
akashvibhute 2:761e3c932ce0 50 PIDFf_limits[1][0] = I_min;
akashvibhute 2:761e3c932ce0 51 PIDFf_limits[1][1] = I_max;
akashvibhute 2:761e3c932ce0 52
akashvibhute 2:761e3c932ce0 53 PIDFf_limits[2][0] = D_min;
akashvibhute 2:761e3c932ce0 54 PIDFf_limits[2][1] = D_max;
akashvibhute 2:761e3c932ce0 55
akashvibhute 2:761e3c932ce0 56 PIDFf_limits[3][0] = Ff_min;
akashvibhute 2:761e3c932ce0 57 PIDFf_limits[3][1] = Ff_max;
akashvibhute 2:761e3c932ce0 58 }
akashvibhute 2:761e3c932ce0 59
akashvibhute 2:761e3c932ce0 60 void pidControl::setOutputRange(float outMin, float outMax)
akashvibhute 2:761e3c932ce0 61 {
akashvibhute 2:761e3c932ce0 62 output_limits[0]=outMin;
akashvibhute 2:761e3c932ce0 63 output_limits[1]=outMax;
akashvibhute 2:761e3c932ce0 64 }
akashvibhute 2:761e3c932ce0 65
akashvibhute 2:761e3c932ce0 66 float pidControl::calcOutput(float setPoint, float feedback, float servoTime)
akashvibhute 2:761e3c932ce0 67 {
akashvibhute 2:761e3c932ce0 68 controlError[1] = setPoint - feedback; //control error
akashvibhute 2:761e3c932ce0 69
akashvibhute 2:761e3c932ce0 70 //proportional term
akashvibhute 2:761e3c932ce0 71 PIDFf_terms[0] = PIDFf_gains[0]*controlError[1];
akashvibhute 2:761e3c932ce0 72 PIDFf_terms[0] = generalFunctions::constrain_f(PIDFf_terms[0], PIDFf_limits[0][0], PIDFf_limits[0][1]);
akashvibhute 2:761e3c932ce0 73
akashvibhute 2:761e3c932ce0 74
akashvibhute 2:761e3c932ce0 75 //integral term
akashvibhute 2:761e3c932ce0 76 PIDFf_terms[1] += PIDFf_gains[1]*controlError[1]*servoTime;
akashvibhute 2:761e3c932ce0 77 PIDFf_terms[1] = generalFunctions::constrain_f(PIDFf_terms[1], PIDFf_limits[1][0], PIDFf_limits[1][1]);
akashvibhute 2:761e3c932ce0 78
akashvibhute 2:761e3c932ce0 79 //differential term
akashvibhute 2:761e3c932ce0 80 PIDFf_terms[2] = PIDFf_gains[2] * (controlError[1] - controlError[0]) / servoTime;
akashvibhute 2:761e3c932ce0 81 PIDFf_terms[2] = generalFunctions::constrain_f(PIDFf_terms[2], PIDFf_limits[2][0], PIDFf_limits[2][1]);
akashvibhute 2:761e3c932ce0 82
akashvibhute 2:761e3c932ce0 83 //feedforward term
akashvibhute 2:761e3c932ce0 84 PIDFf_terms[3] = PIDFf_gains[3]*setPoint;
akashvibhute 2:761e3c932ce0 85 PIDFf_terms[3] = generalFunctions::constrain_f(PIDFf_terms[3], PIDFf_limits[3][0], PIDFf_limits[3][1]);
akashvibhute 2:761e3c932ce0 86
akashvibhute 2:761e3c932ce0 87 //summ term
akashvibhute 2:761e3c932ce0 88 Summ_term = PIDFf_terms[0] + PIDFf_terms[1] + PIDFf_terms[2] + PIDFf_terms[3];
akashvibhute 2:761e3c932ce0 89 Summ_term = generalFunctions::constrain_f(Summ_term, output_limits[0], output_limits[1]);
akashvibhute 2:761e3c932ce0 90
akashvibhute 2:761e3c932ce0 91 controlError[0] = controlError[1]; //store control error for usage in next iteration
akashvibhute 2:761e3c932ce0 92
akashvibhute 2:761e3c932ce0 93 return(Summ_term);
akashvibhute 2:761e3c932ce0 94 }