Servo.h

Go to the documentation of this file.

#pragma once

#include "mbed.h"

#include "util.h"

namespace nxpcup {

class Servo {
public:
    struct Config {
        PinName pin; 
        int8_t correctionAngle = 0; 
        uint8_t servoMinMaxAngle = 90; 
        int8_t defaultCenterAngle = 0; 
        bool isReverseSignal = false; 
        uint32_t minUs = 900; 
        uint32_t maxUs = 2100; 
        static constexpr uint16_t PERIOD_US = 20000; // 50 Hz
        static constexpr uint16_t CENTER_US = 1500;
    };

    Servo(Config config)
        : Servo(config.pin, config.minUs, config.maxUs)
    {
        m_isReverseSignal = config.isReverseSignal;
        setAngleCorrection(config.correctionAngle);
        setAngleMinMaxCenter(config.servoMinMaxAngle);
        setAngleCenter(config.defaultCenterAngle);
    }

    Servo(PinName pin, uint16_t minUs = 1000, uint16_t maxUs = 2000)
        : m_minUs(minUs)
        , m_maxUs(maxUs)
    {
#if defined SERVO_HARDWARE_PWM
        servo = new PwmOut(pin);
#elif defined SERVO_SOFTWARE_PWM
        servo = new SoftPWM(pin);
#endif

        servo->period_us(Config::PERIOD_US);
        servo->pulsewidth_us(Config::CENTER_US);
    }

    void setAngleCorrection(int8_t angle) { m_correctionAngle = angle; }

    void setAngleMinMax(uint8_t min, uint8_t max)
    {
        m_minAngle = min + m_correctionAngle;
        m_maxAngle = max + m_correctionAngle;
    }

    void setAngleMinMaxCenter(uint8_t diffAngle)
    {
        m_minAngle = 90 - diffAngle + m_correctionAngle;
        m_maxAngle = 90 + diffAngle + m_correctionAngle;
    }

    void setAngle(uint8_t degree)
    {
        m_lastAngle = nxpcup::clamp<uint8_t>(degree + m_correctionAngle, m_minAngle, m_maxAngle);
        uint16_t positionInMicrosecond = 0;
        if (m_isReverseSignal) {
            positionInMicrosecond = m_maxUs - (m_lastAngle * usDiff) / 180; // 180 degree = max range
        } else {
            positionInMicrosecond = m_minUs + (m_lastAngle * usDiff) / 180; // 180 degree = max range
        }
        setMicrosecond(positionInMicrosecond);
    }

    void setAngleCenter(int8_t degree, bool reverse = false)
    {
        setAngle(90 + ((reverse ? -1 : 1) * degree));
    }

    uint8_t angle() const { return m_lastAngle; }

    int8_t centerAngle() const { return angle() - 90; }

    int getMinAngle() const { return m_minAngle; }

    int getMaxAngle() const { return m_maxAngle; }

private:
    void setMicrosecond(uint16_t microsecond)
    {
        servo->pulsewidth_us(microsecond);
    }

#if defined SERVO_HARDWARE_PWM
    PwmOut* servo;
#elif defined SERVO_SOFTWARE_PWM
    SoftPWM* servo;
#endif

    uint8_t m_minAngle = 0;
    uint8_t m_maxAngle = 180;
    int8_t m_correctionAngle = 0;
    uint8_t m_lastAngle = 0;
    bool m_isReverseSignal = false;

    const uint32_t m_minUs;
    const uint32_t m_maxUs;
    const uint32_t usDiff = m_maxUs - m_minUs;
};

} // namespace nxpcup