Merge branch 'main' into Actual_Climber_Branch

src/main/cpp/ControllerInterface.cpp

@@ -68,6 +68,15 @@ void ControllerInterface::UpdateLauncherInput(RobotControlData &controlData){
     controlData.coralInput.setFlywheelToL1Speed = m_pilot.GetAButton();
     controlData.coralInput.setFlywheelToL2Speed = m_pilot.GetBButton();
     controlData.coralInput.disableFlywheels = m_pilot.GetYButton();
+
+    if (m_pilot.GetXButton())
+    {
+        controlData.coralInput.indexerSpeeds = 0.7f;
+    }
+    else
+    {
+        controlData.coralInput.indexerSpeeds = 0.0f;
+    }
 }
 
 void ControllerInterface::UpdateSmartplannerInput(RobotControlData &controlData)

src/main/cpp/HAL/CoralLauncherHAL.cpp

@@ -42,8 +42,13 @@ void CoralLauncher::SetWheelSpeeds(double rightSpeed, double leftSpeed)
 {
     m_desiredRightSpeed = rightSpeed;
     m_desiredLeftSpeed = leftSpeed;
-    m_rightMotor.GetClosedLoopController().SetReference(rightSpeed, rev::spark::SparkLowLevel::ControlType::kVelocity);
-    m_leftMotor.GetClosedLoopController().SetReference(leftSpeed, rev::spark::SparkLowLevel::ControlType::kVelocity);
+
+    // Don't use PID to go to 0 to avoid stripping belts
+    auto rightControlType = (std::fabs(rightSpeed) <= 1.0f) ? rev::spark::SparkLowLevel::ControlType::kDutyCycle : rev::spark::SparkLowLevel::ControlType::kVelocity;
+    auto leftControlType = (std::fabs(leftSpeed) <= 1.0f) ? rev::spark::SparkLowLevel::ControlType::kDutyCycle : rev::spark::SparkLowLevel::ControlType::kVelocity;
+
+    m_rightMotor.GetClosedLoopController().SetReference(rightSpeed, rightControlType);
+    m_leftMotor.GetClosedLoopController().SetReference(leftSpeed, leftControlType);
 }
 void CoralLauncher::SetIndexerSpeeds(double indexerSpeed)
 {
@@ -60,7 +65,7 @@ double CoralLauncher::GetLeftWheelSpeed()
 }
 bool CoralLauncher::AreFlywheelsAtDesiredSpeed()
 {
-    return ((abs(GetRightWheelSpeed() - m_desiredRightSpeed)<=SMALL_NUM)&&(abs(GetLeftWheelSpeed() - m_desiredLeftSpeed)<=SMALL_NUM));
+    return ((std::fabs(GetRightWheelSpeed() - m_desiredRightSpeed)<=SMALL_NUM)&&(std::fabs(GetLeftWheelSpeed() - m_desiredLeftSpeed)<=SMALL_NUM));
 }
 
 bool CoralLauncher::BeamBreakStatus()

src/main/cpp/InputManager/CoralLauncherManager.cpp

