Implemented ClimberHAL, ClimberManager (#22)

* Implemented ClimberHAL, ClimberManager

* Resolved comments, changed kCoast to kBrake for climberConfig, updated CAN id of ClimberMotor to 50 (needs to set later)

* resolved some comments, started reworking climber code
-callum

* reworked/completed the climber system -callum

* changes for climber based on feedback

* [nick] Fix merge conflict

* from testing climber on robot

---------

Co-authored-by: Student <ratpack-student@gmail.com>
Co-authored-by: Me <108602466+TheSiddyBoi@users.noreply.github.com>
Co-authored-by: TheSiddyBoi <sid.pandit04@gmail.com>

src/main/cpp/ControllerInterface.cpp

@@ -6,13 +6,38 @@ void ControllerInterface::UpdateRobotControlData(RobotControlData &controlData)
     UpdateSwerveInput(controlData);
     UpdateLauncherInput(controlData);
     UpdateSmartplannerInput(controlData);
+    UpdateClimberInput(controlData);
+
     // code for the VibrateController function
     if (m_timer.Get().value()>=m_globalDuration)
     {
         m_pilot.SetRumble(frc::GenericHID::RumbleType::kLeftRumble, 0.0);
         m_pilot.SetRumble(frc::GenericHID::RumbleType::kRightRumble, 0.0);
     }
 };
+void ControllerInterface::UpdateClimberInput(RobotControlData &controlData)
+{
+    // if ((m_copilot.GetRightY() > 0.1)||(m_copilot.GetLeftY() > 0.1)){
+    //     controlData.climberInput.Unspool = true;
+    //     controlData.climberInput.Respool = false;
+    // }
+    // else if ((m_copilot.GetRightY() < -0.1)||(m_copilot.GetLeftY() < -0.1)){
+    //     controlData.climberInput.Unspool = false;
+    //     controlData.climberInput.Respool = true;
+    // }
+    // else{
+    //     controlData.climberInput.Unspool = false;
+    //     controlData.climberInput.Respool = false;
+    // }
+    if (std::fabs(m_copilot.GetRightY()) > 0.1)
+    {
+        controlData.climberInput.ClimberSpeed = m_copilot.GetRightY();
+    }
+    else
+    {
+        controlData.climberInput.ClimberSpeed = 0;
+    }
+}
 
 void ControllerInterface::UpdateSwerveInput(RobotControlData &controlData)
 {  

src/main/cpp/HAL/ClimberHAL.cpp

@@ -0,0 +1,30 @@
+#include "HAL/ClimberHAL.h"
+#include <rev/config/SparkMaxConfig.h>
+#include "ratpack/SparkMaxDebugMacro.h"
+#include "MechanismConfig.h"
+
+Climber::Climber()
+{
+    rev::spark::SparkMaxConfig climber_config{};
+    climber_config.closedLoop.SetFeedbackSensor(rev::spark::ClosedLoopConfig::FeedbackSensor::kPrimaryEncoder);
+    climber_config.closedLoop.Pidf(ratbot::ClimberConfig::P,ratbot::ClimberConfig::I,ratbot::ClimberConfig::D,ratbot::ClimberConfig::F);
+    climber_config.encoder.VelocityConversionFactor(ratbot::ClimberConfig::VEL_CONV_FACTOR);
+    climber_config.Inverted(ratbot::ClimberConfig::INVERTED);
+    climber_config.SetIdleMode(ratbot::ClimberConfig::IDLE_MODE);
+    climber_config.SmartCurrentLimit(ratbot::ClimberConfig::CURRENT_LIM);
+    climber_config.VoltageCompensation(ratbot::VOLTAGE_COMPENSATION);
+
+    START_RETRYING(CLIMBER_CONFIG)
+    m_climberMotor.Configure(climber_config, rev::spark::SparkMax::ResetMode::kResetSafeParameters, rev::spark::SparkMax::PersistMode::kPersistParameters);
+    END_RETRYING    
+}
+
+void Climber::SetClimberSpeed(double speed)
+{
+    m_climberMotor.Set(speed);
+}
+
+double Climber::GetClimberPosition()
+{
+    return m_climberMotor.GetEncoder().GetPosition();
+}

src/main/cpp/InputManager/ClimberManager.cpp

@@ -0,0 +1,16 @@
+#include "InputManager/ClimberManager.h"
+
+
+void ClimberManager::HandleInput(RobotControlData &robotControlData)
+{
+
+    if(m_matchTimer.HasElapsed(105_s))
+    {
+        m_Climber.SetClimberSpeed(robotControlData.climberInput.ClimberSpeed);
+    }
+}
+
+void ClimberManager::ResetState(){
+    m_matchTimer.Reset();
+    m_matchTimer.Start();
+}

src/main/cpp/Robot.cpp

@@ -75,6 +75,7 @@ void Robot::AutonomousExit() {}
 
 void Robot::TeleopInit() {
   m_coralLauncherManager.ResetState();
+  m_ClimberManager.ResetState();
 }
 
 void Robot::TeleopPeriodic() {
@@ -117,7 +118,9 @@ 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_coralLauncherManager.HandleInput(_robot_control_data);  
+  m_ClimberManager.HandleInput(_robot_control_data);
 }
 
 void Robot::TeleopExit() {}

src/main/include/ControllerInterface.h

@@ -14,6 +14,7 @@ class ControllerInterface
         void UpdateSwerveInput(RobotControlData &controlData);
         void UpdateLauncherInput(RobotControlData &controlData);
         void UpdateSmartplannerInput(RobotControlData &controlData);
+        void UpdateClimberInput(RobotControlData &controlData);
 
         frc::XboxController m_pilot{0};
         frc::XboxController m_copilot{1};

src/main/include/HAL/ClimberHAL.h

@@ -0,0 +1,28 @@
+#pragma once
+#include "RobotControlData.h"
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc/DigitalInput.h>
+#include <rev/SparkMax.h>
+#include <rev/SparkAbsoluteEncoder.h>
+#include <rev/SparkClosedLoopController.h>
+#include <frc/trajectory/TrapezoidProfile.h>
+#include <units/length.h>
+#include <units/velocity.h>
+#include <units/angle.h>
+#include <units/acceleration.h>
+#include <units/angular_velocity.h>
+#include <units/angular_acceleration.h>
+
+class Climber
+{
+    public:
+        Climber();
+        ~Climber() = default; 
+        double GetClimberPosition();
+        void SetClimberSpeed(double speed); 
+
+    private:
+        rev::spark::SparkMax m_climberMotor{30, rev::spark::SparkMax::MotorType::kBrushless};
+        rev::spark::SparkClosedLoopController m_climberMotorPID = m_climberMotor.GetClosedLoopController();
+        rev::spark::SparkAbsoluteEncoder m_climberMotorAbsEncoder = m_climberMotor.GetAbsoluteEncoder();                      
+};

src/main/include/InputManager/ClimberManager.h

@@ -0,0 +1,18 @@
+#pragma once
+
+#include "HAL/ClimberHAL.h"
+#include "RobotControlData.h"
+#include <frc/Timer.h>
+
+
+class ClimberManager
+{
+public:
+    ClimberManager() = default;
+    ~ClimberManager() = default;
+    void ResetState();
+    void HandleInput(RobotControlData &robotControlData);
+private:
+    Climber m_Climber;
+    frc::Timer m_matchTimer;
+};

src/main/include/MechanismConfig.h

@@ -30,6 +30,21 @@ namespace ratbot
             rev::spark::SparkBaseConfig::IdleMode IDLE_MODE = rev::spark::SparkBaseConfig::IdleMode::kCoast;
         } 
     }
