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.hpp

Committer:
joel_ssc
Date:
2019-05-13
Revision:
92:52a91656458a
Parent:
59:4c04d5c7aed1

File content as of revision 92:52a91656458a:

#ifndef PIDCONTROLLER_H
#define PIDCONTROLLER_H

#include "mbed.h"

class PIDController {
public:
    PIDController();
    
    void update(float position, float velocity, float dt);
    float getOutput();
    
    void setPgain(float gain);
    void setIgain(float gain);
    void setDgain(float gain);
    
    void writeSetPoint(float cmd);
    
    void setHiLimit(float high_limit);
    void setLoLimit(float low_limit);
    
    void toggleDeadBand(bool toggle);
    void setDeadBand(float deadband);
    
    void setHeadingFlag(bool heading_flag);
    
protected:
    float _setPoint;
    float _error;
    float _integral;
    float _output;

    float _Pgain;
    float _Dgain;
    float _Igain;
    float _hiLimit; //these parameters clamp the allowable output
    float _loLimit; //these parameters clamp the allowable output
    float _deadband;
    bool _deadbandFlag;
    bool _headingFlag;
    
    float _temp_velocity;
};

#endif