Merge pull request #186 from rogy-AquaLab/device

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/schneider_model.hpp

@@ -6,6 +6,8 @@
 #include "mbed.h"
 
 #include "MPU6050.h"
+#include "device/input.hpp"
+#include "device/output.hpp"
 #include "packet/input.hpp"
 #include "packet/output.hpp"
 
@@ -61,8 +63,6 @@ class Schneider {
      */
     packet::OutputValues last_output;
 
-    auto read_input() -> packet::InputValues;
-
     /**
      * @brief 状態方程式の計算を行う関数
      */
@@ -92,15 +92,8 @@ class Schneider {
 
     auto write_output(const packet::OutputValues& output) -> void;
 
-    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
+    device::InputModules input_modules;
+    device::OutputModules output_modules;
 };
 }  // namespace omniboat
 

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);
+}

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);
+}

src/schneider_model.cpp

@@ -26,32 +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}),
-    adcIn1(A4),
-    adcIn2(A5),
-    volume(A6),
-    mpu(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;
-
+    input_modules({A4, A5}, A6, {D4, D5}),
+    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() {
@@ -66,7 +51,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 +76,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 +108,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;
@@ -270,23 +247,14 @@ 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の値を出力する
         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);
+    this->output_modules.write(output);
 }
 
 }  // namespace omniboat