Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

1文字変数を粛正 #173

Merged
merged 5 commits into from
May 10, 2024
+60 −54
Merged

1文字変数を粛正 #173

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
6c543e4
1文字変数を粛正
rein0925 May 3, 2024
a40865b
慣性モーメントの変数名を変更
rein0925 May 8, 2024
b8b02b4
コメントにも変数の変更を反映
rein0925 May 8, 2024
93eea89
コメントを基に修正
rein0925 May 10, 2024
5b49cd5
Merge branch 'main' into purge-single-letter-variable
H1rono May 10, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 3 additions & 3 deletions include/schneider_model.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,12 +42,12 @@ class Schneider {
/**
* @brief 入力値
*/
std::array<float, 4> q;
std::array<float, 4> inputs;
H1rono marked this conversation as resolved.
Show resolved Hide resolved

/**
* @brief qに対しての出力
* @brief inputsに対しての出力
*/
std::array<float, 3> x;
std::array<float, 3> outputs;
H1rono marked this conversation as resolved.
Show resolved Hide resolved

/**
* @brief ヤコビ行列の計算を行う関数\nヤコビ行列は、入力からモータの出力を計算するための行列
Expand Down
108 changes: 57 additions & 51 deletions src/schneider_model.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,15 +21,15 @@ constexpr float schneider_PI = 3.1415927F;
* @brief z軸周りの慣性モーメント
* @note もっと正確な値の方がいいかも
*/
constexpr float I = 1.0F; // NOLINT: FIXME
constexpr float inertia_z = 1;
/**
* @brief ステップ幅
*/
constexpr float a = 0.1F; // NOLINT: FIXME
constexpr float step_width_a = 0.1;

Schneider::Schneider() :
q(),
x(),
inputs(),
outputs(),
adcIn1(A4),
adcIn2(A5),
volume(A6),
Expand All @@ -55,11 +55,11 @@ Schneider::~Schneider() {

void Schneider::init() {
using std::fill;
constexpr float initialQ = 0.01F;
constexpr float initialX = 0.0F;
constexpr float initialInputs = 0.01F;
constexpr float initialOutputs = 0.0F;

fill(this->q.begin(), this->q.end(), initialQ);
fill(this->x.begin(), this->x.end(), initialX);
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();
if (whoami) {
Expand Down Expand Up @@ -88,8 +88,8 @@ void Schneider::one_step() {
// ボリュームの値を読み取る
const float volume_value = volume.read();

this->q[0] = 0;
this->q[1] = 0;
this->inputs[0] = 0;
this->inputs[1] = 0;

const bool joyEffective = abs(joy[0]) > joy_min && abs(joy[1]) > joy_min;
const bool volumeEffective = volume_value < volume_under || volume_over < volume_value;
Expand Down Expand Up @@ -133,25 +133,25 @@ inline std::array<std::array<float, 3>, 4> Schneider::cal_tjacob() const {
using std::cos;
using std::sin;
std::array<std::array<float, 3>, 4> t_jacobianmatrix;
t_jacobianmatrix[0][0] = cos(this->q[2]);
t_jacobianmatrix[0][1] = sin(this->q[2]);
t_jacobianmatrix[0][2] = (a + sin(this->q[2])) / I;
t_jacobianmatrix[1][0] = cos(this->q[3]);
t_jacobianmatrix[1][1] = sin(this->q[3]);
t_jacobianmatrix[1][2] = (-a - sin(this->q[3])) / I;
t_jacobianmatrix[2][0] = -this->q[0] * sin(this->q[2]);
t_jacobianmatrix[2][1] = this->q[0] * cos(this->q[2]);
t_jacobianmatrix[2][2] = this->q[0] * cos(this->q[2]) / I;
t_jacobianmatrix[3][0] = -this->q[1] * sin(this->q[3]);
t_jacobianmatrix[3][1] = this->q[1] * cos(this->q[3]);
t_jacobianmatrix[3][2] = -this->q[1] * cos(this->q[3]) / I;
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<float, 3>& joy) -> void {
using std::pow;
// ステップ幅
constexpr float e = 0.01F; // NOLINT: FIXME
constexpr float step_width_e = 0.01;
// 試行回数
constexpr size_t trial_num = 1000U;

Expand All @@ -161,7 +161,7 @@ auto Schneider::cal_q(const std::array<float, 3>& joy) -> void {
: (joy[0] < 0 && joy[1] >= 0) ? 3
: 5;
for (int i = 2; i < 4; ++i) {
this->q[i] = coef * schneider_PI / 4;
this->inputs[i] = coef * schneider_PI / 4;
}

trace::toggle(LedId::Second);
Expand All @@ -171,21 +171,22 @@ auto Schneider::cal_q(const std::array<float, 3>& joy) -> void {

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);
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<std::array<float, 3>, 4> t_jacobianmatrix = this->cal_tjacob();
for (int j = 0; j < 4; j++) {
for (int k = 0; k < 3; k++) {
this->q[j] -= e * t_jacobianmatrix[j][k] * (this->x[k] - joy[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->q[j] -= pow(2 * this->q[j] - 1, biasOrder);
this->inputs[j] -= pow(2 * this->inputs[j] - 1, biasOrder);
}
}
}
Expand All @@ -195,46 +196,51 @@ auto Schneider::cal_q(const std::array<float, 3>& joy) -> void {
inline void Schneider::state_equation() {
using std::cos;
using std::sin;
this->x[0] = this->q[0] * cos(this->q[2]) + this->q[1] * cos(this->q[3]);
this->x[1] = this->q[0] * sin(this->q[2]) + this->q[1] * sin(this->q[3]);
this->x[2] = (a * (this->q[0] - this->q[1]) + this->q[0] * sin(this->q[2])
- this->q[1] * sin(this->q[3]))
/ I;
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;
}

void Schneider::set_q(const std::array<float, 3>& gyro) {
using std::abs;
// 系への入力値の実効下限値
constexpr float input_min = 0.4F;

if (abs(this->q[0] <= input_min)) {
this->q[0] = 0;
if (abs(this->inputs[0] <= input_min)) {
this->inputs[0] = 0;
}
if (abs(this->q[1] <= input_min)) {
this->q[1] = 0;
if (abs(this->inputs[1] <= input_min)) {
this->inputs[1] = 0;
}
this->fet_1 = this->q[0];
this->fet_2 = this->q[1];
this->fet_1 = this->inputs[0];
this->fet_2 = this->inputs[1];

while (this->q[2] >= schneider_PI) {
this->q[2] -= 2 * schneider_PI;
while (this->inputs[2] >= schneider_PI) {
this->inputs[2] -= 2 * schneider_PI;
}
while (this->q[3] >= schneider_PI) {
this->q[3] -= 2 * schneider_PI;
while (this->inputs[3] >= schneider_PI) {
this->inputs[3] -= 2 * schneider_PI;
}
while (this->q[2] < -schneider_PI) {
this->q[2] += 2 * schneider_PI;
while (this->inputs[2] < -schneider_PI) {
this->inputs[2] += 2 * schneider_PI;
}
while (this->q[3] < -schneider_PI) {
this->q[3] += 2 * schneider_PI;
while (this->inputs[3] < -schneider_PI) {
this->inputs[3] += 2 * schneider_PI;
}

if (0 < this->q[2] && this->q[2] < schneider_PI) {
const int width = static_cast<int>(500 + 1900 / schneider_PI * this->q[2] - 2200 * gyro[2]);
if (0 < this->inputs[2] && this->inputs[2] < schneider_PI) {
const int width
= static_cast<int>(500 + 1900 / schneider_PI * this->inputs[2] - 2200 * gyro[2]);
this->servo_1.pulsewidth_us(width);
}
if (0 < this->q[3] && this->q[3] < schneider_PI) {
const int width = static_cast<int>(500 + 1900 / schneider_PI * this->q[3] + 2200 * gyro[2]);
if (0 < this->inputs[3] && this->inputs[3] < schneider_PI) {
const int width
= static_cast<int>(500 + 1900 / schneider_PI * this->inputs[3] + 2200 * gyro[2]);
this->servo_2.pulsewidth_us(width);
}
}
Expand Down