Diff: 06_AttitudeControl/pidAttitudeControl.cpp

Revision:

10:ea4b90be68dc

Child:

12:ad3f323fcafc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/06_AttitudeControl/pidAttitudeControl.cpp	Sat Apr 23 10:29:24 2016 +0000
@@ -0,0 +1,96 @@
+#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);
+}
\ No newline at end of file