From 932940cdaba4a1079bc2976fd3d6da3e5a09f917 Mon Sep 17 00:00:00 2001 From: H1rono Date: Sat, 18 May 2024 02:20:44 +0900 Subject: [PATCH 1/3] :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 inputs = {0, 0, 0}; + + /// inputsに対しての出力 + std::array outputs = {0, 0, 0}; + + /// 実際に出力された最後の値 outputsとは違う + packet::OutputValues last_output{}; + + /** + * @brief ヤコビ行列の計算を行う関数 + * ヤコビ行列は、入力からモータの出力を計算するための行列 + * @return std::array, 4> 計算したヤコビ行列 + */ + auto cal_tjacob() const -> std::array, 4>; + + /// 状態方程式の計算を行う関数 + auto state_equation() -> void; + + /// モータへの出力を計算する関数 + /// モータへの出力は、勾配を使って目的関数を最小化するように計算する + auto cal_q(const std::array& joy) -> void; + + /** + * @brief 機体を旋回(=平行移動+回転) させるように出力する関数 + * @param gyro MPUの入力 `packet::IntputValues::gyro` + * @return packet::OutputValues アクチュエーターに出力する値 ここで`last_output`は更新しない + */ + auto set_q(const std::array& 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 +#include + +#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& joy) -> std::array { + // ジョイスティックの中心値 + 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 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, 4> { + using std::cos; + using std::sin; + std::array, 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& 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, 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& gyro) -> packet::OutputValues { + using std::abs; + using float_pair = std::pair; + // 系への入力値の実効下限値 + 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; + // 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 Date: Sat, 18 May 2024 02:21:06 +0900 Subject: [PATCH 2/3] :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 inputs; - - /** - * @brief inputsに対しての出力 - */ - std::array outputs; - - /** - * @brief ヤコビ行列の計算を行う関数\nヤコビ行列は、入力からモータの出力を計算するための行列 - */ - std::array, 4> cal_tjacob() const; - - /** - * @brief 最後にアクチュエーターに反映させた値 - */ - packet::OutputValues last_output; - - /** - * @brief 状態方程式の計算を行う関数 - */ - void state_equation(); - - /** - * @brief - * モータへの出力を計算する関数\nモータへの出力は、勾配を使って目的関数を最小化するように計算する - */ - auto cal_q(const std::array& joy) -> void; - - /** - * @brief モータへの信号値に変換する関数 - */ - auto set_q(const std::array& 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& joy) -> std::array } 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 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, 4> Schneider::cal_tjacob() const { - using std::cos; - using std::sin; - std::array, 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& 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, 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& gyro) -> packet::OutputValues { - using std::abs; - using float_pair = std::pair; - // 系への入力値の実効下限値 - 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; - // 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 Date: Sat, 18 May 2024 12:07:58 +0900 Subject: [PATCH 3/3] :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 #include +#include #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, 4> { @@ -160,7 +162,7 @@ auto service::Service::set_q(const std::array& 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& 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)