JServoControllerAdvanced.h

Go to the documentation of this file.

#ifndef J_SERVO_CONTROLLER_ADVANCED_H
#define J_SERVO_CONTROLLER_ADVANCED_H
#include "JMotorDriver/JMotorDriverServoAdvanced.h"
#include "JServoController.h"
#include <Arduino.h>
class JServoControllerAdvanced : public JServoController {
protected:
    JMotorDriverServoAdvanced& adServo;
    unsigned long weakenTimeout;
    float defaultFreq;
    float weakFreq;
    bool weakened;
    unsigned long startWeakTimeout;
    unsigned long enabledMillis;
 
public:
    JServoControllerAdvanced(JMotorDriverServoAdvanced& _servo, float _weakFreq = .5, unsigned long _weakenTimeout = 0, float _normalFreq = 1.0, unsigned long _startWeakTimeout = 0, 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)
        : JServoController(_servo, _reverse, velLimit, accelLimit, decelLimit, _disableTimeout, _minAngleLimit, _maxAngleLimit, _pos, _minSetAngle, _maxSetAngle, minServoVal, maxServoVal, _preventGoingWrongWay, _preventGoingTooFast, _stoppingDecelLimit)
        , adServo(_servo)
    {
        weakenTimeout = _weakenTimeout;
        startWeakTimeout = _startWeakTimeout;
        defaultFreq = _normalFreq;
        weakFreq = _weakFreq;
        adServo.adjustFrequency(_normalFreq);
        weakened = false;
        enabledMillis = millis();
    }
    virtual void run()
    {
        if (startWeakTimeout != 0 && millis() - enabledMillis < startWeakTimeout) {
            if (!weakened) {
                setStrengthWeak();
            }
            weakened = true;
        } else if (weakenTimeout != 0) {
            if (millis() - lastMovedMillis > weakenTimeout) {
                if (weakened == false) {
                    setStrengthWeak();
                }
                weakened = true;
            } else {
                if (weakened == true) {
                    setStrengthNormal();
                }
                weakened = false;
            }
        }
        JServoController::run();
    }
    // override JServoController
    bool setEnable(bool _enable)
    {
        enabledMillis = millis();
        return JServoController::setEnable(_enable);
    }
    void setStrengthWeak()
    {
        weakened = true;
        adjustServoFrequency(weakFreq);
    }
    void setStrengthNormal()
    {
        weakened = false;
        adjustServoFrequency(defaultFreq);
    }
    void adjustServoFrequency(float freq = 1.0)
    {
        adServo.adjustFrequency(freq);
        writeAngleToServo(dL.getPosition());
    }
    void setWeakFreq(float _weak)
    {
        weakFreq = _weak;
        if (weakened) {
            adjustServoFrequency(weakFreq);
        }
    }
    void setNormalFreq(float _norm)
    {
        defaultFreq = _norm;
        if (!weakened) {
            adjustServoFrequency(defaultFreq);
        }
    }
    bool getWeakened()
    {
        return weakened;
    }
    void setWeakTimeout(unsigned long _timeout)
    {
        weakenTimeout = _timeout;
    }
    unsigned long getWeakenTimeout()
    {
        return weakenTimeout;
    }
    void setStartWeakTimeout(unsigned long _timeout)
    {
        startWeakTimeout = _timeout;
    }
    unsigned long getStartWeakTimeout()
    {
        return startWeakTimeout;
    }
    float getWeakFreq()
    {
        return weakFreq;
    }
    float getNormalFreq()
    {
        return defaultFreq;
    }
};
#endif