From ec53b91432f8da014f684df3a2456bdc6bc16d5b Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Tue, 18 Feb 2025 15:47:04 -0600 Subject: [PATCH 01/27] Basic command controller implementation --- src/main/cpp/RobotContainer.cpp | 5 + src/main/cpp/commands/CommandController.cpp | 115 ++++++++++++++++++ .../deploy/pathplanner/autos/Out Auto.auto | 8 +- .../deploy/pathplanner/autos/Spin Auto.auto | 6 + src/main/include/Constants.h | 39 +++++- src/main/include/RobotContainer.h | 13 ++ src/main/include/commands/CommandController.h | 38 ++++++ 7 files changed, 219 insertions(+), 5 deletions(-) create mode 100644 src/main/cpp/commands/CommandController.cpp create mode 100644 src/main/include/commands/CommandController.h diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index cdbaf60..f2b0339 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -19,6 +19,7 @@ #include #include +#include #include "commands/Autos.h" #include "commands/ExampleCommand.h" @@ -34,6 +35,10 @@ RobotContainer::RobotContainer() { m_chooser.SetDefaultOption(AutoConstants::kOutAuto, AutoConstants::kSpinAuto); m_chooser.AddOption(AutoConstants::kSpinAuto, AutoConstants::kSpinAuto); + pathplanner::NamedCommands::registerCommand("hehe", std::move(m_commandFactory.MoveToPositionL1())); + pathplanner::NamedCommands::registerCommand("hehe2", std::move(m_commandFactory.MoveToPositionL2())); + pathplanner::NamedCommands::registerCommand("hehe3", std::move(m_commandFactory.MoveToPositionL3())); + // Configure the button bindings ConfigureBindings(); diff --git a/src/main/cpp/commands/CommandController.cpp b/src/main/cpp/commands/CommandController.cpp new file mode 100644 index 0000000..2818ba1 --- /dev/null +++ b/src/main/cpp/commands/CommandController.cpp @@ -0,0 +1,115 @@ +#include "commands/CommandController.h" + +#include + +#include "Constants.h" + +frc2::CommandPtr CommandController::MoveToPositionL1() { + return m_subsystems.elevator->MoveToPositionAbsolute(CommandConstants::kElevatorL1Position) + .AndThen(m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralL1Position)); +} + +frc2::CommandPtr CommandController::MoveToPositionL2() { + return m_subsystems.elevator->MoveToPositionAbsolute(CommandConstants::kElevatorL2Position) + .AndThen(m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralL2Position)); +} + +frc2::CommandPtr CommandController::MoveToPositionL3() { + return m_subsystems.elevator->MoveToPositionAbsolute(CommandConstants::kElevatorL3Position) + .AndThen(m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralL3Position)); +} + +frc2::CommandPtr CommandController::FeedCoral() { + return + m_subsystems.elevator->MoveToPositionAbsolute(CommandConstants::kElevatorL3Position) + .AndThen(frc2::FunctionalCommand( + // On init + []() {}, + // On execute + [this]() { + m_subsystems.coralIntake->MoveAtPercent(CommandConstants::kCoralFeedSpeed); + }, + // On end + [this](bool interupted) { + m_subsystems.coralIntake->Stop(); + }, + // Is finished + []() { + return false; + }, + // Requirements + {m_subsystems.coralIntake} + ).ToPtr()); +} + +frc2::CommandPtr CommandController::ExpelCoral() { + return frc2::FunctionalCommand( + // On init + []() {}, + // On execute + [this]() { + m_subsystems.coralIntake->MoveAtPercent(CommandConstants::kCoralExpelSpeed); + }, + // On end + [this](bool interupted) { + m_subsystems.coralIntake->Stop(); + }, + // Is finished + []() { + return false; + }, + // Requirements + {m_subsystems.coralIntake} + ).ToPtr(); +} + +frc2::CommandPtr CommandController::IntakeAlgae() { + return m_subsystems.algaeArm->MoveToPositionAbsolute(CommandConstants::kAlgaeIntakePosition) + .AndThen(frc2::FunctionalCommand( + // On init + []() {}, + // On execute + [this]() { + m_subsystems.algaeIntake->MoveAtPercent(CommandConstants::kAlgaeIntakeSpeed); + }, + // On end + [this](bool interupted) { + m_subsystems.algaeIntake->Stop(); + }, + // Is finished + []() { + return false; + }, + // Requirements + {m_subsystems.algaeIntake} + ).ToPtr()); +} + +frc2::CommandPtr CommandController::ExpelAlgae() { + return frc2::FunctionalCommand( + // On init + []() {}, + // On execute + [this]() { + m_subsystems.algaeIntake->MoveAtPercent(CommandConstants::kAlgaeExpelSpeed); + }, + // On end + [this](bool interupted) { + m_subsystems.algaeIntake->Stop(); + }, + // Is finished + []() { + return false; + }, + // Requirements + {m_subsystems.algaeIntake} + ).ToPtr(); +} + +frc2::CommandPtr CommandController::ClimberDown() { + return m_subsystems.climber->MoveToPositionAbsolute(CommandConstants::kClimberDownAngle); +} + +frc2::CommandPtr CommandController::ClimberUp() { + return m_subsystems.climber->MoveToPositionAbsolute(CommandConstants::kClimberUpAngle); +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Out Auto.auto b/src/main/deploy/pathplanner/autos/Out Auto.auto index c419b83..2a31471 100644 --- a/src/main/deploy/pathplanner/autos/Out Auto.auto +++ b/src/main/deploy/pathplanner/autos/Out Auto.auto @@ -7,7 +7,13 @@ { "type": "path", "data": { - "pathName": "Out" + "pathName": "Spin" + } + }, + { + "type": "named", + "data": { + "name": null } } ] diff --git a/src/main/deploy/pathplanner/autos/Spin Auto.auto b/src/main/deploy/pathplanner/autos/Spin Auto.auto index dff1ae6..2a31471 100644 --- a/src/main/deploy/pathplanner/autos/Spin Auto.auto +++ b/src/main/deploy/pathplanner/autos/Spin Auto.auto @@ -9,6 +9,12 @@ "data": { "pathName": "Spin" } + }, + { + "type": "named", + "data": { + "name": null + } } ] } diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index 14aeb18..8000c10 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -201,7 +201,7 @@ 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; @@ -236,7 +236,7 @@ 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; @@ -247,9 +247,9 @@ namespace CoralArmConstants{ 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; static const subzero::SingleAxisMechanism kCoralArmMechanism = { @@ -303,4 +303,35 @@ namespace ClimberConstants{ const frc::TrapezoidProfile::Constraints kRotationalAxisConstraints; +} + +namespace CommandConstants { + // Placeholder values + constexpr units::meter_t kElevatorL1Position = 5_in; + constexpr units::meter_t kElevatorL2Position = 10_in; + constexpr units::meter_t kElevatorL3Position = 15_in; + constexpr units::meter_t kElevatorFeedPosition = 4_in; + + // Placeholder values + constexpr units::degree_t kCoralL1Position = 10_deg; + constexpr units::degree_t kCoralL2Position = 20_deg; + constexpr units::degree_t kCoralL3Position = 30_deg; + constexpr units::degree_t kCoralFeedPosition = 10_deg; + + // Placeholder values + constexpr units::degree_t kAlgaeIntakePosition = 50_deg; + constexpr units::degree_t kAlgaeStorePosition = 30_deg; + constexpr units::degree_t kAlgaeStowPosition = 0_deg; + + // Placeholder values + constexpr double kCoralFeedSpeed = 0.25; + constexpr double kCoralExpelSpeed = 0.25; + + // Placeholder values + constexpr double kAlgaeIntakeSpeed = 0.3; + constexpr double kAlgaeExpelSpeed = 0.3; + + // Placeholder values + constexpr units::degree_t kClimberDownAngle = 0_deg; + constexpr units::degree_t kClimberUpAngle = 90_deg; } \ No newline at end of file diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index 8ef2672..759411a 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/CommandFactory.h" /** * This class is where the bulk of the robot should be declared. Since @@ -71,4 +73,15 @@ class RobotContainer { IntakeSubsystem m_coralIntake{CoralArmConstants::kIntakeMotorId}; ClimberSubsystem m_climber; + + Subsystems_t subsystems = { + .algaeArm = &m_algaeArm, + .coralArm = &m_coralArm, + .elevator = &m_elevator, + .coralIntake = &m_coralIntake, + .algaeIntake = &m_algaeIntake, + .climber = &m_climber + }; + + CommandController m_commandFactory{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..62cad51 --- /dev/null +++ b/src/main/include/commands/CommandController.h @@ -0,0 +1,38 @@ +#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 ClimberDown(); + frc2::CommandPtr ClimberUp(); +private: + Subsystems_t m_subsystems; +}; \ No newline at end of file From fe87557ad2ac045bcf314ad2ac01d0e28ff7e5b7 Mon Sep 17 00:00:00 2001 From: 5690 <5690Programmers@gmail.com> Date: Tue, 18 Feb 2025 20:04:33 -0600 Subject: [PATCH 02/27] Basic command implementation and controller binding --- src/main/cpp/RobotContainer.cpp | 22 +++++++++---- src/main/cpp/commands/CommandController.cpp | 32 +++++++------------ src/main/cpp/subsystems/IntakeSubsystem.cpp | 7 ++++ src/main/include/Constants.h | 16 ++++++++-- src/main/include/RobotContainer.h | 11 ++++--- src/main/include/subsystems/IntakeSubsystem.h | 8 ++++- 6 files changed, 60 insertions(+), 36 deletions(-) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index f2b0339..fc712d8 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -35,9 +35,9 @@ RobotContainer::RobotContainer() { m_chooser.SetDefaultOption(AutoConstants::kOutAuto, AutoConstants::kSpinAuto); m_chooser.AddOption(AutoConstants::kSpinAuto, AutoConstants::kSpinAuto); - pathplanner::NamedCommands::registerCommand("hehe", std::move(m_commandFactory.MoveToPositionL1())); - pathplanner::NamedCommands::registerCommand("hehe2", std::move(m_commandFactory.MoveToPositionL2())); - pathplanner::NamedCommands::registerCommand("hehe3", std::move(m_commandFactory.MoveToPositionL3())); + pathplanner::NamedCommands::registerCommand("hehe", std::move(m_commandController.MoveToPositionL1())); + pathplanner::NamedCommands::registerCommand("hehe2", std::move(m_commandController.MoveToPositionL2())); + pathplanner::NamedCommands::registerCommand("hehe3", std::move(m_commandController.MoveToPositionL3())); // Configure the button bindings ConfigureBindings(); @@ -60,10 +60,18 @@ 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.Y().OnTrue(m_commandController.FeedCoral()); + m_operatorController.X().OnTrue(m_commandController.IntakeAlgae()); + + m_operatorController.LeftBumper().WhileTrue(m_commandController.ExpelAlgae()); + m_operatorController.RightBumper().WhileTrue(m_commandController.ExpelCoral()); } frc2::CommandPtr RobotContainer::GetAutonomousCommand() { diff --git a/src/main/cpp/commands/CommandController.cpp b/src/main/cpp/commands/CommandController.cpp index 2818ba1..acca120 100644 --- a/src/main/cpp/commands/CommandController.cpp +++ b/src/main/cpp/commands/CommandController.cpp @@ -20,26 +20,12 @@ frc2::CommandPtr CommandController::MoveToPositionL3() { } frc2::CommandPtr CommandController::FeedCoral() { - return - m_subsystems.elevator->MoveToPositionAbsolute(CommandConstants::kElevatorL3Position) - .AndThen(frc2::FunctionalCommand( - // On init - []() {}, - // On execute - [this]() { - m_subsystems.coralIntake->MoveAtPercent(CommandConstants::kCoralFeedSpeed); - }, - // On end - [this](bool interupted) { - m_subsystems.coralIntake->Stop(); - }, - // Is finished - []() { - return false; - }, - // Requirements - {m_subsystems.coralIntake} - ).ToPtr()); + return m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralFeedPosition) + .AndThen(m_subsystems.coralIntake->MoveAtPercent(CommandConstants::kCoralIntakeSpeed) + .Until([this]() { + return m_subsystems.coralIntake->HasGamePiece(); + }) + .WithTimeout(CoralArmConstants::kIntakeTimeout)); } frc2::CommandPtr CommandController::ExpelCoral() { @@ -82,7 +68,11 @@ frc2::CommandPtr CommandController::IntakeAlgae() { }, // Requirements {m_subsystems.algaeIntake} - ).ToPtr()); + ).ToPtr()) + .Until([this]() { + return m_subsystems.coralIntake->HasGamePiece(); + }) + .WithTimeout(AlgaeArmConstants::kIntakeTimeout); } frc2::CommandPtr CommandController::ExpelAlgae() { 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/include/Constants.h b/src/main/include/Constants.h index 8000c10..418657d 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -59,7 +59,8 @@ typedef */ namespace OperatorConstants { -inline constexpr int kDriverControllerPort = 0; +inline constexpr int kDriverControllerPort = 4; +inline constexpr int kOperatorControllerPort = 1; } // namespace OperatorConstants @@ -204,6 +205,8 @@ namespace AlgaeArmConstants{ constexpr double kD = 0.0; constexpr double kIZone = 0.0; constexpr double kFF = 0.0; + + constexpr double kHasAlgaeCurrent = 20; constexpr units::revolutions_per_minute_t kMaxRpm = 5676_rpm; constexpr units::degree_t kHomeRotation = 0_deg; @@ -214,6 +217,8 @@ namespace AlgaeArmConstants{ constexpr double kVelocityScalar = 1.0; constexpr units::degree_t kTolerance = 2_deg; constexpr units::meter_t kArmLength = 17_in; + + constexpr units::second_t kIntakeTimeout = 5_s; static const subzero::SingleAxisMechanism kAlgaeArmMechanism = { // length @@ -251,6 +256,11 @@ namespace CoralArmConstants{ constexpr double kVelocityScalar = 1.0; constexpr units::degree_t kTolerance = 0.5_deg; constexpr units::meter_t kArmLength = 0.2_m; + + constexpr units::second_t kIntakeTimeout = 3_s; + + // placeholder + constexpr double kHasCoralCurrent = 30; static const subzero::SingleAxisMechanism kCoralArmMechanism = { // length @@ -316,7 +326,7 @@ namespace CommandConstants { constexpr units::degree_t kCoralL1Position = 10_deg; constexpr units::degree_t kCoralL2Position = 20_deg; constexpr units::degree_t kCoralL3Position = 30_deg; - constexpr units::degree_t kCoralFeedPosition = 10_deg; + constexpr units::degree_t kCoralFeedPosition = 90_deg; // Placeholder values constexpr units::degree_t kAlgaeIntakePosition = 50_deg; @@ -324,7 +334,7 @@ namespace CommandConstants { constexpr units::degree_t kAlgaeStowPosition = 0_deg; // Placeholder values - constexpr double kCoralFeedSpeed = 0.25; + constexpr double kCoralIntakeSpeed = 0.25; constexpr double kCoralExpelSpeed = 0.25; // Placeholder values diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index 759411a..cae7edf 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -27,7 +27,7 @@ #include "subsystems/AlgaeArmSubsystem.h" #include "subsystems/CoralArmSubsystem.h" #include "subsystems/ClimberSubsystem.h" -#include "commands/CommandFactory.h" +#include "commands/CommandController.h" /** * This class is where the bulk of the robot should be declared. Since @@ -51,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; @@ -67,10 +70,10 @@ 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}; CoralArmSubsystem m_coralArm; - IntakeSubsystem m_coralIntake{CoralArmConstants::kIntakeMotorId}; + IntakeSubsystem m_coralIntake{CoralArmConstants::kIntakeMotorId, CoralArmConstants::kHasCoralCurrent}; ClimberSubsystem m_climber; @@ -83,5 +86,5 @@ class RobotContainer { .climber = &m_climber }; - CommandController m_commandFactory{subsystems}; + CommandController m_commandController{subsystems}; }; \ No newline at end of file diff --git a/src/main/include/subsystems/IntakeSubsystem.h b/src/main/include/subsystems/IntakeSubsystem.h index 245abdc..51d1b2c 100644 --- a/src/main/include/subsystems/IntakeSubsystem.h +++ b/src/main/include/subsystems/IntakeSubsystem.h @@ -4,14 +4,20 @@ #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) + : m_motor{id, rev::spark::SparkLowLevel::MotorType::kBrushless}, m_hasGamePieceCurrent{hasGamePieceCurrent} {} frc2::CommandPtr MoveAtPercent(double percent); frc2::CommandPtr Stop(); + + bool HasGamePiece(); private: rev::spark::SparkMax m_motor; + + double m_hasGamePieceCurrent; }; \ No newline at end of file From 2ca09d37ddceeccb37ce28be42015fe014151a8c Mon Sep 17 00:00:00 2001 From: WowMuchDoge <97766806+WowMuchDoge@users.noreply.github.com> Date: Wed, 19 Feb 2025 21:20:15 -0600 Subject: [PATCH 03/27] Elevator zero position command and proper expel command implementation --- .vscode/settings.json | 16 +++- src/main/cpp/RobotContainer.cpp | 8 +- src/main/cpp/commands/CommandController.cpp | 77 +++++-------------- src/main/cpp/subsystems/CoralArmSubsystem.cpp | 2 +- src/main/cpp/subsystems/ElevatorSubsystem.cpp | 5 ++ src/main/include/Constants.h | 13 ++-- src/main/include/commands/CommandController.h | 6 ++ .../include/subsystems/CoralArmSubsystem.h | 8 +- .../include/subsystems/ElevatorSubsystem.h | 11 ++- 9 files changed, 68 insertions(+), 78 deletions(-) create mode 100644 src/main/cpp/subsystems/ElevatorSubsystem.cpp 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/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index fc712d8..b626c06 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -35,9 +35,9 @@ RobotContainer::RobotContainer() { m_chooser.SetDefaultOption(AutoConstants::kOutAuto, AutoConstants::kSpinAuto); m_chooser.AddOption(AutoConstants::kSpinAuto, AutoConstants::kSpinAuto); - pathplanner::NamedCommands::registerCommand("hehe", std::move(m_commandController.MoveToPositionL1())); - pathplanner::NamedCommands::registerCommand("hehe2", std::move(m_commandController.MoveToPositionL2())); - pathplanner::NamedCommands::registerCommand("hehe3", std::move(m_commandController.MoveToPositionL3())); + pathplanner::NamedCommands::registerCommand("Test", std::move(m_commandController.MoveToPositionL1())); + pathplanner::NamedCommands::registerCommand("Test2", std::move(m_commandController.MoveToPositionL2())); + pathplanner::NamedCommands::registerCommand("Test3", std::move(m_commandController.MoveToPositionL3())); // Configure the button bindings ConfigureBindings(); @@ -69,6 +69,8 @@ void RobotContainer::ConfigureBindings() { m_operatorController.Y().OnTrue(m_commandController.FeedCoral()); m_operatorController.X().OnTrue(m_commandController.IntakeAlgae()); + m_operatorController.A().WhileTrue(m_commandController.ExpelCoral()); + m_operatorController.B().WhileTrue(m_commandController.ExpelAlgae()); m_operatorController.LeftBumper().WhileTrue(m_commandController.ExpelAlgae()); m_operatorController.RightBumper().WhileTrue(m_commandController.ExpelCoral()); diff --git a/src/main/cpp/commands/CommandController.cpp b/src/main/cpp/commands/CommandController.cpp index acca120..d1ce1bb 100644 --- a/src/main/cpp/commands/CommandController.cpp +++ b/src/main/cpp/commands/CommandController.cpp @@ -21,79 +21,28 @@ frc2::CommandPtr CommandController::MoveToPositionL3() { frc2::CommandPtr CommandController::FeedCoral() { return m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralFeedPosition) - .AndThen(m_subsystems.coralIntake->MoveAtPercent(CommandConstants::kCoralIntakeSpeed) + .AndThen(m_subsystems.coralIntake->MoveAtPercent(CommandConstants::kCoralFeedSpeed) .Until([this]() { return m_subsystems.coralIntake->HasGamePiece(); }) - .WithTimeout(CoralArmConstants::kIntakeTimeout)); + .WithTimeout(CommandConstants::kCoralFeedTimeout)); } frc2::CommandPtr CommandController::ExpelCoral() { - return frc2::FunctionalCommand( - // On init - []() {}, - // On execute - [this]() { - m_subsystems.coralIntake->MoveAtPercent(CommandConstants::kCoralExpelSpeed); - }, - // On end - [this](bool interupted) { - m_subsystems.coralIntake->Stop(); - }, - // Is finished - []() { - return false; - }, - // Requirements - {m_subsystems.coralIntake} - ).ToPtr(); + return m_subsystems.coralIntake->MoveAtPercent(CommandConstants::kCoralExpelSpeed); } frc2::CommandPtr CommandController::IntakeAlgae() { return m_subsystems.algaeArm->MoveToPositionAbsolute(CommandConstants::kAlgaeIntakePosition) - .AndThen(frc2::FunctionalCommand( - // On init - []() {}, - // On execute - [this]() { - m_subsystems.algaeIntake->MoveAtPercent(CommandConstants::kAlgaeIntakeSpeed); - }, - // On end - [this](bool interupted) { - m_subsystems.algaeIntake->Stop(); - }, - // Is finished - []() { - return false; - }, - // Requirements - {m_subsystems.algaeIntake} - ).ToPtr()) - .Until([this]() { - return m_subsystems.coralIntake->HasGamePiece(); + .AndThen(m_subsystems.algaeIntake->MoveAtPercent(CommandConstants::kAlgaeIntakeSpeed)) + .Until([this]() { + return m_subsystems.algaeIntake->HasGamePiece(); }) - .WithTimeout(AlgaeArmConstants::kIntakeTimeout); + .WithTimeout(CommandConstants::kAlgaeIntakeTimeout); } frc2::CommandPtr CommandController::ExpelAlgae() { - return frc2::FunctionalCommand( - // On init - []() {}, - // On execute - [this]() { - m_subsystems.algaeIntake->MoveAtPercent(CommandConstants::kAlgaeExpelSpeed); - }, - // On end - [this](bool interupted) { - m_subsystems.algaeIntake->Stop(); - }, - // Is finished - []() { - return false; - }, - // Requirements - {m_subsystems.algaeIntake} - ).ToPtr(); + return m_subsystems.algaeIntake->MoveAtPercent(CommandConstants::kAlgaeExpelSpeed); } frc2::CommandPtr CommandController::ClimberDown() { @@ -102,4 +51,14 @@ frc2::CommandPtr CommandController::ClimberDown() { frc2::CommandPtr CommandController::ClimberUp() { return m_subsystems.climber->MoveToPositionAbsolute(CommandConstants::kClimberUpAngle); +} + +frc2::CommandPtr CommandController::SetElevatorZeroPosition() { + return m_subsystems.coralArm->MoveToPositionAbsolute(CoralArmConstants::kMinRotation) + .AndThen(frc2::InstantCommand( + [this]() { + m_subsystems.elevator->SetEncoderPosition(CommandConstants::kElevatorStartPosition); + } + ).ToPtr()) + .AndThen(m_subsystems.elevator->MoveToPositionAbsolute(ElevatorConstants::kMinDistance)); } \ 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/ElevatorSubsystem.cpp b/src/main/cpp/subsystems/ElevatorSubsystem.cpp new file mode 100644 index 0000000..d158e90 --- /dev/null +++ b/src/main/cpp/subsystems/ElevatorSubsystem.cpp @@ -0,0 +1,5 @@ +#include + +void ElevatorSubsystem::SetEncoderPosition(units::meter_t pos) { + // m_enc.SetPosition((pos / ElevatorConstants::kRelativeDistancePerRev).value()); +} \ No newline at end of file diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index 418657d..78c9f6b 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -165,6 +165,7 @@ namespace ElevatorConstants { const int kFollowerElevatorMotorCanId = 7; const int kBottomLimitSwitchPort = 1; + const int kTopLimitSwitchPort = 2; // Placeholder values const double kElevatorP = 100.0; @@ -217,8 +218,6 @@ namespace AlgaeArmConstants{ constexpr double kVelocityScalar = 1.0; constexpr units::degree_t kTolerance = 2_deg; constexpr units::meter_t kArmLength = 17_in; - - constexpr units::second_t kIntakeTimeout = 5_s; static const subzero::SingleAxisMechanism kAlgaeArmMechanism = { // length @@ -248,7 +247,7 @@ namespace CoralArmConstants{ 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; @@ -257,8 +256,6 @@ namespace CoralArmConstants{ constexpr units::degree_t kTolerance = 0.5_deg; constexpr units::meter_t kArmLength = 0.2_m; - constexpr units::second_t kIntakeTimeout = 3_s; - // placeholder constexpr double kHasCoralCurrent = 30; @@ -321,6 +318,7 @@ namespace CommandConstants { constexpr units::meter_t kElevatorL2Position = 10_in; constexpr units::meter_t kElevatorL3Position = 15_in; constexpr units::meter_t kElevatorFeedPosition = 4_in; + constexpr units::meter_t kElevatorStartPosition = 5_in; // Placeholder values constexpr units::degree_t kCoralL1Position = 10_deg; @@ -334,7 +332,7 @@ namespace CommandConstants { constexpr units::degree_t kAlgaeStowPosition = 0_deg; // Placeholder values - constexpr double kCoralIntakeSpeed = 0.25; + constexpr double kCoralFeedSpeed = 0.25; constexpr double kCoralExpelSpeed = 0.25; // Placeholder values @@ -344,4 +342,7 @@ namespace CommandConstants { // Placeholder values constexpr units::degree_t kClimberDownAngle = 0_deg; constexpr units::degree_t kClimberUpAngle = 90_deg; + + constexpr units::second_t kCoralFeedTimeout = 3_s; + constexpr units::second_t kAlgaeIntakeTimeout = 5_s; } \ No newline at end of file diff --git a/src/main/include/commands/CommandController.h b/src/main/include/commands/CommandController.h index 62cad51..5966400 100644 --- a/src/main/include/commands/CommandController.h +++ b/src/main/include/commands/CommandController.h @@ -33,6 +33,12 @@ class CommandController { frc2::CommandPtr ClimberDown(); frc2::CommandPtr ClimberUp(); + + /** + @brief Must be called first during auto in order + to zero the elevator properly + */ + frc2::CommandPtr SetElevatorZeroPosition(); private: Subsystems_t m_subsystems; }; \ No newline at end of file diff --git a/src/main/include/subsystems/CoralArmSubsystem.h b/src/main/include/subsystems/CoralArmSubsystem.h index 20797f0..0ed1fdf 100644 --- a/src/main/include/subsystems/CoralArmSubsystem.h +++ b/src/main/include/subsystems/CoralArmSubsystem.h @@ -22,7 +22,8 @@ class CoralArmSubsystem : 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 Date: Thu, 20 Feb 2025 18:51:49 -0600 Subject: [PATCH 04/27] Create new paths --- src/main/cpp/RobotContainer.cpp | 3 +- src/main/cpp/commands/CommandController.cpp | 17 +++++- src/main/cpp/subsystems/ElevatorSubsystem.cpp | 2 +- ...Center To Rear Center Reef (1 Coral).auto} | 8 +-- .../autos/Far Left Auto (3 Coral).auto | 43 +++++++++++++++ .../autos/Far Right Auto (3 Coral).auto | 43 +++++++++++++++ .../deploy/pathplanner/autos/Spin Auto.auto | 25 --------- .../Center Start to Center Rear Reef.path | 54 +++++++++++++++++++ .../paths/Far Left Source to Left Reef.path | 54 +++++++++++++++++++ .../paths/Far Right Source to Reef Right.path | 54 +++++++++++++++++++ .../paths/Left Reef to Far Left Source.path | 54 +++++++++++++++++++ .../paths/Left Start to Reef Left.path | 54 +++++++++++++++++++ .../paths/Right Reef to Far Right Source.path | 54 +++++++++++++++++++ .../paths/Right Start to Reef Right.path | 54 +++++++++++++++++++ src/main/include/Constants.h | 5 ++ src/main/include/commands/CommandController.h | 3 ++ 16 files changed, 491 insertions(+), 36 deletions(-) rename src/main/deploy/pathplanner/autos/{Out Auto.auto => Center To Rear Center Reef (1 Coral).auto} (65%) create mode 100644 src/main/deploy/pathplanner/autos/Far Left Auto (3 Coral).auto create mode 100644 src/main/deploy/pathplanner/autos/Far Right Auto (3 Coral).auto delete mode 100644 src/main/deploy/pathplanner/autos/Spin Auto.auto create mode 100644 src/main/deploy/pathplanner/paths/Center Start to Center Rear Reef.path create mode 100644 src/main/deploy/pathplanner/paths/Far Left Source to Left Reef.path create mode 100644 src/main/deploy/pathplanner/paths/Far Right Source to Reef Right.path create mode 100644 src/main/deploy/pathplanner/paths/Left Reef to Far Left Source.path create mode 100644 src/main/deploy/pathplanner/paths/Left Start to Reef Left.path create mode 100644 src/main/deploy/pathplanner/paths/Right Reef to Far Right Source.path create mode 100644 src/main/deploy/pathplanner/paths/Right Start to Reef Right.path diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index b626c06..ac2ba24 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -67,10 +67,9 @@ void RobotContainer::ConfigureBindings() { m_operatorController.POVRight().OnTrue(m_commandController.MoveToPositionL3()); m_operatorController.POVDown().OnTrue(m_commandController.FeedCoral()); - m_operatorController.Y().OnTrue(m_commandController.FeedCoral()); - m_operatorController.X().OnTrue(m_commandController.IntakeAlgae()); m_operatorController.A().WhileTrue(m_commandController.ExpelCoral()); m_operatorController.B().WhileTrue(m_commandController.ExpelAlgae()); + m_operatorController.X().OnTrue(m_commandController.IntakeAlgae()); m_operatorController.LeftBumper().WhileTrue(m_commandController.ExpelAlgae()); m_operatorController.RightBumper().WhileTrue(m_commandController.ExpelCoral()); diff --git a/src/main/cpp/commands/CommandController.cpp b/src/main/cpp/commands/CommandController.cpp index d1ce1bb..315fc52 100644 --- a/src/main/cpp/commands/CommandController.cpp +++ b/src/main/cpp/commands/CommandController.cpp @@ -61,4 +61,19 @@ frc2::CommandPtr CommandController::SetElevatorZeroPosition() { } ).ToPtr()) .AndThen(m_subsystems.elevator->MoveToPositionAbsolute(ElevatorConstants::kMinDistance)); -} \ No newline at end of file +} + +frc2::CommandPtr CommandController::RemoveAlgaeFromL2() { + return m_subsystems.elevator->MoveToPositionAbsolute(CommandConstants::kElevatorRemoveAlgaeFromL2Position) + .AndThen(m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralArmRemoveAlgaeFromL2Position)) + .AndThen(m_subsystems.coralIntake->MoveAtPercent(CommandConstants::kCoralFeedSpeed)) + .WithTimeout(CommandConstants::kRemoveAlgaeFromReefTimeout); + ; +} + +frc2::CommandPtr CommandController::RemoveAlgaeFromL3() { + return m_subsystems.elevator->MoveToPositionAbsolute(CommandConstants::kElevatorRemoveAlgaeFromL3Position) + .AndThen(m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralArmRemoveAlgaeFromL3Position)) + .AndThen(m_subsystems.coralIntake->MoveAtPercent(CommandConstants::kCoralFeedSpeed)) + .WithTimeout(CommandConstants::kRemoveAlgaeFromReefTimeout); +} diff --git a/src/main/cpp/subsystems/ElevatorSubsystem.cpp b/src/main/cpp/subsystems/ElevatorSubsystem.cpp index d158e90..05b0759 100644 --- a/src/main/cpp/subsystems/ElevatorSubsystem.cpp +++ b/src/main/cpp/subsystems/ElevatorSubsystem.cpp @@ -1,5 +1,5 @@ #include void ElevatorSubsystem::SetEncoderPosition(units::meter_t pos) { - // m_enc.SetPosition((pos / ElevatorConstants::kRelativeDistancePerRev).value()); + m_enc.SetPosition((pos / ElevatorConstants::kRelativeDistancePerRev).value()); } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Out Auto.auto b/src/main/deploy/pathplanner/autos/Center To Rear Center Reef (1 Coral).auto similarity index 65% rename from src/main/deploy/pathplanner/autos/Out Auto.auto rename to src/main/deploy/pathplanner/autos/Center To Rear Center Reef (1 Coral).auto index 2a31471..6bbf703 100644 --- a/src/main/deploy/pathplanner/autos/Out Auto.auto +++ b/src/main/deploy/pathplanner/autos/Center To Rear Center Reef (1 Coral).auto @@ -7,13 +7,7 @@ { "type": "path", "data": { - "pathName": "Spin" - } - }, - { - "type": "named", - "data": { - "name": null + "pathName": "Center Start to Center Rear Reef" } } ] diff --git a/src/main/deploy/pathplanner/autos/Far Left Auto (3 Coral).auto b/src/main/deploy/pathplanner/autos/Far Left Auto (3 Coral).auto new file mode 100644 index 0000000..b8528dd --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Far Left Auto (3 Coral).auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Start to Reef Left" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Reef to Far Left Source" + } + }, + { + "type": "path", + "data": { + "pathName": "Far Left Source to Left Reef" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Reef to Far Left Source" + } + }, + { + "type": "path", + "data": { + "pathName": "Far Left Source to Left Reef" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Far Right Auto (3 Coral).auto b/src/main/deploy/pathplanner/autos/Far Right Auto (3 Coral).auto new file mode 100644 index 0000000..3c2e48e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Far Right Auto (3 Coral).auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Start to Reef Right" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Reef to Far Right Source" + } + }, + { + "type": "path", + "data": { + "pathName": "Far Right Source to Reef Right" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Reef to Far Right Source" + } + }, + { + "type": "path", + "data": { + "pathName": "Far Right Source to Reef Right" + } + } + ] + } + }, + "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 2a31471..0000000 --- a/src/main/deploy/pathplanner/autos/Spin Auto.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Spin" - } - }, - { - "type": "named", - "data": { - "name": null - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Start to Center Rear Reef.path b/src/main/deploy/pathplanner/paths/Center Start to Center Rear Reef.path new file mode 100644 index 0000000..3f226a4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Start to Center Rear Reef.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.364732142857142, + "y": 3.9205357142857142 + }, + "prevControl": null, + "nextControl": { + "x": 5.925974025974026, + "y": 3.949025974025974 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.90698051948052, + "y": 3.9205357142857142 + }, + "prevControl": { + "x": 7.317248376623375, + "y": 3.915787337662337 + }, + "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": 178.97696981133205 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -178.81881108667335 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Far Left Source to Left Reef.path b/src/main/deploy/pathplanner/paths/Far Left Source to Left Reef.path new file mode 100644 index 0000000..bd485d4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Far Left Source to Left Reef.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.766396103896104, + "y": 7.34411525974026 + }, + "prevControl": null, + "nextControl": { + "x": 2.526136363636364, + "y": 6.5558847402597396 + }, + "isLocked": false, + "linkedName": "Far Left Source" + }, + { + "anchor": { + "x": 3.5992694805194803, + "y": 5.117126623376623 + }, + "prevControl": { + "x": 2.9914772727272725, + "y": 6.000324675324675 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "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": -58.24051991518723 + }, + "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.path b/src/main/deploy/pathplanner/paths/Far Right Source to Reef Right.path new file mode 100644 index 0000000..a0059bd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Far Right Source to Reef Right.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.6809253246720754, + "y": 0.7106331168816484 + }, + "prevControl": null, + "nextControl": { + "x": 2.6258522727272724, + "y": 1.7837662337662332 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.708, + "y": 2.895 + }, + "prevControl": { + "x": 2.625852272727272, + "y": 1.7932629870129873 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right 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": 59.349332043143804 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 55.61965527615502 + }, + "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..5489fa3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Reef to Far Left Source.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5992694805194803, + "y": 5.117126623376623 + }, + "prevControl": null, + "nextControl": { + "x": 2.9962256493506496, + "y": 6.057305194805195 + }, + "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": [], + "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": -58.24051991518723 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Start to Reef Left.path b/src/main/deploy/pathplanner/paths/Left Start to Reef Left.path new file mode 100644 index 0000000..15ba5e6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Start to Reef Left.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.345738636363635, + "y": 7.225405844155843 + }, + "prevControl": null, + "nextControl": { + "x": 7.10535764945343, + "y": 7.156725562181498 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.5992694805194803, + "y": 5.117126623376623 + }, + "prevControl": { + "x": 4.852840909090911, + "y": 5.895860389610388 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "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": -58.24051991518723 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -62.40270413166291 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Reef to Far Right Source.path b/src/main/deploy/pathplanner/paths/Right Reef to Far Right Source.path new file mode 100644 index 0000000..be6a78a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Reef to Far Right Source.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.708, + "y": 2.895 + }, + "prevControl": null, + "nextControl": { + "x": 2.8015422077922074, + "y": 2.006939935064935 + }, + "isLocked": false, + "linkedName": "Right Reef" + }, + { + "anchor": { + "x": 1.6809253246720754, + "y": 0.7106331168816484 + }, + "prevControl": { + "x": 2.7920454545454545, + "y": 2.0021915584415586 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Far Right Source" + } + ], + "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": 55.61965527615502 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 59.349332043143804 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Start to Reef Right.path b/src/main/deploy/pathplanner/paths/Right Start to Reef Right.path new file mode 100644 index 0000000..6776b85 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Start to Reef Right.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.511931818181819, + "y": 0.806 + }, + "prevControl": null, + "nextControl": { + "x": 6.856655223624603, + "y": 1.1146431630804878 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.708, + "y": 2.895 + }, + "prevControl": { + "x": 5.0945304253536525, + "y": 2.068789937622381 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right 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": 59.349332043143804 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 58.861027563021295 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index 78c9f6b..a3c2515 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -319,12 +319,16 @@ namespace CommandConstants { constexpr units::meter_t kElevatorL3Position = 15_in; constexpr units::meter_t kElevatorFeedPosition = 4_in; constexpr units::meter_t kElevatorStartPosition = 5_in; + constexpr units::meter_t kElevatorRemoveAlgaeFromL2Position = 10_in; + constexpr units::meter_t kElevatorRemoveAlgaeFromL3Position = 15_in; // Placeholder values constexpr units::degree_t kCoralL1Position = 10_deg; constexpr units::degree_t kCoralL2Position = 20_deg; constexpr units::degree_t kCoralL3Position = 30_deg; constexpr units::degree_t kCoralFeedPosition = 90_deg; + constexpr units::degree_t kCoralArmRemoveAlgaeFromL2Position = 70_deg; + constexpr units::degree_t kCoralArmRemoveAlgaeFromL3Position = 90_deg; // Placeholder values constexpr units::degree_t kAlgaeIntakePosition = 50_deg; @@ -345,4 +349,5 @@ namespace CommandConstants { constexpr units::second_t kCoralFeedTimeout = 3_s; constexpr units::second_t kAlgaeIntakeTimeout = 5_s; + constexpr units::second_t kRemoveAlgaeFromReefTimeout = 3_s; } \ No newline at end of file diff --git a/src/main/include/commands/CommandController.h b/src/main/include/commands/CommandController.h index 5966400..308eed0 100644 --- a/src/main/include/commands/CommandController.h +++ b/src/main/include/commands/CommandController.h @@ -34,6 +34,9 @@ class CommandController { frc2::CommandPtr ClimberDown(); frc2::CommandPtr ClimberUp(); + frc2::CommandPtr RemoveAlgaeFromL2(); + frc2::CommandPtr RemoveAlgaeFromL3(); + /** @brief Must be called first during auto in order to zero the elevator properly From 2ea014684d606a4c084f0e018aeb3ea68423aacb Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Fri, 21 Feb 2025 18:11:09 -0600 Subject: [PATCH 05/27] Values for operating --- src/main/cpp/RobotContainer.cpp | 5 ++- src/main/cpp/commands/CommandController.cpp | 6 ++- src/main/cpp/subsystems/ElevatorSubsystem.cpp | 3 +- src/main/include/Constants.h | 42 +++++++++---------- .../include/subsystems/AlgaeArmSubsystem.h | 4 +- 5 files changed, 31 insertions(+), 29 deletions(-) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index ac2ba24..64f84ac 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -70,9 +70,10 @@ void RobotContainer::ConfigureBindings() { m_operatorController.A().WhileTrue(m_commandController.ExpelCoral()); m_operatorController.B().WhileTrue(m_commandController.ExpelAlgae()); m_operatorController.X().OnTrue(m_commandController.IntakeAlgae()); + m_operatorController.Y().OnTrue(m_commandController.SetElevatorZeroPosition()); - m_operatorController.LeftBumper().WhileTrue(m_commandController.ExpelAlgae()); - m_operatorController.RightBumper().WhileTrue(m_commandController.ExpelCoral()); + // m_operatorController.LeftBumper().WhileTrue(m_commandController.ExpelAlgae()); + // m_operatorController.RightBumper().WhileTrue(m_commandController.ExpelCoral()); } frc2::CommandPtr RobotContainer::GetAutonomousCommand() { diff --git a/src/main/cpp/commands/CommandController.cpp b/src/main/cpp/commands/CommandController.cpp index 315fc52..ab7963f 100644 --- a/src/main/cpp/commands/CommandController.cpp +++ b/src/main/cpp/commands/CommandController.cpp @@ -6,7 +6,8 @@ frc2::CommandPtr CommandController::MoveToPositionL1() { return m_subsystems.elevator->MoveToPositionAbsolute(CommandConstants::kElevatorL1Position) - .AndThen(m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralL1Position)); + .AndThen(m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralL1Position)) + .AndThen(m_subsystems.algaeArm->MoveToPositionAbsolute(CommandConstants::kAlgaeArmL1Position)); } frc2::CommandPtr CommandController::MoveToPositionL2() { @@ -60,7 +61,8 @@ frc2::CommandPtr CommandController::SetElevatorZeroPosition() { m_subsystems.elevator->SetEncoderPosition(CommandConstants::kElevatorStartPosition); } ).ToPtr()) - .AndThen(m_subsystems.elevator->MoveToPositionAbsolute(ElevatorConstants::kMinDistance)); + .AndThen(m_subsystems.elevator->MoveToPositionAbsolute(ElevatorConstants::kMinDistance)) + ; } frc2::CommandPtr CommandController::RemoveAlgaeFromL2() { diff --git a/src/main/cpp/subsystems/ElevatorSubsystem.cpp b/src/main/cpp/subsystems/ElevatorSubsystem.cpp index 05b0759..8e2bbba 100644 --- a/src/main/cpp/subsystems/ElevatorSubsystem.cpp +++ b/src/main/cpp/subsystems/ElevatorSubsystem.cpp @@ -1,5 +1,6 @@ #include void ElevatorSubsystem::SetEncoderPosition(units::meter_t pos) { - m_enc.SetPosition((pos / ElevatorConstants::kRelativeDistancePerRev).value()); + // Basesingleaxis applies a conversion factor so we don't provide rotations + m_enc.SetPosition(pos.value()); } \ No newline at end of file diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index a3c2515..8f7fcd2 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -59,7 +59,7 @@ typedef */ namespace OperatorConstants { -inline constexpr int kDriverControllerPort = 4; +inline constexpr int kDriverControllerPort = 0; inline constexpr int kOperatorControllerPort = 1; } // namespace OperatorConstants @@ -71,7 +71,7 @@ namespace DriveConstants { // the robot, rather the allowed maximum speeds // Test sewrve -// constexpr units::meters_per_second_t kMaxSpeed = 4.8_mps; +// constexpr units::meters_per_second_t kMaxSpeed = 4.8_mps;+ // Season robot constexpr units::meters_per_second_t kMaxSpeed = 4.92_mps; @@ -314,39 +314,37 @@ namespace ClimberConstants{ namespace CommandConstants { // Placeholder values - constexpr units::meter_t kElevatorL1Position = 5_in; - constexpr units::meter_t kElevatorL2Position = 10_in; - constexpr units::meter_t kElevatorL3Position = 15_in; - constexpr units::meter_t kElevatorFeedPosition = 4_in; - constexpr units::meter_t kElevatorStartPosition = 5_in; - constexpr units::meter_t kElevatorRemoveAlgaeFromL2Position = 10_in; - constexpr units::meter_t kElevatorRemoveAlgaeFromL3Position = 15_in; + 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 = 4.875_in; + constexpr units::meter_t kElevatorStartPosition = 4.875_in; + constexpr units::meter_t kElevatorRemoveAlgaeFromL2Position = 0.38_m; + constexpr units::meter_t kElevatorRemoveAlgaeFromL3Position = 0.50_m; + + constexpr units::degree_t kCoralL1Position = 260_deg; + constexpr units::degree_t kCoralL2Position = 260_deg; + constexpr units::degree_t kCoralL3Position = 244_deg; + constexpr units::degree_t kCoralFeedPosition = 80_deg; + constexpr units::degree_t kCoralArmRemoveAlgaeFromL2Position = 219_deg; + constexpr units::degree_t kCoralArmRemoveAlgaeFromL3Position = 188_deg; - // Placeholder values - constexpr units::degree_t kCoralL1Position = 10_deg; - constexpr units::degree_t kCoralL2Position = 20_deg; - constexpr units::degree_t kCoralL3Position = 30_deg; - constexpr units::degree_t kCoralFeedPosition = 90_deg; - constexpr units::degree_t kCoralArmRemoveAlgaeFromL2Position = 70_deg; - constexpr units::degree_t kCoralArmRemoveAlgaeFromL3Position = 90_deg; - - // Placeholder values constexpr units::degree_t kAlgaeIntakePosition = 50_deg; constexpr units::degree_t kAlgaeStorePosition = 30_deg; constexpr units::degree_t kAlgaeStowPosition = 0_deg; + constexpr units::degree_t kAlgaeArmL1Position = 34_deg; - // Placeholder values constexpr double kCoralFeedSpeed = 0.25; - constexpr double kCoralExpelSpeed = 0.25; + constexpr double kCoralExpelSpeed = -0.25; - // Placeholder values constexpr double kAlgaeIntakeSpeed = 0.3; - constexpr double kAlgaeExpelSpeed = 0.3; + constexpr double kAlgaeExpelSpeed = -0.3; // Placeholder values constexpr units::degree_t kClimberDownAngle = 0_deg; constexpr units::degree_t kClimberUpAngle = 90_deg; + // Placeholder constexpr units::second_t kCoralFeedTimeout = 3_s; constexpr units::second_t kAlgaeIntakeTimeout = 5_s; constexpr units::second_t kRemoveAlgaeFromReefTimeout = 3_s; diff --git a/src/main/include/subsystems/AlgaeArmSubsystem.h b/src/main/include/subsystems/AlgaeArmSubsystem.h index 912b816..a0285da 100644 --- a/src/main/include/subsystems/AlgaeArmSubsystem.h +++ b/src/main/include/subsystems/AlgaeArmSubsystem.h @@ -39,7 +39,7 @@ class AlgaeArmSubsystem : public subzero::RotationalSingleAxisSubsystem Date: Fri, 21 Feb 2025 23:44:37 -0600 Subject: [PATCH 06/27] added methods and partial implement in drive subsytem --- src/main/cpp/subsystems/DriveSubsystem.cpp | 23 ++++++++- src/main/include/Constants.h | 50 +++++++++++++++++++- src/main/include/subsystems/DriveSubsystem.h | 37 ++++++++++++++- 3 files changed, 105 insertions(+), 5 deletions(-) diff --git a/src/main/cpp/subsystems/DriveSubsystem.cpp b/src/main/cpp/subsystems/DriveSubsystem.cpp index f7cec5c..38c86c3 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" @@ -220,4 +227,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/include/Constants.h b/src/main/include/Constants.h index 14aeb18..7ae722c 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 @@ -303,4 +309,46 @@ namespace ClimberConstants{ const frc::TrapezoidProfile::Constraints kRotationalAxisConstraints; -} \ No newline at end of file +} + +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{0_deg, -23.461_deg, 180_deg}}; +static const frc::Transform3d kRobotToCam{ + frc::Translation3d{5.714_in, 0_in, 23.533_in}, + frc::Rotation3d{0_deg, -23.461_deg, 0_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/subsystems/DriveSubsystem.h b/src/main/include/subsystems/DriveSubsystem.h index fce5227..042c6fd 100644 --- a/src/main/include/subsystems/DriveSubsystem.h +++ b/src/main/include/subsystems/DriveSubsystem.h @@ -5,9 +5,12 @@ #pragma once #include +#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. @@ -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; +}; + + From d08b727304ea6d4b12c5344c910ffd8f21ba02cf Mon Sep 17 00:00:00 2001 From: WowMuchDoge <97766806+WowMuchDoge@users.noreply.github.com> Date: Sat, 22 Feb 2025 07:50:48 -0600 Subject: [PATCH 07/27] Add constants for auto names and configure some name commands --- src/main/cpp/RobotContainer.cpp | 21 ++++++++++--------- .../Center Start to Center Rear Reef.path | 9 +++++++- src/main/include/Constants.h | 5 +++-- ....2.2.json => PathplannerLib-2025.2.3.json} | 8 +++---- 4 files changed, 26 insertions(+), 17 deletions(-) rename vendordeps/{PathplannerLib-2025.2.2.json => PathplannerLib-2025.2.3.json} (87%) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 64f84ac..762b7a7 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -32,12 +32,13 @@ RobotContainer::RobotContainer() { // 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::kFarLeftAuto, AutoConstants::kFarLeftAuto); + m_chooser.AddOption(AutoConstants::kFarRightAuto, AutoConstants::kFarRightAuto); - pathplanner::NamedCommands::registerCommand("Test", std::move(m_commandController.MoveToPositionL1())); - pathplanner::NamedCommands::registerCommand("Test2", std::move(m_commandController.MoveToPositionL2())); - pathplanner::NamedCommands::registerCommand("Test3", std::move(m_commandController.MoveToPositionL3())); + pathplanner::NamedCommands::registerCommand("Test Named Command", frc2::InstantCommand([]() { std::cout << "Test Command Was Called" << std::endl; }).ToPtr()); + pathplanner::NamedCommands::registerCommand("L2 Position", std::move(m_commandController.MoveToPositionL2())); + pathplanner::NamedCommands::registerCommand("L3 Position", std::move(m_commandController.MoveToPositionL3())); // Configure the button bindings ConfigureBindings(); @@ -77,15 +78,15 @@ void RobotContainer::ConfigureBindings() { } 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()); } diff --git a/src/main/deploy/pathplanner/paths/Center Start to Center Rear Reef.path b/src/main/deploy/pathplanner/paths/Center Start to Center Rear Reef.path index 3f226a4..8297d2c 100644 --- a/src/main/deploy/pathplanner/paths/Center Start to Center Rear Reef.path +++ b/src/main/deploy/pathplanner/paths/Center Start to Center Rear Reef.path @@ -31,7 +31,14 @@ "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "Test2", + "waypointRelativePos": 0.5486891385767628, + "endWaypointRelativePos": null, + "command": null + } + ], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index 8f7fcd2..f03247b 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -140,8 +140,9 @@ constexpr auto kMaxAcceleration = 3_mps_sq; constexpr auto kMaxAngularSpeed = 3.142_rad_per_s; constexpr auto kMaxAngularAcceleration = 3.142_rad_per_s_sq; -const std::string kOutAuto = "Out Auto"; -const std::string kSpinAuto = "Spin Auto"; +const std::string kCenterToCenterAuto = "Center To Rear Center Reef (1 Coral)"; +const std::string kFarLeftAuto = "Far Left Auto (3 Coral)"; +const std::string kFarRightAuto = "Far Right Auto (3 Coral)"; constexpr double kPXController = 0.5; constexpr double kPYController = 0.5; 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, From f87e49aedb056f63d352671e68b373ced523e893 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Sat, 22 Feb 2025 08:51:16 -0600 Subject: [PATCH 08/27] Adjust constants for elevator and coral arm --- src/main/cpp/RobotContainer.cpp | 10 ++++--- src/main/cpp/commands/CommandController.cpp | 24 +++++++---------- src/main/include/Constants.h | 26 ++++++++++--------- src/main/include/RobotContainer.h | 4 +-- src/main/include/commands/CommandController.h | 6 ----- .../include/subsystems/AlgaeArmSubsystem.h | 2 +- .../include/subsystems/CoralArmSubsystem.h | 2 +- src/main/include/subsystems/IntakeSubsystem.h | 12 +++++++-- 8 files changed, 45 insertions(+), 41 deletions(-) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 762b7a7..3853c18 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. @@ -58,6 +59,9 @@ RobotContainer::RobotContainer() { true); }, {&m_drive})); + + // Has to be raised before match so coral arm is within frame perimeter + m_elevator.SetEncoderPosition(ElevatorConstants::kElevatorStartPosition); } void RobotContainer::ConfigureBindings() { @@ -68,10 +72,10 @@ void RobotContainer::ConfigureBindings() { 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_commandController.ExpelAlgae()); - m_operatorController.X().OnTrue(m_commandController.IntakeAlgae()); - m_operatorController.Y().OnTrue(m_commandController.SetElevatorZeroPosition()); + m_operatorController.X().WhileTrue(m_commandController.ExpelAlgae()); + m_operatorController.Y().OnTrue(m_commandController.IntakeAlgae()); // m_operatorController.LeftBumper().WhileTrue(m_commandController.ExpelAlgae()); // m_operatorController.RightBumper().WhileTrue(m_commandController.ExpelCoral()); diff --git a/src/main/cpp/commands/CommandController.cpp b/src/main/cpp/commands/CommandController.cpp index ab7963f..bee2903 100644 --- a/src/main/cpp/commands/CommandController.cpp +++ b/src/main/cpp/commands/CommandController.cpp @@ -6,22 +6,29 @@ 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::kAlgaeArmL1Position)); + .AndThen(m_subsystems.algaeArm->MoveToPositionAbsolute(CommandConstants::kAlgaeArmReefPosition)); } frc2::CommandPtr CommandController::MoveToPositionL2() { return m_subsystems.elevator->MoveToPositionAbsolute(CommandConstants::kElevatorL2Position) - .AndThen(m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralL2Position)); + .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) - .AndThen(m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralL3Position)); + .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.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(); @@ -54,17 +61,6 @@ frc2::CommandPtr CommandController::ClimberUp() { return m_subsystems.climber->MoveToPositionAbsolute(CommandConstants::kClimberUpAngle); } -frc2::CommandPtr CommandController::SetElevatorZeroPosition() { - return m_subsystems.coralArm->MoveToPositionAbsolute(CoralArmConstants::kMinRotation) - .AndThen(frc2::InstantCommand( - [this]() { - m_subsystems.elevator->SetEncoderPosition(CommandConstants::kElevatorStartPosition); - } - ).ToPtr()) - .AndThen(m_subsystems.elevator->MoveToPositionAbsolute(ElevatorConstants::kMinDistance)) - ; -} - frc2::CommandPtr CommandController::RemoveAlgaeFromL2() { return m_subsystems.elevator->MoveToPositionAbsolute(CommandConstants::kElevatorRemoveAlgaeFromL2Position) .AndThen(m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralArmRemoveAlgaeFromL2Position)) diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index f03247b..a993c9f 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -175,11 +175,13 @@ namespace ElevatorConstants { const double kElevatorIZone = 0.0; const double kElevatorFF = 0.0; + 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 kMaxDistance = 20.1_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; @@ -208,7 +210,7 @@ namespace AlgaeArmConstants{ constexpr double kIZone = 0.0; constexpr double kFF = 0.0; - constexpr double kHasAlgaeCurrent = 20; + constexpr double kHasAlgaeCurrent = 30; constexpr units::revolutions_per_minute_t kMaxRpm = 5676_rpm; constexpr units::degree_t kHomeRotation = 0_deg; @@ -314,32 +316,32 @@ namespace ClimberConstants{ } namespace CommandConstants { - // Placeholder values + // 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 = 4.875_in; - constexpr units::meter_t kElevatorStartPosition = 4.875_in; + constexpr units::meter_t kElevatorFeedPosition = 3_in; constexpr units::meter_t kElevatorRemoveAlgaeFromL2Position = 0.38_m; constexpr units::meter_t kElevatorRemoveAlgaeFromL3Position = 0.50_m; constexpr units::degree_t kCoralL1Position = 260_deg; - constexpr units::degree_t kCoralL2Position = 260_deg; - constexpr units::degree_t kCoralL3Position = 244_deg; - constexpr units::degree_t kCoralFeedPosition = 80_deg; + constexpr units::degree_t kCoralL2Position = 234_deg; + constexpr units::degree_t kCoralL3Position = 210_deg; + constexpr units::degree_t kCoralFeedPosition = 70_deg; constexpr units::degree_t kCoralArmRemoveAlgaeFromL2Position = 219_deg; constexpr units::degree_t kCoralArmRemoveAlgaeFromL3Position = 188_deg; - constexpr units::degree_t kAlgaeIntakePosition = 50_deg; + constexpr units::degree_t kAlgaeIntakePosition = 30_deg; constexpr units::degree_t kAlgaeStorePosition = 30_deg; constexpr units::degree_t kAlgaeStowPosition = 0_deg; - constexpr units::degree_t kAlgaeArmL1Position = 34_deg; + constexpr units::degree_t kAlgaeArmReefPosition = 10_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.3; + constexpr double kAlgaeIntakeSpeed = -0.3; + constexpr double kAlgaeExpelSpeed = 0.3; // Placeholder values constexpr units::degree_t kClimberDownAngle = 0_deg; diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index cae7edf..499750c 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -70,10 +70,10 @@ class RobotContainer { ElevatorSubsystem m_elevator{(frc::MechanismObject2d*)m_elevatorMech.GetRoot("Elevator", 0.25, 0.25)}; AlgaeArmSubsystem m_algaeArm; - IntakeSubsystem m_algaeIntake{AlgaeArmConstants::kIntakeMotorId, AlgaeArmConstants::kHasAlgaeCurrent}; + IntakeSubsystem m_algaeIntake{AlgaeArmConstants::kIntakeMotorId, AlgaeArmConstants::kHasAlgaeCurrent, true}; CoralArmSubsystem m_coralArm; - IntakeSubsystem m_coralIntake{CoralArmConstants::kIntakeMotorId, CoralArmConstants::kHasCoralCurrent}; + IntakeSubsystem m_coralIntake{CoralArmConstants::kIntakeMotorId, CoralArmConstants::kHasCoralCurrent, true}; ClimberSubsystem m_climber; diff --git a/src/main/include/commands/CommandController.h b/src/main/include/commands/CommandController.h index 308eed0..2145421 100644 --- a/src/main/include/commands/CommandController.h +++ b/src/main/include/commands/CommandController.h @@ -36,12 +36,6 @@ class CommandController { frc2::CommandPtr RemoveAlgaeFromL2(); frc2::CommandPtr RemoveAlgaeFromL3(); - - /** - @brief Must be called first during auto in order - to zero the elevator properly - */ - frc2::CommandPtr SetElevatorZeroPosition(); 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 a0285da..7f3ce03 100644 --- a/src/main/include/subsystems/AlgaeArmSubsystem.h +++ b/src/main/include/subsystems/AlgaeArmSubsystem.h @@ -39,7 +39,7 @@ class AlgaeArmSubsystem : public subzero::RotationalSingleAxisSubsystem #include +#include class IntakeSubsystem : public frc2::SubsystemBase { public: - IntakeSubsystem(const int id, double hasGamePieceCurrent) - : m_motor{id, rev::spark::SparkLowLevel::MotorType::kBrushless}, m_hasGamePieceCurrent{hasGamePieceCurrent} {} + 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); @@ -18,6 +25,7 @@ class IntakeSubsystem : public frc2::SubsystemBase { bool HasGamePiece(); private: rev::spark::SparkMax m_motor; + rev::spark::SparkMaxConfig m_config; double m_hasGamePieceCurrent; }; \ No newline at end of file From 66c2731991553576f9b925f260527f923953e726 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Sat, 22 Feb 2025 11:44:23 -0600 Subject: [PATCH 09/27] Use motor controller limit switches instead of digital inputs on the rio --- src/main/cpp/subsystems/ElevatorSubsystem.cpp | 21 +++++++++++++++++++ .../include/subsystems/ElevatorSubsystem.h | 13 +++++++----- 2 files changed, 29 insertions(+), 5 deletions(-) diff --git a/src/main/cpp/subsystems/ElevatorSubsystem.cpp b/src/main/cpp/subsystems/ElevatorSubsystem.cpp index 8e2bbba..4ec419f 100644 --- a/src/main/cpp/subsystems/ElevatorSubsystem.cpp +++ b/src/main/cpp/subsystems/ElevatorSubsystem.cpp @@ -3,4 +3,25 @@ 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()) { + SetEncoderPosition(ElevatorConstants::kMaxDistance); + } else if (GetMinLimitSwitch()) { + SetEncoderPosition(ElevatorConstants::kMinDistance); + } + + frc::SmartDashboard::PutBoolean("Forward limit switch state", GetMaxLimitSwitch()); + frc::SmartDashboard::PutBoolean("Reverse limit switch state", GetMinLimitSwitch()); + + subzero::LinearSingleAxisSubsystem::Periodic(); } \ No newline at end of file diff --git a/src/main/include/subsystems/ElevatorSubsystem.h b/src/main/include/subsystems/ElevatorSubsystem.h index 5527d27..1f70112 100644 --- a/src/main/include/subsystems/ElevatorSubsystem.h +++ b/src/main/include/subsystems/ElevatorSubsystem.h @@ -35,9 +35,9 @@ class ElevatorSubsystem : public subzero::LinearSingleAxisSubsystem Date: Sat, 22 Feb 2025 12:09:35 -0600 Subject: [PATCH 10/27] Lower elevator P --- src/main/include/Constants.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index a993c9f..868e1b5 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -169,7 +169,7 @@ namespace ElevatorConstants { const int kTopLimitSwitchPort = 2; // Placeholder values - const double kElevatorP = 100.0; + const double kElevatorP = 10.0; const double kElevatorI = 0.0; const double kElevatorD = 0.0; const double kElevatorIZone = 0.0; @@ -345,7 +345,7 @@ namespace CommandConstants { // Placeholder values constexpr units::degree_t kClimberDownAngle = 0_deg; - constexpr units::degree_t kClimberUpAngle = 90_deg; + constexpr units::degree_t kClimberUpAngle = 110_deg; // Placeholder constexpr units::second_t kCoralFeedTimeout = 3_s; From 335c9261c0f84fac7ddfe9206ed3fd5814cbf4b0 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Sat, 22 Feb 2025 12:49:02 -0600 Subject: [PATCH 11/27] Reset rotation command --- src/main/cpp/RobotContainer.cpp | 8 ++++++++ src/main/cpp/subsystems/DriveSubsystem.cpp | 4 ++++ src/main/include/subsystems/DriveSubsystem.h | 2 +- 3 files changed, 13 insertions(+), 1 deletion(-) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 3853c18..42edd45 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -77,6 +77,12 @@ void RobotContainer::ConfigureBindings() { m_operatorController.X().WhileTrue(m_commandController.ExpelAlgae()); m_operatorController.Y().OnTrue(m_commandController.IntakeAlgae()); + m_driverController.RightStick().OnTrue(frc2::InstantCommand( + [this]() { + m_drive.ResetRotation(); + } + ).ToPtr()); + // m_operatorController.LeftBumper().WhileTrue(m_commandController.ExpelAlgae()); // m_operatorController.RightBumper().WhileTrue(m_commandController.ExpelCoral()); } @@ -97,6 +103,8 @@ frc2::CommandPtr RobotContainer::GetAutonomousCommand() { 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() { diff --git a/src/main/cpp/subsystems/DriveSubsystem.cpp b/src/main/cpp/subsystems/DriveSubsystem.cpp index 38c86c3..6932b8b 100644 --- a/src/main/cpp/subsystems/DriveSubsystem.cpp +++ b/src/main/cpp/subsystems/DriveSubsystem.cpp @@ -220,6 +220,10 @@ void DriveSubsystem::ResetOdometry(frc::Pose2d pose) { pose); } +void DriveSubsystem::ResetRotation() { + m_odometry.ResetRotation(frc::Rotation2d()); +} + void DriveSubsystem::OffsetRotation(frc::Rotation2d offset) { m_pidgey.SetYaw(offset.Degrees() + m_pidgey.GetYaw().GetValue()); m_odometry.ResetPosition( diff --git a/src/main/include/subsystems/DriveSubsystem.h b/src/main/include/subsystems/DriveSubsystem.h index 042c6fd..fbb839e 100644 --- a/src/main/include/subsystems/DriveSubsystem.h +++ b/src/main/include/subsystems/DriveSubsystem.h @@ -112,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, From dc5e58d326e4b21af625bf6feeec63dadb2003fb Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Sat, 22 Feb 2025 14:33:59 -0600 Subject: [PATCH 12/27] Fix climbing command --- src/main/cpp/RobotContainer.cpp | 13 ++++++++++++- src/main/cpp/commands/CommandController.cpp | 8 -------- src/main/include/Constants.h | 2 ++ src/main/include/commands/CommandController.h | 3 --- 4 files changed, 14 insertions(+), 12 deletions(-) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 42edd45..cd3887e 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -60,8 +61,17 @@ RobotContainer::RobotContainer() { }, {&m_drive})); + m_climber.SetDefaultCommand( + frc2::RunCommand( + [this]() { + m_climber.RunMotorPercentage((-m_driverController.GetLeftTriggerAxis() + + m_driverController.GetRightTriggerAxis()) * ClimberConstants::kPercentageScalar); + } + ) + ); + // Has to be raised before match so coral arm is within frame perimeter - m_elevator.SetEncoderPosition(ElevatorConstants::kElevatorStartPosition); + // m_elevator.SetEncoderPosition(ElevatorConstants::kElevatorStartPosition); } void RobotContainer::ConfigureBindings() { @@ -79,6 +89,7 @@ void RobotContainer::ConfigureBindings() { m_driverController.RightStick().OnTrue(frc2::InstantCommand( [this]() { + std::cout << "Resetting rotation" << std::endl; m_drive.ResetRotation(); } ).ToPtr()); diff --git a/src/main/cpp/commands/CommandController.cpp b/src/main/cpp/commands/CommandController.cpp index bee2903..a1ffce6 100644 --- a/src/main/cpp/commands/CommandController.cpp +++ b/src/main/cpp/commands/CommandController.cpp @@ -53,14 +53,6 @@ frc2::CommandPtr CommandController::ExpelAlgae() { return m_subsystems.algaeIntake->MoveAtPercent(CommandConstants::kAlgaeExpelSpeed); } -frc2::CommandPtr CommandController::ClimberDown() { - return m_subsystems.climber->MoveToPositionAbsolute(CommandConstants::kClimberDownAngle); -} - -frc2::CommandPtr CommandController::ClimberUp() { - return m_subsystems.climber->MoveToPositionAbsolute(CommandConstants::kClimberUpAngle); -} - frc2::CommandPtr CommandController::RemoveAlgaeFromL2() { return m_subsystems.elevator->MoveToPositionAbsolute(CommandConstants::kElevatorRemoveAlgaeFromL2Position) .AndThen(m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralArmRemoveAlgaeFromL2Position)) diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index f2afbc9..d8960cd 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -302,6 +302,8 @@ namespace ClimberConstants{ constexpr double kVelocityScalar = 1.0; constexpr units::degree_t kTolerance = 2_deg; constexpr units::meter_t kArmLength = 17_in; + + constexpr double kPercentageScalar = 0.1; static const subzero::SingleAxisMechanism kClimberMechanism = { // length diff --git a/src/main/include/commands/CommandController.h b/src/main/include/commands/CommandController.h index 2145421..6ea09ee 100644 --- a/src/main/include/commands/CommandController.h +++ b/src/main/include/commands/CommandController.h @@ -30,9 +30,6 @@ class CommandController { frc2::CommandPtr IntakeAlgae(); frc2::CommandPtr ExpelAlgae(); - - frc2::CommandPtr ClimberDown(); - frc2::CommandPtr ClimberUp(); frc2::CommandPtr RemoveAlgaeFromL2(); frc2::CommandPtr RemoveAlgaeFromL3(); From 869b403e986105f6787e876bb06cb60c01425425 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Sat, 22 Feb 2025 18:19:35 -0600 Subject: [PATCH 13/27] More commands and angle fixes --- src/main/cpp/RobotContainer.cpp | 29 ++++++++++-------- src/main/cpp/commands/CommandController.cpp | 30 ++++++++++++++++++- src/main/cpp/subsystems/DriveSubsystem.cpp | 10 +++---- src/main/cpp/subsystems/ElevatorSubsystem.cpp | 12 ++++++-- .../Center Start to Center Rear Reef.path | 4 +-- src/main/include/Constants.h | 28 ++++++++++++----- src/main/include/commands/CommandController.h | 2 ++ .../include/subsystems/ElevatorSubsystem.h | 6 ++-- 8 files changed, 87 insertions(+), 34 deletions(-) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index cd3887e..4c08fd3 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -38,7 +39,7 @@ RobotContainer::RobotContainer() { m_chooser.AddOption(AutoConstants::kFarLeftAuto, AutoConstants::kFarLeftAuto); m_chooser.AddOption(AutoConstants::kFarRightAuto, AutoConstants::kFarRightAuto); - pathplanner::NamedCommands::registerCommand("Test Named Command", frc2::InstantCommand([]() { std::cout << "Test Command Was Called" << std::endl; }).ToPtr()); + pathplanner::NamedCommands::registerCommand("Test Command", frc2::InstantCommand([]() { std::cout << "Test Command Was Called" << std::endl; }).ToPtr()); pathplanner::NamedCommands::registerCommand("L2 Position", std::move(m_commandController.MoveToPositionL2())); pathplanner::NamedCommands::registerCommand("L3 Position", std::move(m_commandController.MoveToPositionL3())); @@ -61,14 +62,15 @@ RobotContainer::RobotContainer() { }, {&m_drive})); - m_climber.SetDefaultCommand( - frc2::RunCommand( - [this]() { - m_climber.RunMotorPercentage((-m_driverController.GetLeftTriggerAxis() - + m_driverController.GetRightTriggerAxis()) * ClimberConstants::kPercentageScalar); - } - ) - ); + // m_climber.SetDefaultCommand( + // frc2::RunCommand( + // [this]() { + // m_climber.RunMotorPercentage((-m_driverController.GetLeftTriggerAxis() + // + m_driverController.GetRightTriggerAxis()) * ClimberConstants::kPercentageScalar); + // }, + // {&m_climber} + // ) + // ); // Has to be raised before match so coral arm is within frame perimeter // m_elevator.SetEncoderPosition(ElevatorConstants::kElevatorStartPosition); @@ -82,12 +84,15 @@ void RobotContainer::ConfigureBindings() { m_operatorController.POVRight().OnTrue(m_commandController.MoveToPositionL3()); m_operatorController.POVDown().OnTrue(m_commandController.FeedCoral()); - m_operatorController.A().WhileTrue(m_commandController.ExpelCoral()); m_operatorController.X().WhileTrue(m_commandController.ExpelAlgae()); m_operatorController.Y().OnTrue(m_commandController.IntakeAlgae()); - m_driverController.RightStick().OnTrue(frc2::InstantCommand( + m_operatorController.LeftBumper().OnTrue(m_commandController.RemoveAlgaeFromL2()); + m_operatorController.RightBumper().OnTrue(m_commandController.RemoveAlgaeFromL3()); + + m_driverController.LeftBumper().OnTrue(m_commandController.HomeElevator()); + m_driverController.RightBumper().OnTrue(frc2::InstantCommand( [this]() { std::cout << "Resetting rotation" << std::endl; m_drive.ResetRotation(); @@ -115,7 +120,7 @@ frc2::CommandPtr RobotContainer::GetAutonomousCommand() { void RobotContainer::Periodic() { frc::SmartDashboard::PutData("Robot Elevator", &m_elevatorMech); - frc::SmartDashboard::PutData("Zero Odometry", new frc2::InstantCommand([this]() { m_drive.ResetRotation(); })); + // frc::SmartDashboard::PutData("Zero Odometry", new frc2::InstantCommand([this]() { m_drive.ResetRotation(); })); } void RobotContainer::Initialize() { diff --git a/src/main/cpp/commands/CommandController.cpp b/src/main/cpp/commands/CommandController.cpp index a1ffce6..b962d2e 100644 --- a/src/main/cpp/commands/CommandController.cpp +++ b/src/main/cpp/commands/CommandController.cpp @@ -56,7 +56,7 @@ frc2::CommandPtr CommandController::ExpelAlgae() { frc2::CommandPtr CommandController::RemoveAlgaeFromL2() { return m_subsystems.elevator->MoveToPositionAbsolute(CommandConstants::kElevatorRemoveAlgaeFromL2Position) .AndThen(m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralArmRemoveAlgaeFromL2Position)) - .AndThen(m_subsystems.coralIntake->MoveAtPercent(CommandConstants::kCoralFeedSpeed)) + .AndThen(m_subsystems.coralIntake->MoveAtPercent(CommandConstants::kCoralExpelSpeed)) .WithTimeout(CommandConstants::kRemoveAlgaeFromReefTimeout); ; } @@ -67,3 +67,31 @@ frc2::CommandPtr CommandController::RemoveAlgaeFromL3() { .AndThen(m_subsystems.coralIntake->MoveAtPercent(CommandConstants::kCoralFeedSpeed)) .WithTimeout(CommandConstants::kRemoveAlgaeFromReefTimeout); } + +frc2::CommandPtr CommandController::HomeElevator() { + return + frc2::InstantCommand( + [this]() { + std::cout << "Homing elevator" << std::endl; + m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralFeedPosition); + } + ).ToPtr() + .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); + } + ); +} \ No newline at end of file diff --git a/src/main/cpp/subsystems/DriveSubsystem.cpp b/src/main/cpp/subsystems/DriveSubsystem.cpp index 6932b8b..70bd9f9 100644 --- a/src/main/cpp/subsystems/DriveSubsystem.cpp +++ b/src/main/cpp/subsystems/DriveSubsystem.cpp @@ -75,7 +75,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() { @@ -221,7 +221,7 @@ void DriveSubsystem::ResetOdometry(frc::Pose2d pose) { } void DriveSubsystem::ResetRotation() { - m_odometry.ResetRotation(frc::Rotation2d()); + m_pidgey.Reset(); } void DriveSubsystem::OffsetRotation(frc::Rotation2d offset) { @@ -235,12 +235,12 @@ void DriveSubsystem::OffsetRotation(frc::Rotation2d offset) { void DriveSubsystem::AddVisionMeasurement(const frc::Pose2d& visionMeasurement, units::second_t timestamp) { - poseEstimator.AddVisionMeasurement(visionMeasurement, 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); + // 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 index 4ec419f..1bf169f 100644 --- a/src/main/cpp/subsystems/ElevatorSubsystem.cpp +++ b/src/main/cpp/subsystems/ElevatorSubsystem.cpp @@ -6,17 +6,19 @@ void ElevatorSubsystem::SetEncoderPosition(units::meter_t pos) { } bool ElevatorSubsystem::GetMinLimitSwitch() { - return !m_leadMotor.GetReverseLimitSwitch().Get(); + return m_leadMotor.GetReverseLimitSwitch().Get(); } bool ElevatorSubsystem::GetMaxLimitSwitch() { - return !m_leadMotor.GetForwardLimitSwitch().Get(); + 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); } @@ -24,4 +26,8 @@ void ElevatorSubsystem::Periodic() { frc::SmartDashboard::PutBoolean("Reverse limit switch state", GetMinLimitSwitch()); subzero::LinearSingleAxisSubsystem::Periodic(); -} \ No newline at end of file +} + +void ElevatorSubsystem::UpdatePidSettings(subzero::PidSettings settings) { + m_elevatorController.UpdatePidSettings(settings); +} diff --git a/src/main/deploy/pathplanner/paths/Center Start to Center Rear Reef.path b/src/main/deploy/pathplanner/paths/Center Start to Center Rear Reef.path index 8297d2c..96ff9d6 100644 --- a/src/main/deploy/pathplanner/paths/Center Start to Center Rear Reef.path +++ b/src/main/deploy/pathplanner/paths/Center Start to Center Rear Reef.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [ { - "name": "Test2", - "waypointRelativePos": 0.5486891385767628, + "name": "Test Command", + "waypointRelativePos": 0.6191061910619094, "endWaypointRelativePos": null, "command": null } diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index d8960cd..488528f 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -40,6 +40,10 @@ #include #include +#ifndef M_PI + #define M_PI 3.141592653589793238462643383279502884197 +#endif + using SparkMaxPidController = subzero::PidMotorController Date: Sat, 22 Feb 2025 19:54:24 -0600 Subject: [PATCH 14/27] Fix some angles and add discrete coral intake --- src/main/cpp/RobotContainer.cpp | 1 + src/main/include/Constants.h | 12 ++++++------ 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 4c08fd3..76eea8e 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -85,6 +85,7 @@ void RobotContainer::ConfigureBindings() { 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().OnTrue(m_commandController.IntakeAlgae()); diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index 488528f..cb7ba47 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -340,23 +340,23 @@ namespace CommandConstants { 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 = 3_in; + constexpr units::meter_t kElevatorFeedPosition = 1.2_in; constexpr units::meter_t kElevatorRemoveAlgaeFromL2Position = 0.38_m; constexpr units::meter_t kElevatorRemoveAlgaeFromL3Position = 0.50_m; constexpr units::degree_t kCoralL1Position = 254_deg; - constexpr units::degree_t kCoralL2Position = 234_deg; - constexpr units::degree_t kCoralL3Position = 210_deg; - constexpr units::degree_t kCoralFeedPosition = 70_deg; + constexpr units::degree_t kCoralL2Position = 233_deg; + constexpr units::degree_t kCoralL3Position = 213_deg; + constexpr units::degree_t kCoralFeedPosition = 67_deg; constexpr units::degree_t kCoralArmRemoveAlgaeFromL2Position = 204_deg; - constexpr units::degree_t kCoralArmRemoveAlgaeFromL3Position = 188_deg; + constexpr units::degree_t kCoralArmRemoveAlgaeFromL3Position = 248.5_deg; constexpr units::degree_t kAlgaeIntakePosition = 39_deg; constexpr units::degree_t kAlgaeStorePosition = 30_deg; constexpr units::degree_t kAlgaeStowPosition = 0_deg; constexpr units::degree_t kAlgaeArmReefPosition = 20_deg; - constexpr double kCoralFeedSpeed = 0.25; + constexpr double kCoralFeedSpeed = -0.25; constexpr double kCoralExpelSpeed = -0.25; constexpr double kCoralIntakeRetainCoralSpeed = 0.25; From 16b137e2ecf7422dd84b5606ed7aa95353e69bee Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Sat, 22 Feb 2025 22:07:30 -0600 Subject: [PATCH 15/27] Remove climbing --- src/main/cpp/RobotContainer.cpp | 6 +++++- src/main/include/Constants.h | 4 ++-- src/main/include/RobotContainer.h | 4 ++-- 3 files changed, 9 insertions(+), 5 deletions(-) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 76eea8e..f09e050 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -67,6 +67,10 @@ RobotContainer::RobotContainer() { // [this]() { // m_climber.RunMotorPercentage((-m_driverController.GetLeftTriggerAxis() // + m_driverController.GetRightTriggerAxis()) * ClimberConstants::kPercentageScalar); + + // std::cout << "Controller input: " << (-m_driverController.GetLeftTriggerAxis() + // + m_driverController.GetRightTriggerAxis()) * ClimberConstants::kPercentageScalar + // << ", Current relative position: " << m_climber.GetCurrentPosition().value() << std::endl; // }, // {&m_climber} // ) @@ -128,5 +132,5 @@ void RobotContainer::Initialize() { m_elevator.OnInit(); m_coralArm.OnInit(); m_algaeArm.OnInit(); - m_climber.OnInit(); + // m_climber.OnInit(); } \ No newline at end of file diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index cb7ba47..c8b92d7 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -307,7 +307,7 @@ 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; @@ -315,7 +315,7 @@ namespace ClimberConstants{ constexpr units::degree_t kTolerance = 2_deg; constexpr units::meter_t kArmLength = 17_in; - constexpr double kPercentageScalar = 0.1; + constexpr double kPercentageScalar = 0.8; static const subzero::SingleAxisMechanism kClimberMechanism = { // length diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index 499750c..a176cd6 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -75,7 +75,7 @@ class RobotContainer { CoralArmSubsystem m_coralArm; IntakeSubsystem m_coralIntake{CoralArmConstants::kIntakeMotorId, CoralArmConstants::kHasCoralCurrent, true}; - ClimberSubsystem m_climber; + // ClimberSubsystem m_climber; Subsystems_t subsystems = { .algaeArm = &m_algaeArm, @@ -83,7 +83,7 @@ class RobotContainer { .elevator = &m_elevator, .coralIntake = &m_coralIntake, .algaeIntake = &m_algaeIntake, - .climber = &m_climber + .climber = NULL }; CommandController m_commandController{subsystems}; From 4b29956d2e0c980c21ff567cf6ef74bdf7fc8fca Mon Sep 17 00:00:00 2001 From: 5690 <5690Programmers@gmail.com> Date: Sun, 23 Feb 2025 08:52:15 -0600 Subject: [PATCH 16/27] New WPILib version and update some constants --- build.gradle | 2 +- src/main/cpp/subsystems/ElevatorSubsystem.cpp | 2 +- src/main/include/Constants.h | 8 ++++---- 3 files changed, 6 insertions(+), 6 deletions(-) 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/subsystems/ElevatorSubsystem.cpp b/src/main/cpp/subsystems/ElevatorSubsystem.cpp index 1bf169f..2fc6ae9 100644 --- a/src/main/cpp/subsystems/ElevatorSubsystem.cpp +++ b/src/main/cpp/subsystems/ElevatorSubsystem.cpp @@ -30,4 +30,4 @@ void ElevatorSubsystem::Periodic() { void ElevatorSubsystem::UpdatePidSettings(subzero::PidSettings settings) { m_elevatorController.UpdatePidSettings(settings); -} +} \ No newline at end of file diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index c8b92d7..712a90e 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -346,17 +346,17 @@ namespace CommandConstants { constexpr units::degree_t kCoralL1Position = 254_deg; constexpr units::degree_t kCoralL2Position = 233_deg; - constexpr units::degree_t kCoralL3Position = 213_deg; - constexpr units::degree_t kCoralFeedPosition = 67_deg; + constexpr units::degree_t kCoralL3Position = 217_deg; + constexpr units::degree_t kCoralFeedPosition = 77_deg; constexpr units::degree_t kCoralArmRemoveAlgaeFromL2Position = 204_deg; - constexpr units::degree_t kCoralArmRemoveAlgaeFromL3Position = 248.5_deg; + constexpr units::degree_t kCoralArmRemoveAlgaeFromL3Position = 210_deg; constexpr units::degree_t kAlgaeIntakePosition = 39_deg; constexpr units::degree_t kAlgaeStorePosition = 30_deg; constexpr units::degree_t kAlgaeStowPosition = 0_deg; constexpr units::degree_t kAlgaeArmReefPosition = 20_deg; - constexpr double kCoralFeedSpeed = -0.25; + constexpr double kCoralFeedSpeed = 0.25; constexpr double kCoralExpelSpeed = -0.25; constexpr double kCoralIntakeRetainCoralSpeed = 0.25; From 2095a16ff4e4d3b1a0e6e950287689d2b0dd7e7e Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Sun, 23 Feb 2025 11:54:16 -0600 Subject: [PATCH 17/27] Use proper command for home --- src/main/cpp/commands/CommandController.cpp | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/src/main/cpp/commands/CommandController.cpp b/src/main/cpp/commands/CommandController.cpp index b962d2e..812cb0f 100644 --- a/src/main/cpp/commands/CommandController.cpp +++ b/src/main/cpp/commands/CommandController.cpp @@ -70,12 +70,7 @@ frc2::CommandPtr CommandController::RemoveAlgaeFromL3() { frc2::CommandPtr CommandController::HomeElevator() { return - frc2::InstantCommand( - [this]() { - std::cout << "Homing elevator" << std::endl; - m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralFeedPosition); - } - ).ToPtr() + m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralFeedPosition) .AndThen(frc2::InstantCommand( [this]() { m_subsystems.elevator->SetEncoderPosition(ElevatorConstants::kMaxDistance); From f96868a689727d8333767d2a43c31c89a99a8fac Mon Sep 17 00:00:00 2001 From: 5690 <5690Programmers@gmail.com> Date: Sun, 23 Feb 2025 13:06:20 -0600 Subject: [PATCH 18/27] updated version --- ...enix6-25.2.1.json => Phoenix6-25.3.0.json} | 88 +++++++++++++------ 1 file changed, 59 insertions(+), 29 deletions(-) rename vendordeps/{Phoenix6-25.2.1.json => Phoenix6-25.3.0.json} (86%) 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 From 9181925b4f440a22edf426c7c5307e134f411b78 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Sun, 23 Feb 2025 14:25:40 -0600 Subject: [PATCH 19/27] Use event markers --- src/main/cpp/RobotContainer.cpp | 4 ++-- .../pathplanner/paths/Center Start to Center Rear Reef.path | 4 ++-- src/main/include/Constants.h | 3 ++- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index f09e050..a005f4d 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -32,14 +32,14 @@ RobotContainer::RobotContainer() { // Initialize all of your commands and subsystems here - // m_chooser.SetDefaultOption(AutoConstants::kDefaultAutoName, AutoConstants::kDefaultAutoName); + m_chooser.SetDefaultOption(AutoConstants::kDefaultAutoName, AutoConstants::kDefaultAutoName); frc::SmartDashboard::PutData(&m_chooser); m_chooser.SetDefaultOption(AutoConstants::kCenterToCenterAuto, AutoConstants::kCenterToCenterAuto); m_chooser.AddOption(AutoConstants::kFarLeftAuto, AutoConstants::kFarLeftAuto); m_chooser.AddOption(AutoConstants::kFarRightAuto, AutoConstants::kFarRightAuto); - pathplanner::NamedCommands::registerCommand("Test Command", frc2::InstantCommand([]() { std::cout << "Test Command Was Called" << std::endl; }).ToPtr()); + pathplanner::NamedCommands::registerCommand("Test Command", frc2::cmd::Print("Test Command")); pathplanner::NamedCommands::registerCommand("L2 Position", std::move(m_commandController.MoveToPositionL2())); pathplanner::NamedCommands::registerCommand("L3 Position", std::move(m_commandController.MoveToPositionL3())); diff --git a/src/main/deploy/pathplanner/paths/Center Start to Center Rear Reef.path b/src/main/deploy/pathplanner/paths/Center Start to Center Rear Reef.path index 96ff9d6..31ec341 100644 --- a/src/main/deploy/pathplanner/paths/Center Start to Center Rear Reef.path +++ b/src/main/deploy/pathplanner/paths/Center Start to Center Rear Reef.path @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [ { - "name": "Test Command", - "waypointRelativePos": 0.6191061910619094, + "name": "Test Event Marker", + "waypointRelativePos": 0.5050641458474013, "endWaypointRelativePos": null, "command": null } diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index 712a90e..cfdbba3 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -153,6 +153,7 @@ constexpr auto kMaxAngularAcceleration = 3.142_rad_per_s_sq; const std::string kCenterToCenterAuto = "Center To Rear Center Reef (1 Coral)"; const std::string kFarLeftAuto = "Far Left Auto (3 Coral)"; const std::string kFarRightAuto = "Far Right Auto (3 Coral)"; +const std::string kDefaultAutoName = kCenterToCenterAuto; constexpr double kPXController = 0.5; constexpr double kPYController = 0.5; @@ -375,7 +376,7 @@ namespace CommandConstants { namespace VisionConstants { static constexpr std::string_view kFrontCamera{"PhotonVision"}; -static constexpr std::string_view kRearCamera{"Photonvision2"}; +static constexpr std::string_view kRearCamera{"PhotonVision2"}; static const frc::Transform3d kRobotToCam2{ frc::Translation3d{2.147_in, 0_in, 23.369_in}, frc::Rotation3d{0_deg, -23.461_deg, 180_deg}}; From 3e0de2052b5ebbe8499225e4b2e945b0427e5f1e Mon Sep 17 00:00:00 2001 From: 5690 <5690Programmers@gmail.com> Date: Mon, 24 Feb 2025 15:58:16 -0600 Subject: [PATCH 20/27] Constant and auto changes --- src/main/cpp/RobotContainer.cpp | 14 +++++++++----- src/main/cpp/commands/CommandController.cpp | 15 ++++++++++----- .../Center To Rear Center Reef (1 Coral).auto | 18 ++++++++++++++++++ .../Center Start to Center Rear Reef.path | 9 +-------- src/main/include/Constants.h | 11 +++++++---- 5 files changed, 45 insertions(+), 22 deletions(-) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index f09e050..151f5eb 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -38,10 +38,14 @@ RobotContainer::RobotContainer() { m_chooser.SetDefaultOption(AutoConstants::kCenterToCenterAuto, AutoConstants::kCenterToCenterAuto); m_chooser.AddOption(AutoConstants::kFarLeftAuto, AutoConstants::kFarLeftAuto); m_chooser.AddOption(AutoConstants::kFarRightAuto, AutoConstants::kFarRightAuto); - - pathplanner::NamedCommands::registerCommand("Test Command", frc2::InstantCommand([]() { std::cout << "Test Command Was Called" << std::endl; }).ToPtr()); - pathplanner::NamedCommands::registerCommand("L2 Position", std::move(m_commandController.MoveToPositionL2())); - pathplanner::NamedCommands::registerCommand("L3 Position", std::move(m_commandController.MoveToPositionL3())); + + // pathplanner::NamedCommands::registerCommand("Test Command", frc2::InstantCommand([]() { std::cout << "Test Command Was Called" << std::endl; }).ToPtr()); + 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("Home Elevator", std::move(m_commandController.HomeElevator())); + pathplanner::NamedCommands::registerCommand("To Feed Position", std::move(m_commandController.FeedCoral())); + pathplanner::NamedCommands::registerCommand("Expel Coral", std::move(m_commandController.ExpelCoral().WithTimeout(CommandConstants::kExpelCoralTimeout))); // Configure the button bindings ConfigureBindings(); @@ -91,7 +95,7 @@ void RobotContainer::ConfigureBindings() { 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().OnTrue(m_commandController.IntakeAlgae()); + m_operatorController.Y().WhileTrue(m_commandController.IntakeAlgae()); m_operatorController.LeftBumper().OnTrue(m_commandController.RemoveAlgaeFromL2()); m_operatorController.RightBumper().OnTrue(m_commandController.RemoveAlgaeFromL3()); diff --git a/src/main/cpp/commands/CommandController.cpp b/src/main/cpp/commands/CommandController.cpp index 812cb0f..5f27e23 100644 --- a/src/main/cpp/commands/CommandController.cpp +++ b/src/main/cpp/commands/CommandController.cpp @@ -27,6 +27,7 @@ frc2::CommandPtr CommandController::MoveToPositionL3() { 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) @@ -43,10 +44,11 @@ frc2::CommandPtr CommandController::ExpelCoral() { 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); + // .Until([this]() { + // return m_subsystems.algaeIntake->HasGamePiece(); + // }) + // .WithTimeout(CommandConstants::kAlgaeIntakeTimeout); + ; } frc2::CommandPtr CommandController::ExpelAlgae() { @@ -55,14 +57,17 @@ frc2::CommandPtr CommandController::ExpelAlgae() { 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); + .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); diff --git a/src/main/deploy/pathplanner/autos/Center To Rear Center Reef (1 Coral).auto b/src/main/deploy/pathplanner/autos/Center To Rear Center Reef (1 Coral).auto index 6bbf703..ce59964 100644 --- a/src/main/deploy/pathplanner/autos/Center To Rear Center Reef (1 Coral).auto +++ b/src/main/deploy/pathplanner/autos/Center To Rear Center Reef (1 Coral).auto @@ -4,11 +4,29 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "Home Elevator" + } + }, + { + "type": "named", + "data": { + "name": "To L1 Position" + } + }, { "type": "path", "data": { "pathName": "Center Start to Center Rear Reef" } + }, + { + "type": "named", + "data": { + "name": "Expel Coral" + } } ] } diff --git a/src/main/deploy/pathplanner/paths/Center Start to Center Rear Reef.path b/src/main/deploy/pathplanner/paths/Center Start to Center Rear Reef.path index 96ff9d6..3f226a4 100644 --- a/src/main/deploy/pathplanner/paths/Center Start to Center Rear Reef.path +++ b/src/main/deploy/pathplanner/paths/Center Start to Center Rear Reef.path @@ -31,14 +31,7 @@ "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Test Command", - "waypointRelativePos": 0.6191061910619094, - "endWaypointRelativePos": null, - "command": null - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index 712a90e..b93659c 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -344,24 +344,26 @@ namespace CommandConstants { constexpr units::meter_t kElevatorRemoveAlgaeFromL2Position = 0.38_m; constexpr units::meter_t kElevatorRemoveAlgaeFromL3Position = 0.50_m; - constexpr units::degree_t kCoralL1Position = 254_deg; + constexpr units::degree_t kCoralL1Position = 249_deg; constexpr units::degree_t kCoralL2Position = 233_deg; - constexpr units::degree_t kCoralL3Position = 217_deg; + constexpr units::degree_t kCoralL3Position = 215_deg; constexpr units::degree_t kCoralFeedPosition = 77_deg; + constexpr units::degree_t kCoralHomePosition = 110_deg; constexpr units::degree_t kCoralArmRemoveAlgaeFromL2Position = 204_deg; constexpr units::degree_t kCoralArmRemoveAlgaeFromL3Position = 210_deg; - constexpr units::degree_t kAlgaeIntakePosition = 39_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 double kCoralFeedSpeed = 0.25; constexpr double kCoralExpelSpeed = -0.25; constexpr double kCoralIntakeRetainCoralSpeed = 0.25; constexpr double kAlgaeIntakeSpeed = -0.3; - constexpr double kAlgaeExpelSpeed = 0.3; + constexpr double kAlgaeExpelSpeed = 0.65; // Placeholder values constexpr units::degree_t kClimberDownAngle = 0_deg; @@ -371,6 +373,7 @@ namespace CommandConstants { 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 = 3_s; } namespace VisionConstants { From adf9c8a694b8cf2a9752b188736a57c572dcddb5 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Mon, 24 Feb 2025 19:07:08 -0600 Subject: [PATCH 21/27] New auto path and new named comands --- src/main/cpp/RobotContainer.cpp | 2 + .../Center To Rear Center Reef (1 Coral).auto | 18 -------- .../autos/Far Left Auto (3 Coral).auto | 24 ++++++++++ .../autos/Far Right Auto (3 Coral).auto | 26 +---------- .../Center Start to Center Rear Reef.path | 30 ++++++++---- .../paths/Far Left Source to Left Reef.path | 22 +++++++-- ...Right Source to Reef Right Left Side.path} | 32 +++++++++---- .../paths/Left Reef to Far Left Source.path | 22 +++++++-- .../paths/Left Start to Reef Left.path | 46 +++++++++++++++---- .../paths/Right Reef to Far Right Source.path | 28 +++++++---- ...Right Start to Reef Right Right Side.path} | 30 ++++++++---- src/main/include/Constants.h | 4 +- 12 files changed, 184 insertions(+), 100 deletions(-) rename src/main/deploy/pathplanner/paths/{Far Right Source to Reef Right.path => Far Right Source to Reef Right Left Side.path} (61%) rename src/main/deploy/pathplanner/paths/{Right Start to Reef Right.path => Right Start to Reef Right Right Side.path} (63%) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index c380a47..14a72ae 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -43,6 +43,8 @@ RobotContainer::RobotContainer() { 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.RemoveAlgaeFromL3())); pathplanner::NamedCommands::registerCommand("Home Elevator", std::move(m_commandController.HomeElevator())); pathplanner::NamedCommands::registerCommand("To Feed Position", std::move(m_commandController.FeedCoral())); pathplanner::NamedCommands::registerCommand("Expel Coral", std::move(m_commandController.ExpelCoral().WithTimeout(CommandConstants::kExpelCoralTimeout))); diff --git a/src/main/deploy/pathplanner/autos/Center To Rear Center Reef (1 Coral).auto b/src/main/deploy/pathplanner/autos/Center To Rear Center Reef (1 Coral).auto index ce59964..6bbf703 100644 --- a/src/main/deploy/pathplanner/autos/Center To Rear Center Reef (1 Coral).auto +++ b/src/main/deploy/pathplanner/autos/Center To Rear Center Reef (1 Coral).auto @@ -4,29 +4,11 @@ "type": "sequential", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "Home Elevator" - } - }, - { - "type": "named", - "data": { - "name": "To L1 Position" - } - }, { "type": "path", "data": { "pathName": "Center Start to Center Rear Reef" } - }, - { - "type": "named", - "data": { - "name": "Expel Coral" - } } ] } diff --git a/src/main/deploy/pathplanner/autos/Far Left Auto (3 Coral).auto b/src/main/deploy/pathplanner/autos/Far Left Auto (3 Coral).auto index b8528dd..a318f52 100644 --- a/src/main/deploy/pathplanner/autos/Far Left Auto (3 Coral).auto +++ b/src/main/deploy/pathplanner/autos/Far Left Auto (3 Coral).auto @@ -4,12 +4,24 @@ "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "Home" + } + }, { "type": "path", "data": { "pathName": "Left Start to Reef Left" } }, + { + "type": "named", + "data": { + "name": "Expel Coral" + } + }, { "type": "path", "data": { @@ -22,6 +34,12 @@ "pathName": "Far Left Source to Left Reef" } }, + { + "type": "named", + "data": { + "name": "Expel Coral" + } + }, { "type": "path", "data": { @@ -33,6 +51,12 @@ "data": { "pathName": "Far Left Source to Left Reef" } + }, + { + "type": "named", + "data": { + "name": "Expel Coral" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/Far Right Auto (3 Coral).auto b/src/main/deploy/pathplanner/autos/Far Right Auto (3 Coral).auto index 3c2e48e..105fde5 100644 --- a/src/main/deploy/pathplanner/autos/Far Right Auto (3 Coral).auto +++ b/src/main/deploy/pathplanner/autos/Far Right Auto (3 Coral).auto @@ -7,31 +7,7 @@ { "type": "path", "data": { - "pathName": "Right Start to Reef Right" - } - }, - { - "type": "path", - "data": { - "pathName": "Right Reef to Far Right Source" - } - }, - { - "type": "path", - "data": { - "pathName": "Far Right Source to Reef Right" - } - }, - { - "type": "path", - "data": { - "pathName": "Right Reef to Far Right Source" - } - }, - { - "type": "path", - "data": { - "pathName": "Far Right Source to Reef Right" + "pathName": "Right Start to Reef Right Right Side" } } ] diff --git a/src/main/deploy/pathplanner/paths/Center Start to Center Rear Reef.path b/src/main/deploy/pathplanner/paths/Center Start to Center Rear Reef.path index 3f226a4..f6db494 100644 --- a/src/main/deploy/pathplanner/paths/Center Start to Center Rear Reef.path +++ b/src/main/deploy/pathplanner/paths/Center Start to Center Rear Reef.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.364732142857142, - "y": 3.9205357142857142 + "x": 7.600204918032786, + "y": 3.8511782786885242 }, "prevControl": null, "nextControl": { - "x": 5.925974025974026, - "y": 3.949025974025974 + "x": 6.16144680114967, + "y": 3.879668538428784 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.90698051948052, - "y": 3.9205357142857142 + "x": 5.957889344262295, + "y": 3.8511782786885242 }, "prevControl": { - "x": 7.317248376623375, - "y": 3.915787337662337 + "x": 7.36815720140515, + "y": 3.846429902065147 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,19 @@ "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "", + "waypointRelativePos": 0.2970965563808233, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "To L3 Position" + } + } + } + ], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/Far Left Source to Left Reef.path b/src/main/deploy/pathplanner/paths/Far Left Source to Left Reef.path index bd485d4..f2a47c6 100644 --- a/src/main/deploy/pathplanner/paths/Far Left Source to Left Reef.path +++ b/src/main/deploy/pathplanner/paths/Far Left Source to Left Reef.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 3.5992694805194803, - "y": 5.117126623376623 + "x": 3.5685000000000002, + "y": 5.204749999999999 }, "prevControl": { - "x": 2.9914772727272725, - "y": 6.000324675324675 + "x": 2.960707792207792, + "y": 6.087948051948051 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,19 @@ "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "", + "waypointRelativePos": 0.2214719783929785, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "To L2 Position" + } + } + } + ], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/Far Right Source to Reef Right.path b/src/main/deploy/pathplanner/paths/Far Right Source to Reef Right Left Side.path similarity index 61% rename from src/main/deploy/pathplanner/paths/Far Right Source to Reef Right.path rename to src/main/deploy/pathplanner/paths/Far Right Source to Reef Right Left Side.path index a0059bd..4b52d8d 100644 --- a/src/main/deploy/pathplanner/paths/Far Right Source to Reef Right.path +++ b/src/main/deploy/pathplanner/paths/Far Right Source to Reef Right Left Side.path @@ -8,30 +8,42 @@ }, "prevControl": null, "nextControl": { - "x": 2.6258522727272724, - "y": 1.7837662337662332 + "x": 5.5575, + "y": 1.9092499999999994 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.708, - "y": 2.895 + "x": 5.0992500000000005, + "y": 2.66 }, "prevControl": { - "x": 2.625852272727272, - "y": 1.7932629870129873 + "x": 5.4502500000000005, + "y": 2.1237500000000002 }, "nextControl": null, "isLocked": false, - "linkedName": "Right Reef" + "linkedName": null } ], "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "", + "waypointRelativePos": 0.09627791563275767, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "To L3 Position" + } + } + } + ], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, @@ -42,13 +54,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 59.349332043143804 + "rotation": 119.53878225955823 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 55.61965527615502 + "rotation": 126.6624195429256 }, "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 index 5489fa3..8ab9262 100644 --- 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 @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.5992694805194803, - "y": 5.117126623376623 + "x": 3.5685000000000002, + "y": 5.204749999999999 }, "prevControl": null, "nextControl": { - "x": 2.9962256493506496, - "y": 6.057305194805195 + "x": 2.3984999999999994, + "y": 6.56 }, "isLocked": false, "linkedName": "Left Reef" @@ -31,7 +31,19 @@ "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "", + "waypointRelativePos": 0.21538461538461356, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "To Feed Position" + } + } + } + ], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/Left Start to Reef Left.path b/src/main/deploy/pathplanner/paths/Left Start to Reef Left.path index 15ba5e6..29ddb86 100644 --- a/src/main/deploy/pathplanner/paths/Left Start to Reef Left.path +++ b/src/main/deploy/pathplanner/paths/Left Start to Reef Left.path @@ -3,25 +3,41 @@ "waypoints": [ { "anchor": { - "x": 7.345738636363635, - "y": 7.225405844155843 + "x": 8.112, + "y": 6.1114999999999995 }, "prevControl": null, "nextControl": { - "x": 7.10535764945343, - "y": 7.156725562181498 + "x": 7.871619013089795, + "y": 6.042819718025655 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.5992694805194803, - "y": 5.117126623376623 + "x": 3.2857500000000006, + "y": 6.1114999999999995 }, "prevControl": { - "x": 4.852840909090911, - "y": 5.895860389610388 + "x": 7.9072499999999994, + "y": 6.0432500000000005 + }, + "nextControl": { + "x": 2.906417371480787, + "y": 6.117101958649017 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.5685000000000002, + "y": 5.204749999999999 + }, + "prevControl": { + "x": 3.3832500000000003, + "y": 5.672749999999999 }, "nextControl": null, "isLocked": false, @@ -31,7 +47,19 @@ "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "", + "waypointRelativePos": 0.704714640198519, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "To L3 Position" + } + } + } + ], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/Right Reef to Far Right Source.path b/src/main/deploy/pathplanner/paths/Right Reef to Far Right Source.path index be6a78a..44fed7e 100644 --- a/src/main/deploy/pathplanner/paths/Right Reef to Far Right Source.path +++ b/src/main/deploy/pathplanner/paths/Right Reef to Far Right Source.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.708, - "y": 2.895 + "x": 3.6270000000000007, + "y": 2.7965 }, "prevControl": null, "nextControl": { - "x": 2.8015422077922074, - "y": 2.006939935064935 + "x": 2.7205422077922075, + "y": 1.9084399350649353 }, "isLocked": false, "linkedName": "Right Reef" @@ -25,13 +25,25 @@ }, "nextControl": null, "isLocked": false, - "linkedName": "Far Right Source" + "linkedName": null } ], "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "", + "waypointRelativePos": 0.1955334987593047, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "To Feed Position" + } + } + } + ], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, @@ -42,13 +54,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 55.61965527615502 + "rotation": 56.33393573151792 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 59.349332043143804 + "rotation": 58.8775298032081 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Start to Reef Right.path b/src/main/deploy/pathplanner/paths/Right Start to Reef Right Right Side.path similarity index 63% rename from src/main/deploy/pathplanner/paths/Right Start to Reef Right.path rename to src/main/deploy/pathplanner/paths/Right Start to Reef Right Right Side.path index 6776b85..2a169e9 100644 --- a/src/main/deploy/pathplanner/paths/Right Start to Reef Right.path +++ b/src/main/deploy/pathplanner/paths/Right Start to Reef Right Right Side.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 6.856655223624603, - "y": 1.1146431630804878 + "x": 6.122999999999999, + "y": 1.8507499999999992 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.708, - "y": 2.895 + "x": 3.6270000000000007, + "y": 2.7965 }, "prevControl": { - "x": 5.0945304253536525, - "y": 2.068789937622381 + "x": 4.670249999999999, + "y": 1.9969999999999999 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,19 @@ "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "", + "waypointRelativePos": 0.1, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "To L3 Position" + } + } + } + ], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, @@ -42,13 +54,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 59.349332043143804 + "rotation": 58.8775298032081 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 58.861027563021295 + "rotation": 140.23980522440598 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index 9bb0c87..dda2888 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -382,10 +382,10 @@ 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{0_deg, -23.461_deg, 180_deg}}; + 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{0_deg, -23.461_deg, 0_deg}}; + 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{ From c7314d4b8449cb717387ec6d81362c7a704a3649 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Mon, 24 Feb 2025 19:59:01 -0600 Subject: [PATCH 22/27] New auto for L2 coral --- .../Center To Rear Center Reef (1 Coral).auto | 19 ----- .../autos/Far Right Auto (3 Coral).auto | 19 ----- ...3 Coral).auto => Left Auto (2 Coral).auto} | 22 +---- .../Center Start to Center Rear Reef.path | 66 --------------- ...ath => Far Left Source to Right Reef.path} | 18 ++-- ... Right Source to Reef Right Left Side.path | 66 --------------- .../paths/Left Reef to Far Left Source.path | 12 +-- ...=> Left Start to Reef Left Side Left.path} | 26 +++--- .../paths/Left Start to Reef Left.path | 82 ------------------- .../paths/Right Reef to Far Right Source.path | 66 --------------- 10 files changed, 30 insertions(+), 366 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/Center To Rear Center Reef (1 Coral).auto delete mode 100644 src/main/deploy/pathplanner/autos/Far Right Auto (3 Coral).auto rename src/main/deploy/pathplanner/autos/{Far Left Auto (3 Coral).auto => Left Auto (2 Coral).auto} (62%) delete mode 100644 src/main/deploy/pathplanner/paths/Center Start to Center Rear Reef.path rename src/main/deploy/pathplanner/paths/{Far Left Source to Left Reef.path => Far Left Source to Right Reef.path} (77%) delete mode 100644 src/main/deploy/pathplanner/paths/Far Right Source to Reef Right Left Side.path rename src/main/deploy/pathplanner/paths/{Right Start to Reef Right Right Side.path => Left Start to Reef Left Side Left.path} (69%) delete mode 100644 src/main/deploy/pathplanner/paths/Left Start to Reef Left.path delete mode 100644 src/main/deploy/pathplanner/paths/Right Reef to Far Right Source.path diff --git a/src/main/deploy/pathplanner/autos/Center To Rear Center Reef (1 Coral).auto b/src/main/deploy/pathplanner/autos/Center To Rear Center Reef (1 Coral).auto deleted file mode 100644 index 6bbf703..0000000 --- a/src/main/deploy/pathplanner/autos/Center To Rear Center Reef (1 Coral).auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Center Start to Center Rear Reef" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Far Right Auto (3 Coral).auto b/src/main/deploy/pathplanner/autos/Far Right Auto (3 Coral).auto deleted file mode 100644 index 105fde5..0000000 --- a/src/main/deploy/pathplanner/autos/Far Right Auto (3 Coral).auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Right Start to Reef Right Right Side" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Far Left Auto (3 Coral).auto b/src/main/deploy/pathplanner/autos/Left Auto (2 Coral).auto similarity index 62% rename from src/main/deploy/pathplanner/autos/Far Left Auto (3 Coral).auto rename to src/main/deploy/pathplanner/autos/Left Auto (2 Coral).auto index a318f52..021f895 100644 --- a/src/main/deploy/pathplanner/autos/Far Left Auto (3 Coral).auto +++ b/src/main/deploy/pathplanner/autos/Left Auto (2 Coral).auto @@ -13,7 +13,7 @@ { "type": "path", "data": { - "pathName": "Left Start to Reef Left" + "pathName": "Left Start to Reef Left Side Left" } }, { @@ -31,25 +31,7 @@ { "type": "path", "data": { - "pathName": "Far Left Source to Left Reef" - } - }, - { - "type": "named", - "data": { - "name": "Expel Coral" - } - }, - { - "type": "path", - "data": { - "pathName": "Left Reef to Far Left Source" - } - }, - { - "type": "path", - "data": { - "pathName": "Far Left Source to Left Reef" + "pathName": "Far Left Source to Right Reef" } }, { diff --git a/src/main/deploy/pathplanner/paths/Center Start to Center Rear Reef.path b/src/main/deploy/pathplanner/paths/Center Start to Center Rear Reef.path deleted file mode 100644 index f6db494..0000000 --- a/src/main/deploy/pathplanner/paths/Center Start to Center Rear Reef.path +++ /dev/null @@ -1,66 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.600204918032786, - "y": 3.8511782786885242 - }, - "prevControl": null, - "nextControl": { - "x": 6.16144680114967, - "y": 3.879668538428784 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.957889344262295, - "y": 3.8511782786885242 - }, - "prevControl": { - "x": 7.36815720140515, - "y": 3.846429902065147 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "", - "waypointRelativePos": 0.2970965563808233, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "To L3 Position" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 178.97696981133205 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": -178.81881108667335 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Far Left Source to Left Reef.path b/src/main/deploy/pathplanner/paths/Far Left Source to Right Reef.path similarity index 77% rename from src/main/deploy/pathplanner/paths/Far Left Source to Left Reef.path rename to src/main/deploy/pathplanner/paths/Far Left Source to Right Reef.path index f2a47c6..03528dd 100644 --- a/src/main/deploy/pathplanner/paths/Far Left Source to Left Reef.path +++ b/src/main/deploy/pathplanner/paths/Far Left Source to Right Reef.path @@ -8,24 +8,24 @@ }, "prevControl": null, "nextControl": { - "x": 2.526136363636364, - "y": 6.5558847402597396 + "x": 2.766396103896104, + "y": 7.34411525974026 }, "isLocked": false, "linkedName": "Far Left Source" }, { "anchor": { - "x": 3.5685000000000002, - "y": 5.204749999999999 + "x": 5.166700819672132, + "y": 5.457530737704917 }, "prevControl": { - "x": 2.960707792207792, - "y": 6.087948051948051 + "x": 7.036782786885246, + "y": 6.6563012295081965 }, "nextControl": null, "isLocked": false, - "linkedName": "Left Reef" + "linkedName": null } ], "rotationTargets": [], @@ -34,7 +34,7 @@ "eventMarkers": [ { "name": "", - "waypointRelativePos": 0.2214719783929785, + "waypointRelativePos": 0.2619851451721815, "endWaypointRelativePos": null, "command": { "type": "named", @@ -54,7 +54,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -58.24051991518723 + "rotation": -120.69972255081439 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/Far Right Source to Reef Right Left Side.path b/src/main/deploy/pathplanner/paths/Far Right Source to Reef Right Left Side.path deleted file mode 100644 index 4b52d8d..0000000 --- a/src/main/deploy/pathplanner/paths/Far Right Source to Reef Right Left Side.path +++ /dev/null @@ -1,66 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.6809253246720754, - "y": 0.7106331168816484 - }, - "prevControl": null, - "nextControl": { - "x": 5.5575, - "y": 1.9092499999999994 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.0992500000000005, - "y": 2.66 - }, - "prevControl": { - "x": 5.4502500000000005, - "y": 2.1237500000000002 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "", - "waypointRelativePos": 0.09627791563275767, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "To L3 Position" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 119.53878225955823 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 126.6624195429256 - }, - "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 index 8ab9262..5ef64d3 100644 --- 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 @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.5685000000000002, - "y": 5.204749999999999 + "x": 5.46639344262295, + "y": 5.205788934426229 }, "prevControl": null, "nextControl": { - "x": 2.3984999999999994, - "y": 6.56 + "x": 7.492315573770492, + "y": 6.63232581967213 }, "isLocked": false, "linkedName": "Left Reef" @@ -34,7 +34,7 @@ "eventMarkers": [ { "name": "", - "waypointRelativePos": 0.21538461538461356, + "waypointRelativePos": 0.237677245104656, "endWaypointRelativePos": null, "command": { "type": "named", @@ -60,7 +60,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -58.24051991518723 + "rotation": -120.19162296095763 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Start to Reef Right Right Side.path b/src/main/deploy/pathplanner/paths/Left Start to Reef Left Side Left.path similarity index 69% rename from src/main/deploy/pathplanner/paths/Right Start to Reef Right Right Side.path rename to src/main/deploy/pathplanner/paths/Left Start to Reef Left Side Left.path index 2a169e9..843a90a 100644 --- a/src/main/deploy/pathplanner/paths/Right Start to Reef Right Right Side.path +++ b/src/main/deploy/pathplanner/paths/Left Start to Reef Left Side Left.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 7.511931818181819, - "y": 0.806 + "x": 8.112, + "y": 6.1114999999999995 }, "prevControl": null, "nextControl": { - "x": 6.122999999999999, - "y": 1.8507499999999992 + "x": 8.204010853244533, + "y": 5.879047848181151 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.6270000000000007, - "y": 2.7965 + "x": 5.46639344262295, + "y": 5.205788934426229 }, "prevControl": { - "x": 4.670249999999999, - "y": 1.9969999999999999 + "x": 6.868954918032787, + "y": 5.577407786885246 }, "nextControl": null, "isLocked": false, - "linkedName": "Right Reef" + "linkedName": "Left Reef" } ], "rotationTargets": [], @@ -34,12 +34,12 @@ "eventMarkers": [ { "name": "", - "waypointRelativePos": 0.1, + "waypointRelativePos": 0.35, "endWaypointRelativePos": null, "command": { "type": "named", "data": { - "name": "To L3 Position" + "name": "To L2 Position" } } } @@ -54,13 +54,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 58.8775298032081 + "rotation": -120.19162296095763 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 140.23980522440598 + "rotation": -114.3536820505589 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Start to Reef Left.path b/src/main/deploy/pathplanner/paths/Left Start to Reef Left.path deleted file mode 100644 index 29ddb86..0000000 --- a/src/main/deploy/pathplanner/paths/Left Start to Reef Left.path +++ /dev/null @@ -1,82 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 8.112, - "y": 6.1114999999999995 - }, - "prevControl": null, - "nextControl": { - "x": 7.871619013089795, - "y": 6.042819718025655 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.2857500000000006, - "y": 6.1114999999999995 - }, - "prevControl": { - "x": 7.9072499999999994, - "y": 6.0432500000000005 - }, - "nextControl": { - "x": 2.906417371480787, - "y": 6.117101958649017 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.5685000000000002, - "y": 5.204749999999999 - }, - "prevControl": { - "x": 3.3832500000000003, - "y": 5.672749999999999 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "Left Reef" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "", - "waypointRelativePos": 0.704714640198519, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "To L3 Position" - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -58.24051991518723 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": -62.40270413166291 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Reef to Far Right Source.path b/src/main/deploy/pathplanner/paths/Right Reef to Far Right Source.path deleted file mode 100644 index 44fed7e..0000000 --- a/src/main/deploy/pathplanner/paths/Right Reef to Far Right Source.path +++ /dev/null @@ -1,66 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6270000000000007, - "y": 2.7965 - }, - "prevControl": null, - "nextControl": { - "x": 2.7205422077922075, - "y": 1.9084399350649353 - }, - "isLocked": false, - "linkedName": "Right Reef" - }, - { - "anchor": { - "x": 1.6809253246720754, - "y": 0.7106331168816484 - }, - "prevControl": { - "x": 2.7920454545454545, - "y": 2.0021915584415586 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "", - "waypointRelativePos": 0.1955334987593047, - "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": 56.33393573151792 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 58.8775298032081 - }, - "useDefaultConstraints": true -} \ No newline at end of file From 391462e90d7fe9e33a7581260e4872919688a70c Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Tue, 25 Feb 2025 17:12:36 -0600 Subject: [PATCH 23/27] New autos and named commands --- src/main/cpp/RobotContainer.cpp | 4 +- .../autos/Center To Left Source.auto | 55 ++++++++++++++++ .../Center To Right Source (2 Coral).auto | 55 ++++++++++++++++ .../autos/Left Auto (2 Coral).auto | 6 ++ .../autos/Right Auto (2 Coral).auto | 55 ++++++++++++++++ .../Center Start To Reef Left Side Left.path | 66 +++++++++++++++++++ ...Center Start To Reef Right Side Right.path | 66 +++++++++++++++++++ .../Center Start to Reef Bottom Center.path | 61 +++++++++++++++++ ... Right Source To Reef Right Side Left.path | 66 +++++++++++++++++++ .../paths/Left Reef to Far Left Source.path | 4 +- .../Left Start to Reef Left Side Left.path | 14 ++-- ... Right Side Right To Far Right Source.path | 66 +++++++++++++++++++ .../Right Start To Reef Right Side Right.path | 61 +++++++++++++++++ src/main/include/Constants.h | 4 +- 14 files changed, 571 insertions(+), 12 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Center To Left Source.auto create mode 100644 src/main/deploy/pathplanner/autos/Center To Right Source (2 Coral).auto create mode 100644 src/main/deploy/pathplanner/autos/Right Auto (2 Coral).auto create mode 100644 src/main/deploy/pathplanner/paths/Center Start To Reef Left Side Left.path create mode 100644 src/main/deploy/pathplanner/paths/Center Start To Reef Right Side Right.path create mode 100644 src/main/deploy/pathplanner/paths/Center Start to Reef Bottom Center.path create mode 100644 src/main/deploy/pathplanner/paths/Far Right Source To Reef Right Side Left.path create mode 100644 src/main/deploy/pathplanner/paths/Reef Right Side Right To Far Right Source.path create mode 100644 src/main/deploy/pathplanner/paths/Right Start To Reef Right Side Right.path diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 14a72ae..70a0b96 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -44,7 +44,7 @@ RobotContainer::RobotContainer() { 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.RemoveAlgaeFromL3())); + pathplanner::NamedCommands::registerCommand("Remove Algae", std::move(m_commandController.RemoveAlgaeFromL2())); pathplanner::NamedCommands::registerCommand("Home Elevator", std::move(m_commandController.HomeElevator())); pathplanner::NamedCommands::registerCommand("To Feed Position", std::move(m_commandController.FeedCoral())); pathplanner::NamedCommands::registerCommand("Expel Coral", std::move(m_commandController.ExpelCoral().WithTimeout(CommandConstants::kExpelCoralTimeout))); @@ -100,7 +100,7 @@ void RobotContainer::ConfigureBindings() { 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_commandController.RemoveAlgaeFromL3()); m_driverController.LeftBumper().OnTrue(m_commandController.HomeElevator()); m_driverController.RightBumper().OnTrue(frc2::InstantCommand( 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 index 021f895..6c246f0 100644 --- a/src/main/deploy/pathplanner/autos/Left Auto (2 Coral).auto +++ b/src/main/deploy/pathplanner/autos/Left Auto (2 Coral).auto @@ -28,6 +28,12 @@ "pathName": "Left Reef to Far Left Source" } }, + { + "type": "named", + "data": { + "name": "Pause" + } + }, { "type": "path", "data": { 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/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..5d77194 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Start to Reef Bottom Center.path @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.566, + "y": 4.103 + }, + "prevControl": null, + "nextControl": { + "x": 7.263749999999999, + "y": 4.0835 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.976749999999999, + "y": 4.103 + }, + "prevControl": { + "x": 6.571499999999998, + "y": 4.09325 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Remove Algae", + "waypointRelativePos": 0, + "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": -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/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 Reef to Far Left Source.path b/src/main/deploy/pathplanner/paths/Left Reef to Far Left Source.path index 5ef64d3..cc2de16 100644 --- 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 @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 7.492315573770492, - "y": 6.63232581967213 + "x": 6.50325, + "y": 6.95 }, "isLocked": false, "linkedName": "Left Reef" 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 index 843a90a..5cf9299 100644 --- 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 @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.112, - "y": 6.1114999999999995 + "x": 7.763595779220778, + "y": 7.424837662337662 }, "prevControl": null, "nextControl": { - "x": 8.204010853244533, - "y": 5.879047848181151 + "x": 6.695211038961039, + "y": 6.408685064935064 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 5.205788934426229 }, "prevControl": { - "x": 6.868954918032787, - "y": 5.577407786885246 + "x": 6.685714285714286, + "y": 6.418181818181818 }, "nextControl": null, "isLocked": false, @@ -60,7 +60,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -114.3536820505589 + "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 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/include/Constants.h b/src/main/include/Constants.h index dda2888..3a62629 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -161,6 +161,8 @@ constexpr double kPThetaController = 0.5; extern const frc::TrapezoidProfile::Constraints kThetaControllerConstraints; + +constexpr units::second_t kFeedWaitTime = 2_s; } // namespace AutoConstants namespace OIConstants { @@ -374,7 +376,7 @@ namespace CommandConstants { 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 = 3_s; + constexpr units::second_t kExpelCoralTimeout = 1_s; } namespace VisionConstants { From 248d5c2d3f401035ebf70e134fe78a821baa3b57 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Thu, 27 Feb 2025 11:10:42 -0600 Subject: [PATCH 24/27] New auto and constant changes --- src/main/cpp/RobotContainer.cpp | 13 ++-- src/main/cpp/commands/CommandController.cpp | 9 +++ .../autos/Center Auto (1 Coral).auto | 43 +++++++++++++ .../Center Reef Bottom To Slight Rear.path | 61 +++++++++++++++++++ .../Center Start to Reef Bottom Center.path | 4 +- .../Slight Rear To Center Reef Bottom.path | 54 ++++++++++++++++ src/main/include/Constants.h | 10 ++- src/main/include/commands/CommandController.h | 3 + 8 files changed, 186 insertions(+), 11 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Center Auto (1 Coral).auto create mode 100644 src/main/deploy/pathplanner/paths/Center Reef Bottom To Slight Rear.path create mode 100644 src/main/deploy/pathplanner/paths/Slight Rear To Center Reef Bottom.path diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 70a0b96..e3bffda 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -35,9 +35,10 @@ RobotContainer::RobotContainer() { m_chooser.SetDefaultOption(AutoConstants::kDefaultAutoName, AutoConstants::kDefaultAutoName); frc::SmartDashboard::PutData(&m_chooser); - m_chooser.SetDefaultOption(AutoConstants::kCenterToCenterAuto, AutoConstants::kCenterToCenterAuto); - m_chooser.AddOption(AutoConstants::kFarLeftAuto, AutoConstants::kFarLeftAuto); - m_chooser.AddOption(AutoConstants::kFarRightAuto, AutoConstants::kFarRightAuto); + m_chooser.SetDefaultOption(AutoConstants::kCenterAuto, AutoConstants::kCenterAuto); + // m_chooser.AddOption(AutoConstants::kFarLeftAuto, AutoConstants::kFarLeftAuto); + // m_chooser.AddOption(AutoConstants::kFarRightAuto, AutoConstants::kFarRightAuto); + m_chooser.AddOption(AutoConstants::kCenterAuto, AutoConstants::kCenterAuto); // pathplanner::NamedCommands::registerCommand("Test Command", frc2::cmd::Print("Test Command")); pathplanner::NamedCommands::registerCommand("To L1 Position", std::move(m_commandController.MoveToPositionL1())); @@ -100,7 +101,7 @@ void RobotContainer::ConfigureBindings() { 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.LeftBumper().OnTrue(m_commandController.HomeElevator()); m_driverController.RightBumper().OnTrue(frc2::InstantCommand( @@ -110,8 +111,8 @@ void RobotContainer::ConfigureBindings() { } ).ToPtr()); - // m_operatorController.LeftBumper().WhileTrue(m_commandController.ExpelAlgae()); - // m_operatorController.RightBumper().WhileTrue(m_commandController.ExpelCoral()); + m_driverController.POVUp().OnTrue(m_commandController.ClimbUp()); + m_driverController.POVDown().OnTrue(m_commandController.ClimbDown()); } frc2::CommandPtr RobotContainer::GetAutonomousCommand() { diff --git a/src/main/cpp/commands/CommandController.cpp b/src/main/cpp/commands/CommandController.cpp index 5f27e23..7047776 100644 --- a/src/main/cpp/commands/CommandController.cpp +++ b/src/main/cpp/commands/CommandController.cpp @@ -94,4 +94,13 @@ frc2::CommandPtr CommandController::HomeElevator() { m_subsystems.elevator->UpdatePidSettings(ElevatorConstants::kElevatorPidSettings); } ); +} + +frc2::CommandPtr CommandController::ClimbUp() { + return m_subsystems.algaeArm->MoveToPositionAbsolute(CommandConstants::kClimbAlgaeArmPosition) + .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/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..3bfa3d1 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center Auto (1 Coral).auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Home" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Start to Reef Bottom Center" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Reef Bottom To Slight Rear" + } + }, + { + "type": "path", + "data": { + "pathName": "Slight Rear To Center Reef Bottom" + } + }, + { + "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/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..c236905 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Reef Bottom To Slight Rear.path @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.976749999999999, + "y": 4.103 + }, + "prevControl": null, + "nextControl": { + "x": 6.976749999999999, + "y": 4.103 + }, + "isLocked": false, + "linkedName": "Reef Bottom Center" + }, + { + "anchor": { + "x": 7.000819672131148, + "y": 4.103 + }, + "prevControl": { + "x": 6.293545081967214, + "y": 4.126895491803278 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Slight Rear" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "To L2 Position", + "waypointRelativePos": 0.7481431465226057, + "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": -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 Reef Bottom Center.path b/src/main/deploy/pathplanner/paths/Center Start to Reef Bottom Center.path index 5d77194..341df98 100644 --- 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 @@ -25,7 +25,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Reef Bottom Center" } ], "rotationTargets": [], @@ -34,7 +34,7 @@ "eventMarkers": [ { "name": "Remove Algae", - "waypointRelativePos": 0, + "waypointRelativePos": 0.521269412559083, "endWaypointRelativePos": null, "command": null } diff --git a/src/main/deploy/pathplanner/paths/Slight Rear To Center Reef Bottom.path b/src/main/deploy/pathplanner/paths/Slight Rear To Center Reef Bottom.path new file mode 100644 index 0000000..70cfa2d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Slight Rear To Center Reef Bottom.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.000819672131148, + "y": 4.103 + }, + "prevControl": null, + "nextControl": { + "x": 6.473360655737705, + "y": 4.09093237704918 + }, + "isLocked": false, + "linkedName": "Slight Rear" + }, + { + "anchor": { + "x": 5.976749999999999, + "y": 4.103 + }, + "prevControl": { + "x": 6.521311475409835, + "y": 4.09093237704918 + }, + "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": -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 3a62629..76eb6ee 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -153,6 +153,7 @@ constexpr auto kMaxAngularAcceleration = 3.142_rad_per_s_sq; const std::string kCenterToCenterAuto = "Center To Rear Center Reef (1 Coral)"; const std::string kFarLeftAuto = "Far Left Auto (3 Coral)"; const std::string kFarRightAuto = "Far Right Auto (3 Coral)"; +const std::string kCenterAuto = "Center Auto (1 Coral)"; const std::string kDefaultAutoName = kCenterToCenterAuto; constexpr double kPXController = 0.5; @@ -343,13 +344,15 @@ namespace CommandConstants { 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 = 1.2_in; + 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 = 18_in; + constexpr units::meter_t kElevatorClimbDownPosition = 0.0_in; constexpr units::degree_t kCoralL1Position = 249_deg; - constexpr units::degree_t kCoralL2Position = 233_deg; - constexpr units::degree_t kCoralL3Position = 215_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 kCoralArmRemoveAlgaeFromL2Position = 204_deg; @@ -360,6 +363,7 @@ namespace CommandConstants { 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; diff --git a/src/main/include/commands/CommandController.h b/src/main/include/commands/CommandController.h index 7e16904..128230a 100644 --- a/src/main/include/commands/CommandController.h +++ b/src/main/include/commands/CommandController.h @@ -35,6 +35,9 @@ class CommandController { frc2::CommandPtr RemoveAlgaeFromL3(); frc2::CommandPtr HomeElevator(); + + frc2::CommandPtr ClimbUp(); + frc2::CommandPtr ClimbDown(); private: Subsystems_t m_subsystems; }; \ No newline at end of file From edca7a024dc90935238d62bd4510c5a5e91c0f04 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Thu, 27 Feb 2025 18:23:08 -0600 Subject: [PATCH 25/27] Day one changes --- src/main/cpp/RobotContainer.cpp | 12 +++-- src/main/cpp/commands/CommandController.cpp | 1 + src/main/cpp/subsystems/DriveSubsystem.cpp | 5 ++ .../autos/Center Auto (1 Coral).auto | 29 ++++++---- .../Bottom Left Reef To Slight Rear.path | 54 +++++++++++++++++++ .../Center Reef Bottom To Slight Rear.path | 25 ++++----- .../Center Start To Bottom Left Reef.path | 54 +++++++++++++++++++ .../Center Start to Reef Bottom Center.path | 27 ++++------ .../paths/Center To Rear Right Reef.path | 54 +++++++++++++++++++ ...ight Rear To Center Reef Bottom Left.path} | 18 +++---- src/main/include/Constants.h | 9 ++-- 11 files changed, 226 insertions(+), 62 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/Bottom Left Reef To Slight Rear.path create mode 100644 src/main/deploy/pathplanner/paths/Center Start To Bottom Left Reef.path create mode 100644 src/main/deploy/pathplanner/paths/Center To Rear Right Reef.path rename src/main/deploy/pathplanner/paths/{Slight Rear To Center Reef Bottom.path => Slight Rear To Center Reef Bottom Left.path} (75%) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index e3bffda..90892b8 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -35,7 +35,7 @@ RobotContainer::RobotContainer() { m_chooser.SetDefaultOption(AutoConstants::kDefaultAutoName, AutoConstants::kDefaultAutoName); frc::SmartDashboard::PutData(&m_chooser); - m_chooser.SetDefaultOption(AutoConstants::kCenterAuto, AutoConstants::kCenterAuto); + m_chooser.SetDefaultOption(AutoConstants::kDefaultAutoName, AutoConstants::kDefaultAutoName); // m_chooser.AddOption(AutoConstants::kFarLeftAuto, AutoConstants::kFarLeftAuto); // m_chooser.AddOption(AutoConstants::kFarRightAuto, AutoConstants::kFarRightAuto); m_chooser.AddOption(AutoConstants::kCenterAuto, AutoConstants::kCenterAuto); @@ -46,9 +46,10 @@ RobotContainer::RobotContainer() { 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("Home Elevator", std::move(m_commandController.HomeElevator())); 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(); @@ -101,8 +102,12 @@ void RobotContainer::ConfigureBindings() { 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]() { @@ -110,9 +115,6 @@ void RobotContainer::ConfigureBindings() { m_drive.ResetRotation(); } ).ToPtr()); - - m_driverController.POVUp().OnTrue(m_commandController.ClimbUp()); - m_driverController.POVDown().OnTrue(m_commandController.ClimbDown()); } frc2::CommandPtr RobotContainer::GetAutonomousCommand() { diff --git a/src/main/cpp/commands/CommandController.cpp b/src/main/cpp/commands/CommandController.cpp index 7047776..565a794 100644 --- a/src/main/cpp/commands/CommandController.cpp +++ b/src/main/cpp/commands/CommandController.cpp @@ -98,6 +98,7 @@ frc2::CommandPtr CommandController::HomeElevator() { frc2::CommandPtr CommandController::ClimbUp() { return m_subsystems.algaeArm->MoveToPositionAbsolute(CommandConstants::kClimbAlgaeArmPosition) + .AndThen(m_subsystems.algaeArm->MoveToPositionAbsolute(50_deg)) .AndThen(m_subsystems.elevator->MoveToPositionAbsolute(CommandConstants::kElevatorClimbUpPosition)); } diff --git a/src/main/cpp/subsystems/DriveSubsystem.cpp b/src/main/cpp/subsystems/DriveSubsystem.cpp index 70bd9f9..8ae8383 100644 --- a/src/main/cpp/subsystems/DriveSubsystem.cpp +++ b/src/main/cpp/subsystems/DriveSubsystem.cpp @@ -45,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( @@ -85,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; diff --git a/src/main/deploy/pathplanner/autos/Center Auto (1 Coral).auto b/src/main/deploy/pathplanner/autos/Center Auto (1 Coral).auto index 3bfa3d1..43c5367 100644 --- a/src/main/deploy/pathplanner/autos/Center Auto (1 Coral).auto +++ b/src/main/deploy/pathplanner/autos/Center Auto (1 Coral).auto @@ -11,27 +11,34 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "Center Start to Reef Bottom Center" + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Start To Bottom Left Reef" + } + }, + { + "type": "named", + "data": { + "name": "To L1 Position" + } + } + ] } }, { - "type": "path", + "type": "named", "data": { - "pathName": "Center Reef Bottom To Slight Rear" + "name": "Expel Coral" } }, { "type": "path", "data": { - "pathName": "Slight Rear To Center Reef Bottom" - } - }, - { - "type": "named", - "data": { - "name": "Expel Coral" + "pathName": "Bottom Left Reef To Slight Rear" } } ] 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 index c236905..5dd2699 100644 --- 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 @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.976749999999999, - "y": 4.103 + "x": 5.885963114754098, + "y": 4.00701844262295 }, "prevControl": null, "nextControl": { - "x": 6.976749999999999, - "y": 4.103 + "x": 6.885963114754098, + "y": 4.00701844262295 }, "isLocked": false, "linkedName": "Reef Bottom Center" }, { "anchor": { - "x": 7.000819672131148, - "y": 4.103 + "x": 6.880942622950819, + "y": 4.00701844262295 }, "prevControl": { - "x": 6.293545081967214, - "y": 4.126895491803278 + "x": 6.173668032786885, + "y": 4.030913934426229 }, "nextControl": null, "isLocked": false, @@ -31,14 +31,7 @@ "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "To L2 Position", - "waypointRelativePos": 0.7481431465226057, - "endWaypointRelativePos": null, - "command": null - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, 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 Bottom Center.path b/src/main/deploy/pathplanner/paths/Center Start to Reef Bottom Center.path index 341df98..9379b09 100644 --- 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 @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.566, - "y": 4.103 + "x": 7.67325, + "y": 4.015249999999999 }, "prevControl": null, "nextControl": { - "x": 7.263749999999999, - "y": 4.0835 + "x": 7.3709999999999996, + "y": 3.9957499999999992 }, "isLocked": false, - "linkedName": null + "linkedName": "Center Start" }, { "anchor": { - "x": 5.976749999999999, - "y": 4.103 + "x": 5.885963114754098, + "y": 4.00701844262295 }, "prevControl": { - "x": 6.571499999999998, - "y": 4.09325 + "x": 6.480713114754097, + "y": 3.997268442622951 }, "nextControl": null, "isLocked": false, @@ -31,14 +31,7 @@ "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Remove Algae", - "waypointRelativePos": 0.521269412559083, - "endWaypointRelativePos": null, - "command": null - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, 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/Slight Rear To Center Reef Bottom.path b/src/main/deploy/pathplanner/paths/Slight Rear To Center Reef Bottom Left.path similarity index 75% rename from src/main/deploy/pathplanner/paths/Slight Rear To Center Reef Bottom.path rename to src/main/deploy/pathplanner/paths/Slight Rear To Center Reef Bottom Left.path index 70cfa2d..1426307 100644 --- a/src/main/deploy/pathplanner/paths/Slight Rear To Center Reef Bottom.path +++ b/src/main/deploy/pathplanner/paths/Slight Rear To Center Reef Bottom Left.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 7.000819672131148, - "y": 4.103 + "x": 6.880942622950819, + "y": 4.00701844262295 }, "prevControl": null, "nextControl": { - "x": 6.473360655737705, - "y": 4.09093237704918 + "x": 6.353483606557376, + "y": 3.9949508196721304 }, "isLocked": false, "linkedName": "Slight Rear" }, { "anchor": { - "x": 5.976749999999999, - "y": 4.103 + "x": 5.909938524590164, + "y": 4.210809426229508 }, "prevControl": { - "x": 6.521311475409835, - "y": 4.09093237704918 + "x": 6.4544999999999995, + "y": 4.198741803278688 }, "nextControl": null, "isLocked": false, - "linkedName": "Reef Bottom Center" + "linkedName": null } ], "rotationTargets": [], diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index 76eb6ee..3e945fa 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -154,7 +154,7 @@ const std::string kCenterToCenterAuto = "Center To Rear Center Reef (1 Coral)"; const std::string kFarLeftAuto = "Far Left Auto (3 Coral)"; const std::string kFarRightAuto = "Far Right Auto (3 Coral)"; const std::string kCenterAuto = "Center Auto (1 Coral)"; -const std::string kDefaultAutoName = kCenterToCenterAuto; +const std::string kDefaultAutoName = kCenterAuto; constexpr double kPXController = 0.5; constexpr double kPYController = 0.5; @@ -347,7 +347,7 @@ namespace CommandConstants { 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 = 18_in; + constexpr units::meter_t kElevatorClimbUpPosition = 20.1_in; constexpr units::meter_t kElevatorClimbDownPosition = 0.0_in; constexpr units::degree_t kCoralL1Position = 249_deg; @@ -355,8 +355,8 @@ namespace CommandConstants { 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 kCoralArmRemoveAlgaeFromL2Position = 204_deg; - constexpr units::degree_t kCoralArmRemoveAlgaeFromL3Position = 210_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; @@ -381,6 +381,7 @@ namespace CommandConstants { 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 { From da9f837936b0b581b5941b0619b94108b6c8aa74 Mon Sep 17 00:00:00 2001 From: 5690 <5690Programmers@gmail.com> Date: Mon, 3 Mar 2025 15:14:32 -0600 Subject: [PATCH 26/27] Final NLR changes --- src/main/cpp/RobotContainer.cpp | 11 ++-- src/main/cpp/commands/CommandController.cpp | 2 +- .../autos/Center Auto (0 Coral).auto | 25 +++++++++ .../autos/Center Auto (1 Coral).auto | 6 --- .../autos/Left To Left Reef (1 Coral).auto | 44 +++++++++++++++ .../autos/Right To Right Reef (1 Coral).auto | 44 +++++++++++++++ .../Center Reef Bottom To Slight Rear.path | 4 +- .../pathplanner/paths/Center Straight.path | 54 +++++++++++++++++++ ...Left Border Start To Bottom Left Reef.path | 54 +++++++++++++++++++ ...ght Border Start To Bottom Right Reef.path | 54 +++++++++++++++++++ ...light Rear To Center Reef Bottom Left.path | 4 +- src/main/include/Constants.h | 10 ++-- 12 files changed, 290 insertions(+), 22 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Center Auto (0 Coral).auto create mode 100644 src/main/deploy/pathplanner/autos/Left To Left Reef (1 Coral).auto create mode 100644 src/main/deploy/pathplanner/autos/Right To Right Reef (1 Coral).auto create mode 100644 src/main/deploy/pathplanner/paths/Center Straight.path create mode 100644 src/main/deploy/pathplanner/paths/Left Border Start To Bottom Left Reef.path create mode 100644 src/main/deploy/pathplanner/paths/Right Border Start To Bottom Right Reef.path diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 90892b8..c048a45 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -32,13 +32,12 @@ 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::kDefaultAutoName, AutoConstants::kDefaultAutoName); - // m_chooser.AddOption(AutoConstants::kFarLeftAuto, AutoConstants::kFarLeftAuto); - // m_chooser.AddOption(AutoConstants::kFarRightAuto, AutoConstants::kFarRightAuto); - m_chooser.AddOption(AutoConstants::kCenterAuto, AutoConstants::kCenterAuto); + 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())); diff --git a/src/main/cpp/commands/CommandController.cpp b/src/main/cpp/commands/CommandController.cpp index 565a794..235decc 100644 --- a/src/main/cpp/commands/CommandController.cpp +++ b/src/main/cpp/commands/CommandController.cpp @@ -98,7 +98,7 @@ frc2::CommandPtr CommandController::HomeElevator() { frc2::CommandPtr CommandController::ClimbUp() { return m_subsystems.algaeArm->MoveToPositionAbsolute(CommandConstants::kClimbAlgaeArmPosition) - .AndThen(m_subsystems.algaeArm->MoveToPositionAbsolute(50_deg)) + .AndThen(m_subsystems.coralArm->MoveToPositionAbsolute(CommandConstants::kCoralClimbPosition)) .AndThen(m_subsystems.elevator->MoveToPositionAbsolute(CommandConstants::kElevatorClimbUpPosition)); } diff --git a/src/main/deploy/pathplanner/autos/Center Auto (0 Coral).auto b/src/main/deploy/pathplanner/autos/Center Auto (0 Coral).auto new file mode 100644 index 0000000..262d9c8 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center Auto (0 Coral).auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Home" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Straight" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Center Auto (1 Coral).auto b/src/main/deploy/pathplanner/autos/Center Auto (1 Coral).auto index 43c5367..97bbf04 100644 --- a/src/main/deploy/pathplanner/autos/Center Auto (1 Coral).auto +++ b/src/main/deploy/pathplanner/autos/Center Auto (1 Coral).auto @@ -34,12 +34,6 @@ "data": { "name": "Expel Coral" } - }, - { - "type": "path", - "data": { - "pathName": "Bottom Left Reef To Slight Rear" - } } ] } 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 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/paths/Center Reef Bottom To Slight Rear.path b/src/main/deploy/pathplanner/paths/Center Reef Bottom To Slight Rear.path index 5dd2699..3cf67dc 100644 --- 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 @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 6.880942622950819, + "x": 6.571278409090909, "y": 4.00701844262295 }, "prevControl": { - "x": 6.173668032786885, + "x": 5.864003818926975, "y": 4.030913934426229 }, "nextControl": null, 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/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/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/Slight Rear To Center Reef Bottom Left.path b/src/main/deploy/pathplanner/paths/Slight Rear To Center Reef Bottom Left.path index 1426307..0cb0e60 100644 --- 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 @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 6.880942622950819, + "x": 6.571278409090909, "y": 4.00701844262295 }, "prevControl": null, "nextControl": { - "x": 6.353483606557376, + "x": 6.043819392697466, "y": 3.9949508196721304 }, "isLocked": false, diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index 3e945fa..c2b9279 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -150,11 +150,10 @@ constexpr auto kMaxAcceleration = 3_mps_sq; constexpr auto kMaxAngularSpeed = 3.142_rad_per_s; constexpr auto kMaxAngularAcceleration = 3.142_rad_per_s_sq; -const std::string kCenterToCenterAuto = "Center To Rear Center Reef (1 Coral)"; -const std::string kFarLeftAuto = "Far Left Auto (3 Coral)"; -const std::string kFarRightAuto = "Far Right Auto (3 Coral)"; -const std::string kCenterAuto = "Center Auto (1 Coral)"; -const std::string kDefaultAutoName = kCenterAuto; +const std::string kCenterToCenterAuto = "Center Auto (1 Coral)"; +const std::string kRightToRightReefAuto = "Right To Right Reef (1 Coral)"; +const std::string kLeftToLeftReefAuto = "Left To Left Reef (1 Coral)"; +const std::string kForwardAuto = "Center Auto (0 Coral)"; constexpr double kPXController = 0.5; constexpr double kPYController = 0.5; @@ -355,6 +354,7 @@ namespace CommandConstants { 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; From 6f91a3a6b01d4db12f6a4bccf769b9b5e0dd1ef2 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Tue, 4 Mar 2025 17:21:27 -0600 Subject: [PATCH 27/27] Remove climber comments --- src/main/cpp/RobotContainer.cpp | 18 ------------------ src/main/include/RobotContainer.h | 2 -- 2 files changed, 20 deletions(-) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index c048a45..66e1448 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -68,23 +68,6 @@ RobotContainer::RobotContainer() { true); }, {&m_drive})); - - // m_climber.SetDefaultCommand( - // frc2::RunCommand( - // [this]() { - // m_climber.RunMotorPercentage((-m_driverController.GetLeftTriggerAxis() - // + m_driverController.GetRightTriggerAxis()) * ClimberConstants::kPercentageScalar); - - // std::cout << "Controller input: " << (-m_driverController.GetLeftTriggerAxis() - // + m_driverController.GetRightTriggerAxis()) * ClimberConstants::kPercentageScalar - // << ", Current relative position: " << m_climber.GetCurrentPosition().value() << std::endl; - // }, - // {&m_climber} - // ) - // ); - - // Has to be raised before match so coral arm is within frame perimeter - // m_elevator.SetEncoderPosition(ElevatorConstants::kElevatorStartPosition); } void RobotContainer::ConfigureBindings() { @@ -140,5 +123,4 @@ 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/include/RobotContainer.h b/src/main/include/RobotContainer.h index a176cd6..0f07fbf 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -75,8 +75,6 @@ class RobotContainer { CoralArmSubsystem m_coralArm; IntakeSubsystem m_coralIntake{CoralArmConstants::kIntakeMotorId, CoralArmConstants::kHasCoralCurrent, true}; - // ClimberSubsystem m_climber; - Subsystems_t subsystems = { .algaeArm = &m_algaeArm, .coralArm = &m_coralArm,