Implement new controller
Dependencies: mbed-rtos mbed QEI BNO055 MPU6050_DMP_Nucleo-I2Cdev virgo3_imuHandler_Orion_PCB MAX17048 Servo
Fork of Orion_newPCB_test by
06_AttitudeControl/pidAttitudeControl.cpp
- Committer:
- akashvibhute
- Date:
- 2016-04-23
- Revision:
- 10:ea4b90be68dc
- Child:
- 12:ad3f323fcafc
File content as of revision 10:ea4b90be68dc:
#include "pidAttitudeControl.h" pidAttitudeControl::pidAttitudeControl() { for(int i=0; i<4; i++) PIDFf_gains[i]=0; for(int i=0; i<4; i++) { for(int j=0; j<2; j++) PIDFf_limits[i][j]=0; } for(int i=0; i<2; i++) output_limits[i]=0; for(int i=0; i<2; i++) controlError[i] =0; for(int i=0; i<4; i++) PIDFf_terms[i]=0; Summ_term=0; //load default control values from config file PIDFf_limits[0][0] = (-1.0)*AttPIDFf_lim_P; PIDFf_limits[0][1] = (1.0)*AttPIDFf_lim_P; PIDFf_limits[1][0] = (-1.0)*AttPIDFf_lim_I; PIDFf_limits[1][1] = (1.0)*AttPIDFf_lim_I; PIDFf_limits[2][0] = (-1.0)*AttPIDFf_lim_D; PIDFf_limits[2][1] = (1.0)*AttPIDFf_lim_D; PIDFf_limits[3][0] = (-1.0)*AttPIDFf_lim_Ff; PIDFf_limits[3][1] = (1.0)*AttPIDFf_lim_Ff; output_limits[0]=AttPIDFf_range_min; output_limits[1]=AttPIDFf_range_max; PIDFf_gains[0]=AttPIDFf_kP/1000; PIDFf_gains[1]=AttPIDFf_kI/1000; PIDFf_gains[2]=AttPIDFf_kD/1000; PIDFf_gains[3]=AttPIDFf_kFf/1000; prevSetPoint=0.0; } void pidAttitudeControl::setGains(float kP,float kI, float kD, float kFf) { PIDFf_gains[0]=kP/1000; PIDFf_gains[1]=kI/1000; PIDFf_gains[2]=kD/1000; PIDFf_gains[3]=kFf/1000; } void pidAttitudeControl::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) { PIDFf_limits[0][0] = P_min; PIDFf_limits[0][1] = P_max; PIDFf_limits[1][0] = I_min; PIDFf_limits[1][1] = I_max; PIDFf_limits[2][0] = D_min; PIDFf_limits[2][1] = D_max; PIDFf_limits[3][0] = Ff_min; PIDFf_limits[3][1] = Ff_max; } void pidAttitudeControl::setOutputRange(float outMin, float outMax) { output_limits[0]=outMin; output_limits[1]=outMax; } float pidAttitudeControl::calcOutput(float setPoint, float feedback, float servoTime) { controlError[1] = setPoint - feedback; //control error //proportional term PIDFf_terms[0] = PIDFf_gains[0]*controlError[1]; PIDFf_terms[0] = generalFunctions::constrain_f(PIDFf_terms[0], PIDFf_limits[0][0], PIDFf_limits[0][1]); //integral term PIDFf_terms[1] += PIDFf_gains[1]*controlError[1]*servoTime; PIDFf_terms[1] = generalFunctions::constrain_f(PIDFf_terms[1], PIDFf_limits[1][0], PIDFf_limits[1][1]); //differential term PIDFf_terms[2] = PIDFf_gains[2] * (controlError[1] - controlError[0]) / servoTime; PIDFf_terms[2] = generalFunctions::constrain_f(PIDFf_terms[2], PIDFf_limits[2][0], PIDFf_limits[2][1]); //feedforward term PIDFf_terms[3] = PIDFf_gains[3]*setPoint; PIDFf_terms[3] = generalFunctions::constrain_f(PIDFf_terms[3], PIDFf_limits[3][0], PIDFf_limits[3][1]); //summ term Summ_term = PIDFf_terms[0] + PIDFf_terms[1] + PIDFf_terms[2] + PIDFf_terms[3]; Summ_term = generalFunctions::constrain_f(Summ_term, output_limits[0], output_limits[1]); controlError[0] = controlError[1]; //store control error for usage in next iteration return(Summ_term); }