most functionality to splashdwon, find neutral and start mission. short timeouts still in code for testing, will adjust to go directly to sit_idle after splashdown
Dependencies: mbed MODSERIAL FATFileSystem
PidController/PidController.cpp
- Committer:
- danstrider
- Date:
- 2017-10-23
- Revision:
- 10:085ab7328054
File content as of revision 10:085ab7328054:
#include "PidController.hpp" PIDController::PIDController() { _error = 0.0; _integral = 0.0; _loLimit = 0.0; _hiLimit = 0.0; _deadbandFlag = false; _deadband = 0.0; } void PIDController::update(float position, float velocity, float dt) { // error update equations _error = _setPoint - position; _integral += (_error*dt); // pid controller equation _output = _Pgain*_error + _Igain*_integral + _Dgain*velocity ; // limiting on output & integral anti-windup if (_hiLimit != 0.0 and _output > _hiLimit) { _output = _hiLimit; _integral -= _error*dt; // implement saturation } if (_loLimit != 0.0 and _output < _loLimit) { _output = _loLimit; _integral += _error*dt; // implement saturation } // within deadband on error zeros output if (_deadbandFlag) { if (abs(_error) < _deadband) { _output = 0.0; } } } void PIDController::writeSetPoint(float cmd) { _setPoint = cmd; } float PIDController::getOutput() { return _output; } void PIDController::setPgain(float gain) { _Pgain = gain; } void PIDController::setIgain(float gain) { _Igain = gain; } void PIDController::setDgain(float gain) { _Dgain = gain; } void PIDController::toggleDeadBand(bool toggle) { _deadbandFlag = toggle; } void PIDController::setDeadBand(float deadband) { _deadband = deadband; }