@@ -5,32 +5,38 @@ void CoralLauncherManager::HandleInput(RobotControlData &robotControlData){
     if(robotControlData.coralInput.setFlywheelToL1Speed){
         m_setFlywheelToL1Speed = true;
         m_setFlywheelToL2Speed = false;
+        m_setFlywheelToZeroSpeed = false;
     }else if (robotControlData.coralInput.setFlywheelToL2Speed){
         m_setFlywheelToL1Speed = false;
         m_setFlywheelToL2Speed = true;
+        m_setFlywheelToZeroSpeed = false;
+    }else if (robotControlData.coralInput.disableFlywheels){
+        m_setFlywheelToL1Speed = false;
+        m_setFlywheelToL2Speed = false;
+        m_setFlywheelToZeroSpeed = true;
     }
 
     if(m_setFlywheelToL1Speed){
-        m_CoralLauncher.SetWheelSpeeds(1.0,1.0); //configure speeds
+        m_CoralLauncher.SetWheelSpeeds(300.0, 500.0); //configure speeds
     }
     if(m_setFlywheelToL2Speed){
-        m_CoralLauncher.SetWheelSpeeds(1.0,1.0); //config speeds
+        m_CoralLauncher.SetWheelSpeeds(1050.0, 1050.0); //config speeds
     }
-    m_CoralLauncher.SetIndexerSpeeds(robotControlData.coralInput.indexerSpeeds);
-    if(m_CoralLauncher.BeamBreakStatus()){
-        m_CoralLauncher.SetWheelSpeeds(0,0);
+    if(m_setFlywheelToZeroSpeed){
+        m_CoralLauncher.SetWheelSpeeds(0.0,0.0);
     }
+    m_CoralLauncher.SetIndexerSpeeds(robotControlData.coralInput.indexerSpeeds);
+    // if(m_CoralLauncher.BeamBreakStatus()){
+    //     m_CoralLauncher.SetWheelSpeeds(0,0);
+    // }
     robotControlData.coralOutput.isBeamBroken = m_CoralLauncher.BeamBreakStatus();
     robotControlData.coralOutput.leftSpeed = m_CoralLauncher.GetLeftWheelSpeed();
     robotControlData.coralOutput.rightSpeed = m_CoralLauncher.GetRightWheelSpeed();
     robotControlData.coralOutput.flywheelsAtSpeed = m_CoralLauncher.AreFlywheelsAtDesiredSpeed();
-    if(robotControlData.coralInput.disableFlywheels){
-        m_CoralLauncher.SetWheelSpeeds(0,0);
-    }
-
 }
 
 void CoralLauncherManager::ResetState(){
     m_setFlywheelToL1Speed = false;
     m_setFlywheelToL2Speed = false;
+    m_setFlywheelToZeroSpeed = true;
 }

src/main/cpp/Robot.cpp

@@ -74,6 +74,7 @@ void Robot::AutonomousPeriodic() {
 void Robot::AutonomousExit() {}
 
 void Robot::TeleopInit() {
+  m_coralLauncherManager.ResetState();
   m_ClimberManager.ResetState();
 }
 
@@ -117,6 +118,8 @@ void Robot::TeleopPeriodic() {
     m_rotateToFeeder.reset();
     _swerve.Drive(_robot_control_data.swerveInput.xTranslation, _robot_control_data.swerveInput.yTranslation, _robot_control_data.swerveInput.rotation);
   }
+
+  m_coralLauncherManager.HandleInput(_robot_control_data);  
   m_ClimberManager.HandleInput(_robot_control_data);
 }
 

src/main/include/HAL/AlgaeRemoverHAL.h

@@ -29,8 +29,8 @@ class AlgaeRemover
     private:
         void SetAngle(double angle);
 
-        rev::spark::SparkMax m_armMotor{40, rev::spark::SparkMax::MotorType::kBrushless};
-        rev::spark::SparkMax m_removerMotor{41, rev::spark::SparkMax::MotorType::kBrushless};
+        rev::spark::SparkMax m_armMotor{100, rev::spark::SparkMax::MotorType::kBrushless};
+        rev::spark::SparkMax m_removerMotor{101, rev::spark::SparkMax::MotorType::kBrushless};
         double m_removerSpeed;
         frc::Timer m_Timer = frc::Timer(); 
         int m_algaeRemoverState = 0;

src/main/include/HAL/CoralLauncherHAL.h

@@ -17,11 +17,11 @@ class CoralLauncher
         bool AreFlywheelsAtDesiredSpeed();
         bool BeamBreakStatus();
     private:
-        rev::spark::SparkMax m_rightMotor{30, rev::spark::SparkMax::MotorType::kBrushless};
-        rev::spark::SparkMax m_leftMotor{31, rev::spark::SparkMax::MotorType::kBrushless};
-        rev::spark::SparkMax m_indexer1{32, rev::spark::SparkMax::MotorType::kBrushless};
-        rev::spark::SparkMax m_indexer2{33, rev::spark::SparkMax::MotorType::kBrushless};
-        frc::DigitalInput m_beam_break{34}; //moved from CoralLauncherManager; move back if needed
+        rev::spark::SparkMax m_rightMotor{43, rev::spark::SparkMax::MotorType::kBrushless};
+        rev::spark::SparkMax m_leftMotor{42, rev::spark::SparkMax::MotorType::kBrushless};
+        rev::spark::SparkMax m_indexer1{40, rev::spark::SparkMax::MotorType::kBrushless};
+        rev::spark::SparkMax m_indexer2{41, rev::spark::SparkMax::MotorType::kBrushless};
+        frc::DigitalInput m_beam_break{0}; //moved from CoralLauncherManager; move back if needed
         double m_desiredRightSpeed;
         double m_desiredLeftSpeed; 
         const double SMALL_NUM = 0.001; //change variable name accordingly; make a more suitable name                            

src/main/include/InputManager/CoralLauncherManager.h

@@ -8,7 +8,7 @@
 class CoralLauncherManager
 {
 public:
-    CoralLauncherManager();
+    CoralLauncherManager() = default;
     ~CoralLauncherManager() = default;
 
     void ResetState();
@@ -17,4 +17,5 @@ class CoralLauncherManager
     CoralLauncher m_CoralLauncher;
     bool m_setFlywheelToL1Speed = false;
     bool m_setFlywheelToL2Speed = false;
+    bool m_setFlywheelToZeroSpeed = false;
 };

src/main/include/MechanismConfig.h

@@ -19,14 +19,14 @@ namespace ratbot
             const double F = 0.0;
             const double VEL_CONV_FACTOR = 1.0;
             const double CURRENT_LIM = 30.0;
-            const bool INVERTED = false;
+            const bool INVERTED = true;
             rev::spark::SparkBaseConfig::IdleMode IDLE_MODE = rev::spark::SparkBaseConfig::IdleMode::kCoast;
         }
 
         namespace Indexer
         {
             const double CURRENT_LIM = 20.0;
-            const bool INVERTED = false;
+            const bool INVERTED = true;
             rev::spark::SparkBaseConfig::IdleMode IDLE_MODE = rev::spark::SparkBaseConfig::IdleMode::kCoast;
         } 
     }

