LowerBodyの定義を修正 #8

- noexceptキーワードを追加
- メンバ変数を追加して計算を高効率化

include/ssr/LowerBody.hpp

@@ -16,6 +16,8 @@
  */
 #define SSR_LOWER_BODY_HPP
 
+#include <math.h>
+#include <ssr/Vector2D.hpp>
 #include <ssr/MotorDriver.hpp>
 
 /**
@@ -26,20 +28,38 @@ namespace ssr {
      * @brief 足回りを操作
      */
     class LowerBody {
-    public:
+    private:
+        /**
+         * @brief モーター1の方向
+         */
+        const Vector2D<float> _dir1;
+
+        /**
+         * @brief モーター2の方向
+         */
+        const Vector2D<float> _dir2;
+
+        /**
+         * @brief モーター3の方向
+         */
+        const Vector2D<float> _dir3;
+
         /**
          * @brief 正面のモーター
          */
-        MotorDriver motor1;
+        MotorDriver _motor1;
+
         /**
          * @brief 左後ろのモーター
          */
-        MotorDriver motor2;
+        MotorDriver _motor2;
+
         /**
          * @brief 右後ろのモーター
          */
-        MotorDriver motor3;
+        MotorDriver _motor3;
 
+    public:
         // デフォルトコンストラクタを禁止
         LowerBody() = delete;
         // コピーコンストラクタを禁止
@@ -99,7 +119,7 @@ namespace ssr {
          * @param v_y float 初期並行速度のy成分。デフォルトは0
          * @param v_theta float 中心からタイヤまでの距離*角速度 の初期値。デフォルトは0
          */
-        void begin(float v_x = 0, float v_y = 0, float v_theta = 0);
+        void begin(float v_x = 0, float v_y = 0, float v_theta = 0) noexcept;
 
     private:
         /**
@@ -108,15 +128,15 @@ namespace ssr {
          * @param v2 float モーター2の速度
          * @param v3 float モーター3の速度
          */
-        void _setPowers_raw(float v1, float v2, float v3);
+        void _setPowers_raw(float v1, float v2, float v3) noexcept;
 
         /**
          * @brief モーターの速度を正規化して設定する
          * @param v1 float モーター1の速度
          * @param v2 float モーター2の速度
          * @param v3 float モーター3の速度
          */
-        void _setPowers_normalized(float v1, float v2, float v3);
+        void _setPowers_normalized(float v1, float v2, float v3) noexcept;
 
     public:
 
@@ -126,28 +146,28 @@ namespace ssr {
          * @param v2 float モーター2の速度
          * @param v3 float モーター3の速度
          */
-        void setPowers(float v1, float v2, float v3);
+        void setPowers(float v1, float v2, float v3) noexcept;
 
         /**
          * @brief 並行移動と回転を同時に設定する
          * @param v_x float 並行速度のx成分
          * @param v_y float 並行速度のy成分
          * @param v_theta float 中心からタイヤまでの距離 * 角速度
          */
-        void twist(float v_x, float v_y, float v_theta);
+        void twist(float v_x, float v_y, float v_theta) noexcept;
 
         /**
          * @brief 回転なしの並行移動を設定する
          * @param v_x float 並行速度のx成分
          * @param v_y float 並行速度のy成分
          */
-        void parallel(float v_x, float v_y);
+        inline void parallel(float v_x, float v_y) noexcept;
 
         /**
          * @brief 並行移動なしで回転を設定する
          * @param v_theta float 中心からタイヤまでの距離 * 角速度
          */
-        void rotate(float v_theta);
+        inline void rotate(float v_theta) noexcept;
     }; // class LowerBody
 } // namespace ssr
 

src/ssr/LowerBody.cpp

@@ -6,22 +6,23 @@ ssr::LowerBody::LowerBody(
     ssr::PinType dir1, ssr::PinType pwm1,
     ssr::PinType dir2, ssr::PinType pwm2,
     ssr::PinType dir3, ssr::PinType pwm3
-) : motor1(dir1, pwm1), motor2(dir2, pwm2), motor3(dir3, pwm3) {}
+) : _dir1(cos(PI / 2), sin(PI / 2)), _dir2(cos(PI * 7 / 6), sin(PI * 7 / 6)), _dir3(cos(PI * 11 / 6), sin(PI * 11 / 6)),
+    _motor1(dir1, pwm1), _motor2(dir2, pwm2), _motor3(dir3, pwm3) {}
 
 void ssr::LowerBody::begin(float v_x, float v_y, float v_theta) {
-    motor1.begin();
-    motor2.begin();
-    motor3.begin();
+    _motor1.begin();
+    _motor2.begin();
+    _motor3.begin();
     twist(v_x, v_y, v_theta);
 }
 
 void ssr::LowerBody::_setPowers_raw(float v1, float v2, float v3) {
     int16_t p1 = static_cast<int16_t>(v1);
     int16_t p2 = static_cast<int16_t>(v2);
     int16_t p3 = static_cast<int16_t>(v3);
-    motor1.setPower(p1);
-    motor2.setPower(p2);
-    motor3.setPower(p3);
+    _motor1.setPower(p1);
+    _motor2.setPower(p2);
+    _motor3.setPower(p3);
     #ifdef SSR_VERBOSE
     char buffer[256] = "";
     snprintf_P(buffer, 255, PSTR("[ssr::LowerBody] set motor powers as %4d, %4d, %4d\n"), p1, p2, p3);
@@ -45,9 +46,9 @@ void ssr::LowerBody::setPowers(float v1, float v2, float v3) {
 }
 
 void ssr::LowerBody::twist(float v_x, float v_y, float v_theta) {
-    float v1 = cos(PI * 1 / 2) * v_x + sin(PI * 1 / 2) * v_y + v_theta;
-    float v2 = cos(PI * 7 / 6) * v_x + sin(PI * 7 / 6) * v_y + v_theta;
-    float v3 = cos(PI * 11 / 6) * v_x + sin(PI * 11 / 6) * v_y + v_theta;
+    float v1 = _dir1.dot(v_x, v_y) + v_theta;
+    float v2 = _dir2.dot(v_x, v_y) + v_theta;
+    float v3 = _dir3.dot(v_x, v_y) + v_theta;
     setPowers(v1, v2, v3);
 }