Refactored launcher code.

FRC830 · Feb 18, 2024 · dcc82da · dcc82da
1 parent f35c963
commit dcc82da
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 13 deletions.
diff --git a/src/main/cpp/subsystems/LauncherHAL.cpp b/src/main/cpp/subsystems/LauncherHAL.cpp
@@ -26,27 +26,29 @@ LauncherHAL::LauncherHAL()
     configs.Slot0.kI = FLYWHEEL_I;
     configs.Slot0.kD = FLYWHEEL_D;
 
-    m_FlywheelActMotorA.GetConfigurator().Apply(configs);
-    m_FlywheelActMotorB.GetConfigurator().Apply(configs);
+    m_FlywheelBottom.SetControl(m_FlywheelTopFollower);
 
-    m_FlywheelActMotorB.SetControl(m_FlywheelActMtrBFollower);
+    m_FlywheelTop.SetInverted(INVERT_FLYWHEEL);
+
+    m_FlywheelTop.GetConfigurator().Apply(configs);
+    m_FlywheelBottom.GetConfigurator().Apply(configs);
 
     m_PvtMotor.BurnFlash();
     m_IndMotor.BurnFlash();
 }
 
 void LauncherHAL::SetFlywheelSpeed(double speed)
 {   
-    if (speed > 1.0)
+    if (std::fabs(speed) > 1.0)
     {
         units::angular_velocity::turns_per_second_t speed_tps = units::angular_velocity::turns_per_second_t(speed);
         ctre::phoenix6::controls::VelocityVoltage m_request{speed_tps};
-        m_FlywheelActMotorA.SetControl(m_request.WithVelocity(speed_tps));
+        m_FlywheelTop.SetControl(m_request.WithVelocity(speed_tps));
     }
     else
     {
         ctre::phoenix6::controls::DutyCycleOut m_request{0.0};
-        m_FlywheelActMotorA.SetControl(m_request.WithOutput(0.0));
+        m_FlywheelTop.SetControl(m_request.WithOutput(0.0));
     }
 }
 
@@ -121,7 +123,7 @@ double LauncherHAL::GetAngle()
 
 double LauncherHAL::GetFlywheelSpeed()
 {
-    return m_FlywheelActMotorA.GetVelocity().GetValueAsDouble();
+    return m_FlywheelTop.GetVelocity().GetValueAsDouble();
 }
 
 void LauncherHAL::ResetProfiledMoveState()

diff --git a/src/main/include/subsystems/LauncherHAL.h b/src/main/include/subsystems/LauncherHAL.h
@@ -25,14 +25,14 @@ class LauncherHAL
     private:
         rev::CANSparkMax m_PvtMotor{LAUNCHER_PVT_MTR_ID, rev::CANSparkMax::MotorType::kBrushless};
         rev::CANSparkMax m_IndMotor{LAUNCHER_IND_MTR_ID, rev::CANSparkMax::MotorType::kBrushless};
-        ctre::phoenix6::hardware::TalonFX m_FlywheelActMotorA{FLYWHEEL_ACT_MTR_A_ID};
-        ctre::phoenix6::hardware::TalonFX m_FlywheelActMotorB{FLYWHEEL_ACT_MTR_B_ID};
+        ctre::phoenix6::hardware::TalonFX m_FlywheelTop{FLYWHEEL_TOP_ID};
+        ctre::phoenix6::hardware::TalonFX m_FlywheelBottom{FLYWHEEL_BOTTOM_ID};
 
         rev::SparkRelativeEncoder m_PvtRelEncoder = m_PvtMotor.GetEncoder(rev::SparkRelativeEncoder::Type::kHallSensor, 42);
         rev::SparkPIDController m_PvtPID = m_PvtMotor.GetPIDController();
         rev::SparkAbsoluteEncoder m_PvtAbsEncoder= m_PvtMotor.GetAbsoluteEncoder(rev::SparkAbsoluteEncoder::Type::kDutyCycle);
 
-        ctre::phoenix6::controls::Follower m_FlywheelActMtrBFollower = ctre::phoenix6::controls::Follower(FLYWHEEL_ACT_MTR_B_ID, true);
+        ctre::phoenix6::controls::Follower m_FlywheelTopFollower = ctre::phoenix6::controls::Follower(FLYWHEEL_TOP_ID, true);
 
         frc::Timer m_Timer;
 

diff --git a/src/main/include/subsystems/SubSystemConfig.h b/src/main/include/subsystems/SubSystemConfig.h
@@ -44,8 +44,8 @@ const bool LFT_PVT_MTR_INVERTED = true;
 // ###########################################################
 // #                        Flywheel                         #
 // ###########################################################
-const int FLYWHEEL_ACT_MTR_A_ID = 4;
-const int FLYWHEEL_ACT_MTR_B_ID = 5;
+const int FLYWHEEL_TOP_ID = 22;
+const int FLYWHEEL_BOTTOM_ID = 23;
 const int LAUNCHER_PVT_MTR_ID = 6;
 const int LAUNCHER_IND_MTR_ID = 7;
 
@@ -62,5 +62,5 @@ const double PVT_D = 0.0;
 const int LAUNCHER_PVT_CURRENT_LIMIT = 20;
 const int FLYWHEEL_ACT_CURRENT_LIMIT = 20;
 const int LAUNCHER_IND_CURRENT_LIMIT = 20;
-// do we need cancoders for the falcons? 
 
+const bool INVERT_FLYWHEEL = true;