Team Virgo v3 / Orion_newPCB_test_LV

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 Team Virgo v3

06_AttitudeControl/pidAttitudeControl.cpp@10:ea4b90be68dc, 2016-04-23 (annotated)

Committer:
akashvibhute
Date:
Sat Apr 23 10:29:24 2016 +0000
Revision:
10:ea4b90be68dc
Child:
12:ad3f323fcafc
virgo 3.3 with 6v motors

Who changed what in which revision?

UserRevisionLine numberNew 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 10:ea4b90be68dc 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 }