diff --git a/.vscode/settings.json b/.vscode/settings.json index 5705711..ce2f691 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -93,6 +93,9 @@ "typeindex": "cpp", "typeinfo": "cpp", "valarray": "cpp", - "variant": "cpp" + "variant": "cpp", + "xmemory": "cpp", + "xstring": "cpp", + "xutility": "cpp" } } diff --git a/README.md b/README.md index 7481617..05e63e4 100644 --- a/README.md +++ b/README.md @@ -84,14 +84,9 @@ Following the WPILib command based structure we have broken our robot up into a ## Digital Input Ports -| Port | Device | -| :---: | :--------------------------: | -| 1 | Center Upper Beam Break | -| 2 | Lower Podium Side Beam Break | -| 3 | Lower Center Beam Break | -| 4 | Lower Amp Side Beam Break | -| 5 | Upper Amp Side Beam Break | -| 6 | Upper Podium Side Beam Break | +| Port | Device | +| :---: | :------------------------------: | +| 1 | Lower Elevator Limit Switch | ## Network Map diff --git a/SubZero-common b/SubZero-common index 3c01c9c..f890b47 160000 --- a/SubZero-common +++ b/SubZero-common @@ -1 +1 @@ -Subproject commit 3c01c9ce669d96bec7d854ee44118f6a3bac9ca0 +Subproject commit f890b47ca056c55762f1d6c4a07655addf0066cb diff --git a/build.gradle b/build.gradle index 7119ab2..2eda0b8 100644 --- a/build.gradle +++ b/build.gradle @@ -58,11 +58,12 @@ model { sources.cpp { source { - srcDir 'src/main/cpp' + srcDirs 'src/main/cpp', 'SubZero-common/lib/cpp' include '**/*.cpp', '**/*.cc' + } exportedHeaders { - srcDir 'src/main/include' + srcDirs 'src/main/include', 'SubZero-common/lib/include', 'SubZero-common/lib/cpp' } } diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..c4b7efd 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -88,5 +88,11 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + } ] } diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 6a1eb4b..cfe63bb 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -18,6 +18,11 @@ Robot::Robot() {} */ void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); + m_container.Periodic(); +} + +void Robot::RobotInit() { + m_container.Initialize(); } /** diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index af5824b..cdbaf60 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -17,10 +17,13 @@ #include #include #include + #include #include "commands/Autos.h" #include "commands/ExampleCommand.h" +#include "subsystems/AlgaeArmSubsystem.h" +#include "subsystems/CoralArmSubsystem.h" RobotContainer::RobotContainer() { // Initialize all of your commands and subsystems here @@ -52,9 +55,10 @@ RobotContainer::RobotContainer() { } void RobotContainer::ConfigureBindings() { - frc2::JoystickButton(&m_driverController, - frc::XboxController::Button::kRightBumper) - .WhileTrue(new frc2::RunCommand([this] { m_drive.SetX(); }, {&m_drive})); + m_driverController.A().OnTrue(m_algaeArm.MoveToPositionAbsolute(50_deg)); + m_driverController.B().OnTrue(m_algaeArm.MoveToPositionAbsolute(5_deg)); + m_driverController.X().OnTrue(m_coralArm.MoveToPositionAbsolute(120_deg)); + m_driverController.Y().OnTrue(m_coralArm.MoveToPositionAbsolute(5_deg)); } frc2::CommandPtr RobotContainer::GetAutonomousCommand() { @@ -70,3 +74,14 @@ frc2::CommandPtr RobotContainer::GetAutonomousCommand() { return pathplanner::PathPlannerAuto(m_autoSelected) .AndThen(frc2::InstantCommand([this, offset]() { m_drive.OffsetRotation(offset); }).ToPtr()); } + +void RobotContainer::Periodic() { + frc::SmartDashboard::PutData("Robot Elevator", &m_elevatorMech); +} + +void RobotContainer::Initialize() { + m_elevator.OnInit(); + m_coralArm.OnInit(); + m_algaeArm.OnInit(); + m_climber.OnInit(); +} \ No newline at end of file diff --git a/src/main/cpp/subsystems/AlgaeArmSubsystem.cpp b/src/main/cpp/subsystems/AlgaeArmSubsystem.cpp new file mode 100644 index 0000000..6d9ee10 --- /dev/null +++ b/src/main/cpp/subsystems/AlgaeArmSubsystem.cpp @@ -0,0 +1,4 @@ + +#include "subsystems/AlgaeArmSubsystem.h" + +#include diff --git a/src/main/cpp/subsystems/CoralArmSubsystem.cpp b/src/main/cpp/subsystems/CoralArmSubsystem.cpp new file mode 100644 index 0000000..9b51669 --- /dev/null +++ b/src/main/cpp/subsystems/CoralArmSubsystem.cpp @@ -0,0 +1,3 @@ +#include "subsystems/CoralArmSubsystem.h" + +#include diff --git a/src/main/cpp/subsystems/IntakeSubsystem.cpp b/src/main/cpp/subsystems/IntakeSubsystem.cpp new file mode 100644 index 0000000..76b2741 --- /dev/null +++ b/src/main/cpp/subsystems/IntakeSubsystem.cpp @@ -0,0 +1,34 @@ +#include + +#include +#include + +frc2::CommandPtr IntakeSubsystem::MoveAtPercent(double percent) { + return frc2::FunctionalCommand( + // On init + []() {}, + // On execute + [this, percent]() { + m_motor.Set(percent); + }, + // On end + [this](bool interupted) { + m_motor.StopMotor(); + }, + // Is finished + []() { + return false; + }, + // Requirements + {this} + ).ToPtr(); +} + +frc2::CommandPtr IntakeSubsystem::Stop() { + return frc2::InstantCommand( + [this]() { + m_motor.StopMotor(); + }, + {this} + ).ToPtr(); +} \ No newline at end of file diff --git a/src/main/include/CommonCompile.h b/src/main/include/CommonCompile.h new file mode 100644 index 0000000..5c600b4 --- /dev/null +++ b/src/main/include/CommonCompile.h @@ -0,0 +1,6 @@ +#pragma once + +#include +#include +#include +#include \ No newline at end of file diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index ffc5878..14aeb18 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -10,15 +10,48 @@ #include #include #include +#include +#include +#include #include #include #pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using SparkMaxPidController = + subzero::PidMotorController; + +using SparkMaxPidControllerTuner = + subzero::PidMotorControllerTuner; +typedef + subzero::PidMotorController SparkFlexController; + /** * The Constants header provides a convenient place for teams to hold robot-wide * numerical or bool constants. This should not be used for any other purpose. + * numerical or bool constants. This should not be used for any other purpose. * * It is generally a good idea to place constants into subsystem- or * command-specific namespaces within this header, which can then be used where @@ -43,6 +76,8 @@ namespace DriveConstants { constexpr units::meters_per_second_t kMaxSpeed = 4.92_mps; constexpr units::radians_per_second_t kMaxAngularSpeed{2 * std::numbers::pi}; +const int kPigeonCanId = 13; + constexpr double kDirectionSlewRate = 1.2; // radians per second constexpr double kMagnitudeSlewRate = 1.8; // percent per second (1 = 100%) constexpr double kRotationalSlewRate = 2.0; // percent per second (1 = 100%) @@ -60,15 +95,15 @@ constexpr double kRearLeftChassisAngularOffset = std::numbers::pi; constexpr double kRearRightChassisAngularOffset = std::numbers::pi / 2; // SPARK MAX CAN IDs -constexpr int kFrontLeftDrivingCanId = 8; -constexpr int kRearLeftDrivingCanId = 6; -constexpr int kFrontRightDrivingCanId = 2; -constexpr int kRearRightDrivingCanId = 4; - -constexpr int kFrontLeftTurningCanId = 7; -constexpr int kRearLeftTurningCanId = 5; -constexpr int kFrontRightTurningCanId = 1; -constexpr int kRearRightTurningCanId = 3; +constexpr int kFrontLeftDrivingCanId = 18; +constexpr int kRearLeftDrivingCanId = 1; +constexpr int kFrontRightDrivingCanId = 4; +constexpr int kRearRightDrivingCanId = 9; + +constexpr int kFrontLeftTurningCanId = 2; +constexpr int kRearLeftTurningCanId = 20; +constexpr int kFrontRightTurningCanId = 5; +constexpr int kRearRightTurningCanId = 8; } // namespace DriveConstants namespace ModuleConstants { @@ -122,4 +157,150 @@ constexpr double kDriveDeadband = 0.05; namespace GyroConstants { constexpr units::time::second_t kPrintPeriod{1000_ms}; +} + +namespace ElevatorConstants { + const int kLeadElevatorMotorCanId = 19; + const int kFollowerElevatorMotorCanId = 7; + + const int kBottomLimitSwitchPort = 1; + + // Placeholder values + const double kElevatorP = 100.0; + const double kElevatorI = 0.0; + const double kElevatorD = 0.0; + const double kElevatorIZone = 0.0; + const double kElevatorFF = 0.0; + + constexpr units::revolutions_per_minute_t kMaxRpm = 5676_rpm; + + // Placeholder values + constexpr units::meter_t kMinDistance = 0_in; + constexpr units::meter_t kMaxDistance = 18_in; + constexpr units::meter_t kRelativeDistancePerRev = 5.51977829236_in / 36; // 36:1 ratio gearbox + constexpr units::meters_per_second_t kDefaultVelocity = 0.66_mps; + constexpr double kVelocityScalar = 1.0; + constexpr units::meter_t kTolerance = 0.5_in; + + // Placeholder + const subzero::SingleAxisMechanism kElevatorMechanism { + // min length + 2_in, + // angle + 90_deg, + // line width + 10.0, + // color + subzero::ColorConstants::kRed}; + + const frc::TrapezoidProfile::Constraints kElevatorProfileConstraints{1_fps * 10, 0.75_fps_sq * 20}; +}; + +namespace AlgaeArmConstants{ + constexpr int kArmMotorId = 17; + constexpr int kIntakeMotorId = 6; + constexpr double kP = 0.1; + constexpr double kI = 0.0; + constexpr double kD = 0.001; + constexpr double kIZone = 0.0; + constexpr double kFF = 0.0; + + constexpr units::revolutions_per_minute_t kMaxRpm = 5676_rpm; + constexpr units::degree_t kHomeRotation = 0_deg; + constexpr units::degree_t kMaxRotation = 85_deg; + constexpr units::degree_t kRelativeDistancePerRev = 360_deg / 75; + constexpr units::degree_t kAbsoluteDistancePerRev = 360_deg; + constexpr units::degrees_per_second_t kDefaultVelocity = 10_deg_per_s; + constexpr double kVelocityScalar = 1.0; + constexpr units::degree_t kTolerance = 2_deg; + constexpr units::meter_t kArmLength = 17_in; + + static const subzero::SingleAxisMechanism kAlgaeArmMechanism = { + // length + 0.2_m, + // min angle + 110_deg, + // line width + 6, + // color + subzero::ColorConstants::kBlue}; + + // const frc::TrapezoidProfile::Constraints + // kRotationalAxisConstraints{360_deg_per_s * 2.2, 360_deg_per_s_sq * 2.2}; + + const frc::TrapezoidProfile::Constraints + kRotationalAxisConstraints; + +} + +namespace CoralArmConstants{ + constexpr int kArmMotorId = 16; + constexpr int kIntakeMotorId = 15; + constexpr double kP = 0.5; + constexpr double kI = 0.0; + constexpr double kD = 0.0; + constexpr double kIZone = 0.0; + constexpr double kFF = 0.0; + + constexpr units::revolutions_per_minute_t kMaxRpm = 1_rpm; + constexpr units::degree_t kHomeRotation = 5_deg; + constexpr units::degree_t kMaxRotation = 290_deg; + constexpr units::degree_t kRelativeDistancePerRev = 360_deg / (15 * 4.7); // 4.7 is the ratio of the chain gear + constexpr units::degree_t kAbsoluteDistancePerRev = 360_deg; + constexpr units::degrees_per_second_t kDefaultVelocity = 10_deg_per_s; + constexpr double kVelocityScalar = 1.0; + constexpr units::degree_t kTolerance = 2_deg; + constexpr units::meter_t kArmLength = 0.2_m; + + static const subzero::SingleAxisMechanism kCoralArmMechanism = { + // length + 0.2_m, + // min angle + 110_deg, + // line width + 6, + // color + subzero::ColorConstants::kBlue}; + + const frc::TrapezoidProfile::Constraints + kRotationalAxisConstraints{360_deg_per_s * 2.2, 360_deg_per_s_sq * 2.2}; + +} + +namespace ClimberConstants{ + constexpr int kArmMotorId = 14; + + // Placeholder values + constexpr double kP = 0.2; + constexpr double kI = 0.0; + constexpr double kD = 0.0; + constexpr double kIZone = 0.0; + constexpr double kFF = 0.0; + + constexpr units::revolutions_per_minute_t kMaxRpm = 5676_rpm; + constexpr units::degree_t kHomeRotation = 0_deg; + constexpr units::degree_t kMaxRotation = 85_deg; + constexpr units::degree_t kRelativeDistancePerRev = 360_deg / 125; + constexpr units::degree_t kAbsoluteDistancePerRev = 360_deg; + constexpr units::degrees_per_second_t kDefaultVelocity = 10_deg_per_s; + constexpr double kVelocityScalar = 1.0; + constexpr units::degree_t kTolerance = 2_deg; + constexpr units::meter_t kArmLength = 17_in; + + static const subzero::SingleAxisMechanism kClimberMechanism = { + // length + kArmLength, + // min angle + 110_deg, + // line width + 6, + // color + subzero::ColorConstants::kBlue}; + + // const frc::TrapezoidProfile::Constraints + // kRotationalAxisConstraints{360_deg_per_s * 2.2, 360_deg_per_s_sq * 2.2}; + + const frc::TrapezoidProfile::Constraints + kRotationalAxisConstraints; + } \ No newline at end of file diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 794e643..0afe066 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -23,6 +23,7 @@ class Robot : public frc::TimedRobot { void TeleopPeriodic() override; void TestPeriodic() override; void SimulationInit() override; + void RobotInit() override; void SimulationPeriodic() override; private: diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index bf580e1..8ef2672 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -14,10 +14,18 @@ #include #include #include +#include +#include + +#include "CommonCompile.h" #include "Constants.h" #include "subsystems/DriveSubsystem.h" #include "subsystems/ExampleSubsystem.h" +#include "subsystems/ElevatorSubsystem.h" +#include "subsystems/AlgaeArmSubsystem.h" +#include "subsystems/CoralArmSubsystem.h" +#include "subsystems/ClimberSubsystem.h" /** * This class is where the bulk of the robot should be declared. Since @@ -32,9 +40,13 @@ class RobotContainer { frc2::CommandPtr GetAutonomousCommand(); + void Periodic(); + + void Initialize(); + private: // Replace with CommandPS4Controller or CommandJoystick if needed - frc::XboxController m_driverController{ + frc2::CommandXboxController m_driverController{ OperatorConstants::kDriverControllerPort}; // The robot's subsystems are defined here... @@ -46,4 +58,17 @@ class RobotContainer { frc::SendableChooser m_chooser; std::string m_autoSelected; -}; + + frc::Mechanism2d m_elevatorMech{0.5, 0.5}; + frc::MechanismRoot2d* m_root = m_elevatorMech.GetRoot("Elevator", 0.25, 0.25); + + ElevatorSubsystem m_elevator{(frc::MechanismObject2d*)m_elevatorMech.GetRoot("Elevator", 0.25, 0.25)}; + + AlgaeArmSubsystem m_algaeArm; + IntakeSubsystem m_algaeIntake{AlgaeArmConstants::kIntakeMotorId}; + + CoralArmSubsystem m_coralArm; + IntakeSubsystem m_coralIntake{CoralArmConstants::kIntakeMotorId}; + + ClimberSubsystem m_climber; +}; \ No newline at end of file diff --git a/src/main/include/subsystems/AlgaeArmSubsystem.h b/src/main/include/subsystems/AlgaeArmSubsystem.h new file mode 100644 index 0000000..912b816 --- /dev/null +++ b/src/main/include/subsystems/AlgaeArmSubsystem.h @@ -0,0 +1,75 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Constants.h" +#include "subsystems/IntakeSubsystem.h" + +class AlgaeArmSubsystem : public subzero::RotationalSingleAxisSubsystem { + public: + explicit AlgaeArmSubsystem(frc::MechanismObject2d* node = nullptr) + : subzero::RotationalSingleAxisSubsystem{ + "Algae Arm", + frc::RobotBase::IsReal() + ? dynamic_cast(algaeArmController) + : dynamic_cast(simAlgaeArmController), + { + AlgaeArmConstants::kHomeRotation, + // Max distance + AlgaeArmConstants::kMaxRotation, + // Distance per revolution of relative encoder + AlgaeArmConstants::kRelativeDistancePerRev, + // Distance per revolution of absolute encoder + AlgaeArmConstants::kAbsoluteDistancePerRev, + // Default velocity + AlgaeArmConstants::kDefaultVelocity, + // Velocity scalar + AlgaeArmConstants::kVelocityScalar, + // Tolerance + AlgaeArmConstants::kTolerance, + // Min limit switch + std::nullopt, + // Max limit switch + std::nullopt, + // Reversed + true, + // Mechanism2d + AlgaeArmConstants::kAlgaeArmMechanism, + // Conversion Function + std::nullopt, + + [] { return false; }, + + AlgaeArmConstants::kRotationalAxisConstraints}, + AlgaeArmConstants::kArmLength, + node} { + + } + private: + + rev::spark::SparkMax m_algaeArmMotor{AlgaeArmConstants::kArmMotorId, + rev::spark::SparkLowLevel::MotorType::kBrushless}; + rev::spark::SparkClosedLoopController m_pidController = m_algaeArmMotor.GetClosedLoopController(); + rev::spark::SparkRelativeEncoder m_enc = m_algaeArmMotor.GetEncoder(); + rev::spark::SparkAbsoluteEncoder m_absEnc = m_algaeArmMotor.GetAbsoluteEncoder(); + subzero::PidSettings algaeArmPidSettings = { + AlgaeArmConstants::kP, AlgaeArmConstants::kI, AlgaeArmConstants::kD, + AlgaeArmConstants::kIZone, AlgaeArmConstants::kFF, false}; + SparkMaxPidController algaeArmController{"Algae Arm", + m_algaeArmMotor, + m_enc, + m_pidController, + algaeArmPidSettings, + &m_absEnc, + AlgaeArmConstants::kMaxRpm}; + subzero::SimPidMotorController simAlgaeArmController{"Sim Algae Arm", algaeArmPidSettings, + AlgaeArmConstants::kMaxRpm}; + +}; \ No newline at end of file diff --git a/src/main/include/subsystems/ClimberSubsystem.h b/src/main/include/subsystems/ClimberSubsystem.h new file mode 100644 index 0000000..b817a50 --- /dev/null +++ b/src/main/include/subsystems/ClimberSubsystem.h @@ -0,0 +1,67 @@ +#pragma once + +#include +#include + +#include +#include +#include +#include + +#include "Constants.h" + +class ClimberSubsystem : public subzero::RotationalSingleAxisSubsystem { + public: + explicit ClimberSubsystem(frc::MechanismObject2d* node = nullptr) + : subzero::RotationalSingleAxisSubsystem{ + "Climber Arm", + frc::RobotBase::IsReal() + ? dynamic_cast(climberController) + : dynamic_cast(simClimberController), + { + ClimberConstants::kHomeRotation, + // Max distance + ClimberConstants::kMaxRotation, + // Distance per revolution of relative encoder + ClimberConstants::kRelativeDistancePerRev, + // Distance per revolution of absolute encoder + ClimberConstants::kAbsoluteDistancePerRev, + // Default velocity + ClimberConstants::kDefaultVelocity, + // Velocity scalar + ClimberConstants::kVelocityScalar, + // Tolerance + ClimberConstants::kTolerance, + // Min limit switch + std::nullopt, + // Max limit switch + std::nullopt, + // Reversed + true, + // Mechanism2d + ClimberConstants::kClimberMechanism, + // Conversion Function + std::nullopt, + + [] { return false; }, ClimberConstants::kRotationalAxisConstraints}, + CoralArmConstants::kArmLength, + node} {} + + private: + + rev::spark::SparkMax m_climberMotor{ClimberConstants::kArmMotorId, + rev::spark::SparkLowLevel::MotorType::kBrushless}; + rev::spark::SparkClosedLoopController m_pidController = m_climberMotor.GetClosedLoopController(); + rev::spark::SparkRelativeEncoder m_enc = m_climberMotor.GetEncoder(); + subzero::PidSettings climberPidSettings = { + ClimberConstants::kP, ClimberConstants::kI, ClimberConstants::kD, + ClimberConstants::kIZone, ClimberConstants::kFF, true}; + SparkMaxPidController climberController{"Climb Arm", + m_climberMotor, + m_enc, + m_pidController, + climberPidSettings, + nullptr, + ClimberConstants::kMaxRpm}; + subzero::SimPidMotorController simClimberController{"Sim Climb Arm", climberPidSettings, ClimberConstants::kMaxRpm}; +}; \ No newline at end of file diff --git a/src/main/include/subsystems/CoralArmSubsystem.h b/src/main/include/subsystems/CoralArmSubsystem.h new file mode 100644 index 0000000..20797f0 --- /dev/null +++ b/src/main/include/subsystems/CoralArmSubsystem.h @@ -0,0 +1,82 @@ +#pragma once + +#include +#include + +#include +#include +#include +#include + +#include "Constants.h" +#include "subsystems/IntakeSubsystem.h" + +//pretty much a copy paste of algae arm subsystem with differet names right now, subject to change later + +class CoralArmSubsystem : public subzero::RotationalSingleAxisSubsystem { + public: + explicit CoralArmSubsystem(frc::MechanismObject2d* node = nullptr) + : subzero::RotationalSingleAxisSubsystem{ + "Coral Arm", + frc::RobotBase::IsReal() + ? dynamic_cast(m_coralArmController) + : dynamic_cast(simCoralArmController), + { + CoralArmConstants::kHomeRotation, + // Max distance + CoralArmConstants::kMaxRotation, + // Distance per revolution of relative encoder + CoralArmConstants::kRelativeDistancePerRev, + // Distance per revolution of absolute encoder + CoralArmConstants::kAbsoluteDistancePerRev, + // Default velocity + CoralArmConstants::kDefaultVelocity, + // Velocity scalar + CoralArmConstants::kVelocityScalar, + // Tolerance + CoralArmConstants::kTolerance, + // Min limit switch + std::nullopt, + // Max limit switch + std::nullopt, + // Reversed + true, + // Mechanism2d + CoralArmConstants::kCoralArmMechanism, + // Conversion Function + std::nullopt, + + [] { return false; }, CoralArmConstants::kRotationalAxisConstraints}, + CoralArmConstants::kArmLength, + node} { + } + + void Periodic() override { + subzero::RotationalSingleAxisSubsystem::Periodic(); + + m_tuner.UpdateFromShuffleboard(); + } + + private: + rev::spark::SparkMax m_coralArmMotor{CoralArmConstants::kArmMotorId, + rev::spark::SparkLowLevel::MotorType::kBrushless}; + rev::spark::SparkClosedLoopController m_pidController = m_coralArmMotor.GetClosedLoopController(); + rev::spark::SparkRelativeEncoder m_enc = m_coralArmMotor.GetEncoder(); + rev::spark::SparkAbsoluteEncoder m_absEnc = m_coralArmMotor.GetAbsoluteEncoder(); + subzero::PidSettings coralArmPidSettings = { + CoralArmConstants::kP, CoralArmConstants::kI, CoralArmConstants::kD, + CoralArmConstants::kIZone, CoralArmConstants::kFF, false}; + SparkMaxPidController m_coralArmController{"Coral Arm", + m_coralArmMotor, + m_enc, + m_pidController, + coralArmPidSettings, + &m_absEnc, + CoralArmConstants::kMaxRpm}; + + SparkMaxPidControllerTuner m_tuner{m_coralArmController}; + + subzero::SimPidMotorController simCoralArmController{"Sim Arm", coralArmPidSettings, + CoralArmConstants::kMaxRpm}; + +}; \ No newline at end of file diff --git a/src/main/include/subsystems/DriveSubsystem.h b/src/main/include/subsystems/DriveSubsystem.h index 60972ac..fce5227 100644 --- a/src/main/include/subsystems/DriveSubsystem.h +++ b/src/main/include/subsystems/DriveSubsystem.h @@ -128,7 +128,7 @@ class DriveSubsystem : public frc2::SubsystemBase { MAXSwerveModule m_rearRight; // Gyro Sensor - ctre::phoenix6::hardware::Pigeon2 m_pidgey{13, "rio"}; + ctre::phoenix6::hardware::Pigeon2 m_pidgey{DriveConstants::kPigeonCanId, "rio"}; units::time::second_t currentTime{frc::Timer::GetFPGATimestamp()}; // Odometry class for tracking robot pose diff --git a/src/main/include/subsystems/ElevatorSubsystem.h b/src/main/include/subsystems/ElevatorSubsystem.h new file mode 100644 index 0000000..e8c8466 --- /dev/null +++ b/src/main/include/subsystems/ElevatorSubsystem.h @@ -0,0 +1,89 @@ +#pragma once + +#include + +#include + +#include +#include +#include +#include +#include + +#include "Constants.h" + +class ElevatorSubsystem : public subzero::LinearSingleAxisSubsystem { +public: + explicit ElevatorSubsystem(frc::MechanismObject2d* node = nullptr) + : subzero::LinearSingleAxisSubsystem{ + "Elevator Subsystem", + frc::RobotBase::IsReal() ? + dynamic_cast(m_elevatorController) : + dynamic_cast(simElevatorController), + { // Min distance + ElevatorConstants::kMinDistance, + // Max distance + ElevatorConstants::kMaxDistance, + // Distance per revolution of relative encoder + ElevatorConstants::kRelativeDistancePerRev, + // Distance per revolution of absolute encoder + std::nullopt, + // Default velocity + ElevatorConstants::kDefaultVelocity, + // Velocity scalar + ElevatorConstants::kVelocityScalar, + // Tolerance + ElevatorConstants::kTolerance, + // Min limit switch + &m_bottomLimitSwitchPort, + // Max limit switch + std::nullopt, + // Reversed + false, + // Mechanism2d + ElevatorConstants::kElevatorMechanism, + // Conversion function to output values in different + // units in shuffleboard + std::nullopt, + // Ignore soft limits if `true` is returned + []() { return false; }, + // Trapazoid profile constraints + ElevatorConstants::kElevatorProfileConstraints + }, + node + } { + m_followerConfig.Follow(m_leadMotor, true); + m_followerMotor.Configure(m_followerConfig, rev::spark::SparkBase::ResetMode::kNoResetSafeParameters, + rev::spark::SparkBase::PersistMode::kPersistParameters); + } + +private: + rev::spark::SparkMax m_leadMotor{ + ElevatorConstants::kLeadElevatorMotorCanId, + rev::spark::SparkLowLevel::MotorType::kBrushless}; + + rev::spark::SparkMax m_followerMotor{ + ElevatorConstants::kFollowerElevatorMotorCanId, + rev::spark::SparkLowLevel::MotorType::kBrushless}; + + // Here so we can set the foller motor as a follower + rev::spark::SparkMaxConfig m_followerConfig; + + frc::DigitalInput m_bottomLimitSwitchPort{ElevatorConstants::kBottomLimitSwitchPort}; + + rev::spark::SparkClosedLoopController m_pidController = m_leadMotor.GetClosedLoopController(); + rev::spark::SparkRelativeEncoder m_enc = m_leadMotor.GetEncoder(); + subzero::PidSettings m_elevatorPidSettings = { + ElevatorConstants::kElevatorP, ElevatorConstants::kElevatorI, ElevatorConstants::kElevatorD, + ElevatorConstants::kElevatorIZone, ElevatorConstants::kElevatorFF, false}; + SparkMaxPidController m_elevatorController{"Elevator", + m_leadMotor, + m_enc, + m_pidController, + m_elevatorPidSettings, + nullptr, + ElevatorConstants::kMaxRpm}; + + subzero::SimPidMotorController simElevatorController{"Sim Elevator", m_elevatorPidSettings, + ElevatorConstants::kMaxRpm}; +}; \ No newline at end of file diff --git a/src/main/include/subsystems/IntakeSubsystem.h b/src/main/include/subsystems/IntakeSubsystem.h new file mode 100644 index 0000000..245abdc --- /dev/null +++ b/src/main/include/subsystems/IntakeSubsystem.h @@ -0,0 +1,17 @@ +#pragma once + +#include +#include + +#include + +class IntakeSubsystem : public frc2::SubsystemBase { +public: + IntakeSubsystem(const int id) : m_motor{id, rev::spark::SparkLowLevel::MotorType::kBrushless} {} + + frc2::CommandPtr MoveAtPercent(double percent); + + frc2::CommandPtr Stop(); +private: + rev::spark::SparkMax m_motor; +}; \ No newline at end of file diff --git a/vendordeps/PathplannerLib-2025.2.1.json b/vendordeps/PathplannerLib-2025.2.2.json similarity index 87% rename from vendordeps/PathplannerLib-2025.2.1.json rename to vendordeps/PathplannerLib-2025.2.2.json index 71e25f3..a5bf9ee 100644 --- a/vendordeps/PathplannerLib-2025.2.1.json +++ b/vendordeps/PathplannerLib-2025.2.2.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.2.1.json", + "fileName": "PathplannerLib-2025.2.2.json", "name": "PathplannerLib", - "version": "2025.2.1", + "version": "2025.2.2", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.2.1" + "version": "2025.2.2" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.2.1", + "version": "2025.2.2", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/REVLib-2025.0.1.json b/vendordeps/REVLib-2025.0.2.json similarity index 90% rename from vendordeps/REVLib-2025.0.1.json rename to vendordeps/REVLib-2025.0.2.json index c998054..c29aefa 100644 --- a/vendordeps/REVLib-2025.0.1.json +++ b/vendordeps/REVLib-2025.0.2.json @@ -1,7 +1,7 @@ { - "fileName": "REVLib-2025.0.1.json", + "fileName": "REVLib-2025.0.2.json", "name": "REVLib", - "version": "2025.0.1", + "version": "2025.0.2", "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2025.0.1" + "version": "2025.0.2" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.1", + "version": "2025.0.2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -36,7 +36,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2025.0.1", + "version": "2025.0.2", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -53,7 +53,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.1", + "version": "2025.0.2", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false,