diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path index e655fcd..24bafdf 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 3.4559075342465753, - "y": 3.123458904109589 + "x": 12.272, + "y": 2.975 }, "prevControl": { - "x": 2.4559075342465753, - "y": 3.123458904109589 + "x": 10.940715672291898, + "y": 2.975 }, "nextControl": null, "isLocked": false, @@ -30,7 +30,18 @@ ], "rotationTargets": [], "constraintZones": [], - "pointTowardsZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 0.4, + "y": 5.5 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 0.15, + "maxWaypointRelativePos": 0.4, + "name": "Point Towards Zone" + } + ], "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, @@ -42,7 +53,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 57.80426606528677 + "rotation": 59.99999999999999 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index aae9a27..3f1ac7e 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -9,7 +9,7 @@ "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 48.35, + "robotMass": 53.0, "robotMOI": 6.883, "robotTrackwidth": 0.546, "driveWheelRadius": 0.038, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3f216d1..5787481 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -65,12 +65,6 @@ public class RobotContainer { manipulatorPivot = new ManipulatorPivot(); - //commands for pathplanner - NamedCommands.registerCommand("Shoot Coral L4", manipulator.runManipulator(() -> 0.4, true)); - NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.35)); - NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position)); - NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition)); - driver = new CommandXboxController(OIConstants.kDriverControllerPort); operator = new CommandXboxController(OIConstants.kOperatorControllerPort); @@ -147,7 +141,7 @@ public class RobotContainer { ); driver.leftTrigger().whileTrue( - manipulator.runUntilCollected(() -> 0.35) + manipulator.runUntilCollected(() -> 0.35).andThen(manipulator.retractCommand(() -> 0.35)) ); driver.start().and(driver.back()).onTrue( @@ -210,6 +204,10 @@ public class RobotContainer { private void configureNamedCommands() { NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand()); + NamedCommands.registerCommand("Shoot Coral L4", manipulator.runManipulator(() -> 0.4, true)); + NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.35)); + NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position)); + NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition)); } //creates tabs and transforms them on the shuffleboard diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 8ed1c02..9774d91 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -1,6 +1,9 @@ package frc.robot.constants; +import java.util.HashMap; + public class VisionConstants { + // global coordinate map of all tags. index is the tag id. // Units: inches and degrees. {x, y, z, z-rotation, y-rotation} // This is for ANDYMARK FIELDS found in NE. Not for WELDED FIELDS. @@ -28,4 +31,26 @@ public class VisionConstants { {209.49, 158.3, 12.13, 0, 0}, {193.1, 129.97, 12.13, 300, 0}, }; + + //map of coral placing setpoints based on the tag that is on the same reef face + // and the on the left or right branch of that side of the reef + // + public HashMap blueReefMap = new HashMap<>(); + public HashMap redReefMap = new HashMap<>(); + + VisionConstants(){ + blueReefMap.put(17, new double[] {3.703, 3.975, 3.982, 2.806}); + blueReefMap.put(18, new double[] {3.183, 4.191, 3.183, 3.857}); + blueReefMap.put(19, new double[] {3.986, 5.24, 3.701, 5.076}); + blueReefMap.put(20, new double[] {5.275, 5.075, 4.991, 5.246}); + blueReefMap.put(21, new double[] {5.789, 3.862, 5.789, 4.194}); + blueReefMap.put(22, new double[] {4.993, 2.816, 5.272, 2.996}); + + blueReefMap.put(11, new double[] {3.703+12.272, 3.975, 3.982+12.272, 2.806}); + blueReefMap.put(10, new double[] {3.183+12.272, 4.191, 3.183, 3.857}); + blueReefMap.put(9, new double[] {3.986+12.272, 5.24, 3.701+12.272, 5.076}); + blueReefMap.put(8, new double[] {5.275+12.272, 5.075, 4.991+12.272, 5.246}); + blueReefMap.put(7, new double[] {5.789+12.272, 3.862, 5.789+12.272, 4.194}); + blueReefMap.put(6, new double[] {4.993+12.272, 2.816, 5.272+12.272, 2.996}); + } } diff --git a/src/main/java/frc/robot/subsystems/Manipulator.java b/src/main/java/frc/robot/subsystems/Manipulator.java index d7ebed8..4964937 100644 --- a/src/main/java/frc/robot/subsystems/Manipulator.java +++ b/src/main/java/frc/robot/subsystems/Manipulator.java @@ -74,6 +74,13 @@ public class Manipulator extends SubsystemBase { }).until(() -> !coralBeamBreak.get()); } + public Command retractCommand(DoubleSupplier speed){ + return run(() -> { + manipulatorMotor.set(-speed.getAsDouble()); + } + ).until(() -> coralBeamBreak.get()); + } + /** * Runs the manipulator in a way that will bring the coral to a reliable holding position *