quadcopter cufe / Mbed 2 deprecated 2006_Theta_Control_yaw

Quadcopter Attitude Control(Yaw-Pitch-Roll)

Dependencies:   mbed

PID_Alt.h@0:e63996fd7d3e, 2015-07-03 (annotated)

Committer:
khaledelmadawi
Date:
Fri Jul 03 11:16:02 2015 +0000
Revision:
0:e63996fd7d3e
Quadcopter Attitude Control(Yaw-Pitch-Roll)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
khaledelmadawi 0:e63996fd7d3e 1 #ifndef _PID_Alt
khaledelmadawi 0:e63996fd7d3e 2 #define _PID_Alt
khaledelmadawi 0:e63996fd7d3e 3 #include "SHARPIR.h"
khaledelmadawi 0:e63996fd7d3e 4 #include "mbed.h"
khaledelmadawi 0:e63996fd7d3e 5 #define D_BAUDRATE 115200
khaledelmadawi 0:e63996fd7d3e 6
khaledelmadawi 0:e63996fd7d3e 7 class PID_Alt{
khaledelmadawi 0:e63996fd7d3e 8 public:
khaledelmadawi 0:e63996fd7d3e 9 PID_Alt(int Mnum,float KP,float KI,float KD,float KDD,PinName AnalogPort);
khaledelmadawi 0:e63996fd7d3e 10 float findNominalVal(float altitude,float CosR,float CosP,char Calibrated);
khaledelmadawi 0:e63996fd7d3e 11 void Set_OUT_Limits(float LowerLimit,float UpperLimit);//setting Output Limits.
khaledelmadawi 0:e63996fd7d3e 12 void Set_IN_Limits(float LowerLimit,float UpperLimit);//setting Input Limits.
khaledelmadawi 0:e63996fd7d3e 13 void Check_OUT_Limits(float *RPMSetPoint);//checking Input Limits in the Realtime operation.
khaledelmadawi 0:e63996fd7d3e 14 void Check_IN_Limits(float *O_PWM);//checking Output Limits in Realtime operation.
khaledelmadawi 0:e63996fd7d3e 15 float Get_IR_Readings();
khaledelmadawi 0:e63996fd7d3e 16 float Get_OUT_POWER_SP();
khaledelmadawi 0:e63996fd7d3e 17 float Get_Current_IR();
khaledelmadawi 0:e63996fd7d3e 18 float GetErrorV();
khaledelmadawi 0:e63996fd7d3e 19 float GetErrorVD();
khaledelmadawi 0:e63996fd7d3e 20 float GetErrorVDD();
khaledelmadawi 0:e63996fd7d3e 21 float GetErrorVDOLD();
khaledelmadawi 0:e63996fd7d3e 22 float Get_reqestedAlt();
khaledelmadawi 0:e63996fd7d3e 23
khaledelmadawi 0:e63996fd7d3e 24 float MeanIR(int num);
khaledelmadawi 0:e63996fd7d3e 25 private:
khaledelmadawi 0:e63996fd7d3e 26 SHARPIR _IR;
khaledelmadawi 0:e63996fd7d3e 27 float PowerSetPoint;//,RPMSetPoint;
khaledelmadawi 0:e63996fd7d3e 28 float Out_PWM,Out_PWM2;
khaledelmadawi 0:e63996fd7d3e 29 float O_Upper_Limit,O_Lower_Limit;
khaledelmadawi 0:e63996fd7d3e 30 float I_Upper_Limit,I_Lower_Limit;
khaledelmadawi 0:e63996fd7d3e 31 float ErrorV,ErrorI,ErrorVD,ErrorVold,ErrorVDD,ErrorVDOLD,altitudeOld,nominalVal2;
khaledelmadawi 0:e63996fd7d3e 32 float V_req,V_curr,CURRENT_Alt;
khaledelmadawi 0:e63996fd7d3e 33 float h;
khaledelmadawi 0:e63996fd7d3e 34 float EVDOLDGetter,reqestedAlt;
khaledelmadawi 0:e63996fd7d3e 35 float _KP,_KD,_KI,_KDD;
khaledelmadawi 0:e63996fd7d3e 36 int Meannum;
khaledelmadawi 0:e63996fd7d3e 37 Serial debugSerial;
khaledelmadawi 0:e63996fd7d3e 38
khaledelmadawi 0:e63996fd7d3e 39 };
khaledelmadawi 0:e63996fd7d3e 40 PID_Alt::PID_Alt(int Mnum,float KP,float KI,float KD,float KDD,PinName AnalogPort):_IR(AnalogPort), debugSerial(USBTX, USBRX){
khaledelmadawi 0:e63996fd7d3e 41 _KP=KP;
khaledelmadawi 0:e63996fd7d3e 42 _KD=KD;
khaledelmadawi 0:e63996fd7d3e 43 _KI=KI;
khaledelmadawi 0:e63996fd7d3e 44 _KDD=KDD;
khaledelmadawi 0:e63996fd7d3e 45 Meannum=Mnum;
khaledelmadawi 0:e63996fd7d3e 46 V_req=0;
khaledelmadawi 0:e63996fd7d3e 47 V_curr=0;
khaledelmadawi 0:e63996fd7d3e 48 ErrorV=0;
khaledelmadawi 0:e63996fd7d3e 49 ErrorI=0;
khaledelmadawi 0:e63996fd7d3e 50 ErrorVD=0;
khaledelmadawi 0:e63996fd7d3e 51 ErrorVold=0;
khaledelmadawi 0:e63996fd7d3e 52 ErrorVDD=0;
khaledelmadawi 0:e63996fd7d3e 53 ErrorVDOLD=0;
khaledelmadawi 0:e63996fd7d3e 54 altitudeOld=0;
khaledelmadawi 0:e63996fd7d3e 55 nominalVal2=0.00115;
khaledelmadawi 0:e63996fd7d3e 56 debugSerial.baud(D_BAUDRATE);
khaledelmadawi 0:e63996fd7d3e 57
khaledelmadawi 0:e63996fd7d3e 58
khaledelmadawi 0:e63996fd7d3e 59 }
khaledelmadawi 0:e63996fd7d3e 60 float PID_Alt::findNominalVal(float altitude,float CosR,float CosP,char Calibrated){
khaledelmadawi 0:e63996fd7d3e 61 float curr_alt=0;
khaledelmadawi 0:e63996fd7d3e 62 Check_IN_Limits(&altitude);
khaledelmadawi 0:e63996fd7d3e 63 reqestedAlt=altitude;
khaledelmadawi 0:e63996fd7d3e 64 curr_alt=MeanIR(Meannum)*CosR*CosP;
khaledelmadawi 0:e63996fd7d3e 65 if(curr_alt<=9){
khaledelmadawi 0:e63996fd7d3e 66 curr_alt=150;
khaledelmadawi 0:e63996fd7d3e 67 }
khaledelmadawi 0:e63996fd7d3e 68 CURRENT_Alt=curr_alt;
khaledelmadawi 0:e63996fd7d3e 69 Check_IN_Limits(&curr_alt);
khaledelmadawi 0:e63996fd7d3e 70 V_req=altitude-curr_alt;
khaledelmadawi 0:e63996fd7d3e 71 V_curr=curr_alt-altitudeOld;
khaledelmadawi 0:e63996fd7d3e 72 altitudeOld=curr_alt;
khaledelmadawi 0:e63996fd7d3e 73 ErrorV= V_curr-V_req;
khaledelmadawi 0:e63996fd7d3e 74 ErrorVD= ErrorV-ErrorVold;
khaledelmadawi 0:e63996fd7d3e 75 ErrorVDD=ErrorVD-ErrorVDOLD;
khaledelmadawi 0:e63996fd7d3e 76 EVDOLDGetter=ErrorVDOLD;
khaledelmadawi 0:e63996fd7d3e 77 debugSerial.printf("E:%f ED:%f EDD:%f EDOld:%f\r\n",ErrorV,ErrorVD,ErrorVDD,ErrorVDOLD);
khaledelmadawi 0:e63996fd7d3e 78 if(Calibrated)
khaledelmadawi 0:e63996fd7d3e 79 nominalVal2=nominalVal2-_KP*ErrorV-_KD*ErrorVD-_KDD*ErrorVDD;
khaledelmadawi 0:e63996fd7d3e 80 ErrorVold=ErrorV;
khaledelmadawi 0:e63996fd7d3e 81 ErrorVDOLD=ErrorVD;
khaledelmadawi 0:e63996fd7d3e 82
khaledelmadawi 0:e63996fd7d3e 83 //debugSerial.printf("error:%f aError:%f alt:%f NV:%f Real:%f\r\n",(MeanIR(Meannum)-altitude), alpha*(MeanIR(Meannum)-altitude),altitude,nominalVal2,MeanIR(Meannum));
khaledelmadawi 0:e63996fd7d3e 84 Check_OUT_Limits(&nominalVal2);
khaledelmadawi 0:e63996fd7d3e 85 return nominalVal2;
khaledelmadawi 0:e63996fd7d3e 86 }
khaledelmadawi 0:e63996fd7d3e 87 float PID_Alt::MeanIR(int num)
khaledelmadawi 0:e63996fd7d3e 88 {
khaledelmadawi 0:e63996fd7d3e 89 float MeanReading=0;
khaledelmadawi 0:e63996fd7d3e 90 for(int i=0;i<num;i++)MeanReading+=Get_IR_Readings();
khaledelmadawi 0:e63996fd7d3e 91 return MeanReading/num;
khaledelmadawi 0:e63996fd7d3e 92 }
khaledelmadawi 0:e63996fd7d3e 93 float PID_Alt::Get_reqestedAlt(){
khaledelmadawi 0:e63996fd7d3e 94 return reqestedAlt;
khaledelmadawi 0:e63996fd7d3e 95 }
khaledelmadawi 0:e63996fd7d3e 96 float PID_Alt::GetErrorVDOLD(){
khaledelmadawi 0:e63996fd7d3e 97 return EVDOLDGetter;
khaledelmadawi 0:e63996fd7d3e 98 }
khaledelmadawi 0:e63996fd7d3e 99 float PID_Alt::GetErrorV(){
khaledelmadawi 0:e63996fd7d3e 100 return ErrorV;
khaledelmadawi 0:e63996fd7d3e 101 }
khaledelmadawi 0:e63996fd7d3e 102 float PID_Alt::GetErrorVD(){
khaledelmadawi 0:e63996fd7d3e 103 return ErrorVD;
khaledelmadawi 0:e63996fd7d3e 104 }
khaledelmadawi 0:e63996fd7d3e 105 float PID_Alt::GetErrorVDD(){
khaledelmadawi 0:e63996fd7d3e 106 return ErrorVDD;
khaledelmadawi 0:e63996fd7d3e 107 }
khaledelmadawi 0:e63996fd7d3e 108 float PID_Alt::Get_OUT_POWER_SP()
khaledelmadawi 0:e63996fd7d3e 109 {
khaledelmadawi 0:e63996fd7d3e 110 return nominalVal2;
khaledelmadawi 0:e63996fd7d3e 111 }
khaledelmadawi 0:e63996fd7d3e 112 float PID_Alt::Get_IR_Readings()
khaledelmadawi 0:e63996fd7d3e 113 {
khaledelmadawi 0:e63996fd7d3e 114 return _IR.cm();
khaledelmadawi 0:e63996fd7d3e 115 }
khaledelmadawi 0:e63996fd7d3e 116 float PID_Alt::Get_Current_IR()
khaledelmadawi 0:e63996fd7d3e 117 {
khaledelmadawi 0:e63996fd7d3e 118 return CURRENT_Alt;
khaledelmadawi 0:e63996fd7d3e 119 }
khaledelmadawi 0:e63996fd7d3e 120 //setting Output Limits.
khaledelmadawi 0:e63996fd7d3e 121 void PID_Alt::Set_OUT_Limits(float LowerLimt,float UpperLimit)
khaledelmadawi 0:e63996fd7d3e 122 {
khaledelmadawi 0:e63996fd7d3e 123 O_Upper_Limit=UpperLimit;
khaledelmadawi 0:e63996fd7d3e 124 O_Lower_Limit=LowerLimt;
khaledelmadawi 0:e63996fd7d3e 125 //initializations
khaledelmadawi 0:e63996fd7d3e 126 // power=O_Lower_Limit;
khaledelmadawi 0:e63996fd7d3e 127 // prev_power=O_Lower_Limit;
khaledelmadawi 0:e63996fd7d3e 128 }
khaledelmadawi 0:e63996fd7d3e 129 //setting Input Limits.
khaledelmadawi 0:e63996fd7d3e 130 void PID_Alt::Set_IN_Limits(float LowerLimit,float UpperLimit)
khaledelmadawi 0:e63996fd7d3e 131 {
khaledelmadawi 0:e63996fd7d3e 132 I_Upper_Limit=UpperLimit;
khaledelmadawi 0:e63996fd7d3e 133 I_Lower_Limit=LowerLimit;
khaledelmadawi 0:e63996fd7d3e 134 // prev_alt=I_Lower_Limit;
khaledelmadawi 0:e63996fd7d3e 135 }
khaledelmadawi 0:e63996fd7d3e 136 //checking Input Limits in the Realtime operation.
khaledelmadawi 0:e63996fd7d3e 137 void PID_Alt::Check_OUT_Limits(float *RPMSetPoint)
khaledelmadawi 0:e63996fd7d3e 138 {
khaledelmadawi 0:e63996fd7d3e 139 if(*RPMSetPoint>O_Upper_Limit)*RPMSetPoint=O_Upper_Limit;
khaledelmadawi 0:e63996fd7d3e 140 if(*RPMSetPoint<O_Lower_Limit)*RPMSetPoint=O_Lower_Limit;
khaledelmadawi 0:e63996fd7d3e 141 }
khaledelmadawi 0:e63996fd7d3e 142 //checking Output Limits in Realtime operation.
khaledelmadawi 0:e63996fd7d3e 143 void PID_Alt::Check_IN_Limits(float *O_PWM)
khaledelmadawi 0:e63996fd7d3e 144 {
khaledelmadawi 0:e63996fd7d3e 145 if(*O_PWM>I_Upper_Limit)*O_PWM=I_Upper_Limit;
khaledelmadawi 0:e63996fd7d3e 146 if(*O_PWM<I_Lower_Limit)*O_PWM=I_Lower_Limit;
khaledelmadawi 0:e63996fd7d3e 147 }
khaledelmadawi 0:e63996fd7d3e 148
khaledelmadawi 0:e63996fd7d3e 149 #endif