JControlLoopBasic.h

Go to the documentation of this file.

#ifndef J_CONTROL_LOOP_BASIC_H
#define J_CONTROL_LOOP_BASIC_H
#include "JControlLoop.h"
class JControlLoopBasic : public JControlLoop {
protected:
    float P;
    unsigned long timeout;
    unsigned long lastMovedMillis;
    bool noReverseVoltage;
 
public:
    JControlLoopBasic(float kP, unsigned long _timeout = 0, bool _noReverseVoltage = false)
    {
        P = kP;
        timeout = _timeout;
        lastMovedMillis = 0;
        noReverseVoltage = _noReverseVoltage;
    }
    float calc(JMotorControllerClosed* controller)
    {
        if (controller->getPosDeltaSetpoint() != 0 || abs(controller->getPosSetpoint() - controller->getPos()) > controller->getDistFromSetpointLimit() || abs(controller->getVel()) > controller->getMinVel()) {
            lastMovedMillis = millis();
        }
        result = 0;
        if (timeout == 0 || millis() - lastMovedMillis <= timeout) {
            error = (controller->getPosSetpoint() - controller->getPos());
            if (noReverseVoltage) {
                if (controller->getPosDeltaSetpoint() > 0) {
                    ctrlLoopOut = max(P * error, (float)0);
                } else if (controller->getPosDeltaSetpoint() < 0) {
                    ctrlLoopOut = min(P * error, (float)0);
                } else {
                    ctrlLoopOut = 0;
                }
            } else {
                ctrlLoopOut = P * error;
            }
            result = ctrlLoopOut + controller->getPosDeltaSetpoint();
        }
 
        return result;
    }
 
    void setKP(float _p)
    {
        P = _p;
    }
    float getKP()
    {
        return P;
    }
    void setTimeout(unsigned long _timeout)
    {
        timeout = _timeout;
    }
    unsigned long getTimeout()
    {
        return timeout;
    }
    void resetTime()
    {
        lastMovedMillis = millis();
    }
};
#endif