diff --git a/include/schneider_model.hpp b/include/schneider_model.hpp index 57b4712..3b74f52 100644 --- a/include/schneider_model.hpp +++ b/include/schneider_model.hpp @@ -7,71 +7,8 @@ #include "MPU6050.h" -constexpr float schneider_PI = 3.141592653589; - namespace omniboat { -/** - * @brief z軸周りの慣性モーメント - * @note もっと正確な値の方がいいかも - */ -constexpr float I = 1; // NOLINT: FIXME -/** - * @brief ステップ幅 - */ -constexpr float e = 0.01; // NOLINT: FIXME -constexpr float a = 0.1; // NOLINT: FIXME - -/** - * @brief 試行回数 - */ -constexpr int trial_num = 1000; - -/** - * @brief volumeの閾値 - */ -constexpr float volumeThreshold = 0.5F; - -/** - * @brief ジョイスティックの下限値 - */ -constexpr float joyThreshold = 0.4F; - -/** - * @brief ジョイスティックの中央値 - */ -constexpr float joyCenter = 0.5F; - -/** - * @brief volumeの下限値・上限値 - */ -constexpr std::pair volumeIneffectiveRange = {0.4F, 0.7F}; - -/** - * @brief pulsewidthの小さいほうの値 - */ -constexpr int minorRotatePulsewidthUs = 550; - -/** - * @brief pulsewidthの大きいほうの値 - */ -constexpr int majorRotatePulsewidthUs = 2350; - -/** - * @brief 座標・姿勢の目標値と現在値の偏差の有効範囲の最小値 (それ未満は偏差0とみなす) - */ -constexpr float diffThreshold = 0.001F; - -/** - * @brief サーボのPWM周期 - */ -constexpr int pwmPeriodMs = 20; - -/** - * @brief DCモータ用FETへのPWM出力(duty比) - */ -constexpr float fetDuty = 0.5F; - /** * @brief モータへの出力を計算するクラス */ diff --git a/src/schneider_model.cpp b/src/schneider_model.cpp index 24693eb..dbed16d 100644 --- a/src/schneider_model.cpp +++ b/src/schneider_model.cpp @@ -10,6 +10,20 @@ namespace omniboat { +/** + * @brief π + */ +constexpr float schneider_PI = 3.1415927F; +/** + * @brief z軸周りの慣性モーメント + * @note もっと正確な値の方がいいかも + */ +constexpr float I = 1.0F; // NOLINT: FIXME +/** + * @brief ステップ幅 + */ +constexpr float a = 0.1F; // NOLINT: FIXME + Schneider::Schneider() : q(), x(), @@ -24,13 +38,15 @@ Schneider::Schneider() : led1(PA_1), led2(PA_3), led3(PA_4) { + constexpr uint8_t servo_pwm_period_ms = 20U; + this->led(1); this->led(2); this->led(3); printf("start up\n"); - this->servo_1.period_ms(pwmPeriodMs); - this->servo_2.period_ms(pwmPeriodMs); + this->servo_1.period_ms(servo_pwm_period_ms); + this->servo_2.period_ms(servo_pwm_period_ms); } Schneider::~Schneider() { @@ -55,6 +71,12 @@ void Schneider::init() { 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; + this->led(3); // ジャイロセンサの値を読み取る @@ -69,9 +91,8 @@ void Schneider::one_step() { this->q[0] = 0; this->q[1] = 0; - const bool joyEffective = abs(joy[0]) > joyThreshold || abs(joy[1]) > joyThreshold; - const bool volumeEffective = volume_value < volumeIneffectiveRange.first - || volumeIneffectiveRange.second < volume_value; + const bool joyEffective = abs(joy[0]) > joy_min && abs(joy[1]) > joy_min; + const bool volumeEffective = volume_value < volume_under || volume_over < volume_value; if (joyEffective) { this->cal_q(joy); @@ -96,11 +117,14 @@ void Schneider::debug() { } auto Schneider::read_joy() -> std::array { + // ジョイスティックの中心値 + 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 - joyCenter) * 2; - const auto dest_y = (joy_y - joyCenter) * 2; + 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}; } @@ -126,6 +150,11 @@ inline std::array, 4> Schneider::cal_tjacob() const { auto Schneider::cal_q(const std::array& joy) -> void { using std::pow; + // ステップ幅 + constexpr float e = 0.01F; // NOLINT: FIXME + // 試行回数 + constexpr size_t trial_num = 1000U; + // 初期値 const float coef = (joy[0] >= 0 && joy[1] >= 0) ? 1 : (joy[0] >= 0 && joy[1] < 0) ? -1 @@ -136,12 +165,15 @@ auto Schneider::cal_q(const std::array& joy) -> void { } led(2); - for (int i = 0; i < trial_num; i++) { + for (size_t i = 0; i < trial_num; i++) { + // 目標値との差の2乗ノルム(diff)の実効下限値 + constexpr float diff_min = 0.001F; + this->state_equation(); double diff = pow(this->x[0] - joy[0], 2) + pow(this->x[1] - joy[1], 2) + pow(this->x[2] - joy[2], 2); - if (diff < diffThreshold) { + if (diff < diff_min) { break; } @@ -172,10 +204,13 @@ inline void Schneider::state_equation() { void Schneider::set_q(const std::array& gyro) { using std::abs; - if (abs(this->q[0] <= joyThreshold)) { + // 系への入力値の実効下限値 + constexpr float input_min = 0.4F; + + if (abs(this->q[0] <= input_min)) { this->q[0] = 0; } - if (abs(this->q[1] <= joyThreshold)) { + if (abs(this->q[1] <= input_min)) { this->q[1] = 0; } this->fet_1 = this->q[0]; @@ -205,15 +240,23 @@ void Schneider::set_q(const std::array& gyro) { } void Schneider::rotate(const float& volume_value) { - this->fet_1 = fetDuty; - this->fet_2 = fetDuty; + // 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 < volumeThreshold) { - this->servo_1.pulsewidth_us(minorRotatePulsewidthUs); - this->servo_2.pulsewidth_us(majorRotatePulsewidthUs); + 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(minorRotatePulsewidthUs); - this->servo_1.pulsewidth_us(majorRotatePulsewidthUs); + this->servo_2.pulsewidth_us(minor_rotate_pulsewidth_us); + this->servo_1.pulsewidth_us(major_rotate_pulsewidth_us); } }