Skip to content

Commit

Permalink
Merge pull request #173 from rogy-AquaLab/purge-single-letter-variable
Browse files Browse the repository at this point in the history 
1文字変数を粛正
  • Loading branch information
@H1rono
H1rono authored May 10, 2024
2 parents ef4202a + 5b49cd5 commit efe83cf
Show file tree
Hide file tree
Showing 2 changed files with 60 additions and 54 deletions.
6 changes: 3 additions & 3 deletions include/schneider_model.hpp
View file
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;

/**
* @brief qに対しての出力
* @brief inputsに対しての出力
*/
std::array<float, 3> x;
std::array<float, 3> outputs;

/**
* @brief ヤコビ行列の計算を行う関数\nヤコビ行列は、入力からモータの出力を計算するための行列
Expand Down
108 changes: 57 additions & 51 deletions src/schneider_model.cpp
View file
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

0 comments on commit efe83cf

Please sign in to comment.