Gurvan PRIEM / Mbed 2 deprecated RaptorControl

An incomplete quadcopter control programme.

Dependencies:   mbed

DaclePID/DaclePID.cpp@0:9cb9445a11f0, 2013-07-17 (annotated)

Committer:
Gurvan
Date:
Wed Jul 17 15:58:25 2013 +0000
Revision:
0:9cb9445a11f0
Pour Zobson, fi(r)st commit.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Gurvan 0:9cb9445a11f0 1 #include "DaclePID.h"
Gurvan 0:9cb9445a11f0 2
Gurvan 0:9cb9445a11f0 3
Gurvan 0:9cb9445a11f0 4 DaclePID::DaclePID(float Kc, float Ti, float Td, float interval) {
Gurvan 0:9cb9445a11f0 5
Gurvan 0:9cb9445a11f0 6 setCoeff(Kc, Ti, Td);
Gurvan 0:9cb9445a11f0 7 dT_ = interval;
Gurvan 0:9cb9445a11f0 8
Gurvan 0:9cb9445a11f0 9 accError_ = 0.0;
Gurvan 0:9cb9445a11f0 10 pidSortie_ = 0.0;
Gurvan 0:9cb9445a11f0 11 erreurPres_ = 0.0;
Gurvan 0:9cb9445a11f0 12 minInput_=0; maxInput_=3.3;
Gurvan 0:9cb9445a11f0 13 minOutput_=0; maxOutput_=3.3;
Gurvan 0:9cb9445a11f0 14 consigne_=0;
Gurvan 0:9cb9445a11f0 15 biais_=0;
Gurvan 0:9cb9445a11f0 16
Gurvan 0:9cb9445a11f0 17 }
Gurvan 0:9cb9445a11f0 18
Gurvan 0:9cb9445a11f0 19 void DaclePID::setCoeff(float Kc, float Ti, float Td){
Gurvan 0:9cb9445a11f0 20
Gurvan 0:9cb9445a11f0 21 Kc_ = Kc;
Gurvan 0:9cb9445a11f0 22 Ti_ = Ti;
Gurvan 0:9cb9445a11f0 23 Td_= Td;
Gurvan 0:9cb9445a11f0 24
Gurvan 0:9cb9445a11f0 25 }
Gurvan 0:9cb9445a11f0 26
Gurvan 0:9cb9445a11f0 27 void DaclePID::setInputLimits(float minInput, float maxInput){
Gurvan 0:9cb9445a11f0 28 minInput_=minInput; maxInput_=maxInput;
Gurvan 0:9cb9445a11f0 29 }
Gurvan 0:9cb9445a11f0 30
Gurvan 0:9cb9445a11f0 31 void DaclePID::setOutputLimits(float minOutput, float maxOutput){
Gurvan 0:9cb9445a11f0 32 minOutput_=minOutput; maxOutput_=maxOutput;
Gurvan 0:9cb9445a11f0 33 }
Gurvan 0:9cb9445a11f0 34
Gurvan 0:9cb9445a11f0 35 float DaclePID::dacalcul(float erreur){
Gurvan 0:9cb9445a11f0 36
Gurvan 0:9cb9445a11f0 37 float _erreur = to01(erreur)-to01(consigne_);
Gurvan 0:9cb9445a11f0 38
Gurvan 0:9cb9445a11f0 39 accError_ += _erreur;
Gurvan 0:9cb9445a11f0 40 float dErreur = (_erreur - erreurPres_) /dT_;
Gurvan 0:9cb9445a11f0 41
Gurvan 0:9cb9445a11f0 42 pidSortie_ = Kc_ * (_erreur + (Ti_ * accError_ * dT_) + (Td_ * dErreur));
Gurvan 0:9cb9445a11f0 43
Gurvan 0:9cb9445a11f0 44 erreurPres_ = _erreur;
Gurvan 0:9cb9445a11f0 45
Gurvan 0:9cb9445a11f0 46 return from01(pidSortie_) - from01(biais_);
Gurvan 0:9cb9445a11f0 47
Gurvan 0:9cb9445a11f0 48 }
Gurvan 0:9cb9445a11f0 49
Gurvan 0:9cb9445a11f0 50 float DaclePID::to01(float x){
Gurvan 0:9cb9445a11f0 51 return (x-minInput_)/(maxInput_-minInput_);
Gurvan 0:9cb9445a11f0 52 }
Gurvan 0:9cb9445a11f0 53
Gurvan 0:9cb9445a11f0 54 float DaclePID::from01(float x){
Gurvan 0:9cb9445a11f0 55 return (maxOutput_-minOutput_)*x + minOutput_;
Gurvan 0:9cb9445a11f0 56 }