From a4fad3989421520e023d72e08ebba082b385016a Mon Sep 17 00:00:00 2001
From: H1rono <hronok66@gmail.com>
Date: Thu, 16 May 2024 16:26:28 +0900
Subject: [PATCH 01/13] :recycle: Extract `schneider_PI` to `utils::PI`

---
 include/utils.hpp       | 12 ++++++++++++
 src/schneider_model.cpp | 34 +++++++++++++++-------------------
 2 files changed, 27 insertions(+), 19 deletions(-)
 create mode 100644 include/utils.hpp

diff --git a/include/utils.hpp b/include/utils.hpp
new file mode 100644
index 0000000..f65e5a8
--- /dev/null
+++ b/include/utils.hpp
@@ -0,0 +1,12 @@
+#ifndef OMNIBOAT_ROBOKIT_UTILS_HPP
+#define OMNIBOAT_ROBOKIT_UTILS_HPP
+
+/// ユーティリティ関数・定数
+namespace utils {
+
+/// 円周率π
+constexpr float PI = 3.1415927F;
+
+}  // namespace utils
+
+#endif  // OMNIBOAT_ROBOKIT_UTILS_HPP
diff --git a/src/schneider_model.cpp b/src/schneider_model.cpp
index 002dbac..12be31f 100644
--- a/src/schneider_model.cpp
+++ b/src/schneider_model.cpp
@@ -8,15 +8,13 @@
 
 #include "schneider_model.hpp"
 #include "trace.hpp"
+#include "utils.hpp"
 
 namespace omniboat {
 
 using trace::LedId;
+using utils::PI;
 
-/**
- * @brief π
- */
-constexpr float schneider_PI = 3.1415927F;
 /**
  * @brief z軸周りの慣性モーメント
  * @note もっと正確な値の方がいいかも
@@ -161,7 +159,7 @@ auto Schneider::cal_q(const std::array<float, 3>& joy) -> void {
                        : (joy[0] < 0 && joy[1] >= 0) ? 3
                                                      : 5;
     for (int i = 2; i < 4; ++i) {
-        this->inputs[i] = coef * schneider_PI / 4;
+        this->inputs[i] = coef * PI / 4;
     }
 
     trace::toggle(LedId::Second);
@@ -220,27 +218,25 @@ void Schneider::set_q(const std::array<float, 3>& gyro) {
     this->fet_1 = this->inputs[0];
     this->fet_2 = this->inputs[1];
 
-    while (this->inputs[2] >= schneider_PI) {
-        this->inputs[2] -= 2 * schneider_PI;
+    while (this->inputs[2] >= PI) {
+        this->inputs[2] -= 2 * PI;
     }
-    while (this->inputs[3] >= schneider_PI) {
-        this->inputs[3] -= 2 * schneider_PI;
+    while (this->inputs[3] >= PI) {
+        this->inputs[3] -= 2 * PI;
     }
-    while (this->inputs[2] < -schneider_PI) {
-        this->inputs[2] += 2 * schneider_PI;
+    while (this->inputs[2] < -PI) {
+        this->inputs[2] += 2 * PI;
     }
-    while (this->inputs[3] < -schneider_PI) {
-        this->inputs[3] += 2 * schneider_PI;
+    while (this->inputs[3] < -PI) {
+        this->inputs[3] += 2 * PI;
     }
 
-    if (0 < this->inputs[2] && this->inputs[2] < schneider_PI) {
-        const int width
-            = static_cast<int>(500 + 1900 / schneider_PI * this->inputs[2] - 2200 * gyro[2]);
+    if (0 < this->inputs[2] && this->inputs[2] < PI) {
+        const int width = static_cast<int>(500 + 1900 / PI * this->inputs[2] - 2200 * gyro[2]);
         this->servo_1.pulsewidth_us(width);
     }
-    if (0 < this->inputs[3] && this->inputs[3] < schneider_PI) {
-        const int width
-            = static_cast<int>(500 + 1900 / schneider_PI * this->inputs[3] + 2200 * gyro[2]);
+    if (0 < this->inputs[3] && this->inputs[3] < PI) {
+        const int width = static_cast<int>(500 + 1900 / PI * this->inputs[3] + 2200 * gyro[2]);
         this->servo_2.pulsewidth_us(width);
     }
 }

From 66f82cf52ae1f274b26dd65ccc13a611bb75c04e Mon Sep 17 00:00:00 2001
From: H1rono <hronok66@gmail.com>
Date: Thu, 16 May 2024 18:26:34 +0900
Subject: [PATCH 02/13] :sparkles: Add `packet::OutputValues`

---
 include/packet/output.hpp | 47 +++++++++++++++++++++++++++++++++++++++
 src/packet/output.cpp     | 23 +++++++++++++++++++
 2 files changed, 70 insertions(+)
 create mode 100644 include/packet/output.hpp
 create mode 100644 src/packet/output.cpp

diff --git a/include/packet/output.hpp b/include/packet/output.hpp
new file mode 100644
index 0000000..2aa7181
--- /dev/null
+++ b/include/packet/output.hpp
@@ -0,0 +1,47 @@
+#ifndef OMNIBOAT_ROBOKIT_PACKET_OUTPUT_HPP
+#define OMNIBOAT_ROBOKIT_PACKET_OUTPUT_HPP
+
+#include <array>
+#include <utility>
+
+namespace packet {
+
+struct OutputValues {
+    /**
+     * @brief サーボモーターの角度 (0~pi)
+     * @note piの値はutils::PI
+     */
+    std::pair<float, float> servo = {0, 0};
+
+    /**
+     * @brief DCモーターの出力 (0~1)
+     * @note first, secondはservoのfirst, secondと対応させること
+     * @details 0で停止, 1で最大出力
+     */
+    std::pair<float, float> dc_motor = {0, 0};
+
+    /**
+     * @brief Construct a new Output Values object
+     * @param servo サーボモーターの角度
+     * @param dc_motor DCモーターの出力
+     */
+    OutputValues(const std::pair<float, float> &servo, const std::pair<float, float> &dc_motor);
+
+    OutputValues() = default;
+    ~OutputValues() = default;
+    OutputValues(const OutputValues &) = default;
+    auto operator=(const OutputValues &) -> OutputValues & = default;
+    OutputValues(OutputValues &&) = default;
+    auto operator=(OutputValues &&) -> OutputValues & = default;
+
+    /**
+     * @brief 値が適切かどうかを判定する
+     * @return true 適切
+     * @return false 不適切; servo, dc_motorのいずれかが指定された範囲にない
+     */
+    auto is_valid() const -> bool;
+};
+
+}  // namespace packet
+
+#endif  // OMNIBOAT_ROBOKIT_PACKET_OUTPUT_HPP
diff --git a/src/packet/output.cpp b/src/packet/output.cpp
new file mode 100644
index 0000000..da29861
--- /dev/null
+++ b/src/packet/output.cpp
@@ -0,0 +1,23 @@
+#include "packet/output.hpp"
+
+#include "utils.hpp"
+
+using float_pair = std::pair<float, float>;
+
+packet::OutputValues::OutputValues(const float_pair &servo, const float_pair &dc_motor) :
+    servo(servo), dc_motor(dc_motor) {}
+
+constexpr auto pi_range(float value) -> bool {
+    return 0 <= value && value <= utils::PI;
+}
+
+constexpr auto dc_range(float value) -> bool {
+    constexpr float DC_MAX = 1.0F;
+    return 0 <= value && value <= DC_MAX;
+}
+
+auto packet::OutputValues::is_valid() const -> bool {
+    const bool servo_valid = pi_range(this->servo.first) && pi_range(this->servo.second);
+    const bool dc_valid = dc_range(this->dc_motor.first) && dc_range(this->dc_motor.second);
+    return servo_valid && dc_valid;
+}

From ef43829bcba9221e4ead288c470406440a218a95 Mon Sep 17 00:00:00 2001
From: H1rono <hronok66@gmail.com>
Date: Fri, 17 May 2024 09:49:16 +0900
Subject: [PATCH 03/13] :recycle: Use `packet::OutputValues`

---
 include/schneider_model.hpp | 18 ++++++++-
 src/schneider_model.cpp     | 78 +++++++++++++++++++++++++------------
 2 files changed, 69 insertions(+), 27 deletions(-)

diff --git a/include/schneider_model.hpp b/include/schneider_model.hpp
index b563012..9022529 100644
--- a/include/schneider_model.hpp
+++ b/include/schneider_model.hpp
@@ -6,6 +6,7 @@
 #include "mbed.h"
 
 #include "MPU6050.h"
