New version of quadcopter software written to OO principles
Dependencies: mbed MODSERIAL filter mbed-rtos ConfigFile PID PPM FreeIMU_external_magnetometer TinyGPS
Global/PidWrapper/PidWrapper.cpp@4:9ffbf9101992, 2015-05-08 (annotated)
- Committer:
- joe4465
- Date:
- Fri May 08 09:07:38 2015 +0000
- Revision:
- 4:9ffbf9101992
- Parent:
- 2:969dfa4f2436
End of FYP
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
joe4465 | 2:969dfa4f2436 | 1 | #include "PidWrapper.h" |
joe4465 | 2:969dfa4f2436 | 2 | |
joe4465 | 2:969dfa4f2436 | 3 | PidWrapper::PidWrapper(){} |
joe4465 | 2:969dfa4f2436 | 4 | |
joe4465 | 2:969dfa4f2436 | 5 | PidWrapper::~PidWrapper(){} |
joe4465 | 2:969dfa4f2436 | 6 | |
joe4465 | 2:969dfa4f2436 | 7 | bool PidWrapper::initialise(PidParameter pidParameter, double inputMin, double inputMax, double outputMin, double outputMax, float updateTime) |
joe4465 | 2:969dfa4f2436 | 8 | { |
joe4465 | 2:969dfa4f2436 | 9 | _pid = new PID(pidParameter.p, pidParameter.i, pidParameter.d, updateTime); |
joe4465 | 2:969dfa4f2436 | 10 | _pid->setInputLimits(inputMin, inputMax); |
joe4465 | 2:969dfa4f2436 | 11 | _pid->setOutputLimits(outputMin, outputMax); |
joe4465 | 2:969dfa4f2436 | 12 | _pid->setMode(AUTO_MODE); |
joe4465 | 2:969dfa4f2436 | 13 | _pid->setSetPoint(0.0); |
joe4465 | 2:969dfa4f2436 | 14 | _pid->setBias(0); |
joe4465 | 2:969dfa4f2436 | 15 | |
joe4465 | 2:969dfa4f2436 | 16 | DEBUG("PID wrapper initialised\r\n"); |
joe4465 | 2:969dfa4f2436 | 17 | return true; |
joe4465 | 2:969dfa4f2436 | 18 | } |
joe4465 | 2:969dfa4f2436 | 19 | |
joe4465 | 2:969dfa4f2436 | 20 | double PidWrapper::compute(double setPoint, double processValue) |
joe4465 | 2:969dfa4f2436 | 21 | { |
joe4465 | 2:969dfa4f2436 | 22 | _pid->setSetPoint(setPoint); |
joe4465 | 2:969dfa4f2436 | 23 | _pid->setProcessValue(processValue); |
joe4465 | 2:969dfa4f2436 | 24 | return _pid->compute(); |
joe4465 | 2:969dfa4f2436 | 25 | } |
joe4465 | 2:969dfa4f2436 | 26 | |
joe4465 | 2:969dfa4f2436 | 27 | PidWrapper::PidParameter PidWrapper::getPidParameters() |
joe4465 | 2:969dfa4f2436 | 28 | { |
joe4465 | 2:969dfa4f2436 | 29 | PidWrapper::PidParameter pidParameters; |
joe4465 | 2:969dfa4f2436 | 30 | pidParameters.p = _pid->getPParam(); |
joe4465 | 2:969dfa4f2436 | 31 | pidParameters.i = _pid->getIParam(); |
joe4465 | 2:969dfa4f2436 | 32 | pidParameters.d = _pid->getDParam(); |
joe4465 | 2:969dfa4f2436 | 33 | |
joe4465 | 2:969dfa4f2436 | 34 | return pidParameters; |
joe4465 | 2:969dfa4f2436 | 35 | } |
joe4465 | 2:969dfa4f2436 | 36 | |
joe4465 | 2:969dfa4f2436 | 37 | void PidWrapper::reset() |
joe4465 | 2:969dfa4f2436 | 38 | { |
joe4465 | 2:969dfa4f2436 | 39 | _pid->reset(); |
joe4465 | 2:969dfa4f2436 | 40 | } |
joe4465 | 2:969dfa4f2436 | 41 | |
joe4465 | 2:969dfa4f2436 | 42 | void PidWrapper::setPidParameters(PidWrapper::PidParameter pidParameters) |
joe4465 | 2:969dfa4f2436 | 43 | { |
joe4465 | 2:969dfa4f2436 | 44 | _pid->setTunings(pidParameters.p, pidParameters.i, pidParameters.d); |
joe4465 | 2:969dfa4f2436 | 45 | DEBUG("P %1.8f, I %1.8f, D %1.8f\r\n", pidParameters.p, pidParameters.i, pidParameters.d); |
joe4465 | 2:969dfa4f2436 | 46 | } |
joe4465 | 2:969dfa4f2436 | 47 | |
joe4465 | 2:969dfa4f2436 | 48 | void PidWrapper::setBias(float bias) |
joe4465 | 2:969dfa4f2436 | 49 | { |
joe4465 | 2:969dfa4f2436 | 50 | _pid->setBias(bias); |
joe4465 | 2:969dfa4f2436 | 51 | } |