Motor.hpp

[詳解]

//
//    モータードライバ制御クラス
//
//    Copyright (c) 2022 udonrobo
//
 
#pragma once
 
#ifdef ARDUINO
 
#    include <Arduino.h>
#    include <Udon/Algorithm/MovingAverage.hpp>
#    include <Udon/Types/Direction.hpp>
 
namespace Udon
{

    template <size_t SmoothLevel>
    class SmoothlyMotor2
    {
        const uint8_t pinA;
        const uint8_t pinP;
 
        Udon::Direction direction;
 
        Udon::MovingAverage<SmoothLevel> ma;
 
    public:

        SmoothlyMotor2(const uint8_t pinA, const uint8_t pinP, Udon::Direction direction = Udon::Direction::Forward)
            : pinA(pinA)
            , pinP(pinP)
            , direction(direction)
            , ma{}
        {
        }

        void begin()
        {
            pinMode(pinA, OUTPUT);
        }

        void move(const int16_t power)
        {
            const int16_t p = ma(constrain(power, -250, 250)) * Udon::DirectionToSign(direction);
            digitalWrite(pinA, p >= 0 ? HIGH : LOW);
            analogWrite(pinP, abs(p));
        }

        void stop()
        {
            move(0);
        }

        void show() const
        {
            Serial.print(ma.getValue());
            Serial.print('\t');
        }
    };

    template <size_t SmoothLevel>
    class SmoothlyMotor3
    {
        const uint8_t pinA;
        const uint8_t pinB;
        const uint8_t pinP;
 
        Udon::Direction direction;
 
        Udon::MovingAverage<SmoothLevel> ma;
 
    public:

        SmoothlyMotor3(const uint8_t pinA, const uint8_t pinB, const uint8_t pinP, Udon::Direction direction = Udon::Direction::Forward)
            : pinA(pinA)
            , pinB(pinB)
            , pinP(pinP)
            , direction(direction)
            , ma{}
        {
        }

        void begin()
        {
            pinMode(pinA, OUTPUT);
            pinMode(pinB, OUTPUT);
        }

        void move(const int16_t power)
        {
            const int16_t p = ma(constrain(power, -250, 250)) * Udon::DirectionToSign(direction);
            digitalWrite(pinA, p >= 0 ? HIGH : LOW);
            digitalWrite(pinB, p <= 0 ? HIGH : LOW);
            analogWrite(pinP, abs(p));
        }

        void stop()
        {
            move(0);
        }

        void show() const
        {
            Serial.print(ma.getValue());
            Serial.print('\t');
        }
    };
 
    using Motor2 = SmoothlyMotor2<50>;
    using Motor3 = SmoothlyMotor3<50>;
 
#ifdef __AVR_ATmega328P__

    inline void ChangePwmPeriod()
    {
        TCCR1B &= B11111000;
        TCCR1B |= B00000001;
        TCCR2B &= B11111000;
        TCCR2B |= B00000001;
    }
 
#endif
 
}    // namespace Udon
 
#endif