Removed reset profiled move state.

FRC830 · Mar 15, 2024 · b86d1de · b86d1de
1 parent f8d1294
commit b86d1de
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 2 deletions.
diff --git 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;
 }
diff --git 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
@@ -44,6 +44,6 @@ class LauncherHAL
  double m_ProfileStartPos; 
  double m_indexerSpeed;
  int m_profileState = 0;
-
+ double m_currentAngle;
  void SetAngle(double angle);
 };