service

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

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

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

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

include/schneider_model.hpp

@@ -5,7 +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 {
 
@@ -28,75 +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 状態方程式の計算を行う関数
-     */
-    void state_equation();
-
-    /**
-     * @brief
-     * モータへの出力を計算する関数\nモータへの出力は、勾配を使って目的関数を最小化するように計算する
-     */
-    auto cal_q(const std::array<float, 3>& joy) -> void;
-
-    /**
-     * @brief モータへの信号値に変換する関数
-     */
-    void set_q(const std::array<float, 3>& gyro);
-
-    /**
-     * @brief ジョイコンの値を読み取り、目標値を算出して配列として返す
-     *
-     * @return std::array<float, 3> index0: x, index1: y, index2: rotation
-     */
-    auto read_joy() -> std::array<float, 3>;
-
-    /**
-     * @brief つまみの値をから機体を回転させる関数
-     */
-    void rotate(const float& volume_value);
-
-    /**
-     * @brief ジャイロセンサの値を読み取る
-     */
-    auto read_gyro() -> std::array<float, 3>;
-
-    AnalogIn adcIn1;  // ジョイスティック
-    AnalogIn adcIn2;  // ジョイスティック
-    AnalogIn volume;
-    MPU6050 mpu;  // 慣性計測ユニット(imu)
-
-    PwmOut servo_1;  // servo
-    PwmOut servo_2;  // servo
-    PwmOut fet_1;    // DC
-    PwmOut fet_2;    // DC
+    // flip_shneider と同様
+    // /**
+    //  * @brief ボタンが押されたときに機体を停止させる関数(割り込み処理)
+    //  */
+    // void ticker_flip();
+
+    device::InputModules input_modules;
+    device::OutputModules output_modules;
+    service::Service service;
 };
+
 }  // namespace omniboat
 
 #endif