Skip to content

Commit

Permalink
Merge pull request #174 from rogy-AquaLab/limit-scope-of-constexpr-va…
Browse files Browse the repository at this point in the history 
…rialbes

limited the scope of constexpr variables in the namespace `schneider_model`
  • Loading branch information
@H1rono
H1rono authored May 8, 2024
2 parents 95f86cf + 2ba6486 commit 7cd7749
Show file tree
Hide file tree
Showing 2 changed files with 61 additions and 81 deletions.
63 changes: 0 additions & 63 deletions include/schneider_model.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<float, float> 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 モータへの出力を計算するクラス
*/
Expand Down
79 changes: 61 additions & 18 deletions src/schneider_model.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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(),
Expand All @@ -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() {
Expand All @@ -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);

// ジャイロセンサの値を読み取る
Expand All @@ -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);
Expand All @@ -96,11 +117,14 @@ void Schneider::debug() {
}

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 - 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};
}
Expand All @@ -126,6 +150,11 @@ inline std::array<std::array<float, 3>, 4> Schneider::cal_tjacob() const {

auto Schneider::cal_q(const std::array<float, 3>& 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
Expand All @@ -136,12 +165,15 @@ auto Schneider::cal_q(const std::array<float, 3>& 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;
}

Expand Down Expand Up @@ -172,10 +204,13 @@ inline void Schneider::state_equation() {

void Schneider::set_q(const std::array<float, 3>& 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];
Expand Down Expand Up @@ -205,15 +240,23 @@ void Schneider::set_q(const std::array<float, 3>& 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);
}
}

Expand Down

0 comments on commit 7cd7749

Please sign in to comment.