Merge branch 'develop' into encoder-changes

ut-ras · Jan 24, 2025 · f8f0f68 · f8f0f68
2 parents 882b481 + 8993ccf
commit f8f0f68
Show file tree

Hide file tree

Showing 3 changed files with 24 additions and 48 deletions.
diff --git a/ut-robomaster/src/drivers.hpp b/ut-robomaster/src/drivers.hpp
@@ -1,5 +1,6 @@
 #pragma once
 
+#include "tap/communication/serial/ref_serial_data.hpp"
 #include "tap/drivers.hpp"
 
 #include "communication/cv_board.hpp"
@@ -15,6 +16,12 @@ class Drivers : public tap::Drivers
     communication::RttStream rtt;
 
     bool isKillSwitched() { return !remote.isConnected(); }
+    bool isGameActive()
+    {
+        return this->refSerial.getGameData().gameStage ==
+               tap::communication::serial::RefSerialData::Rx::GameStage::IN_GAME;
+    }
+
 };  // class Drivers
 
 }  // namespace src
diff --git a/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp b/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp
@@ -3,54 +3,27 @@
 namespace commands
 {
 
-void CommandSentryPosition::initialize() { startup_time = getTimeMilliseconds(); }
+void CommandSentryPosition::initialize() { moveTimer.stop(); }
 
 void CommandSentryPosition::execute()
 {
-    Vector2f move = Vector2f(0.0f);
-    float spin = 0.0f;
-
-    // if (prevTime == 0.0f)
-    // {
-    //     prev_time = getTimeMilliseconds();
-    // }
-
-    // if (getTimeMilliseconds() - prev_time > 5000.0f)
-    // {
-    //     direction *= -1;
-    //     prev_time = getTimeMilliseconds();
-    // }
-
-    // if (!isStarted)
-    // {
-    //     isStarted = true;
-    //     startTime = getTimeMilliseconds() / 1000.0f;
-    // }
-
-    // float time = getTimeMilliseconds() / 1000.0f - startTime;
-
-    // if (time < 2.0f)
-    // {
-    //     float t = time / 2.0f;
-    //     float fac = 4.0f * (1.0f - t) * t;
-    //     move = Vector2f(0.0f, fac * 0.1f);
-    // }
-    // else if (time < 3.0f)
-    // {
-    //     spin = 0.2f;
-    // }
-
-    spin = 1.0f;
+    // wait until game starts then wait some more to avoid excess drifting
+    if (drivers->isGameActive() && moveTimer.isStopped())
+    {
+        moveTimer.restart(10'000);  // 10s
+    }
 
-    if (getTimeMilliseconds() - startup_time < 35000.0f)
+    if (!moveTimer.isExpired())
     {
-        spin = 0.0f;
+        chassis->input(Vector2f(0.0f), 0.0f);
+        return;
     }
 
-    chassis->input(move, spin);
+    // speen
+    chassis->input(Vector2f(0.0f), 1.0f);
 }
 
 void CommandSentryPosition::end(bool) { chassis->input(Vector2f(0.0f), 0.0f); }
 bool CommandSentryPosition::isFinished() const { return false; }
 
-}  // namespace commands
+}  // namespace commands
diff --git a/ut-robomaster/src/subsystems/chassis/command_sentry_position.hpp b/ut-robomaster/src/subsystems/chassis/command_sentry_position.hpp
@@ -1,6 +1,6 @@
-#ifndef COMMAND_SENTRY_POSITION_HPP_
-#define COMMAND_SENTRY_POSITION_HPP_
+#pragma once
 
+#include "tap/architecture/timeout.hpp"
 #include "tap/control/command.hpp"
 
 #include "chassis_subsystem.hpp"
@@ -11,7 +11,7 @@ namespace commands
 using namespace tap::communication::serial;
 using namespace modm;
 using subsystems::chassis::ChassisSubsystem;
-using tap::arch::clock::getTimeMilliseconds;
+using tap::arch::MilliTimeout;
 
 class CommandSentryPosition : public tap::control::Command
 {
@@ -36,10 +36,6 @@ class CommandSentryPosition : public tap::control::Command
 private:
     src::Drivers *drivers;
     ChassisSubsystem *chassis;
-    // bool isStarted = false;
-    // float startTime = 0.0f;
-    float startup_time = 0.0f;
+    MilliTimeout moveTimer;
 };
-}  // namespace commands
-
-#endif
+}  // namespace commands