From 0de07bee1435283c69b9e88f3906316ec03f769e Mon Sep 17 00:00:00 2001 From: Okawa Yusuke Date: Tue, 24 Dec 2024 03:22:06 +0900 Subject: [PATCH] =?UTF-8?q?Udon::Direction=20=E5=B0=8E=E5=85=A5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Udon/Com/Driver/Encoder.hpp | 8 ++--- src/Udon/Com/Driver/Motor.hpp | 7 +++-- src/Udon/Driver/EncoderPico.hpp | 9 ++++-- src/Udon/Driver/Motor.hpp | 29 ++++++++++++------- src/Udon/Driver/RoboMasterMotor.hpp | 20 +++++-------- src/Udon/Types/Direction.hpp | 23 +++++++++++++++ .../src/Driver/RoboMasterMotor.cpp | 4 +-- 7 files changed, 66 insertions(+), 34 deletions(-) create mode 100644 src/Udon/Types/Direction.hpp diff --git a/src/Udon/Com/Driver/Encoder.hpp b/src/Udon/Com/Driver/Encoder.hpp index 92867d982..42257e650 100644 --- a/src/Udon/Com/Driver/Encoder.hpp +++ b/src/Udon/Com/Driver/Encoder.hpp @@ -12,9 +12,9 @@ #include #include #include - #include #include +#include namespace Udon { @@ -32,7 +32,7 @@ namespace Udon Udon::DeltaTime deltaTime; - bool direction; + Udon::Direction direction; int32_t count{}; int32_t deltaCount{}; @@ -43,7 +43,7 @@ namespace Udon /// @brief コンストラクタ /// @param reader 受信クラスオブジェクト /// @param direction 回転方向 - EncoderBy(ReaderType&& reader, bool direction) + EncoderBy(ReaderType&& reader, Udon::Direction direction = Udon::Direction::Forward) : reader(std::move(reader)) , deltaTime() , direction(direction) @@ -59,7 +59,7 @@ namespace Udon if (const auto countOpt = reader.getMessage()) { - count = countOpt->count * (direction ? 1 : -1); + count = countOpt->count * Udon::DirectionToSign(direction); } const auto curr = getCount(); diff --git a/src/Udon/Com/Driver/Motor.hpp b/src/Udon/Com/Driver/Motor.hpp index 7ade77b78..6dc2ff81c 100644 --- a/src/Udon/Com/Driver/Motor.hpp +++ b/src/Udon/Com/Driver/Motor.hpp @@ -13,6 +13,7 @@ #include #include #include +#include namespace Udon { @@ -28,14 +29,14 @@ namespace Udon int16_t power; // 出力値 -255 ~ 255 - bool direction; // 回転方向 true: forward, false: backward + Udon::Direction direction; // 回転方向 public: /// @brief コンストラクタ /// @param writer 送信クラスオブジェクト /// @param direction 回転方向 - MotorBy(WriterType&& writer, bool direction) + MotorBy(WriterType&& writer, Udon::Direction direction = Udon::Direction::Forward) : writer(std::move(writer)) , power() , direction(direction) @@ -47,7 +48,7 @@ namespace Udon void move(int16_t p) { power = Constrain(p, (int16_t)-255, (int16_t)255); - writer.setMessage({ static_cast(power * (direction ? 1 : -1)) }); + writer.setMessage({ static_cast(power * Udon::DirectionToSign(direction)) }); Udon::Traits::MaybeInvokeUpdate(writer); } diff --git a/src/Udon/Driver/EncoderPico.hpp b/src/Udon/Driver/EncoderPico.hpp index 6382efb4e..d3fe73a46 100644 --- a/src/Udon/Driver/EncoderPico.hpp +++ b/src/Udon/Driver/EncoderPico.hpp @@ -12,6 +12,7 @@ # include "Pio/QuadratureEncoder.pio.hpp" # include +# include namespace Udon { @@ -22,15 +23,19 @@ namespace Udon uint8_t pinA; uint8_t pinB; + Udon::Direction direction; + Pio::StateMachine stateMachine; // PIO使用時 public: /// @brief コンストラクタ /// @param pinA エンコーダーのA相ピン /// @param pinB エンコーダーのB相ピン - EncoderPico(uint8_t pinA, uint8_t pinB) + /// @param direction 回転方向 + EncoderPico(uint8_t pinA, uint8_t pinB, Udon::Direction direction = Udon::Direction::Forward) : pinA(pinA) , pinB(pinB) + , direction(direction) , stateMachine() { } @@ -68,7 +73,7 @@ namespace Udon /// @return エンコーダーの値 int32_t read() const { - return Pio::Encoder::quadrature_encoder_get_count(stateMachine.pio, stateMachine.index); + return Pio::Encoder::quadrature_encoder_get_count(stateMachine.pio, stateMachine.index) * Udon::DirectionToSign(direction); } /// @brief エンコーダーの値を表示する diff --git a/src/Udon/Driver/Motor.hpp b/src/Udon/Driver/Motor.hpp index 8ae3c9bb6..25fce9c76 100644 --- a/src/Udon/Driver/Motor.hpp +++ b/src/Udon/Driver/Motor.hpp @@ -10,6 +10,7 @@ # include # include +# include namespace Udon { @@ -22,17 +23,20 @@ namespace Udon const uint8_t pinA; const uint8_t pinP; - Udon::MovingAverage movingAverage; + Udon::Direction direction; + + Udon::MovingAverage ma; public: /// @brief コンストラクタ /// @param pinA 方向ピン /// @param pinP PWM ピン - SmoothlyMotor2(const uint8_t pinA, const uint8_t pinP) + SmoothlyMotor2(const uint8_t pinA, const uint8_t pinP, Udon::Direction direction = Udon::Direction::Forward) : pinA(pinA) , pinP(pinP) - , movingAverage{} + , direction(direction) + , ma{} { } @@ -46,7 +50,7 @@ namespace Udon /// @param power パワー (-250 ～ 250) void move(const int16_t power) { - const int16_t p = movingAverage(constrain(power, -250, 250)); + const int16_t p = ma(constrain(power, -250, 250)) * Udon::DirectionToSign(direction); digitalWrite(pinA, p >= 0 ? HIGH : LOW); analogWrite(pinP, abs(p)); } @@ -60,7 +64,7 @@ namespace Udon /// @brief パワーをシリアル出力 void show() const { - Serial.print(movingAverage.getValue()); + Serial.print(ma.getValue()); Serial.print('\t'); } }; @@ -74,7 +78,9 @@ namespace Udon const uint8_t pinB; const uint8_t pinP; - Udon::MovingAverage movingAverage; + Udon::Direction direction; + + Udon::MovingAverage ma; public: @@ -82,11 +88,13 @@ namespace Udon /// @param pinA 方向ピン /// @param pinB 方向ピン /// @param pinP PWM ピン - SmoothlyMotor3(const uint8_t pinA, const uint8_t pinB, const uint8_t pinP) + /// @param direction 回転方向 + 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) - , movingAverage{} + , direction(direction) + , ma{} { } @@ -101,8 +109,7 @@ namespace Udon /// @param power パワー (-250 ～ 250) void move(const int16_t power) { - movingAverage.update(constrain(power, -250, 250)); - const int16_t p = movingAverage.getValue(); + 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)); @@ -117,7 +124,7 @@ namespace Udon /// @brief パワーをシリアル出力 void show() const { - Serial.print(movingAverage.getValue()); + Serial.print(ma.getValue()); Serial.print('\t'); } }; diff --git a/src/Udon/Driver/RoboMasterMotor.hpp b/src/Udon/Driver/RoboMasterMotor.hpp index 2e950a0ec..fd5e3f0a0 100644 --- a/src/Udon/Driver/RoboMasterMotor.hpp +++ b/src/Udon/Driver/RoboMasterMotor.hpp @@ -8,6 +8,7 @@ #include #include +#include namespace Udon { @@ -24,7 +25,7 @@ namespace Udon /// @param bus CAN通信バス /// @param motorId モーターID (1~8) /// @param direction 回転方向 - RoboMasterBase(Udon::ICanBus& bus, uint8_t motorId, bool direction = true) + RoboMasterBase(Udon::ICanBus& bus, uint8_t motorId, Udon::Direction direction = Udon::Direction::Forward) : bus{ bus } , motorId{ motorId } , direction{ direction } @@ -62,7 +63,7 @@ namespace Udon /// @return 角度 [rad] double getAngle() const { - return (angle + offsetAngle) * 2 * Udon::Pi / ppr * directionSign(); + return (angle + offsetAngle) * 2 * Udon::Pi / ppr * Udon::DirectionToSign(direction); } @@ -71,7 +72,7 @@ namespace Udon /// @return 角度 [rad] double getRawAngle() const { - return angle * 2 * Udon::Pi / ppr * directionSign(); + return angle * 2 * Udon::Pi / ppr * Udon::DirectionToSign(direction); } @@ -79,7 +80,7 @@ namespace Udon /// @return 速度 [rpm] int16_t getVelocity() const { - const auto velocity = (nodeRx->data[2] << 8 | nodeRx->data[3]) * directionSign(); + const auto velocity = (nodeRx->data[2] << 8 | nodeRx->data[3]) * Udon::DirectionToSign(direction); return static_cast(velocity); } @@ -88,7 +89,7 @@ namespace Udon /// @return トルク電流 [mA] int16_t getTorqueCurrent() const { - const int current = (nodeRx->data[4] << 8 | nodeRx->data[5]) * directionSign(); + const int current = (nodeRx->data[4] << 8 | nodeRx->data[5]) * Udon::DirectionToSign(direction); return static_cast(current); } @@ -105,7 +106,7 @@ namespace Udon /// @param current 電流値 void setCurrent(int16_t current) { - current = current * directionSign(); + current = current * Udon::DirectionToSign(direction); const auto transmitCurrent = static_cast(current * 16384 / 20000); @@ -131,7 +132,7 @@ namespace Udon uint8_t motorId; /// @brief 回転方向 - bool direction; + Udon::Direction direction; /// @brief 送信バッファ Udon::CanTxNode* nodeTx; @@ -149,11 +150,6 @@ namespace Udon /// @brief エンコーダー分解能 static constexpr int32_t ppr = 8192; - int16_t directionSign() const - { - return direction ? 1 : -1; - } - static void onReceive(void* p) { auto self = static_cast(p); diff --git a/src/Udon/Types/Direction.hpp b/src/Udon/Types/Direction.hpp new file mode 100644 index 000000000..6c3103914 --- /dev/null +++ b/src/Udon/Types/Direction.hpp @@ -0,0 +1,23 @@ +#pragma once + +namespace Udon +{ + + /// @brief 方向 + /// @note 回転方向を表す + enum class Direction + { + Forward, + Backward, + }; + + inline int DirectionToSign(Direction direction) + { + switch (direction) + { + case Direction::Forward: return 1; + case Direction::Backward: return -1; + } + } + +} // namespace Udon diff --git a/test/ArduinoLint/src/Driver/RoboMasterMotor.cpp b/test/ArduinoLint/src/Driver/RoboMasterMotor.cpp index 556d7d3ab..05ec13801 100644 --- a/test/ArduinoLint/src/Driver/RoboMasterMotor.cpp +++ b/test/ArduinoLint/src/Driver/RoboMasterMotor.cpp @@ -20,7 +20,7 @@ __attribute__((unused)) static void test() { DummyBus bus; { - Udon::RoboMasterC610 motor{ bus, 0x000, true }; + Udon::RoboMasterC610 motor{ bus, 0x000, Udon::Direction::Forward }; motor.setCurrent(0); motor.getAngle(); motor.getVelocity(); @@ -29,7 +29,7 @@ __attribute__((unused)) static void test() } { - Udon::RoboMasterC620 motor{ bus, 0x000, true }; + Udon::RoboMasterC620 motor{ bus, 0x000, Udon::Direction::Backward }; motor.setCurrent(0); motor.getAngle(); motor.getVelocity();