#KL26-142 目標地点に向かって移動する動作クラスの作成 by yutaro-1214 · Pull Request #36 · KatLab-MiyazakiUniv/etrobocon2026

yutaro-1214 · 2026-07-18T07:40:28Z

チェックリスト

clang-format している
コーディング規約に準じている
チケットの完了条件を満たしている

変更点

Positionクラス（自己位置を保持するクラス）
Odometryクラス（モーターの動きから自己位置を更新するクラス）
Navigatorクラス（自分の位置から目標地点への距離と方向を計算するクラス）
GoalNavigationクラス（目標地点へ回頭してから移動する動作クラス）
の追加

それに伴うRobotクラスの更新

Pidクラスにreset()（PIDの内部状態をリセットする）コマンドを追加
（テストはまだです）

動作テスト

実験方法

実験結果

添付資料

https://app.notion.com/p/uom-katlab/3acdd5b1cc188010a209fe2c36806d81?source=copy_link

takuchi17

オドメトリの更新周りが主です。

あと、回頭直進は既に作ってる動作クラスを使ってやる。

takuchi17 · 2026-07-23T05:03:56Z

datafilesは変更する必要あった？なければ変更なしで。

takuchi17 · 2026-07-23T05:04:15Z

変更しない

takuchi17 · 2026-07-23T05:04:44Z

+      double tolerance = fromString<double>(params[3]);
+      return make_unique<RelativeAngleCondition>(robot, targetAngle, tolerance);
+    }
    // ↓ 他の条件コマンドはここに追加していく


この行は消しちゃっていい

takuchi17 · 2026-07-23T05:05:09Z

      double targetDistance = fromString<double>(params[2]);
      return make_unique<DistanceCondition>(robot, targetDistance);
    }
