JServoController.h

Go to the documentation of this file.

#ifndef J_SERVO_CONTROLLER_H
#define J_SERVO_CONTROLLER_H
#include "Derivs_Limiter.h"
#include "JMotorController/JMotorController.h"
#include "JMotorDriver/JMotorDriverServo.h"
#include <Arduino.h>
// see bottom of file for #includes of subclasses and other JServoController relevant files
class JServoController : public JMotorController {
protected:
    JMotorDriverServo& servo;
    unsigned long disableTimeout;
    float minAngleLimit;
    float maxAngleLimit;
    float minSetAngle;
    float maxSetAngle;
    bool reverse;
    unsigned long lastMovedMillis;
    bool enabled;
    bool sleeping;
 
public:
    Derivs_Limiter dL;
 
    JServoController(JMotorDriverServo& _servo, bool _reverse = false, float velLimit = INFINITY, float accelLimit = INFINITY, float decelLimit = NAN, unsigned long _disableTimeout = 0, float _minAngleLimit = 0, float _maxAngleLimit = 180, float _pos = 90, float _minSetAngle = 0, float _maxSetAngle = 180, int minServoVal = 544, int maxServoVal = 2400, bool _preventGoingWrongWay = true, bool _preventGoingTooFast = true, float _stoppingDecelLimit = INFINITY)
        : servo(_servo)
        , dL(Derivs_Limiter(max(velLimit, float(0.0)), accelLimit, decelLimit, _pos, _pos, 0, _preventGoingWrongWay, _preventGoingTooFast, min(_minAngleLimit, _maxAngleLimit), max(_minAngleLimit, _maxAngleLimit), _stoppingDecelLimit))
    {
        enabled = false;
        sleeping = false;
        lastMovedMillis = 0;
        minAngleLimit = _minAngleLimit;
        maxAngleLimit = _maxAngleLimit;
        disableTimeout = _disableTimeout;
        reverse = _reverse;
        minSetAngle = _minSetAngle;
        maxSetAngle = _maxSetAngle;
        servo.setServoValues(minServoVal, maxServoVal);
    }
    virtual void run()
    {
        if (enabled)
            dL.calc();
        if (dL.getPositionDelta() != 0.0) {
            lastMovedMillis = millis();
        }
        if (disableTimeout != 0) {
            sleeping = millis() - lastMovedMillis > disableTimeout;
            if (sleeping) {
                if (servo.getEnable() == true) {
                    servo.disable();
                }
            } else { // activate
                if (servo.getEnable() == false && enabled) {
                    servo.enable();
                }
            }
        } else {
            sleeping = false;
        }
        if (servo.getEnable() && enabled) {
            writeAngleToServo(dL.getPosition());
        }
    }
    void setAngleImmediate(float angle, bool _run = true)
    {
        angle = constrain(angle, min(minAngleLimit, maxAngleLimit), max(minAngleLimit, maxAngleLimit));
        if (dL.getTarget() != angle) {
            wake();
        }
        dL.setTarget(angle);
        dL.setPosition(angle);
        if (_run)
            run();
    }
    void setAngleImmediateInc(float angleDiff, bool _run = true)
    {
        float angle = constrain(getPos() + angleDiff, min(minAngleLimit, maxAngleLimit), max(minAngleLimit, maxAngleLimit));
        if (dL.getTarget() != angle) {
            wake();
        }
        dL.setTarget(angle);
        dL.setPosition(angle);
        if (_run)
            run();
    }
    void setAngleSmoothed(float angle, bool _run = true)
    {
        angle = constrain(angle, min(minAngleLimit, maxAngleLimit), max(minAngleLimit, maxAngleLimit));
        if (dL.getTarget() != angle)
            wake();
        dL.setTarget(angle);
        if (_run)
            run();
    }
    void setDisableTimeout(unsigned long _timeout)
    {
        disableTimeout = _timeout;
    }
 