src/main/include/Robot.h

@@ -9,7 +9,6 @@
 #include <frc/TimedRobot.h>
 #include <frc2/command/CommandPtr.h>
 #include <pathplanner/lib/commands/PathPlannerAuto.h>
-#include "PhotonVisionCamera.h"
 
 #include "ratpack/swerve/AnalogAbsoluteEncoder.h"
 #include "ratpack/swerve/NavXGyro.h"
@@ -18,13 +17,12 @@
 #include "ratpack/swerve/WPISwerveModule.h"
 #include "ratpack/swerve/WPISwerveDrive.h"
 
-  #include "InputManager/CoralLauncherManager.h"
 #include "PhotonVisionCamera.h"
-
 #include "ControllerInterface.h"
 #include "RobotControlData.h"
 #include "MoveToPose.h"
 #include "InputManager/ClimberManager.h"
+#include "InputManager/CoralLauncherManager.h"
 
 class Robot : public frc::TimedRobot {
  public:
@@ -48,8 +46,6 @@ class Robot : public frc::TimedRobot {
  private:
   double GetSwerveDeadZone();
 
-  std::optional<frc2::CommandPtr> m_autonomousCommand;
-
   static const int NUM_MODULES = 4;
 
   std::array<AnalogAbsoluteEncoder, NUM_MODULES> _abs_encoders;
@@ -62,9 +58,8 @@ class Robot : public frc::TimedRobot {
   ControllerInterface _controller_interface;
   RobotControlData _robot_control_data;
   MoveToPose m_rotateToFeeder;
+  CoralLauncherManager m_coralLauncherManager;
 
-  frc::Rotation2d ROTATION_TO_FEEDER = frc::Rotation2d(units::degree_t{90.0});
-  frc::Transform3d ROBOT_TO_CAMERA = frc::Transform3d(frc::Translation3d(5_m, 0_m, 0.5_m), frc::Rotation3d(0_rad, 0_rad, 0_rad));
   int m_state = 0;
   frc2::Command* m_auto;