diff --git a/src/main/cpp/ControllerInterface.cpp b/src/main/cpp/ControllerInterface.cpp index aaaeb45..5a494db 100644 --- a/src/main/cpp/ControllerInterface.cpp +++ b/src/main/cpp/ControllerInterface.cpp @@ -6,6 +6,8 @@ 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) { @@ -13,6 +15,29 @@ void ControllerInterface::UpdateRobotControlData(RobotControlData &controlData) 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) { diff --git a/src/main/cpp/HAL/ClimberHAL.cpp b/src/main/cpp/HAL/ClimberHAL.cpp new file mode 100644 index 0000000..40d8bfc --- /dev/null +++ b/src/main/cpp/HAL/ClimberHAL.cpp @@ -0,0 +1,30 @@ +#include "HAL/ClimberHAL.h" +#include +#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(); +} diff --git a/src/main/cpp/InputManager/ClimberManager.cpp b/src/main/cpp/InputManager/ClimberManager.cpp new file mode 100644 index 0000000..7e4a5c0 --- /dev/null +++ b/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(); +} \ No newline at end of file diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 82cc92f..add0e59 100644 --- a/src/main/cpp/Robot.cpp +++ b/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() {} diff --git a/src/main/include/ControllerInterface.h b/src/main/include/ControllerInterface.h index 9a5810d..504af5e 100644 --- a/src/main/include/ControllerInterface.h +++ b/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}; diff --git a/src/main/include/HAL/ClimberHAL.h b/src/main/include/HAL/ClimberHAL.h new file mode 100644 index 0000000..1174275 --- /dev/null +++ b/src/main/include/HAL/ClimberHAL.h @@ -0,0 +1,28 @@ +#pragma once +#include "RobotControlData.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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(); +}; \ No newline at end of file diff --git a/src/main/include/InputManager/ClimberManager.h b/src/main/include/InputManager/ClimberManager.h new file mode 100644 index 0000000..a1b4c7b --- /dev/null +++ b/src/main/include/InputManager/ClimberManager.h @@ -0,0 +1,18 @@ +#pragma once + +#include "HAL/ClimberHAL.h" +#include "RobotControlData.h" +#include + + +class ClimberManager +{ +public: + ClimberManager() = default; + ~ClimberManager() = default; + void ResetState(); + void HandleInput(RobotControlData &robotControlData); +private: + Climber m_Climber; + frc::Timer m_matchTimer; +}; \ No newline at end of file diff --git a/src/main/include/MechanismConfig.h b/src/main/include/MechanismConfig.h index a1caef5..82b83a5 100644 --- a/src/main/include/MechanismConfig.h +++ b/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}); diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 75aa84b..c831c91 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -9,7 +9,6 @@ #include #include #include -#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 m_autoChooser; std::shared_ptr m_cam; + + ClimberManager m_ClimberManager; }; diff --git a/src/main/include/RobotControlData.h b/src/main/include/RobotControlData.h index d6dce61..7689966 100644 --- a/src/main/include/RobotControlData.h +++ b/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;