From 2317b8341debb7945b408fe7d0c96635d68e5600 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Tue, 7 Jan 2025 18:52:28 -0600 Subject: [PATCH 01/42] added MAXswerve example subsystem --- src/main/cpp/subsystems/DriveSubsystem.cpp | 123 ++++++++++++++++++ src/main/cpp/subsystems/MAXSwerveModule.cpp | 67 ++++++++++ src/main/include/Constants.h | 91 ++++++++++++- src/main/include/RobotContainer.h | 3 + src/main/include/subsystems/Configs.h | 68 ++++++++++ src/main/include/subsystems/DriveSubsystem.h | 118 +++++++++++++++++ src/main/include/subsystems/MAXSwerveModule.h | 70 ++++++++++ 7 files changed, 537 insertions(+), 3 deletions(-) create mode 100644 src/main/cpp/subsystems/DriveSubsystem.cpp create mode 100644 src/main/cpp/subsystems/MAXSwerveModule.cpp create mode 100644 src/main/include/subsystems/Configs.h create mode 100644 src/main/include/subsystems/DriveSubsystem.h create mode 100644 src/main/include/subsystems/MAXSwerveModule.h diff --git a/src/main/cpp/subsystems/DriveSubsystem.cpp b/src/main/cpp/subsystems/DriveSubsystem.cpp new file mode 100644 index 0000000..e90df82 --- /dev/null +++ b/src/main/cpp/subsystems/DriveSubsystem.cpp @@ -0,0 +1,123 @@ +// 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. + +#include "subsystems/DriveSubsystem.h" + +#include +#include +#include +#include +#include + +#include "Constants.h" + +using namespace DriveConstants; + +DriveSubsystem::DriveSubsystem() + : m_frontLeft{kFrontLeftDrivingCanId, kFrontLeftTurningCanId, + kFrontLeftChassisAngularOffset}, + m_rearLeft{kRearLeftDrivingCanId, kRearLeftTurningCanId, + kRearLeftChassisAngularOffset}, + m_frontRight{kFrontRightDrivingCanId, kFrontRightTurningCanId, + kFrontRightChassisAngularOffset}, + m_rearRight{kRearRightDrivingCanId, kRearRightTurningCanId, + kRearRightChassisAngularOffset}, + m_odometry{kDriveKinematics, + frc::Rotation2d(units::radian_t{ + m_gyro.GetAngle(frc::ADIS16470_IMU::IMUAxis::kZ)}), + {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), + m_rearLeft.GetPosition(), m_rearRight.GetPosition()}, + frc::Pose2d{}} { + // Usage reporting for MAXSwerve template + HAL_Report(HALUsageReporting::kResourceType_RobotDrive, + HALUsageReporting::kRobotDriveSwerve_MaxSwerve); +} + +void DriveSubsystem::Periodic() { + // Implementation of subsystem periodic method goes here. + m_odometry.Update(frc::Rotation2d(units::radian_t{ + m_gyro.GetAngle(frc::ADIS16470_IMU::IMUAxis::kZ)}), + {m_frontLeft.GetPosition(), m_rearLeft.GetPosition(), + m_frontRight.GetPosition(), m_rearRight.GetPosition()}); +} + +void DriveSubsystem::Drive(units::meters_per_second_t xSpeed, + units::meters_per_second_t ySpeed, + units::radians_per_second_t rot, + bool fieldRelative) { + // Convert the commanded speeds into the correct units for the drivetrain + units::meters_per_second_t xSpeedDelivered = + xSpeed.value() * DriveConstants::kMaxSpeed; + units::meters_per_second_t ySpeedDelivered = + ySpeed.value() * DriveConstants::kMaxSpeed; + units::radians_per_second_t rotDelivered = + rot.value() * DriveConstants::kMaxAngularSpeed; + + auto states = kDriveKinematics.ToSwerveModuleStates( + fieldRelative + ? frc::ChassisSpeeds::FromFieldRelativeSpeeds( + xSpeedDelivered, ySpeedDelivered, rotDelivered, + frc::Rotation2d(units::radian_t{ + m_gyro.GetAngle(frc::ADIS16470_IMU::IMUAxis::kZ)})) + : frc::ChassisSpeeds{xSpeedDelivered, ySpeedDelivered, rotDelivered}); + + kDriveKinematics.DesaturateWheelSpeeds(&states, DriveConstants::kMaxSpeed); + + auto [fl, fr, bl, br] = states; + + m_frontLeft.SetDesiredState(fl); + m_frontRight.SetDesiredState(fr); + m_rearLeft.SetDesiredState(bl); + m_rearRight.SetDesiredState(br); +} + +void DriveSubsystem::SetX() { + m_frontLeft.SetDesiredState( + frc::SwerveModuleState{0_mps, frc::Rotation2d{45_deg}}); + m_frontRight.SetDesiredState( + frc::SwerveModuleState{0_mps, frc::Rotation2d{-45_deg}}); + m_rearLeft.SetDesiredState( + frc::SwerveModuleState{0_mps, frc::Rotation2d{-45_deg}}); + m_rearRight.SetDesiredState( + frc::SwerveModuleState{0_mps, frc::Rotation2d{45_deg}}); +} + +void DriveSubsystem::SetModuleStates( + wpi::array desiredStates) { + kDriveKinematics.DesaturateWheelSpeeds(&desiredStates, + DriveConstants::kMaxSpeed); + m_frontLeft.SetDesiredState(desiredStates[0]); + m_frontRight.SetDesiredState(desiredStates[1]); + m_rearLeft.SetDesiredState(desiredStates[2]); + m_rearRight.SetDesiredState(desiredStates[3]); +} + +void DriveSubsystem::ResetEncoders() { + m_frontLeft.ResetEncoders(); + m_rearLeft.ResetEncoders(); + m_frontRight.ResetEncoders(); + m_rearRight.ResetEncoders(); +} + +units::degree_t DriveSubsystem::GetHeading() const { + return frc::Rotation2d( + units::radian_t{m_gyro.GetAngle(frc::ADIS16470_IMU::IMUAxis::kZ)}) + .Degrees(); +} + +void DriveSubsystem::ZeroHeading() { m_gyro.Reset(); } + +double DriveSubsystem::GetTurnRate() { + return -m_gyro.GetRate(frc::ADIS16470_IMU::IMUAxis::kZ).value(); +} + +frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); } + +void DriveSubsystem::ResetOdometry(frc::Pose2d pose) { + m_odometry.ResetPosition( + GetHeading(), + {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), + m_rearLeft.GetPosition(), m_rearRight.GetPosition()}, + pose); +} \ No newline at end of file diff --git a/src/main/cpp/subsystems/MAXSwerveModule.cpp b/src/main/cpp/subsystems/MAXSwerveModule.cpp new file mode 100644 index 0000000..0594adc --- /dev/null +++ b/src/main/cpp/subsystems/MAXSwerveModule.cpp @@ -0,0 +1,67 @@ +// 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. + +#include "subsystems/MAXSwerveModule.h" + +#include + +#include "Subsystems/Configs.h" + +using namespace rev::spark; + +MAXSwerveModule::MAXSwerveModule(const int drivingCANId, const int turningCANId, + const double chassisAngularOffset) + : m_drivingSpark(drivingCANId, SparkMax::MotorType::kBrushless), + m_turningSpark(turningCANId, SparkMax::MotorType::kBrushless) { + // Apply the respective configurations to the SPARKS. Reset parameters before + // applying the configuration to bring the SPARK to a known good state. + // Persist the settings to the SPARK to avoid losing them on a power cycle. + m_drivingSpark.Configure(Configs::MAXSwerveModule::DrivingConfig(), + SparkBase::ResetMode::kResetSafeParameters, + SparkBase::PersistMode::kPersistParameters); + m_turningSpark.Configure(Configs::MAXSwerveModule::TurningConfig(), + SparkBase::ResetMode::kResetSafeParameters, + SparkBase::PersistMode::kPersistParameters); + + m_chassisAngularOffset = chassisAngularOffset; + m_desiredState.angle = + frc::Rotation2d(units::radian_t{m_turningAbsoluteEncoder.GetPosition()}); + m_drivingEncoder.SetPosition(0); +} + +frc::SwerveModuleState MAXSwerveModule::GetState() const { + return {units::meters_per_second_t{m_drivingEncoder.GetVelocity()}, + units::radian_t{m_turningAbsoluteEncoder.GetPosition() - + m_chassisAngularOffset}}; +} + +frc::SwerveModulePosition MAXSwerveModule::GetPosition() const { + return {units::meter_t{m_drivingEncoder.GetPosition()}, + units::radian_t{m_turningAbsoluteEncoder.GetPosition() - + m_chassisAngularOffset}}; +} + +void MAXSwerveModule::SetDesiredState( + const frc::SwerveModuleState& desiredState) { + // Apply chassis angular offset to the desired state. + frc::SwerveModuleState correctedDesiredState{}; + correctedDesiredState.speed = desiredState.speed; + correctedDesiredState.angle = + desiredState.angle + + frc::Rotation2d(units::radian_t{m_chassisAngularOffset}); + + // Optimize the reference state to avoid spinning further than 90 degrees. + correctedDesiredState.Optimize( + frc::Rotation2d(units::radian_t{m_turningAbsoluteEncoder.GetPosition()})); + + m_drivingClosedLoopController.SetReference( + (double)correctedDesiredState.speed, SparkMax::ControlType::kVelocity); + m_turningClosedLoopController.SetReference( + correctedDesiredState.angle.Radians().value(), + SparkMax::ControlType::kPosition); + + m_desiredState = desiredState; +} + +void MAXSwerveModule::ResetEncoders() { m_drivingEncoder.SetPosition(0); } \ No newline at end of file diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index 87dd5b9..c18eb84 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -2,20 +2,105 @@ // 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 +#include +#include +#include +#include +#include +#include +#include + +#include + #pragma once /** * The Constants header provides a convenient place for teams to hold robot-wide - * numerical or boolean constants. This should not be used for any other - * purpose. + * numerical or bool constants. This should not be used for any other purpose. * * It is generally a good idea to place constants into subsystem- or * command-specific namespaces within this header, which can then be used where * they are needed. */ - namespace OperatorConstants { inline constexpr int kDriverControllerPort = 0; } // namespace OperatorConstants + + + +namespace DriveConstants { +// Driving Parameters - Note that these are not the maximum capable speeds of +// the robot, rather the allowed maximum speeds +constexpr units::meters_per_second_t kMaxSpeed = 4.8_mps; +constexpr units::radians_per_second_t kMaxAngularSpeed{2 * std::numbers::pi}; + +constexpr double kDirectionSlewRate = 1.2; // radians per second +constexpr double kMagnitudeSlewRate = 1.8; // percent per second (1 = 100%) +constexpr double kRotationalSlewRate = 2.0; // percent per second (1 = 100%) + +// Chassis configuration +constexpr units::meter_t kTrackWidth = + 0.6731_m; // Distance between centers of right and left wheels on robot +constexpr units::meter_t kWheelBase = + 0.6731_m; // Distance between centers of front and back wheels on robot + +// Angular offsets of the modules relative to the chassis in radians +constexpr double kFrontLeftChassisAngularOffset = -std::numbers::pi / 2; +constexpr double kFrontRightChassisAngularOffset = 0; +constexpr double kRearLeftChassisAngularOffset = std::numbers::pi; +constexpr double kRearRightChassisAngularOffset = std::numbers::pi / 2; + +// SPARK MAX CAN IDs +constexpr int kFrontLeftDrivingCanId = 11; +constexpr int kRearLeftDrivingCanId = 13; +constexpr int kFrontRightDrivingCanId = 15; +constexpr int kRearRightDrivingCanId = 17; + +constexpr int kFrontLeftTurningCanId = 10; +constexpr int kRearLeftTurningCanId = 12; +constexpr int kFrontRightTurningCanId = 14; +constexpr int kRearRightTurningCanId = 16; +} // namespace DriveConstants + +namespace ModuleConstants { +// The MAXSwerve module can be configured with one of three pinion gears: 12T, +// 13T, or 14T. This changes the drive speed of the module (a pinion gear with +// more teeth will result in a robot that drives faster). +constexpr int kDrivingMotorPinionTeeth = 14; + +// Calculations required for driving motor conversion factors and feed forward +constexpr double kDrivingMotorFreeSpeedRps = + 5676.0 / 60; // NEO free speed is 5676 RPM +constexpr units::meter_t kWheelDiameter = 0.0762_m; +constexpr units::meter_t kWheelCircumference = + kWheelDiameter * std::numbers::pi; +// 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 +// teeth on the bevel pinion +constexpr double kDrivingMotorReduction = + (45.0 * 22) / (kDrivingMotorPinionTeeth * 15); +constexpr double kDriveWheelFreeSpeedRps = + (kDrivingMotorFreeSpeedRps * kWheelCircumference.value()) / + kDrivingMotorReduction; +} // namespace ModuleConstants + +namespace AutoConstants { +constexpr auto kMaxSpeed = 3_mps; +constexpr auto kMaxAcceleration = 3_mps_sq; +constexpr auto kMaxAngularSpeed = 3.142_rad_per_s; +constexpr auto kMaxAngularAcceleration = 3.142_rad_per_s_sq; + +constexpr double kPXController = 0.5; +constexpr double kPYController = 0.5; +constexpr double kPThetaController = 0.5; + +extern const frc::TrapezoidProfile::Constraints + kThetaControllerConstraints; +} // namespace AutoConstants + +namespace OIConstants { +constexpr int kDriverControllerPort = 0; +constexpr double kDriveDeadband = 0.05; +} // namespace OIConstants \ No newline at end of file diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index 697b9a2..06e4b0e 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -8,6 +8,7 @@ #include #include "Constants.h" +#include "subsystems/DriveSubsystem.h" #include "subsystems/ExampleSubsystem.h" /** @@ -29,6 +30,8 @@ class RobotContainer { OperatorConstants::kDriverControllerPort}; // The robot's subsystems are defined here... + DriveSubsystem m_drive; + ExampleSubsystem m_subsystem; void ConfigureBindings(); diff --git a/src/main/include/subsystems/Configs.h b/src/main/include/subsystems/Configs.h new file mode 100644 index 0000000..5dd868e --- /dev/null +++ b/src/main/include/subsystems/Configs.h @@ -0,0 +1,68 @@ +#pragma once + +#include + +#include "Constants.h" + +using namespace rev::spark; + +namespace Configs { +class MAXSwerveModule { + public: + static SparkMaxConfig& DrivingConfig() { + static SparkMaxConfig drivingConfig{}; + + // Use module constants to calculate conversion factors and feed forward + // gain. + double drivingFactor = ModuleConstants::kWheelDiameter.value() * + std::numbers::pi / + ModuleConstants::kDrivingMotorReduction; + double drivingVelocityFeedForward = + 1 / ModuleConstants::kDriveWheelFreeSpeedRps; + + drivingConfig.SetIdleMode(SparkBaseConfig::IdleMode::kBrake) + .SmartCurrentLimit(50); + drivingConfig.encoder + .PositionConversionFactor(drivingFactor) // meters + .VelocityConversionFactor(drivingFactor / 60.0); // meters per second + drivingConfig.closedLoop + .SetFeedbackSensor(ClosedLoopConfig::FeedbackSensor::kPrimaryEncoder) + // These are example gains you may need to them for your own robot! + .Pid(0.04, 0, 0) + .VelocityFF(drivingVelocityFeedForward) + .OutputRange(-1, 1); + + return drivingConfig; + } + + static SparkMaxConfig& TurningConfig() { + static SparkMaxConfig turningConfig{}; + + // Use module constants to calculate conversion factor + double turningFactor = 2 * std::numbers::pi; + + turningConfig.SetIdleMode(SparkBaseConfig::IdleMode::kBrake) + .SmartCurrentLimit(20); + turningConfig + .absoluteEncoder + // Invert the turning encoder, since the output shaft rotates in the + // opposite direction of the steering motor in the MAXSwerve Module. + .Inverted(true) + .PositionConversionFactor(turningFactor) // radians + .VelocityConversionFactor(turningFactor / 60.0); // radians per second + turningConfig.closedLoop + .SetFeedbackSensor(ClosedLoopConfig::FeedbackSensor::kAbsoluteEncoder) + // These are example gains you may need to them for your own robot! + .Pid(1, 0, 0) + .OutputRange(-1, 1) + // Enable PID wrap around for the turning motor. This will allow the + // PID controller to go through 0 to get to the setpoint i.e. going + // from 350 degrees to 10 degrees will go through 0 rather than the + // other direction which is a longer route. + .PositionWrappingEnabled(true) + .PositionWrappingInputRange(0, turningFactor); + + return turningConfig; + } +}; +} // namespace Configs \ No newline at end of file diff --git a/src/main/include/subsystems/DriveSubsystem.h b/src/main/include/subsystems/DriveSubsystem.h new file mode 100644 index 0000000..271cee4 --- /dev/null +++ b/src/main/include/subsystems/DriveSubsystem.h @@ -0,0 +1,118 @@ +// 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. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Constants.h" +#include "MAXSwerveModule.h" + +class DriveSubsystem : public frc2::SubsystemBase { + public: + DriveSubsystem(); + + /** + * Will be called periodically whenever the CommandScheduler runs. + */ + void Periodic() override; + + // Subsystem methods go here. + + /** + * Drives the robot at given x, y and theta speeds. Speeds range from [-1, 1] + * and the linear speeds have no effect on the angular speed. + * + * @param xSpeed Speed of the robot in the x direction + * (forward/backwards). + * @param ySpeed Speed of the robot in the y direction (sideways). + * @param rot Angular rate of the robot. + * @param fieldRelative Whether the provided x and y speeds are relative to + * the field. + */ + void Drive(units::meters_per_second_t xSpeed, + units::meters_per_second_t ySpeed, units::radians_per_second_t rot, + bool fieldRelative); + + /** + * Sets the wheels into an X formation to prevent movement. + */ + void SetX(); + + /** + * Resets the drive encoders to currently read a position of 0. + */ + void ResetEncoders(); + + /** + * Sets the drive MotorControllers to a power from -1 to 1. + */ + void SetModuleStates(wpi::array desiredStates); + + /** + * Returns the heading of the robot. + * + * @return the robot's heading in degrees, from 180 to 180 + */ + units::degree_t GetHeading() const; + + /** + * Zeroes the heading of the robot. + */ + void ZeroHeading(); + + /** + * Returns the turn rate of the robot. + * + * @return The turn rate of the robot, in degrees per second + */ + double GetTurnRate(); + + /** + * Returns the currently-estimated pose of the robot. + * + * @return The pose. + */ + frc::Pose2d GetPose(); + + /** + * Resets the odometry to the specified pose. + * + * @param pose The pose to which to set the odometry. + */ + void ResetOdometry(frc::Pose2d pose); + + frc::SwerveDriveKinematics<4> kDriveKinematics{ + frc::Translation2d{DriveConstants::kWheelBase / 2, + DriveConstants::kTrackWidth / 2}, + frc::Translation2d{DriveConstants::kWheelBase / 2, + -DriveConstants::kTrackWidth / 2}, + frc::Translation2d{-DriveConstants::kWheelBase / 2, + DriveConstants::kTrackWidth / 2}, + frc::Translation2d{-DriveConstants::kWheelBase / 2, + -DriveConstants::kTrackWidth / 2}}; + + private: + // Components (e.g. motor controllers and sensors) should generally be + // declared private and exposed only through public methods. + + MAXSwerveModule m_frontLeft; + MAXSwerveModule m_rearLeft; + MAXSwerveModule m_frontRight; + MAXSwerveModule m_rearRight; + + // The gyro sensor + frc::ADIS16470_IMU m_gyro; + + // Odometry class for tracking robot pose + // 4 defines the number of modules + frc::SwerveDriveOdometry<4> m_odometry; +}; \ No newline at end of file diff --git a/src/main/include/subsystems/MAXSwerveModule.h b/src/main/include/subsystems/MAXSwerveModule.h new file mode 100644 index 0000000..de517f5 --- /dev/null +++ b/src/main/include/subsystems/MAXSwerveModule.h @@ -0,0 +1,70 @@ +// 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. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +using namespace rev::spark; + +class MAXSwerveModule { + public: + /** + * Constructs a MAXSwerveModule and configures the driving and turning motor, + * encoder, and PID controller. This configuration is specific to the REV + * MAXSwerve Module built with NEOs, SPARKS MAX, and a Through Bore + * Encoder. + */ + MAXSwerveModule(int driveCANId, int turningCANId, + double chassisAngularOffset); + + /** + * Returns the current state of the module. + * + * @return The current state of the module. + */ + frc::SwerveModuleState GetState() const; + + /** + * Returns the current position of the module. + * + * @return The current position of the module. + */ + frc::SwerveModulePosition GetPosition() const; + + /** + * Sets the desired state for the module. + * + * @param desiredState Desired state with speed and angle. + */ + void SetDesiredState(const frc::SwerveModuleState& state); + + /** + * Zeroes all the SwerveModule encoders. + */ + void ResetEncoders(); + + private: + SparkMax m_drivingSpark; + SparkMax m_turningSpark; + + SparkRelativeEncoder m_drivingEncoder = m_drivingSpark.GetEncoder(); + SparkAbsoluteEncoder m_turningAbsoluteEncoder = + m_turningSpark.GetAbsoluteEncoder(); + + SparkClosedLoopController m_drivingClosedLoopController = + m_drivingSpark.GetClosedLoopController(); + SparkClosedLoopController m_turningClosedLoopController = + m_turningSpark.GetClosedLoopController(); + + double m_chassisAngularOffset = 0; + frc::SwerveModuleState m_desiredState{units::meters_per_second_t{0.0}, + frc::Rotation2d()}; +}; \ No newline at end of file From 31d27c5976f0c63cd2fad073c26784ea6144b0af Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Tue, 7 Jan 2025 19:03:02 -0600 Subject: [PATCH 02/42] fixed odometry inversion?(maybe) --- src/main/cpp/subsystems/DriveSubsystem.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/cpp/subsystems/DriveSubsystem.cpp b/src/main/cpp/subsystems/DriveSubsystem.cpp index e90df82..aa0b38e 100644 --- a/src/main/cpp/subsystems/DriveSubsystem.cpp +++ b/src/main/cpp/subsystems/DriveSubsystem.cpp @@ -38,8 +38,8 @@ void DriveSubsystem::Periodic() { // Implementation of subsystem periodic method goes here. m_odometry.Update(frc::Rotation2d(units::radian_t{ m_gyro.GetAngle(frc::ADIS16470_IMU::IMUAxis::kZ)}), - {m_frontLeft.GetPosition(), m_rearLeft.GetPosition(), - m_frontRight.GetPosition(), m_rearRight.GetPosition()}); + {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), + m_rearLeft.GetPosition(), m_rearRight.GetPosition()}); } void DriveSubsystem::Drive(units::meters_per_second_t xSpeed, From 6598acf438798fe48ffdb57d67b534fd7b614abf Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Fri, 10 Jan 2025 15:35:19 -0600 Subject: [PATCH 03/42] Fix CAN IDs and implement driver controller --- src/main/cpp/RobotContainer.cpp | 43 ++++++++++++++------ src/main/cpp/subsystems/DriveSubsystem.cpp | 4 +- src/main/include/Constants.h | 18 ++++---- src/main/include/RobotContainer.h | 13 ++++-- src/main/include/subsystems/DriveSubsystem.h | 9 ++++ 5 files changed, 61 insertions(+), 26 deletions(-) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 4300501..683c320 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -4,7 +4,17 @@ #include "RobotContainer.h" -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include "commands/Autos.h" #include "commands/ExampleCommand.h" @@ -13,20 +23,29 @@ RobotContainer::RobotContainer() { // Initialize all of your commands and subsystems here // Configure the button bindings - ConfigureBindings(); + ConfigureBindings(); + + // Set up default drive command + // The left stick controls translation of the robot. + // Turning is controlled by the X axis of the right stick. + m_drive.SetDefaultCommand(frc2::RunCommand( + [this] { + m_drive.Drive( + -units::meters_per_second_t{frc::ApplyDeadband( + m_driverController.GetLeftY(), OIConstants::kDriveDeadband)}, + -units::meters_per_second_t{frc::ApplyDeadband( + m_driverController.GetLeftX(), OIConstants::kDriveDeadband)}, + -units::radians_per_second_t{frc::ApplyDeadband( + m_driverController.GetRightX(), OIConstants::kDriveDeadband)}, + true); + }, + {&m_drive})); } void RobotContainer::ConfigureBindings() { - // Configure your trigger bindings here - - // Schedule `ExampleCommand` when `exampleCondition` changes to `true` - frc2::Trigger([this] { - return m_subsystem.ExampleCondition(); - }).OnTrue(ExampleCommand(&m_subsystem).ToPtr()); - - // Schedule `ExampleMethodCommand` when the Xbox controller's B button is - // pressed, cancelling on release. - m_driverController.B().WhileTrue(m_subsystem.ExampleMethodCommand()); + frc2::JoystickButton(&m_driverController, + frc::XboxController::Button::kRightBumper) + .WhileTrue(new frc2::RunCommand([this] { m_drive.SetX(); }, {&m_drive})); } frc2::CommandPtr RobotContainer::GetAutonomousCommand() { diff --git a/src/main/cpp/subsystems/DriveSubsystem.cpp b/src/main/cpp/subsystems/DriveSubsystem.cpp index aa0b38e..e90df82 100644 --- a/src/main/cpp/subsystems/DriveSubsystem.cpp +++ b/src/main/cpp/subsystems/DriveSubsystem.cpp @@ -38,8 +38,8 @@ void DriveSubsystem::Periodic() { // Implementation of subsystem periodic method goes here. m_odometry.Update(frc::Rotation2d(units::radian_t{ m_gyro.GetAngle(frc::ADIS16470_IMU::IMUAxis::kZ)}), - {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), - m_rearLeft.GetPosition(), m_rearRight.GetPosition()}); + {m_frontLeft.GetPosition(), m_rearLeft.GetPosition(), + m_frontRight.GetPosition(), m_rearRight.GetPosition()}); } void DriveSubsystem::Drive(units::meters_per_second_t xSpeed, diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index c18eb84..d8edc9a 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -54,15 +54,15 @@ constexpr double kRearLeftChassisAngularOffset = std::numbers::pi; constexpr double kRearRightChassisAngularOffset = std::numbers::pi / 2; // SPARK MAX CAN IDs -constexpr int kFrontLeftDrivingCanId = 11; -constexpr int kRearLeftDrivingCanId = 13; -constexpr int kFrontRightDrivingCanId = 15; -constexpr int kRearRightDrivingCanId = 17; - -constexpr int kFrontLeftTurningCanId = 10; -constexpr int kRearLeftTurningCanId = 12; -constexpr int kFrontRightTurningCanId = 14; -constexpr int kRearRightTurningCanId = 16; +constexpr int kFrontLeftDrivingCanId = 8; +constexpr int kRearLeftDrivingCanId = 6; +constexpr int kFrontRightDrivingCanId = 2; +constexpr int kRearRightDrivingCanId = 4; + +constexpr int kFrontLeftTurningCanId = 7; +constexpr int kRearLeftTurningCanId = 5; +constexpr int kFrontRightTurningCanId = 1; +constexpr int kRearRightTurningCanId = 3; } // namespace DriveConstants namespace ModuleConstants { diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index 06e4b0e..afa6a58 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -4,8 +4,15 @@ #pragma once -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include "Constants.h" #include "subsystems/DriveSubsystem.h" @@ -26,7 +33,7 @@ class RobotContainer { private: // Replace with CommandPS4Controller or CommandJoystick if needed - frc2::CommandXboxController m_driverController{ + frc::XboxController m_driverController{ OperatorConstants::kDriverControllerPort}; // The robot's subsystems are defined here... diff --git a/src/main/include/subsystems/DriveSubsystem.h b/src/main/include/subsystems/DriveSubsystem.h index 271cee4..2ee587a 100644 --- a/src/main/include/subsystems/DriveSubsystem.h +++ b/src/main/include/subsystems/DriveSubsystem.h @@ -12,6 +12,15 @@ #include #include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include "Constants.h" #include "MAXSwerveModule.h" From 99891840c6b06c8b76e3e46bcd1387c228a25961 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Fri, 10 Jan 2025 17:58:57 -0600 Subject: [PATCH 04/42] Robot relative swerve drive --- src/main/cpp/subsystems/DriveSubsystem.cpp | 6 +- src/main/include/subsystems/DriveSubsystem.h | 1 + src/main/include/subsystems/MAXSwerveModule.h | 3 +- ....1.1.json => PathplannerLib-2025.2.1.json} | 8 +- vendordeps/Phoenix6-25.1.0.json | 389 ++++++++++++++++++ 5 files changed, 399 insertions(+), 8 deletions(-) rename vendordeps/{PathplannerLib-2025.1.1.json => PathplannerLib-2025.2.1.json} (87%) create mode 100644 vendordeps/Phoenix6-25.1.0.json diff --git a/src/main/cpp/subsystems/DriveSubsystem.cpp b/src/main/cpp/subsystems/DriveSubsystem.cpp index e90df82..f0c370c 100644 --- a/src/main/cpp/subsystems/DriveSubsystem.cpp +++ b/src/main/cpp/subsystems/DriveSubsystem.cpp @@ -38,8 +38,8 @@ void DriveSubsystem::Periodic() { // Implementation of subsystem periodic method goes here. m_odometry.Update(frc::Rotation2d(units::radian_t{ m_gyro.GetAngle(frc::ADIS16470_IMU::IMUAxis::kZ)}), - {m_frontLeft.GetPosition(), m_rearLeft.GetPosition(), - m_frontRight.GetPosition(), m_rearRight.GetPosition()}); + {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), + m_rearLeft.GetPosition(), m_rearRight.GetPosition()}); } void DriveSubsystem::Drive(units::meters_per_second_t xSpeed, @@ -118,6 +118,6 @@ void DriveSubsystem::ResetOdometry(frc::Pose2d pose) { m_odometry.ResetPosition( GetHeading(), {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), - m_rearLeft.GetPosition(), m_rearRight.GetPosition()}, + m_rearLeft.GetPosition(), m_rearRight.GetPosition()}, pose); } \ No newline at end of file diff --git a/src/main/include/subsystems/DriveSubsystem.h b/src/main/include/subsystems/DriveSubsystem.h index 2ee587a..147f45a 100644 --- a/src/main/include/subsystems/DriveSubsystem.h +++ b/src/main/include/subsystems/DriveSubsystem.h @@ -21,6 +21,7 @@ #include #include #include +#include #include "Constants.h" #include "MAXSwerveModule.h" diff --git a/src/main/include/subsystems/MAXSwerveModule.h b/src/main/include/subsystems/MAXSwerveModule.h index de517f5..d23d196 100644 --- a/src/main/include/subsystems/MAXSwerveModule.h +++ b/src/main/include/subsystems/MAXSwerveModule.h @@ -10,6 +10,7 @@ #include #include #include +#include #include using namespace rev::spark; @@ -52,7 +53,7 @@ class MAXSwerveModule { void ResetEncoders(); private: - SparkMax m_drivingSpark; + SparkFlex m_drivingSpark; SparkMax m_turningSpark; SparkRelativeEncoder m_drivingEncoder = m_drivingSpark.GetEncoder(); diff --git a/vendordeps/PathplannerLib-2025.1.1.json b/vendordeps/PathplannerLib-2025.2.1.json similarity index 87% rename from vendordeps/PathplannerLib-2025.1.1.json rename to vendordeps/PathplannerLib-2025.2.1.json index 6322388..71e25f3 100644 --- a/vendordeps/PathplannerLib-2025.1.1.json +++ b/vendordeps/PathplannerLib-2025.2.1.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.1.1.json", + "fileName": "PathplannerLib-2025.2.1.json", "name": "PathplannerLib", - "version": "2025.1.1", + "version": "2025.2.1", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.1.1" + "version": "2025.2.1" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.1.1", + "version": "2025.2.1", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6-25.1.0.json b/vendordeps/Phoenix6-25.1.0.json new file mode 100644 index 0000000..473f6a8 --- /dev/null +++ b/vendordeps/Phoenix6-25.1.0.json @@ -0,0 +1,389 @@ +{ + "fileName": "Phoenix6-25.1.0.json", + "name": "CTRE-Phoenix (v6)", + "version": "25.1.0", + "frcYear": "2025", + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json", + "conflictsWith": [ + { + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", + "offlineFileName": "Phoenix6-replay-frc2025-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "25.1.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "25.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "api-cpp-sim", + "version": "25.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.1.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "25.1.0", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "25.1.0", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "25.1.0", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "25.1.0", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "25.1.0", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "25.1.0", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "25.1.0", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simCANCoder", + "version": "25.1.0", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "25.1.0", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "25.1.0", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "25.1.0", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "25.1.0", + "libName": "CTRE_SimProCANrange", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file From 58b5daa22213995098cc84ec139efbe8239899bd Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Fri, 10 Jan 2025 18:12:41 -0600 Subject: [PATCH 05/42] CTRE pigeon partial implementation --- src/main/cpp/subsystems/DriveSubsystem.cpp | 42 ++++++++++++++++++++ src/main/include/Constants.h | 6 ++- src/main/include/subsystems/DriveSubsystem.h | 3 ++ 3 files changed, 50 insertions(+), 1 deletion(-) diff --git a/src/main/cpp/subsystems/DriveSubsystem.cpp b/src/main/cpp/subsystems/DriveSubsystem.cpp index f0c370c..3b0287c 100644 --- a/src/main/cpp/subsystems/DriveSubsystem.cpp +++ b/src/main/cpp/subsystems/DriveSubsystem.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include "Constants.h" @@ -32,6 +33,8 @@ DriveSubsystem::DriveSubsystem() // Usage reporting for MAXSwerve template HAL_Report(HALUsageReporting::kResourceType_RobotDrive, HALUsageReporting::kRobotDriveSwerve_MaxSwerve); + + /* Configure Pigeon2 */ } void DriveSubsystem::Periodic() { @@ -40,6 +43,45 @@ void DriveSubsystem::Periodic() { m_gyro.GetAngle(frc::ADIS16470_IMU::IMUAxis::kZ)}), {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), m_rearLeft.GetPosition(), m_rearRight.GetPosition()}); + + if (frc::Timer::GetFPGATimestamp() - currentTime >= GyroConstants::kPrintPeriod) { + currentTime += GyroConstants::kPrintPeriod; + + /** + * GetYaw automatically calls Refresh(), no need to manually refresh. + * + * StatusSignalValues also have the "ostream <<" operator implemented, to provide + * a useful print of the signal + */ + auto &yaw = pidgey.GetYaw(); + std::cout << "Yaw is " << yaw << " with " << yaw.GetTimestamp().GetLatency().value() << " seconds of latency" << std::endl; + + /** + * Get the Gravity Vector Z component StatusSignalValue without refreshing + */ + auto &gravityZ = pidgey.GetGravityVectorZ(false); + /* This time wait for the signal to reduce latency */ + gravityZ.WaitForUpdate(GyroConstants::kPrintPeriod); // Wait up to our period + /** + * This uses the explicit GetValue and GetUnits methods to print, even though it's not + * necessary for the ostream print + */ + std::cout << "Gravity Vector in the Z direction is " << + gravityZ.GetValue().value() << " " << + gravityZ.GetUnits() << " with " << + gravityZ.GetTimestamp().GetLatency().value() << " seconds of latency" << + std::endl; + /** + * Notice when running this example that the second print's latency is always shorter than the first print's latency. + * This is because we explicitly wait for the signal using the WaitForUpdate() method instead of using the Refresh() + * method, which only gets the last cached value (similar to how Phoenix v5 works). + * This can be used to make sure we synchronously update our control loop from the CAN bus, reducing any latency or jitter in + * CAN bus measurements. + * When the device is on a CANivore, the reported latency is very close to the true latency of the sensor, as the CANivore + * timestamps when it receives the frame. This can be further used for latency compensation. + */ + std::cout << std::endl; + } } void DriveSubsystem::Drive(units::meters_per_second_t xSpeed, diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index d8edc9a..f60cda0 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -103,4 +103,8 @@ extern const frc::TrapezoidProfile::Constraints namespace OIConstants { constexpr int kDriverControllerPort = 0; constexpr double kDriveDeadband = 0.05; -} // namespace OIConstants \ No newline at end of file +} // namespace OIConstants + +namespace GyroConstants { + constexpr units::time::second_t kPrintPeriod{500_ms}; +} \ No newline at end of file diff --git a/src/main/include/subsystems/DriveSubsystem.h b/src/main/include/subsystems/DriveSubsystem.h index 147f45a..439403c 100644 --- a/src/main/include/subsystems/DriveSubsystem.h +++ b/src/main/include/subsystems/DriveSubsystem.h @@ -119,6 +119,9 @@ class DriveSubsystem : public frc2::SubsystemBase { MAXSwerveModule m_frontRight; MAXSwerveModule m_rearRight; + ctre::phoenix6::hardware::Pigeon2 pidgey{13, "rio"}; + units::time::second_t currentTime{frc::Timer::GetFPGATimestamp()}; + // The gyro sensor frc::ADIS16470_IMU m_gyro; From 6ed8b4ac87b50cb677423857832d8452b18a9020 Mon Sep 17 00:00:00 2001 From: WowMuchDoge <97766806+WowMuchDoge@users.noreply.github.com> Date: Sat, 11 Jan 2025 22:07:04 -0600 Subject: [PATCH 06/42] CTRE Pigeon implementation (untested) --- .vscode/settings.json | 82 +++++++++++++++++++- src/main/cpp/subsystems/DriveSubsystem.cpp | 18 +++-- src/main/include/subsystems/DriveSubsystem.h | 6 +- 3 files changed, 94 insertions(+), 12 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 5e795ff..5705711 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -14,5 +14,85 @@ "**/.factorypath": true, "**/*~": true }, - "C_Cpp.default.configurationProvider": "vscode-wpilib" + "C_Cpp.default.configurationProvider": "vscode-wpilib", + "files.associations": { + "compare": "cpp", + "concepts": "cpp", + "cstddef": "cpp", + "cstdint": "cpp", + "cstdlib": "cpp", + "initializer_list": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "xtr1common": "cpp", + "any": "cpp", + "array": "cpp", + "atomic": "cpp", + "bit": "cpp", + "*.tcc": "cpp", + "bitset": "cpp", + "cctype": "cpp", + "cfenv": "cpp", + "chrono": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "codecvt": "cpp", + "complex": "cpp", + "condition_variable": "cpp", + "cstdarg": "cpp", + "cstdio": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "deque": "cpp", + "forward_list": "cpp", + "list": "cpp", + "map": "cpp", + "set": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "exception": "cpp", + "expected": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "regex": "cpp", + "source_location": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "fstream": "cpp", + "future": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "numbers": "cpp", + "ostream": "cpp", + "ranges": "cpp", + "semaphore": "cpp", + "span": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "stop_token": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cinttypes": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp", + "valarray": "cpp", + "variant": "cpp" + } } diff --git a/src/main/cpp/subsystems/DriveSubsystem.cpp b/src/main/cpp/subsystems/DriveSubsystem.cpp index 3b0287c..a3e9eda 100644 --- a/src/main/cpp/subsystems/DriveSubsystem.cpp +++ b/src/main/cpp/subsystems/DriveSubsystem.cpp @@ -26,7 +26,7 @@ DriveSubsystem::DriveSubsystem() kRearRightChassisAngularOffset}, m_odometry{kDriveKinematics, frc::Rotation2d(units::radian_t{ - m_gyro.GetAngle(frc::ADIS16470_IMU::IMUAxis::kZ)}), + pidgey.GetYaw().GetValue()}), {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), m_rearLeft.GetPosition(), m_rearRight.GetPosition()}, frc::Pose2d{}} { @@ -35,12 +35,16 @@ DriveSubsystem::DriveSubsystem() HALUsageReporting::kRobotDriveSwerve_MaxSwerve); /* Configure Pigeon2 */ + + pidgey.SetYaw(144_deg, 100_ms); // Set our yaw to 144 degrees and wait up to 100 milliseconds for the setter to take affect + 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 " << pidgey.GetYaw() << std::endl; } void DriveSubsystem::Periodic() { // Implementation of subsystem periodic method goes here. m_odometry.Update(frc::Rotation2d(units::radian_t{ - m_gyro.GetAngle(frc::ADIS16470_IMU::IMUAxis::kZ)}), + pidgey.GetYaw().GetValue()}), {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), m_rearLeft.GetPosition(), m_rearRight.GetPosition()}); @@ -101,7 +105,7 @@ void DriveSubsystem::Drive(units::meters_per_second_t xSpeed, ? frc::ChassisSpeeds::FromFieldRelativeSpeeds( xSpeedDelivered, ySpeedDelivered, rotDelivered, frc::Rotation2d(units::radian_t{ - m_gyro.GetAngle(frc::ADIS16470_IMU::IMUAxis::kZ)})) + pidgey.GetYaw().GetValue()})) : frc::ChassisSpeeds{xSpeedDelivered, ySpeedDelivered, rotDelivered}); kDriveKinematics.DesaturateWheelSpeeds(&states, DriveConstants::kMaxSpeed); @@ -142,16 +146,16 @@ void DriveSubsystem::ResetEncoders() { m_rearRight.ResetEncoders(); } -units::degree_t DriveSubsystem::GetHeading() const { +units::degree_t DriveSubsystem::GetHeading() { return frc::Rotation2d( - units::radian_t{m_gyro.GetAngle(frc::ADIS16470_IMU::IMUAxis::kZ)}) + units::radian_t{pidgey.GetYaw().GetValue()}) .Degrees(); } -void DriveSubsystem::ZeroHeading() { m_gyro.Reset(); } +void DriveSubsystem::ZeroHeading() { pidgey.Reset(); } double DriveSubsystem::GetTurnRate() { - return -m_gyro.GetRate(frc::ADIS16470_IMU::IMUAxis::kZ).value(); + return -pidgey.GetAngularVelocityZWorld().GetValue().value(); } frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); } diff --git a/src/main/include/subsystems/DriveSubsystem.h b/src/main/include/subsystems/DriveSubsystem.h index 439403c..5e8d11e 100644 --- a/src/main/include/subsystems/DriveSubsystem.h +++ b/src/main/include/subsystems/DriveSubsystem.h @@ -72,7 +72,7 @@ class DriveSubsystem : public frc2::SubsystemBase { * * @return the robot's heading in degrees, from 180 to 180 */ - units::degree_t GetHeading() const; + units::degree_t GetHeading(); /** * Zeroes the heading of the robot. @@ -119,12 +119,10 @@ class DriveSubsystem : public frc2::SubsystemBase { MAXSwerveModule m_frontRight; MAXSwerveModule m_rearRight; + // Gyro Sensor ctre::phoenix6::hardware::Pigeon2 pidgey{13, "rio"}; units::time::second_t currentTime{frc::Timer::GetFPGATimestamp()}; - // The gyro sensor - frc::ADIS16470_IMU m_gyro; - // Odometry class for tracking robot pose // 4 defines the number of modules frc::SwerveDriveOdometry<4> m_odometry; From 72576335460dc88fb5366b38bb405375568a05af Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Mon, 13 Jan 2025 16:41:34 -0600 Subject: [PATCH 07/42] Remove delay (still robot oriented) --- src/main/cpp/subsystems/DriveSubsystem.cpp | 73 +++++++++++----------- src/main/include/Constants.h | 2 +- 2 files changed, 37 insertions(+), 38 deletions(-) diff --git a/src/main/cpp/subsystems/DriveSubsystem.cpp b/src/main/cpp/subsystems/DriveSubsystem.cpp index a3e9eda..cf54a90 100644 --- a/src/main/cpp/subsystems/DriveSubsystem.cpp +++ b/src/main/cpp/subsystems/DriveSubsystem.cpp @@ -47,44 +47,43 @@ void DriveSubsystem::Periodic() { pidgey.GetYaw().GetValue()}), {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), m_rearLeft.GetPosition(), m_rearRight.GetPosition()}); - - if (frc::Timer::GetFPGATimestamp() - currentTime >= GyroConstants::kPrintPeriod) { - currentTime += GyroConstants::kPrintPeriod; - - /** - * GetYaw automatically calls Refresh(), no need to manually refresh. - * - * StatusSignalValues also have the "ostream <<" operator implemented, to provide - * a useful print of the signal - */ auto &yaw = pidgey.GetYaw(); - std::cout << "Yaw is " << yaw << " with " << yaw.GetTimestamp().GetLatency().value() << " seconds of latency" << std::endl; - - /** - * Get the Gravity Vector Z component StatusSignalValue without refreshing - */ - auto &gravityZ = pidgey.GetGravityVectorZ(false); - /* This time wait for the signal to reduce latency */ - gravityZ.WaitForUpdate(GyroConstants::kPrintPeriod); // Wait up to our period - /** - * This uses the explicit GetValue and GetUnits methods to print, even though it's not - * necessary for the ostream print - */ - std::cout << "Gravity Vector in the Z direction is " << - gravityZ.GetValue().value() << " " << - gravityZ.GetUnits() << " with " << - gravityZ.GetTimestamp().GetLatency().value() << " seconds of latency" << - std::endl; - /** - * Notice when running this example that the second print's latency is always shorter than the first print's latency. - * This is because we explicitly wait for the signal using the WaitForUpdate() method instead of using the Refresh() - * method, which only gets the last cached value (similar to how Phoenix v5 works). - * This can be used to make sure we synchronously update our control loop from the CAN bus, reducing any latency or jitter in - * CAN bus measurements. - * When the device is on a CANivore, the reported latency is very close to the true latency of the sensor, as the CANivore - * timestamps when it receives the frame. This can be further used for latency compensation. - */ - std::cout << std::endl; + if (frc::Timer::GetFPGATimestamp() - currentTime >= GyroConstants::kPrintPeriod) { + // currentTime += GyroConstants::kPrintPeriod; + std::cout << yaw.GetValue().value() << std::endl; + // /** + // * GetYaw automatically calls Refresh(), no need to manually refresh. + // * + // * StatusSignalValues also have the "ostream <<" operator implemented, to provide + // * a useful print of the signal + // */ + // std::cout << "Yaw is " << yaw << " with " << yaw.GetTimestamp().GetLatency().value() << " seconds of latency" << std::endl; + + // /** + // * Get the Gravity Vector Z component StatusSignalValue without refreshing + // */ + // auto &gravityZ = pidgey.GetGravityVectorZ(false); + // /* This time wait for the signal to reduce latency */ + // gravityZ.WaitForUpdate(GyroConstants::kPrintPeriod); // Wait up to our period + // /** + // * This uses the explicit GetValue and GetUnits methods to print, even though it's not + // * necessary for the ostream print + // */ + // std::cout << "Gravity Vector in the Z direction is " << + // gravityZ.GetValue().value() << " " << + // gravityZ.GetUnits() << " with " << + // gravityZ.GetTimestamp().GetLatency().value() << " seconds of latency" << + // std::endl; + // /** + // * Notice when running this example that the second print's latency is always shorter than the first print's latency. + // * This is because we explicitly wait for the signal using the WaitForUpdate() method instead of using the Refresh() + // * method, which only gets the last cached value (similar to how Phoenix v5 works). + // * This can be used to make sure we synchronously update our control loop from the CAN bus, reducing any latency or jitter in + // * CAN bus measurements. + // * When the device is on a CANivore, the reported latency is very close to the true latency of the sensor, as the CANivore + // * timestamps when it receives the frame. This can be further used for latency compensation. + // */ + // std::cout << std::endl; } } diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index f60cda0..4e052a2 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -106,5 +106,5 @@ constexpr double kDriveDeadband = 0.05; } // namespace OIConstants namespace GyroConstants { - constexpr units::time::second_t kPrintPeriod{500_ms}; + constexpr units::time::second_t kPrintPeriod{100_ms}; } \ No newline at end of file From 34b37a65dfd0a1ed4499bfa7267b7cf1f2e55a3b Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Tue, 14 Jan 2025 18:43:51 -0600 Subject: [PATCH 08/42] Fix gyro and add basic auto --- src/main/cpp/RobotContainer.cpp | 3 +- src/main/cpp/subsystems/DriveSubsystem.cpp | 55 ++++++++++-- .../pathplanner/autos/Example Auto.auto | 19 +++++ src/main/deploy/pathplanner/navgrid.json | 1 + .../pathplanner/paths/Example Path.path | 54 ++++++++++++ src/main/deploy/pathplanner/settings.json | 32 +++++++ src/main/include/Constants.h | 4 +- src/main/include/subsystems/DriveSubsystem.h | 6 +- ...enix6-25.1.0.json => Phoenix6-25.2.0.json} | 84 +++++++++++++------ ...Lib-2025.0.0.json => REVLib-2025.0.1.json} | 15 ++-- 10 files changed, 227 insertions(+), 46 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Example Auto.auto create mode 100644 src/main/deploy/pathplanner/navgrid.json create mode 100644 src/main/deploy/pathplanner/paths/Example Path.path create mode 100644 src/main/deploy/pathplanner/settings.json rename vendordeps/{Phoenix6-25.1.0.json => Phoenix6-25.2.0.json} (85%) rename vendordeps/{REVLib-2025.0.0.json => REVLib-2025.0.1.json} (86%) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 683c320..6de9ec8 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -13,6 +13,7 @@ #include #include #include +#include #include #include @@ -50,5 +51,5 @@ void RobotContainer::ConfigureBindings() { frc2::CommandPtr RobotContainer::GetAutonomousCommand() { // An example command will be run in autonomous - return autos::ExampleAuto(&m_subsystem); + return pathplanner::PathPlannerAuto("Example Auto").ToPtr(); } diff --git a/src/main/cpp/subsystems/DriveSubsystem.cpp b/src/main/cpp/subsystems/DriveSubsystem.cpp index cf54a90..2ea7ac2 100644 --- a/src/main/cpp/subsystems/DriveSubsystem.cpp +++ b/src/main/cpp/subsystems/DriveSubsystem.cpp @@ -4,8 +4,12 @@ #include "subsystems/DriveSubsystem.h" +#include +#include +#include #include #include +#include #include #include #include @@ -24,7 +28,7 @@ DriveSubsystem::DriveSubsystem() kFrontRightChassisAngularOffset}, m_rearRight{kRearRightDrivingCanId, kRearRightTurningCanId, kRearRightChassisAngularOffset}, - m_odometry{kDriveKinematics, + m_odometry{m_driveKinematics, frc::Rotation2d(units::radian_t{ pidgey.GetYaw().GetValue()}), {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), @@ -34,9 +38,35 @@ DriveSubsystem::DriveSubsystem() HAL_Report(HALUsageReporting::kResourceType_RobotDrive, HALUsageReporting::kRobotDriveSwerve_MaxSwerve); + pathplanner::RobotConfig config = pathplanner::RobotConfig::fromGUISettings(); + + pathplanner::AutoBuilder::configure( + [this](){ return GetPose(); }, // Robot pose supplier + [this](frc::Pose2d pose){ ResetOdometry(pose); }, // Method to reset odometry (will be called if your auto has a starting pose) + [this](){ return getRobotRelativeSpeeds(); }, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + [this](auto speeds, auto feedforwards){ Drive(speeds); }, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards + std::make_shared( // PPHolonomicController is the built in path following controller for holonomic drive trains + pathplanner::PIDConstants(5.0, 0.0, 0.0), // Translation PID constants + pathplanner::PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants + ), + config, // The robot configuration + []() { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + auto alliance = frc::DriverStation::GetAlliance(); + if (alliance) { + return alliance.value() == frc::DriverStation::Alliance::kRed; + } + return false; + }, + this // Reference to this subsystem to set requirements + ); + /* Configure Pigeon2 */ - pidgey.SetYaw(144_deg, 100_ms); // Set our yaw to 144 degrees and wait up to 100 milliseconds for the setter to take affect + pidgey.SetYaw(0_deg, 100_ms); // Set our yaw to 0 degrees and wait up to 100 milliseconds for the setter to take affect 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 " << pidgey.GetYaw() << std::endl; } @@ -50,7 +80,6 @@ void DriveSubsystem::Periodic() { auto &yaw = pidgey.GetYaw(); if (frc::Timer::GetFPGATimestamp() - currentTime >= GyroConstants::kPrintPeriod) { // currentTime += GyroConstants::kPrintPeriod; - std::cout << yaw.GetValue().value() << std::endl; // /** // * GetYaw automatically calls Refresh(), no need to manually refresh. // * @@ -99,7 +128,7 @@ void DriveSubsystem::Drive(units::meters_per_second_t xSpeed, units::radians_per_second_t rotDelivered = rot.value() * DriveConstants::kMaxAngularSpeed; - auto states = kDriveKinematics.ToSwerveModuleStates( + auto states = m_driveKinematics.ToSwerveModuleStates( fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds( xSpeedDelivered, ySpeedDelivered, rotDelivered, @@ -107,7 +136,7 @@ void DriveSubsystem::Drive(units::meters_per_second_t xSpeed, pidgey.GetYaw().GetValue()})) : frc::ChassisSpeeds{xSpeedDelivered, ySpeedDelivered, rotDelivered}); - kDriveKinematics.DesaturateWheelSpeeds(&states, DriveConstants::kMaxSpeed); + m_driveKinematics.DesaturateWheelSpeeds(&states, DriveConstants::kMaxSpeed); auto [fl, fr, bl, br] = states; @@ -117,6 +146,20 @@ void DriveSubsystem::Drive(units::meters_per_second_t xSpeed, m_rearRight.SetDesiredState(br); } +void DriveSubsystem::Drive(frc::ChassisSpeeds speeds) { + DriveSubsystem::SetModuleStates( + m_driveKinematics.ToSwerveModuleStates(speeds)); +} + +frc::ChassisSpeeds DriveSubsystem::getRobotRelativeSpeeds() { + auto fl = m_frontLeft.GetState(); + auto fr = m_frontRight.GetState(); + auto rl = m_rearLeft.GetState(); + auto rr = m_rearRight.GetState(); + + return m_driveKinematics.ToChassisSpeeds(fl, fr, rl, rr); +} + void DriveSubsystem::SetX() { m_frontLeft.SetDesiredState( frc::SwerveModuleState{0_mps, frc::Rotation2d{45_deg}}); @@ -130,7 +173,7 @@ void DriveSubsystem::SetX() { void DriveSubsystem::SetModuleStates( wpi::array desiredStates) { - kDriveKinematics.DesaturateWheelSpeeds(&desiredStates, + m_driveKinematics.DesaturateWheelSpeeds(&desiredStates, DriveConstants::kMaxSpeed); m_frontLeft.SetDesiredState(desiredStates[0]); m_frontRight.SetDesiredState(desiredStates[1]); diff --git a/src/main/deploy/pathplanner/autos/Example Auto.auto b/src/main/deploy/pathplanner/autos/Example Auto.auto new file mode 100644 index 0000000..70b7ab2 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Example Auto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Example Path" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..23e0db9 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path new file mode 100644 index 0000000..3f475e5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 6.0 + }, + "prevControl": { + "x": 3.0, + "y": 6.0 + }, + "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": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 0000000..12a5244 --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,32 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "defaultNominalVoltage": 12.0, + "robotMass": 25.13, + "robotMOI": 6.883, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.0381, + "driveGearing": 5.143, + "maxDriveSpeed": 3.0, + "driveMotorType": "vortex", + "driveCurrentLimit": 50.0, + "wheelCOF": 1.2, + "flModuleX": 0.273, + "flModuleY": 0.273, + "frModuleX": 0.273, + "frModuleY": -0.273, + "blModuleX": -0.273, + "blModuleY": 0.273, + "brModuleX": -0.273, + "brModuleY": -0.273, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [] +} \ No newline at end of file diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index 4e052a2..ce07e39 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -77,10 +77,10 @@ constexpr double kDrivingMotorFreeSpeedRps = constexpr units::meter_t kWheelDiameter = 0.0762_m; constexpr units::meter_t kWheelCircumference = kWheelDiameter * std::numbers::pi; -// 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 +// 45 teeth on the wheel's bevel gear, 21 teeth on the first-stage spur gear, 15 // teeth on the bevel pinion constexpr double kDrivingMotorReduction = - (45.0 * 22) / (kDrivingMotorPinionTeeth * 15); + (45.0 * 21) / (kDrivingMotorPinionTeeth * 15); constexpr double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumference.value()) / kDrivingMotorReduction; diff --git a/src/main/include/subsystems/DriveSubsystem.h b/src/main/include/subsystems/DriveSubsystem.h index 5e8d11e..490ecdb 100644 --- a/src/main/include/subsystems/DriveSubsystem.h +++ b/src/main/include/subsystems/DriveSubsystem.h @@ -52,6 +52,8 @@ class DriveSubsystem : public frc2::SubsystemBase { units::meters_per_second_t ySpeed, units::radians_per_second_t rot, bool fieldRelative); + void Drive(frc::ChassisSpeeds speeds); + /** * Sets the wheels into an X formation to prevent movement. */ @@ -100,7 +102,7 @@ class DriveSubsystem : public frc2::SubsystemBase { */ void ResetOdometry(frc::Pose2d pose); - frc::SwerveDriveKinematics<4> kDriveKinematics{ + frc::SwerveDriveKinematics<4> m_driveKinematics{ frc::Translation2d{DriveConstants::kWheelBase / 2, DriveConstants::kTrackWidth / 2}, frc::Translation2d{DriveConstants::kWheelBase / 2, @@ -114,6 +116,8 @@ class DriveSubsystem : public frc2::SubsystemBase { // Components (e.g. motor controllers and sensors) should generally be // declared private and exposed only through public methods. + frc::ChassisSpeeds getRobotRelativeSpeeds(); + MAXSwerveModule m_frontLeft; MAXSwerveModule m_rearLeft; MAXSwerveModule m_frontRight; diff --git a/vendordeps/Phoenix6-25.1.0.json b/vendordeps/Phoenix6-25.2.0.json similarity index 85% rename from vendordeps/Phoenix6-25.1.0.json rename to vendordeps/Phoenix6-25.2.0.json index 473f6a8..38747fb 100644 --- a/vendordeps/Phoenix6-25.1.0.json +++ b/vendordeps/Phoenix6-25.2.0.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-25.1.0.json", + "fileName": "Phoenix6-25.2.0.json", "name": "CTRE-Phoenix (v6)", - "version": "25.1.0", + "version": "25.2.0", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.1.0" + "version": "25.2.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.1.0", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.1.0", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.1.0", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.1.0", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.1.0", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.1.0", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.1.0", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.1.0", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,21 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.1.0", + "version": "25.2.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.1.0", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.1.0", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.1.0", + "version": "25.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -196,7 +210,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.1.0", + "version": "25.2.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -212,7 +226,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.1.0", + "version": "25.2.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -228,7 +242,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.1.0", + "version": "25.2.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -244,7 +258,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.1.0", + "version": "25.2.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -260,7 +274,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.1.0", + "version": "25.2.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -276,7 +290,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.1.0", + "version": "25.2.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -292,7 +306,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.1.0", + "version": "25.2.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +322,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.1.0", + "version": "25.2.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -324,7 +338,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.1.0", + "version": "25.2.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -337,10 +351,26 @@ ], "simMode": "swsim" }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "25.2.0", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.1.0", + "version": "25.2.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -356,7 +386,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.1.0", + "version": "25.2.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -372,7 +402,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.1.0", + "version": "25.2.0", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib-2025.0.0.json b/vendordeps/REVLib-2025.0.1.json similarity index 86% rename from vendordeps/REVLib-2025.0.0.json rename to vendordeps/REVLib-2025.0.1.json index cde6011..c998054 100644 --- a/vendordeps/REVLib-2025.0.0.json +++ b/vendordeps/REVLib-2025.0.1.json @@ -1,7 +1,7 @@ { - "fileName": "REVLib-2025.0.0.json", + "fileName": "REVLib-2025.0.1.json", "name": "REVLib", - "version": "2025.0.0", + "version": "2025.0.1", "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,19 +12,18 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2025.0.0" + "version": "2025.0.1" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.0", + "version": "2025.0.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ "windowsx86-64", - "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", @@ -37,14 +36,13 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2025.0.0", + "version": "2025.0.1", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", @@ -55,14 +53,13 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.0", + "version": "2025.0.1", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, "skipInvalidPlatforms": true, "binaryPlatforms": [ "windowsx86-64", - "windowsx86", "linuxarm64", "linuxx86-64", "linuxathena", From 7234501b32c48d1baa4d8c166837e40c78224b24 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Tue, 14 Jan 2025 19:51:02 -0600 Subject: [PATCH 09/42] Offset gyro after auto --- src/main/cpp/RobotContainer.cpp | 10 +++++++++- src/main/cpp/subsystems/DriveSubsystem.cpp | 9 +++++++++ src/main/include/subsystems/DriveSubsystem.h | 4 ++++ 3 files changed, 22 insertions(+), 1 deletion(-) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 6de9ec8..9fe2d49 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -51,5 +51,13 @@ void RobotContainer::ConfigureBindings() { frc2::CommandPtr RobotContainer::GetAutonomousCommand() { // An example command will be run in autonomous - return pathplanner::PathPlannerAuto("Example Auto").ToPtr(); + + auto command = pathplanner::PathPlannerAuto("Example Auto"); + auto rot = command.getStartingPose().Rotation().Degrees(); + + auto offset = m_drive.GetHeading() - rot; + + m_drive.OffsetRotation(offset); + + return pathplanner::PathPlannerAuto("Example Auto").AndThen(frc2::InstantCommand([this, offset]() { m_drive.OffsetRotation(offset); }).ToPtr()); } diff --git a/src/main/cpp/subsystems/DriveSubsystem.cpp b/src/main/cpp/subsystems/DriveSubsystem.cpp index 2ea7ac2..ef44e31 100644 --- a/src/main/cpp/subsystems/DriveSubsystem.cpp +++ b/src/main/cpp/subsystems/DriveSubsystem.cpp @@ -208,4 +208,13 @@ void DriveSubsystem::ResetOdometry(frc::Pose2d pose) { {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), m_rearLeft.GetPosition(), m_rearRight.GetPosition()}, pose); +} + +void DriveSubsystem::OffsetRotation(frc::Rotation2d offset) { + pidgey.SetYaw(offset.Degrees() + pidgey.GetYaw().GetValue()); + m_odometry.ResetPosition( + pidgey.GetYaw().GetValue(), + {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), + m_rearLeft.GetPosition(), m_rearRight.GetPosition()}, + m_odometry.GetPose()); } \ No newline at end of file diff --git a/src/main/include/subsystems/DriveSubsystem.h b/src/main/include/subsystems/DriveSubsystem.h index 490ecdb..ed50c23 100644 --- a/src/main/include/subsystems/DriveSubsystem.h +++ b/src/main/include/subsystems/DriveSubsystem.h @@ -102,6 +102,10 @@ class DriveSubsystem : public frc2::SubsystemBase { */ void ResetOdometry(frc::Pose2d pose); + void OffsetRotation(frc::Rotation2d rot); + + void ResetOdometry(); + frc::SwerveDriveKinematics<4> m_driveKinematics{ frc::Translation2d{DriveConstants::kWheelBase / 2, DriveConstants::kTrackWidth / 2}, From 1cf094946702d5d4de71c931823c9f7814d3fe45 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Thu, 16 Jan 2025 18:23:20 -0600 Subject: [PATCH 10/42] Pathplanner path editor working state --- .../pathplanner/autos/Example Auto.auto | 19 ------------------- src/main/deploy/pathplanner/settings.json | 10 +++++----- 2 files changed, 5 insertions(+), 24 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/Example Auto.auto diff --git a/src/main/deploy/pathplanner/autos/Example Auto.auto b/src/main/deploy/pathplanner/autos/Example Auto.auto deleted file mode 100644 index 70b7ab2..0000000 --- a/src/main/deploy/pathplanner/autos/Example Auto.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Example Path" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 12a5244..5bf438c 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -9,14 +9,14 @@ "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 25.13, + "robotMass": 74.088, "robotMOI": 6.883, "robotTrackwidth": 0.546, - "driveWheelRadius": 0.0381, + "driveWheelRadius": 0.048, "driveGearing": 5.143, - "maxDriveSpeed": 3.0, - "driveMotorType": "vortex", - "driveCurrentLimit": 50.0, + "maxDriveSpeed": 5.45, + "driveMotorType": "krakenX60", + "driveCurrentLimit": 60.0, "wheelCOF": 1.2, "flModuleX": 0.273, "flModuleY": 0.273, From f56c61bf38d8b72f443622c6d83ce2048dd7fcdb Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Thu, 16 Jan 2025 19:02:38 -0600 Subject: [PATCH 11/42] Fix pathplanner bug?? --- .../pathplanner/autos/Example Auto.auto | 19 +++++++++++++++++++ .../paths/{Example Path.path => Spin.path} | 10 +++++----- src/main/deploy/pathplanner/settings.json | 12 ++++++------ 3 files changed, 30 insertions(+), 11 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Example Auto.auto rename src/main/deploy/pathplanner/paths/{Example Path.path => Spin.path} (85%) diff --git a/src/main/deploy/pathplanner/autos/Example Auto.auto b/src/main/deploy/pathplanner/autos/Example Auto.auto new file mode 100644 index 0000000..dff1ae6 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Example Auto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Spin" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Spin.path similarity index 85% rename from src/main/deploy/pathplanner/paths/Example Path.path rename to src/main/deploy/pathplanner/paths/Spin.path index 3f475e5..fe2cb5a 100644 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/Spin.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 4.0, - "y": 6.0 + "x": 4.627254098360655, + "y": 6.536424180327868 }, "prevControl": { - "x": 3.0, - "y": 6.0 + "x": 4.9054815573770485, + "y": 6.518442622950819 }, "nextControl": null, "isLocked": false, @@ -42,7 +42,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 90.0 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 5bf438c..b6ce400 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -12,12 +12,12 @@ "robotMass": 74.088, "robotMOI": 6.883, "robotTrackwidth": 0.546, - "driveWheelRadius": 0.048, - "driveGearing": 5.143, - "maxDriveSpeed": 5.45, - "driveMotorType": "krakenX60", - "driveCurrentLimit": 60.0, - "wheelCOF": 1.2, + "driveWheelRadius": 0.0381, + "driveGearing": 4.5, + "maxDriveSpeed": 6.01, + "driveMotorType": "vortex", + "driveCurrentLimit": 50.0, + "wheelCOF": 1.46, "flModuleX": 0.273, "flModuleY": 0.273, "frModuleX": 0.273, From 4fd701baf6791e0d43fac5d6719a77f68e833cf6 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Thu, 16 Jan 2025 19:45:43 -0600 Subject: [PATCH 12/42] Fix gyro zeroing at end of auto --- src/main/cpp/RobotContainer.cpp | 8 +++++--- src/main/cpp/subsystems/DriveSubsystem.cpp | 5 ++++- .../autos/{Example Auto.auto => Spin Auto.auto} | 0 src/main/deploy/pathplanner/paths/Spin.path | 4 ++-- src/main/include/Constants.h | 2 +- 5 files changed, 12 insertions(+), 7 deletions(-) rename src/main/deploy/pathplanner/autos/{Example Auto.auto => Spin Auto.auto} (100%) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 9fe2d49..d853388 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include "commands/Autos.h" #include "commands/ExampleCommand.h" @@ -52,12 +53,13 @@ void RobotContainer::ConfigureBindings() { frc2::CommandPtr RobotContainer::GetAutonomousCommand() { // An example command will be run in autonomous - auto command = pathplanner::PathPlannerAuto("Example Auto"); + auto command = pathplanner::PathPlannerAuto("Spin Auto"); auto rot = command.getStartingPose().Rotation().Degrees(); auto offset = m_drive.GetHeading() - rot; - m_drive.OffsetRotation(offset); + std::cout << "Offset " << offset.value() << std::endl; - return pathplanner::PathPlannerAuto("Example Auto").AndThen(frc2::InstantCommand([this, offset]() { m_drive.OffsetRotation(offset); }).ToPtr()); + return pathplanner::PathPlannerAuto("Spin Auto") + .AndThen(frc2::InstantCommand([this, offset]() { m_drive.OffsetRotation(offset); }).ToPtr()); } diff --git a/src/main/cpp/subsystems/DriveSubsystem.cpp b/src/main/cpp/subsystems/DriveSubsystem.cpp index ef44e31..fafcbdd 100644 --- a/src/main/cpp/subsystems/DriveSubsystem.cpp +++ b/src/main/cpp/subsystems/DriveSubsystem.cpp @@ -79,7 +79,10 @@ void DriveSubsystem::Periodic() { m_rearLeft.GetPosition(), m_rearRight.GetPosition()}); auto &yaw = pidgey.GetYaw(); if (frc::Timer::GetFPGATimestamp() - currentTime >= GyroConstants::kPrintPeriod) { - // currentTime += GyroConstants::kPrintPeriod; + + std::cout << "Yaw: " << yaw.GetValue().value() << std::endl; + + currentTime += GyroConstants::kPrintPeriod; // /** // * GetYaw automatically calls Refresh(), no need to manually refresh. // * diff --git a/src/main/deploy/pathplanner/autos/Example Auto.auto b/src/main/deploy/pathplanner/autos/Spin Auto.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/Example Auto.auto rename to src/main/deploy/pathplanner/autos/Spin Auto.auto diff --git a/src/main/deploy/pathplanner/paths/Spin.path b/src/main/deploy/pathplanner/paths/Spin.path index fe2cb5a..27fbf36 100644 --- a/src/main/deploy/pathplanner/paths/Spin.path +++ b/src/main/deploy/pathplanner/paths/Spin.path @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 90.0 + "rotation": 0.0 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index ce07e39..63eeb6e 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -106,5 +106,5 @@ constexpr double kDriveDeadband = 0.05; } // namespace OIConstants namespace GyroConstants { - constexpr units::time::second_t kPrintPeriod{100_ms}; + constexpr units::time::second_t kPrintPeriod{1000_ms}; } \ No newline at end of file From 6a48defd1dec242b546a703cb794f2c2dd0b6fb6 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Sat, 25 Jan 2025 13:51:00 -0600 Subject: [PATCH 13/42] Update vendor deps --- ...enix6-25.2.0.json => Phoenix6-25.2.1.json} | 58 +++++++-------- vendordeps/photonlib.json | 71 +++++++++++++++++++ 2 files changed, 100 insertions(+), 29 deletions(-) rename vendordeps/{Phoenix6-25.2.0.json => Phoenix6-25.2.1.json} (92%) create mode 100644 vendordeps/photonlib.json diff --git a/vendordeps/Phoenix6-25.2.0.json b/vendordeps/Phoenix6-25.2.1.json similarity index 92% rename from vendordeps/Phoenix6-25.2.0.json rename to vendordeps/Phoenix6-25.2.1.json index 38747fb..1397da1 100644 --- a/vendordeps/Phoenix6-25.2.0.json +++ b/vendordeps/Phoenix6-25.2.1.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-25.2.0.json", + "fileName": "Phoenix6-25.2.1.json", "name": "CTRE-Phoenix (v6)", - "version": "25.2.0", + "version": "25.2.1", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.2.0" + "version": "25.2.1" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.2.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.2.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.0", + "version": "25.2.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -210,7 +210,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.2.0", + "version": "25.2.1", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -226,7 +226,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.0", + "version": "25.2.1", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -242,7 +242,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.2.0", + "version": "25.2.1", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -258,7 +258,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.0", + "version": "25.2.1", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -274,7 +274,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.0", + "version": "25.2.1", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -290,7 +290,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.0", + "version": "25.2.1", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -306,7 +306,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.0", + "version": "25.2.1", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -322,7 +322,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.0", + "version": "25.2.1", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -338,7 +338,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.0", + "version": "25.2.1", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -354,7 +354,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.0", + "version": "25.2.1", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -370,7 +370,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.0", + "version": "25.2.1", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -386,7 +386,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.0", + "version": "25.2.1", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -402,7 +402,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.0", + "version": "25.2.1", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..6af3d3e --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,71 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2025.1.1", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2025", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.1.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2025.1.1", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.1.1", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2025.1.1" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2025.1.1" + } + ] +} \ No newline at end of file From 7539b740f4e5ad2f26ef05fb0809455275c2f7c3 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Sat, 25 Jan 2025 17:03:12 -0600 Subject: [PATCH 14/42] Add sendable chooser for autos --- simgui-ds.json | 92 +++++++++++++++++++ src/main/cpp/RobotContainer.cpp | 17 +++- .../deploy/pathplanner/autos/Out Auto.auto | 19 ++++ src/main/include/Constants.h | 4 + src/main/include/RobotContainer.h | 4 + 5 files changed, 131 insertions(+), 5 deletions(-) create mode 100644 simgui-ds.json create mode 100644 src/main/deploy/pathplanner/autos/Out Auto.auto diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index d853388..af5824b 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -14,6 +14,7 @@ #include #include #include +#include #include #include #include @@ -24,8 +25,14 @@ RobotContainer::RobotContainer() { // Initialize all of your commands and subsystems here + // m_chooser.SetDefaultOption(AutoConstants::kDefaultAutoName, AutoConstants::kDefaultAutoName); + + frc::SmartDashboard::PutData(&m_chooser); + m_chooser.SetDefaultOption(AutoConstants::kOutAuto, AutoConstants::kSpinAuto); + m_chooser.AddOption(AutoConstants::kSpinAuto, AutoConstants::kSpinAuto); + // Configure the button bindings - ConfigureBindings(); + ConfigureBindings(); // Set up default drive command // The left stick controls translation of the robot. @@ -51,15 +58,15 @@ void RobotContainer::ConfigureBindings() { } frc2::CommandPtr RobotContainer::GetAutonomousCommand() { - // An example command will be run in autonomous - auto command = pathplanner::PathPlannerAuto("Spin Auto"); auto rot = command.getStartingPose().Rotation().Degrees(); auto offset = m_drive.GetHeading() - rot; - std::cout << "Offset " << offset.value() << std::endl; + m_autoSelected = m_chooser.GetSelected(); + + std::cout << "Auto Selected: \"" << m_autoSelected << "\".\n"; - return pathplanner::PathPlannerAuto("Spin Auto") + return pathplanner::PathPlannerAuto(m_autoSelected) .AndThen(frc2::InstantCommand([this, offset]() { m_drive.OffsetRotation(offset); }).ToPtr()); } diff --git a/src/main/deploy/pathplanner/autos/Out Auto.auto b/src/main/deploy/pathplanner/autos/Out Auto.auto new file mode 100644 index 0000000..c419b83 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Out Auto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Out" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index 63eeb6e..58d5842 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -12,6 +12,7 @@ #include #include +#include #pragma once @@ -92,6 +93,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"; + constexpr double kPXController = 0.5; constexpr double kPYController = 0.5; constexpr double kPThetaController = 0.5; diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index afa6a58..bf580e1 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -13,6 +13,7 @@ #include #include #include +#include #include "Constants.h" #include "subsystems/DriveSubsystem.h" @@ -42,4 +43,7 @@ class RobotContainer { ExampleSubsystem m_subsystem; void ConfigureBindings(); + + frc::SendableChooser m_chooser; + std::string m_autoSelected; }; From ad89ea79ddbf45a1b544c3cc14d074b989f2a144 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Tue, 28 Jan 2025 16:05:12 -0600 Subject: [PATCH 15/42] Change constants for 2025 season robot --- src/main/include/Constants.h | 21 ++++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index 58d5842..ffc5878 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -35,7 +35,12 @@ inline constexpr int kDriverControllerPort = 0; namespace DriveConstants { // Driving Parameters - Note that these are not the maximum capable speeds of // the robot, rather the allowed maximum speeds -constexpr units::meters_per_second_t kMaxSpeed = 4.8_mps; + +// Test sewrve +// constexpr units::meters_per_second_t kMaxSpeed = 4.8_mps; + +// Season robot +constexpr units::meters_per_second_t kMaxSpeed = 4.92_mps; constexpr units::radians_per_second_t kMaxAngularSpeed{2 * std::numbers::pi}; constexpr double kDirectionSlewRate = 1.2; // radians per second @@ -44,9 +49,9 @@ constexpr double kRotationalSlewRate = 2.0; // percent per second (1 = 100%) // Chassis configuration constexpr units::meter_t kTrackWidth = - 0.6731_m; // Distance between centers of right and left wheels on robot + 24_in; // Distance between centers of right and left wheels on robot constexpr units::meter_t kWheelBase = - 0.6731_m; // Distance between centers of front and back wheels on robot + 24_in; // Distance between centers of front and back wheels on robot // Angular offsets of the modules relative to the chassis in radians constexpr double kFrontLeftChassisAngularOffset = -std::numbers::pi / 2; @@ -70,7 +75,7 @@ namespace ModuleConstants { // The MAXSwerve module can be configured with one of three pinion gears: 12T, // 13T, or 14T. This changes the drive speed of the module (a pinion gear with // more teeth will result in a robot that drives faster). -constexpr int kDrivingMotorPinionTeeth = 14; +constexpr int kDrivingMotorPinionTeeth = 12; // Calculations required for driving motor conversion factors and feed forward constexpr double kDrivingMotorFreeSpeedRps = @@ -80,8 +85,14 @@ constexpr units::meter_t kWheelCircumference = kWheelDiameter * std::numbers::pi; // 45 teeth on the wheel's bevel gear, 21 teeth on the first-stage spur gear, 15 // teeth on the bevel pinion + +// Test swerve bot +// constexpr double kDrivingMotorReduction = +// (45.0 * 21) / (kDrivingMotorPinionTeeth * 15); + constexpr double kDrivingMotorReduction = - (45.0 * 21) / (kDrivingMotorPinionTeeth * 15); + (45.0 * 22.0) / (kDrivingMotorPinionTeeth * 15); + constexpr double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumference.value()) / kDrivingMotorReduction; From 477eeec9c35b749fb6260a0045b6e67c68188785 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Tue, 28 Jan 2025 19:31:22 -0600 Subject: [PATCH 16/42] Add common in gradle include directory --- SubZero-common | 2 +- build.gradle | 4 +- src/main/include/Constants.h | 33 ++++++++++ .../include/subsystems/ElevatorSubsystem.h | 65 +++++++++++++++++++ ....2.1.json => PathplannerLib-2025.2.2.json} | 8 +-- ...Lib-2025.0.1.json => REVLib-2025.0.2.json} | 12 ++-- 6 files changed, 111 insertions(+), 13 deletions(-) create mode 100644 src/main/include/subsystems/ElevatorSubsystem.h rename vendordeps/{PathplannerLib-2025.2.1.json => PathplannerLib-2025.2.2.json} (87%) rename vendordeps/{REVLib-2025.0.1.json => REVLib-2025.0.2.json} (90%) diff --git a/SubZero-common b/SubZero-common index 4c7c4c0..d8dc9cb 160000 --- a/SubZero-common +++ b/SubZero-common @@ -1 +1 @@ -Subproject commit 4c7c4c093292704d97a775358a5cbbccb991c06a +Subproject commit d8dc9cb4818f84de0d0b4f8bc58a1edde6b60778 diff --git a/build.gradle b/build.gradle index 7119ab2..43c7cb9 100644 --- a/build.gradle +++ b/build.gradle @@ -58,11 +58,11 @@ model { sources.cpp { source { - srcDir 'src/main/cpp' + srcDirs 'src/main/cpp', 'SubZero-common/lib/cpp' include '**/*.cpp', '**/*.cc' } exportedHeaders { - srcDir 'src/main/include' + srcDirs 'src/main/include', 'SubZero-common/lib/include', 'SubZero-common/lib/cpp' } } diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index ffc5878..7775aa0 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -10,6 +10,7 @@ #include #include #include +#include #include #include @@ -122,4 +123,36 @@ constexpr double kDriveDeadband = 0.05; namespace GyroConstants { constexpr units::time::second_t kPrintPeriod{1000_ms}; +} + +namespace ElevatorConstants { + // Placeholder value + const int kElevatorMotorCanId = -1; + + // Placeholder values + const double kElevatorP = -1.0; + const double kElevatorI = -1.0; + const double kElevatorD = -1.0; + const double kElevatorIZone = -1.0; + const double kElevatorFF = -1.0; + + // Placeholder value + constexpr units::revolutions_per_minute_t kMaxRpm = 5676_rpm; + + // Placeholder values + constexpr units::meter_t kMinDistance = 0_in; + constexpr units::meter_t kMaxDistance = 24_in; + constexpr units::meter_t kRelativeDistancePerRev = 0.1_in; // TODO do math to figure out value + constexpr units::meter_t kAbsoluteDistancePerRev = 0.1_in; + constexpr units::meters_per_second_t kDefaultVelocity = 1_mps; + constexpr double kVelocityScalar = 1.0; + constexpr units::meter_t kTolerance = 1_m; + + // Placeholder + subzero::SingleAxisMechanism kElevatorMechanism { + 24_in, + 0_deg, + 0.0, + frc::Color8Bit() + }; } \ No newline at end of file diff --git a/src/main/include/subsystems/ElevatorSubsystem.h b/src/main/include/subsystems/ElevatorSubsystem.h new file mode 100644 index 0000000..1799309 --- /dev/null +++ b/src/main/include/subsystems/ElevatorSubsystem.h @@ -0,0 +1,65 @@ +#pragma once + +#include +#include + +#include "Constants.h" + +class ElevatorSubsystem : LinearSingleAxisSubsystem { +public: + explicit ElevatorSubsystem() : LinearSingleAxisSubsystem{ + "Elevator Subsystem", + frc::RobotBase::IsReal() + ? dynamic_cast(armController) + : dynamic_cast(simArmController), + + { // Min distance + ElevatorConstants::kMinDistance, + // Max distance + ElevatorConstants::kMaxDistance, + // Distance per revolution of relative encoder + ElevatorConstants::kRelativeDistancePerRev, + // Distance per revolution of absolute encoder + ElevatorConstants::kAbsoluteDistancePerRev, + // Default velocity + ElevatorConstants::kDefaultVelocity, + // Velocity scalar + ElevatorConstants::kVelocityScalar, + // Tolerance + ElevatorConstants::kTolerance, + // Min limit switch + std::nullopt, + // Max limit switch + std::nullopt, + // Reversed + false, + // Mechanism2d + std::nullopt, + + [] {return false}, + + + } + } {} + +private: + rev::CANSparkMax m_motor{ + ElevatorConstants::kElevatorMotorCanId, + rev::CANSparkLowLevel::MotorType::kBrushless}; + + rev::SparkPIDController m_pidController = m_motor.GetPIDController(); + rev::SparkRelativeEncoder m_enc = m_motor.GetEncoder(); + rev::SparkAbsoluteEncoder m_absEnc = m_motor.GetAbsoluteEncoder(); + subzero::PidSettings elevatorPidSettings = { + ElevatorConstants::kElevatorP, ElevatorConstants::kElevatorI, ElevatorConstants::kElevatorD, + ElevatorConstants::kElevatorIZone, ArmConstants::kArmFF}; + SparkMaxController elevatorController{"Elevator", + m_motor, + m_enc, + m_PidController, + elevatorPidSettings, + &m_absEnc, + ElevatorConstants::kMaxRpm}; + subzero::SimPidMotorController simArmController{"Sim Elevator", elevatorPidSettings, + IntakingConstants::kMaxRpm}; +}; \ No newline at end of file diff --git a/vendordeps/PathplannerLib-2025.2.1.json b/vendordeps/PathplannerLib-2025.2.2.json similarity index 87% rename from vendordeps/PathplannerLib-2025.2.1.json rename to vendordeps/PathplannerLib-2025.2.2.json index 71e25f3..a5bf9ee 100644 --- a/vendordeps/PathplannerLib-2025.2.1.json +++ b/vendordeps/PathplannerLib-2025.2.2.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.2.1.json", + "fileName": "PathplannerLib-2025.2.2.json", "name": "PathplannerLib", - "version": "2025.2.1", + "version": "2025.2.2", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.2.1" + "version": "2025.2.2" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.2.1", + "version": "2025.2.2", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/REVLib-2025.0.1.json b/vendordeps/REVLib-2025.0.2.json similarity index 90% rename from vendordeps/REVLib-2025.0.1.json rename to vendordeps/REVLib-2025.0.2.json index c998054..c29aefa 100644 --- a/vendordeps/REVLib-2025.0.1.json +++ b/vendordeps/REVLib-2025.0.2.json @@ -1,7 +1,7 @@ { - "fileName": "REVLib-2025.0.1.json", + "fileName": "REVLib-2025.0.2.json", "name": "REVLib", - "version": "2025.0.1", + "version": "2025.0.2", "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2025.0.1" + "version": "2025.0.2" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.1", + "version": "2025.0.2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -36,7 +36,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2025.0.1", + "version": "2025.0.2", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -53,7 +53,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.1", + "version": "2025.0.2", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, From 4bff4d1fc074e135a757716ff6ce73596824f1c4 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Thu, 30 Jan 2025 18:03:06 -0600 Subject: [PATCH 17/42] Change `pidgey` to `m_pidgey` --- src/main/cpp/subsystems/DriveSubsystem.cpp | 26 ++++++++++---------- src/main/include/subsystems/DriveSubsystem.h | 2 +- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/main/cpp/subsystems/DriveSubsystem.cpp b/src/main/cpp/subsystems/DriveSubsystem.cpp index fafcbdd..f7cec5c 100644 --- a/src/main/cpp/subsystems/DriveSubsystem.cpp +++ b/src/main/cpp/subsystems/DriveSubsystem.cpp @@ -30,7 +30,7 @@ DriveSubsystem::DriveSubsystem() kRearRightChassisAngularOffset}, m_odometry{m_driveKinematics, frc::Rotation2d(units::radian_t{ - pidgey.GetYaw().GetValue()}), + m_pidgey.GetYaw().GetValue()}), {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), m_rearLeft.GetPosition(), m_rearRight.GetPosition()}, frc::Pose2d{}} { @@ -66,18 +66,18 @@ DriveSubsystem::DriveSubsystem() /* Configure Pigeon2 */ - pidgey.SetYaw(0_deg, 100_ms); // Set our yaw to 0 degrees and wait up to 100 milliseconds for the setter to take affect - 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 " << pidgey.GetYaw() << std::endl; + 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; } void DriveSubsystem::Periodic() { // Implementation of subsystem periodic method goes here. m_odometry.Update(frc::Rotation2d(units::radian_t{ - pidgey.GetYaw().GetValue()}), + m_pidgey.GetYaw().GetValue()}), {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), m_rearLeft.GetPosition(), m_rearRight.GetPosition()}); - auto &yaw = pidgey.GetYaw(); + auto &yaw = m_pidgey.GetYaw(); if (frc::Timer::GetFPGATimestamp() - currentTime >= GyroConstants::kPrintPeriod) { std::cout << "Yaw: " << yaw.GetValue().value() << std::endl; @@ -94,7 +94,7 @@ void DriveSubsystem::Periodic() { // /** // * Get the Gravity Vector Z component StatusSignalValue without refreshing // */ - // auto &gravityZ = pidgey.GetGravityVectorZ(false); + // auto &gravityZ = m_pidgey.GetGravityVectorZ(false); // /* This time wait for the signal to reduce latency */ // gravityZ.WaitForUpdate(GyroConstants::kPrintPeriod); // Wait up to our period // /** @@ -136,7 +136,7 @@ void DriveSubsystem::Drive(units::meters_per_second_t xSpeed, ? frc::ChassisSpeeds::FromFieldRelativeSpeeds( xSpeedDelivered, ySpeedDelivered, rotDelivered, frc::Rotation2d(units::radian_t{ - pidgey.GetYaw().GetValue()})) + m_pidgey.GetYaw().GetValue()})) : frc::ChassisSpeeds{xSpeedDelivered, ySpeedDelivered, rotDelivered}); m_driveKinematics.DesaturateWheelSpeeds(&states, DriveConstants::kMaxSpeed); @@ -193,14 +193,14 @@ void DriveSubsystem::ResetEncoders() { units::degree_t DriveSubsystem::GetHeading() { return frc::Rotation2d( - units::radian_t{pidgey.GetYaw().GetValue()}) + units::radian_t{m_pidgey.GetYaw().GetValue()}) .Degrees(); } -void DriveSubsystem::ZeroHeading() { pidgey.Reset(); } +void DriveSubsystem::ZeroHeading() { m_pidgey.Reset(); } double DriveSubsystem::GetTurnRate() { - return -pidgey.GetAngularVelocityZWorld().GetValue().value(); + return -m_pidgey.GetAngularVelocityZWorld().GetValue().value(); } frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); } @@ -214,9 +214,9 @@ void DriveSubsystem::ResetOdometry(frc::Pose2d pose) { } void DriveSubsystem::OffsetRotation(frc::Rotation2d offset) { - pidgey.SetYaw(offset.Degrees() + pidgey.GetYaw().GetValue()); + m_pidgey.SetYaw(offset.Degrees() + m_pidgey.GetYaw().GetValue()); m_odometry.ResetPosition( - pidgey.GetYaw().GetValue(), + m_pidgey.GetYaw().GetValue(), {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), m_rearLeft.GetPosition(), m_rearRight.GetPosition()}, m_odometry.GetPose()); diff --git a/src/main/include/subsystems/DriveSubsystem.h b/src/main/include/subsystems/DriveSubsystem.h index ed50c23..60972ac 100644 --- a/src/main/include/subsystems/DriveSubsystem.h +++ b/src/main/include/subsystems/DriveSubsystem.h @@ -128,7 +128,7 @@ class DriveSubsystem : public frc2::SubsystemBase { MAXSwerveModule m_rearRight; // Gyro Sensor - ctre::phoenix6::hardware::Pigeon2 pidgey{13, "rio"}; + ctre::phoenix6::hardware::Pigeon2 m_pidgey{13, "rio"}; units::time::second_t currentTime{frc::Timer::GetFPGATimestamp()}; // Odometry class for tracking robot pose From 37831be5fb631ee983c506949e76128c8af2111a Mon Sep 17 00:00:00 2001 From: 5690 <5690Programmers@gmail.com> Date: Fri, 31 Jan 2025 17:24:39 -0600 Subject: [PATCH 18/42] skeletonized arm subsytem and made a bunch of constants for it --- src/main/cpp/RobotContainer.cpp | 1 + src/main/include/constants/ArmConstants.h | 41 +++++++++++ .../include/subsystems/AlgeaArmSubsystem.h | 72 +++++++++++++++++++ 3 files changed, 114 insertions(+) create mode 100644 src/main/include/constants/ArmConstants.h create mode 100644 src/main/include/subsystems/AlgeaArmSubsystem.h diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 4300501..14ddc85 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -8,6 +8,7 @@ #include "commands/Autos.h" #include "commands/ExampleCommand.h" +#include "subsystems/AlgeaArmSubsystem.h" RobotContainer::RobotContainer() { // Initialize all of your commands and subsystems here diff --git a/src/main/include/constants/ArmConstants.h b/src/main/include/constants/ArmConstants.h new file mode 100644 index 0000000..8b87e3c --- /dev/null +++ b/src/main/include/constants/ArmConstants.h @@ -0,0 +1,41 @@ +#pragma once +#include +#include +#include +#include +//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! +//ALL VALUES ARE CURRENTLY PLACE HOLDERS FOR KNOW!!! +//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + +namespace AlgaeArmConstants{ + constexpr int KArmMotorId = -1; + constexpr double kArmP = -1; + constexpr double kArmI = -1; + constexpr double kArmD = -1; + constexpr double kArmIZone = -1; + constexpr double kArmFF = -1; + + constexpr units::revolutions_per_minute_t kMaxRpm = 1_rpm; + constexpr units::degree_t kHomeRotation = 10_deg; + constexpr units::degree_t kMaxRotation = 190_deg; + constexpr units::degree_t kArmRelativeDistancePerRev = 360_deg * (1 / 8.75); + constexpr units::degree_t kArmAbsoluteDistancePerRev = 360_deg; + constexpr units::degrees_per_second_t kDefaultVelocity = 10_deg_per_s; + constexpr double kVelocityScalar = 1.0; + constexpr units::degree_t kTolerance = 2_deg; + constexpr units::meter_t kArmLength = 0.2_m; + + static const subzero::SingleAxisMechanism kArmMechanism = { + // length + 0.2_m, + // min angle + 110_deg, + // line width + 6, + // color + subzero::ColorConstants::kBlue}; + + const frc::TrapezoidProfile::Constraints + kRotationalAxisConstraints{360_deg_per_s * 2.2, 360_deg_per_s_sq * 2.2}; +} + diff --git a/src/main/include/subsystems/AlgeaArmSubsystem.h b/src/main/include/subsystems/AlgeaArmSubsystem.h new file mode 100644 index 0000000..6b49254 --- /dev/null +++ b/src/main/include/subsystems/AlgeaArmSubsystem.h @@ -0,0 +1,72 @@ +#pragma once + +#include +#include +#include +#include +#include "constants/ArmConstants.h" +#include "Constants.h" + +class ArmSubsystem : public RotationalSingleAxisSubsystem { + public: + explicit ArmSubsystem(frc::MechanismObject2d* node = nullptr) + : RotationalSingleAxisSubsystem{ + "Arm", + frc::RobotBase::IsReal() + ? dynamic_cast(armController) + : dynamic_cast(simArmController), + { + AlgeaArmConstants::kHomeRotation, + // Max distance + AlgeaArmConstants::kMaxRotation, + // Distance per revolution of relative encoder + AlgeaArmConstants::kArmRelativeDistancePerRev, + // Distance per revolution of absolute encoder + AlgeaArmConstants::kArmAbsoluteDistancePerRev, + // Default velocity + AlgeaArmConstants::kDefaultVelocity, + // Velocity scalar + AlgeaArmConstants::kVelocityScalar, + // Tolerance + AlgeaArmConstants::kTolerance, + // Min limit switch + std::nullopt, + // Max limit switch + std::nullopt, + // Reversed + true, + // Mechanism2d + AlgeaArmConstants::kArmMechanism, + // Conversion Function + std::nullopt, + + [] { return false; }, AlgeaArmConstants::kRotationalAxisConstraints}, + AlgeaArmConstants::kArmLength, + node} { + m_Motor.SetIdleMode(rev::CANSparkBase::IdleMode::kBrake); + + + + } + + + + private: + rev::CANSparkMax m_Motor{AlgeaArmConstants::kArmMotorId, + rev::CANSparkLowLevel::MotorType::kBrushless}; + rev::SparkPIDController m_PidController = m_Motor.GetPIDController(); + rev::SparkRelativeEncoder m_enc = m_Motor.GetEncoder(); + rev::SparkAbsoluteEncoder m_absEnc = m_Motor.GetAbsoluteEncoder(); + subzero::PidSettings armPidSettings = { + AlgaeArmConstants::kArmP, AlgaeArmConstants::kArmI, AlgaeArmConstants::kArmD, + AlgaeArmConstants::kArmIZone, AlgeaArmConstants::kArmFF}; + SparkMaxController armController{"Arm", + m_Motor, + m_enc, + m_PidController, + armPidSettings, + &m_absEnc, + AlgaeArmConstants::kMaxRpm}; + subzero::SimPidMotorController simArmController{"Sim Arm", armPidSettings, + AlgaeArmConstants::kMaxRpm}; +}; \ No newline at end of file From 4c6ec4bd54a9405c18b8a9e70a767f0c25027bf0 Mon Sep 17 00:00:00 2001 From: 5690 <5690Programmers@gmail.com> Date: Mon, 3 Feb 2025 17:30:50 -0600 Subject: [PATCH 19/42] updated rev names and added the fixed common plus fixed dumb spelling mistake(algea -> algae) --- SubZero-common | 2 +- build.gradle | 5 +- src/main/cpp/RobotContainer.cpp | 2 +- src/main/include/Constants.h | 14 +++++- src/main/include/constants/ArmConstants.h | 5 +- ...lgeaArmSubsystem.h => AlgaeArmSubsystem.h} | 47 ++++++++++--------- ....2.1.json => PathplannerLib-2025.2.2.json} | 8 ++-- ...Lib-2025.0.1.json => REVLib-2025.0.2.json} | 12 ++--- 8 files changed, 56 insertions(+), 39 deletions(-) rename src/main/include/subsystems/{AlgeaArmSubsystem.h => AlgaeArmSubsystem.h} (50%) rename vendordeps/{PathplannerLib-2025.2.1.json => PathplannerLib-2025.2.2.json} (87%) rename vendordeps/{REVLib-2025.0.1.json => REVLib-2025.0.2.json} (90%) diff --git a/SubZero-common b/SubZero-common index 4c7c4c0..3372753 160000 --- a/SubZero-common +++ b/SubZero-common @@ -1 +1 @@ -Subproject commit 4c7c4c093292704d97a775358a5cbbccb991c06a +Subproject commit 3372753254e5a39e024b0d9feb051faead8b01ba diff --git a/build.gradle b/build.gradle index 7119ab2..2eda0b8 100644 --- a/build.gradle +++ b/build.gradle @@ -58,11 +58,12 @@ model { sources.cpp { source { - srcDir 'src/main/cpp' + srcDirs 'src/main/cpp', 'SubZero-common/lib/cpp' include '**/*.cpp', '**/*.cc' + } exportedHeaders { - srcDir 'src/main/include' + srcDirs 'src/main/include', 'SubZero-common/lib/include', 'SubZero-common/lib/cpp' } } diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 9cd6390..48fc324 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -21,7 +21,7 @@ #include "commands/Autos.h" #include "commands/ExampleCommand.h" -#include "subsystems/AlgeaArmSubsystem.h" +#include "subsystems/AlgaeArmSubsystem.h" RobotContainer::RobotContainer() { // Initialize all of your commands and subsystems here diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index ffc5878..d6f2a11 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -10,6 +10,7 @@ #include #include #include +#include #include #include @@ -30,7 +31,16 @@ inline constexpr int kDriverControllerPort = 0; } // namespace OperatorConstants - +typedef + subzero::PidMotorController SparkMaxController; +typedef + subzero::PidMotorController SparkFlexController; namespace DriveConstants { // Driving Parameters - Note that these are not the maximum capable speeds of @@ -86,6 +96,8 @@ constexpr units::meter_t kWheelCircumference = // 45 teeth on the wheel's bevel gear, 21 teeth on the first-stage spur gear, 15 // teeth on the bevel pinion + + // Test swerve bot // constexpr double kDrivingMotorReduction = // (45.0 * 21) / (kDrivingMotorPinionTeeth * 15); diff --git a/src/main/include/constants/ArmConstants.h b/src/main/include/constants/ArmConstants.h index 8b87e3c..9e0235e 100644 --- a/src/main/include/constants/ArmConstants.h +++ b/src/main/include/constants/ArmConstants.h @@ -7,8 +7,10 @@ //ALL VALUES ARE CURRENTLY PLACE HOLDERS FOR KNOW!!! //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + + namespace AlgaeArmConstants{ - constexpr int KArmMotorId = -1; + constexpr int kArmMotorId = -1; constexpr double kArmP = -1; constexpr double kArmI = -1; constexpr double kArmD = -1; @@ -37,5 +39,6 @@ namespace AlgaeArmConstants{ const frc::TrapezoidProfile::Constraints kRotationalAxisConstraints{360_deg_per_s * 2.2, 360_deg_per_s_sq * 2.2}; + } diff --git a/src/main/include/subsystems/AlgeaArmSubsystem.h b/src/main/include/subsystems/AlgaeArmSubsystem.h similarity index 50% rename from src/main/include/subsystems/AlgeaArmSubsystem.h rename to src/main/include/subsystems/AlgaeArmSubsystem.h index 6b49254..7a6fd62 100644 --- a/src/main/include/subsystems/AlgeaArmSubsystem.h +++ b/src/main/include/subsystems/AlgaeArmSubsystem.h @@ -1,34 +1,35 @@ #pragma once #include +#include #include #include #include #include "constants/ArmConstants.h" #include "Constants.h" -class ArmSubsystem : public RotationalSingleAxisSubsystem { +class AlgaeArmSubsystem : public subzero::RotationalSingleAxisSubsystem { public: - explicit ArmSubsystem(frc::MechanismObject2d* node = nullptr) - : RotationalSingleAxisSubsystem{ + explicit AlgaeArmSubsystem(frc::MechanismObject2d* node = nullptr) + : subzero::RotationalSingleAxisSubsystem{ "Arm", frc::RobotBase::IsReal() - ? dynamic_cast(armController) - : dynamic_cast(simArmController), + ? dynamic_cast(armController) + : dynamic_cast(simArmController), { - AlgeaArmConstants::kHomeRotation, + AlgaeArmConstants::kHomeRotation, // Max distance - AlgeaArmConstants::kMaxRotation, + AlgaeArmConstants::kMaxRotation, // Distance per revolution of relative encoder - AlgeaArmConstants::kArmRelativeDistancePerRev, + AlgaeArmConstants::kArmRelativeDistancePerRev, // Distance per revolution of absolute encoder - AlgeaArmConstants::kArmAbsoluteDistancePerRev, + AlgaeArmConstants::kArmAbsoluteDistancePerRev, // Default velocity - AlgeaArmConstants::kDefaultVelocity, + AlgaeArmConstants::kDefaultVelocity, // Velocity scalar - AlgeaArmConstants::kVelocityScalar, + AlgaeArmConstants::kVelocityScalar, // Tolerance - AlgeaArmConstants::kTolerance, + AlgaeArmConstants::kTolerance, // Min limit switch std::nullopt, // Max limit switch @@ -36,14 +37,14 @@ class ArmSubsystem : public RotationalSingleAxisSubsystem { // Reversed true, // Mechanism2d - AlgeaArmConstants::kArmMechanism, + AlgaeArmConstants::kArmMechanism, // Conversion Function std::nullopt, - [] { return false; }, AlgeaArmConstants::kRotationalAxisConstraints}, - AlgeaArmConstants::kArmLength, - node} { - m_Motor.SetIdleMode(rev::CANSparkBase::IdleMode::kBrake); + [] { return false; }, AlgaeArmConstants::kRotationalAxisConstraints}, + AlgaeArmConstants::kArmLength, + nullptr} { + //m_Motor.SetIdleMode(rev::spark::SparkBase::IdleMode::kBrake); @@ -52,14 +53,14 @@ class ArmSubsystem : public RotationalSingleAxisSubsystem { private: - rev::CANSparkMax m_Motor{AlgeaArmConstants::kArmMotorId, - rev::CANSparkLowLevel::MotorType::kBrushless}; - rev::SparkPIDController m_PidController = m_Motor.GetPIDController(); - rev::SparkRelativeEncoder m_enc = m_Motor.GetEncoder(); - rev::SparkAbsoluteEncoder m_absEnc = m_Motor.GetAbsoluteEncoder(); + rev::spark::SparkMax m_Motor{AlgaeArmConstants::kArmMotorId, + rev::spark::SparkLowLevel::MotorType::kBrushless}; + rev::spark::SparkClosedLoopController m_PidController = m_Motor.GetClosedLoopController(); + rev::spark::SparkRelativeEncoder m_enc = m_Motor.GetEncoder(); + rev::spark::SparkAbsoluteEncoder m_absEnc = m_Motor.GetAbsoluteEncoder(); subzero::PidSettings armPidSettings = { AlgaeArmConstants::kArmP, AlgaeArmConstants::kArmI, AlgaeArmConstants::kArmD, - AlgaeArmConstants::kArmIZone, AlgeaArmConstants::kArmFF}; + AlgaeArmConstants::kArmIZone, AlgaeArmConstants::kArmFF}; SparkMaxController armController{"Arm", m_Motor, m_enc, diff --git a/vendordeps/PathplannerLib-2025.2.1.json b/vendordeps/PathplannerLib-2025.2.2.json similarity index 87% rename from vendordeps/PathplannerLib-2025.2.1.json rename to vendordeps/PathplannerLib-2025.2.2.json index 71e25f3..a5bf9ee 100644 --- a/vendordeps/PathplannerLib-2025.2.1.json +++ b/vendordeps/PathplannerLib-2025.2.2.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.2.1.json", + "fileName": "PathplannerLib-2025.2.2.json", "name": "PathplannerLib", - "version": "2025.2.1", + "version": "2025.2.2", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.2.1" + "version": "2025.2.2" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.2.1", + "version": "2025.2.2", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/REVLib-2025.0.1.json b/vendordeps/REVLib-2025.0.2.json similarity index 90% rename from vendordeps/REVLib-2025.0.1.json rename to vendordeps/REVLib-2025.0.2.json index c998054..c29aefa 100644 --- a/vendordeps/REVLib-2025.0.1.json +++ b/vendordeps/REVLib-2025.0.2.json @@ -1,7 +1,7 @@ { - "fileName": "REVLib-2025.0.1.json", + "fileName": "REVLib-2025.0.2.json", "name": "REVLib", - "version": "2025.0.1", + "version": "2025.0.2", "frcYear": "2025", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2025.0.1" + "version": "2025.0.2" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.1", + "version": "2025.0.2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -36,7 +36,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2025.0.1", + "version": "2025.0.2", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -53,7 +53,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2025.0.1", + "version": "2025.0.2", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, From 3c1cd42d3a40b7819ad2c2ed583323ec2ad0d956 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Mon, 3 Feb 2025 19:26:46 -0600 Subject: [PATCH 20/42] Finish elevator subsystem skeleton --- .vscode/settings.json | 5 ++- SubZero-common | 2 +- src/main/cpp/RobotContainer.cpp | 2 + src/main/include/Constants.h | 16 ++++++- .../include/subsystems/ElevatorSubsystem.h | 44 +++++++++++-------- 5 files changed, 46 insertions(+), 23 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 5705711..ce2f691 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -93,6 +93,9 @@ "typeindex": "cpp", "typeinfo": "cpp", "valarray": "cpp", - "variant": "cpp" + "variant": "cpp", + "xmemory": "cpp", + "xstring": "cpp", + "xutility": "cpp" } } diff --git a/SubZero-common b/SubZero-common index d8dc9cb..bf48188 160000 --- a/SubZero-common +++ b/SubZero-common @@ -1 +1 @@ -Subproject commit d8dc9cb4818f84de0d0b4f8bc58a1edde6b60778 +Subproject commit bf48188e1be668ccb1a4b3681096bd9b3aa27ac1 diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index af5824b..2045fe2 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -17,6 +17,8 @@ #include #include #include +#include + #include #include "commands/Autos.h" diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index 7775aa0..2d65b43 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -2,6 +2,8 @@ // 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. +#pragma once + #include #include #include @@ -11,11 +13,19 @@ #include #include #include +#include #include #include -#pragma once +typedef + subzero::PidMotorController SparkMaxPidController; +typedef + subzero::PidMotorController SparkFlexController; /** * The Constants header provides a convenient place for teams to hold robot-wide @@ -149,10 +159,12 @@ namespace ElevatorConstants { constexpr units::meter_t kTolerance = 1_m; // Placeholder - subzero::SingleAxisMechanism kElevatorMechanism { + static const subzero::SingleAxisMechanism kElevatorMechanism { 24_in, 0_deg, 0.0, frc::Color8Bit() }; + + const frc::TrapezoidProfile::Constraints kElevatorProfileConstraints{}; } \ No newline at end of file diff --git a/src/main/include/subsystems/ElevatorSubsystem.h b/src/main/include/subsystems/ElevatorSubsystem.h index 1799309..e878453 100644 --- a/src/main/include/subsystems/ElevatorSubsystem.h +++ b/src/main/include/subsystems/ElevatorSubsystem.h @@ -1,18 +1,22 @@ #pragma once #include + +#include + +#include #include +#include +#include #include "Constants.h" -class ElevatorSubsystem : LinearSingleAxisSubsystem { +class ElevatorSubsystem : subzero::LinearSingleAxisSubsystem { public: - explicit ElevatorSubsystem() : LinearSingleAxisSubsystem{ + explicit ElevatorSubsystem() + : subzero::LinearSingleAxisSubsystem{ "Elevator Subsystem", - frc::RobotBase::IsReal() - ? dynamic_cast(armController) - : dynamic_cast(simArmController), - + elevatorController, { // Min distance ElevatorConstants::kMinDistance, // Max distance @@ -34,32 +38,34 @@ class ElevatorSubsystem : LinearSingleAxisSubsystem { // Reversed false, // Mechanism2d + ElevatorConstants::kElevatorMechanism, + std::nullopt, - [] {return false}, + []() { return false; }, - - } + ElevatorConstants::kElevatorProfileConstraints + } } {} private: - rev::CANSparkMax m_motor{ + rev::spark::SparkMax m_motor{ ElevatorConstants::kElevatorMotorCanId, - rev::CANSparkLowLevel::MotorType::kBrushless}; - - rev::SparkPIDController m_pidController = m_motor.GetPIDController(); - rev::SparkRelativeEncoder m_enc = m_motor.GetEncoder(); - rev::SparkAbsoluteEncoder m_absEnc = m_motor.GetAbsoluteEncoder(); + rev::spark::SparkLowLevel::MotorType::kBrushless}; + rev::spark::SparkClosedLoopController m_pidController = m_motor.GetClosedLoopController(); + rev::spark::SparkRelativeEncoder m_enc = m_motor.GetEncoder(); + rev::spark::SparkAbsoluteEncoder m_absEnc = m_motor.GetAbsoluteEncoder(); subzero::PidSettings elevatorPidSettings = { ElevatorConstants::kElevatorP, ElevatorConstants::kElevatorI, ElevatorConstants::kElevatorD, - ElevatorConstants::kElevatorIZone, ArmConstants::kArmFF}; - SparkMaxController elevatorController{"Elevator", + ElevatorConstants::kElevatorIZone, ElevatorConstants::kElevatorFF, true}; + SparkMaxPidController elevatorController{"Elevator", m_motor, m_enc, - m_PidController, + m_pidController, elevatorPidSettings, &m_absEnc, ElevatorConstants::kMaxRpm}; + subzero::SimPidMotorController simArmController{"Sim Elevator", elevatorPidSettings, - IntakingConstants::kMaxRpm}; + ElevatorConstants::kMaxRpm}; }; \ No newline at end of file From 0b5340c1bd9b1884aa94f749601c50d7ebad9b6b Mon Sep 17 00:00:00 2001 From: 5690 <5690Programmers@gmail.com> Date: Tue, 4 Feb 2025 16:28:44 -0600 Subject: [PATCH 21/42] changed member and const names to avoid cofusion --- src/main/include/constants/ArmConstants.h | 18 +++++----- .../include/subsystems/AlgaeArmSubsystem.h | 34 +++++++++---------- 2 files changed, 26 insertions(+), 26 deletions(-) diff --git a/src/main/include/constants/ArmConstants.h b/src/main/include/constants/ArmConstants.h index 9e0235e..fe50041 100644 --- a/src/main/include/constants/ArmConstants.h +++ b/src/main/include/constants/ArmConstants.h @@ -10,24 +10,24 @@ namespace AlgaeArmConstants{ - constexpr int kArmMotorId = -1; - constexpr double kArmP = -1; - constexpr double kArmI = -1; - constexpr double kArmD = -1; - constexpr double kArmIZone = -1; - constexpr double kArmFF = -1; + constexpr int kMotorId = -1; + constexpr double kP = -1; + constexpr double kI = -1; + constexpr double kD = -1; + constexpr double kIZone = -1; + constexpr double kFF = -1; constexpr units::revolutions_per_minute_t kMaxRpm = 1_rpm; constexpr units::degree_t kHomeRotation = 10_deg; constexpr units::degree_t kMaxRotation = 190_deg; - constexpr units::degree_t kArmRelativeDistancePerRev = 360_deg * (1 / 8.75); - constexpr units::degree_t kArmAbsoluteDistancePerRev = 360_deg; + constexpr units::degree_t kRelativeDistancePerRev = 360_deg * (1 / 8.75); + constexpr units::degree_t kAbsoluteDistancePerRev = 360_deg; constexpr units::degrees_per_second_t kDefaultVelocity = 10_deg_per_s; constexpr double kVelocityScalar = 1.0; constexpr units::degree_t kTolerance = 2_deg; constexpr units::meter_t kArmLength = 0.2_m; - static const subzero::SingleAxisMechanism kArmMechanism = { + static const subzero::SingleAxisMechanism kAlgaeArmMechanism = { // length 0.2_m, // min angle diff --git a/src/main/include/subsystems/AlgaeArmSubsystem.h b/src/main/include/subsystems/AlgaeArmSubsystem.h index 7a6fd62..c380eb0 100644 --- a/src/main/include/subsystems/AlgaeArmSubsystem.h +++ b/src/main/include/subsystems/AlgaeArmSubsystem.h @@ -14,16 +14,16 @@ class AlgaeArmSubsystem : public subzero::RotationalSingleAxisSubsystem{ "Arm", frc::RobotBase::IsReal() - ? dynamic_cast(armController) - : dynamic_cast(simArmController), + ? dynamic_cast(algaeArmController) + : dynamic_cast(simAlgaeArmController), { AlgaeArmConstants::kHomeRotation, // Max distance AlgaeArmConstants::kMaxRotation, // Distance per revolution of relative encoder - AlgaeArmConstants::kArmRelativeDistancePerRev, + AlgaeArmConstants::kRelativeDistancePerRev, // Distance per revolution of absolute encoder - AlgaeArmConstants::kArmAbsoluteDistancePerRev, + AlgaeArmConstants::kAbsoluteDistancePerRev, // Default velocity AlgaeArmConstants::kDefaultVelocity, // Velocity scalar @@ -37,7 +37,7 @@ class AlgaeArmSubsystem : public subzero::RotationalSingleAxisSubsystem Date: Tue, 4 Feb 2025 19:16:40 -0600 Subject: [PATCH 22/42] Instantiate elevator subsystem (linker error) --- src/main/cpp/RobotContainer.cpp | 1 - src/main/include/Constants.h | 6 ++--- src/main/include/RobotContainer.h | 3 +++ .../include/subsystems/ElevatorSubsystem.h | 26 ++++++++++--------- 4 files changed, 20 insertions(+), 16 deletions(-) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 2045fe2..86a43b0 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -17,7 +17,6 @@ #include #include #include -#include #include diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index 2d65b43..f927cc3 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -18,10 +18,10 @@ #include #include -typedef +using SparkMaxPidController = subzero::PidMotorController SparkMaxPidController; + rev::spark::SparkAbsoluteEncoder, rev::spark::SparkMaxConfig> ; typedef subzero::PidMotorController::Constraints kElevatorProfileConstraints{}; + extern const frc::TrapezoidProfile::Constraints kElevatorProfileConstraints{}; } \ No newline at end of file diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index bf580e1..d487f53 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -18,6 +18,7 @@ #include "Constants.h" #include "subsystems/DriveSubsystem.h" #include "subsystems/ExampleSubsystem.h" +#include "subsystems/ElevatorSubsystem.h" /** * This class is where the bulk of the robot should be declared. Since @@ -46,4 +47,6 @@ class RobotContainer { frc::SendableChooser m_chooser; std::string m_autoSelected; + + ElevatorSubsystem m_elevator; }; diff --git a/src/main/include/subsystems/ElevatorSubsystem.h b/src/main/include/subsystems/ElevatorSubsystem.h index e878453..4fdaa53 100644 --- a/src/main/include/subsystems/ElevatorSubsystem.h +++ b/src/main/include/subsystems/ElevatorSubsystem.h @@ -11,13 +11,13 @@ #include "Constants.h" -class ElevatorSubsystem : subzero::LinearSingleAxisSubsystem { +class ElevatorSubsystem : public subzero::LinearSingleAxisSubsystem { public: explicit ElevatorSubsystem() : subzero::LinearSingleAxisSubsystem{ "Elevator Subsystem", - elevatorController, - { // Min distance + dynamic_cast(m_elevatorController), + { // Min distance ElevatorConstants::kMinDistance, // Max distance ElevatorConstants::kMaxDistance, @@ -39,13 +39,15 @@ class ElevatorSubsystem : subzero::LinearSingleAxisSubsystem Date: Thu, 6 Feb 2025 18:18:01 -0600 Subject: [PATCH 23/42] added intake motor --- src/main/include/RobotContainer.h | 1 + src/main/include/constants/ArmConstants.h | 3 +- .../include/subsystems/AlgaeArmSubsystem.h | 28 +++++++++++++++---- 3 files changed, 25 insertions(+), 7 deletions(-) diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index bf580e1..4133696 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -18,6 +18,7 @@ #include "Constants.h" #include "subsystems/DriveSubsystem.h" #include "subsystems/ExampleSubsystem.h" +#include "subsystems/AlgaeArmSubsystem.h" /** * This class is where the bulk of the robot should be declared. Since diff --git a/src/main/include/constants/ArmConstants.h b/src/main/include/constants/ArmConstants.h index fe50041..458b176 100644 --- a/src/main/include/constants/ArmConstants.h +++ b/src/main/include/constants/ArmConstants.h @@ -10,7 +10,8 @@ namespace AlgaeArmConstants{ - constexpr int kMotorId = -1; + constexpr int kArmMotorId = -1; + constexpr int kIntakeMotorId = 20; constexpr double kP = -1; constexpr double kI = -1; constexpr double kD = -1; diff --git a/src/main/include/subsystems/AlgaeArmSubsystem.h b/src/main/include/subsystems/AlgaeArmSubsystem.h index c380eb0..3b26319 100644 --- a/src/main/include/subsystems/AlgaeArmSubsystem.h +++ b/src/main/include/subsystems/AlgaeArmSubsystem.h @@ -5,6 +5,7 @@ #include #include #include + #include "constants/ArmConstants.h" #include "Constants.h" @@ -45,24 +46,38 @@ class AlgaeArmSubsystem : public subzero::RotationalSingleAxisSubsystem Date: Thu, 6 Feb 2025 18:48:28 -0600 Subject: [PATCH 24/42] commited plagerism of myself (added coral arm skeleton) --- src/main/include/constants/ArmConstants.h | 34 +++++++ .../include/subsystems/CoralArmSubsystem.h | 90 +++++++++++++++++++ 2 files changed, 124 insertions(+) create mode 100644 src/main/include/subsystems/CoralArmSubsystem.h diff --git a/src/main/include/constants/ArmConstants.h b/src/main/include/constants/ArmConstants.h index 458b176..789fde6 100644 --- a/src/main/include/constants/ArmConstants.h +++ b/src/main/include/constants/ArmConstants.h @@ -43,3 +43,37 @@ namespace AlgaeArmConstants{ } +namespace CoralArmConstants{ + constexpr int kArmMotorId = -1; + constexpr int kIntakeMotorId = 20; + constexpr double kP = -1; + constexpr double kI = -1; + constexpr double kD = -1; + constexpr double kIZone = -1; + constexpr double kFF = -1; + + constexpr units::revolutions_per_minute_t kMaxRpm = 1_rpm; + constexpr units::degree_t kHomeRotation = 10_deg; + constexpr units::degree_t kMaxRotation = 190_deg; + constexpr units::degree_t kRelativeDistancePerRev = 360_deg * (1 / 8.75); + constexpr units::degree_t kAbsoluteDistancePerRev = 360_deg; + constexpr units::degrees_per_second_t kDefaultVelocity = 10_deg_per_s; + constexpr double kVelocityScalar = 1.0; + constexpr units::degree_t kTolerance = 2_deg; + constexpr units::meter_t kArmLength = 0.2_m; + + static const subzero::SingleAxisMechanism kCoralArmMechanism = { + // length + 0.2_m, + // min angle + 110_deg, + // line width + 6, + // color + subzero::ColorConstants::kBlue}; + + const frc::TrapezoidProfile::Constraints + kRotationalAxisConstraints{360_deg_per_s * 2.2, 360_deg_per_s_sq * 2.2}; + +} + diff --git a/src/main/include/subsystems/CoralArmSubsystem.h b/src/main/include/subsystems/CoralArmSubsystem.h new file mode 100644 index 0000000..2f3b7a1 --- /dev/null +++ b/src/main/include/subsystems/CoralArmSubsystem.h @@ -0,0 +1,90 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "constants/ArmConstants.h" +#include "Constants.h" + +//pretty much a copy paste of algae arm subsystem with differet names right now, subject to change later + +class CoralArmSubsystem : public subzero::RotationalSingleAxisSubsystem { + public: + explicit CoralArmSubsystem(frc::MechanismObject2d* node = nullptr) + : subzero::RotationalSingleAxisSubsystem{ + "Arm", + frc::RobotBase::IsReal() + ? dynamic_cast(coralArmController) + : dynamic_cast(simCoralArmController), + { + CoralArmConstants::kHomeRotation, + // Max distance + CoralArmConstants::kMaxRotation, + // Distance per revolution of relative encoder + CoralArmConstants::kRelativeDistancePerRev, + // Distance per revolution of absolute encoder + CoralArmConstants::kAbsoluteDistancePerRev, + // Default velocity + CoralArmConstants::kDefaultVelocity, + // Velocity scalar + CoralArmConstants::kVelocityScalar, + // Tolerance + CoralArmConstants::kTolerance, + // Min limit switch + std::nullopt, + // Max limit switch + std::nullopt, + // Reversed + true, + // Mechanism2d + CoralArmConstants::kCoralArmMechanism, + // Conversion Function + std::nullopt, + + [] { return false; }, CoralArmConstants::kRotationalAxisConstraints}, + CoralArmConstants::kArmLength, + nullptr} { + //m_Motor.SetIdleMode(rev::spark::SparkBase::IdleMode::kBrake); + } + + void Periodic() override; + + void SimulationPeriodic() override; + + void Stop(); + + void In(double); + + + void Out(double); + + + + + private: + rev::spark::SparkMax m_intakeMotor{ + CoralArmConstants::kIntakeMotorId, + rev::spark::SparkLowLevel::MotorType::kBrushless}; + + rev::spark::SparkMax m_armMotor{CoralArmConstants::kArmMotorId, + rev::spark::SparkLowLevel::MotorType::kBrushless}; + rev::spark::SparkClosedLoopController m_pidController = m_armMotor.GetClosedLoopController(); + rev::spark::SparkRelativeEncoder m_enc = m_armMotor.GetEncoder(); + rev::spark::SparkAbsoluteEncoder m_absEnc = m_armMotor.GetAbsoluteEncoder(); + subzero::PidSettings coralArmPidSettings = { + CoralArmConstants::kP, CoralArmConstants::kI, CoralArmConstants::kD, + CoralArmConstants::kIZone, CoralArmConstants::kFF}; + SparkMaxController coralArmController{"Arm", + m_armMotor, + m_enc, + m_pidController, + coralArmPidSettings, + &m_absEnc, + CoralArmConstants::kMaxRpm}; + subzero::SimPidMotorController simCoralArmController{"Sim Arm", coralArmPidSettings, + CoralArmConstants::kMaxRpm}; + +}; \ No newline at end of file From 6845cd5ba873f0441f98ada26565cf1153cae00d Mon Sep 17 00:00:00 2001 From: 5690 <5690Programmers@gmail.com> Date: Thu, 6 Feb 2025 18:54:29 -0600 Subject: [PATCH 25/42] =?UTF-8?q?i=20forgor=20=F0=9F=92=80?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/cpp/RobotContainer.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 48fc324..86bb36e 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -22,6 +22,7 @@ #include "commands/Autos.h" #include "commands/ExampleCommand.h" #include "subsystems/AlgaeArmSubsystem.h" +#include "subsystems/CoralArmSubsystem.h" RobotContainer::RobotContainer() { // Initialize all of your commands and subsystems here From b0b9d75d9189372b00d7b45365b1f1a8aa73565d Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Sat, 8 Feb 2025 11:51:32 -0600 Subject: [PATCH 26/42] Inlucde cpp files to fix linker error --- src/main/include/Constants.h | 4 ++-- src/main/include/subsystems/ElevatorSubsystem.h | 3 +++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index f927cc3..71eeca5 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -159,12 +159,12 @@ namespace ElevatorConstants { constexpr units::meter_t kTolerance = 1_m; // Placeholder - static const subzero::SingleAxisMechanism kElevatorMechanism { + const subzero::SingleAxisMechanism kElevatorMechanism { 24_in, 0_deg, 0.0, frc::Color8Bit() }; - extern const frc::TrapezoidProfile::Constraints kElevatorProfileConstraints{}; + const frc::TrapezoidProfile::Constraints kElevatorProfileConstraints{}; } \ No newline at end of file diff --git a/src/main/include/subsystems/ElevatorSubsystem.h b/src/main/include/subsystems/ElevatorSubsystem.h index 4fdaa53..87f8e5c 100644 --- a/src/main/include/subsystems/ElevatorSubsystem.h +++ b/src/main/include/subsystems/ElevatorSubsystem.h @@ -8,6 +8,9 @@ #include #include #include +#include +#include +#include #include "Constants.h" From c8860d67073475acfbf7aa54fa66b717f085a4c7 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Sat, 8 Feb 2025 12:26:37 -0600 Subject: [PATCH 27/42] Use sim mechanism for elevator (not working) --- simgui-ds.json | 6 ++++++ src/main/cpp/RobotContainer.cpp | 21 ++++++++++++++++--- src/main/include/Constants.h | 13 ++++++------ src/main/include/RobotContainer.h | 3 ++- .../include/subsystems/ElevatorSubsystem.h | 8 ++++--- 5 files changed, 38 insertions(+), 13 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..c4b7efd 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -88,5 +88,11 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + } ] } diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 86a43b0..b61a0af 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -53,9 +53,24 @@ RobotContainer::RobotContainer() { } void RobotContainer::ConfigureBindings() { - frc2::JoystickButton(&m_driverController, - frc::XboxController::Button::kRightBumper) - .WhileTrue(new frc2::RunCommand([this] { m_drive.SetX(); }, {&m_drive})); + // m_drive.SetDefaultCommand(frc2::RunCommand( + // [this] { + // InputUtils::DeadzoneAxes axes = InputUtils::CalculateCircularDeadzone( + // m_driverController.GetLeftX(), m_driverController.GetLeftY(), + // OIConstants::kDriveDeadband); + + // ITurnToTarget* turnToTarget = m_shouldAim ? &m_turnToPose : nullptr; + + // m_drive.Drive( + // -units::meters_per_second_t{axes.y}, + // -units::meters_per_second_t{axes.x}, + // -units::radians_per_second_t{frc::ApplyDeadband( + // m_driverController.GetRightX(), OIConstants::kDriveDeadband)}, + // true, true, kLoopTime, turnToTarget); + // }, + // {&m_drive})); + + m_driverController.A().OnTrue(m_elevator.MoveToPositionAbsolute(10_m)); } frc2::CommandPtr RobotContainer::GetAutonomousCommand() { diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index 71eeca5..7a431ef 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -14,6 +14,7 @@ #include #include #include +#include #include #include @@ -154,16 +155,16 @@ namespace ElevatorConstants { constexpr units::meter_t kMaxDistance = 24_in; constexpr units::meter_t kRelativeDistancePerRev = 0.1_in; // TODO do math to figure out value constexpr units::meter_t kAbsoluteDistancePerRev = 0.1_in; - constexpr units::meters_per_second_t kDefaultVelocity = 1_mps; + constexpr units::meters_per_second_t kDefaultVelocity = 0.66_mps; constexpr double kVelocityScalar = 1.0; - constexpr units::meter_t kTolerance = 1_m; + constexpr units::meter_t kTolerance = 0.5_in; // Placeholder const subzero::SingleAxisMechanism kElevatorMechanism { - 24_in, - 0_deg, - 0.0, - frc::Color8Bit() + 2_in, + 90_deg, + 6.0, + subzero::ColorConstants::kBlue }; const frc::TrapezoidProfile::Constraints kElevatorProfileConstraints{}; diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index d487f53..9326a7c 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -14,6 +14,7 @@ #include #include #include +#include #include "Constants.h" #include "subsystems/DriveSubsystem.h" @@ -35,7 +36,7 @@ class RobotContainer { private: // Replace with CommandPS4Controller or CommandJoystick if needed - frc::XboxController m_driverController{ + frc2::CommandXboxController m_driverController{ OperatorConstants::kDriverControllerPort}; // The robot's subsystems are defined here... diff --git a/src/main/include/subsystems/ElevatorSubsystem.h b/src/main/include/subsystems/ElevatorSubsystem.h index 87f8e5c..be65f55 100644 --- a/src/main/include/subsystems/ElevatorSubsystem.h +++ b/src/main/include/subsystems/ElevatorSubsystem.h @@ -19,7 +19,9 @@ class ElevatorSubsystem : public subzero::LinearSingleAxisSubsystem{ "Elevator Subsystem", - dynamic_cast(m_elevatorController), + frc::RobotBase::IsReal() ? + dynamic_cast(m_elevatorController) : + dynamic_cast(simElevatorController), { // Min distance ElevatorConstants::kMinDistance, // Max distance @@ -71,6 +73,6 @@ class ElevatorSubsystem : public subzero::LinearSingleAxisSubsystem Date: Sun, 9 Feb 2025 19:00:57 -0600 Subject: [PATCH 28/42] Fix simulation --- simgui-ds.json | 8 +++- src/main/cpp/Robot.cpp | 5 +++ src/main/cpp/RobotContainer.cpp | 11 +++++- src/main/include/Constants.h | 39 ++++++++++--------- src/main/include/Robot.h | 1 + src/main/include/RobotContainer.h | 10 ++++- .../include/subsystems/ElevatorSubsystem.h | 9 +++-- 7 files changed, 57 insertions(+), 26 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index c4b7efd..ae2dc78 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "Joysticks": { + "window": { + "visible": false + } + }, "keyboardJoysticks": [ { "axisConfig": [ @@ -91,8 +96,7 @@ ], "robotJoysticks": [ { - "guid": "78696e70757401000000000000000000", - "useGamepad": true + "guid": "Keyboard0" } ] } diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 6a1eb4b..cfe63bb 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -18,6 +18,11 @@ Robot::Robot() {} */ void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); + m_container.Periodic(); +} + +void Robot::RobotInit() { + m_container.Initialize(); } /** diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index b61a0af..5f5a11a 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -70,7 +70,8 @@ void RobotContainer::ConfigureBindings() { // }, // {&m_drive})); - m_driverController.A().OnTrue(m_elevator.MoveToPositionAbsolute(10_m)); + m_driverController.A().OnTrue(m_elevator.MoveToPositionAbsolute(7_in)); + m_driverController.B().OnTrue(m_elevator.MoveToPositionAbsolute(2_in)); } frc2::CommandPtr RobotContainer::GetAutonomousCommand() { @@ -86,3 +87,11 @@ frc2::CommandPtr RobotContainer::GetAutonomousCommand() { return pathplanner::PathPlannerAuto(m_autoSelected) .AndThen(frc2::InstantCommand([this, offset]() { m_drive.OffsetRotation(offset); }).ToPtr()); } + +void RobotContainer::Periodic() { + frc::SmartDashboard::PutData("Robot Elevator", &m_elevatorMech); +} + +void RobotContainer::Initialize() { + m_elevator.OnInit(); +} \ No newline at end of file diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index 7a431ef..84be65b 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -22,7 +22,7 @@ using SparkMaxPidController = subzero::PidMotorController ; + rev::spark::SparkAbsoluteEncoder, rev::spark::SparkMaxConfig>; typedef subzero::PidMotorController::Constraints kElevatorProfileConstraints{}; -} \ No newline at end of file + // min length + 2_in, + // angle + 90_deg, + // line width + 10.0, + // color + subzero::ColorConstants::kRed}; + + const frc::TrapezoidProfile::Constraints kElevatorProfileConstraints{1_fps * 10, 0.75_fps_sq * 20}; +}; \ No newline at end of file diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h index 794e643..0afe066 100644 --- a/src/main/include/Robot.h +++ b/src/main/include/Robot.h @@ -23,6 +23,7 @@ class Robot : public frc::TimedRobot { void TeleopPeriodic() override; void TestPeriodic() override; void SimulationInit() override; + void RobotInit() override; void SimulationPeriodic() override; private: diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index 9326a7c..5895cd9 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -15,6 +15,7 @@ #include #include #include +#include #include "Constants.h" #include "subsystems/DriveSubsystem.h" @@ -34,6 +35,10 @@ class RobotContainer { frc2::CommandPtr GetAutonomousCommand(); + void Periodic(); + + void Initialize(); + private: // Replace with CommandPS4Controller or CommandJoystick if needed frc2::CommandXboxController m_driverController{ @@ -48,6 +53,9 @@ class RobotContainer { frc::SendableChooser m_chooser; std::string m_autoSelected; + + frc::Mechanism2d m_elevatorMech{0.5, 0.5}; + frc::MechanismRoot2d* m_root = m_elevatorMech.GetRoot("Elevator", 0.25, 0.25); - ElevatorSubsystem m_elevator; + ElevatorSubsystem m_elevator{(frc::MechanismObject2d*)m_elevatorMech.GetRoot("Elevator", 0.25, 0.25)}; }; diff --git a/src/main/include/subsystems/ElevatorSubsystem.h b/src/main/include/subsystems/ElevatorSubsystem.h index be65f55..e1cfce7 100644 --- a/src/main/include/subsystems/ElevatorSubsystem.h +++ b/src/main/include/subsystems/ElevatorSubsystem.h @@ -11,16 +11,17 @@ #include #include #include +#include #include "Constants.h" class ElevatorSubsystem : public subzero::LinearSingleAxisSubsystem { public: - explicit ElevatorSubsystem() + explicit ElevatorSubsystem(frc::MechanismObject2d* node = nullptr) : subzero::LinearSingleAxisSubsystem{ "Elevator Subsystem", - frc::RobotBase::IsReal() ? - dynamic_cast(m_elevatorController) : + // frc::RobotBase::IsReal() ? + // dynamic_cast(m_elevatorController) : dynamic_cast(simElevatorController), { // Min distance ElevatorConstants::kMinDistance, @@ -52,7 +53,7 @@ class ElevatorSubsystem : public subzero::LinearSingleAxisSubsystem Date: Mon, 10 Feb 2025 19:18:38 -0600 Subject: [PATCH 29/42] Amp arm on 2024 bot using Algae arm --- src/main/cpp/RobotContainer.cpp | 48 ++++++++++--------- src/main/include/Constants.h | 2 +- src/main/include/RobotContainer.h | 15 ++++-- src/main/include/constants/ArmConstants.h | 24 +++++----- .../include/subsystems/AlgaeArmSubsystem.h | 27 +++++------ .../include/subsystems/CoralArmSubsystem.h | 17 ++++--- 6 files changed, 70 insertions(+), 63 deletions(-) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index d3cd845..b570f0c 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -40,18 +40,18 @@ RobotContainer::RobotContainer() { // Set up default drive command // The left stick controls translation of the robot. // Turning is controlled by the X axis of the right stick. - m_drive.SetDefaultCommand(frc2::RunCommand( - [this] { - m_drive.Drive( - -units::meters_per_second_t{frc::ApplyDeadband( - m_driverController.GetLeftY(), OIConstants::kDriveDeadband)}, - -units::meters_per_second_t{frc::ApplyDeadband( - m_driverController.GetLeftX(), OIConstants::kDriveDeadband)}, - -units::radians_per_second_t{frc::ApplyDeadband( - m_driverController.GetRightX(), OIConstants::kDriveDeadband)}, - true); - }, - {&m_drive})); + // m_drive.SetDefaultCommand(frc2::RunCommand( + // [this] { + // m_drive.Drive( + // -units::meters_per_second_t{frc::ApplyDeadband( + // m_driverController.GetLeftY(), OIConstants::kDriveDeadband)}, + // -units::meters_per_second_t{frc::ApplyDeadband( + // m_driverController.GetLeftX(), OIConstants::kDriveDeadband)}, + // -units::radians_per_second_t{frc::ApplyDeadband( + // m_driverController.GetRightX(), OIConstants::kDriveDeadband)}, + // true); + // }, + // {&m_drive})); } void RobotContainer::ConfigureBindings() { @@ -72,28 +72,30 @@ void RobotContainer::ConfigureBindings() { // }, // {&m_drive})); - m_driverController.A().OnTrue(m_elevator.MoveToPositionAbsolute(7_in)); - m_driverController.B().OnTrue(m_elevator.MoveToPositionAbsolute(2_in)); + // m_driverController.A().OnTrue(m_elevator.MoveToPositionAbsolute(7_in)); + // m_driverController.B().OnTrue(m_elevator.MoveToPositionAbsolute(2_in)); + + m_driverController.A().OnTrue(m_arm.MoveToPositionAbsolute(90_deg)); } frc2::CommandPtr RobotContainer::GetAutonomousCommand() { - auto command = pathplanner::PathPlannerAuto("Spin Auto"); - auto rot = command.getStartingPose().Rotation().Degrees(); + // auto command = pathplanner::PathPlannerAuto("Spin Auto"); + // auto rot = command.getStartingPose().Rotation().Degrees(); - auto offset = m_drive.GetHeading() - rot; + // auto offset = m_drive.GetHeading() - rot; - m_autoSelected = m_chooser.GetSelected(); + // m_autoSelected = m_chooser.GetSelected(); - std::cout << "Auto Selected: \"" << m_autoSelected << "\".\n"; + // std::cout << "Auto Selected: \"" << m_autoSelected << "\".\n"; - return pathplanner::PathPlannerAuto(m_autoSelected) - .AndThen(frc2::InstantCommand([this, offset]() { m_drive.OffsetRotation(offset); }).ToPtr()); + // return pathplanner::PathPlannerAuto(m_autoSelected) + // .AndThen(frc2::InstantCommand([this, offset]() { m_drive.OffsetRotation(offset); }).ToPtr()); } void RobotContainer::Periodic() { - frc::SmartDashboard::PutData("Robot Elevator", &m_elevatorMech); + // frc::SmartDashboard::PutData("Robot Elevator", &m_elevatorMech); } void RobotContainer::Initialize() { - m_elevator.OnInit(); + // m_elevator.OnInit(); } \ No newline at end of file diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index 8e2cb72..fd0d71a 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -165,7 +165,7 @@ namespace ElevatorConstants { constexpr units::revolutions_per_minute_t kMaxRpm = 5676_rpm; // Placeholder values - constexpr units::meter_t kMinDistance =2_in; + constexpr units::meter_t kMinDistance = 2_in; constexpr units::meter_t kMaxDistance = 24_in; constexpr units::meter_t kRelativeDistancePerRev = 1_in / 23.1; // TODO do math to figure out value constexpr units::meter_t kAbsoluteDistancePerRev = 1_in / 23.1; diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index 2e83a9e..cbbd0e8 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -22,6 +22,7 @@ #include "subsystems/ExampleSubsystem.h" #include "subsystems/ElevatorSubsystem.h" #include "subsystems/AlgaeArmSubsystem.h" +#include "subsystems/CoralArmSubsystem.h" /** * This class is where the bulk of the robot should be declared. Since @@ -46,7 +47,7 @@ class RobotContainer { OperatorConstants::kDriverControllerPort}; // The robot's subsystems are defined here... - DriveSubsystem m_drive; + // DriveSubsystem m_drive; ExampleSubsystem m_subsystem; @@ -55,8 +56,12 @@ class RobotContainer { frc::SendableChooser m_chooser; std::string m_autoSelected; - frc::Mechanism2d m_elevatorMech{0.5, 0.5}; - frc::MechanismRoot2d* m_root = m_elevatorMech.GetRoot("Elevator", 0.25, 0.25); + // frc::Mechanism2d m_elevatorMech{0.5, 0.5}; + // frc::MechanismRoot2d* m_root = m_elevatorMech.GetRoot("Elevator", 0.25, 0.25); - ElevatorSubsystem m_elevator{(frc::MechanismObject2d*)m_elevatorMech.GetRoot("Elevator", 0.25, 0.25)}; -}; + // ElevatorSubsystem m_elevator{(frc::MechanismObject2d*)m_elevatorMech.GetRoot("Elevator", 0.25, 0.25)}; + + AlgaeArmSubsystem m_arm; + + // CoralArmSubsystem m_coralArm; +}; \ No newline at end of file diff --git a/src/main/include/constants/ArmConstants.h b/src/main/include/constants/ArmConstants.h index ff8ba54..50cf1bc 100644 --- a/src/main/include/constants/ArmConstants.h +++ b/src/main/include/constants/ArmConstants.h @@ -10,13 +10,13 @@ namespace AlgaeArmConstants{ - constexpr int kArmMotorId = -1; + constexpr int kArmMotorId = 62; constexpr int kIntakeMotorId = 20; - constexpr double kP = -1; - constexpr double kI = -1; - constexpr double kD = -1; - constexpr double kIZone = -1; - constexpr double kFF = -1; + constexpr double kP = 0.075; + constexpr double kI = 0.0; + constexpr double kD = 0.0; + constexpr double kIZone = 0.1; + constexpr double kFF = 0.1; constexpr units::revolutions_per_minute_t kMaxRpm = 1_rpm; constexpr units::degree_t kHomeRotation = 10_deg; @@ -44,13 +44,13 @@ namespace AlgaeArmConstants{ } namespace CoralArmConstants{ - constexpr int kArmMotorId = 62; + constexpr int kArmMotorId = 20; constexpr int kIntakeMotorId = 20; - constexpr double kP = -1; - constexpr double kI = -1; - constexpr double kD = -1; - constexpr double kIZone = -1; - constexpr double kFF = -1; + constexpr double kP = 0.1; + constexpr double kI = 0.1; + constexpr double kD = 0.1; + constexpr double kIZone = 0.1; + constexpr double kFF = 0.1; constexpr units::revolutions_per_minute_t kMaxRpm = 1_rpm; constexpr units::degree_t kHomeRotation = 10_deg; diff --git a/src/main/include/subsystems/AlgaeArmSubsystem.h b/src/main/include/subsystems/AlgaeArmSubsystem.h index 5b79695..342633e 100644 --- a/src/main/include/subsystems/AlgaeArmSubsystem.h +++ b/src/main/include/subsystems/AlgaeArmSubsystem.h @@ -6,10 +6,11 @@ #include #include +#include #include -#include -#include +#include +#include #include "constants/ArmConstants.h" #include "Constants.h" @@ -48,29 +49,25 @@ class AlgaeArmSubsystem : public subzero::RotationalSingleAxisSubsystem +#include + #include #include #include #include + +#include #include -#include -#include +#include #include "constants/ArmConstants.h" #include "Constants.h" @@ -53,16 +56,16 @@ class CoralArmSubsystem : public subzero::RotationalSingleAxisSubsystem Date: Mon, 10 Feb 2025 19:37:34 -0600 Subject: [PATCH 30/42] Simulation and instantiation of other classes --- simgui-ds.json | 8 +-- src/main/cpp/RobotContainer.cpp | 65 ++++++++--------------- src/main/include/RobotContainer.h | 10 ++-- src/main/include/constants/ArmConstants.h | 6 +-- 4 files changed, 33 insertions(+), 56 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index ae2dc78..c4b7efd 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,9 +1,4 @@ { - "Joysticks": { - "window": { - "visible": false - } - }, "keyboardJoysticks": [ { "axisConfig": [ @@ -96,7 +91,8 @@ ], "robotJoysticks": [ { - "guid": "Keyboard0" + "guid": "78696e70757401000000000000000000", + "useGamepad": true } ] } diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index b570f0c..dc19073 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -40,62 +40,43 @@ RobotContainer::RobotContainer() { // Set up default drive command // The left stick controls translation of the robot. // Turning is controlled by the X axis of the right stick. - // m_drive.SetDefaultCommand(frc2::RunCommand( - // [this] { - // m_drive.Drive( - // -units::meters_per_second_t{frc::ApplyDeadband( - // m_driverController.GetLeftY(), OIConstants::kDriveDeadband)}, - // -units::meters_per_second_t{frc::ApplyDeadband( - // m_driverController.GetLeftX(), OIConstants::kDriveDeadband)}, - // -units::radians_per_second_t{frc::ApplyDeadband( - // m_driverController.GetRightX(), OIConstants::kDriveDeadband)}, - // true); - // }, - // {&m_drive})); + m_drive.SetDefaultCommand(frc2::RunCommand( + [this] { + m_drive.Drive( + -units::meters_per_second_t{frc::ApplyDeadband( + m_driverController.GetLeftY(), OIConstants::kDriveDeadband)}, + -units::meters_per_second_t{frc::ApplyDeadband( + m_driverController.GetLeftX(), OIConstants::kDriveDeadband)}, + -units::radians_per_second_t{frc::ApplyDeadband( + m_driverController.GetRightX(), OIConstants::kDriveDeadband)}, + true); + }, + {&m_drive})); } void RobotContainer::ConfigureBindings() { - // m_drive.SetDefaultCommand(frc2::RunCommand( - // [this] { - // InputUtils::DeadzoneAxes axes = InputUtils::CalculateCircularDeadzone( - // m_driverController.GetLeftX(), m_driverController.GetLeftY(), - // OIConstants::kDriveDeadband); - - // ITurnToTarget* turnToTarget = m_shouldAim ? &m_turnToPose : nullptr; - - // m_drive.Drive( - // -units::meters_per_second_t{axes.y}, - // -units::meters_per_second_t{axes.x}, - // -units::radians_per_second_t{frc::ApplyDeadband( - // m_driverController.GetRightX(), OIConstants::kDriveDeadband)}, - // true, true, kLoopTime, turnToTarget); - // }, - // {&m_drive})); - - // m_driverController.A().OnTrue(m_elevator.MoveToPositionAbsolute(7_in)); - // m_driverController.B().OnTrue(m_elevator.MoveToPositionAbsolute(2_in)); - - m_driverController.A().OnTrue(m_arm.MoveToPositionAbsolute(90_deg)); + m_driverController.A().OnTrue(m_elevator.MoveToPositionAbsolute(20_in)); + m_driverController.B().OnTrue(m_elevator.MoveToPositionAbsolute(2_in)); } frc2::CommandPtr RobotContainer::GetAutonomousCommand() { - // auto command = pathplanner::PathPlannerAuto("Spin Auto"); - // auto rot = command.getStartingPose().Rotation().Degrees(); + auto command = pathplanner::PathPlannerAuto("Spin Auto"); + auto rot = command.getStartingPose().Rotation().Degrees(); - // auto offset = m_drive.GetHeading() - rot; + auto offset = m_drive.GetHeading() - rot; - // m_autoSelected = m_chooser.GetSelected(); + m_autoSelected = m_chooser.GetSelected(); - // std::cout << "Auto Selected: \"" << m_autoSelected << "\".\n"; + std::cout << "Auto Selected: \"" << m_autoSelected << "\".\n"; - // return pathplanner::PathPlannerAuto(m_autoSelected) - // .AndThen(frc2::InstantCommand([this, offset]() { m_drive.OffsetRotation(offset); }).ToPtr()); + return pathplanner::PathPlannerAuto(m_autoSelected) + .AndThen(frc2::InstantCommand([this, offset]() { m_drive.OffsetRotation(offset); }).ToPtr()); } void RobotContainer::Periodic() { - // frc::SmartDashboard::PutData("Robot Elevator", &m_elevatorMech); + frc::SmartDashboard::PutData("Robot Elevator", &m_elevatorMech); } void RobotContainer::Initialize() { - // m_elevator.OnInit(); + m_elevator.OnInit(); } \ No newline at end of file diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index cbbd0e8..2bea100 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -47,7 +47,7 @@ class RobotContainer { OperatorConstants::kDriverControllerPort}; // The robot's subsystems are defined here... - // DriveSubsystem m_drive; + DriveSubsystem m_drive; ExampleSubsystem m_subsystem; @@ -56,12 +56,12 @@ class RobotContainer { frc::SendableChooser m_chooser; std::string m_autoSelected; - // frc::Mechanism2d m_elevatorMech{0.5, 0.5}; - // frc::MechanismRoot2d* m_root = m_elevatorMech.GetRoot("Elevator", 0.25, 0.25); + frc::Mechanism2d m_elevatorMech{0.5, 0.5}; + frc::MechanismRoot2d* m_root = m_elevatorMech.GetRoot("Elevator", 0.25, 0.25); - // ElevatorSubsystem m_elevator{(frc::MechanismObject2d*)m_elevatorMech.GetRoot("Elevator", 0.25, 0.25)}; + ElevatorSubsystem m_elevator{(frc::MechanismObject2d*)m_elevatorMech.GetRoot("Elevator", 0.25, 0.25)}; AlgaeArmSubsystem m_arm; - // CoralArmSubsystem m_coralArm; + CoralArmSubsystem m_coralArm; }; \ No newline at end of file diff --git a/src/main/include/constants/ArmConstants.h b/src/main/include/constants/ArmConstants.h index 50cf1bc..7164039 100644 --- a/src/main/include/constants/ArmConstants.h +++ b/src/main/include/constants/ArmConstants.h @@ -10,13 +10,13 @@ namespace AlgaeArmConstants{ - constexpr int kArmMotorId = 62; + constexpr int kArmMotorId = 0; // Placeholder constexpr int kIntakeMotorId = 20; constexpr double kP = 0.075; constexpr double kI = 0.0; constexpr double kD = 0.0; - constexpr double kIZone = 0.1; - constexpr double kFF = 0.1; + constexpr double kIZone = 0.0; + constexpr double kFF = 0.0; constexpr units::revolutions_per_minute_t kMaxRpm = 1_rpm; constexpr units::degree_t kHomeRotation = 10_deg; From 034d99c8a7d093c60a4274eb8c5116a38d0e6dba Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Tue, 11 Feb 2025 15:28:09 -0600 Subject: [PATCH 31/42] Add .cpp files to header to ensure compilation --- src/main/include/CommonCompile.h | 6 ++++++ src/main/include/RobotContainer.h | 2 ++ src/main/include/subsystems/AlgaeArmSubsystem.h | 4 ---- src/main/include/subsystems/CoralArmSubsystem.h | 4 ---- src/main/include/subsystems/ElevatorSubsystem.h | 3 --- 5 files changed, 8 insertions(+), 11 deletions(-) create mode 100644 src/main/include/CommonCompile.h diff --git a/src/main/include/CommonCompile.h b/src/main/include/CommonCompile.h new file mode 100644 index 0000000..5c600b4 --- /dev/null +++ b/src/main/include/CommonCompile.h @@ -0,0 +1,6 @@ +#pragma once + +#include +#include +#include +#include \ No newline at end of file diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index 2bea100..7fb8862 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -17,6 +17,8 @@ #include #include +#include "CommonCompile.h" + #include "Constants.h" #include "subsystems/DriveSubsystem.h" #include "subsystems/ExampleSubsystem.h" diff --git a/src/main/include/subsystems/AlgaeArmSubsystem.h b/src/main/include/subsystems/AlgaeArmSubsystem.h index 342633e..7016722 100644 --- a/src/main/include/subsystems/AlgaeArmSubsystem.h +++ b/src/main/include/subsystems/AlgaeArmSubsystem.h @@ -6,10 +6,6 @@ #include #include -#include -#include -#include - #include #include "constants/ArmConstants.h" #include "Constants.h" diff --git a/src/main/include/subsystems/CoralArmSubsystem.h b/src/main/include/subsystems/CoralArmSubsystem.h index 4576cba..ee63b0a 100644 --- a/src/main/include/subsystems/CoralArmSubsystem.h +++ b/src/main/include/subsystems/CoralArmSubsystem.h @@ -8,10 +8,6 @@ #include #include -#include -#include -#include - #include "constants/ArmConstants.h" #include "Constants.h" diff --git a/src/main/include/subsystems/ElevatorSubsystem.h b/src/main/include/subsystems/ElevatorSubsystem.h index 05819f0..49a91a6 100644 --- a/src/main/include/subsystems/ElevatorSubsystem.h +++ b/src/main/include/subsystems/ElevatorSubsystem.h @@ -8,9 +8,6 @@ #include #include #include -#include -#include -#include #include #include "Constants.h" From 516f48b32ae9b4350bd31190ebfd88cb4a0a45d1 Mon Sep 17 00:00:00 2001 From: 5690 <5690Programmers@gmail.com> Date: Tue, 11 Feb 2025 18:05:45 -0600 Subject: [PATCH 32/42] made concrete methods for AlgeaArmSubsystem --- src/main/cpp/subsystems/AlgaeArmSubsystem.cpp | 30 +++++++++++++++++++ .../include/subsystems/AlgaeArmSubsystem.h | 30 ++++++++++--------- .../include/subsystems/CoralArmSubsystem.h | 13 ++++---- 3 files changed, 54 insertions(+), 19 deletions(-) create mode 100644 src/main/cpp/subsystems/AlgaeArmSubsystem.cpp diff --git a/src/main/cpp/subsystems/AlgaeArmSubsystem.cpp b/src/main/cpp/subsystems/AlgaeArmSubsystem.cpp new file mode 100644 index 0000000..5a32a90 --- /dev/null +++ b/src/main/cpp/subsystems/AlgaeArmSubsystem.cpp @@ -0,0 +1,30 @@ + +#include "subsystems/AlgaeArmSubsystem.h" + +void AlgaeArmSubsystem::Periodic() { + +} + +// TODO: make the set value for in and out a constant + +frc2::CommandPtr AlgaeArmSubsystem::StopIntake() { + return + frc2::InstantCommand([this] { \ + m_intakeMotor.Set(0); + }).ToPtr(); +} + + +frc2::CommandPtr AlgaeArmSubsystem::In() { + return + frc2::InstantCommand([this] { \ + m_intakeMotor.Set(0.25); + }).ToPtr(); +} + +frc2::CommandPtr AlgaeArmSubsystem::Out() { + return + frc2::InstantCommand([this] { \ + m_intakeMotor.Set(0.25); + }).ToPtr(); +} diff --git a/src/main/include/subsystems/AlgaeArmSubsystem.h b/src/main/include/subsystems/AlgaeArmSubsystem.h index 7016722..ec54a36 100644 --- a/src/main/include/subsystems/AlgaeArmSubsystem.h +++ b/src/main/include/subsystems/AlgaeArmSubsystem.h @@ -5,6 +5,8 @@ #include #include #include +#include +#include #include #include "constants/ArmConstants.h" @@ -49,32 +51,32 @@ class AlgaeArmSubsystem : public subzero::RotationalSingleAxisSubsystem Date: Tue, 11 Feb 2025 19:59:49 -0600 Subject: [PATCH 33/42] Fully implement algae arm subsystem and run intake (doesn't work) --- simgui-ds.json | 3 +- src/main/cpp/RobotContainer.cpp | 3 +- src/main/cpp/subsystems/AlgaeArmSubsystem.cpp | 8 +++-- src/main/include/Constants.h | 24 +++++++-------- src/main/include/constants/ArmConstants.h | 8 ++--- .../include/subsystems/AlgaeArmSubsystem.h | 1 + .../include/subsystems/ElevatorSubsystem.h | 29 +++++++++++++------ 7 files changed, 44 insertions(+), 32 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index c4b7efd..69b1a3c 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -91,8 +91,7 @@ ], "robotJoysticks": [ { - "guid": "78696e70757401000000000000000000", - "useGamepad": true + "guid": "Keyboard0" } ] } diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index dc19073..4a4e1fc 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -55,8 +55,7 @@ RobotContainer::RobotContainer() { } void RobotContainer::ConfigureBindings() { - m_driverController.A().OnTrue(m_elevator.MoveToPositionAbsolute(20_in)); - m_driverController.B().OnTrue(m_elevator.MoveToPositionAbsolute(2_in)); + m_driverController.A().OnTrue(m_arm.In()); } frc2::CommandPtr RobotContainer::GetAutonomousCommand() { diff --git a/src/main/cpp/subsystems/AlgaeArmSubsystem.cpp b/src/main/cpp/subsystems/AlgaeArmSubsystem.cpp index 5a32a90..c91c018 100644 --- a/src/main/cpp/subsystems/AlgaeArmSubsystem.cpp +++ b/src/main/cpp/subsystems/AlgaeArmSubsystem.cpp @@ -1,6 +1,8 @@ #include "subsystems/AlgaeArmSubsystem.h" +#include + void AlgaeArmSubsystem::Periodic() { } @@ -17,14 +19,14 @@ frc2::CommandPtr AlgaeArmSubsystem::StopIntake() { frc2::CommandPtr AlgaeArmSubsystem::In() { return - frc2::InstantCommand([this] { \ + frc2::RunCommand([this] { \ m_intakeMotor.Set(0.25); - }).ToPtr(); + }, {this}).ToPtr(); } frc2::CommandPtr AlgaeArmSubsystem::Out() { return frc2::InstantCommand([this] { \ - m_intakeMotor.Set(0.25); + m_intakeMotor.Set(-0.25); }).ToPtr(); } diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index fd0d71a..43efb80 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -86,15 +86,15 @@ constexpr double kRearLeftChassisAngularOffset = std::numbers::pi; constexpr double kRearRightChassisAngularOffset = std::numbers::pi / 2; // SPARK MAX CAN IDs -constexpr int kFrontLeftDrivingCanId = 8; -constexpr int kRearLeftDrivingCanId = 6; -constexpr int kFrontRightDrivingCanId = 2; -constexpr int kRearRightDrivingCanId = 4; - -constexpr int kFrontLeftTurningCanId = 7; -constexpr int kRearLeftTurningCanId = 5; -constexpr int kFrontRightTurningCanId = 1; -constexpr int kRearRightTurningCanId = 3; +constexpr int kFrontLeftDrivingCanId = 18; +constexpr int kRearLeftDrivingCanId = 1; +constexpr int kFrontRightDrivingCanId = 4; +constexpr int kRearRightDrivingCanId = 9; + +constexpr int kFrontLeftTurningCanId = 2; +constexpr int kRearLeftTurningCanId = 20; +constexpr int kFrontRightTurningCanId = 5; +constexpr int kRearRightTurningCanId = 8; } // namespace DriveConstants namespace ModuleConstants { @@ -152,7 +152,8 @@ namespace GyroConstants { namespace ElevatorConstants { // Placeholder value - const int kElevatorMotorCanId = 0; + const int kLeadElevatorMotorCanId = 7; + const int kFollowerElevatorMotorCanId = 19; // Placeholder values const double kElevatorP = 0.075; @@ -167,8 +168,7 @@ namespace ElevatorConstants { // Placeholder values constexpr units::meter_t kMinDistance = 2_in; constexpr units::meter_t kMaxDistance = 24_in; - constexpr units::meter_t kRelativeDistancePerRev = 1_in / 23.1; // TODO do math to figure out value - constexpr units::meter_t kAbsoluteDistancePerRev = 1_in / 23.1; + constexpr units::meter_t kRelativeDistancePerRev = 5.51977829236_in / 125; // 125:1 ratio gearbox constexpr units::meters_per_second_t kDefaultVelocity = 0.66_mps; constexpr double kVelocityScalar = 1.0; constexpr units::meter_t kTolerance = 0.5_in; diff --git a/src/main/include/constants/ArmConstants.h b/src/main/include/constants/ArmConstants.h index 7164039..827b27b 100644 --- a/src/main/include/constants/ArmConstants.h +++ b/src/main/include/constants/ArmConstants.h @@ -10,8 +10,8 @@ namespace AlgaeArmConstants{ - constexpr int kArmMotorId = 0; // Placeholder - constexpr int kIntakeMotorId = 20; + constexpr int kArmMotorId = 17; + constexpr int kIntakeMotorId = 6; constexpr double kP = 0.075; constexpr double kI = 0.0; constexpr double kD = 0.0; @@ -44,8 +44,8 @@ namespace AlgaeArmConstants{ } namespace CoralArmConstants{ - constexpr int kArmMotorId = 20; - constexpr int kIntakeMotorId = 20; + constexpr int kArmMotorId = 16; + constexpr int kIntakeMotorId = 15; constexpr double kP = 0.1; constexpr double kI = 0.1; constexpr double kD = 0.1; diff --git a/src/main/include/subsystems/AlgaeArmSubsystem.h b/src/main/include/subsystems/AlgaeArmSubsystem.h index ec54a36..b41c583 100644 --- a/src/main/include/subsystems/AlgaeArmSubsystem.h +++ b/src/main/include/subsystems/AlgaeArmSubsystem.h @@ -48,6 +48,7 @@ class AlgaeArmSubsystem : public subzero::RotationalSingleAxisSubsystem Date: Thu, 13 Feb 2025 19:30:28 -0600 Subject: [PATCH 34/42] Algae arm + intaking for both arms --- src/main/cpp/RobotContainer.cpp | 6 ++++- src/main/cpp/subsystems/AlgaeArmSubsystem.cpp | 26 +++++++++++++------ src/main/cpp/subsystems/CoralArmSubsystem.cpp | 25 ++++++++++++++++++ src/main/include/RobotContainer.h | 2 +- src/main/include/constants/ArmConstants.h | 19 ++++++++------ .../include/subsystems/AlgaeArmSubsystem.h | 15 +++++------ .../include/subsystems/CoralArmSubsystem.h | 17 +++--------- .../include/subsystems/ElevatorSubsystem.h | 2 +- 8 files changed, 71 insertions(+), 41 deletions(-) create mode 100644 src/main/cpp/subsystems/CoralArmSubsystem.cpp diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 4a4e1fc..150c406 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -55,7 +55,9 @@ RobotContainer::RobotContainer() { } void RobotContainer::ConfigureBindings() { - m_driverController.A().OnTrue(m_arm.In()); + m_driverController.A().OnTrue(m_algaeArm.In()); + m_driverController.B().OnTrue(m_coralArm.MoveAtSpeed(-0.7)); + m_driverController.X().OnTrue(m_algaeArm.MoveToPositionAbsolute(40_deg)); } frc2::CommandPtr RobotContainer::GetAutonomousCommand() { @@ -78,4 +80,6 @@ void RobotContainer::Periodic() { void RobotContainer::Initialize() { m_elevator.OnInit(); + m_coralArm.OnInit(); + m_algaeArm.OnInit(); } \ No newline at end of file diff --git a/src/main/cpp/subsystems/AlgaeArmSubsystem.cpp b/src/main/cpp/subsystems/AlgaeArmSubsystem.cpp index c91c018..071d51c 100644 --- a/src/main/cpp/subsystems/AlgaeArmSubsystem.cpp +++ b/src/main/cpp/subsystems/AlgaeArmSubsystem.cpp @@ -3,10 +3,6 @@ #include -void AlgaeArmSubsystem::Periodic() { - -} - // TODO: make the set value for in and out a constant frc2::CommandPtr AlgaeArmSubsystem::StopIntake() { @@ -18,10 +14,24 @@ frc2::CommandPtr AlgaeArmSubsystem::StopIntake() { frc2::CommandPtr AlgaeArmSubsystem::In() { - return - frc2::RunCommand([this] { \ - m_intakeMotor.Set(0.25); - }, {this}).ToPtr(); + return frc2::FunctionalCommand( + // On init + []() {}, + // On execute + [this]() { + m_intakeMotor.Set(0.7); + }, + // On end + [this](bool interupted) { + m_intakeMotor.StopMotor(); + }, + // Is finished + []() { + return false; + }, + // Requirements + {this} + ).ToPtr(); } frc2::CommandPtr AlgaeArmSubsystem::Out() { diff --git a/src/main/cpp/subsystems/CoralArmSubsystem.cpp b/src/main/cpp/subsystems/CoralArmSubsystem.cpp new file mode 100644 index 0000000..e13ae18 --- /dev/null +++ b/src/main/cpp/subsystems/CoralArmSubsystem.cpp @@ -0,0 +1,25 @@ +#include "subsystems/CoralArmSubsystem.h" + +#include + +frc2::CommandPtr CoralArmSubsystem::MoveAtSpeed(double speed) { + return frc2::FunctionalCommand( + // On init + []() {}, + // On execute + [this, speed]() { + m_intakeMotor.Set(speed); + std::cout << "Running motor"; + }, + // On end + [this](bool interupted) { + m_intakeMotor.StopMotor(); + }, + // Is finished + []() { + return false; + }, + // Requirements + {this} + ).ToPtr(); +} diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index 7fb8862..ac7b793 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -63,7 +63,7 @@ class RobotContainer { ElevatorSubsystem m_elevator{(frc::MechanismObject2d*)m_elevatorMech.GetRoot("Elevator", 0.25, 0.25)}; - AlgaeArmSubsystem m_arm; + AlgaeArmSubsystem m_algaeArm; CoralArmSubsystem m_coralArm; }; \ No newline at end of file diff --git a/src/main/include/constants/ArmConstants.h b/src/main/include/constants/ArmConstants.h index 827b27b..c3f035a 100644 --- a/src/main/include/constants/ArmConstants.h +++ b/src/main/include/constants/ArmConstants.h @@ -12,21 +12,21 @@ namespace AlgaeArmConstants{ constexpr int kArmMotorId = 17; constexpr int kIntakeMotorId = 6; - constexpr double kP = 0.075; + constexpr double kP = 0.2; constexpr double kI = 0.0; - constexpr double kD = 0.0; + constexpr double kD = 0.001; constexpr double kIZone = 0.0; constexpr double kFF = 0.0; - constexpr units::revolutions_per_minute_t kMaxRpm = 1_rpm; - constexpr units::degree_t kHomeRotation = 10_deg; - constexpr units::degree_t kMaxRotation = 190_deg; - constexpr units::degree_t kRelativeDistancePerRev = 360_deg * (1 / 8.75); + constexpr units::revolutions_per_minute_t kMaxRpm = 5676_rpm; + constexpr units::degree_t kHomeRotation = 0_deg; + constexpr units::degree_t kMaxRotation = 85_deg; + constexpr units::degree_t kRelativeDistancePerRev = 360_deg / 75; constexpr units::degree_t kAbsoluteDistancePerRev = 360_deg; constexpr units::degrees_per_second_t kDefaultVelocity = 10_deg_per_s; constexpr double kVelocityScalar = 1.0; constexpr units::degree_t kTolerance = 2_deg; - constexpr units::meter_t kArmLength = 0.2_m; + constexpr units::meter_t kArmLength = 17_in; static const subzero::SingleAxisMechanism kAlgaeArmMechanism = { // length @@ -38,8 +38,11 @@ namespace AlgaeArmConstants{ // color subzero::ColorConstants::kBlue}; + // const frc::TrapezoidProfile::Constraints + // kRotationalAxisConstraints{360_deg_per_s * 2.2, 360_deg_per_s_sq * 2.2}; + const frc::TrapezoidProfile::Constraints - kRotationalAxisConstraints{360_deg_per_s * 2.2, 360_deg_per_s_sq * 2.2}; + kRotationalAxisConstraints; } diff --git a/src/main/include/subsystems/AlgaeArmSubsystem.h b/src/main/include/subsystems/AlgaeArmSubsystem.h index b41c583..9623e6f 100644 --- a/src/main/include/subsystems/AlgaeArmSubsystem.h +++ b/src/main/include/subsystems/AlgaeArmSubsystem.h @@ -16,7 +16,7 @@ class AlgaeArmSubsystem : public subzero::RotationalSingleAxisSubsystem{ - "Arm", + "Algae Arm", frc::RobotBase::IsReal() ? dynamic_cast(algaeArmController) : dynamic_cast(simAlgaeArmController), @@ -45,20 +45,19 @@ class AlgaeArmSubsystem : public subzero::RotationalSingleAxisSubsystem{ - "Arm", + "Coral Arm", frc::RobotBase::IsReal() ? dynamic_cast(coralArmController) : dynamic_cast(simCoralArmController), @@ -52,19 +52,8 @@ class CoralArmSubsystem : public subzero::RotationalSingleAxisSubsystem Date: Thu, 13 Feb 2025 19:55:27 -0600 Subject: [PATCH 35/42] Slightly tuned PID values --- src/main/cpp/RobotContainer.cpp | 1 + src/main/include/constants/ArmConstants.h | 16 ++++++++-------- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 150c406..c667f85 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -58,6 +58,7 @@ void RobotContainer::ConfigureBindings() { m_driverController.A().OnTrue(m_algaeArm.In()); m_driverController.B().OnTrue(m_coralArm.MoveAtSpeed(-0.7)); m_driverController.X().OnTrue(m_algaeArm.MoveToPositionAbsolute(40_deg)); + m_driverController.Y().OnTrue(m_coralArm.MoveToPositionAbsolute(150_deg)); } frc2::CommandPtr RobotContainer::GetAutonomousCommand() { diff --git a/src/main/include/constants/ArmConstants.h b/src/main/include/constants/ArmConstants.h index c3f035a..a01b28e 100644 --- a/src/main/include/constants/ArmConstants.h +++ b/src/main/include/constants/ArmConstants.h @@ -49,16 +49,16 @@ namespace AlgaeArmConstants{ namespace CoralArmConstants{ constexpr int kArmMotorId = 16; constexpr int kIntakeMotorId = 15; - constexpr double kP = 0.1; - constexpr double kI = 0.1; - constexpr double kD = 0.1; - constexpr double kIZone = 0.1; - constexpr double kFF = 0.1; + constexpr double kP = 0.25; + constexpr double kI = 0.0; + constexpr double kD = 0.0; + constexpr double kIZone = 0.0; + constexpr double kFF = 0.0; constexpr units::revolutions_per_minute_t kMaxRpm = 1_rpm; - constexpr units::degree_t kHomeRotation = 10_deg; - constexpr units::degree_t kMaxRotation = 190_deg; - constexpr units::degree_t kRelativeDistancePerRev = 360_deg * (1 / 8.75); + constexpr units::degree_t kHomeRotation = 5_deg; + constexpr units::degree_t kMaxRotation = 180_deg; + constexpr units::degree_t kRelativeDistancePerRev = 360_deg / (75 * 4.7); // 4.7 is the ratio of the chain gear constexpr units::degree_t kAbsoluteDistancePerRev = 360_deg; constexpr units::degrees_per_second_t kDefaultVelocity = 10_deg_per_s; constexpr double kVelocityScalar = 1.0; From e2310bae566ccd3c79bfaf72956e3383351e5107 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Fri, 14 Feb 2025 16:19:22 -0600 Subject: [PATCH 36/42] Create `IntakeSubsystem` class and move arm constants --- README.md | 11 +-- src/main/cpp/subsystems/IntakeSubsystem.cpp | 24 ++++++ src/main/include/Constants.h | 81 +++++++++++++++++- src/main/include/constants/ArmConstants.h | 82 ------------------- .../include/subsystems/AlgaeArmSubsystem.h | 5 +- .../include/subsystems/CoralArmSubsystem.h | 1 - src/main/include/subsystems/DriveSubsystem.h | 2 +- .../include/subsystems/ElevatorSubsystem.h | 4 +- src/main/include/subsystems/IntakeSubsystem.h | 15 ++++ 9 files changed, 126 insertions(+), 99 deletions(-) create mode 100644 src/main/cpp/subsystems/IntakeSubsystem.cpp delete mode 100644 src/main/include/constants/ArmConstants.h create mode 100644 src/main/include/subsystems/IntakeSubsystem.h diff --git a/README.md b/README.md index 7481617..05e63e4 100644 --- a/README.md +++ b/README.md @@ -84,14 +84,9 @@ Following the WPILib command based structure we have broken our robot up into a ## Digital Input Ports -| Port | Device | -| :---: | :--------------------------: | -| 1 | Center Upper Beam Break | -| 2 | Lower Podium Side Beam Break | -| 3 | Lower Center Beam Break | -| 4 | Lower Amp Side Beam Break | -| 5 | Upper Amp Side Beam Break | -| 6 | Upper Podium Side Beam Break | +| Port | Device | +| :---: | :------------------------------: | +| 1 | Lower Elevator Limit Switch | ## Network Map diff --git a/src/main/cpp/subsystems/IntakeSubsystem.cpp b/src/main/cpp/subsystems/IntakeSubsystem.cpp new file mode 100644 index 0000000..8e2a3bc --- /dev/null +++ b/src/main/cpp/subsystems/IntakeSubsystem.cpp @@ -0,0 +1,24 @@ +#include + +#include + +frc2::CommandPtr IntakeSubsystem::MoveAtPercent(double percent) { + return frc2::FunctionalCommand( + // On init + []() {}, + // On execute + [this, percent]() { + m_motor.Set(percent); + }, + // On end + [this](bool interupted) { + m_motor.StopMotor(); + }, + // Is finished + []() { + return false; + }, + // Requirements + {this} + ).ToPtr(); +} \ No newline at end of file diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index 43efb80..ec217f4 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -11,6 +11,8 @@ #include #include #include +#include +#include #include #include @@ -69,6 +71,8 @@ namespace DriveConstants { constexpr units::meters_per_second_t kMaxSpeed = 4.92_mps; constexpr units::radians_per_second_t kMaxAngularSpeed{2 * std::numbers::pi}; +const int kPigeonCanId = 13; + constexpr double kDirectionSlewRate = 1.2; // radians per second constexpr double kMagnitudeSlewRate = 1.8; // percent per second (1 = 100%) constexpr double kRotationalSlewRate = 2.0; // percent per second (1 = 100%) @@ -151,10 +155,11 @@ namespace GyroConstants { } namespace ElevatorConstants { - // Placeholder value const int kLeadElevatorMotorCanId = 7; const int kFollowerElevatorMotorCanId = 19; + const int kBottomLimitSwitchPort = 1; + // Placeholder values const double kElevatorP = 0.075; const double kElevatorI = 0.0; @@ -162,7 +167,6 @@ namespace ElevatorConstants { const double kElevatorIZone = 0.1; const double kElevatorFF = 0.1; - // Placeholder value constexpr units::revolutions_per_minute_t kMaxRpm = 5676_rpm; // Placeholder values @@ -185,4 +189,75 @@ namespace ElevatorConstants { subzero::ColorConstants::kRed}; const frc::TrapezoidProfile::Constraints kElevatorProfileConstraints{1_fps * 10, 0.75_fps_sq * 20}; -}; \ No newline at end of file +}; + +namespace AlgaeArmConstants{ + constexpr int kArmMotorId = 17; + constexpr int kIntakeMotorId = 6; + constexpr double kP = 0.2; + constexpr double kI = 0.0; + constexpr double kD = 0.001; + constexpr double kIZone = 0.0; + constexpr double kFF = 0.0; + + constexpr units::revolutions_per_minute_t kMaxRpm = 5676_rpm; + constexpr units::degree_t kHomeRotation = 0_deg; + constexpr units::degree_t kMaxRotation = 85_deg; + constexpr units::degree_t kRelativeDistancePerRev = 360_deg / 75; + constexpr units::degree_t kAbsoluteDistancePerRev = 360_deg; + constexpr units::degrees_per_second_t kDefaultVelocity = 10_deg_per_s; + constexpr double kVelocityScalar = 1.0; + constexpr units::degree_t kTolerance = 2_deg; + constexpr units::meter_t kArmLength = 17_in; + + static const subzero::SingleAxisMechanism kAlgaeArmMechanism = { + // length + 0.2_m, + // min angle + 110_deg, + // line width + 6, + // color + subzero::ColorConstants::kBlue}; + + // const frc::TrapezoidProfile::Constraints + // kRotationalAxisConstraints{360_deg_per_s * 2.2, 360_deg_per_s_sq * 2.2}; + + const frc::TrapezoidProfile::Constraints + kRotationalAxisConstraints; + +} + +namespace CoralArmConstants{ + constexpr int kArmMotorId = 16; + constexpr int kIntakeMotorId = 15; + constexpr double kP = 0.25; + constexpr double kI = 0.0; + constexpr double kD = 0.0; + constexpr double kIZone = 0.0; + constexpr double kFF = 0.0; + + constexpr units::revolutions_per_minute_t kMaxRpm = 1_rpm; + constexpr units::degree_t kHomeRotation = 5_deg; + constexpr units::degree_t kMaxRotation = 180_deg; + constexpr units::degree_t kRelativeDistancePerRev = 360_deg / (75 * 4.7); // 4.7 is the ratio of the chain gear + constexpr units::degree_t kAbsoluteDistancePerRev = 360_deg; + constexpr units::degrees_per_second_t kDefaultVelocity = 10_deg_per_s; + constexpr double kVelocityScalar = 1.0; + constexpr units::degree_t kTolerance = 2_deg; + constexpr units::meter_t kArmLength = 0.2_m; + + static const subzero::SingleAxisMechanism kCoralArmMechanism = { + // length + 0.2_m, + // min angle + 110_deg, + // line width + 6, + // color + subzero::ColorConstants::kBlue}; + + const frc::TrapezoidProfile::Constraints + kRotationalAxisConstraints{360_deg_per_s * 2.2, 360_deg_per_s_sq * 2.2}; + +} \ No newline at end of file diff --git a/src/main/include/constants/ArmConstants.h b/src/main/include/constants/ArmConstants.h deleted file mode 100644 index a01b28e..0000000 --- a/src/main/include/constants/ArmConstants.h +++ /dev/null @@ -1,82 +0,0 @@ -#pragma once -#include -#include -#include -#include -//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! -//ALL VALUES ARE CURRENTLY PLACE HOLDERS FOR KNOW!!! -//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - - - -namespace AlgaeArmConstants{ - constexpr int kArmMotorId = 17; - constexpr int kIntakeMotorId = 6; - constexpr double kP = 0.2; - constexpr double kI = 0.0; - constexpr double kD = 0.001; - constexpr double kIZone = 0.0; - constexpr double kFF = 0.0; - - constexpr units::revolutions_per_minute_t kMaxRpm = 5676_rpm; - constexpr units::degree_t kHomeRotation = 0_deg; - constexpr units::degree_t kMaxRotation = 85_deg; - constexpr units::degree_t kRelativeDistancePerRev = 360_deg / 75; - constexpr units::degree_t kAbsoluteDistancePerRev = 360_deg; - constexpr units::degrees_per_second_t kDefaultVelocity = 10_deg_per_s; - constexpr double kVelocityScalar = 1.0; - constexpr units::degree_t kTolerance = 2_deg; - constexpr units::meter_t kArmLength = 17_in; - - static const subzero::SingleAxisMechanism kAlgaeArmMechanism = { - // length - 0.2_m, - // min angle - 110_deg, - // line width - 6, - // color - subzero::ColorConstants::kBlue}; - - // const frc::TrapezoidProfile::Constraints - // kRotationalAxisConstraints{360_deg_per_s * 2.2, 360_deg_per_s_sq * 2.2}; - - const frc::TrapezoidProfile::Constraints - kRotationalAxisConstraints; - -} - -namespace CoralArmConstants{ - constexpr int kArmMotorId = 16; - constexpr int kIntakeMotorId = 15; - constexpr double kP = 0.25; - constexpr double kI = 0.0; - constexpr double kD = 0.0; - constexpr double kIZone = 0.0; - constexpr double kFF = 0.0; - - constexpr units::revolutions_per_minute_t kMaxRpm = 1_rpm; - constexpr units::degree_t kHomeRotation = 5_deg; - constexpr units::degree_t kMaxRotation = 180_deg; - constexpr units::degree_t kRelativeDistancePerRev = 360_deg / (75 * 4.7); // 4.7 is the ratio of the chain gear - constexpr units::degree_t kAbsoluteDistancePerRev = 360_deg; - constexpr units::degrees_per_second_t kDefaultVelocity = 10_deg_per_s; - constexpr double kVelocityScalar = 1.0; - constexpr units::degree_t kTolerance = 2_deg; - constexpr units::meter_t kArmLength = 0.2_m; - - static const subzero::SingleAxisMechanism kCoralArmMechanism = { - // length - 0.2_m, - // min angle - 110_deg, - // line width - 6, - // color - subzero::ColorConstants::kBlue}; - - const frc::TrapezoidProfile::Constraints - kRotationalAxisConstraints{360_deg_per_s * 2.2, 360_deg_per_s_sq * 2.2}; - -} - diff --git a/src/main/include/subsystems/AlgaeArmSubsystem.h b/src/main/include/subsystems/AlgaeArmSubsystem.h index 9623e6f..94045a1 100644 --- a/src/main/include/subsystems/AlgaeArmSubsystem.h +++ b/src/main/include/subsystems/AlgaeArmSubsystem.h @@ -7,10 +7,10 @@ #include #include #include - #include -#include "constants/ArmConstants.h" + #include "Constants.h" +#include "subsystems/IntakeSubsystem.h" class AlgaeArmSubsystem : public subzero::RotationalSingleAxisSubsystem { public: @@ -66,7 +66,6 @@ class AlgaeArmSubsystem : public subzero::RotationalSingleAxisSubsystem #include -#include "constants/ArmConstants.h" #include "Constants.h" //pretty much a copy paste of algae arm subsystem with differet names right now, subject to change later diff --git a/src/main/include/subsystems/DriveSubsystem.h b/src/main/include/subsystems/DriveSubsystem.h index 60972ac..fce5227 100644 --- a/src/main/include/subsystems/DriveSubsystem.h +++ b/src/main/include/subsystems/DriveSubsystem.h @@ -128,7 +128,7 @@ class DriveSubsystem : public frc2::SubsystemBase { MAXSwerveModule m_rearRight; // Gyro Sensor - ctre::phoenix6::hardware::Pigeon2 m_pidgey{13, "rio"}; + ctre::phoenix6::hardware::Pigeon2 m_pidgey{DriveConstants::kPigeonCanId, "rio"}; units::time::second_t currentTime{frc::Timer::GetFPGATimestamp()}; // Odometry class for tracking robot pose diff --git a/src/main/include/subsystems/ElevatorSubsystem.h b/src/main/include/subsystems/ElevatorSubsystem.h index 9793579..a926e5a 100644 --- a/src/main/include/subsystems/ElevatorSubsystem.h +++ b/src/main/include/subsystems/ElevatorSubsystem.h @@ -35,7 +35,7 @@ class ElevatorSubsystem : public subzero::LinearSingleAxisSubsystem +#include + +#include + +class IntakeSubsystem : public frc2::SubsystemBase { +public: + IntakeSubsystem(const int id) : m_motor{id, rev::spark::SparkLowLevel::MotorType::kBrushless} {} + + frc2::CommandPtr MoveAtPercent(double percent); +private: + rev::spark::SparkMax m_motor; +}; \ No newline at end of file From 61ce52fbedbf86d55af5db1fa73be5862494373c Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Fri, 14 Feb 2025 16:51:17 -0600 Subject: [PATCH 37/42] Use `IntakeSubsystem` in arms and override periodic in `ElevatorSubsystem` --- src/main/cpp/RobotContainer.cpp | 5 +-- src/main/cpp/subsystems/AlgaeArmSubsystem.cpp | 40 ++----------------- src/main/cpp/subsystems/CoralArmSubsystem.cpp | 24 ++--------- src/main/cpp/subsystems/IntakeSubsystem.cpp | 10 +++++ .../include/subsystems/AlgaeArmSubsystem.h | 12 +----- .../include/subsystems/CoralArmSubsystem.h | 7 ++-- .../include/subsystems/ElevatorSubsystem.h | 5 +++ src/main/include/subsystems/IntakeSubsystem.h | 2 + 8 files changed, 30 insertions(+), 75 deletions(-) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index c667f85..25d44db 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -55,10 +55,7 @@ RobotContainer::RobotContainer() { } void RobotContainer::ConfigureBindings() { - m_driverController.A().OnTrue(m_algaeArm.In()); - m_driverController.B().OnTrue(m_coralArm.MoveAtSpeed(-0.7)); - m_driverController.X().OnTrue(m_algaeArm.MoveToPositionAbsolute(40_deg)); - m_driverController.Y().OnTrue(m_coralArm.MoveToPositionAbsolute(150_deg)); + // m_driverController.A().OnTrue(m_elevator.MoveToPositionAbsolute(10_in)); } frc2::CommandPtr RobotContainer::GetAutonomousCommand() { diff --git a/src/main/cpp/subsystems/AlgaeArmSubsystem.cpp b/src/main/cpp/subsystems/AlgaeArmSubsystem.cpp index 071d51c..218ffe1 100644 --- a/src/main/cpp/subsystems/AlgaeArmSubsystem.cpp +++ b/src/main/cpp/subsystems/AlgaeArmSubsystem.cpp @@ -3,40 +3,6 @@ #include -// TODO: make the set value for in and out a constant - -frc2::CommandPtr AlgaeArmSubsystem::StopIntake() { - return - frc2::InstantCommand([this] { \ - m_intakeMotor.Set(0); - }).ToPtr(); -} - - -frc2::CommandPtr AlgaeArmSubsystem::In() { - return frc2::FunctionalCommand( - // On init - []() {}, - // On execute - [this]() { - m_intakeMotor.Set(0.7); - }, - // On end - [this](bool interupted) { - m_intakeMotor.StopMotor(); - }, - // Is finished - []() { - return false; - }, - // Requirements - {this} - ).ToPtr(); -} - -frc2::CommandPtr AlgaeArmSubsystem::Out() { - return - frc2::InstantCommand([this] { \ - m_intakeMotor.Set(-0.25); - }).ToPtr(); -} +IntakeSubsystem& AlgaeArmSubsystem::GetIntakeSubsystem() { + return m_intake; +} \ No newline at end of file diff --git a/src/main/cpp/subsystems/CoralArmSubsystem.cpp b/src/main/cpp/subsystems/CoralArmSubsystem.cpp index e13ae18..fcf727b 100644 --- a/src/main/cpp/subsystems/CoralArmSubsystem.cpp +++ b/src/main/cpp/subsystems/CoralArmSubsystem.cpp @@ -2,24 +2,6 @@ #include -frc2::CommandPtr CoralArmSubsystem::MoveAtSpeed(double speed) { - return frc2::FunctionalCommand( - // On init - []() {}, - // On execute - [this, speed]() { - m_intakeMotor.Set(speed); - std::cout << "Running motor"; - }, - // On end - [this](bool interupted) { - m_intakeMotor.StopMotor(); - }, - // Is finished - []() { - return false; - }, - // Requirements - {this} - ).ToPtr(); -} +IntakeSubsystem& CoralArmSubsystem::GetIntakeSubsystem() { + return m_intake; +} \ No newline at end of file diff --git a/src/main/cpp/subsystems/IntakeSubsystem.cpp b/src/main/cpp/subsystems/IntakeSubsystem.cpp index 8e2a3bc..76b2741 100644 --- a/src/main/cpp/subsystems/IntakeSubsystem.cpp +++ b/src/main/cpp/subsystems/IntakeSubsystem.cpp @@ -1,5 +1,6 @@ #include +#include #include frc2::CommandPtr IntakeSubsystem::MoveAtPercent(double percent) { @@ -21,4 +22,13 @@ frc2::CommandPtr IntakeSubsystem::MoveAtPercent(double percent) { // Requirements {this} ).ToPtr(); +} + +frc2::CommandPtr IntakeSubsystem::Stop() { + return frc2::InstantCommand( + [this]() { + m_motor.StopMotor(); + }, + {this} + ).ToPtr(); } \ No newline at end of file diff --git a/src/main/include/subsystems/AlgaeArmSubsystem.h b/src/main/include/subsystems/AlgaeArmSubsystem.h index 94045a1..1b428f1 100644 --- a/src/main/include/subsystems/AlgaeArmSubsystem.h +++ b/src/main/include/subsystems/AlgaeArmSubsystem.h @@ -53,18 +53,10 @@ class AlgaeArmSubsystem : public subzero::RotationalSingleAxisSubsystem #include "Constants.h" +#include "subsystems/IntakeSubsystem.h" //pretty much a copy paste of algae arm subsystem with differet names right now, subject to change later @@ -54,10 +55,10 @@ class CoralArmSubsystem : public subzero::RotationalSingleAxisSubsystem::Periodic(); + std::cout << (m_bottomLimitSwitchPort.Get() ? "Limit switch active" : "Limit switch inactive") << std::endl; + } + private: rev::spark::SparkMax m_leadMotor{ ElevatorConstants::kLeadElevatorMotorCanId, diff --git a/src/main/include/subsystems/IntakeSubsystem.h b/src/main/include/subsystems/IntakeSubsystem.h index 301ed55..245abdc 100644 --- a/src/main/include/subsystems/IntakeSubsystem.h +++ b/src/main/include/subsystems/IntakeSubsystem.h @@ -10,6 +10,8 @@ class IntakeSubsystem : public frc2::SubsystemBase { IntakeSubsystem(const int id) : m_motor{id, rev::spark::SparkLowLevel::MotorType::kBrushless} {} frc2::CommandPtr MoveAtPercent(double percent); + + frc2::CommandPtr Stop(); private: rev::spark::SparkMax m_motor; }; \ No newline at end of file From 8715691ffbb84eee4119b6456b615a89ec60dcd0 Mon Sep 17 00:00:00 2001 From: 5690 <5690Programmers@gmail.com> Date: Fri, 14 Feb 2025 17:56:57 -0600 Subject: [PATCH 38/42] made climber subsystem and constants --- src/main/include/Constants.h | 38 +++++++++++ .../include/subsystems/ClimberSubsystem.h | 68 +++++++++++++++++++ 2 files changed, 106 insertions(+) create mode 100644 src/main/include/subsystems/ClimberSubsystem.h diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index ec217f4..da6f9fc 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -260,4 +260,42 @@ namespace CoralArmConstants{ const frc::TrapezoidProfile::Constraints kRotationalAxisConstraints{360_deg_per_s * 2.2, 360_deg_per_s_sq * 2.2}; +} + +namespace ClimberConstants{ + constexpr int kArmMotorId = 14; + + // Placeholder values + constexpr double kP = 0.2; + constexpr double kI = 0.0; + constexpr double kD = 0.001; + constexpr double kIZone = 0.0; + constexpr double kFF = 0.0; + + constexpr units::revolutions_per_minute_t kMaxRpm = 5676_rpm; + constexpr units::degree_t kHomeRotation = 0_deg; + constexpr units::degree_t kMaxRotation = 85_deg; + constexpr units::degree_t kRelativeDistancePerRev = 360_deg / 75; + constexpr units::degree_t kAbsoluteDistancePerRev = 360_deg; + constexpr units::degrees_per_second_t kDefaultVelocity = 10_deg_per_s; + constexpr double kVelocityScalar = 1.0; + constexpr units::degree_t kTolerance = 2_deg; + constexpr units::meter_t kArmLength = 17_in; + + static const subzero::SingleAxisMechanism kClimberMechanism = { + // length + 0.2_m, + // min angle + 110_deg, + // line width + 6, + // color + subzero::ColorConstants::kBlue}; + + // const frc::TrapezoidProfile::Constraints + // kRotationalAxisConstraints{360_deg_per_s * 2.2, 360_deg_per_s_sq * 2.2}; + + const frc::TrapezoidProfile::Constraints + kRotationalAxisConstraints; + } \ No newline at end of file diff --git a/src/main/include/subsystems/ClimberSubsystem.h b/src/main/include/subsystems/ClimberSubsystem.h new file mode 100644 index 0000000..72a68a0 --- /dev/null +++ b/src/main/include/subsystems/ClimberSubsystem.h @@ -0,0 +1,68 @@ +#pragma once + +#include +#include + +#include +#include +#include +#include + +#include "Constants.h" + +class ClimberSubsystem : public subzero::RotationalSingleAxisSubsystem { + public: + explicit ClimberSubsystem(frc::MechanismObject2d* node = nullptr) + : subzero::RotationalSingleAxisSubsystem{ + "Climber Arm", + frc::RobotBase::IsReal() + ? dynamic_cast(climberController) + : dynamic_cast(simClimberController), + { + ClimberConstants::kHomeRotation, + // Max distance + ClimberConstants::kMaxRotation, + // Distance per revolution of relative encoder + ClimberConstants::kRelativeDistancePerRev, + // Distance per revolution of absolute encoder + ClimberConstants::kAbsoluteDistancePerRev, + // Default velocity + ClimberConstants::kDefaultVelocity, + // Velocity scalar + ClimberConstants::kVelocityScalar, + // Tolerance + ClimberConstants::kTolerance, + // Min limit switch + std::nullopt, + // Max limit switch + std::nullopt, + // Reversed + true, + // Mechanism2d + ClimberConstants::kClimberMechanism, + // Conversion Function + std::nullopt, + + [] { return false; }, ClimberConstants::kRotationalAxisConstraints}, + CoralArmConstants::kArmLength, + node} {} + + private: + + rev::spark::SparkMax m_climberMotor{ClimberConstants::kArmMotorId, + rev::spark::SparkLowLevel::MotorType::kBrushless}; + rev::spark::SparkClosedLoopController m_pidController = m_climberMotor.GetClosedLoopController(); + rev::spark::SparkRelativeEncoder m_enc = m_climberMotor.GetEncoder(); + rev::spark::SparkAbsoluteEncoder m_absEnc = m_climberMotor.GetAbsoluteEncoder(); + subzero::PidSettings climberPidSettings = { + ClimberConstants::kP, ClimberConstants::kI, ClimberConstants::kD, + ClimberConstants::kIZone, ClimberConstants::kFF, true}; + SparkMaxPidController climberController{"Coral Arm", + m_climberMotor, + m_enc, + m_pidController, + climberPidSettings, + &m_absEnc, + ClimberConstants::kMaxRpm}; + subzero::SimPidMotorController simClimberController{"Sim Arm", climberPidSettings, ClimberConstants::kMaxRpm}; +}; \ No newline at end of file From 75980771bdc7296de9b818e2f0e1d3a0b3904e47 Mon Sep 17 00:00:00 2001 From: 5690Progammers <5690programmers@gmail.com> Date: Fri, 14 Feb 2025 20:37:42 -0600 Subject: [PATCH 39/42] Fix elevator with bigger PID constants --- simgui-ds.json | 3 ++- src/main/cpp/RobotContainer.cpp | 5 ++++- src/main/include/Constants.h | 18 +++++++++--------- .../include/subsystems/ElevatorSubsystem.h | 7 +------ 4 files changed, 16 insertions(+), 17 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 69b1a3c..c4b7efd 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -91,7 +91,8 @@ ], "robotJoysticks": [ { - "guid": "Keyboard0" + "guid": "78696e70757401000000000000000000", + "useGamepad": true } ] } diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 25d44db..12b4085 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -55,7 +55,10 @@ RobotContainer::RobotContainer() { } void RobotContainer::ConfigureBindings() { - // m_driverController.A().OnTrue(m_elevator.MoveToPositionAbsolute(10_in)); + // m_driverController.A().OnTrue(m_elevator.MoveToPositionRelative(20_in)); + // m_driverController.B().OnTrue(m_elevator.MoveToPositionAbsolute(0_m)); + m_driverController.X().OnTrue(m_coralArm.GetIntakeSubsystem().MoveAtPercent(-0.35)); + m_driverController.Y().OnTrue(m_algaeArm.GetIntakeSubsystem().MoveAtPercent(-0.50)); } frc2::CommandPtr RobotContainer::GetAutonomousCommand() { diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index da6f9fc..ff6f790 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -155,24 +155,24 @@ namespace GyroConstants { } namespace ElevatorConstants { - const int kLeadElevatorMotorCanId = 7; - const int kFollowerElevatorMotorCanId = 19; + const int kLeadElevatorMotorCanId = 19; + const int kFollowerElevatorMotorCanId = 7; const int kBottomLimitSwitchPort = 1; // Placeholder values - const double kElevatorP = 0.075; + const double kElevatorP = 280.0; const double kElevatorI = 0.0; - const double kElevatorD = 0.075; - const double kElevatorIZone = 0.1; - const double kElevatorFF = 0.1; + const double kElevatorD = 0.0; + const double kElevatorIZone = 0.0; + const double kElevatorFF = 0.0; constexpr units::revolutions_per_minute_t kMaxRpm = 5676_rpm; // Placeholder values - constexpr units::meter_t kMinDistance = 2_in; - constexpr units::meter_t kMaxDistance = 24_in; - constexpr units::meter_t kRelativeDistancePerRev = 5.51977829236_in / 125; // 125:1 ratio gearbox + constexpr units::meter_t kMinDistance = 0_in; + constexpr units::meter_t kMaxDistance = 20_in; + constexpr units::meter_t kRelativeDistancePerRev = 5.51977829236_in / 36; // 36:1 ratio gearbox constexpr units::meters_per_second_t kDefaultVelocity = 0.66_mps; constexpr double kVelocityScalar = 1.0; constexpr units::meter_t kTolerance = 0.5_in; diff --git a/src/main/include/subsystems/ElevatorSubsystem.h b/src/main/include/subsystems/ElevatorSubsystem.h index a96ff57..c11a7ec 100644 --- a/src/main/include/subsystems/ElevatorSubsystem.h +++ b/src/main/include/subsystems/ElevatorSubsystem.h @@ -57,11 +57,6 @@ class ElevatorSubsystem : public subzero::LinearSingleAxisSubsystem::Periodic(); - std::cout << (m_bottomLimitSwitchPort.Get() ? "Limit switch active" : "Limit switch inactive") << std::endl; - } - private: rev::spark::SparkMax m_leadMotor{ ElevatorConstants::kLeadElevatorMotorCanId, @@ -80,7 +75,7 @@ class ElevatorSubsystem : public subzero::LinearSingleAxisSubsystem Date: Sat, 15 Feb 2025 12:56:56 -0600 Subject: [PATCH 40/42] Add basic PID tunings and change intake subsystem --- src/main/cpp/RobotContainer.cpp | 8 +++---- src/main/cpp/subsystems/AlgaeArmSubsystem.cpp | 4 ---- src/main/cpp/subsystems/CoralArmSubsystem.cpp | 4 ---- src/main/include/Constants.h | 17 +++++++++------ src/main/include/RobotContainer.h | 2 ++ .../include/subsystems/AlgaeArmSubsystem.h | 8 ++----- .../include/subsystems/CoralArmSubsystem.h | 21 ++++++++++--------- .../include/subsystems/ElevatorSubsystem.h | 2 +- 8 files changed, 31 insertions(+), 35 deletions(-) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 12b4085..1a75c56 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -55,10 +55,10 @@ RobotContainer::RobotContainer() { } void RobotContainer::ConfigureBindings() { - // m_driverController.A().OnTrue(m_elevator.MoveToPositionRelative(20_in)); - // m_driverController.B().OnTrue(m_elevator.MoveToPositionAbsolute(0_m)); - m_driverController.X().OnTrue(m_coralArm.GetIntakeSubsystem().MoveAtPercent(-0.35)); - m_driverController.Y().OnTrue(m_algaeArm.GetIntakeSubsystem().MoveAtPercent(-0.50)); + m_driverController.A().OnTrue(m_algaeArm.MoveToPositionAbsolute(50_deg)); + m_driverController.B().OnTrue(m_algaeArm.MoveToPositionAbsolute(5_deg)); + m_driverController.X().OnTrue(m_coralArm.MoveToPositionAbsolute(120_deg)); + m_driverController.Y().OnTrue(m_coralArm.MoveToPositionAbsolute(5_deg)); } frc2::CommandPtr RobotContainer::GetAutonomousCommand() { diff --git a/src/main/cpp/subsystems/AlgaeArmSubsystem.cpp b/src/main/cpp/subsystems/AlgaeArmSubsystem.cpp index 218ffe1..6d9ee10 100644 --- a/src/main/cpp/subsystems/AlgaeArmSubsystem.cpp +++ b/src/main/cpp/subsystems/AlgaeArmSubsystem.cpp @@ -2,7 +2,3 @@ #include "subsystems/AlgaeArmSubsystem.h" #include - -IntakeSubsystem& AlgaeArmSubsystem::GetIntakeSubsystem() { - return m_intake; -} \ No newline at end of file diff --git a/src/main/cpp/subsystems/CoralArmSubsystem.cpp b/src/main/cpp/subsystems/CoralArmSubsystem.cpp index fcf727b..9b51669 100644 --- a/src/main/cpp/subsystems/CoralArmSubsystem.cpp +++ b/src/main/cpp/subsystems/CoralArmSubsystem.cpp @@ -1,7 +1,3 @@ #include "subsystems/CoralArmSubsystem.h" #include - -IntakeSubsystem& CoralArmSubsystem::GetIntakeSubsystem() { - return m_intake; -} \ No newline at end of file diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index ff6f790..e178eab 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -38,6 +38,11 @@ using SparkMaxPidController = subzero::PidMotorController; + +using SparkMaxPidControllerTuner = + subzero::PidMotorControllerTuner; typedef subzero::PidMotorController{ "Coral Arm", frc::RobotBase::IsReal() - ? dynamic_cast(coralArmController) + ? dynamic_cast(m_coralArmController) : dynamic_cast(simCoralArmController), { CoralArmConstants::kHomeRotation, @@ -49,17 +49,15 @@ class CoralArmSubsystem : public subzero::RotationalSingleAxisSubsystem::Periodic(); + + m_tuner.UpdateFromShuffleboard(); + } private: - IntakeSubsystem m_intake{CoralArmConstants::kIntakeMotorId}; - rev::spark::SparkMax m_coralArmMotor{CoralArmConstants::kArmMotorId, rev::spark::SparkLowLevel::MotorType::kBrushless}; rev::spark::SparkClosedLoopController m_pidController = m_coralArmMotor.GetClosedLoopController(); @@ -67,14 +65,17 @@ class CoralArmSubsystem : public subzero::RotationalSingleAxisSubsystem Date: Sat, 15 Feb 2025 13:48:14 -0600 Subject: [PATCH 41/42] Create instance of climber and fix values --- src/main/cpp/RobotContainer.cpp | 1 + src/main/include/Constants.h | 6 +++--- src/main/include/RobotContainer.h | 3 +++ src/main/include/subsystems/ClimberSubsystem.h | 7 +++---- 4 files changed, 10 insertions(+), 7 deletions(-) diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 1a75c56..cdbaf60 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -83,4 +83,5 @@ 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/Constants.h b/src/main/include/Constants.h index e178eab..14aeb18 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -273,14 +273,14 @@ namespace ClimberConstants{ // Placeholder values constexpr double kP = 0.2; constexpr double kI = 0.0; - constexpr double kD = 0.001; + constexpr double kD = 0.0; constexpr double kIZone = 0.0; constexpr double kFF = 0.0; constexpr units::revolutions_per_minute_t kMaxRpm = 5676_rpm; constexpr units::degree_t kHomeRotation = 0_deg; constexpr units::degree_t kMaxRotation = 85_deg; - constexpr units::degree_t kRelativeDistancePerRev = 360_deg / 75; + constexpr units::degree_t kRelativeDistancePerRev = 360_deg / 125; constexpr units::degree_t kAbsoluteDistancePerRev = 360_deg; constexpr units::degrees_per_second_t kDefaultVelocity = 10_deg_per_s; constexpr double kVelocityScalar = 1.0; @@ -289,7 +289,7 @@ namespace ClimberConstants{ static const subzero::SingleAxisMechanism kClimberMechanism = { // length - 0.2_m, + kArmLength, // min angle 110_deg, // line width diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index 7575f7a..8ef2672 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -25,6 +25,7 @@ #include "subsystems/ElevatorSubsystem.h" #include "subsystems/AlgaeArmSubsystem.h" #include "subsystems/CoralArmSubsystem.h" +#include "subsystems/ClimberSubsystem.h" /** * This class is where the bulk of the robot should be declared. Since @@ -68,4 +69,6 @@ class RobotContainer { CoralArmSubsystem m_coralArm; IntakeSubsystem m_coralIntake{CoralArmConstants::kIntakeMotorId}; + + ClimberSubsystem m_climber; }; \ No newline at end of file diff --git a/src/main/include/subsystems/ClimberSubsystem.h b/src/main/include/subsystems/ClimberSubsystem.h index 72a68a0..b817a50 100644 --- a/src/main/include/subsystems/ClimberSubsystem.h +++ b/src/main/include/subsystems/ClimberSubsystem.h @@ -53,16 +53,15 @@ class ClimberSubsystem : public subzero::RotationalSingleAxisSubsystem Date: Sat, 15 Feb 2025 14:15:18 -0600 Subject: [PATCH 42/42] Use fixed common --- SubZero-common | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/SubZero-common b/SubZero-common index 3c01c9c..f890b47 160000 --- a/SubZero-common +++ b/SubZero-common @@ -1 +1 @@ -Subproject commit 3c01c9ce669d96bec7d854ee44118f6a3bac9ca0 +Subproject commit f890b47ca056c55762f1d6c4a07655addf0066cb