Added and fixed hoard position.

FRC830 · Mar 22, 2024 · 4cd2c5e · 4cd2c5e
1 parent 198ff1d
commit 4cd2c5e
Show file tree

Hide file tree

Showing 5 changed files with 28 additions and 8 deletions.
diff --git a/src/main/cpp/ControllerInterface.cpp b/src/main/cpp/ControllerInterface.cpp
@@ -36,6 +36,7 @@ void ControllerInterface::UpdateLauncherInput(RobotControlData &controlData)
  controlData.launcherInput.goToStowPos = m_copilot.GetAButton();
  controlData.launcherInput.goToSubPos = m_copilot.GetYButton();
  controlData.launcherInput.runIndexerBackward = m_copilot.GetLeftTriggerAxis() >= 0.2;
+ controlData.launcherInput.goToHoardePos = m_copilot.GetBButton();
  // controlData.launcherInput.runIndexerForward = m_copilot.GetRightTriggerAxis() >= 0.2;
  //controlData.launcherInput.runIndexerForward = m_copilot.GetXButton();
  //controlData.launcherInput.runIndexerBackward = m_copilot.GetBButton();

diff --git a/src/main/cpp/subsystems/LauncherHAL.cpp b/src/main/cpp/subsystems/LauncherHAL.cpp
@@ -175,6 +175,7 @@ void LauncherHAL::SetIndexerSpeed(double speed)
  m_IndMotor.Set(m_indexerSpeed);
 }
 
+
 void LauncherHAL::SetAngle(double angle)
 {
  if (angle > MAX_PIVOT_ANGLE)
@@ -191,19 +192,15 @@ 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: 
  {
  m_ProfileStartPos = GetAngle();
 
- m_Timer.Restart();
+ m_Timer.Stop();
+ m_Timer.Reset();
+ m_Timer.Start();
 
  m_profileState++;
 
@@ -259,5 +256,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
@@ -5,6 +5,7 @@ void LauncherManager::ResetLauncher()
 {
  m_goToStowPos = false;
  m_goToSubPos = false;
+ m_goToHoardePos = false;
  m_visionResetProfiledMoveState = false;
 }
 
@@ -19,6 +20,7 @@ void LauncherManager::HandleInput(LauncherInput &launcherInput, LauncherOutput &
  m_visionResetProfiledMoveState = true;
  m_goToStowPos = false;
  m_goToSubPos = false;
+ m_goToHoardePos = false;
  }
 
  m_launcher.SetFlywheelSpeed(launcherInput.visionSpeedSetpoint);
@@ -37,13 +39,23 @@ void LauncherManager::HandleInput(LauncherInput &launcherInput, LauncherOutput &
  if(launcherInput.goToStowPos){
  m_goToStowPos = true;
  m_goToSubPos = false;
+ m_goToHoardePos = false;
  m_launcher.ResetProfiledMoveState();
  }
  else if (launcherInput.goToSubPos) {
  m_goToStowPos = false;
  m_goToSubPos = true;
+ m_goToHoardePos = false;
 
  m_launcher.ResetProfiledMoveState();
+ } else if (launcherInput.goToHoardePos) {
+
+ m_goToHoardePos = true; 
+ m_goToStowPos = false;
+ m_goToSubPos = false;
+
+ m_launcher.ResetProfiledMoveState();
+
  }
 
  if (m_goToStowPos)
@@ -64,6 +76,12 @@ void LauncherManager::HandleInput(LauncherInput &launcherInput, LauncherOutput &
  m_launcher.ProfiledMoveToAngle(SUB_ANGLE);
  m_launcher.SetFlywheelSpeed(SUB_SPEED);
  }
+ if (m_goToHoardePos) {
+
+ m_launcher.ProfiledMoveToAngle(HOARDE_ANG);
+ m_launcher.SetFlywheelSpeed(HOARDE_SPD);
+
+ }
  }
 
  if (launcherInput.runIndexerBackwardSlow)

diff --git a/src/main/include/subsystems/SubSystemConfig.h b/src/main/include/subsystems/SubSystemConfig.h
@@ -79,7 +79,9 @@ const double DOWN_MAX_ACC = 200.0;
 const double MAX_JERK = 1250.0;
 
 const double SUB_ANGLE = 55.0;
+const double HOARDE_ANG = 40.0;
 const double STOW_ANGLE = 10.0;
 const double SUB_SPEED = 150.0;
+const double HOARDE_SPD = 70.0;
 const double INDEXER_SPEED = 1.0;
 const double INDEXER_SLOW_SPEED = 0.15;
diff --git a/src/main/include/subsystems/subsystemsManagers/LauncherManager.h b/src/main/include/subsystems/subsystemsManagers/LauncherManager.h
@@ -14,6 +14,7 @@ struct LauncherInput
  bool runIndexerForward; //Indexer is the little "kick" to help the Note transfer from the Intake to the Launcher
  bool runIndexerBackward;
  bool runIndexerBackwardSlow;
+ bool goToHoardePos;
 };
 
 struct LauncherOutput
@@ -32,5 +33,6 @@ class LauncherManager
  LauncherHAL m_launcher;
  bool m_goToStowPos;
  bool m_goToSubPos;
+ bool m_goToHoardePos;
  bool m_visionResetProfiledMoveState = false;
 };