+    case CONDITION_COMMAND::ABSOLUTEANGLE: {


Suggested change

case CONDITION_COMMAND::ABSOLUTEANGLE: {

case CONDITION_COMMAND::ABSOLUTE_ANGLE: {

takuchi17 · 2026-07-23T05:05:19Z

+      double tolerance = fromString<double>(params[3]);
+      return make_unique<AbsoluteAngleCondition>(robot, targetAngle, tolerance);
+    }
+    case CONDITION_COMMAND::RELATIVEANGLE: {


Suggested change

case CONDITION_COMMAND::RELATIVEANGLE: {

case CONDITION_COMMAND::RELATIVE_ANGLE: {

takuchi17 · 2026-07-23T05:07:44Z

+  enum class MOTION_COMMAND { STRAIGHT, ABSOLUTEROTATION, RELATIVEROTATION, NONE };
  // 条件コマンド名を持つ列挙型クラス
-  enum class CONDITION_COMMAND { DISTANCE, NONE };
+  enum class CONDITION_COMMAND { DISTANCE, ABSOLUTEANGLE, RELATIVEANGLE, NONE };


Suggested change

enum class CONDITION_COMMAND { DISTANCE, ABSOLUTEANGLE, RELATIVEANGLE, NONE };

enum class CONDITION_COMMAND { DISTANCE, ABSOLUTE_ANGLE, RELATIVE_ANGLE, NONE };

takuchi17 · 2026-07-23T05:11:23Z

+   * @param left 左エンコーダ値[deg]
+   * @param right 右エンコーダ値[deg]
+   */
+  void initialize(int32_t left, int32_t right);


leftとrightだけじゃ何かよくわからない。

takuchi17 · 2026-07-23T05:11:30Z

+   * @param right 右エンコーダ値[deg]
+   * @param heading IMU方位角[deg]
+   */
+  void update(int32_t left, int32_t right, double heading);


こっちも

takuchi17 · 2026-07-23T05:11:41Z

+  int32_t prevLeft;
+  int32_t prevRight;


takuchi17 · 2026-07-23T05:19:56Z

+  // オドメトリ更新
+  robot.getOdometry().update(robot.getWheelMotorControllerInstance().getLeftCount(),
+                             robot.getWheelMotorControllerInstance().getRightCount(),
+                             robot.getIMUControllerInstance().getAzimuth());
+
+  // 現在位置から目標方向を毎周期更新
+  targetAngle = robot.getNavigator().calculateHeading(goalX, goalY);
+
+  double currentAngle = robot.getIMUControllerInstance().getAzimuth();
+
+  double angleDeviation = AngleNormalizer::normalizeAngle(targetAngle - currentAngle);
+
+  switch(state) {
+    //--------------------------------------------------
+    // 回頭
+    //--------------------------------------------------
+    case State::ROTATE: {
+      double turningPower = anglePid.calculatePid(angleDeviation);
+
+      robot.getWheelMotorControllerInstance().setRightPower(turningPower);
+      robot.getWheelMotorControllerInstance().setLeftPower(-turningPower);
+
+      // 十分向けたら直進へ
+      if(std::fabs(angleDeviation) <= ANGLE_TOLERANCE) {
+        anglePid.reset();  // PIDの内部状態をリセット
+        state = State::STRAIGHT;
+      }
+
+      break;
+    }
+
+    //--------------------------------------------------
+    // 直進
+    //--------------------------------------------------
+    case State::STRAIGHT: {
+      double requiredRightPower = speedCalculator.calculateRightMotorPower();
+
+      double requiredLeftPower = speedCalculator.calculateLeftMotorPower();
+
+      double turningPower = anglePid.calculatePid(angleDeviation);
+
+      robot.getWheelMotorControllerInstance().setRightPower(requiredRightPower + turningPower);
+
+      robot.getWheelMotorControllerInstance().setLeftPower(requiredLeftPower - turningPower);
+
+      break;
+    }
+  }


prepareで回頭角とか直進距離を算出して、あとはStraightクラスとRotationクラスを使って順番に動作すればよくない？

オドメトリの更新は全動作クラスで行う方針だし。
となるとBaseMotionでオドメトリの更新処理を書く必要あり。

miyahara046

ごめんなさい中身がほとんどないレビューです。

miyahara046 · 2026-07-23T05:52:52Z

+  double goalX;      ///< 目標X座標(mm)
+  double goalY;      ///< 目標Y座標(mm)
+  double tolerance;  ///< 到達判定距離(mm)


///<この形でのコメントにしてる意図が知りたい

だいたいchatGPTで作ったので普通にミスです。

miyahara046 · 2026-07-23T06:31:35Z

+      robot.getWheelMotorControllerInstance().setRightPower(turningPower);
+      robot.getWheelMotorControllerInstance().setLeftPower(-turningPower);
+
+      // 十分向けたら直進へ


// 目標方位に達したらとかじゃない？

miyahara046 · 2026-07-23T06:32:51Z

+  /// 回頭・方位補正用PID
+  Pid anglePid;
+
+  /// 左右速度PID計算
+  SpeedCalculator speedCalculator;
+
+  /// 現在向くべき目標角度
+  double targetAngle;
+
+  /// 回頭完了と判定する角度誤差(°)
+  static constexpr double ANGLE_TOLERANCE = 2.0;


個々のコメントの書き方も他のクラスの書き方と合わせてほしい。
変数　//コメント

HaruArima08

とりあえず、細かい箇所のみです。
実際にこの動作クラスを使って走らせた動画があれば、そのリンクを貼ってくれると嬉しいです。

HaruArima08 · 2026-07-23T05:48:01Z

これで暫定のPIDは決まった感じ？

HaruArima08 · 2026-07-23T05:55:17Z

+ public:
+  /**
+   * @brief コンストラクタ
+   * @param position 更新対象


Suggested change

* @param position 更新対象

* @param position 更新する位置情報

HaruArima08 · 2026-07-23T05:56:27Z

+  void update(int32_t left, int32_t right, double heading);
+
+ private:
+  Position& position;


コメント入れる

HaruArima08 · 2026-07-23T05:57:01Z

+#include <cmath>
+#include "AngleNormalizer.h"
+#include "Mileage.h"


.hに置く

HaruArima08 · 2026-07-23T05:59:37Z

+  double calculateHeading(double goalX, double goalY) const;
+
+ private:
+  const Position& position;


コメント入れる

HaruArima08 · 2026-07-23T05:59:49Z

+
+#include "Navigator.h"
+
+#include <cmath>


.hに置く

HaruArima08 · 2026-07-23T06:00:22Z

+namespace {
+  constexpr double RAD_TO_DEG = 180.0 / M_PI;
+}


SysytemInfoに定義してるのを使う

HaruArima08 · 2026-07-23T06:02:15Z

+  double goalX;      ///< 目標X座標(mm)
+  double goalY;      ///< 目標Y座標(mm)
+  double tolerance;  ///< 到達判定距離(mm)


Suggested change

double goalX; ///< 目標X座標(mm)

double goalY; ///< 目標Y座標(mm)

double tolerance; ///< 到達判定距離(mm)

double goalX; // 目標X座標(mm)

double goalY; // 目標Y座標(mm)

double tolerance; // 到達判定距離(mm)

yutaro-1214 added 18 commits June 18, 2026 18:44

add: Position.cpp,.hとOdometry.cpp,.hの追加

4127326

add: Navigatorクラスの追加

91994da

update:EtRobocon2026.cppをテスト用に変更

f589da1

update:EtRobocon2026.cppの内容を削除

3b57b10

Merge branch 'main' into work-KL26-123-kodama

d1085b4

update:MotionParserに動作や条件を追加し、テスト用にEtRobocon2026.cppを更新

d79b5d9

update:MotionParserに動作や条件を追加し、テスト用にEtRobocon2026.cppを更新

09e13ba

update:テスト走行用コードの更新(EtRobocon2026.cpp)

ad4d83c

update:testCode

7dc96ef

update:Odometry,Navigatorの改良

7aa3edb

update:仮のコース用に変更

631fa2e

update:微細な修正

2f6ed8c

update: 自己位置推定に必要なファイルの追加とそれに伴うRobotの更新

2035f86

add: GoalNavigationクラスの追加とPIDクラスにresetPidを追加

5db02a4

add:GoalDistanceConditionの追加など

4ff17cb

update:buildが通るように改良

c140b0f

refactor:コメントの追加

9900697

update:EtRobocon2026.cppの内容を削除

1baeff2

yutaro-1214 requested a review from a team July 18, 2026 07:40

takuchi17 requested changes Jul 23, 2026

View reviewed changes

miyahara046 requested changes Jul 23, 2026

View reviewed changes

HaruArima08 requested changes Jul 23, 2026

View reviewed changes

yutaro-1214 added 7 commits July 23, 2026 17:41

update:GoalNavigationクラスの再作成と不要なファイルの削除

1393a98

update:BaseMotion内でオドメトリ更新をするように変更

f70885c

update:build通るように修正

15e807d

Merge branch 'work-KL26-142' into ticket-KL26-142

5111de3

update:datefilesの修正

1576e3c

update:コメントなどの修正

3361f40

Merge branch 'work-KL26-142' into ticket-KL26-142

46dbafa

yutaro-1214 requested a review from a team July 29, 2026 07:58

	case CONDITION_COMMAND::ABSOLUTEANGLE: {
	case CONDITION_COMMAND::ABSOLUTE_ANGLE: {

	case CONDITION_COMMAND::RELATIVEANGLE: {
	case CONDITION_COMMAND::RELATIVE_ANGLE: {

	enum class CONDITION_COMMAND { DISTANCE, ABSOLUTEANGLE, RELATIVEANGLE, NONE };
	enum class CONDITION_COMMAND { DISTANCE, ABSOLUTE_ANGLE, RELATIVE_ANGLE, NONE };

	* @param position 更新対象
	* @param position 更新する位置情報


		#include "Navigator.h"

		#include <cmath>

Uh oh!

Conversation

yutaro-1214 commented Jul 18, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

チェックリスト

変更点

動作テスト

実験方法

実験結果

添付資料

Uh oh!

takuchi17 left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

miyahara046 left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

HaruArima08 left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants

yutaro-1214 commented Jul 18, 2026 •

edited

Loading