JMotorDriverEsp32Servo.h

Go to the documentation of this file.

#ifndef J_MOTOR_DRIVER_ESP32_SERVO_H
#define J_MOTOR_DRIVER_ESP32_SERVO_H
#include "JMotorDriverServoAdvanced.h"
#include <Arduino.h>
 
class JMotorDriverEsp32Servo : public JMotorDriverServoAdvanced {
private:
    bool enabled = false;
    byte servoPin;
    byte pwmChannel;
    float SERVO_TICKS_PER_MICROSECOND = 1;
    int SERVO_FREQ = 50;
    int SERVO_RES = 14;
 
public:
    JMotorDriverEsp32Servo(byte _pwmChannel, byte _servoPin, int _freq = 50, int _resBits = 14, int _minServoValue = 544, int _maxServoValue = 2400, bool _constrainRange = true)
    {
        minServoValue = _minServoValue;
        maxServoValue = _maxServoValue;
        enabled = false;
        servoPin = _servoPin;
        pwmChannel = _pwmChannel;
        constrainRange = _constrainRange;
        SERVO_FREQ = _freq;
        SERVO_RES = _resBits;
        SERVO_TICKS_PER_MICROSECOND = (float)(1 << SERVO_RES) * SERVO_FREQ / 1000000.0;
    }
 
    void adjustFrequency(float freq = 1.0)
    {
        setFrequencyAndResolution(freq * 50.0);
    }
 
    float setFrequencyAndResolution(int freq = 50, int resBits = 14)
    {
        if (freq == SERVO_FREQ && resBits == SERVO_RES) {
            return SERVO_TICKS_PER_MICROSECOND; // already set
        }
        SERVO_FREQ = freq;
        SERVO_RES = resBits;
        SERVO_TICKS_PER_MICROSECOND = (float)(1 << SERVO_RES) * SERVO_FREQ / 1000000.0;
        unsigned long startMicros = micros();
        while (digitalRead(servoPin) == HIGH && micros() - startMicros <= maxServoValue)
            ; // wait for pulse to go low to avoid cutting it short and causing the servo to twitch
        ledcDetachPin(servoPin);
        ledcSetup(pwmChannel, SERVO_FREQ, SERVO_RES);
        if (enabled) {
            ledcAttachPin(servoPin, pwmChannel);
            ledcWrite(pwmChannel, SERVO_TICKS_PER_MICROSECOND * setMicroseconds);
        }
        return SERVO_TICKS_PER_MICROSECOND;
    }
    bool set(float _val)
    {
        if (enabled) {
            float val;
            if (constrainRange)
                val = constrain(_val, -1.0, 1.0);
            else
                val = _val;
            setMicroseconds = ((maxServoValue + minServoValue) / 2 + (maxServoValue - minServoValue) * val / 2);
            ledcWrite(pwmChannel, SERVO_TICKS_PER_MICROSECOND * setMicroseconds);
        }
        return abs(_val) < 1.0;
    }
    bool setEnable(bool _enable)
    {
        if (_enable) {
            if (!enabled) {
                // actually enable
                enabled = true;
                pinMode(servoPin, OUTPUT);
                ledcSetup(pwmChannel, SERVO_FREQ, SERVO_RES);
                ledcAttachPin(servoPin, pwmChannel);
                return true;
            }
        } else { // disable
            if (enabled) {
                // actually disable
                enabled = false;
                unsigned long startMicros = micros();
                while (digitalRead(servoPin) == HIGH && micros() - startMicros <= maxServoValue)
                    ; // wait for pulse to go low to avoid cutting it short and causing the servo to twitch
                ledcDetachPin(servoPin);
                pinMode(servoPin, OUTPUT);
                digitalWrite(servoPin, LOW);
                return true;
            }
        }
        return false;
    }
 
    bool getEnable()
    {
        return enabled;
    }
    float getMaxRange()
    {
        return 1.0;
    }
    float getMinRange()
    {
        return -1.0;
    }
};
#endif