[sid] Implemented ClimberHAL, ClimberManager

src/main/cpp/HAL/ClimberHAL.cpp

@@ -5,7 +5,18 @@
 
 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)

src/main/cpp/InputManager/ClimberManager.cpp

@@ -1,28 +1,50 @@
 #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;
-    }
+// void ClimberManager::HandleInput(RobotControlData &robotControlData){
+//     if (m_isFirstIteration)
+//     {
+//         m_matchTimer.Restart();
+//         m_isFirstIteration = false;
+//     }
+//     if((robotControlData.climberInput.run && robotControlData.climberInput.run != m_prevClimberRun) && m_matchTimer.HasElapsed(150_s)){
+//         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;
-    }
+//     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();
+//     robotControlData.climberOutput.ClimberSpeed = m_Climber.GetClimberSpeed();
+// }
 
+void ClimberManager::HandleInput(RobotControlData &robotControlData)
+{
+    if (/*controler says left*/)
+    {
+        m_Climber.SetClimberSpeed(1)
+    }
+    else if (/*controller says right*/)
+    {
+        m_Climber.SetClimberSpeed(-1)
+    }
+    else
+    {
+        // stop
+    }
 }
 
 void ClimberManager::ResetState(){
     m_climberRun = false;
+    m_prevClimberRun = false;
     m_climberTimer.Reset();
+    m_isFirstIteration = true;
 }

src/main/include/InputManager/ClimberManager.h

@@ -2,7 +2,7 @@
 
 #include "HAL/ClimberHAL.h"
 #include "RobotControlData.h"
-#include <frc/DigitalInput.h>
+// #include <frc/DigitalInput.h>
 
 
 class ClimberManager
@@ -14,6 +14,8 @@ class ClimberManager
     void HandleInput(RobotControlData &robotControlData);
 private:
     Climber m_Climber;
+    frc::Timer m_matchTimer;
+    bool m_isFirstIteration = true;
     bool m_prevClimberRun;
     bool m_climberRun;
     frc::Timer m_climberTimer;