[sid] Implemented ClimberHAL, ClimberManager

src/main/cpp/HAL/ClimberHAL.cpp

@@ -0,0 +1,19 @@
+#include "HAL/ClimberHAL.h"
+#include <rev/config/SparkMaxConfig.h>
+#include "ratpack/SparkMaxDebugMacro.h"
+#include "MechanismConfig.h"
+
+Climber::Climber()
+{
+
+}
+
+void Climber::SetClimberSpeed(double speed)
+{
+    m_climberMotor.Set(speed);
+}
+
+double Climber::GetClimberSpeed()
+{
+    return m_climberMotor.GetEncoder().GetVelocity();
+}

src/main/cpp/InputManager/ClimberManager.cpp

@@ -0,0 +1,28 @@
+#include "InputManager/ClimberManager.h"
+
+
+void ClimberManager::HandleInput(RobotControlData &robotControlData){
+    if(robotControlData.climberInput.run && robotControlData.climberInput.run != m_prevClimberRun){
+        m_climberTimer.Reset();
+        m_prevClimberRun = m_climberRun;
+        m_climberRun = true;
+    }
+
+    if(m_climberRun){
+        if (m_climberTimer.GetTimestamp() > m_climberDuration)
+        {
+            m_Climber.SetClimberSpeed(0.0);
+            return;
+        }
+        m_Climber.SetClimberSpeed(m_climberMotorSpeed);
+        m_prevClimberRun = robotControlData.climberInput.run;
+    }
+
+    robotControlData.climberOutput.ClimberSpeed = m_Climber.GetClimberSpeed();
+
+}
+
+void ClimberManager::ResetState(){
+    m_climberRun = false;
+    m_climberTimer.Reset();
+}

src/main/include/HAL/ClimberHAL.h

@@ -0,0 +1,31 @@
+#pragma once
+#include "RobotControlData.h"
+#include <frc/Timer.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 GetClimberSpeed();
+        void SetClimberSpeed(double speed); 
+
+    private:
+        rev::spark::SparkMax m_climberMotor{50, rev::spark::SparkMax::MotorType::kBrushless};
+        double m_climberSpeed;
+        rev::spark::SparkClosedLoopController m_climberMotorPID = m_climberMotor.GetClosedLoopController();
+        rev::spark::SparkAbsoluteEncoder m_climberMotorAbsEncoder = m_climberMotor.GetAbsoluteEncoder();
+        double m_ProfileStartPos;                       
+};

src/main/include/InputManager/ClimberManager.h

@@ -0,0 +1,22 @@
+#pragma once
+
+#include "HAL/ClimberHAL.h"
+#include "RobotControlData.h"
+#include <frc/DigitalInput.h>
+
+
+class ClimberManager
+{
+public:
+    ClimberManager();
+    ~ClimberManager() = default;
+    void ResetState();
+    void HandleInput(RobotControlData &robotControlData);
+private:
+    Climber m_Climber;
+    bool m_prevClimberRun;
+    bool m_climberRun;
+    frc::Timer m_climberTimer;
+    units::second_t m_climberDuration = 4.0_s; //to be changed
+    double m_climberMotorSpeed = 3; //to be changed!! 
+};

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/RobotControlData.h

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