diff --git a/.vscode/settings.json b/.vscode/settings.json index ce2f691..021dd2c 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -96,6 +96,20 @@ "variant": "cpp", "xmemory": "cpp", "xstring": "cpp", - "xutility": "cpp" + "xutility": "cpp", + "charconv": "cpp", + "format": "cpp", + "ios": "cpp", + "locale": "cpp", + "xfacet": "cpp", + "xhash": "cpp", + "xiosbase": "cpp", + "xlocale": "cpp", + "xlocbuf": "cpp", + "xlocinfo": "cpp", + "xlocmes": "cpp", + "xlocmon": "cpp", + "xlocnum": "cpp", + "xloctime": "cpp" } } diff --git a/build.gradle b/build.gradle index 2eda0b8..964369d 100644 --- a/build.gradle +++ b/build.gradle @@ -1,7 +1,7 @@ plugins { id "cpp" id "google-test-test-suite" - id "edu.wpi.first.GradleRIO" version "2025.1.1" + id "edu.wpi.first.GradleRIO" version "2025.3.1" } // Define my targets (RoboRIO) and artifacts (deployable files) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index cdbaf60..66e1448 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -1,3 +1,4 @@ + // Copyright (c) FIRST and other WPILib contributors. // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. @@ -17,8 +18,11 @@ #include #include #include +#include +#include #include +#include #include "commands/Autos.h" #include "commands/ExampleCommand.h" @@ -28,11 +32,23 @@ RobotContainer::RobotContainer() { // Initialize all of your commands and subsystems here - // m_chooser.SetDefaultOption(AutoConstants::kDefaultAutoName, AutoConstants::kDefaultAutoName); - frc::SmartDashboard::PutData(&m_chooser); - m_chooser.SetDefaultOption(AutoConstants::kOutAuto, AutoConstants::kSpinAuto); - m_chooser.AddOption(AutoConstants::kSpinAuto, AutoConstants::kSpinAuto); + m_chooser.SetDefaultOption(AutoConstants::kCenterToCenterAuto, AutoConstants::kCenterToCenterAuto); + m_chooser.AddOption(AutoConstants::kCenterToCenterAuto, AutoConstants::kCenterToCenterAuto); + m_chooser.AddOption(AutoConstants::kLeftToLeftReefAuto, AutoConstants::kLeftToLeftReefAuto); + m_chooser.AddOption(AutoConstants::kRightToRightReefAuto, AutoConstants::kRightToRightReefAuto); + m_chooser.AddOption(AutoConstants::kForwardAuto, AutoConstants::kForwardAuto); + + // pathplanner::NamedCommands::registerCommand("Test Command", frc2::cmd::Print("Test Command")); + pathplanner::NamedCommands::registerCommand("To L1 Position", std::move(m_commandController.MoveToPositionL1())); + pathplanner::NamedCommands::registerCommand("To L2 Position", std::move(m_commandController.MoveToPositionL2())); + pathplanner::NamedCommands::registerCommand("To L3 Position", std::move(m_commandController.MoveToPositionL3())); + pathplanner::NamedCommands::registerCommand("To Feed Position", std::move(m_commandController.FeedCoral())); + pathplanner::NamedCommands::registerCommand("Remove Algae", std::move(m_commandController.RemoveAlgaeFromL2())); + pathplanner::NamedCommands::registerCommand("To Feed Position", std::move(m_commandController.FeedCoral())); + pathplanner::NamedCommands::registerCommand("Expel Coral", std::move(m_commandController.ExpelCoral().WithTimeout(CommandConstants::kExpelCoralTimeout))); + pathplanner::NamedCommands::registerCommand("Home", std::move(m_commandController.HomeElevator())); + pathplanner::NamedCommands::registerCommand("Delayed To L2 Position", std::move(frc2::cmd::Wait(CommandConstants::kWaitBeforeL2).AndThen(m_commandController.MoveToPositionL2()))); // Configure the button bindings ConfigureBindings(); @@ -55,33 +71,56 @@ RobotContainer::RobotContainer() { } void RobotContainer::ConfigureBindings() { - 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)); + // NOT FINAL + + m_operatorController.POVLeft().OnTrue(m_commandController.MoveToPositionL1()); + m_operatorController.POVUp().OnTrue(m_commandController.MoveToPositionL2()); + m_operatorController.POVRight().OnTrue(m_commandController.MoveToPositionL3()); + m_operatorController.POVDown().OnTrue(m_commandController.FeedCoral()); + + m_operatorController.A().WhileTrue(m_commandController.ExpelCoral()); + m_operatorController.B().WhileTrue(m_coralIntake.MoveAtPercent(CommandConstants::kCoralFeedSpeed)); + m_operatorController.X().WhileTrue(m_commandController.ExpelAlgae()); + m_operatorController.Y().WhileTrue(m_commandController.IntakeAlgae()); + + m_operatorController.LeftBumper().OnTrue(m_commandController.RemoveAlgaeFromL2()); + // m_operatorController.RightBumper().OnTrue(m_commandController.RemoveAlgaeFromL3()); + m_operatorController.RightBumper().OnTrue(m_algaeArm.MoveToPositionAbsolute(CommandConstants::kClimbAlgaeArmPosition)); + + m_driverController.POVUp().OnTrue(m_commandController.ClimbUp()); + m_driverController.POVDown().OnTrue(m_commandController.ClimbDown()); + + m_driverController.LeftBumper().OnTrue(m_commandController.HomeElevator()); + m_driverController.RightBumper().OnTrue(frc2::InstantCommand( + [this]() { + std::cout << "Resetting rotation" << std::endl; + m_drive.ResetRotation(); + } + ).ToPtr()); } frc2::CommandPtr RobotContainer::GetAutonomousCommand() { - auto command = pathplanner::PathPlannerAuto("Spin Auto"); + m_autoSelected = m_chooser.GetSelected(); + + std::cout << "Auto Selected: \"" << m_autoSelected << "\".\n"; + + auto command = pathplanner::PathPlannerAuto(m_autoSelected); auto rot = command.getStartingPose().Rotation().Degrees(); auto offset = m_drive.GetHeading() - rot; - - m_autoSelected = m_chooser.GetSelected(); - std::cout << "Auto Selected: \"" << m_autoSelected << "\".\n"; - 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); + + // frc::SmartDashboard::PutData("Zero Odometry", new frc2::InstantCommand([this]() { m_drive.ResetRotation(); })); } 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/commands/CommandController.cpp b/src/main/cpp/commands/CommandController.cpp new file mode 100644 index 0000000..235decc --- /dev/null +++ b/src/main/cpp/commands/CommandController.cpp @@ -0,0 +1,107 @@ +#include "commands/CommandController.h" + +#include + +#include "Constants.h" + +frc2::CommandPtr CommandController::MoveToPositionL1() { + return m_subsystems.elevator->MoveToPositionAbsolute(CommandConstants::kElevatorL1Position) + .RaceWith(m_subsystems.coralIntake->MoveAtPercent(CommandConstants::kCoralIntakeRetainCoralSpeed)) + .AndThen(m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralL1Position)) + .AndThen(m_subsystems.algaeArm->MoveToPositionAbsolute(CommandConstants::kAlgaeArmReefPosition)); +} + +frc2::CommandPtr CommandController::MoveToPositionL2() { + return m_subsystems.elevator->MoveToPositionAbsolute(CommandConstants::kElevatorL2Position) + .RaceWith(m_subsystems.coralIntake->MoveAtPercent(CommandConstants::kCoralIntakeRetainCoralSpeed)) + .AndThen(m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralL2Position)) + .AndThen(m_subsystems.algaeArm->MoveToPositionAbsolute(CommandConstants::kAlgaeArmReefPosition)); +} + +frc2::CommandPtr CommandController::MoveToPositionL3() { + return m_subsystems.elevator->MoveToPositionAbsolute(CommandConstants::kElevatorL3Position) + .RaceWith(m_subsystems.coralIntake->MoveAtPercent(CommandConstants::kCoralIntakeRetainCoralSpeed)) + .AndThen(m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralL3Position)) + .AndThen(m_subsystems.algaeArm->MoveToPositionAbsolute(CommandConstants::kAlgaeArmReefPosition)); +} + +frc2::CommandPtr CommandController::FeedCoral() { + return m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralFeedPosition) + .AndThen(m_subsystems.algaeArm->MoveToPositionAbsolute(CommandConstants::kAlgaeArmReefPosition)) + .AndThen(m_subsystems.elevator->MoveToPositionAbsolute(CommandConstants::kElevatorFeedPosition)) + .RaceWith(m_subsystems.coralIntake->MoveAtPercent(CommandConstants::kCoralIntakeRetainCoralSpeed)) + .AndThen(m_subsystems.coralIntake->MoveAtPercent(CommandConstants::kCoralFeedSpeed) + .Until([this]() { + return m_subsystems.coralIntake->HasGamePiece(); + }) + .WithTimeout(CommandConstants::kCoralFeedTimeout)); +} + +frc2::CommandPtr CommandController::ExpelCoral() { + return m_subsystems.coralIntake->MoveAtPercent(CommandConstants::kCoralExpelSpeed); +} + +frc2::CommandPtr CommandController::IntakeAlgae() { + return m_subsystems.algaeArm->MoveToPositionAbsolute(CommandConstants::kAlgaeIntakePosition) + .AndThen(m_subsystems.algaeIntake->MoveAtPercent(CommandConstants::kAlgaeIntakeSpeed)) + // .Until([this]() { + // return m_subsystems.algaeIntake->HasGamePiece(); + // }) + // .WithTimeout(CommandConstants::kAlgaeIntakeTimeout); + ; +} + +frc2::CommandPtr CommandController::ExpelAlgae() { + return m_subsystems.algaeIntake->MoveAtPercent(CommandConstants::kAlgaeExpelSpeed); +} + +frc2::CommandPtr CommandController::RemoveAlgaeFromL2() { + return m_subsystems.elevator->MoveToPositionAbsolute(CommandConstants::kElevatorRemoveAlgaeFromL2Position) + .AndThen(m_subsystems.algaeArm->MoveToPositionAbsolute(CommandConstants::kRemoveAlgaeAlgaeArmPosition)) + .AndThen(m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralArmRemoveAlgaeFromL2Position)) + .AndThen(m_subsystems.coralIntake->MoveAtPercent(CommandConstants::kCoralExpelSpeed)) + .WithTimeout(CommandConstants::kRemoveAlgaeFromReefTimeout) + .RaceWith(m_subsystems.algaeIntake->MoveAtPercent(CommandConstants::kAlgaeIntakeSpeed)) + ; +} + +frc2::CommandPtr CommandController::RemoveAlgaeFromL3() { + return m_subsystems.elevator->MoveToPositionAbsolute(CommandConstants::kElevatorRemoveAlgaeFromL3Position) + .AndThen(m_subsystems.algaeArm->MoveToPositionAbsolute(CommandConstants::kRemoveAlgaeAlgaeArmPosition)) + .AndThen(m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralArmRemoveAlgaeFromL3Position)) + .AndThen(m_subsystems.coralIntake->MoveAtPercent(CommandConstants::kCoralFeedSpeed)) + .WithTimeout(CommandConstants::kRemoveAlgaeFromReefTimeout); +} + +frc2::CommandPtr CommandController::HomeElevator() { + return + m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralFeedPosition) + .AndThen(frc2::InstantCommand( + [this]() { + m_subsystems.elevator->SetEncoderPosition(ElevatorConstants::kMaxDistance); + } + ).ToPtr()) + .AndThen(frc2::InstantCommand( + [this]() { + std::cout << "Setting homing PID settings" << std::endl; + m_subsystems.elevator->UpdatePidSettings(ElevatorConstants::kHomePidSettings); + } + ).ToPtr()) + .AndThen(m_subsystems.elevator->MoveToPositionAbsolute(ElevatorConstants::kMinDistance)) + .FinallyDo( + [this]() { + std::cout << "Setting typical PID settings" << std::endl; + m_subsystems.elevator->UpdatePidSettings(ElevatorConstants::kElevatorPidSettings); + } + ); +} + +frc2::CommandPtr CommandController::ClimbUp() { + return m_subsystems.algaeArm->MoveToPositionAbsolute(CommandConstants::kClimbAlgaeArmPosition) + .AndThen(m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralClimbPosition)) + .AndThen(m_subsystems.elevator->MoveToPositionAbsolute(CommandConstants::kElevatorClimbUpPosition)); +} + +frc2::CommandPtr CommandController::ClimbDown() { + return m_subsystems.elevator->MoveToPositionAbsolute(CommandConstants::kElevatorClimbDownPosition); +} \ No newline at end of file diff --git a/src/main/cpp/subsystems/CoralArmSubsystem.cpp b/src/main/cpp/subsystems/CoralArmSubsystem.cpp index 9b51669..3b69d19 100644 --- a/src/main/cpp/subsystems/CoralArmSubsystem.cpp +++ b/src/main/cpp/subsystems/CoralArmSubsystem.cpp @@ -1,3 +1,3 @@ #include "subsystems/CoralArmSubsystem.h" -#include +#include \ No newline at end of file diff --git a/src/main/cpp/subsystems/DriveSubsystem.cpp b/src/main/cpp/subsystems/DriveSubsystem.cpp index f7cec5c..8ae8383 100644 --- a/src/main/cpp/subsystems/DriveSubsystem.cpp +++ b/src/main/cpp/subsystems/DriveSubsystem.cpp @@ -2,17 +2,24 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -#include "subsystems/DriveSubsystem.h" - +#include "subsystems/DriveSubsystem.h" +#include +#include +#include +#include +#include +#include #include #include #include +#include #include #include #include #include #include #include +#include #include #include "Constants.h" @@ -38,6 +45,8 @@ DriveSubsystem::DriveSubsystem() HAL_Report(HALUsageReporting::kResourceType_RobotDrive, HALUsageReporting::kRobotDriveSwerve_MaxSwerve); + frc::SmartDashboard::PutData("Field", &m_field); + pathplanner::RobotConfig config = pathplanner::RobotConfig::fromGUISettings(); pathplanner::AutoBuilder::configure( @@ -68,7 +77,7 @@ DriveSubsystem::DriveSubsystem() m_pidgey.SetYaw(0_deg, 100_ms); // Set our yaw to 0 degrees and wait up to 100 milliseconds for the setter to take affect m_pidgey.GetYaw().WaitForUpdate(100_ms); // And wait up to 100 milliseconds for the yaw to take affect - std::cout << "Set the yaw to 144 degrees, we are currently at " << m_pidgey.GetYaw() << std::endl; + // std::cout << "Set the yaw to 144 degrees, we are currently at " << m_pidgey.GetYaw() << std::endl; } void DriveSubsystem::Periodic() { @@ -78,6 +87,9 @@ void DriveSubsystem::Periodic() { {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), m_rearLeft.GetPosition(), m_rearRight.GetPosition()}); auto &yaw = m_pidgey.GetYaw(); + + m_field.SetRobotPose(m_odometry.GetPose()); + if (frc::Timer::GetFPGATimestamp() - currentTime >= GyroConstants::kPrintPeriod) { std::cout << "Yaw: " << yaw.GetValue().value() << std::endl; @@ -213,6 +225,10 @@ void DriveSubsystem::ResetOdometry(frc::Pose2d pose) { pose); } +void DriveSubsystem::ResetRotation() { + m_pidgey.Reset(); +} + void DriveSubsystem::OffsetRotation(frc::Rotation2d offset) { m_pidgey.SetYaw(offset.Degrees() + m_pidgey.GetYaw().GetValue()); m_odometry.ResetPosition( @@ -220,4 +236,16 @@ void DriveSubsystem::OffsetRotation(frc::Rotation2d offset) { {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), m_rearLeft.GetPosition(), m_rearRight.GetPosition()}, m_odometry.GetPose()); +} + +void DriveSubsystem::AddVisionMeasurement(const frc::Pose2d& visionMeasurement, + units::second_t timestamp) { + // poseEstimator.AddVisionMeasurement(visionMeasurement, timestamp); +} + +void DriveSubsystem::AddVisionMeasurement(const frc::Pose2d& visionMeasurement, + units::second_t timestamp, + const Eigen::Vector3d& stdDevs) { + // wpi::array newStdDevs{stdDevs(0), stdDevs(1), stdDevs(2)}; + // poseEstimator.AddVisionMeasurement(visionMeasurement, timestamp, newStdDevs); } \ No newline at end of file diff --git a/src/main/cpp/subsystems/ElevatorSubsystem.cpp b/src/main/cpp/subsystems/ElevatorSubsystem.cpp new file mode 100644 index 0000000..2fc6ae9 --- /dev/null +++ b/src/main/cpp/subsystems/ElevatorSubsystem.cpp @@ -0,0 +1,33 @@ +#include + +void ElevatorSubsystem::SetEncoderPosition(units::meter_t pos) { + // Basesingleaxis applies a conversion factor so we don't provide rotations + m_enc.SetPosition(pos.value()); +} + +bool ElevatorSubsystem::GetMinLimitSwitch() { + return m_leadMotor.GetReverseLimitSwitch().Get(); +} + +bool ElevatorSubsystem::GetMaxLimitSwitch() { + return m_leadMotor.GetForwardLimitSwitch().Get(); +} + +void ElevatorSubsystem::Periodic() { + if (GetMaxLimitSwitch()) { + std::cout << "Elevator at max" << std::endl; + SetEncoderPosition(ElevatorConstants::kMaxDistance); + } else if (GetMinLimitSwitch()) { + std::cout << "Elevator at min" << std::endl; + SetEncoderPosition(ElevatorConstants::kMinDistance); + } + + frc::SmartDashboard::PutBoolean("Forward limit switch state", GetMaxLimitSwitch()); + frc::SmartDashboard::PutBoolean("Reverse limit switch state", GetMinLimitSwitch()); + + subzero::LinearSingleAxisSubsystem::Periodic(); +} + +void ElevatorSubsystem::UpdatePidSettings(subzero::PidSettings settings) { + m_elevatorController.UpdatePidSettings(settings); +} \ No newline at end of file diff --git a/src/main/cpp/subsystems/IntakeSubsystem.cpp b/src/main/cpp/subsystems/IntakeSubsystem.cpp index 76b2741..b6809d6 100644 --- a/src/main/cpp/subsystems/IntakeSubsystem.cpp +++ b/src/main/cpp/subsystems/IntakeSubsystem.cpp @@ -1,5 +1,7 @@ #include +#include + #include #include @@ -31,4 +33,9 @@ frc2::CommandPtr IntakeSubsystem::Stop() { }, {this} ).ToPtr(); +} + +bool IntakeSubsystem::HasGamePiece() { + std::cout << "Motor amperage is " << m_motor.GetOutputCurrent() << " amps." << std::endl; + return m_motor.GetOutputCurrent() > m_hasGamePieceCurrent; } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Out Auto.auto b/src/main/deploy/pathplanner/autos/Center Auto (0 Coral).auto similarity index 63% rename from src/main/deploy/pathplanner/autos/Out Auto.auto rename to src/main/deploy/pathplanner/autos/Center Auto (0 Coral).auto index c419b83..262d9c8 100644 --- a/src/main/deploy/pathplanner/autos/Out Auto.auto +++ b/src/main/deploy/pathplanner/autos/Center Auto (0 Coral).auto @@ -4,10 +4,16 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "Home" + } + }, { "type": "path", "data": { - "pathName": "Out" + "pathName": "Center Straight" } } ] diff --git a/src/main/deploy/pathplanner/autos/Center Auto (1 Coral).auto b/src/main/deploy/pathplanner/autos/Center Auto (1 Coral).auto new file mode 100644 index 0000000..97bbf04 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center Auto (1 Coral).auto @@ -0,0 +1,44 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Home" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Start To Bottom Left Reef" + } + }, + { + "type": "named", + "data": { + "name": "To L1 Position" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Expel Coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Center To Left Source.auto b/src/main/deploy/pathplanner/autos/Center To Left Source.auto new file mode 100644 index 0000000..3cdd981 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center To Left Source.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Home" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Start To Reef Left Side Left" + } + }, + { + "type": "named", + "data": { + "name": "Expel Coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Reef to Far Left Source" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "path", + "data": { + "pathName": "Far Left Source to Right Reef" + } + }, + { + "type": "named", + "data": { + "name": "Expel Coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Center To Right Source (2 Coral).auto b/src/main/deploy/pathplanner/autos/Center To Right Source (2 Coral).auto new file mode 100644 index 0000000..b2544c8 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center To Right Source (2 Coral).auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Home" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Start To Reef Right Side Right" + } + }, + { + "type": "named", + "data": { + "name": "Expel Coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef Right Side Right To Far Right Source" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "path", + "data": { + "pathName": "Far Right Source To Reef Right Side Left" + } + }, + { + "type": "named", + "data": { + "name": "Expel Coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left Auto (2 Coral).auto b/src/main/deploy/pathplanner/autos/Left Auto (2 Coral).auto new file mode 100644 index 0000000..6c246f0 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left Auto (2 Coral).auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Home" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Start to Reef Left Side Left" + } + }, + { + "type": "named", + "data": { + "name": "Expel Coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Reef to Far Left Source" + } + }, + { + "type": "named", + "data": { + "name": "Pause" + } + }, + { + "type": "path", + "data": { + "pathName": "Far Left Source to Right Reef" + } + }, + { + "type": "named", + "data": { + "name": "Expel Coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left To Left Reef (1 Coral).auto b/src/main/deploy/pathplanner/autos/Left To Left Reef (1 Coral).auto new file mode 100644 index 0000000..a57a9a8 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left To Left Reef (1 Coral).auto @@ -0,0 +1,44 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Home" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Border Start To Bottom Left Reef" + } + }, + { + "type": "named", + "data": { + "name": "To L1 Position" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Expel Coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right Auto (2 Coral).auto b/src/main/deploy/pathplanner/autos/Right Auto (2 Coral).auto new file mode 100644 index 0000000..f756f84 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right Auto (2 Coral).auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Home" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Start To Reef Right Side Right" + } + }, + { + "type": "named", + "data": { + "name": "Expel Coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Reef Right Side Right To Far Right Source" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "path", + "data": { + "pathName": "Far Right Source To Reef Right Side Left" + } + }, + { + "type": "named", + "data": { + "name": "Expel Coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right To Right Reef (1 Coral).auto b/src/main/deploy/pathplanner/autos/Right To Right Reef (1 Coral).auto new file mode 100644 index 0000000..391579f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right To Right Reef (1 Coral).auto @@ -0,0 +1,44 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Home" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Border Start To Bottom Right Reef" + } + }, + { + "type": "named", + "data": { + "name": "To L1 Position" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Expel Coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Spin Auto.auto b/src/main/deploy/pathplanner/autos/Spin Auto.auto deleted file mode 100644 index dff1ae6..0000000 --- a/src/main/deploy/pathplanner/autos/Spin Auto.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Spin" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Left Reef To Slight Rear.path b/src/main/deploy/pathplanner/paths/Bottom Left Reef To Slight Rear.path new file mode 100644 index 0000000..a27372e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bottom Left Reef To Slight Rear.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.957249999999999, + "y": 4.210249999999999 + }, + "prevControl": null, + "nextControl": { + "x": 6.4545, + "y": 4.2589999999999995 + }, + "isLocked": false, + "linkedName": "Rear Left Reef" + }, + { + "anchor": { + "x": 7.234500000000001, + "y": 4.210249999999999 + }, + "prevControl": { + "x": 6.46425, + "y": 4.22 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -177.87890360333859 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Reef Bottom To Slight Rear.path b/src/main/deploy/pathplanner/paths/Center Reef Bottom To Slight Rear.path new file mode 100644 index 0000000..3cf67dc --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Reef Bottom To Slight Rear.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.885963114754098, + "y": 4.00701844262295 + }, + "prevControl": null, + "nextControl": { + "x": 6.885963114754098, + "y": 4.00701844262295 + }, + "isLocked": false, + "linkedName": "Reef Bottom Center" + }, + { + "anchor": { + "x": 6.571278409090909, + "y": 4.00701844262295 + }, + "prevControl": { + "x": 5.864003818926975, + "y": 4.030913934426229 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Slight Rear" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -176.70638131331938 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -177.5633517531899 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Start To Bottom Left Reef.path b/src/main/deploy/pathplanner/paths/Center Start To Bottom Left Reef.path new file mode 100644 index 0000000..778556e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Start To Bottom Left Reef.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.67325, + "y": 4.015249999999999 + }, + "prevControl": null, + "nextControl": { + "x": 6.961500000000001, + "y": 4.05425 + }, + "isLocked": false, + "linkedName": "Center Start" + }, + { + "anchor": { + "x": 5.957249999999999, + "y": 4.210249999999999 + }, + "prevControl": { + "x": 6.50325, + "y": 4.17125 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Rear Left Reef" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -178.93908830973575 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Start To Reef Left Side Left.path b/src/main/deploy/pathplanner/paths/Center Start To Reef Left Side Left.path new file mode 100644 index 0000000..aad7556 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Start To Reef Left Side Left.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.47825, + "y": 3.89825 + }, + "prevControl": null, + "nextControl": { + "x": 7.059000000000001, + "y": 5.16575 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.46639344262295, + "y": 5.205788934426229 + }, + "prevControl": { + "x": 6.1425, + "y": 5.96525 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Reef" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "To L2 Position", + "waypointRelativePos": 0.22133995037220952, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "To L2 Position" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -120.19162296095763 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -120.19162296095763 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Start To Reef Right Side Right.path b/src/main/deploy/pathplanner/paths/Center Start To Reef Right Side Right.path new file mode 100644 index 0000000..a2bbda6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Start To Reef Right Side Right.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.45875, + "y": 4.112749999999999 + }, + "prevControl": null, + "nextControl": { + "x": 6.58125, + "y": 3.547249999999999 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.403652597402597, + "y": 2.8806412337662333 + }, + "prevControl": { + "x": 6.230250000000001, + "y": 1.63625 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Reef Right Side Right" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "To L2 Position", + "waypointRelativePos": 0.29875930521091965, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "To L2 Position" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 125.09581678702604 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 125.75388725443663 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Start to Reef Bottom Center.path b/src/main/deploy/pathplanner/paths/Center Start to Reef Bottom Center.path new file mode 100644 index 0000000..9379b09 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Start to Reef Bottom Center.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.67325, + "y": 4.015249999999999 + }, + "prevControl": null, + "nextControl": { + "x": 7.3709999999999996, + "y": 3.9957499999999992 + }, + "isLocked": false, + "linkedName": "Center Start" + }, + { + "anchor": { + "x": 5.885963114754098, + "y": 4.00701844262295 + }, + "prevControl": { + "x": 6.480713114754097, + "y": 3.997268442622951 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Reef Bottom Center" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -177.5633517531899 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -178.93908830973575 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Straight.path b/src/main/deploy/pathplanner/paths/Center Straight.path new file mode 100644 index 0000000..c19e3b9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Straight.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.67325, + "y": 4.015249999999999 + }, + "prevControl": null, + "nextControl": { + "x": 6.870426135794473, + "y": 4.020014108633828 + }, + "isLocked": false, + "linkedName": "Center Start" + }, + { + "anchor": { + "x": 6.172414772727272, + "y": 4.015249999999999 + }, + "prevControl": { + "x": 6.831003392858172, + "y": 4.015249999999999 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -176.36809041722702 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -178.93908830973575 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center To Rear Right Reef.path b/src/main/deploy/pathplanner/paths/Center To Rear Right Reef.path new file mode 100644 index 0000000..102b836 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center To Rear Right Reef.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.67325, + "y": 4.015249999999999 + }, + "prevControl": null, + "nextControl": { + "x": 5.382, + "y": 5.204749999999999 + }, + "isLocked": false, + "linkedName": "Center Start" + }, + { + "anchor": { + "x": 5.957249999999999, + "y": 4.210249999999999 + }, + "prevControl": { + "x": 7.029749999999999, + "y": 5.409499999999999 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Rear Left Reef" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -178.93908830973575 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Far Left Source to Right Reef.path b/src/main/deploy/pathplanner/paths/Far Left Source to Right Reef.path new file mode 100644 index 0000000..03528dd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Far Left Source to Right Reef.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.766396103896104, + "y": 7.34411525974026 + }, + "prevControl": null, + "nextControl": { + "x": 2.766396103896104, + "y": 7.34411525974026 + }, + "isLocked": false, + "linkedName": "Far Left Source" + }, + { + "anchor": { + "x": 5.166700819672132, + "y": 5.457530737704917 + }, + "prevControl": { + "x": 7.036782786885246, + "y": 6.6563012295081965 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "", + "waypointRelativePos": 0.2619851451721815, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "To L2 Position" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -120.69972255081439 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -52.00126755749546 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Far Right Source To Reef Right Side Left.path b/src/main/deploy/pathplanner/paths/Far Right Source To Reef Right Side Left.path new file mode 100644 index 0000000..81311fa --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Far Right Source To Reef Right Side Left.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.6809253246753246, + "y": 0.7391233766233768 + }, + "prevControl": null, + "nextControl": { + "x": 3.978, + "y": 1.3729999999999996 + }, + "isLocked": false, + "linkedName": "Far Right Source" + }, + { + "anchor": { + "x": 5.151988636363637, + "y": 2.6812094155844157 + }, + "prevControl": { + "x": 5.343, + "y": 1.1389999999999998 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "", + "waypointRelativePos": 0.2722427224272228, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "To L2 Position" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 122.58570735391453 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 53.56914187983765 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Border Start To Bottom Left Reef.path b/src/main/deploy/pathplanner/paths/Left Border Start To Bottom Left Reef.path new file mode 100644 index 0000000..d1dbd0c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Border Start To Bottom Left Reef.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.468721590909091, + "y": 7.5100710227272724 + }, + "prevControl": null, + "nextControl": { + "x": 6.202329545454546, + "y": 6.403224431818181 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.08025641025641, + "y": 5.046730769230769 + }, + "prevControl": { + "x": 5.9776995920745915, + "y": 5.894315996503496 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -117.14968169778322 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 179.30130561701654 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Reef to Far Left Source.path b/src/main/deploy/pathplanner/paths/Left Reef to Far Left Source.path new file mode 100644 index 0000000..cc2de16 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Reef to Far Left Source.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.46639344262295, + "y": 5.205788934426229 + }, + "prevControl": null, + "nextControl": { + "x": 6.50325, + "y": 6.95 + }, + "isLocked": false, + "linkedName": "Left Reef" + }, + { + "anchor": { + "x": 1.766396103896104, + "y": 7.34411525974026 + }, + "prevControl": { + "x": 2.65909090909091, + "y": 6.4324269480519485 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Far Left Source" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "", + "waypointRelativePos": 0.237677245104656, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "To Feed Position" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -52.00126755749546 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -120.19162296095763 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Start to Reef Left Side Left.path b/src/main/deploy/pathplanner/paths/Left Start to Reef Left Side Left.path new file mode 100644 index 0000000..5cf9299 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Start to Reef Left Side Left.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.763595779220778, + "y": 7.424837662337662 + }, + "prevControl": null, + "nextControl": { + "x": 6.695211038961039, + "y": 6.408685064935064 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.46639344262295, + "y": 5.205788934426229 + }, + "prevControl": { + "x": 6.685714285714286, + "y": 6.418181818181818 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Reef" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "", + "waypointRelativePos": 0.35, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "To L2 Position" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -120.19162296095763 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -113.76710770458172 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef Right Side Right To Far Right Source.path b/src/main/deploy/pathplanner/paths/Reef Right Side Right To Far Right Source.path new file mode 100644 index 0000000..4b9b26c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Reef Right Side Right To Far Right Source.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.403652597402597, + "y": 2.8806412337662333 + }, + "prevControl": null, + "nextControl": { + "x": 6.15225, + "y": 1.63625 + }, + "isLocked": false, + "linkedName": "Reef Right Side Right" + }, + { + "anchor": { + "x": 1.6809253246753246, + "y": 0.7391233766233768 + }, + "prevControl": { + "x": 4.251000000000001, + "y": 1.7044999999999992 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Far Right Source" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "", + "waypointRelativePos": 0.7511275112751122, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "To Feed Position" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 53.56914187983765 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 125.09581678702604 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Border Start To Bottom Right Reef.path b/src/main/deploy/pathplanner/paths/Right Border Start To Bottom Right Reef.path new file mode 100644 index 0000000..589a8ba --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Border Start To Bottom Right Reef.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.678125, + "y": 0.57981534090909 + }, + "prevControl": null, + "nextControl": { + "x": 6.4915056818181816, + "y": 1.6966335227272735 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.111217948717948, + "y": 3.0239102564102565 + }, + "prevControl": { + "x": 6.23800772144522, + "y": 1.9569500291375306 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 120.7627195342391 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -178.83086067209268 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Start To Reef Right Side Right.path b/src/main/deploy/pathplanner/paths/Right Start To Reef Right Side Right.path new file mode 100644 index 0000000..5a1f5ef --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Start To Reef Right Side Right.path @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.611647727272728, + "y": 0.6536525974025983 + }, + "prevControl": null, + "nextControl": { + "x": 6.339079787908425, + "y": 1.8217499737682514 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.403652597402597, + "y": 2.8806412337662333 + }, + "prevControl": { + "x": 6.405560064935065, + "y": 1.9214691558441552 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Reef Right Side Right" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "To L2 Position", + "waypointRelativePos": 0.21141439205955234, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 125.09581678702604 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 133.60281897270357 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Slight Rear To Center Reef Bottom Left.path b/src/main/deploy/pathplanner/paths/Slight Rear To Center Reef Bottom Left.path new file mode 100644 index 0000000..0cb0e60 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Slight Rear To Center Reef Bottom Left.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.571278409090909, + "y": 4.00701844262295 + }, + "prevControl": null, + "nextControl": { + "x": 6.043819392697466, + "y": 3.9949508196721304 + }, + "isLocked": false, + "linkedName": "Slight Rear" + }, + { + "anchor": { + "x": 5.909938524590164, + "y": 4.210809426229508 + }, + "prevControl": { + "x": 6.4544999999999995, + "y": 4.198741803278688 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -177.5633517531899 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -176.70638131331938 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index 14aeb18..c2b9279 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -13,6 +13,12 @@ #include #include #include +#include +#include +#include +#include +#include + #include #include @@ -34,6 +40,10 @@ #include #include +#ifndef M_PI + #define M_PI 3.141592653589793238462643383279502884197 +#endif + using SparkMaxPidController = subzero::PidMotorController::Constraints kThetaControllerConstraints; + +constexpr units::second_t kFeedWaitTime = 2_s; } // namespace AutoConstants namespace OIConstants { @@ -164,20 +179,31 @@ namespace ElevatorConstants { const int kFollowerElevatorMotorCanId = 7; const int kBottomLimitSwitchPort = 1; + const int kTopLimitSwitchPort = 2; // Placeholder values - const double kElevatorP = 100.0; + const double kElevatorP = 70.0; const double kElevatorI = 0.0; const double kElevatorD = 0.0; const double kElevatorIZone = 0.0; const double kElevatorFF = 0.0; + const subzero::PidSettings kElevatorPidSettings = { + 40.0, 0.0, 0.0, + 0.0, 0.0, false}; + + const subzero::PidSettings kHomePidSettings = { + 10.0, 0.0, 0.0, + 0.0, 0.0, false}; + + constexpr units::meter_t kElevatorStartPosition = 4.875_in; + 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::meter_t kMaxDistance = 20.1_in; + constexpr units::meter_t kRelativeDistancePerRev = (1.685_in * M_PI) / 36; // Pitch diameter of gear with 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; @@ -201,9 +227,11 @@ namespace AlgaeArmConstants{ constexpr int kIntakeMotorId = 6; constexpr double kP = 0.1; constexpr double kI = 0.0; - constexpr double kD = 0.001; + constexpr double kD = 0.0; constexpr double kIZone = 0.0; constexpr double kFF = 0.0; + + constexpr double kHasAlgaeCurrent = 35; constexpr units::revolutions_per_minute_t kMaxRpm = 5676_rpm; constexpr units::degree_t kHomeRotation = 0_deg; @@ -236,21 +264,24 @@ namespace AlgaeArmConstants{ namespace CoralArmConstants{ constexpr int kArmMotorId = 16; constexpr int kIntakeMotorId = 15; - constexpr double kP = 0.5; + constexpr double kP = 0.3; 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 kMinRotation = 50_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 units::degrees_per_second_t kDefaultVelocity = 100_deg_per_s; constexpr double kVelocityScalar = 1.0; - constexpr units::degree_t kTolerance = 2_deg; + constexpr units::degree_t kTolerance = 0.5_deg; constexpr units::meter_t kArmLength = 0.2_m; + + // placeholder + constexpr double kHasCoralCurrent = 36; static const subzero::SingleAxisMechanism kCoralArmMechanism = { // length @@ -279,13 +310,15 @@ namespace ClimberConstants{ 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 kMaxRotation = 110_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; + + constexpr double kPercentageScalar = 0.8; static const subzero::SingleAxisMechanism kClimberMechanism = { // length @@ -303,4 +336,92 @@ namespace ClimberConstants{ const frc::TrapezoidProfile::Constraints kRotationalAxisConstraints; -} \ No newline at end of file +} + +namespace CommandConstants { + // Edging placeholfer + constexpr units::meter_t kElevatorL1Position = 0.25_m; + constexpr units::meter_t kElevatorL2Position = 0.33_m; + constexpr units::meter_t kElevatorL3Position = 0.51_m; + constexpr units::meter_t kElevatorFeedPosition = 0.2_in; + constexpr units::meter_t kElevatorRemoveAlgaeFromL2Position = 0.38_m; + constexpr units::meter_t kElevatorRemoveAlgaeFromL3Position = 0.50_m; + constexpr units::meter_t kElevatorClimbUpPosition = 20.1_in; + constexpr units::meter_t kElevatorClimbDownPosition = 0.0_in; + + constexpr units::degree_t kCoralL1Position = 249_deg; + constexpr units::degree_t kCoralL2Position = 240_deg; + constexpr units::degree_t kCoralL3Position = 218_deg; + constexpr units::degree_t kCoralFeedPosition = 77_deg; + constexpr units::degree_t kCoralHomePosition = 110_deg; + constexpr units::degree_t kCoralClimbPosition = 50_deg; + constexpr units::degree_t kCoralArmRemoveAlgaeFromL2Position = 216_deg; + constexpr units::degree_t kCoralArmRemoveAlgaeFromL3Position = 205_deg; + + constexpr units::degree_t kAlgaeIntakePosition = 38_deg; + constexpr units::degree_t kAlgaeStorePosition = 30_deg; + constexpr units::degree_t kAlgaeStowPosition = 0_deg; + constexpr units::degree_t kAlgaeArmReefPosition = 20_deg; + constexpr units::degree_t kRemoveAlgaeAlgaeArmPosition = 10_deg; + constexpr units::degree_t kClimbAlgaeArmPosition = 80_deg; + + constexpr double kCoralFeedSpeed = 0.25; + constexpr double kCoralExpelSpeed = -0.25; + constexpr double kCoralIntakeRetainCoralSpeed = 0.25; + + constexpr double kAlgaeIntakeSpeed = -0.3; + constexpr double kAlgaeExpelSpeed = 0.65; + + // Placeholder values + constexpr units::degree_t kClimberDownAngle = 0_deg; + constexpr units::degree_t kClimberUpAngle = 110_deg; + + // Placeholder + constexpr units::second_t kCoralFeedTimeout = 3_s; + constexpr units::second_t kAlgaeIntakeTimeout = 5_s; + constexpr units::second_t kRemoveAlgaeFromReefTimeout = 3_s; + constexpr units::second_t kExpelCoralTimeout = 1_s; + constexpr units::second_t kWaitBeforeL2 = 0.5_s; +} + +namespace VisionConstants { +static constexpr std::string_view kFrontCamera{"PhotonVision"}; +static constexpr std::string_view kRearCamera{"PhotonVision2"}; +static const frc::Transform3d kRobotToCam2{ + frc::Translation3d{2.147_in, 0_in, 23.369_in}, + frc::Rotation3d{-90_deg, -0_deg, -140_deg}}; +static const frc::Transform3d kRobotToCam{ + frc::Translation3d{5.714_in, 0_in, 23.533_in}, + frc::Rotation3d{90_deg, -23.461_deg, 140_deg}}; +constexpr photon::PoseStrategy kPoseStrategy = + photon::PoseStrategy::MULTI_TAG_PNP_ON_COPROCESSOR; +static const frc::AprilTagFieldLayout kTagLayout{ + frc::LoadAprilTagLayoutField(frc::AprilTagField::k2024Crescendo)}; +static const Eigen::Matrix kSingleTagStdDevs{4, 4, 8}; +static const Eigen::Matrix kMultiTagStdDevs{0.5, 0.5, 1}; + +const std::string kLimelightName = "limelight"; +constexpr double kKnownPixelWidth = 58; +constexpr units::inch_t kNoteWidth = 14_in; +constexpr units::inch_t kKnownCalibrationDistance = 60_in; +constexpr units::inch_t kCalibrationDistanceAreaPercentage = + kKnownCalibrationDistance * kKnownPixelWidth; +constexpr auto focalLength = kCalibrationDistanceAreaPercentage / kNoteWidth; + +constexpr units::degree_t kCameraAngle = -20_deg; +constexpr units::inch_t kCameraLensHeight = 15_in; +constexpr double kConfidenceThreshold = 0.3; +constexpr double kTrigDistancePercentage = 0.5; +constexpr double kAreaPercentageThreshold = 0.04; +constexpr uint8_t kMaxTrackedTargets = 10; + +constexpr double kMinAngleDeg = -30.0; +constexpr double kMaxAngleDeg = 30.0; + +static const wpi::array kDrivetrainStd = {0.1, 0.1, 0.1}; +static const wpi::array kVisionStd = {0.9, 0.9, 0.9}; + +constexpr units::degree_t kGamepieceRotation = 180_deg; +constexpr frc::Pose2d kSimGamepiecePose = + frc::Pose2d(7_m, 4_m, frc::Rotation2d(kGamepieceRotation)); +} // namespace VisionConstants \ No newline at end of file diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index 8ef2672..0f07fbf 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -16,6 +16,7 @@ #include #include #include +#include #include "CommonCompile.h" @@ -26,6 +27,7 @@ #include "subsystems/AlgaeArmSubsystem.h" #include "subsystems/CoralArmSubsystem.h" #include "subsystems/ClimberSubsystem.h" +#include "commands/CommandController.h" /** * This class is where the bulk of the robot should be declared. Since @@ -49,6 +51,9 @@ class RobotContainer { frc2::CommandXboxController m_driverController{ OperatorConstants::kDriverControllerPort}; + frc2::CommandXboxController m_operatorController{ + OperatorConstants::kOperatorControllerPort}; + // The robot's subsystems are defined here... DriveSubsystem m_drive; @@ -65,10 +70,19 @@ class RobotContainer { ElevatorSubsystem m_elevator{(frc::MechanismObject2d*)m_elevatorMech.GetRoot("Elevator", 0.25, 0.25)}; AlgaeArmSubsystem m_algaeArm; - IntakeSubsystem m_algaeIntake{AlgaeArmConstants::kIntakeMotorId}; + IntakeSubsystem m_algaeIntake{AlgaeArmConstants::kIntakeMotorId, AlgaeArmConstants::kHasAlgaeCurrent, true}; CoralArmSubsystem m_coralArm; - IntakeSubsystem m_coralIntake{CoralArmConstants::kIntakeMotorId}; - - ClimberSubsystem m_climber; + IntakeSubsystem m_coralIntake{CoralArmConstants::kIntakeMotorId, CoralArmConstants::kHasCoralCurrent, true}; + + Subsystems_t subsystems = { + .algaeArm = &m_algaeArm, + .coralArm = &m_coralArm, + .elevator = &m_elevator, + .coralIntake = &m_coralIntake, + .algaeIntake = &m_algaeIntake, + .climber = NULL + }; + + CommandController m_commandController{subsystems}; }; \ No newline at end of file diff --git a/src/main/include/commands/CommandController.h b/src/main/include/commands/CommandController.h new file mode 100644 index 0000000..128230a --- /dev/null +++ b/src/main/include/commands/CommandController.h @@ -0,0 +1,43 @@ +#pragma once + +#include + +#include "subsystems/AlgaeArmSubsystem.h" +#include "subsystems/CoralArmSubsystem.h" +#include "subsystems/ElevatorSubsystem.h" +#include "subsystems/IntakeSubsystem.h" +#include "subsystems/ClimberSubsystem.h" + +typedef struct { + AlgaeArmSubsystem *algaeArm; + CoralArmSubsystem *coralArm; + ElevatorSubsystem *elevator; + IntakeSubsystem *coralIntake; + IntakeSubsystem *algaeIntake; + ClimberSubsystem *climber; +} Subsystems_t; + +class CommandController { +public: + CommandController(Subsystems_t subsystems) : m_subsystems{subsystems} {} + + frc2::CommandPtr MoveToPositionL1(); + frc2::CommandPtr MoveToPositionL2(); + frc2::CommandPtr MoveToPositionL3(); + + frc2::CommandPtr FeedCoral(); + frc2::CommandPtr ExpelCoral(); + + frc2::CommandPtr IntakeAlgae(); + frc2::CommandPtr ExpelAlgae(); + + frc2::CommandPtr RemoveAlgaeFromL2(); + frc2::CommandPtr RemoveAlgaeFromL3(); + + frc2::CommandPtr HomeElevator(); + + frc2::CommandPtr ClimbUp(); + frc2::CommandPtr ClimbDown(); +private: + Subsystems_t m_subsystems; +}; \ No newline at end of file diff --git a/src/main/include/subsystems/AlgaeArmSubsystem.h b/src/main/include/subsystems/AlgaeArmSubsystem.h index 912b816..7f3ce03 100644 --- a/src/main/include/subsystems/AlgaeArmSubsystem.h +++ b/src/main/include/subsystems/AlgaeArmSubsystem.h @@ -61,7 +61,7 @@ class AlgaeArmSubsystem : public subzero::RotationalSingleAxisSubsystem(m_coralArmController) : dynamic_cast(simCoralArmController), { - CoralArmConstants::kHomeRotation, + // Min distance + CoralArmConstants::kMinRotation, // Max distance CoralArmConstants::kMaxRotation, // Distance per revolution of relative encoder @@ -45,8 +46,8 @@ class CoralArmSubsystem : public subzero::RotationalSingleAxisSubsystem +#include #include #include #include +#include +#include #include #include #include @@ -22,13 +25,18 @@ #include #include #include +#include +#include +#include +#include +#include #include "Constants.h" #include "MAXSwerveModule.h" class DriveSubsystem : public frc2::SubsystemBase { public: - DriveSubsystem(); + explicit DriveSubsystem(); /** * Will be called periodically whenever the CommandScheduler runs. @@ -104,7 +112,7 @@ class DriveSubsystem : public frc2::SubsystemBase { void OffsetRotation(frc::Rotation2d rot); - void ResetOdometry(); + void ResetRotation(); frc::SwerveDriveKinematics<4> m_driveKinematics{ frc::Translation2d{DriveConstants::kWheelBase / 2, @@ -116,6 +124,12 @@ class DriveSubsystem : public frc2::SubsystemBase { frc::Translation2d{-DriveConstants::kWheelBase / 2, -DriveConstants::kTrackWidth / 2}}; + void AddVisionMeasurement(const frc::Pose2d& visionMeasurement, + units::second_t timestamp); + void AddVisionMeasurement(const frc::Pose2d& visionMeasurement, + units::second_t timestamp, + const Eigen::Vector3d& stdDevs); + private: // Components (e.g. motor controllers and sensors) should generally be // declared private and exposed only through public methods. @@ -134,4 +148,23 @@ class DriveSubsystem : public frc2::SubsystemBase { // Odometry class for tracking robot pose // 4 defines the number of modules frc::SwerveDriveOdometry<4> m_odometry; -}; \ No newline at end of file + + frc::SwerveDrivePoseEstimator<4> poseEstimator{ + m_driveKinematics, + GetHeading(), + {m_frontLeft.GetPosition(), m_rearLeft.GetPosition(), + m_frontRight.GetPosition(), m_rearRight.GetPosition()}, + frc::Pose2d{0_m, 0_m, 0_rad}, + VisionConstants::kDrivetrainStd, + VisionConstants::kVisionStd}; + nt::StructArrayPublisher m_publisher; + nt::StructArrayPublisher m_desiredPublisher; + + // Pose viewing + frc::Field2d m_field; + frc::Pose2d m_lastGoodPosition; + + subzero::PhotonVisionEstimators* m_vision; +}; + + diff --git a/src/main/include/subsystems/ElevatorSubsystem.h b/src/main/include/subsystems/ElevatorSubsystem.h index e8c8466..6e2af10 100644 --- a/src/main/include/subsystems/ElevatorSubsystem.h +++ b/src/main/include/subsystems/ElevatorSubsystem.h @@ -35,7 +35,7 @@ class ElevatorSubsystem : public subzero::LinearSingleAxisSubsystem #include +#include +#include class IntakeSubsystem : public frc2::SubsystemBase { public: - IntakeSubsystem(const int id) : m_motor{id, rev::spark::SparkLowLevel::MotorType::kBrushless} {} + IntakeSubsystem(const int id, double hasGamePieceCurrent, bool idleOnBrakeMode) + : m_motor{id, rev::spark::SparkLowLevel::MotorType::kBrushless}, m_hasGamePieceCurrent{hasGamePieceCurrent} { + m_config.SetIdleMode(idleOnBrakeMode ? rev::spark::SparkBaseConfig::IdleMode::kBrake + : rev::spark::SparkBaseConfig::IdleMode::kCoast); + + m_motor.Configure(m_config, rev::spark::SparkBase::ResetMode::kNoResetSafeParameters, + rev::spark::SparkBase::PersistMode::kPersistParameters); + } frc2::CommandPtr MoveAtPercent(double percent); frc2::CommandPtr Stop(); + + bool HasGamePiece(); private: rev::spark::SparkMax m_motor; + rev::spark::SparkMaxConfig m_config; + + double m_hasGamePieceCurrent; }; \ No newline at end of file diff --git a/vendordeps/PathplannerLib-2025.2.2.json b/vendordeps/PathplannerLib-2025.2.3.json similarity index 87% rename from vendordeps/PathplannerLib-2025.2.2.json rename to vendordeps/PathplannerLib-2025.2.3.json index a5bf9ee..9151ce4 100644 --- a/vendordeps/PathplannerLib-2025.2.2.json +++ b/vendordeps/PathplannerLib-2025.2.3.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.2.2.json", + "fileName": "PathplannerLib-2025.2.3.json", "name": "PathplannerLib", - "version": "2025.2.2", + "version": "2025.2.3", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.2.2" + "version": "2025.2.3" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.2.2", + "version": "2025.2.3", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6-25.2.1.json b/vendordeps/Phoenix6-25.3.0.json similarity index 86% rename from vendordeps/Phoenix6-25.2.1.json rename to vendordeps/Phoenix6-25.3.0.json index 1397da1..a7e4762 100644 --- a/vendordeps/Phoenix6-25.2.1.json +++ b/vendordeps/Phoenix6-25.3.0.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-25.2.1.json", + "fileName": "Phoenix6-25.3.0.json", "name": "CTRE-Phoenix (v6)", - "version": "25.2.1", + "version": "25.3.0", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.2.1" + "version": "25.3.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.2.1", + "version": "25.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.1", + "version": "25.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.2.1", + "version": "25.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.1", + "version": "25.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.1", + "version": "25.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.1", + "version": "25.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.1", + "version": "25.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.1", + "version": "25.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.1", + "version": "25.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.1", + "version": "25.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.1", + "version": "25.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.1", + "version": "25.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,21 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.1", + "version": "25.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -210,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.2.1", + "version": "25.3.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -226,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.1", + "version": "25.3.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -242,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.2.1", + "version": "25.3.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -258,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.1", + "version": "25.3.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -274,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.1", + "version": "25.3.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -290,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.1", + "version": "25.3.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -306,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.1", + "version": "25.3.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -322,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.1", + "version": "25.3.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -338,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.1", + "version": "25.3.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -354,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.1", + "version": "25.3.0", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -370,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.1", + "version": "25.3.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -386,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.1", + "version": "25.3.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -402,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.1", + "version": "25.3.0", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -414,6 +428,22 @@ "osxuniversal" ], "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.3.0", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" } ] } \ No newline at end of file