+#include "packet/output.hpp"
 
 namespace omniboat {
 
@@ -54,6 +55,11 @@ class Schneider {
      */
     std::array<std::array<float, 3>, 4> cal_tjacob() const;
 
+    /**
+     * @brief 最後にアクチュエーターに反映させた値
+     */
+    packet::OutputValues last_output;
+
     /**
      * @brief 状態方程式の計算を行う関数
      */
@@ -68,7 +74,7 @@ class Schneider {
     /**
      * @brief モータへの信号値に変換する関数
      */
-    void set_q(const std::array<float, 3>& gyro);
+    auto set_q(const std::array<float, 3>& gyro) -> packet::OutputValues;
 
     /**
      * @brief ジョイコンの値を読み取り、目標値を算出して配列として返す
@@ -80,13 +86,21 @@ class Schneider {
     /**
      * @brief つまみの値をから機体を回転させる関数
      */
-    void rotate(const float& volume_value);
+    auto rotate(const float& volume_value) const -> packet::OutputValues;
+
+    /**
+     * @brief FETへの出力(=DCモーター)を止める関数
+     * @return packet::OutputValues write_outputに渡す値
+     */
+    auto stop_fet() const -> packet::OutputValues;
 
     /**
      * @brief ジャイロセンサの値を読み取る
      */
     auto read_gyro() -> std::array<float, 3>;
 
+    auto write_output(const packet::OutputValues& output) -> void;
+
     AnalogIn adcIn1;  // ジョイスティック
     AnalogIn adcIn2;  // ジョイスティック
     AnalogIn volume;
diff --git a/src/schneider_model.cpp b/src/schneider_model.cpp
index 12be31f..54b2223 100644
--- a/src/schneider_model.cpp
+++ b/src/schneider_model.cpp
@@ -3,6 +3,7 @@
 #include <cmath>
 #include <cstdio>
 #include <thread>
+#include <utility>
 
 #include "rtos.h"
 
@@ -25,9 +26,15 @@ constexpr float inertia_z = 1;
  */
 constexpr float step_width_a = 0.1;
 
+// サーボモータ出力値(pulse width)
+// FIXME: もともと550, 2350だったので要検証
+constexpr int minor_pulsewidth_us = 500;
+constexpr int major_pulsewidth_us = 2400;
+
 Schneider::Schneider() :
     inputs(),
     outputs(),
+    last_output({0, 0}, {0, 0}),
     adcIn1(A4),
     adcIn2(A5),
     volume(A6),
@@ -92,15 +99,16 @@ void Schneider::one_step() {
     const bool joyEffective = abs(joy[0]) > joy_min && abs(joy[1]) > joy_min;
     const bool volumeEffective = volume_value < volume_under || volume_over < volume_value;
 
+    packet::OutputValues output;
     if (joyEffective) {
         this->cal_q(joy);
-        this->set_q(gyro);
+        output = this->set_q(gyro);
     } else if (volumeEffective) {
-        this->rotate(volume_value);
+        output = this->rotate(volume_value);
     } else {
-        this->fet_1 = 0;
-        this->fet_2 = 0;
+        output = this->stop_fet();
     }
+    this->write_output(output);
     this->debug();
     trace::toggle(LedId::Third);
 }
@@ -204,8 +212,9 @@ inline void Schneider::state_equation() {
           / inertia_z;
 }
 
-void Schneider::set_q(const std::array<float, 3>& gyro) {
+auto Schneider::set_q(const std::array<float, 3>& gyro) -> packet::OutputValues {
     using std::abs;
+    using float_pair = std::pair<float, float>;
     // 系への入力値の実効下限値
     constexpr float input_min = 0.4F;
 
@@ -215,8 +224,7 @@ void Schneider::set_q(const std::array<float, 3>& gyro) {
     if (abs(this->inputs[1]) <= input_min) {
         this->inputs[1] = 0;
     }
-    this->fet_1 = this->inputs[0];
-    this->fet_2 = this->inputs[1];
+    const float_pair fet_output = {this->inputs[0], this->inputs[1]};
 
     while (this->inputs[2] >= PI) {
         this->inputs[2] -= 2 * PI;
@@ -231,35 +239,36 @@ void Schneider::set_q(const std::array<float, 3>& gyro) {
         this->inputs[3] += 2 * PI;
     }
 
+    float_pair servo_output = this->last_output.servo;
     if (0 < this->inputs[2] && this->inputs[2] < PI) {
-        const int width = static_cast<int>(500 + 1900 / PI * this->inputs[2] - 2200 * gyro[2]);
-        this->servo_1.pulsewidth_us(width);
+        // 3.64 ~= 2200 * PI / 1900
+        // ea3f45b の src/schneider_model.cpp:238 と同様の計算式になるように
+        // TODO: gyroの係数調整
+        servo_output.first = this->inputs[2] - 3.64 * gyro[2];
     }
     if (0 < this->inputs[3] && this->inputs[3] < PI) {
-        const int width = static_cast<int>(500 + 1900 / PI * this->inputs[3] + 2200 * gyro[2]);
-        this->servo_2.pulsewidth_us(width);
+        servo_output.second = this->inputs[3] + 3.64 * gyro[2];
     }
+    const packet::OutputValues output(servo_output, fet_output);
+    return output;
 }
 
-void Schneider::rotate(const float& volume_value) {
+auto Schneider::rotate(const float& volume_value) const -> packet::OutputValues {
+    using float_pair = std::pair<float, float>;
     // volumeのしきい値
     constexpr float volume_threshold = 0.5F;
-    // サーボモータ出力値(pulse width)
-    constexpr uint16_t minor_rotate_pulsewidth_us = 550U;
-    constexpr uint16_t major_rotate_pulsewidth_us = 2350U;
     // DCモータ出力値(duty比)
     constexpr float fet_duty = 0.5F;
 
-    this->fet_1 = fet_duty;
-    this->fet_2 = fet_duty;
-    // ifとelseで内容が同じだといわれたがそんなことない
-    if (volume_value < volume_threshold) {
-        this->servo_1.pulsewidth_us(minor_rotate_pulsewidth_us);
-        this->servo_2.pulsewidth_us(major_rotate_pulsewidth_us);
-    } else {
-        this->servo_2.pulsewidth_us(minor_rotate_pulsewidth_us);
-        this->servo_1.pulsewidth_us(major_rotate_pulsewidth_us);
-    }
+    const float_pair servo_output
+        = volume_value < volume_threshold ? float_pair{0, PI} : float_pair{PI, 0};
+    const packet::OutputValues output(servo_output, {fet_duty, fet_duty});
+    return output;
+}
+
+auto Schneider::stop_fet() const -> packet::OutputValues {
+    packet::OutputValues output(this->last_output.servo, {0, 0});
+    return output;
 }
 
 auto Schneider::read_gyro() -> std::array<float, 3> {
@@ -268,4 +277,23 @@ auto Schneider::read_gyro() -> std::array<float, 3> {
     return gyro;
 }
 
+/// ラジアン → PWM pulsewidth_us
+constexpr auto servo_value_map(float radian) -> int {
+    return static_cast<int>(
+        (major_pulsewidth_us - minor_pulsewidth_us) * (radian / PI) + minor_pulsewidth_us);
+}
+
+auto Schneider::write_output(const packet::OutputValues& output) -> void {
+    if (!output.is_valid()) {
+        // TODO: outputの値を出力する
+        printf("ERROR: received invalid OutputValues\n");
+        return;
+    }
+    this->last_output = output;
+    this->servo_1.pulsewidth_us(servo_value_map(output.servo.first));
+    this->servo_2.pulsewidth_us(servo_value_map(output.servo.second));
+    this->fet_1.write(output.dc_motor.first);
+    this->fet_2.write(output.dc_motor.second);
+}
+
 }  // namespace omniboat

From 1d5c047a0374d66c7c08e9cd683c014a2f281b5e Mon Sep 17 00:00:00 2001
From: H1rono <hronok66@gmail.com>
Date: Fri, 17 May 2024 10:08:48 +0900
Subject: [PATCH 04/13] :sparkles: Add `packet::InputValues`

---
 include/packet/input.hpp | 45 ++++++++++++++++++++++++++++++++++++++++
 src/packet/input.cpp     | 15 ++++++++++++++
 2 files changed, 60 insertions(+)
 create mode 100644 include/packet/input.hpp
 create mode 100644 src/packet/input.cpp

diff --git a/include/packet/input.hpp b/include/packet/input.hpp
new file mode 100644
index 0000000..d12cf13
--- /dev/null
+++ b/include/packet/input.hpp
@@ -0,0 +1,45 @@
+#ifndef OMNIBOAT_ROBOKIT_PACKET_INPUT_HPP
+#define OMNIBOAT_ROBOKIT_PACKET_INPUT_HPP
+
+#include <array>
+#include <utility>
+
+namespace packet {
+
+struct InputValues {
+    /**
+     * @brief ジョイスティックの入力値 (0~1) (x, y)
+     */
+    std::pair<float, float> joy = {0, 0};
+
+    /**
+     * @brief ツマミの値 (0~1)
+     */
+    float volume = 0;
+
+    /**
+     * @brief ジャイロの値 [rad/s]
+     */
+    std::array<float, 3> gyro = {0, 0, 0};
+
+    InputValues(
+        const std::pair<float, float>& joy, const float& volume, const std::array<float, 3>& gyro);
+
+    InputValues() = default;
+    ~InputValues() = default;
+    InputValues(const InputValues&) = default;
+    auto operator=(const InputValues&) -> InputValues& = default;
+    InputValues(InputValues&&) = default;
+    auto operator=(InputValues&&) -> InputValues& = default;
+
+    /**
+     * @brief 値が適切かどうか判定する
+     * @return true 適切
+     * @return false 不適切; joy, volumeのいずれかが指定範囲にない
+     */
+    auto is_valid() const -> bool;
+};
+
+}  // namespace packet
+
+#endif  // OMNIBOAT_ROBOKIT_PACKET_INPUT_HPP
diff --git a/src/packet/input.cpp b/src/packet/input.cpp
new file mode 100644
index 0000000..35053bc
--- /dev/null
+++ b/src/packet/input.cpp
@@ -0,0 +1,15 @@
+#include "packet/input.hpp"
+
+packet::InputValues::InputValues(
+    const std::pair<float, float> &joy, const float &volume, const std::array<float, 3> &gyro) :
+    joy(joy), volume(volume), gyro(gyro) {}
+
+constexpr auto in_range(const float &value) -> bool {
+    return 0 <= value && value <= 1;
+}
+
+auto packet::InputValues::is_valid() const -> bool {
+    const bool joy_valid = in_range(this->joy.first) && in_range(this->joy.second);
+    const bool volume_valid = in_range(this->volume);
+    return joy_valid && volume_valid;
+}

From 2dbcd7473c70e628c6b0735d7b3a6e1e3b8fe696 Mon Sep 17 00:00:00 2001
From: H1rono <hronok66@gmail.com>
Date: Fri, 17 May 2024 10:53:13 +0900
Subject: [PATCH 05/13] :recycle: Use `packet::InputValues`

---
 include/schneider_model.hpp | 15 +++---------
 src/schneider_model.cpp     | 46 +++++++++++++++----------------------
 2 files changed, 21 insertions(+), 40 deletions(-)

diff --git a/include/schneider_model.hpp b/include/schneider_model.hpp
index 9022529..fc12a17 100644
--- a/include/schneider_model.hpp
+++ b/include/schneider_model.hpp
@@ -6,6 +6,7 @@
 #include "mbed.h"
 
 #include "MPU6050.h"
+#include "packet/input.hpp"
 #include "packet/output.hpp"
 
 namespace omniboat {
@@ -60,6 +61,8 @@ class Schneider {
      */
     packet::OutputValues last_output;
 
+    auto read_input() -> packet::InputValues;
+
     /**
      * @brief 状態方程式の計算を行う関数
      */
@@ -76,13 +79,6 @@ class Schneider {
      */
     auto set_q(const std::array<float, 3>& gyro) -> packet::OutputValues;
 
-    /**
-     * @brief ジョイコンの値を読み取り、目標値を算出して配列として返す
-     *
-     * @return std::array<float, 3> index0: x, index1: y, index2: rotation
-     */
-    auto read_joy() -> std::array<float, 3>;
-
     /**
      * @brief つまみの値をから機体を回転させる関数
      */
@@ -94,11 +90,6 @@ class Schneider {
      */
     auto stop_fet() const -> packet::OutputValues;
 
-    /**
-     * @brief ジャイロセンサの値を読み取る
-     */
-    auto read_gyro() -> std::array<float, 3>;
-
     auto write_output(const packet::OutputValues& output) -> void;
 
     AnalogIn adcIn1;  // ジョイスティック
diff --git a/src/schneider_model.cpp b/src/schneider_model.cpp
index 54b2223..93674c0 100644
--- a/src/schneider_model.cpp
+++ b/src/schneider_model.cpp
@@ -74,6 +74,13 @@ void Schneider::init() {
     }
 }
 
+inline auto map_joy(const std::pair<float, float>& joy) -> std::array<float, 3> {
+    // ジョイスティックの中心値
+    constexpr float joy_center = 0.5F;
+    // [2]はrot
+    return {(joy.first - joy_center) * 2, (joy.second - joy_center) * 2, 0};
+}
+
 void Schneider::one_step() {
     using std::abs;
     // ジョイスティックの値の実効下限値
@@ -84,27 +91,21 @@ void Schneider::one_step() {
 
     trace::toggle(LedId::Third);
 
-    // ジャイロセンサの値を読み取る
-    const auto gyro = this->read_gyro();
-
-    // ジョイコンの値を読み取り、目標値を代入
-    const auto joy = this->read_joy();
-
-    // ボリュームの値を読み取る
-    const float volume_value = volume.read();
+    const packet::InputValues input = this->read_input();
+    const std::array<float, 3> joy = map_joy(input.joy);
 
     this->inputs[0] = 0;
     this->inputs[1] = 0;
 
     const bool joyEffective = abs(joy[0]) > joy_min && abs(joy[1]) > joy_min;
-    const bool volumeEffective = volume_value < volume_under || volume_over < volume_value;
+    const bool volumeEffective = input.volume < volume_under || volume_over < input.volume;
 
     packet::OutputValues output;
     if (joyEffective) {
         this->cal_q(joy);
-        output = this->set_q(gyro);
+        output = this->set_q(input.gyro);
     } else if (volumeEffective) {
-        output = this->rotate(volume_value);
+        output = this->rotate(input.volume);
     } else {
         output = this->stop_fet();
     }
@@ -122,17 +123,12 @@ void Schneider::debug() {
     // printf("\n");
 }
 
-auto Schneider::read_joy() -> std::array<float, 3> {
-    // ジョイスティックの中心値
-    constexpr float joy_center = 0.5F;
-
-    const auto joy_x = this->adcIn1.read();
-    const auto joy_y = this->adcIn2.read();
-
-    const auto dest_x = (joy_x - joy_center) * 2;
-    const auto dest_y = (joy_y - joy_center) * 2;
-    const auto dest_rot = 0.0F;
-    return {dest_x, dest_y, dest_rot};
+auto Schneider::read_input() -> packet::InputValues {
+    const std::pair<float, float> joy(this->adcIn1.read(), this->adcIn2.read());
+    const float volume = this->volume.read();
+    std::array<float, 3> gyro;
+    this->mpu.getGyro(gyro.data());
+    return packet::InputValues(joy, volume, gyro);
 }
 
 inline std::array<std::array<float, 3>, 4> Schneider::cal_tjacob() const {
@@ -271,12 +267,6 @@ auto Schneider::stop_fet() const -> packet::OutputValues {
     return output;
 }
 
-auto Schneider::read_gyro() -> std::array<float, 3> {
-    std::array<float, 3> gyro;
-    this->mpu.getGyro(gyro.data());
-    return gyro;
-}
-
 /// ラジアン → PWM pulsewidth_us
 constexpr auto servo_value_map(float radian) -> int {
     return static_cast<int>(

From cb77ed653038aacf54512257c66367d5db230598 Mon Sep 17 00:00:00 2001
From: H1rono <hronok66@gmail.com>
Date: Fri, 17 May 2024 11:21:28 +0900
Subject: [PATCH 06/13] :adhesive_bandage: Handle clang-tidy errors

---
 include/utils.hpp       |  2 +-
 src/packet/input.cpp    |  2 ++
 src/packet/output.cpp   |  2 ++
 src/schneider_model.cpp | 13 ++++++++-----
 4 files changed, 13 insertions(+), 6 deletions(-)

diff --git a/include/utils.hpp b/include/utils.hpp
index f65e5a8..2e07da4 100644
--- a/include/utils.hpp
+++ b/include/utils.hpp
@@ -5,7 +5,7 @@
 namespace utils {
 
 /// 円周率π
-constexpr float PI = 3.1415927F;
+constexpr float PI = 3.1415927F;  // NOLINT(readability-identifier-length)
 
 }  // namespace utils
 
diff --git a/src/packet/input.cpp b/src/packet/input.cpp
index 35053bc..31dd422 100644
--- a/src/packet/input.cpp
+++ b/src/packet/input.cpp
@@ -1,8 +1,10 @@
 #include "packet/input.hpp"
 
+// NOLINTBEGIN(bugprone-easily-swappable-parameters): FIXME
 packet::InputValues::InputValues(
     const std::pair<float, float> &joy, const float &volume, const std::array<float, 3> &gyro) :
     joy(joy), volume(volume), gyro(gyro) {}
+// NOLINTEND(bugprone-easily-swappable-parameters)
 
 constexpr auto in_range(const float &value) -> bool {
     return 0 <= value && value <= 1;
diff --git a/src/packet/output.cpp b/src/packet/output.cpp
index da29861..d0b7ea6 100644
--- a/src/packet/output.cpp
+++ b/src/packet/output.cpp
@@ -4,8 +4,10 @@
 
 using float_pair = std::pair<float, float>;
 
+// NOLINTBEGIN(bugprone-easily-swappable-parameters): FIXME
 packet::OutputValues::OutputValues(const float_pair &servo, const float_pair &dc_motor) :
     servo(servo), dc_motor(dc_motor) {}
+// NOLINTEND(bugprone-easily-swappable-parameters)
 
 constexpr auto pi_range(float value) -> bool {
     return 0 <= value && value <= utils::PI;
diff --git a/src/schneider_model.cpp b/src/schneider_model.cpp
index 93674c0..6caa8f2 100644
--- a/src/schneider_model.cpp
+++ b/src/schneider_model.cpp
@@ -213,6 +213,10 @@ auto Schneider::set_q(const std::array<float, 3>& gyro) -> packet::OutputValues
     using float_pair = std::pair<float, float>;
     // 系への入力値の実効下限値
     constexpr float input_min = 0.4F;
+    // 3.64 ~= 2200 * PI / 1900
+    // ea3f45b の src/schneider_model.cpp:238 と同様の計算式になるように
+    // TODO: 係数調整
+    constexpr float gyro_coef = 3.64F;
 
     if (abs(this->inputs[0]) <= input_min) {
         this->inputs[0] = 0;
@@ -237,18 +241,16 @@ auto Schneider::set_q(const std::array<float, 3>& gyro) -> packet::OutputValues
 
     float_pair servo_output = this->last_output.servo;
     if (0 < this->inputs[2] && this->inputs[2] < PI) {
-        // 3.64 ~= 2200 * PI / 1900
-        // ea3f45b の src/schneider_model.cpp:238 と同様の計算式になるように
-        // TODO: gyroの係数調整
-        servo_output.first = this->inputs[2] - 3.64 * gyro[2];
+        servo_output.first = this->inputs[2] - gyro_coef * gyro[2];
     }
     if (0 < this->inputs[3] && this->inputs[3] < PI) {
-        servo_output.second = this->inputs[3] + 3.64 * gyro[2];
+        servo_output.second = this->inputs[3] + gyro_coef * gyro[2];
     }
     const packet::OutputValues output(servo_output, fet_output);
     return output;
 }
 
+// NOLINTBEGIN(readability-convert-member-functions-to-static)
 auto Schneider::rotate(const float& volume_value) const -> packet::OutputValues {
     using float_pair = std::pair<float, float>;
     // volumeのしきい値
@@ -261,6 +263,7 @@ auto Schneider::rotate(const float& volume_value) const -> packet::OutputValues
     const packet::OutputValues output(servo_output, {fet_duty, fet_duty});
     return output;
 }
+// NOLINTEND(readability-convert-member-functions-to-static)
 
 auto Schneider::stop_fet() const -> packet::OutputValues {
     packet::OutputValues output(this->last_output.servo, {0, 0});

From fd252e5a33dab3dd5813bd76cf8d07d79e85760b Mon Sep 17 00:00:00 2001
From: H1rono <hronok66@gmail.com>
Date: Fri, 17 May 2024 12:37:42 +0900
Subject: [PATCH 07/13] :sparkles: Add `device::InputModules`

---
 include/device/input.hpp | 66 ++++++++++++++++++++++++++++++++++++++++
 src/device/input.cpp     | 31 +++++++++++++++++++
 2 files changed, 97 insertions(+)
 create mode 100644 include/device/input.hpp
 create mode 100644 src/device/input.cpp

diff --git a/include/device/input.hpp b/include/device/input.hpp
new file mode 100644
index 0000000..fca6e89
--- /dev/null
+++ b/include/device/input.hpp
@@ -0,0 +1,66 @@
+#ifndef OMNIBOAT_ROBOKIT_DEVICE_INPUT_HPP
+#define OMNIBOAT_ROBOKIT_DEVICE_INPUT_HPP
+
+#include <array>
+#include <utility>
+
+#include "AnalogIn.h"
+
+#include "MPU6050.h"
+#include "packet/input.hpp"
+
+namespace device {
+
+/// センサー類まとめ
+class InputModules {
+private:
+    /// ジョイスティックのピン２つ
+    std::pair<mbed::AnalogIn, mbed::AnalogIn> joy;
+
+    /// ツマミ
+    mbed::AnalogIn volume;
+
+    /// 慣性計測ユニット(imu)
+    MPU6050 mpu;
+
+    /// ジョイスティックの値を読む
+    auto read_joy() -> std::pair<float, float>;
+
+    /// IMUの値を読む
+    auto read_gyro() -> std::array<float, 3>;
+
+public:
+    /**
+     * @brief Construct a new Input Modules object
+     * @param joy_pins ジョイスティックのピン2つ
+     * @param volume_pin ツマミのピン
+     * @param mpu_pins MPU6050のピン (sda, scl)
+     */
+    InputModules(
+        const std::pair<PinName, PinName>& joy_pins, const PinName& volume_pin,
+        const std::pair<PinName, PinName>& mpu_pins);
+
+    InputModules() = delete;
+    ~InputModules() = default;
+    InputModules(const InputModules&) = delete;
+    auto operator=(const InputModules&) -> InputModules& = delete;
+    InputModules(InputModules&&) = default;
+    auto operator=(InputModules&&) -> InputModules& = default;
+
+    /**
+     * @brief MPU6050のWHOAMI `MPU6050::testConnection` 参照
+     * @return true success
+     * @return false failure
+     */
+    auto mpu_whoami() -> bool;
+
+    /**
+     * @brief センサー類の値を読む
+     * @return packet::InputValues 読んだ値まとめ
+     */
+    auto read() -> packet::InputValues;
+};
+
+}  // namespace device
+
+#endif  // OMNIBOAT_ROBOKIT_DEVICE_INPUT_HPP
diff --git a/src/device/input.cpp b/src/device/input.cpp
new file mode 100644
index 0000000..a20a989
--- /dev/null
+++ b/src/device/input.cpp
@@ -0,0 +1,31 @@
+#include "device/input.hpp"
+
+auto device::InputModules::read_joy() -> std::pair<float, float> {
+    return {this->joy.first.read(), this->joy.second.read()};
+}
+
+auto device::InputModules::read_gyro() -> std::array<float, 3> {
+    std::array<float, 3> gyro;
+    this->mpu.getGyro(gyro.data());
+    return gyro;
+}
+
+// NOLINTBEGIN(bugprone-easily-swappable-parameters)
+device::InputModules::InputModules(
+    const std::pair<PinName, PinName>& joy_pins, const PinName& volume_pin,
+    const std::pair<PinName, PinName>& mpu_pins) :
+    joy(joy_pins.first, joy_pins.second),
+    volume(volume_pin),
+    mpu(mpu_pins.first, mpu_pins.second) {}
+// NOLINTEND(bugprone-easily-swappable-parameters)
+
+auto device::InputModules::mpu_whoami() -> bool {
+    return this->mpu.testConnection();
+}
+
+auto device::InputModules::read() -> packet::InputValues {
+    const std::pair<float, float> joy = this->read_joy();
+    const float volume = this->volume.read();
+    const std::array<float, 3> gyro = this->read_gyro();
+    return packet::InputValues(joy, volume, gyro);
+}

From e9bef500cf6941b3255813ebbb84540a32f94b03 Mon Sep 17 00:00:00 2001
From: H1rono <hronok66@gmail.com>
Date: Fri, 17 May 2024 12:42:45 +0900
Subject: [PATCH 08/13] :recycle: Use `device::InputModules`

---
 include/schneider_model.hpp |  8 ++------
 src/schneider_model.cpp     | 17 +++--------------
 2 files changed, 5 insertions(+), 20 deletions(-)

diff --git a/include/schneider_model.hpp b/include/schneider_model.hpp
index fc12a17..feab943 100644
--- a/include/schneider_model.hpp
+++ b/include/schneider_model.hpp
@@ -6,6 +6,7 @@
 #include "mbed.h"
 
 #include "MPU6050.h"
+#include "device/input.hpp"
 #include "packet/input.hpp"
 #include "packet/output.hpp"
 
@@ -61,8 +62,6 @@ class Schneider {
      */
     packet::OutputValues last_output;
 
-    auto read_input() -> packet::InputValues;
-
     /**
      * @brief 状態方程式の計算を行う関数
      */
@@ -92,10 +91,7 @@ class Schneider {
 
     auto write_output(const packet::OutputValues& output) -> void;
 
-    AnalogIn adcIn1;  // ジョイスティック
-    AnalogIn adcIn2;  // ジョイスティック
-    AnalogIn volume;
-    MPU6050 mpu;  // 慣性計測ユニット(imu)
+    device::InputModules input_modules;
 
     PwmOut servo_1;  // servo
     PwmOut servo_2;  // servo
diff --git a/src/schneider_model.cpp b/src/schneider_model.cpp
index 6caa8f2..71e2ee0 100644
--- a/src/schneider_model.cpp
+++ b/src/schneider_model.cpp
@@ -35,10 +35,7 @@ Schneider::Schneider() :
     inputs(),
     outputs(),
     last_output({0, 0}, {0, 0}),
-    adcIn1(A4),
-    adcIn2(A5),
-    volume(A6),
-    mpu(D4, D5),
+    input_modules({A4, A5}, A6, {D4, D5}),
     servo_1(PB_4),
     servo_2(PA_11),
     fet_1(PA_9),
@@ -66,7 +63,7 @@ void Schneider::init() {
     fill(this->inputs.begin(), this->inputs.end(), initialInputs);
     fill(this->outputs.begin(), this->outputs.end(), initialOutputs);
     this->cal_tjacob();
-    const bool whoami = this->mpu.testConnection();
+    const bool whoami = this->input_modules.mpu_whoami();
     if (whoami) {
         printf("WHOAMI succeeded\n");
     } else {
@@ -91,7 +88,7 @@ void Schneider::one_step() {
 
     trace::toggle(LedId::Third);
 
-    const packet::InputValues input = this->read_input();
+    const packet::InputValues input = this->input_modules.read();
     const std::array<float, 3> joy = map_joy(input.joy);
 
     this->inputs[0] = 0;
@@ -123,14 +120,6 @@ void Schneider::debug() {
     // printf("\n");
 }
 
-auto Schneider::read_input() -> packet::InputValues {
-    const std::pair<float, float> joy(this->adcIn1.read(), this->adcIn2.read());
-    const float volume = this->volume.read();
-    std::array<float, 3> gyro;
-    this->mpu.getGyro(gyro.data());
-    return packet::InputValues(joy, volume, gyro);
-}
-
 inline std::array<std::array<float, 3>, 4> Schneider::cal_tjacob() const {
     using std::cos;
     using std::sin;

From c72831c32f7ab4d5cb90d8fd43e8bb02b8e89909 Mon Sep 17 00:00:00 2001
From: H1rono <hronok66@gmail.com>
Date: Fri, 17 May 2024 14:45:20 +0900
Subject: [PATCH 09/13] :sparkles: Add `device::OutputModules`

---
 include/device/output.hpp | 65 +++++++++++++++++++++++++++++++++++++++
 src/device/output.cpp     | 43 ++++++++++++++++++++++++++
 2 files changed, 108 insertions(+)
 create mode 100644 include/device/output.hpp
 create mode 100644 src/device/output.cpp

diff --git a/include/device/output.hpp b/include/device/output.hpp
new file mode 100644
index 0000000..f257a72
--- /dev/null
+++ b/include/device/output.hpp
@@ -0,0 +1,65 @@
+#ifndef OMNIBOAT_ROBOKIT_DEVICE_OUTPUT_HPP
+#define OMNIBOAT_ROBOKIT_DEVICE_OUTPUT_HPP
+
+#include <utility>
+
+#include "PwmOut.h"
+
+#include "packet/output.hpp"
+
+namespace device {
+
+/// アクチュエーター類
+class OutputModules {
+private:
+    /// サーボ2つ
+    std::pair<mbed::PwmOut, mbed::PwmOut> servo;
+
+    /// DCモーター (につながっているFET) 2つ
+    std::pair<mbed::PwmOut, mbed::PwmOut> dc_motor;
+
+    /**
+     * @brief サーボに出力する
+     * @param values 出力する値 `OutputValues::servo` と同じ
+     */
+    auto write_servo(const std::pair<float, float>& values) -> void;
+
+    /**
+     * @brief DCモーターに出力する
+     * @param values 出力する値 `OutputModules::dc_motor` と同じ
+     */
+    auto write_dc_motor(const std::pair<float, float>& values) -> void;
+
+public:
+    /**
+     * @brief Construct a new Output Modules object
+     * @param servo_pins サーボのピン2つ
+     * @param dc_motor_pins DCモーター (につながっているFET) のピン2つ
+     */
+    OutputModules(
+        const std::pair<PinName, PinName>& servo_pins,
+        const std::pair<PinName, PinName>& dc_motor_pins);
+
+    OutputModules() = delete;
+    ~OutputModules() = default;
+    OutputModules(const OutputModules&) = delete;
+    auto operator=(const OutputModules&) -> OutputModules& = delete;
+    OutputModules(OutputModules&&) = default;
+    auto operator=(OutputModules&&) -> OutputModules& = default;
+
+    /**
+     * @brief セットアップ処理
+     * @details PWMの周期設定
+     */
+    auto init() -> void;
+
+    /**
+     * @brief アクチュエーターに出力する
+     * @param output 出力する値
+     */
+    auto write(const packet::OutputValues& output) -> void;
+};
+
+}  // namespace device
+
+#endif  // OMNIBOAT_ROBOKIT_DEVICE_OUTPUT_HPP
diff --git a/src/device/output.cpp b/src/device/output.cpp
new file mode 100644
index 0000000..448c223
--- /dev/null
+++ b/src/device/output.cpp
@@ -0,0 +1,43 @@
+#include "device/output.hpp"
+#include "utils.hpp"
+
+// サーボモータ出力値(pulse width)
+// FIXME: もともと550, 2350だったので要検証
+constexpr int minor_pulsewidth_us = 500;
+constexpr int major_pulsewidth_us = 2400;
+
+/// ラジアン → PWM pulsewidth_us
+constexpr auto servo_value_map(float radian) -> int {
+    return static_cast<int>(
+        (major_pulsewidth_us - minor_pulsewidth_us) * (radian / utils::PI) + minor_pulsewidth_us);
+}
+
+auto device::OutputModules::write_servo(const std::pair<float, float>& values) -> void {
+    this->servo.first.pulsewidth_us(servo_value_map(values.first));
+    this->servo.second.pulsewidth_us(servo_value_map(values.second));
+}
+
+auto device::OutputModules::write_dc_motor(const std::pair<float, float>& values) -> void {
+    this->dc_motor.first.write(values.first);
+    this->dc_motor.second.write(values.second);
+}
+
+// NOLINTBEGIN(bugprone-easily-swappable-parameters)
+device::OutputModules::OutputModules(
+    const std::pair<PinName, PinName>& servo_pins,
+    const std::pair<PinName, PinName>& dc_motor_pins) :
+    servo(servo_pins.first, servo_pins.second),
+    dc_motor(dc_motor_pins.first, dc_motor_pins.second) {}
+// NOLINTEND(bugprone-easily-swappable-parameters)
+
+auto device::OutputModules::init() -> void {
+    constexpr int pwm_period_ms = 20;
+
+    this->servo.first.period_ms(pwm_period_ms);
+    this->servo.second.period_ms(pwm_period_ms);
+}
+
+auto device::OutputModules::write(const packet::OutputValues& output) -> void {
+    this->write_servo(output.servo);
+    this->write_dc_motor(output.dc_motor);
+}

From 64f47306bb437524186718f3ad0aad206e283ac2 Mon Sep 17 00:00:00 2001
From: H1rono <hronok66@gmail.com>
Date: Fri, 17 May 2024 14:45:51 +0900
Subject: [PATCH 10/13] :recycle: Use `device::OutputModules`

---
 include/schneider_model.hpp |  7 ++-----
 src/schneider_model.cpp     | 27 +++------------------------
 2 files changed, 5 insertions(+), 29 deletions(-)

diff --git a/include/schneider_model.hpp b/include/schneider_model.hpp
index feab943..d54ec66 100644
--- a/include/schneider_model.hpp
+++ b/include/schneider_model.hpp
@@ -7,6 +7,7 @@
 
 #include "MPU6050.h"
 #include "device/input.hpp"
+#include "device/output.hpp"
 #include "packet/input.hpp"
 #include "packet/output.hpp"
 
@@ -92,11 +93,7 @@ class Schneider {
     auto write_output(const packet::OutputValues& output) -> void;
 
     device::InputModules input_modules;
-
-    PwmOut servo_1;  // servo
-    PwmOut servo_2;  // servo
-    PwmOut fet_1;    // DC
-    PwmOut fet_2;    // DC
+    device::OutputModules output_modules;
 };
 }  // namespace omniboat
 
diff --git a/src/schneider_model.cpp b/src/schneider_model.cpp
index 71e2ee0..0488fa8 100644
--- a/src/schneider_model.cpp
+++ b/src/schneider_model.cpp
@@ -26,29 +26,17 @@ constexpr float inertia_z = 1;
  */
 constexpr float step_width_a = 0.1;
 
-// サーボモータ出力値(pulse width)
-// FIXME: もともと550, 2350だったので要検証
-constexpr int minor_pulsewidth_us = 500;
-constexpr int major_pulsewidth_us = 2400;
-
 Schneider::Schneider() :
     inputs(),
     outputs(),
     last_output({0, 0}, {0, 0}),
     input_modules({A4, A5}, A6, {D4, D5}),
-    servo_1(PB_4),
-    servo_2(PA_11),
-    fet_1(PA_9),
-    fet_2(PA_10) {
-    constexpr uint8_t servo_pwm_period_ms = 20U;
-
+    output_modules({PB_4, PA_11}, {PA_9, PA_10}) {
     trace::toggle(LedId::First);
     trace::toggle(LedId::Second);
     trace::toggle(LedId::Third);
     printf("start up\n");
-
-    this->servo_1.period_ms(servo_pwm_period_ms);
-    this->servo_2.period_ms(servo_pwm_period_ms);
+    this->output_modules.init();
 }
 
 Schneider::~Schneider() {
@@ -259,12 +247,6 @@ auto Schneider::stop_fet() const -> packet::OutputValues {
     return output;
 }
 
-/// ラジアン → PWM pulsewidth_us
-constexpr auto servo_value_map(float radian) -> int {
-    return static_cast<int>(
-        (major_pulsewidth_us - minor_pulsewidth_us) * (radian / PI) + minor_pulsewidth_us);
-}
-
 auto Schneider::write_output(const packet::OutputValues& output) -> void {
     if (!output.is_valid()) {
         // TODO: outputの値を出力する
@@ -272,10 +254,7 @@ auto Schneider::write_output(const packet::OutputValues& output) -> void {
         return;
     }
     this->last_output = output;
-    this->servo_1.pulsewidth_us(servo_value_map(output.servo.first));
-    this->servo_2.pulsewidth_us(servo_value_map(output.servo.second));
-    this->fet_1.write(output.dc_motor.first);
-    this->fet_2.write(output.dc_motor.second);
+    this->output_modules.write(output);
 }
 
 }  // namespace omniboat

From 932940cdaba4a1079bc2976fd3d6da3e5a09f917 Mon Sep 17 00:00:00 2001
From: H1rono <hronok66@gmail.com>
Date: Sat, 18 May 2024 02:20:44 +0900
Subject: [PATCH 11/13] :sparkles: Add `service::Service`

---
 include/service/service.hpp |  79 ++++++++++++++
 src/service/service.cpp     | 207 ++++++++++++++++++++++++++++++++++++
 2 files changed, 286 insertions(+)
 create mode 100644 include/service/service.hpp
 create mode 100644 src/service/service.cpp

diff --git a/include/service/service.hpp b/include/service/service.hpp
new file mode 100644
index 0000000..059f9e7
--- /dev/null
+++ b/include/service/service.hpp
@@ -0,0 +1,79 @@
+#ifndef OMNIBOAT_ROBOKIT_SERVICE_HPP
+#define OMNIBOAT_ROBOKIT_SERVICE_HPP
+
+#include "packet/input.hpp"
+#include "packet/output.hpp"
+
+namespace service {
+
+class Service {
+public:
+    Service() = default;
+    ~Service() = default;
+    Service(const Service&) = default;
+    auto operator=(const Service&) -> Service& = default;
+    Service(Service&&) = default;
+    auto operator=(Service&&) -> Service& = default;
+
+    void init();
+
+    auto call(const packet::InputValues& input_values) -> packet::OutputValues;
+
+    // NOTE
+    // 使われてないのでコメントアウト
+    // 元の実装は f50fb9b の src/schneider_model.cpp:88 を参照
+    // /**
+    //  * @brief 機体を停止させる関数
+    //  */
+    // auto flip_shneider() -> void;
+
+    // TODO: auto debug() -> void;
+
+private:
+    /// 入力値
+    std::array<float, 4> inputs = {0, 0, 0};
+
+    /// inputsに対しての出力
+    std::array<float, 3> outputs = {0, 0, 0};
+
+    /// 実際に出力された最後の値 outputsとは違う
+    packet::OutputValues last_output{};
+
+    /**
+     * @brief ヤコビ行列の計算を行う関数
+     *        ヤコビ行列は、入力からモータの出力を計算するための行列
+     * @return std::array<std::array<float, 3>, 4> 計算したヤコビ行列
+     */
+    auto cal_tjacob() const -> std::array<std::array<float, 3>, 4>;
+
+    /// 状態方程式の計算を行う関数
+    auto state_equation() -> void;
+
+    /// モータへの出力を計算する関数
+    /// モータへの出力は、勾配を使って目的関数を最小化するように計算する
+    auto cal_q(const std::array<float, 3>& joy) -> void;
+
+    /**
+     * @brief 機体を旋回(=平行移動+回転) させるように出力する関数
+     * @param gyro MPUの入力 `packet::IntputValues::gyro`
+     * @return packet::OutputValues アクチュエーターに出力する値 ここで`last_output`は更新しない
+     */
+    auto set_q(const std::array<float, 3>& gyro) -> packet::OutputValues;
+
+    /**
+     * @brief ツマミの値から機体を回転させるように出力する関数
+     * @param volume ツマミの値
+     * @return packet::OutputValues アクチュエーターに出力する値 ここで`last_output`は更新しない
+     */
+    auto rotate(const float& volume) const -> packet::OutputValues;
+
+    /**
+     * @brief DCモーター(につながっているFET)を止めるように出力する関数
+     * @return packet::OutputValues アクチュエーターに出力する値 ここで`last_output`は更新しない
+     */
+    auto stop_dc_motor() const -> packet::OutputValues;
+};
+
+}  // namespace service
+
+#endif  // OMNIBOAT_ROBOKIT_SERVICE_HPP
diff --git a/src/service/service.cpp b/src/service/service.cpp
new file mode 100644
index 0000000..5580c6b
--- /dev/null
+++ b/src/service/service.cpp
@@ -0,0 +1,207 @@
+#include <algorithm>
+#include <cmath>
+
+#include "service/service.hpp"
+#include "trace.hpp"
+#include "utils.hpp"
+
+using trace::LedId;
+using utils::PI;
+
+/**
+ * @brief z軸周りの慣性モーメント
+ * @note もっと正確な値の方がいいかも
+ */
+constexpr float inertia_z = 1;
+/**
+ * @brief ステップ幅
+ */
+constexpr float step_width_a = 0.1;
+
+auto service::Service::init() -> void {
+    using std::fill;
+    constexpr float initial_inputs = 0.01F;
+    constexpr float initial_outputs = 0.0F;
+
+    fill(this->inputs.begin(), this->inputs.end(), initial_inputs);
+    fill(this->outputs.begin(), this->outputs.end(), initial_outputs);
+    this->cal_tjacob();
+}
+
+inline auto map_joy(const std::pair<float, float>& joy) -> std::array<float, 3> {
+    // ジョイスティックの中心値
+    constexpr float joy_center = 0.5F;
+    // [2]はrot
+    return {(joy.first - joy_center) * 2, (joy.second - joy_center) * 2, 0};
+}
+
+auto service::Service::call(const packet::InputValues& input_values) -> packet::OutputValues {
+    using std::abs;
+    // ジョイスティックの値の実効下限値
+    constexpr float joy_min = 0.4F;
+    // つまみの値の実効範囲 (x < volume_under || volume_over < x で有効)
+    constexpr float volume_under = 0.4F;
+    constexpr float volume_over = 0.7F;
+
+    trace::toggle(LedId::Third);
+
+    const std::array<float, 3> joy = map_joy(input_values.joy);
+
+    this->inputs[0] = 0;
+    this->inputs[1] = 0;
+
+    const bool joyEffective = abs(joy[0]) > joy_min && abs(joy[1]) > joy_min;
+    const bool volumeEffective
+        = input_values.volume < volume_under || volume_over < input_values.volume;
+
+    packet::OutputValues output;
+    if (joyEffective) {
+        this->cal_q(joy);
+        output = this->set_q(input_values.gyro);
+    } else if (volumeEffective) {
+        output = this->rotate(input_values.volume);
+    } else {
+        output = this->stop_dc_motor();
+    }
+    this->last_output = output;
+    trace::toggle(LedId::Third);
+}
+
+inline auto service::Service::cal_tjacob() const -> std::array<std::array<float, 3>, 4> {
+    using std::cos;
+    using std::sin;
+    std::array<std::array<float, 3>, 4> t_jacobianmatrix;
+    t_jacobianmatrix[0][0] = cos(this->inputs[2]);
+    t_jacobianmatrix[0][1] = sin(this->inputs[2]);
+    t_jacobianmatrix[0][2] = (step_width_a + sin(this->inputs[2])) / inertia_z;
+    t_jacobianmatrix[1][0] = cos(this->inputs[3]);
+    t_jacobianmatrix[1][1] = sin(this->inputs[3]);
+    t_jacobianmatrix[1][2] = (-step_width_a - sin(this->inputs[3])) / inertia_z;
+    t_jacobianmatrix[2][0] = -this->inputs[0] * sin(this->inputs[2]);
+    t_jacobianmatrix[2][1] = this->inputs[0] * cos(this->inputs[2]);
+    t_jacobianmatrix[2][2] = this->inputs[0] * cos(this->inputs[2]) / inertia_z;
+    t_jacobianmatrix[3][0] = -this->inputs[1] * sin(this->inputs[3]);
+    t_jacobianmatrix[3][1] = this->inputs[1] * cos(this->inputs[3]);
+    t_jacobianmatrix[3][2] = -this->inputs[1] * cos(this->inputs[3]) / inertia_z;
+    return t_jacobianmatrix;
+}
+
+auto service::Service::cal_q(const std::array<float, 3>& joy) -> void {
+    using std::pow;
+    // ステップ幅
+    constexpr float step_width_e = 0.01;
+    // 試行回数
+    constexpr size_t trial_num = 1000U;
+
+    // 初期値
+    const float coef = (joy[0] >= 0 && joy[1] >= 0)  ? 1
+                       : (joy[0] >= 0 && joy[1] < 0) ? -1
+                       : (joy[0] < 0 && joy[1] >= 0) ? 3
+                                                     : 5;
+    for (int i = 2; i < 4; ++i) {
+        this->inputs[i] = coef * PI / 4;
+    }
+
+    trace::toggle(LedId::Second);
+    for (size_t i = 0; i < trial_num; i++) {
+        // 目標値との差の2乗ノルム(diff)の実効下限値
+        constexpr float diff_min = 0.001F;
+
+        this->state_equation();
+
+        double diff = pow(this->outputs[0] - joy[0], 2) + pow(this->outputs[1] - joy[1], 2)
+                      + pow(this->outputs[2] - joy[2], 2);
+        if (diff < diff_min) {
+            break;
+        }
+
+        const std::array<std::array<float, 3>, 4> t_jacobianmatrix = this->cal_tjacob();
+        for (int j = 0; j < 4; j++) {
+            for (int k = 0; k < 3; k++) {
+                this->inputs[j]
+                    -= step_width_e * t_jacobianmatrix[j][k] * (this->outputs[k] - joy[k]);
+            }
+            if (j == 0 || j == 1) {
+                // 0.5に近づくように7次関数でバイアスをかけてる。多分。
+                constexpr int biasOrder = 7;
+                this->inputs[j] -= pow(2 * this->inputs[j] - 1, biasOrder);
+            }
+        }
+    }
+    trace::toggle(LedId::Second);
+}
+
+inline void service::Service::state_equation() {
+    using std::cos;
+    using std::sin;
+    this->outputs[0]
+        = this->inputs[0] * cos(this->inputs[2]) + this->inputs[1] * cos(this->inputs[3]);
+    this->outputs[1]
+        = this->inputs[0] * sin(this->inputs[2]) + this->inputs[1] * sin(this->inputs[3]);
+    this->outputs[2]
+        = (step_width_a * (this->inputs[0] - this->inputs[1])
+           + this->inputs[0] * sin(this->inputs[2]) - this->inputs[1] * sin(this->inputs[3]))
+          / inertia_z;
+}
+
+auto service::Service::set_q(const std::array<float, 3>& gyro) -> packet::OutputValues {
+    using std::abs;
+    using float_pair = std::pair<float, float>;
+    // 系への入力値の実効下限値
+    constexpr float input_min = 0.4F;
+    // 3.64 ~= 2200 * PI / 1900
+    // ea3f45b の src/schneider_model.cpp:238 と同様の計算式になるように
+    // TODO: 係数調整
+    constexpr float gyro_coef = 3.64F;
+
+    if (abs(this->inputs[0]) <= input_min) {
+        this->inputs[0] = 0;
+    }
+    if (abs(this->inputs[1]) <= input_min) {
+        this->inputs[1] = 0;
+    }
+    const float_pair fet_output = {this->inputs[0], this->inputs[1]};
+
+    while (this->inputs[2] >= PI) {
+        this->inputs[2] -= 2 * PI;
+    }
+    while (this->inputs[3] >= PI) {
+        this->inputs[3] -= 2 * PI;
+    }
+    while (this->inputs[2] < -PI) {
+        this->inputs[2] += 2 * PI;
+    }
+    while (this->inputs[3] < -PI) {
+        this->inputs[3] += 2 * PI;
+    }
+
+    float_pair servo_output = this->last_output.servo;
+    if (0 < this->inputs[2] && this->inputs[2] < PI) {
+        servo_output.first = this->inputs[2] - gyro_coef * gyro[2];
+    }
+    if (0 < this->inputs[3] && this->inputs[3] < PI) {
+        servo_output.second = this->inputs[3] + gyro_coef * gyro[2];
+    }
+    const packet::OutputValues output(servo_output, fet_output);
+    return output;
+}
+
+// NOLINTBEGIN(readability-convert-member-functions-to-static)
+auto service::Service::rotate(const float& volume_value) const -> packet::OutputValues {
+    using float_pair = std::pair<float, float>;
+    // volumeのしきい値
+    constexpr float volume_threshold = 0.5F;
+    // DCモータ出力値(duty比)
+    constexpr float fet_duty = 0.5F;
+
+    const float_pair servo_output
+        = volume_value < volume_threshold ? float_pair{0, PI} : float_pair{PI, 0};
+    const packet::OutputValues output(servo_output, {fet_duty, fet_duty});
+    return output;
+}
+// NOLINTEND(readability-convert-member-functions-to-static)
+
+auto service::Service::stop_dc_motor() const -> packet::OutputValues {
+    packet::OutputValues output(this->last_output.servo, {0, 0});
+    return output;
+}

From 5073b041201b052f6651a523a1f060168a898fa4 Mon Sep 17 00:00:00 2001
From: H1rono <hronok66@gmail.com>
Date: Sat, 18 May 2024 02:21:06 +0900
Subject: [PATCH 12/13] :recycle: Use `service::Service`

---
 include/schneider_model.hpp |  73 +++-----------
 src/schneider_model.cpp     | 193 +-----------------------------------
 2 files changed, 19 insertions(+), 247 deletions(-)

diff --git a/include/schneider_model.hpp b/include/schneider_model.hpp
index d54ec66..ef633eb 100644
--- a/include/schneider_model.hpp
+++ b/include/schneider_model.hpp
@@ -5,11 +5,11 @@
 
 #include "mbed.h"
 
-#include "MPU6050.h"
 #include "device/input.hpp"
 #include "device/output.hpp"
 #include "packet/input.hpp"
 #include "packet/output.hpp"
+#include "service/service.hpp"
 
 namespace omniboat {
 
@@ -32,69 +32,26 @@ class Schneider {
     void one_step();
     void init();
 
-    /**
-     * @brief 機体を停止させる関数
-     */
-    void flip_shneider();
+    // TODO:
+    // これ実装されてないのでコメントアウト
+    // commit `f50fb9b` 参照
+    // /**
+    //  * @brief 機体を停止させる関数
+    //  */
+    // void flip_shneider();
 
 private:
-    /**
-     * @brief ボタンが押されたときに機体を停止させる関数(割り込み処理)
-     */
-    void ticker_flip();
-
-    /**
-     * @brief 入力値
-     */
-    std::array<float, 4> inputs;
-
-    /**
-     * @brief inputsに対しての出力
-     */
-    std::array<float, 3> outputs;
-
-    /**
-     * @brief ヤコビ行列の計算を行う関数\nヤコビ行列は、入力からモータの出力を計算するための行列
-     */
-    std::array<std::array<float, 3>, 4> cal_tjacob() const;
-
-    /**
-     * @brief 最後にアクチュエーターに反映させた値
-     */
-    packet::OutputValues last_output;
-
-    /**
-     * @brief 状態方程式の計算を行う関数
-     */
-    void state_equation();
-
-    /**
-     * @brief
-     * モータへの出力を計算する関数\nモータへの出力は、勾配を使って目的関数を最小化するように計算する
-     */
-    auto cal_q(const std::array<float, 3>& joy) -> void;
-
-    /**
-     * @brief モータへの信号値に変換する関数
-     */
-    auto set_q(const std::array<float, 3>& gyro) -> packet::OutputValues;
-
-    /**
-     * @brief つまみの値をから機体を回転させる関数
-     */
-    auto rotate(const float& volume_value) const -> packet::OutputValues;
-
-    /**
-     * @brief FETへの出力(=DCモーター)を止める関数
-     * @return packet::OutputValues write_outputに渡す値
-     */
-    auto stop_fet() const -> packet::OutputValues;
-
-    auto write_output(const packet::OutputValues& output) -> void;
+    // flip_shneider と同様
+    // /**
+    //  * @brief ボタンが押されたときに機体を停止させる関数(割り込み処理)
+    //  */
+    // void ticker_flip();
 
     device::InputModules input_modules;
     device::OutputModules output_modules;
+    service::Service service;
 };
+
 }  // namespace omniboat
 
 #endif
diff --git a/src/schneider_model.cpp b/src/schneider_model.cpp
index 0488fa8..0755bfd 100644
--- a/src/schneider_model.cpp
+++ b/src/schneider_model.cpp
@@ -27,11 +27,7 @@ constexpr float inertia_z = 1;
 constexpr float step_width_a = 0.1;
 
 Schneider::Schneider() :
-    inputs(),
-    outputs(),
-    last_output({0, 0}, {0, 0}),
-    input_modules({A4, A5}, A6, {D4, D5}),
-    output_modules({PB_4, PA_11}, {PA_9, PA_10}) {
+    input_modules({A4, A5}, A6, {D4, D5}), output_modules({PB_4, PA_11}, {PA_9, PA_10}), service() {
     trace::toggle(LedId::First);
     trace::toggle(LedId::Second);
     trace::toggle(LedId::Third);
@@ -44,13 +40,7 @@ Schneider::~Schneider() {
 }
 
 void Schneider::init() {
-    using std::fill;
-    constexpr float initialInputs = 0.01F;
-    constexpr float initialOutputs = 0.0F;
-
-    fill(this->inputs.begin(), this->inputs.end(), initialInputs);
-    fill(this->outputs.begin(), this->outputs.end(), initialOutputs);
-    this->cal_tjacob();
+    this->service.init();
     const bool whoami = this->input_modules.mpu_whoami();
     if (whoami) {
         printf("WHOAMI succeeded\n");
@@ -67,35 +57,9 @@ inline auto map_joy(const std::pair<float, float>& joy) -> std::array<float, 3>
 }
 
 void Schneider::one_step() {
-    using std::abs;
-    // ジョイスティックの値の実効下限値
-    constexpr float joy_min = 0.4F;
-    // つまみの値の実効範囲 (x < volume_under || volume_over < x で有効)
-    constexpr float volume_under = 0.4F;
-    constexpr float volume_over = 0.7F;
-
-    trace::toggle(LedId::Third);
-
     const packet::InputValues input = this->input_modules.read();
-    const std::array<float, 3> joy = map_joy(input.joy);
-
-    this->inputs[0] = 0;
-    this->inputs[1] = 0;
-
-    const bool joyEffective = abs(joy[0]) > joy_min && abs(joy[1]) > joy_min;
-    const bool volumeEffective = input.volume < volume_under || volume_over < input.volume;
-
-    packet::OutputValues output;
-    if (joyEffective) {
-        this->cal_q(joy);
-        output = this->set_q(input.gyro);
-    } else if (volumeEffective) {
-        output = this->rotate(input.volume);
-    } else {
-        output = this->stop_fet();
-    }
-    this->write_output(output);
-    this->debug();
+    const packet::OutputValues output = this->service.call(input);
+    this->output_modules.write(output);
     trace::toggle(LedId::Third);
 }
 
@@ -108,153 +72,4 @@ void Schneider::debug() {
     // printf("\n");
 }
 
-inline std::array<std::array<float, 3>, 4> Schneider::cal_tjacob() const {
-    using std::cos;
-    using std::sin;
-    std::array<std::array<float, 3>, 4> t_jacobianmatrix;
-    t_jacobianmatrix[0][0] = cos(this->inputs[2]);
-    t_jacobianmatrix[0][1] = sin(this->inputs[2]);
-    t_jacobianmatrix[0][2] = (step_width_a + sin(this->inputs[2])) / inertia_z;
-    t_jacobianmatrix[1][0] = cos(this->inputs[3]);
-    t_jacobianmatrix[1][1] = sin(this->inputs[3]);
-    t_jacobianmatrix[1][2] = (-step_width_a - sin(this->inputs[3])) / inertia_z;
-    t_jacobianmatrix[2][0] = -this->inputs[0] * sin(this->inputs[2]);
-    t_jacobianmatrix[2][1] = this->inputs[0] * cos(this->inputs[2]);
-    t_jacobianmatrix[2][2] = this->inputs[0] * cos(this->inputs[2]) / inertia_z;
-    t_jacobianmatrix[3][0] = -this->inputs[1] * sin(this->inputs[3]);
-    t_jacobianmatrix[3][1] = this->inputs[1] * cos(this->inputs[3]);
-    t_jacobianmatrix[3][2] = -this->inputs[1] * cos(this->inputs[3]) / inertia_z;
-    return t_jacobianmatrix;
-}
-
-auto Schneider::cal_q(const std::array<float, 3>& joy) -> void {
-    using std::pow;
-    // ステップ幅
-    constexpr float step_width_e = 0.01;
-    // 試行回数
-    constexpr size_t trial_num = 1000U;
-
-    // 初期値
-    const float coef = (joy[0] >= 0 && joy[1] >= 0)  ? 1
-                       : (joy[0] >= 0 && joy[1] < 0) ? -1
-                       : (joy[0] < 0 && joy[1] >= 0) ? 3
-                                                     : 5;
-    for (int i = 2; i < 4; ++i) {
-        this->inputs[i] = coef * PI / 4;
-    }
-
-    trace::toggle(LedId::Second);
-    for (size_t i = 0; i < trial_num; i++) {
-        // 目標値との差の2乗ノルム(diff)の実効下限値
-        constexpr float diff_min = 0.001F;
-
-        this->state_equation();
-
-        double diff = pow(this->outputs[0] - joy[0], 2) + pow(this->outputs[1] - joy[1], 2)
-                      + pow(this->outputs[2] - joy[2], 2);
-        if (diff < diff_min) {
-            break;
-        }
-
-        const std::array<std::array<float, 3>, 4> t_jacobianmatrix = this->cal_tjacob();
-        for (int j = 0; j < 4; j++) {
-            for (int k = 0; k < 3; k++) {
-                this->inputs[j]
-                    -= step_width_e * t_jacobianmatrix[j][k] * (this->outputs[k] - joy[k]);
-            }
-            if (j == 0 || j == 1) {
-                // 0.5に近づくように7次関数でバイアスをかけてる。多分。
-                constexpr int biasOrder = 7;
-                this->inputs[j] -= pow(2 * this->inputs[j] - 1, biasOrder);
-            }
-        }
-    }
-    trace::toggle(LedId::Second);
-}
-
-inline void Schneider::state_equation() {
-    using std::cos;
-    using std::sin;
-    this->outputs[0]
-        = this->inputs[0] * cos(this->inputs[2]) + this->inputs[1] * cos(this->inputs[3]);
-    this->outputs[1]
-        = this->inputs[0] * sin(this->inputs[2]) + this->inputs[1] * sin(this->inputs[3]);
-    this->outputs[2]
-        = (step_width_a * (this->inputs[0] - this->inputs[1])
-           + this->inputs[0] * sin(this->inputs[2]) - this->inputs[1] * sin(this->inputs[3]))
-          / inertia_z;
-}
-
-auto Schneider::set_q(const std::array<float, 3>& gyro) -> packet::OutputValues {
-    using std::abs;
-    using float_pair = std::pair<float, float>;
-    // 系への入力値の実効下限値
-    constexpr float input_min = 0.4F;
-    // 3.64 ~= 2200 * PI / 1900
-    // ea3f45b の src/schneider_model.cpp:238 と同様の計算式になるように
-    // TODO: 係数調整
-    constexpr float gyro_coef = 3.64F;
-
-    if (abs(this->inputs[0]) <= input_min) {
-        this->inputs[0] = 0;
-    }
-    if (abs(this->inputs[1]) <= input_min) {
-        this->inputs[1] = 0;
-    }
-    const float_pair fet_output = {this->inputs[0], this->inputs[1]};
-
-    while (this->inputs[2] >= PI) {
-        this->inputs[2] -= 2 * PI;
-    }
-    while (this->inputs[3] >= PI) {
-        this->inputs[3] -= 2 * PI;
-    }
-    while (this->inputs[2] < -PI) {
-        this->inputs[2] += 2 * PI;
-    }
-    while (this->inputs[3] < -PI) {
-        this->inputs[3] += 2 * PI;
-    }
-
-    float_pair servo_output = this->last_output.servo;
-    if (0 < this->inputs[2] && this->inputs[2] < PI) {
-        servo_output.first = this->inputs[2] - gyro_coef * gyro[2];
-    }
-    if (0 < this->inputs[3] && this->inputs[3] < PI) {
-        servo_output.second = this->inputs[3] + gyro_coef * gyro[2];
-    }
-    const packet::OutputValues output(servo_output, fet_output);
-    return output;
-}
-
-// NOLINTBEGIN(readability-convert-member-functions-to-static)
-auto Schneider::rotate(const float& volume_value) const -> packet::OutputValues {
-    using float_pair = std::pair<float, float>;
-    // volumeのしきい値
-    constexpr float volume_threshold = 0.5F;
-    // DCモータ出力値(duty比)
-    constexpr float fet_duty = 0.5F;
-
-    const float_pair servo_output
-        = volume_value < volume_threshold ? float_pair{0, PI} : float_pair{PI, 0};
-    const packet::OutputValues output(servo_output, {fet_duty, fet_duty});
-    return output;
-}
-// NOLINTEND(readability-convert-member-functions-to-static)
-
-auto Schneider::stop_fet() const -> packet::OutputValues {
-    packet::OutputValues output(this->last_output.servo, {0, 0});
-    return output;
-}
-
-auto Schneider::write_output(const packet::OutputValues& output) -> void {
-    if (!output.is_valid()) {
-        // TODO: outputの値を出力する
-        printf("ERROR: received invalid OutputValues\n");
-        return;
-    }
-    this->last_output = output;
-    this->output_modules.write(output);
-}
-
 }  // namespace omniboat

From eb13e8b5052dc5479dd911fd8929d5ccb1dbe3c1 Mon Sep 17 00:00:00 2001
From: H1rono <hronok66@gmail.com>
Date: Sat, 18 May 2024 12:07:58 +0900
Subject: [PATCH 13/13] :adhesive_bandage: Fix

---
 src/schneider_model.cpp |  1 -
 src/service/service.cpp | 10 ++++++----
 2 files changed, 6 insertions(+), 5 deletions(-)

diff --git a/src/schneider_model.cpp b/src/schneider_model.cpp
index 0755bfd..e714a7b 100644
--- a/src/schneider_model.cpp
+++ b/src/schneider_model.cpp
@@ -14,7 +14,6 @@
 namespace omniboat {
 
 using trace::LedId;
-using utils::PI;
 
 /**
  * @brief z軸周りの慣性モーメント
diff --git a/src/service/service.cpp b/src/service/service.cpp
index 5580c6b..df1ac19 100644
--- a/src/service/service.cpp
+++ b/src/service/service.cpp
@@ -1,5 +1,6 @@
 #include <algorithm>
 #include <cmath>
+#include <cstdio>
 
 #include "service/service.hpp"
 #include "trace.hpp"
@@ -65,6 +66,7 @@ auto service::Service::call(const packet::InputValues& input_values) -> packet::
     }
     this->last_output = output;
     trace::toggle(LedId::Third);
+    return output;
 }
 
 inline auto service::Service::cal_tjacob() const -> std::array<std::array<float, 3>, 4> {
@@ -160,7 +162,7 @@ auto service::Service::set_q(const std::array<float, 3>& gyro) -> packet::Output
     if (abs(this->inputs[1]) <= input_min) {
         this->inputs[1] = 0;
     }
-    const float_pair fet_output = {this->inputs[0], this->inputs[1]};
+    const float_pair dc_motor_output = {this->inputs[0], this->inputs[1]};
 
     while (this->inputs[2] >= PI) {
         this->inputs[2] -= 2 * PI;
@@ -182,7 +184,7 @@ auto service::Service::set_q(const std::array<float, 3>& gyro) -> packet::Output
     if (0 < this->inputs[3] && this->inputs[3] < PI) {
         servo_output.second = this->inputs[3] + gyro_coef * gyro[2];
     }
-    const packet::OutputValues output(servo_output, fet_output);
+    const packet::OutputValues output(servo_output, dc_motor_output);
     return output;
 }
 
@@ -192,11 +194,11 @@ auto service::Service::rotate(const float& volume_value) const -> packet::Output
     // volumeのしきい値
     constexpr float volume_threshold = 0.5F;
     // DCモータ出力値(duty比)
-    constexpr float fet_duty = 0.5F;
+    constexpr float dc_motor_duty = 0.5F;
 
     const float_pair servo_output
         = volume_value < volume_threshold ? float_pair{0, PI} : float_pair{PI, 0};
-    const packet::OutputValues output(servo_output, {fet_duty, fet_duty});
+    const packet::OutputValues output(servo_output, {dc_motor_duty, dc_motor_duty});
     return output;
 }
 // NOLINTEND(readability-convert-member-functions-to-static)