diff --git a/src/main/cpp/RobotContainer.cpp b/src/main/cpp/RobotContainer.cpp index 66e1448..735eb14 100644 --- a/src/main/cpp/RobotContainer.cpp +++ b/src/main/cpp/RobotContainer.cpp @@ -29,15 +29,22 @@ #include "subsystems/AlgaeArmSubsystem.h" #include "subsystems/CoralArmSubsystem.h" -RobotContainer::RobotContainer() { +RobotContainer::RobotContainer() + : m_isReducedSensitivity{false}, m_lowSensitivityCoefficient{DriveConstants::kLowSensivityCoefficient} { // Initialize all of your commands and subsystems here + frc::SmartDashboard::PutNumber("Low Sensitivity Percentage", m_lowSensitivityCoefficient); + frc::SmartDashboard::PutData(&m_chooser); m_chooser.SetDefaultOption(AutoConstants::kCenterToCenterAuto, AutoConstants::kCenterToCenterAuto); m_chooser.AddOption(AutoConstants::kCenterToCenterAuto, AutoConstants::kCenterToCenterAuto); m_chooser.AddOption(AutoConstants::kLeftToLeftReefAuto, AutoConstants::kLeftToLeftReefAuto); m_chooser.AddOption(AutoConstants::kRightToRightReefAuto, AutoConstants::kRightToRightReefAuto); m_chooser.AddOption(AutoConstants::kForwardAuto, AutoConstants::kForwardAuto); + m_chooser.AddOption(AutoConstants::kRightToRightSourceAuto, AutoConstants::kRightToRightSourceAuto); + m_chooser.AddOption(AutoConstants::kLeftToLeftSourceAuto, AutoConstants::kLeftToLeftSourceAuto); + m_chooser.AddOption(AutoConstants::kCenterToRightSourceAuto, AutoConstants::kCenterToRightSourceAuto); + m_chooser.AddOption(AutoConstants::kCenterToLeftSourceAuto, AutoConstants::kCenterToLeftSourceAuto); // pathplanner::NamedCommands::registerCommand("Test Command", frc2::cmd::Print("Test Command")); pathplanner::NamedCommands::registerCommand("To L1 Position", std::move(m_commandController.MoveToPositionL1())); @@ -50,24 +57,78 @@ RobotContainer::RobotContainer() { pathplanner::NamedCommands::registerCommand("Home", std::move(m_commandController.HomeElevator())); pathplanner::NamedCommands::registerCommand("Delayed To L2 Position", std::move(frc2::cmd::Wait(CommandConstants::kWaitBeforeL2).AndThen(m_commandController.MoveToPositionL2()))); + m_timeSinceControllerInput = 0_s; + m_defaultLastCalled = 0_s; + // Configure the button bindings ConfigureBindings(); + m_zeroedBeforeSetX = false; + // 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::second_t current = frc::Timer::GetFPGATimestamp(); + if (std::hypot(m_driverController.GetLeftX(), m_driverController.GetLeftY()) < OIConstants::kDriveDeadband + && std::fabs(m_driverController.GetRightX()) < OIConstants::kDriveDeadband) { + units::second_t dif = current - m_defaultLastCalled; + m_timeSinceControllerInput += dif; + + if (!m_zeroedBeforeSetX) { + m_drive.Drive( + 0_mps, + 0_mps, + 0_deg_per_s, + true + ); + } + + if (m_timeSinceControllerInput > DriveConstants::kSetXThreshold) { + + m_zeroedBeforeSetX = true; + + m_drive.SetX(); + } + } else { + m_timeSinceControllerInput = 0_s; + + double multiplier = m_isReducedSensitivity ? m_lowSensitivityCoefficient : 1.0; + + m_drive.Drive( -units::meters_per_second_t{frc::ApplyDeadband( - m_driverController.GetLeftY(), OIConstants::kDriveDeadband)}, + m_driverController.GetLeftY() * multiplier, OIConstants::kDriveDeadband)}, -units::meters_per_second_t{frc::ApplyDeadband( - m_driverController.GetLeftX(), OIConstants::kDriveDeadband)}, + m_driverController.GetLeftX() * multiplier, OIConstants::kDriveDeadband)}, -units::radians_per_second_t{frc::ApplyDeadband( - m_driverController.GetRightX(), OIConstants::kDriveDeadband)}, - true); + m_driverController.GetRightX() * multiplier, OIConstants::kDriveDeadband)}, + true); + + m_zeroedBeforeSetX = false; + } + + m_defaultLastCalled = current; }, {&m_drive})); + + // m_climber.SetDefaultCommand( + // frc2::RunCommand( + // [this]() { + // m_climber.RunMotorPercentage((-m_driverController.GetLeftTriggerAxis() + // + m_driverController.GetRightTriggerAxis()) * ClimberConstants::kPercentageScalar); + + // std::cout << "Controller input: " << (-m_driverController.GetLeftTriggerAxis() + // + m_driverController.GetRightTriggerAxis()) * ClimberConstants::kPercentageScalar + // << ", Current relative position: " << m_climber.GetCurrentPosition().value() << std::endl; + // }, + // {&m_climber} + // ) + // ); + + // Has to be raised before match so coral arm is within frame perimeter + // m_elevator.SetEncoderPosition(ElevatorConstants::kElevatorStartPosition); } void RobotContainer::ConfigureBindings() { @@ -90,6 +151,8 @@ void RobotContainer::ConfigureBindings() { m_driverController.POVUp().OnTrue(m_commandController.ClimbUp()); m_driverController.POVDown().OnTrue(m_commandController.ClimbDown()); + m_driverController.A().OnTrue(frc2::InstantCommand([this]() { m_isReducedSensitivity = !m_isReducedSensitivity; }).ToPtr()); + m_driverController.LeftBumper().OnTrue(m_commandController.HomeElevator()); m_driverController.RightBumper().OnTrue(frc2::InstantCommand( [this]() { @@ -116,6 +179,12 @@ frc2::CommandPtr RobotContainer::GetAutonomousCommand() { void RobotContainer::Periodic() { frc::SmartDashboard::PutData("Robot Elevator", &m_elevatorMech); + frc::SmartDashboard::PutBoolean("Is Slow Mde Active", m_isReducedSensitivity); + + m_lowSensitivityCoefficient = frc::SmartDashboard::GetNumber("Low Sensitivity Percentage", DriveConstants::kLowSensivityCoefficient); + + // frc::SmartDashboard::PutData("Zero Odometry", new frc2::InstantCommand([this]() { m_drive.ResetRotation(); })); + // frc::SmartDashboard::PutData("Zero Odometry", new frc2::InstantCommand([this]() { m_drive.ResetRotation(); })); } diff --git a/src/main/cpp/subsystems/DriveSubsystem.cpp b/src/main/cpp/subsystems/DriveSubsystem.cpp index 8ae8383..8b661f5 100644 --- a/src/main/cpp/subsystems/DriveSubsystem.cpp +++ b/src/main/cpp/subsystems/DriveSubsystem.cpp @@ -81,20 +81,33 @@ DriveSubsystem::DriveSubsystem() } void DriveSubsystem::Periodic() { + // The `poseEstimator` object replaces our `m_odometry` // Implementation of subsystem periodic method goes here. - m_odometry.Update(frc::Rotation2d(units::radian_t{ - m_pidgey.GetYaw().GetValue()}), - {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), - m_rearLeft.GetPosition(), m_rearRight.GetPosition()}); - auto &yaw = m_pidgey.GetYaw(); + // m_odometry.Update(frc::Rotation2d(units::radian_t{ + // m_pidgey.GetYaw().GetValue()}), + // {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), + // m_rearLeft.GetPosition(), m_rearRight.GetPosition()}); + // auto &yaw = m_pidgey.GetYaw(); - m_field.SetRobotPose(m_odometry.GetPose()); + poseEstimator.UpdateWithTime(frc::Timer::GetFPGATimestamp(), GetHeading(), GetModulePositions()); + + m_vision.UpdateEstimatedGlobalPose(poseEstimator, true); + + auto updatedPose = poseEstimator.GetEstimatedPosition(); + + m_lastGoodPosition = updatedPose; + m_field.SetRobotPose(updatedPose); + + logDrivebase(); + + frc::SmartDashboard::PutNumber("Robot X", poseEstimator.GetEstimatedPosition().X().value()); + frc::SmartDashboard::PutNumber("Robot Y", poseEstimator.GetEstimatedPosition().Y().value()); if (frc::Timer::GetFPGATimestamp() - currentTime >= GyroConstants::kPrintPeriod) { - std::cout << "Yaw: " << yaw.GetValue().value() << std::endl; + // std::cout << "Yaw: " << yaw.GetValue().value() << std::endl; - currentTime += GyroConstants::kPrintPeriod; + // currentTime += GyroConstants::kPrintPeriod; // /** // * GetYaw automatically calls Refresh(), no need to manually refresh. // * @@ -128,13 +141,42 @@ void DriveSubsystem::Periodic() { // * timestamps when it receives the frame. This can be further used for latency compensation. // */ // std::cout << std::endl; + // } } } +wpi::array DriveSubsystem::GetModulePositions() + const { + return {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), + m_rearLeft.GetPosition(), m_rearRight.GetPosition()}; +} + +void DriveSubsystem::SimulationPeriodic() { + frc::ChassisSpeeds chassisSpeeds = m_driveKinematics.ToChassisSpeeds( + m_frontLeft.GetState(), m_frontRight.GetState(), m_rearLeft.GetState(), + m_rearRight.GetState()); + + m_simPidgey.SetSupplyVoltage(frc::RobotController::GetBatteryVoltage()); + m_simPidgey.SetRawYaw( + GetHeading() + + units::degree_t(chassisSpeeds.omega.convert().value() * + DriveConstants::kPeriodicInterval.value())); + + poseEstimator.Update(GetHeading(), GetModulePositions()); + m_field.SetRobotPose(poseEstimator.GetEstimatedPosition()); +} + void DriveSubsystem::Drive(units::meters_per_second_t xSpeed, units::meters_per_second_t ySpeed, units::radians_per_second_t rot, bool fieldRelative) { + + auto now = frc::Timer::GetFPGATimestamp(); + auto dif = now - m_lastUpdatedTime; + + auto joystickSpeeds = + GetSpeedsFromJoystick(xSpeed, ySpeed, rot, fieldRelative); + // Convert the commanded speeds into the correct units for the drivetrain units::meters_per_second_t xSpeedDelivered = xSpeed.value() * DriveConstants::kMaxSpeed; @@ -144,12 +186,9 @@ void DriveSubsystem::Drive(units::meters_per_second_t xSpeed, rot.value() * DriveConstants::kMaxAngularSpeed; auto states = m_driveKinematics.ToSwerveModuleStates( - fieldRelative - ? frc::ChassisSpeeds::FromFieldRelativeSpeeds( - xSpeedDelivered, ySpeedDelivered, rotDelivered, - frc::Rotation2d(units::radian_t{ - m_pidgey.GetYaw().GetValue()})) - : frc::ChassisSpeeds{xSpeedDelivered, ySpeedDelivered, rotDelivered}); + frc::ChassisSpeeds::Discretize(joystickSpeeds, dif)); + + m_lastUpdatedTime = now; m_driveKinematics.DesaturateWheelSpeeds(&states, DriveConstants::kMaxSpeed); @@ -161,6 +200,17 @@ void DriveSubsystem::Drive(units::meters_per_second_t xSpeed, m_rearRight.SetDesiredState(br); } +frc::ChassisSpeeds DriveSubsystem::GetSpeedsFromJoystick( + units::meters_per_second_t xSpeed, units::meters_per_second_t ySpeed, + units::radians_per_second_t rot, bool fieldRelative) { + return fieldRelative + ? frc::ChassisSpeeds::FromFieldRelativeSpeeds( + xSpeed * DriveConstants::kMaxSpeed.value(), + ySpeed * DriveConstants::kMaxSpeed.value(), + rot * DriveConstants::kMaxAngularSpeed.value(), GetHeading()) + : frc::ChassisSpeeds{xSpeed, ySpeed, rot}; +} + void DriveSubsystem::Drive(frc::ChassisSpeeds speeds) { DriveSubsystem::SetModuleStates( m_driveKinematics.ToSwerveModuleStates(speeds)); @@ -215,37 +265,55 @@ double DriveSubsystem::GetTurnRate() { return -m_pidgey.GetAngularVelocityZWorld().GetValue().value(); } -frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); } +frc::Pose2d DriveSubsystem::GetPose() { return poseEstimator.GetEstimatedPosition(); } void DriveSubsystem::ResetOdometry(frc::Pose2d pose) { - m_odometry.ResetPosition( - GetHeading(), - {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), - m_rearLeft.GetPosition(), m_rearRight.GetPosition()}, - pose); + // m_odometry.ResetPosition( + // GetHeading(), + // {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), + // m_rearLeft.GetPosition(), m_rearRight.GetPosition()}, + // pose); + + m_lastGoodPosition = pose; + poseEstimator.ResetPosition(GetHeading(), GetModulePositions(), pose); } void DriveSubsystem::ResetRotation() { m_pidgey.Reset(); } +void DriveSubsystem::logDrivebase() { + std::vector states_vec = {m_frontLeft.GetState(), m_frontRight.GetState(), + m_rearLeft.GetState(), m_rearRight.GetState()}; + std::span states(states_vec.begin(), + states_vec.end()); + std::vector desired_vec = { + m_frontLeft.GetDesiredState(), m_frontRight.GetDesiredState(), + m_rearLeft.GetDesiredState(), m_rearRight.GetDesiredState()}; + std::span desiredStates(desired_vec.begin(), + desired_vec.end()); + + m_publisher.Set(states); + m_desiredPublisher.Set(desiredStates); +} + void DriveSubsystem::OffsetRotation(frc::Rotation2d offset) { m_pidgey.SetYaw(offset.Degrees() + m_pidgey.GetYaw().GetValue()); - m_odometry.ResetPosition( - m_pidgey.GetYaw().GetValue(), - {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), - m_rearLeft.GetPosition(), m_rearRight.GetPosition()}, - m_odometry.GetPose()); + // m_odometry.ResetPosition( + // m_pidgey.GetYaw().GetValue(), + // {m_frontLeft.GetPosition(), m_frontRight.GetPosition(), + // m_rearLeft.GetPosition(), m_rearRight.GetPosition()}, + // m_odometry.GetPose()); } void DriveSubsystem::AddVisionMeasurement(const frc::Pose2d& visionMeasurement, units::second_t timestamp) { - // poseEstimator.AddVisionMeasurement(visionMeasurement, timestamp); + poseEstimator.AddVisionMeasurement(visionMeasurement, timestamp); } void DriveSubsystem::AddVisionMeasurement(const frc::Pose2d& visionMeasurement, units::second_t timestamp, const Eigen::Vector3d& stdDevs) { - // wpi::array newStdDevs{stdDevs(0), stdDevs(1), stdDevs(2)}; - // poseEstimator.AddVisionMeasurement(visionMeasurement, timestamp, newStdDevs); + wpi::array newStdDevs{stdDevs(0), stdDevs(1), stdDevs(2)}; + poseEstimator.AddVisionMeasurement(visionMeasurement, timestamp, newStdDevs); } \ No newline at end of file diff --git a/src/main/cpp/subsystems/ElevatorSubsystem.cpp b/src/main/cpp/subsystems/ElevatorSubsystem.cpp index 2fc6ae9..c7a3a7c 100644 --- a/src/main/cpp/subsystems/ElevatorSubsystem.cpp +++ b/src/main/cpp/subsystems/ElevatorSubsystem.cpp @@ -15,10 +15,10 @@ bool ElevatorSubsystem::GetMaxLimitSwitch() { void ElevatorSubsystem::Periodic() { if (GetMaxLimitSwitch()) { - std::cout << "Elevator at max" << std::endl; + // std::cout << "Elevator at max" << std::endl; SetEncoderPosition(ElevatorConstants::kMaxDistance); } else if (GetMinLimitSwitch()) { - std::cout << "Elevator at min" << std::endl; + // std::cout << "Elevator at min" << std::endl; SetEncoderPosition(ElevatorConstants::kMinDistance); } diff --git a/src/main/cpp/subsystems/MAXSwerveModule.cpp b/src/main/cpp/subsystems/MAXSwerveModule.cpp index 83b1b7d..ccee435 100644 --- a/src/main/cpp/subsystems/MAXSwerveModule.cpp +++ b/src/main/cpp/subsystems/MAXSwerveModule.cpp @@ -5,6 +5,7 @@ #include "subsystems/MAXSwerveModule.h" #include +#include #include "subsystems/Configs.h" @@ -25,18 +26,50 @@ MAXSwerveModule::MAXSwerveModule(const int drivingCANId, const int turningCANId, SparkBase::PersistMode::kPersistParameters); m_chassisAngularOffset = chassisAngularOffset; - m_desiredState.angle = - frc::Rotation2d(units::radian_t{m_turningAbsoluteEncoder.GetPosition()}); - m_drivingEncoder.SetPosition(0); + if (frc::RobotBase::IsReal()) { + m_desiredState.angle = + frc::Rotation2d(units::radian_t{m_turningAbsoluteEncoder.GetPosition()}); + m_drivingEncoder.SetPosition(0); + } } frc::SwerveModuleState MAXSwerveModule::GetState() const { + if (!frc::RobotBase::IsReal()) { + return GetSimState(); + } + return {units::meters_per_second_t{m_drivingEncoder.GetVelocity()}, units::radian_t{m_turningAbsoluteEncoder.GetPosition() - m_chassisAngularOffset}}; } +frc::SwerveModuleState MAXSwerveModule::GetSimState() const { + return frc::SwerveModuleState{ + m_simDriveEncoderVelocity, + frc::Rotation2d{ + units::radian_t{m_simCurrentAngle.value() - m_chassisAngularOffset}}}; +} + +frc::SwerveModulePosition MAXSwerveModule::GetSimPosition() const { + return frc::SwerveModulePosition{ + m_simDriveEncoderPosition, + frc::Rotation2d{ + units::radian_t{m_simCurrentAngle.value() - m_chassisAngularOffset}}}; +} + +frc::Rotation2d MAXSwerveModule::GetRotation() const { + if (!frc::RobotBase::IsReal()) { + return frc::Rotation2d{m_simCurrentAngle}; + } + return frc::Rotation2d{ + units::radian_t{m_turningAbsoluteEncoder.GetPosition()}}; +} + frc::SwerveModulePosition MAXSwerveModule::GetPosition() const { + if (!frc::RobotBase::IsReal()) { + return GetSimPosition(); + } + return {units::meter_t{m_drivingEncoder.GetPosition()}, units::radian_t{m_turningAbsoluteEncoder.GetPosition() - m_chassisAngularOffset}}; @@ -53,7 +86,7 @@ void MAXSwerveModule::SetDesiredState( // Optimize the reference state to avoid spinning further than 90 degrees. correctedDesiredState.Optimize( - frc::Rotation2d(units::radian_t{m_turningAbsoluteEncoder.GetPosition()})); + GetRotation()); m_drivingClosedLoopController.SetReference( (double)correctedDesiredState.speed, SparkMax::ControlType::kVelocity); @@ -62,6 +95,18 @@ void MAXSwerveModule::SetDesiredState( SparkMax::ControlType::kPosition); m_desiredState = desiredState; + + if (!frc::RobotBase::IsReal()) { + simUpdateDrivePosition(correctedDesiredState); + m_simCurrentAngle = correctedDesiredState.angle.Radians(); + } +} + +void MAXSwerveModule::simUpdateDrivePosition( + const frc::SwerveModuleState& desiredState) { + m_simDriveEncoderVelocity = desiredState.speed; + m_simDriveEncoderPosition += + m_simDriveEncoderVelocity * DriveConstants::kPeriodicInterval; } void MAXSwerveModule::ResetEncoders() { m_drivingEncoder.SetPosition(0); } diff --git a/src/main/deploy/pathplanner/autos/Center To Left Source (2 coral).auto b/src/main/deploy/pathplanner/autos/Center To Left Source (2 coral).auto new file mode 100644 index 0000000..6edf07c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center To Left Source (2 coral).auto @@ -0,0 +1,88 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Home" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Start To Reef Left Side Left" + } + }, + { + "type": "named", + "data": { + "name": "To L2 Position" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Expel Coral" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Reef to Far Left Source" + } + }, + { + "type": "named", + "data": { + "name": "To Feed Position" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Far Left Source to Right Reef" + } + }, + { + "type": "named", + "data": { + "name": "To L2 Position" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Expel Coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Center To Left Source.auto b/src/main/deploy/pathplanner/autos/Center To Left Source.auto index 3cdd981..9f54e32 100644 --- a/src/main/deploy/pathplanner/autos/Center To Left Source.auto +++ b/src/main/deploy/pathplanner/autos/Center To Left Source.auto @@ -11,9 +11,22 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "Center Start To Reef Left Side Left" + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Start To Reef Left Side Left" + } + }, + { + "type": "named", + "data": { + "name": "To L2 Position" + } + } + ] } }, { @@ -23,27 +36,66 @@ } }, { - "type": "path", + "type": "parallel", "data": { - "pathName": "Left Reef to Far Left Source" + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Reef to Far Left Source" + } + }, + { + "type": "named", + "data": { + "name": "To Feed Position" + } + } + ] } }, { - "type": "wait", + "type": "parallel", "data": { - "waitTime": 2.0 + "commands": [ + { + "type": "path", + "data": { + "pathName": "Far Left Source to Right Reef" + } + }, + { + "type": "named", + "data": { + "name": "To L2 Position" + } + } + ] } }, { - "type": "path", + "type": "named", "data": { - "pathName": "Far Left Source to Right Reef" + "name": "Expel Coral" } }, { - "type": "named", + "type": "parallel", "data": { - "name": "Expel Coral" + "commands": [ + { + "type": "path", + "data": { + "pathName": "Far Left Reef To Left Source" + } + }, + { + "type": "named", + "data": { + "name": "To Feed Position" + } + } + ] } } ] diff --git a/src/main/deploy/pathplanner/autos/Left Auto (0 Coral).auto b/src/main/deploy/pathplanner/autos/Left Auto (0 Coral).auto new file mode 100644 index 0000000..a8d6418 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left Auto (0 Coral).auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Home" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Side Forward" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left Auto (2 Coral).auto b/src/main/deploy/pathplanner/autos/Left Auto (2 Coral).auto deleted file mode 100644 index 6c246f0..0000000 --- a/src/main/deploy/pathplanner/autos/Left Auto (2 Coral).auto +++ /dev/null @@ -1,55 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Home" - } - }, - { - "type": "path", - "data": { - "pathName": "Left Start to Reef Left Side Left" - } - }, - { - "type": "named", - "data": { - "name": "Expel Coral" - } - }, - { - "type": "path", - "data": { - "pathName": "Left Reef to Far Left Source" - } - }, - { - "type": "named", - "data": { - "name": "Pause" - } - }, - { - "type": "path", - "data": { - "pathName": "Far Left Source to Right Reef" - } - }, - { - "type": "named", - "data": { - "name": "Expel Coral" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left To Left Source (2 Coral).auto b/src/main/deploy/pathplanner/autos/Left To Left Source (2 Coral).auto new file mode 100644 index 0000000..1a0ef2f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left To Left Source (2 Coral).auto @@ -0,0 +1,94 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Home" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Start to Reef Left Side Left" + } + }, + { + "type": "named", + "data": { + "name": "To L1 Position" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Expel Coral" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Reef to Far Left Source" + } + }, + { + "type": "named", + "data": { + "name": "To Feed Position" + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "To L1 Position" + } + }, + { + "type": "path", + "data": { + "pathName": "Far Left Source to Right Reef" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Expel Coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right Auto (0 Coral).auto b/src/main/deploy/pathplanner/autos/Right Auto (0 Coral).auto new file mode 100644 index 0000000..51b0889 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right Auto (0 Coral).auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Home" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Side Forward" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right Auto (2 Coral).auto b/src/main/deploy/pathplanner/autos/Right Auto (2 Coral).auto deleted file mode 100644 index f756f84..0000000 --- a/src/main/deploy/pathplanner/autos/Right Auto (2 Coral).auto +++ /dev/null @@ -1,55 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Home" - } - }, - { - "type": "path", - "data": { - "pathName": "Right Start To Reef Right Side Right" - } - }, - { - "type": "named", - "data": { - "name": "Expel Coral" - } - }, - { - "type": "path", - "data": { - "pathName": "Reef Right Side Right To Far Right Source" - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "path", - "data": { - "pathName": "Far Right Source To Reef Right Side Left" - } - }, - { - "type": "named", - "data": { - "name": "Expel Coral" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right To Right Source (2 Coral).auto b/src/main/deploy/pathplanner/autos/Right To Right Source (2 Coral).auto new file mode 100644 index 0000000..e89d962 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right To Right Source (2 Coral).auto @@ -0,0 +1,132 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Home" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Start To Reef Right Side Right" + } + }, + { + "type": "named", + "data": { + "name": "To L1 Position" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Expel Coral" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Reef Right Side Right To Far Right Source" + } + }, + { + "type": "named", + "data": { + "name": "To Feed Position" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Far Right Source To Reef Right Side Left" + } + }, + { + "type": "named", + "data": { + "name": "To L1 Position" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Expel Coral" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Far Right Reef To Right Source" + } + }, + { + "type": "named", + "data": { + "name": "To Feed Position" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Far Right Source To Reef Right Side Left" + } + }, + { + "type": "named", + "data": { + "name": "To L1 Position" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Expel Coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Start To Reef Left Side Left.path b/src/main/deploy/pathplanner/paths/Center Start To Reef Left Side Left.path index aad7556..fb68ef7 100644 --- a/src/main/deploy/pathplanner/paths/Center Start To Reef Left Side Left.path +++ b/src/main/deploy/pathplanner/paths/Center Start To Reef Left Side Left.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.46639344262295, - "y": 5.205788934426229 + "x": 5.303999999999999, + "y": 5.039 }, "prevControl": { - "x": 6.1425, - "y": 5.96525 + "x": 5.9801065573770495, + "y": 5.798461065573771 }, "nextControl": null, "isLocked": false, @@ -31,19 +31,7 @@ "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "To L2 Position", - "waypointRelativePos": 0.22133995037220952, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "To L2 Position" - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, @@ -60,7 +48,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -120.19162296095763 + "rotation": 178.7659627954506 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Start To Reef Right Side Right.path b/src/main/deploy/pathplanner/paths/Center Start To Reef Right Side Right.path index a2bbda6..53f30e2 100644 --- a/src/main/deploy/pathplanner/paths/Center Start To Reef Right Side Right.path +++ b/src/main/deploy/pathplanner/paths/Center Start To Reef Right Side Right.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.403652597402597, - "y": 2.8806412337662333 + "x": 5.29425, + "y": 3.0987499999999994 }, "prevControl": { - "x": 6.230250000000001, - "y": 1.63625 + "x": 6.120847402597404, + "y": 1.8543587662337662 }, "nextControl": null, "isLocked": false, @@ -31,19 +31,7 @@ "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "To L2 Position", - "waypointRelativePos": 0.29875930521091965, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "To L2 Position" - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, @@ -60,7 +48,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 125.75388725443663 + "rotation": -179.00470734434913 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Straight.path b/src/main/deploy/pathplanner/paths/Center Straight.path index c19e3b9..c4827c7 100644 --- a/src/main/deploy/pathplanner/paths/Center Straight.path +++ b/src/main/deploy/pathplanner/paths/Center Straight.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 6.172414772727272, + "x": 5.909938524590164, "y": 4.015249999999999 }, "prevControl": { - "x": 6.831003392858172, + "x": 6.568527144721063, "y": 4.015249999999999 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Center To Rear Right Reef.path b/src/main/deploy/pathplanner/paths/Center To Rear Right Reef.path index 102b836..c379c32 100644 --- a/src/main/deploy/pathplanner/paths/Center To Rear Right Reef.path +++ b/src/main/deploy/pathplanner/paths/Center To Rear Right Reef.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 5.382, - "y": 5.204749999999999 + "x": 6.6651639344262295, + "y": 4.186834016393442 }, "isLocked": false, "linkedName": "Center Start" @@ -20,8 +20,8 @@ "y": 4.210249999999999 }, "prevControl": { - "x": 7.029749999999999, - "y": 5.409499999999999 + "x": 6.689139344262295, + "y": 4.1748463114754095 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Far Left Reef To Left Source.path b/src/main/deploy/pathplanner/paths/Far Left Reef To Left Source.path new file mode 100644 index 0000000..5a6a079 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Far Left Reef To Left Source.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.96275, + "y": 5.06825 + }, + "prevControl": null, + "nextControl": { + "x": 5.46975, + "y": 5.92625 + }, + "isLocked": false, + "linkedName": "Left Reef Far Side" + }, + { + "anchor": { + "x": 1.1028688524590162, + "y": 7.339600409836065 + }, + "prevControl": { + "x": 5.50875, + "y": 5.897 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Far Left Source" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -52.00126755749546 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -120.69972255081439 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Far Left Source to Right Reef.path b/src/main/deploy/pathplanner/paths/Far Left Source to Right Reef.path index 03528dd..6ae64fb 100644 --- a/src/main/deploy/pathplanner/paths/Far Left Source to Right Reef.path +++ b/src/main/deploy/pathplanner/paths/Far Left Source to Right Reef.path @@ -3,47 +3,35 @@ "waypoints": [ { "anchor": { - "x": 1.766396103896104, - "y": 7.34411525974026 + "x": 1.1028688524590162, + "y": 7.339600409836065 }, "prevControl": null, "nextControl": { - "x": 2.766396103896104, - "y": 7.34411525974026 + "x": 2.102868852459017, + "y": 7.339600409836065 }, "isLocked": false, "linkedName": "Far Left Source" }, { "anchor": { - "x": 5.166700819672132, - "y": 5.457530737704917 + "x": 4.96275, + "y": 5.06825 }, "prevControl": { - "x": 7.036782786885246, - "y": 6.6563012295081965 + "x": 6.832831967213115, + "y": 6.2670204918032795 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Left Reef Far Side" } ], "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "", - "waypointRelativePos": 0.2619851451721815, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "To L2 Position" - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/Far Right Reef To Right Source.path b/src/main/deploy/pathplanner/paths/Far Right Reef To Right Source.path new file mode 100644 index 0000000..54fae6c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Far Right Reef To Right Source.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.974897540983607, + "y": 2.9880635245901637 + }, + "prevControl": null, + "nextControl": { + "x": 5.540397540983608, + "y": 2.237313524590164 + }, + "isLocked": false, + "linkedName": "Right Reef Far Side" + }, + { + "anchor": { + "x": 0.5274590163934426, + "y": 0.8662397540983602 + }, + "prevControl": { + "x": 4.759118852459015, + "y": 2.3047643442622934 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Far Right Source" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 53.56914187983765 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 122.58570735391453 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Far Right Source To Reef Right Side Left.path b/src/main/deploy/pathplanner/paths/Far Right Source To Reef Right Side Left.path index 81311fa..b3e41c7 100644 --- a/src/main/deploy/pathplanner/paths/Far Right Source To Reef Right Side Left.path +++ b/src/main/deploy/pathplanner/paths/Far Right Source To Reef Right Side Left.path @@ -3,47 +3,35 @@ "waypoints": [ { "anchor": { - "x": 1.6809253246753246, - "y": 0.7391233766233768 + "x": 0.5274590163934426, + "y": 0.8662397540983602 }, "prevControl": null, "nextControl": { - "x": 3.978, - "y": 1.3729999999999996 + "x": 2.8245336917181167, + "y": 1.5001163774749826 }, "isLocked": false, "linkedName": "Far Right Source" }, { "anchor": { - "x": 5.151988636363637, - "y": 2.6812094155844157 + "x": 4.974897540983607, + "y": 2.9880635245901637 }, "prevControl": { - "x": 5.343, - "y": 1.1389999999999998 + "x": 5.16590890461997, + "y": 1.4458541090057477 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Right Reef Far Side" } ], "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "", - "waypointRelativePos": 0.2722427224272228, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "To L2 Position" - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/Left Border Start To Bottom Left Reef.path b/src/main/deploy/pathplanner/paths/Left Border Start To Bottom Left Reef.path index d1dbd0c..c44871b 100644 --- a/src/main/deploy/pathplanner/paths/Left Border Start To Bottom Left Reef.path +++ b/src/main/deploy/pathplanner/paths/Left Border Start To Bottom Left Reef.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.08025641025641, - "y": 5.046730769230769 + "x": 5.142725409836065, + "y": 5.301690573770491 }, "prevControl": { - "x": 5.9776995920745915, - "y": 5.894315996503496 + "x": 6.040168591654247, + "y": 6.149275801043219 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left Reef to Far Left Source.path b/src/main/deploy/pathplanner/paths/Left Reef to Far Left Source.path index cc2de16..671916a 100644 --- a/src/main/deploy/pathplanner/paths/Left Reef to Far Left Source.path +++ b/src/main/deploy/pathplanner/paths/Left Reef to Far Left Source.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.46639344262295, - "y": 5.205788934426229 + "x": 5.303999999999999, + "y": 5.039 }, "prevControl": null, "nextControl": { - "x": 6.50325, - "y": 6.95 + "x": 6.34085655737705, + "y": 6.783211065573771 }, "isLocked": false, "linkedName": "Left Reef" }, { "anchor": { - "x": 1.766396103896104, - "y": 7.34411525974026 + "x": 1.1028688524590162, + "y": 7.339600409836065 }, "prevControl": { - "x": 2.65909090909091, - "y": 6.4324269480519485 + "x": 1.9955636576538218, + "y": 6.427912098147754 }, "nextControl": null, "isLocked": false, @@ -31,19 +31,7 @@ "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "", - "waypointRelativePos": 0.237677245104656, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "To Feed Position" - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/Left Side Forward.path b/src/main/deploy/pathplanner/paths/Left Side Forward.path new file mode 100644 index 0000000..c4c0db0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Side Forward.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.552254098360655, + "y": 7.483452868852458 + }, + "prevControl": null, + "nextControl": { + "x": 6.92889344262295, + "y": 7.279661885245901 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.137704918032787, + "y": 7.06388319672131 + }, + "prevControl": { + "x": 6.904918032786885, + "y": 7.267674180327869 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -177.33699923393286 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Start to Reef Left Side Left.path b/src/main/deploy/pathplanner/paths/Left Start to Reef Left Side Left.path index 5cf9299..6886f2b 100644 --- a/src/main/deploy/pathplanner/paths/Left Start to Reef Left Side Left.path +++ b/src/main/deploy/pathplanner/paths/Left Start to Reef Left Side Left.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.763595779220778, - "y": 7.424837662337662 + "x": 7.566, + "y": 7.25225 }, "prevControl": null, "nextControl": { - "x": 6.695211038961039, - "y": 6.408685064935064 + "x": 6.497615259740261, + "y": 6.236097402597402 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.46639344262295, - "y": 5.205788934426229 + "x": 5.303999999999999, + "y": 5.039 }, "prevControl": { - "x": 6.685714285714286, - "y": 6.418181818181818 + "x": 6.523320843091335, + "y": 6.251392883755589 }, "nextControl": null, "isLocked": false, @@ -31,19 +31,7 @@ "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "", - "waypointRelativePos": 0.35, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "To L2 Position" - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, @@ -60,7 +48,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -113.76710770458172 + "rotation": 180.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Straight.path b/src/main/deploy/pathplanner/paths/Left Straight.path new file mode 100644 index 0000000..15d7cba --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Straight.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.5855, + "y": 7.505749999999999 + }, + "prevControl": null, + "nextControl": { + "x": 6.6690000000000005, + "y": 7.4765 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.24, + "y": 7.29125 + }, + "prevControl": { + "x": 6.649500000000001, + "y": 7.4765 + }, + "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": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -178.85423716182484 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef Right Side Right To Far Right Source.path b/src/main/deploy/pathplanner/paths/Reef Right Side Right To Far Right Source.path index 4b9b26c..4ed9de7 100644 --- a/src/main/deploy/pathplanner/paths/Reef Right Side Right To Far Right Source.path +++ b/src/main/deploy/pathplanner/paths/Reef Right Side Right To Far Right Source.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.403652597402597, - "y": 2.8806412337662333 + "x": 5.29425, + "y": 3.0987499999999994 }, "prevControl": null, "nextControl": { - "x": 6.15225, - "y": 1.63625 + "x": 6.0428474025974035, + "y": 1.8543587662337662 }, "isLocked": false, "linkedName": "Reef Right Side Right" }, { "anchor": { - "x": 1.6809253246753246, - "y": 0.7391233766233768 + "x": 0.5274590163934426, + "y": 0.8662397540983602 }, "prevControl": { - "x": 4.251000000000001, - "y": 1.7044999999999992 + "x": 3.0975336917181187, + "y": 1.8316163774749827 }, "nextControl": null, "isLocked": false, @@ -31,19 +31,7 @@ "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "", - "waypointRelativePos": 0.7511275112751122, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "To Feed Position" - } - } - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, diff --git a/src/main/deploy/pathplanner/paths/Right Border Start To Bottom Right Reef.path b/src/main/deploy/pathplanner/paths/Right Border Start To Bottom Right Reef.path index 589a8ba..8a6b464 100644 --- a/src/main/deploy/pathplanner/paths/Right Border Start To Bottom Right Reef.path +++ b/src/main/deploy/pathplanner/paths/Right Border Start To Bottom Right Reef.path @@ -9,19 +9,19 @@ "prevControl": null, "nextControl": { "x": 6.4915056818181816, - "y": 1.6966335227272735 + "y": 1.6966335227272733 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.111217948717948, - "y": 3.0239102564102565 + "x": 5.238627049180328, + "y": 2.8082479508196716 }, "prevControl": { - "x": 6.23800772144522, - "y": 1.9569500291375306 + "x": 6.3654168219076, + "y": 1.741287723546946 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Right Side Forward.path b/src/main/deploy/pathplanner/paths/Right Side Forward.path new file mode 100644 index 0000000..4906c6a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Side Forward.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.564241803278687, + "y": 0.5545594262295083 + }, + "prevControl": null, + "nextControl": { + "x": 7.036782786885246, + "y": 0.842264344262295 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.305532786885245, + "y": 1.2018954918032796 + }, + "prevControl": { + "x": 7.01280737704918, + "y": 0.8302766393442618 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -178.5679038158353 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Start To Reef Right Side Right.path b/src/main/deploy/pathplanner/paths/Right Start To Reef Right Side Right.path index 5a1f5ef..6e0c9dd 100644 --- a/src/main/deploy/pathplanner/paths/Right Start To Reef Right Side Right.path +++ b/src/main/deploy/pathplanner/paths/Right Start To Reef Right Side Right.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.611647727272728, - "y": 0.6536525974025983 + "x": 7.517250000000001, + "y": 0.7977499999999997 }, "prevControl": null, "nextControl": { - "x": 6.339079787908425, - "y": 1.8217499737682514 + "x": 6.234565573770492, + "y": 2.032483606557377 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.403652597402597, - "y": 2.8806412337662333 + "x": 5.29425, + "y": 3.0987499999999994 }, "prevControl": { - "x": 6.405560064935065, - "y": 1.9214691558441552 + "x": 6.196130189482648, + "y": 2.0433649137747483 }, "nextControl": null, "isLocked": false, @@ -31,14 +31,7 @@ "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "To L2 Position", - "waypointRelativePos": 0.21141439205955234, - "endWaypointRelativePos": null, - "command": null - } - ], + "eventMarkers": [], "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, @@ -55,7 +48,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 133.60281897270357 + "rotation": 178.72446237431527 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Straight.path b/src/main/deploy/pathplanner/paths/Right Straight.path new file mode 100644 index 0000000..e688303 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Straight.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.5855, + "y": 7.515499999999999 + }, + "prevControl": null, + "nextControl": { + "x": 6.4545, + "y": 7.262 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.6354999999999995, + "y": 7.2132499999999995 + }, + "prevControl": { + "x": 6.28875, + "y": 7.681249999999999 + }, + "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": -179.11859600341793 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -179.38394009160075 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/include/Constants.h b/src/main/include/Constants.h index c2b9279..dbbc1ae 100644 --- a/src/main/include/Constants.h +++ b/src/main/include/Constants.h @@ -19,6 +19,12 @@ #include #include +#include +#include +#include +#include +#include + #include #include @@ -87,8 +93,14 @@ namespace DriveConstants { constexpr units::meters_per_second_t kMaxSpeed = 4.92_mps; constexpr units::radians_per_second_t kMaxAngularSpeed{2 * std::numbers::pi}; +constexpr double kLowSensivityCoefficient = 0.3; + +constexpr units::second_t kSetXThreshold = 0.5_s; + const int kPigeonCanId = 13; +constexpr units::second_t kPeriodicInterval = 20_ms; + 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%) @@ -153,6 +165,10 @@ constexpr auto kMaxAngularAcceleration = 3.142_rad_per_s_sq; const std::string kCenterToCenterAuto = "Center Auto (1 Coral)"; const std::string kRightToRightReefAuto = "Right To Right Reef (1 Coral)"; const std::string kLeftToLeftReefAuto = "Left To Left Reef (1 Coral)"; +const std::string kRightToRightSourceAuto = "Right To Right Source (2 Coral)"; +const std::string kLeftToLeftSourceAuto = "Left To Left Source (2 Coral)"; +const std::string kCenterToRightSourceAuto = "Center To Right Source (2 Coral)"; +const std::string kCenterToLeftSourceAuto = "Center To Right Source (2 Coral)"; const std::string kForwardAuto = "Center Auto (0 Coral)"; constexpr double kPXController = 0.5; @@ -167,7 +183,7 @@ constexpr units::second_t kFeedWaitTime = 2_s; namespace OIConstants { constexpr int kDriverControllerPort = 0; -constexpr double kDriveDeadband = 0.05; +constexpr double kDriveDeadband = 0.08; } // namespace OIConstants namespace GyroConstants { @@ -181,15 +197,8 @@ namespace ElevatorConstants { const int kBottomLimitSwitchPort = 1; const int kTopLimitSwitchPort = 2; - // Placeholder values - const double kElevatorP = 70.0; - const double kElevatorI = 0.0; - const double kElevatorD = 0.0; - const double kElevatorIZone = 0.0; - const double kElevatorFF = 0.0; - const subzero::PidSettings kElevatorPidSettings = { - 40.0, 0.0, 0.0, + 160.0, 0.0, 0.0, 0.0, 0.0, false}; const subzero::PidSettings kHomePidSettings = { @@ -206,7 +215,7 @@ namespace ElevatorConstants { constexpr units::meter_t kRelativeDistancePerRev = (1.685_in * M_PI) / 36; // Pitch diameter of gear with 36:1 ratio gearbox constexpr units::meters_per_second_t kDefaultVelocity = 0.66_mps; constexpr double kVelocityScalar = 1.0; - constexpr units::meter_t kTolerance = 0.5_in; + constexpr units::meter_t kTolerance = 0.1_in; // Placeholder const subzero::SingleAxisMechanism kElevatorMechanism { @@ -219,7 +228,7 @@ namespace ElevatorConstants { // color subzero::ColorConstants::kRed}; - const frc::TrapezoidProfile::Constraints kElevatorProfileConstraints{1_fps * 10, 0.75_fps_sq * 20}; + const frc::TrapezoidProfile::Constraints kElevatorProfileConstraints{1_fps * 10, 20_fps_sq}; }; namespace AlgaeArmConstants{ @@ -281,7 +290,7 @@ namespace CoralArmConstants{ constexpr units::meter_t kArmLength = 0.2_m; // placeholder - constexpr double kHasCoralCurrent = 36; + constexpr double kHasCoralCurrent = 38; static const subzero::SingleAxisMechanism kCoralArmMechanism = { // length @@ -351,7 +360,7 @@ namespace CommandConstants { constexpr units::degree_t kCoralL1Position = 249_deg; constexpr units::degree_t kCoralL2Position = 240_deg; - constexpr units::degree_t kCoralL3Position = 218_deg; + constexpr units::degree_t kCoralL3Position = 220_deg; constexpr units::degree_t kCoralFeedPosition = 77_deg; constexpr units::degree_t kCoralHomePosition = 110_deg; constexpr units::degree_t kCoralClimbPosition = 50_deg; @@ -376,8 +385,7 @@ namespace CommandConstants { constexpr units::degree_t kClimberDownAngle = 0_deg; constexpr units::degree_t kClimberUpAngle = 110_deg; - // Placeholder - constexpr units::second_t kCoralFeedTimeout = 3_s; + constexpr units::second_t kCoralFeedTimeout = 4_s; constexpr units::second_t kAlgaeIntakeTimeout = 5_s; constexpr units::second_t kRemoveAlgaeFromReefTimeout = 3_s; constexpr units::second_t kExpelCoralTimeout = 1_s; @@ -385,18 +393,23 @@ namespace CommandConstants { } namespace VisionConstants { -static constexpr std::string_view kFrontCamera{"PhotonVision"}; -static constexpr std::string_view kRearCamera{"PhotonVision2"}; -static const frc::Transform3d kRobotToCam2{ - frc::Translation3d{2.147_in, 0_in, 23.369_in}, - frc::Rotation3d{-90_deg, -0_deg, -140_deg}}; -static const frc::Transform3d kRobotToCam{ - frc::Translation3d{5.714_in, 0_in, 23.533_in}, - frc::Rotation3d{90_deg, -23.461_deg, 140_deg}}; +static constexpr std::string_view kLeftCameraName{"PhotonVisionLeft"}; +// static constexpr std::string_view kRightCameraName{"purplePipeCam"}; +// static constexpr std::string_view kBackCameraName{"PhotonVision3"}; +static const frc::Transform3d kRobotToCamLeft{ + frc::Translation3d{4.52_in, 0_in, 23.41_in}, + frc::Rotation3d{0_deg, -0_deg, 180_deg}}; +// static const frc::Transform3d kRobotToCamRight{ +// frc::Translation3d{2.514_in, 8.839_in, 25.22_in}, +// frc::Rotation3d{-90_deg, 0_deg, 10_deg}}; +// static const frc::Transform3d kRobotToCamBack{ +// frc::Translation3d{4.52_in, 0_in, 23.41_in}, +// frc::Rotation3d{0_deg, -0_deg, 180_deg}}; + constexpr photon::PoseStrategy kPoseStrategy = photon::PoseStrategy::MULTI_TAG_PNP_ON_COPROCESSOR; static const frc::AprilTagFieldLayout kTagLayout{ - frc::LoadAprilTagLayoutField(frc::AprilTagField::k2024Crescendo)}; + frc::LoadAprilTagLayoutField(frc::AprilTagField::k2025ReefscapeWelded)}; static const Eigen::Matrix kSingleTagStdDevs{4, 4, 8}; static const Eigen::Matrix kMultiTagStdDevs{0.5, 0.5, 1}; diff --git a/src/main/include/RobotContainer.h b/src/main/include/RobotContainer.h index 0f07fbf..b1522fb 100644 --- a/src/main/include/RobotContainer.h +++ b/src/main/include/RobotContainer.h @@ -85,4 +85,13 @@ class RobotContainer { }; CommandController m_commandController{subsystems}; + + units::second_t m_timeSinceControllerInput; + units::second_t m_defaultLastCalled; + + bool m_zeroedBeforeSetX; + + bool m_isReducedSensitivity; + + double m_lowSensitivityCoefficient; }; \ No newline at end of file diff --git a/src/main/include/subsystems/DriveSubsystem.h b/src/main/include/subsystems/DriveSubsystem.h index fbb839e..668a5b7 100644 --- a/src/main/include/subsystems/DriveSubsystem.h +++ b/src/main/include/subsystems/DriveSubsystem.h @@ -6,11 +6,14 @@ #include #include +#include #include #include #include #include #include +#include +#include #include #include #include @@ -30,6 +33,11 @@ #include #include #include +#include +#include +#include +#include +#include #include "Constants.h" #include "MAXSwerveModule.h" @@ -62,6 +70,8 @@ class DriveSubsystem : public frc2::SubsystemBase { void Drive(frc::ChassisSpeeds speeds); + frc2::CommandPtr MoveToAngle(units::degree_t angle); + /** * Sets the wheels into an X formation to prevent movement. */ @@ -114,6 +124,8 @@ class DriveSubsystem : public frc2::SubsystemBase { void ResetRotation(); + void SimulationPeriodic() override; + frc::SwerveDriveKinematics<4> m_driveKinematics{ frc::Translation2d{DriveConstants::kWheelBase / 2, DriveConstants::kTrackWidth / 2}, @@ -130,6 +142,13 @@ class DriveSubsystem : public frc2::SubsystemBase { units::second_t timestamp, const Eigen::Vector3d& stdDevs); + wpi::array GetModulePositions() const; + +frc::ChassisSpeeds GetSpeedsFromJoystick( + units::meters_per_second_t xSpeed, units::meters_per_second_t ySpeed, + units::radians_per_second_t rot, bool fieldRelative); + + void logDrivebase(); private: // Components (e.g. motor controllers and sensors) should generally be // declared private and exposed only through public methods. @@ -143,6 +162,8 @@ class DriveSubsystem : public frc2::SubsystemBase { // Gyro Sensor ctre::phoenix6::hardware::Pigeon2 m_pidgey{DriveConstants::kPigeonCanId, "rio"}; + ctre::phoenix6::sim::Pigeon2SimState& m_simPidgey = m_pidgey.GetSimState(); + units::time::second_t currentTime{frc::Timer::GetFPGATimestamp()}; // Odometry class for tracking robot pose @@ -164,7 +185,37 @@ class DriveSubsystem : public frc2::SubsystemBase { frc::Field2d m_field; frc::Pose2d m_lastGoodPosition; - subzero::PhotonVisionEstimators* m_vision; + photon::PhotonPoseEstimator poseLeft{ + // layout + VisionConstants::kTagLayout, + // strategy + VisionConstants::kPoseStrategy, + // offsets + VisionConstants::kRobotToCamLeft}; + + // photon::PhotonPoseEstimator poseRight{ + // // layout + // VisionConstants::kTagLayout, + // // strategy + // VisionConstants::kPoseStrategy, + // // offsets + // VisionConstants::kRobotToCamRight}; + + photon::PhotonCamera m_leftCamera{VisionConstants::kLeftCameraName}; + // photon::PhotonCamera m_rightCamera{VisionConstants::kRightCameraName}; + + std::vector + poseCameras{ + subzero::PhotonVisionEstimators::PhotonCameraEstimator(poseLeft, m_leftCamera), + // subzero::PhotonVisionEstimators::PhotonCameraEstimator(poseRight, m_rightCamera), + }; + + subzero::PhotonVisionEstimators m_vision{poseCameras, + VisionConstants::kSingleTagStdDevs, + VisionConstants::kMultiTagStdDevs + }; + + units::second_t m_lastUpdatedTime; }; diff --git a/src/main/include/subsystems/MAXSwerveModule.h b/src/main/include/subsystems/MAXSwerveModule.h index d23d196..fa74b09 100644 --- a/src/main/include/subsystems/MAXSwerveModule.h +++ b/src/main/include/subsystems/MAXSwerveModule.h @@ -47,12 +47,23 @@ class MAXSwerveModule { */ void SetDesiredState(const frc::SwerveModuleState& state); + frc::SwerveModuleState GetDesiredState() { return m_desiredState; } + /** * Zeroes all the SwerveModule encoders. */ void ResetEncoders(); + frc::SwerveModuleState GetSimState() const; + + frc::SwerveModulePosition GetSimPosition() const; + + frc::Rotation2d GetRotation() const; + private: + void simUpdateDrivePosition( + const frc::SwerveModuleState& desiredState); + SparkFlex m_drivingSpark; SparkMax m_turningSpark; @@ -68,4 +79,9 @@ class MAXSwerveModule { double m_chassisAngularOffset = 0; frc::SwerveModuleState m_desiredState{units::meters_per_second_t{0.0}, frc::Rotation2d()}; + + units::meter_t m_simDriveEncoderPosition; + units::meters_per_second_t m_simDriveEncoderVelocity; + + units::radian_t m_simCurrentAngle; }; \ No newline at end of file