RoboMasterMotor.hpp

[詳解]

//
//    ロボマスモーター
//
//    Copyright (c) 2022-2024 udonrobo
//
 
#pragma once
 
#include <Udon/Com/Can.hpp>
#include <Udon/Algorithm/Math.hpp>
 
namespace Udon
{
 
    namespace Impl
    {
 
        class RoboMasterBase
        {
        public:
            RoboMasterBase(Udon::ICanBus& bus, uint8_t motorId, bool direction = true)
                : bus{ bus }
                , motorId{ motorId }
                , direction{ direction }
                , nodeRx{ bus.createRx(static_cast<uint32_t>(0x200 + Constrain((int)motorId, 1, 8)), 8) }
            {
                if (motorId > 4)
                    nodeTx = bus.createTx(static_cast<uint32_t>(0x1FF), 8);
                else
                    nodeTx = bus.createTx(static_cast<uint32_t>(0x200), 8);
 
                nodeRx->onReceive = onReceive;
                nodeRx->param = this;
            }
 
 
            RoboMasterBase(const RoboMasterBase&) = delete;
 
 
            RoboMasterBase(RoboMasterBase&& other)
                : bus{ other.bus }
                , motorId{ other.motorId }
                , direction{ other.direction }
                , nodeTx{ other.nodeTx }
                , nodeRx{ other.nodeRx }
            {
                nodeRx->param = this;    // ここでotherに登録されていたthisポインタを上書きするため、コピーすることができない
            }
 
 
            double getAngle() const
            {
                return (angle + offsetAngle) * 2 * Udon::Pi / ppr * directionSign();
            }
 
 
            double getRawAngle() const
            {
                return angle * 2 * Udon::Pi / ppr * directionSign();
            }
 
 
            int16_t getVelocity() const
            {
                const auto velocity = (nodeRx->data[2] << 8 | nodeRx->data[3]) * directionSign();
                return static_cast<int16_t>(velocity);
            }
 
 
            int16_t getTorqueCurrent() const
            {
                const int current = (nodeRx->data[4] << 8 | nodeRx->data[5]) * directionSign();
                return static_cast<int16_t>(current);
            }
 
 
            uint8_t getTemperature() const
            {
                return nodeRx->data[6];
            }
 
        protected:
            void setCurrent(int16_t current)
            {
                current = current * directionSign();
 
                const auto transmitCurrent = static_cast<int16_t>(current * 16384 / 20000);    // 16bit符号なし整数に変換
 
                if (motorId > 4)
                {
                    const auto motorIndex = (motorId - 4 - 1) * 2;
                    nodeTx->data[motorIndex + 0] = transmitCurrent >> 8 & 0xff;    // high byte
                    nodeTx->data[motorIndex + 1] = transmitCurrent >> 0 & 0xff;    // low byte
                }
                else
                {
                    const auto motorIndex = (motorId - 1) * 2;
                    nodeTx->data[motorIndex + 0] = transmitCurrent >> 8 & 0xff;    // high byte
                    nodeTx->data[motorIndex + 1] = transmitCurrent >> 0 & 0xff;    // low byte
                }
            }
 
        private:
            Udon::ICanBus& bus;
 
            uint8_t motorId;
 
            bool direction;
 
            Udon::CanTxNode* nodeTx;
 
 
            Udon::CanRxNode* nodeRx;
 
 
            int64_t offsetAngle = 0;
            int64_t angle = 0;
 
 
            static constexpr int32_t ppr = 8192;
 
            int16_t directionSign() const
            {
                return direction ? 1 : -1;
            }
 
            static void onReceive(void* p)
            {
                auto self = static_cast<RoboMasterBase*>(p);
 
                // 変化角を計算
                const auto currentAngle = self->nodeRx->data[0] << 8 | self->nodeRx->data[1];
                const auto dTheta = self->angle - currentAngle;
                self->angle = currentAngle;
 
                // 変化量がいきなり半周を超えた -> 計算値が-π~π間を通過 -> 一周分オフセットを加減算
                if (dTheta > ppr / 2)
                {
                    self->offsetAngle += ppr;
                }
                else if (dTheta < -ppr / 2)
                {
                    self->offsetAngle -= ppr;
                }
            }
        };
 
    }    // namespace Impl
 
 
    class RoboMasterC610
        : public Impl::RoboMasterBase
    {
    public:
        using RoboMasterBase::RoboMasterBase;
 
        static constexpr int16_t CurrentMin = -10000;
 
        static constexpr int16_t CurrentMax = 10000;
 
        void setCurrent(int16_t current)
        {
            RoboMasterBase::setCurrent(Constrain(current, CurrentMin, CurrentMax));
        }
    };
 
 
    class RoboMasterC620
        : public Impl::RoboMasterBase
    {
    public:
        using RoboMasterBase::RoboMasterBase;
 
        static constexpr int16_t CurrentMin = -20000;
 
        static constexpr int16_t CurrentMax = 20000;
 
        void setCurrent(int16_t current)
        {
            RoboMasterBase::setCurrent(Constrain(current, CurrentMin, CurrentMax));
        }
    };
}    // namespace Udon