    unsigned long getDisableTimeout()
    {
        return disableTimeout;
    }
    unsigned long getLastMovedMillis()
    {
        return lastMovedMillis;
    }
    unsigned long getMillisSinceMoved()
    {
        return millis() - lastMovedMillis;
    }
    void setLastMovedMillis(unsigned long mil)
    {
        lastMovedMillis = mil;
    }
    virtual bool setEnable(bool _enable)
    {
 
        if (_enable && !enabled) {
            wake();
            restartRun();
            dL.setVelocity(0);
        }
        servo.setEnable(_enable);
        if (enabled != _enable) {
            enabled = _enable;
            return true;
        } else
            return false;
    }
    bool enable()
    {
        return setEnable(true);
    }
    bool disable()
    {
        return setEnable(false);
    }
    bool getEnable()
    {
        return enabled;
    }
    float getPosTarget()
    {
        return dL.getTarget();
    }
    float getPos()
    {
        return dL.getPosition();
    }
    float getVelocity()
    {
        return dL.getVelocity();
    }
    bool isPosAtTarget()
    {
        return dL.isPosAtTarget();
    }
    bool isPosNotAtTarget()
    {
        return dL.isPosNotAtTarget();
    }
    float distToTarget()
    {
        return dL.distToTarget();
    }
    void restartRun()
    {
        dL.resetTime();
    }
    bool getActive()
    {
        return !sleeping;
    }
    void wake()
    {
        lastMovedMillis = millis();
    }
    void setReverse(bool rev)
    {
        if (rev != reverse) {
            reverse = rev;
        }
    }
    bool getReverse()
    {
        return reverse;
    }
    void setAngleLimits(float _minAngleLimit, float _maxAngleLimit)
    {
        setMinAngleLimit(_minAngleLimit);
        setMaxAngleLimit(_maxAngleLimit);
    }
    void setMinAngleLimit(float _minAngleLimit)
    {
        if (minAngleLimit != _minAngleLimit) {
            minAngleLimit = _minAngleLimit;
        }
        dL.setPosLimits(min(minAngleLimit, maxAngleLimit), max(minAngleLimit, maxAngleLimit));
    }
    void setMaxAngleLimit(float _maxAngleLimit)
    {
        if (maxAngleLimit != _maxAngleLimit) {
            maxAngleLimit = _maxAngleLimit;
        }
        dL.setPosLimits(min(minAngleLimit, maxAngleLimit), max(minAngleLimit, maxAngleLimit));
    }
    float getMinAngleLimit()
    {
        return minAngleLimit;
    }
    float getMaxAngleLimit()
    {
        return maxAngleLimit;
    }
    float getAngleLimitRange()
    {
        return maxAngleLimit - minAngleLimit;
    }
 
    void setSetAngles(float _minSetAngle, float _maxSetAngle)
    {
        setMinSetAngle(_minSetAngle);
        setMaxSetAngle(_maxSetAngle);
    }
    void setMinSetAngle(float _minSetAngle)
    {
        if (minSetAngle != _minSetAngle) {
            minSetAngle = _minSetAngle;
        }
    }
    void setMaxSetAngle(float _maxSetAngle)
    {
        if (maxSetAngle != _maxSetAngle) {
            maxSetAngle = _maxSetAngle;
        }
    }
 
    float getAccelLimit()
    {
        return dL.getAccelLimit();
    }
    float getDecelLimit()
    {
        return dL.getDecelLimit();
    }
    float getVelLimit()
    {
        return dL.getVelLimit();
    }
    void setAccelLimit(float accelLim)
    {
        dL.setAccelLimit(accelLim);
    }
    void setAccelAndDecelLimits(float accelLim, float decelLim = NAN)
    {
        dL.setAccelAndDecelLimits(accelLim, decelLim);
    }
    void setDecelLimit(float decelLim = NAN)
    {
        dL.setDecelLimit(decelLim);
    }
    void setVelLimit(float velLim)
    {
        dL.setVelLimit(velLim);
    }
    void setVelAccelLimits(float velLim, float accelLim, float decelLim = NAN)
    {
        dL.setVelAccelLimits(velLim, accelLim, decelLim);
    }
    void setPosition(float pos, bool _run = true)
    {
        if (pos != dL.getPosition()) {
            wake();
            dL.setPosition(pos);
        }
        if (_run)
            run();
    }
 