+
+    namespace ClimberConfig
+    {
+        const double MAX_ANGLE = 90.0;
+        const double MIN_ANGLE = 0.0;
+        const double P = 0.00025;
+        const double I = 0.0;
+        const double D = 0.35;
+        const double F = 0.0;
+        const double VEL_CONV_FACTOR = 1.0;
+        const double CURRENT_LIM = 30.0;
+        const bool INVERTED = false;
+        rev::spark::SparkBaseConfig::IdleMode IDLE_MODE = rev::spark::SparkBaseConfig::IdleMode::kBrake;
+    }
+
     namespace IntakeConfig
     {
         frc::Rotation2d ROTATION_TO_FEEDER = frc::Rotation2d(units::degree_t{54.0});

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,12 +17,11 @@
 #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 {
@@ -68,4 +66,6 @@ class Robot : public frc::TimedRobot {
   frc::SendableChooser<frc2::Command*> m_autoChooser;
 
   std::shared_ptr<PhotonVisionCamera> m_cam;
+
+  ClimberManager m_ClimberManager;
 };

src/main/include/RobotControlData.h

@@ -23,6 +23,16 @@ struct CoralOutput{
     bool flywheelsAtSpeed;
 };
 
+struct ClimberInput{
+    // bool Unspool;
+    // bool Respool;
+    double ClimberSpeed;
+};
+
+struct ClimberOutput{
+    double ClimberSpeed;
+};
+
 struct SmartPlannerInput
 {
     bool Left_L1;
@@ -45,6 +55,8 @@ struct RobotControlData {
     SwerveInput swerveInput;
     CoralInput coralInput;
     CoralOutput coralOutput;
+    ClimberInput climberInput;
+    ClimberOutput climberOutput;
     SmartPlannerInput plannerInput;
     AlgaeInput algaeInput;
     AlgaeOutput algaeOutput;