Merge pull request #134 from ut-ras/sentry_movement

Improve basic sentry movement
ut-ras · Jun 17, 2024 · d8eac2a · d8eac2a
2 parents fa1ae85 + 10d6f97
commit d8eac2a
Show file tree

Hide file tree

Showing 3 changed files with 22 additions and 2 deletions.
diff --git a/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp b/ut-robomaster/src/subsystems/chassis/command_sentry_position.cpp
@@ -3,13 +3,24 @@
 namespace commands
 {
 
-void CommandSentryPosition::initialize() {}
+void CommandSentryPosition::initialize() { startup_time = getTimeMilliseconds(); }
 
 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;
@@ -31,6 +42,11 @@ void CommandSentryPosition::execute()
 
  spin = 1.0f;
 
+ if (getTimeMilliseconds() - startup_time < 35000.0f)
+ {
+ spin = 0.0f;
+ }
+
  chassis->input(move, spin);
 }
 

diff --git a/ut-robomaster/src/subsystems/chassis/command_sentry_position.hpp b/ut-robomaster/src/subsystems/chassis/command_sentry_position.hpp
@@ -38,6 +38,7 @@ class CommandSentryPosition : public tap::control::Command
  ChassisSubsystem *chassis;
  // bool isStarted = false;
  // float startTime = 0.0f;
+ float startup_time = 0.0f;
 };
 } // namespace commands
 

diff --git a/ut-robomaster/src/subsystems/turret/command_sentry_aim.cpp b/ut-robomaster/src/subsystems/turret/command_sentry_aim.cpp
@@ -4,7 +4,10 @@ namespace commands
 {
 void CommandSentryAim::initialize() {}
 
-void CommandSentryAim::execute() { turret->setTargetWorldAngles(turret->getChassisYaw(), 0.0f); }
+void CommandSentryAim::execute()
+{
+ // turret->setTargetWorldAngles(turret->getChassisYaw(), 0.0f);
+}
 
 void CommandSentryAim::end(bool) {}