JServoControllerGentle.h

Go to the documentation of this file.

#ifndef J_SERVO_CONTROLLER_GENTLE_H
#define J_SERVO_CONTROLLER_GENTLE_H
#include "JServoController.h"
#include "JServoStallSensing.h"
class JServoControllerGentle : public JServoController {
protected:
    bool gripDirPos;
    float targetForce;
    float gripSpeed;
 
public:
    bool grippingGently;
    JServoStallSensing& stallSensor;
 
    JServoControllerGentle(JServoControllerAdvanced servo, JServoStallSensing& servoStallSensor, float _targetForce = .05, float _gripSpeed = 30)
        : JServoController(servo)
        , stallSensor(servoStallSensor)
    {
        gripSpeed = max(_gripSpeed, (float)0);
        grippingGently = false;
        gripDirPos = true;
        targetForce = max(_targetForce, (float)0);
    }
 
    virtual void run()
    {
        stallSensor.run();
        if (grippingGently) {
            if (stallSensor.getMeasurement(false) < targetForce) {
                JServoController::setAngleImmediateInc((gripDirPos ? 1.0 : -1.0) * gripSpeed * dL.getTimeInterval(), false);
            } else if (stallSensor.getMeasurement(false) > targetForce) {
                JServoController::setAngleImmediateInc((!gripDirPos ? 1.0 : -1.0) * gripSpeed * dL.getTimeInterval(), false);
            } else {
                JServoController::setAngleImmediateInc(0, false);
            }
        }
        JServoController::run();
    }
    void gripGently(bool _dirPos, float _targetForce = NAN, float _gripSpeed = NAN, bool _run = false)
    {
        if (!isnan(_targetForce))
            targetForce = _targetForce;
        if (!isnan(_gripSpeed))
            gripSpeed = _gripSpeed;
        gripDirPos = _dirPos;
        grippingGently = true;
        if (_run)
            run();
    }
 
    void setPosition(float pos, bool _run)
    {
        grippingGently = false;
        JServoController::setPosition(pos, _run);
    }
    void setAngleImmediate(float angle, bool _run = true)
    {
        grippingGently = false;
        JServoController::setAngleImmediate(angle, _run);
    }
    void setAngleImmediateInc(float angleDiff, bool _run = true)
    {
        grippingGently = false;
        JServoController::setAngleImmediateInc(angleDiff, _run);
    }
    void setAngleSmoothed(float angle, bool _run = true)
    {
        grippingGently = false;
        JServoController::setAngleSmoothed(angle, _run);
    }
};
#endif