    void setMinServoValue(int value)
    {
        if (servo.getMinServoValue() != value) {
            servo.setMinServoValue(value);
        }
    }
    void setMaxServoValue(int value)
    {
        if (servo.getMaxServoValue() != value) {
            servo.setMaxServoValue(value);
        }
    }
 
    void setServoRangeValues(int min, int max)
    {
        setMinServoValue(min);
        setMaxServoValue(max);
    }
 
    int getMinServoRangeValue()
    {
        return servo.getMinServoValue();
    }
    int getMaxServoRangeValue()
    {
        return servo.getMaxServoValue();
    }
    int getServoValueRange()
    {
        return servo.getServoValueRange();
    }
 
    int getSetMicroseconds()
    {
        return servo.getSetMicroseconds();
    }
    bool getConstrainRange()
    {
        return servo.getConstrainRange();
    }
    void setConstrainRange(bool _constrainRange)
    {
        servo.setConstrainRange(_constrainRange);
    }
 
    void setVel(float vel, bool _run = true)
    {
        dL.setVelConstant(vel);
        if (_run) {
            run();
        }
    }
    void setVelTarget(float vel, bool _run = true)
    {
        dL.setVelTarget(vel);
        if (_run) {
            run();
        }
    }
    float getVelTarget()
    {
        return dL.getVelTarget();
    }
    float getVel()
    {
        return dL.getVelocity();
    }
    float getDriverSetVal()
    {
        // TODO: fix placeholder (JServoController and JServoControllerAdvanced seem to have some issues with sharing some functions but having different driver types
        return NAN;
    }
    bool isDriverInRange()
    {
        // TODO: fix placeholder
        return true;
    }
    float getDriverMinRange()
    {
        return min(minAngleLimit, maxAngleLimit);
    }
    float getDriverMaxRange()
    {
        return max(minAngleLimit, maxAngleLimit);
    }
    float getMaxVel()
    {
        return dL.getVelLimit();
    }
    float getMinVel()
    {
        return 0;
    }
    void setMaxDriverRangeAmount(float _driverRangeAmount)
    {
        // TODO: might not make sense for a servo, but should this do anything?
    }
    bool setPosTarget(float _posTarget, bool _run = true)
    {
        float tempTarget = getPosTarget();
        setAngleSmoothed(_posTarget, _run);
        return tempTarget != getPosTarget();
    }
    bool setPosSetpoint(float _posSetpoint, bool _run = true)
    {
        float tempPos = getPos();
        setAngleImmediate(_posSetpoint, _run);
        return tempPos != getPos();
    }
    bool setPosDelta(float _posDelta, bool _run = true, bool _resetPos = false)
    {
        setVel(_posDelta, _run);
        // TODO: resetting the position doesn't make much sense for a servo, so it isn't handled here.
        return true;
    }
    float getPosSetpoint()
    {
        return getPos();
    }
    float resetPos()
    {
        // TODO: not handled by a servo
        return 0;
    }
    bool isPosModeNotVelocity()
    {
        return dL.isPosModeNotVelocity();
    }
 
protected:
    void
    writeAngleToServo(float ang)
    {
        ang = constrain(ang, min(minAngleLimit, maxAngleLimit), max(minAngleLimit, maxAngleLimit));
        if ((reverse) != (minAngleLimit < maxAngleLimit)) {
            ang = floatMap(ang, maxSetAngle, minSetAngle, servo.getMinRange(), servo.getMaxRange());
        } else { // not reversed
            ang = floatMap(ang, minSetAngle, maxSetAngle, servo.getMinRange(), servo.getMaxRange());
        }
        if (enabled && !sleeping) {
            servo.set(ang);
        }
    }
    float floatMap(float x, float in_min, float in_max, float out_min, float out_max)
    {
        return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
    }
};
 
#include "JServoControllerAdvanced.h"
#include "JServoControllerGentle.h"
#include "JServoControllerStallProtected.h"
#include "JServoCurrentSensor.h"
 
#endif