diff --git a/src/main/deploy/pathplanner/autos/One Coral Left.auto b/src/main/deploy/pathplanner/autos/One Coral Left.auto index 14000f4..e537c2b 100644 --- a/src/main/deploy/pathplanner/autos/One Coral Left.auto +++ b/src/main/deploy/pathplanner/autos/One Coral Left.auto @@ -7,7 +7,7 @@ { "type": "path", "data": { - "pathName": "Start to 60 Right" + "pathName": "Start to 30 Right" } }, { diff --git a/src/main/deploy/pathplanner/autos/Two Coral Left.auto b/src/main/deploy/pathplanner/autos/Two Coral Left.auto index 5ca978d..3e60427 100644 --- a/src/main/deploy/pathplanner/autos/Two Coral Left.auto +++ b/src/main/deploy/pathplanner/autos/Two Coral Left.auto @@ -7,7 +7,7 @@ { "type": "path", "data": { - "pathName": "Start to 60 Right" + "pathName": "Start to 30 Right" } }, { @@ -19,7 +19,7 @@ { "type": "path", "data": { - "pathName": "60 Right to HP" + "pathName": "30 Right to HP" } }, { @@ -31,7 +31,7 @@ { "type": "path", "data": { - "pathName": "HP to 300 Left" + "pathName": "HP to 330 Left" } }, { diff --git a/src/main/deploy/pathplanner/paths/150 Right to HP.path b/src/main/deploy/pathplanner/paths/150 Right to HP.path new file mode 100644 index 0000000..84b1a07 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/150 Right to HP.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.289041095890411, + "y": 2.9732020547945206 + }, + "prevControl": null, + "nextControl": { + "x": 5.950171232876712, + "y": 1.6208904109589037 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.4424657534246574, + "y": 0.7944777397260271 + }, + "prevControl": { + "x": 2.389083904109589, + "y": 2.5374571917808213 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 54.162347045721745 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 120.96375653207336 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/60 Right to HP.path b/src/main/deploy/pathplanner/paths/30 Right to HP.path similarity index 100% rename from src/main/deploy/pathplanner/paths/60 Right to HP.path rename to src/main/deploy/pathplanner/paths/30 Right to HP.path diff --git a/src/main/deploy/pathplanner/paths/300 Right to HP.path b/src/main/deploy/pathplanner/paths/330 Right to HP.path similarity index 100% rename from src/main/deploy/pathplanner/paths/300 Right to HP.path rename to src/main/deploy/pathplanner/paths/330 Right to HP.path diff --git a/src/main/deploy/pathplanner/paths/HP to 300 Left.path b/src/main/deploy/pathplanner/paths/HP to 330 Left.path similarity index 100% rename from src/main/deploy/pathplanner/paths/HP to 300 Left.path rename to src/main/deploy/pathplanner/paths/HP to 330 Left.path diff --git a/src/main/deploy/pathplanner/paths/Right Start to 150 Right.path b/src/main/deploy/pathplanner/paths/Right Start to 150 Right.path new file mode 100644 index 0000000..0b21d36 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Start to 150 Right.path @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.602996575342465, + "y": 2.0115582191780823 + }, + "prevControl": null, + "nextControl": { + "x": 6.491095890410959, + "y": 1.9965325342465756 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.289041095890411, + "y": 2.9882277397260277 + }, + "prevControl": { + "x": 6.130479452054795, + "y": 2.11673801369863 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Lift L4", + "waypointRelativePos": 0.7142857142857124, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 120.25643716352937 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Start to 60 Right.path b/src/main/deploy/pathplanner/paths/Start to 30 Right.path similarity index 100% rename from src/main/deploy/pathplanner/paths/Start to 60 Right.path rename to src/main/deploy/pathplanner/paths/Start to 30 Right.path diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 57e5f05..84c54d3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,6 +9,7 @@ import frc.robot.constants.ClimberPivotConstants; import frc.robot.constants.ElevatorConstants; import frc.robot.constants.OIConstants; import frc.robot.subsystems.ManipulatorPivot; +import frc.robot.subsystems.Vision; import frc.robot.subsystems.ClimberPivot; import frc.robot.subsystems.ClimberRollers; import frc.robot.subsystems.Drivetrain; @@ -19,6 +20,9 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; @@ -45,13 +49,15 @@ public class RobotContainer { private CommandXboxController operator; private SendableChooser autoChooser; + private Vision vision; public RobotContainer() { climberPivot = new ClimberPivot(); climberRollers = new ClimberRollers(); - drivetrain = new Drivetrain(); + vision = new Vision(drivetrain::getGyroValue); + drivetrain = new Drivetrain(vision); elevator = new Elevator(); //elevator = new ElevatorSysID(); @@ -253,7 +259,7 @@ public class RobotContainer { sensorTab.addDouble("ElevMotor1", elevator::getMotor1); sensorTab.addDouble("ElevMotor2", elevator::getMotor2); - } + } public Command getAutonomousCommand() { return autoChooser.getSelected(); diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 38ae227..a715ce1 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -21,6 +21,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.AutoConstants; @@ -35,13 +36,15 @@ public class Drivetrain extends SubsystemBase { protected MAXSwerveModule m_rearRight; // The gyro sensor - private AHRS ahrs; + private AHRS gyro; // Odometry class for tracking robot pose private SwerveDrivePoseEstimator m_estimator; + private Vision vision; + /** Creates a new DriveSubsystem. */ - public Drivetrain() { + public Drivetrain(Vision vision) { m_frontLeft = new MAXSwerveModule( DrivetrainConstants.kFrontLeftDrivingCanId, DrivetrainConstants.kFrontLeftTurningCanId, @@ -66,11 +69,11 @@ public class Drivetrain extends SubsystemBase { DrivetrainConstants.kBackRightChassisAngularOffset ); - ahrs = new AHRS(NavXComType.kMXP_SPI); + gyro = new AHRS(NavXComType.kMXP_SPI); m_estimator = new SwerveDrivePoseEstimator( DrivetrainConstants.kDriveKinematics, - Rotation2d.fromDegrees(ahrs.getAngle()), + Rotation2d.fromDegrees(gyro.getAngle()), new SwerveModulePosition[] { m_frontLeft.getPosition(), m_frontRight.getPosition(), @@ -109,6 +112,24 @@ public class Drivetrain extends SubsystemBase { m_rearLeft.getPosition(), m_rearRight.getPosition() }); + + + // if the detected tags match your alliances reef tags use their pose estimates + if(vision.getOrangeClosestTag() >= 6 || vision.getOrangeClosestTag() <= 11 || DriverStation.getAlliance().equals(Alliance.Red)){ + m_estimator.addVisionMeasurement(vision.getOrangeGlobalPose(), vision.getOrangeTimeStamp()); + + }else if(vision.getOrangeClosestTag() >= 17 || vision.getOrangeClosestTag() <= 22 || DriverStation.getAlliance().equals(Alliance.Blue)){ + m_estimator.addVisionMeasurement(vision.getOrangeGlobalPose(), vision.getOrangeTimeStamp()); + } + + if(vision.getBlackClosestTag() >= 6 || vision.getBlackClosestTag() <= 11 || DriverStation.getAlliance().equals(Alliance.Red)){ + m_estimator.addVisionMeasurement(vision.getBlackGlobalPose(), vision.getBlackTimeStamp()); + }else if(vision.getBlackClosestTag() >= 17 || vision.getBlackClosestTag() <= 22 || DriverStation.getAlliance().equals(Alliance.Blue)){ + m_estimator.addVisionMeasurement(vision.getBlackGlobalPose(), vision.getBlackTimeStamp()); + } + + + } public ChassisSpeeds getCurrentChassisSpeeds() { @@ -228,11 +249,11 @@ public class Drivetrain extends SubsystemBase { /** Zeroes the heading of the robot. */ public void zeroHeading() { - ahrs.reset(); + gyro.reset(); } public double getGyroValue() { - return ahrs.getAngle() * (DrivetrainConstants.kGyroReversed ? -1 : 1); + return gyro.getAngle() * (DrivetrainConstants.kGyroReversed ? -1 : 1); } /** @@ -250,7 +271,7 @@ public class Drivetrain extends SubsystemBase { * @return The turn rate of the robot, in degrees per second */ public double getTurnRate() { - return ahrs.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0); + return gyro.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0); } public void addVisionMeasurement(Pose2d pose, double timestamp){ diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 35e5efe..7398bf5 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -74,7 +74,6 @@ public class Vision{ } public Pose2d getBlackGlobalPose(){ - return relativeToGlobalPose2d(getBlackClosestTag(), new Translation2d(getBlackRelativeX(), getBlackRelativeY()), new Rotation2d(gyroAngle.getAsDouble())); @@ -109,9 +108,8 @@ public class Vision{ } public Pose2d getOrangeGlobalPose(){ - - return relativeToGlobalPose2d(getBlackClosestTag(), - new Translation2d(getBlackRelativeX(), getBlackRelativeY()), + return relativeToGlobalPose2d(getOrangeClosestTag(), + new Translation2d(getOrangeRelativeX(), getOrangeRelativeY()), new Rotation2d(gyroAngle.getAsDouble())); }