diff --git a/ut-robomaster/src/drivers.hpp b/ut-robomaster/src/drivers.hpp index fba71b7..8550e3a 100644 --- 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 index 8f87ce2..7feef19 100644 --- 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 \ No newline at end of file +} // namespace commands diff --git a/ut-robomaster/src/subsystems/chassis/command_sentry_position.hpp b/ut-robomaster/src/subsystems/chassis/command_sentry_position.hpp index 0992000..69aa265 100644 --- 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 \ No newline at end of file