diff --git a/src/main/cpp/subsystems/LauncherHAL.cpp b/src/main/cpp/subsystems/LauncherHAL.cpp index 4998875..13b9f50 100644 --- a/src/main/cpp/subsystems/LauncherHAL.cpp +++ b/src/main/cpp/subsystems/LauncherHAL.cpp @@ -118,6 +118,7 @@ LauncherHAL::LauncherHAL() m_FlywheelTop.GetConfigurator().Apply(configs); m_FlywheelBottom.GetConfigurator().Apply(configs); + m_currentAngle = -100.0; } void LauncherHAL::SetFlywheelSpeed(double speed) @@ -172,6 +173,12 @@ void LauncherHAL::SetAngle(double angle) void LauncherHAL::ProfiledMoveToAngle(double angle) { + if (std::fabs(m_currentAngle - angle) >= 1e-8) + { + m_profileState = 0; + m_currentAngle = angle; + } + switch(m_profileState) { case 0: @@ -234,5 +241,5 @@ double LauncherHAL::GetFlywheelSpeed() void LauncherHAL::ResetProfiledMoveState() { - m_profileState = 0; + // m_profileState = 0; } \ No newline at end of file diff --git a/src/main/cpp/subsystems/subsystemManagers/LauncherManager.cpp b/src/main/cpp/subsystems/subsystemManagers/LauncherManager.cpp index a445d0d..121fe21 100644 --- a/src/main/cpp/subsystems/subsystemManagers/LauncherManager.cpp +++ b/src/main/cpp/subsystems/subsystemManagers/LauncherManager.cpp @@ -7,6 +7,7 @@ void LauncherManager::ResetLauncher() m_goToSubPos = false; m_visionResetProfiledMoveState = false; } + void LauncherManager::HandleInput(LauncherInput &launcherInput, LauncherOutput &launcherOutput, IntakeInput &intakeInput, IntakeOutput &intakeOutput) { diff --git a/src/main/include/subsystems/LauncherHAL.h b/src/main/include/subsystems/LauncherHAL.h index c6c862a..34dda6c 100644 --- a/src/main/include/subsystems/LauncherHAL.h +++ b/src/main/include/subsystems/LauncherHAL.h @@ -44,6 +44,6 @@ class LauncherHAL double m_ProfileStartPos; double m_indexerSpeed; int m_profileState = 0; - + double m_currentAngle; void SetAngle(double angle); }; \ No newline at end of file