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@30:44676e1b38f8, 2018-03-20 (annotated)
- Committer:
- ahmed_lv
- Date:
- Tue Mar 20 05:56:22 2018 +0000
- Revision:
- 30:44676e1b38f8
- Parent:
- 14:5300cf1c57db
Editted Input Variables to PID
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
akashvibhute | 10:ea4b90be68dc | 1 | #include "pidAttitudeControl.h" |
akashvibhute | 10:ea4b90be68dc | 2 | |
akashvibhute | 10:ea4b90be68dc | 3 | pidAttitudeControl::pidAttitudeControl() |
akashvibhute | 10:ea4b90be68dc | 4 | { |
akashvibhute | 10:ea4b90be68dc | 5 | for(int i=0; i<4; i++) PIDFf_gains[i]=0; |
akashvibhute | 10:ea4b90be68dc | 6 | for(int i=0; i<4; i++) { |
akashvibhute | 10:ea4b90be68dc | 7 | for(int j=0; j<2; j++) |
akashvibhute | 10:ea4b90be68dc | 8 | PIDFf_limits[i][j]=0; |
akashvibhute | 10:ea4b90be68dc | 9 | } |
akashvibhute | 10:ea4b90be68dc | 10 | for(int i=0; i<2; i++) output_limits[i]=0; |
akashvibhute | 10:ea4b90be68dc | 11 | for(int i=0; i<2; i++) controlError[i] =0; |
akashvibhute | 10:ea4b90be68dc | 12 | for(int i=0; i<4; i++) PIDFf_terms[i]=0; |
akashvibhute | 10:ea4b90be68dc | 13 | Summ_term=0; |
akashvibhute | 10:ea4b90be68dc | 14 | |
akashvibhute | 10:ea4b90be68dc | 15 | //load default control values from config file |
akashvibhute | 10:ea4b90be68dc | 16 | PIDFf_limits[0][0] = (-1.0)*AttPIDFf_lim_P; |
akashvibhute | 10:ea4b90be68dc | 17 | PIDFf_limits[0][1] = (1.0)*AttPIDFf_lim_P; |
akashvibhute | 10:ea4b90be68dc | 18 | |
akashvibhute | 10:ea4b90be68dc | 19 | PIDFf_limits[1][0] = (-1.0)*AttPIDFf_lim_I; |
akashvibhute | 10:ea4b90be68dc | 20 | PIDFf_limits[1][1] = (1.0)*AttPIDFf_lim_I; |
akashvibhute | 10:ea4b90be68dc | 21 | |
akashvibhute | 10:ea4b90be68dc | 22 | PIDFf_limits[2][0] = (-1.0)*AttPIDFf_lim_D; |
akashvibhute | 10:ea4b90be68dc | 23 | PIDFf_limits[2][1] = (1.0)*AttPIDFf_lim_D; |
akashvibhute | 10:ea4b90be68dc | 24 | |
akashvibhute | 10:ea4b90be68dc | 25 | PIDFf_limits[3][0] = (-1.0)*AttPIDFf_lim_Ff; |
akashvibhute | 10:ea4b90be68dc | 26 | PIDFf_limits[3][1] = (1.0)*AttPIDFf_lim_Ff; |
akashvibhute | 10:ea4b90be68dc | 27 | |
akashvibhute | 10:ea4b90be68dc | 28 | output_limits[0]=AttPIDFf_range_min; |
akashvibhute | 10:ea4b90be68dc | 29 | output_limits[1]=AttPIDFf_range_max; |
akashvibhute | 10:ea4b90be68dc | 30 | |
akashvibhute | 10:ea4b90be68dc | 31 | PIDFf_gains[0]=AttPIDFf_kP/1000; |
akashvibhute | 10:ea4b90be68dc | 32 | PIDFf_gains[1]=AttPIDFf_kI/1000; |
akashvibhute | 10:ea4b90be68dc | 33 | PIDFf_gains[2]=AttPIDFf_kD/1000; |
akashvibhute | 10:ea4b90be68dc | 34 | PIDFf_gains[3]=AttPIDFf_kFf/1000; |
akashvibhute | 10:ea4b90be68dc | 35 | |
akashvibhute | 10:ea4b90be68dc | 36 | prevSetPoint=0.0; |
akashvibhute | 10:ea4b90be68dc | 37 | } |
akashvibhute | 10:ea4b90be68dc | 38 | |
akashvibhute | 10:ea4b90be68dc | 39 | void pidAttitudeControl::setGains(float kP,float kI, float kD, float kFf) |
akashvibhute | 10:ea4b90be68dc | 40 | { |
akashvibhute | 10:ea4b90be68dc | 41 | PIDFf_gains[0]=kP/1000; |
akashvibhute | 10:ea4b90be68dc | 42 | PIDFf_gains[1]=kI/1000; |
akashvibhute | 10:ea4b90be68dc | 43 | PIDFf_gains[2]=kD/1000; |
akashvibhute | 10:ea4b90be68dc | 44 | PIDFf_gains[3]=kFf/1000; |
akashvibhute | 10:ea4b90be68dc | 45 | } |
akashvibhute | 10:ea4b90be68dc | 46 | |
akashvibhute | 10:ea4b90be68dc | 47 | 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) |
akashvibhute | 10:ea4b90be68dc | 48 | { |
akashvibhute | 10:ea4b90be68dc | 49 | PIDFf_limits[0][0] = P_min; |
akashvibhute | 10:ea4b90be68dc | 50 | PIDFf_limits[0][1] = P_max; |
akashvibhute | 10:ea4b90be68dc | 51 | |
akashvibhute | 10:ea4b90be68dc | 52 | PIDFf_limits[1][0] = I_min; |
akashvibhute | 10:ea4b90be68dc | 53 | PIDFf_limits[1][1] = I_max; |
akashvibhute | 10:ea4b90be68dc | 54 | |
akashvibhute | 10:ea4b90be68dc | 55 | PIDFf_limits[2][0] = D_min; |
akashvibhute | 10:ea4b90be68dc | 56 | PIDFf_limits[2][1] = D_max; |
akashvibhute | 10:ea4b90be68dc | 57 | |
akashvibhute | 10:ea4b90be68dc | 58 | PIDFf_limits[3][0] = Ff_min; |
akashvibhute | 10:ea4b90be68dc | 59 | PIDFf_limits[3][1] = Ff_max; |
akashvibhute | 10:ea4b90be68dc | 60 | } |
akashvibhute | 10:ea4b90be68dc | 61 | |
akashvibhute | 10:ea4b90be68dc | 62 | void pidAttitudeControl::setOutputRange(float outMin, float outMax) |
akashvibhute | 10:ea4b90be68dc | 63 | { |
akashvibhute | 10:ea4b90be68dc | 64 | output_limits[0]=outMin; |
akashvibhute | 10:ea4b90be68dc | 65 | output_limits[1]=outMax; |
akashvibhute | 10:ea4b90be68dc | 66 | } |
akashvibhute | 10:ea4b90be68dc | 67 | |
akashvibhute | 10:ea4b90be68dc | 68 | float pidAttitudeControl::calcOutput(float setPoint, float feedback, float servoTime) |
akashvibhute | 10:ea4b90be68dc | 69 | { |
akashvibhute | 10:ea4b90be68dc | 70 | controlError[1] = setPoint - feedback; //control error |
akashvibhute | 12:ad3f323fcafc | 71 | |
akashvibhute | 10:ea4b90be68dc | 72 | //proportional term |
akashvibhute | 10:ea4b90be68dc | 73 | PIDFf_terms[0] = PIDFf_gains[0]*controlError[1]; |
akashvibhute | 10:ea4b90be68dc | 74 | PIDFf_terms[0] = generalFunctions::constrain_f(PIDFf_terms[0], PIDFf_limits[0][0], PIDFf_limits[0][1]); |
akashvibhute | 10:ea4b90be68dc | 75 | |
akashvibhute | 10:ea4b90be68dc | 76 | |
akashvibhute | 10:ea4b90be68dc | 77 | //integral term |
akashvibhute | 10:ea4b90be68dc | 78 | PIDFf_terms[1] += PIDFf_gains[1]*controlError[1]*servoTime; |
akashvibhute | 10:ea4b90be68dc | 79 | PIDFf_terms[1] = generalFunctions::constrain_f(PIDFf_terms[1], PIDFf_limits[1][0], PIDFf_limits[1][1]); |
akashvibhute | 10:ea4b90be68dc | 80 | |
akashvibhute | 10:ea4b90be68dc | 81 | //differential term |
akashvibhute | 10:ea4b90be68dc | 82 | PIDFf_terms[2] = PIDFf_gains[2] * (controlError[1] - controlError[0]) / servoTime; |
akashvibhute | 10:ea4b90be68dc | 83 | PIDFf_terms[2] = generalFunctions::constrain_f(PIDFf_terms[2], PIDFf_limits[2][0], PIDFf_limits[2][1]); |
akashvibhute | 10:ea4b90be68dc | 84 | |
akashvibhute | 10:ea4b90be68dc | 85 | //feedforward term |
akashvibhute | 10:ea4b90be68dc | 86 | PIDFf_terms[3] = PIDFf_gains[3]*setPoint; |
akashvibhute | 10:ea4b90be68dc | 87 | PIDFf_terms[3] = generalFunctions::constrain_f(PIDFf_terms[3], PIDFf_limits[3][0], PIDFf_limits[3][1]); |
akashvibhute | 10:ea4b90be68dc | 88 | |
akashvibhute | 10:ea4b90be68dc | 89 | //summ term |
akashvibhute | 10:ea4b90be68dc | 90 | Summ_term = PIDFf_terms[0] + PIDFf_terms[1] + PIDFf_terms[2] + PIDFf_terms[3]; |
akashvibhute | 10:ea4b90be68dc | 91 | Summ_term = generalFunctions::constrain_f(Summ_term, output_limits[0], output_limits[1]); |
akashvibhute | 10:ea4b90be68dc | 92 | |
akashvibhute | 10:ea4b90be68dc | 93 | controlError[0] = controlError[1]; //store control error for usage in next iteration |
akashvibhute | 10:ea4b90be68dc | 94 | |
akashvibhute | 10:ea4b90be68dc | 95 | return(Summ_term); |
akashvibhute | 10:ea4b90be68dc | 96 | } |