移動平均の足回りを実装 #5

include/ps4operation/LowerBody_MovAve.hpp

@@ -0,0 +1,93 @@
+/**
+ * @file LowerBody_MovAve.hpp
+ * @author H1rono ([email protected])
+ * @brief 移動平均でモーター値を更新する足回り
+ * @version 0.1
+ * @copyright Copyright (c) 2022 ssr2022-saka-maza
+ */
+
+#pragma once
+
+#ifndef PS4_OPERATION_LOWER_BODY_MOV_AVE_HPP
+
+/**
+ * @brief ps4operation/LowerBody_MovAve.hppがインクルードされていることを示すdefine
+ */
+#define PS4_OPERATION_LOWER_BODY_MOV_AVE_HPP
+
+#include <Arduino.h>
+#include <ssr/MovingAverage.hpp>
+#include <ssr/LowerBody.hpp>
+#include <ssr/PS4Operation.hpp>
+
+/**
+ * @brief ssr::PS4Operationを継承したクラス群
+ */
+namespace ps4operation {
+    /**
+     * @brief 足回りを操作するPS4Operation
+     */
+    class LowerBody_MovAve : public ssr::PS4Operation {
+    private:
+        /**
+         * @brief 扱う足回り
+         */
+        ssr::LowerBody _lowerBody;
+
+        /**
+         * @brief x軸方向の移動平均
+         */
+        ssr::MovingAverage<float> _movAve_x;
+
+        /**
+         * @brief y軸方向の移動平均
+         */
+        ssr::MovingAverage<float> _movAve_y;
+
+        /**
+         * @brief 回転方向の移動平均
+         */
+        ssr::MovingAverage<float> _movAve_r;
+
+        /**
+         * @brief ルート２
+         */
+        static const float _sqrt2;
+
+        /**
+         * @brief センサー値からパワーの出力に適した値に変換する
+         * @param v センサー値 0~255
+         * @return constexpr int16_t 適した出力パワー
+         */
+        static float _mapPower(uint8_t v);
+
+    public:
+        /**
+         * @brief Construct a new Lower Body object
+         * @param dir1 モーター1のDIRピン
+         * @param pwm1 モーター1のPWMピン
+         * @param dir2 モーター2のDIRピン
+         * @param pwm2 モーター2のPWMピン
+         * @param dir3 モーター3のDIRピン
+         * @param pwm3 モーター3のPWMピン
+         */
+        LowerBody_MovAve(
+            ssr::PinType dir1, ssr::PinType pwm1,
+            ssr::PinType dir2, ssr::PinType pwm2,
+            ssr::PinType dir3, ssr::PinType pwm3
+        );
+
+        /**
+         * @brief 初期化処理 全体のsetup()内で呼び出すこと
+         */
+        void begin();
+
+        /**
+         * @brief 足回りを操作する
+         * @param value コントローラーのセンサー値
+         */
+        virtual void operate(const ssr::PS4Value & value) override;
+    }; // class LowerBody_MovAve
+} // namespace ps4operation
+
+#endif /* PS4_OPERATION_LOWER_BODY_MOV_AVE_HPP */

src/main.cpp

@@ -3,7 +3,8 @@
 #include <ssr/PS4Controller.hpp>
 #include <ps4operation/Log.hpp>
 #include <ps4operation/Solenoid.hpp>
-#include <ps4operation/LowerBody.hpp>
+// #include <ps4operation/LowerBody.hpp>
+#include <ps4operation/LowerBody_MovAve.hpp>
 #include <ps4operation/ForkLift.hpp>
 #include <ps4operation/Hand.hpp>
 #include <ps4operation/Reloader.hpp>
@@ -31,7 +32,8 @@ void setup() {
     solenoid->begin();
     ps4Controller.addOperation(solenoid);
     /* lower body */
-    ps4operation::LowerBody * lowerBody = new ps4operation::LowerBody(6, 7, 4, 5, 2, 3);
+    // ps4operation::LowerBody * lowerBody = new ps4operation::LowerBody(6, 7, 4, 5, 2, 3);
+    ps4operation::LowerBody_MovAve * lowerBody = new ps4operation::LowerBody_MovAve(6, 7, 4, 5, 2, 3);
     lowerBody->begin();
     ps4Controller.addOperation(lowerBody);
     /* fork lift */

src/ps4operation/LowerBody_MovAve.cpp

@@ -0,0 +1,49 @@
+#include "ps4operation/LowerBody_MovAve.hpp"
+
+const float ps4operation::LowerBody_MovAve::_sqrt2 = sqrtf(2.0f);
+
+float ps4operation::LowerBody_MovAve::_mapPower(uint8_t v) {
+    int16_t trans = v - 128;
+    return abs(trans) <= 8 ? 0 : static_cast<float>(trans) * _sqrt2;
+}
+
+ps4operation::LowerBody_MovAve::LowerBody_MovAve(
+    ssr::PinType dir1, ssr::PinType pwm1,
+    ssr::PinType dir2, ssr::PinType pwm2,
+    ssr::PinType dir3, ssr::PinType pwm3
+) : PS4Operation(), _lowerBody(dir1, pwm1, dir2, pwm2, dir3, pwm3), _movAve_x(16), _movAve_y(16), _movAve_r(16) {}
+
+void ps4operation::LowerBody_MovAve::begin() {
+    _lowerBody.begin();
+}
+
+void ps4operation::LowerBody_MovAve::operate(const ssr::PS4Value & value) {
+    _movAve_x.write(_mapPower(value.lstick.y));
+    _movAve_y.write(_mapPower(value.lstick.x));
+    _movAve_r.write(-_mapPower(value.rstick.x));
+    float x = _movAve_x.read();
+    float y = _movAve_y.read();
+    float r = _movAve_r.read();
+    #ifdef ps4operation_verbose
+    char buffer[256] = "";
+    char * ptr = buffer;
+    ptr += snprintf_P(ptr, 200, PSTR("[ps4operation::LowerBody] twist "));
+    dtostrf(x, 6, 2, ptr); ptr += 6;
+    ptr[0] = ','; ptr[1] = ' '; ptr += 2;
+    dtostrf(y, 6, 2, ptr); ptr += 6;
+    ptr[0] = ','; ptr[1] = ' '; ptr += 2;
+    dtostrf(r, 6, 2, ptr); ptr += 6;
+    Serial.println(buffer);
+    #ifndef SSR_VERBOSE
+    #define ndef_SSR_VERBOSE
+    #define SSR_VERBOSE
+    #endif /* SSR_VERBOSE */
+    #endif /* ps4operation_verbose */
+    _lowerBody.twist(x, y, r);
+    #ifdef ps4operation_verbose
+    #ifdef ndef_SSR_VERBOSE
+    #undef SSR_VERBOSE
+    #undef ndef_SSR_VERBOSE
+    #endif /* ndef_SSR_VERBOSE */
+    #endif /* ps4operation_verbose */
+}