diff --git a/src/main/deploy/pathplanner/autos/1 L4 Center.auto b/src/main/deploy/pathplanner/autos/1 L4 Center.auto new file mode 100644 index 0000000..b29b219 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/1 L4 Center.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Start to H" + } + }, + { + "type": "named", + "data": { + "name": "Shoot Coral L4" + } + }, + { + "type": "path", + "data": { + "pathName": "H Backup" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/150 Right to HP.path b/src/main/deploy/pathplanner/paths/150 Right to HP.path index 84b1a07..b648396 100644 --- a/src/main/deploy/pathplanner/paths/150 Right to HP.path +++ b/src/main/deploy/pathplanner/paths/150 Right to HP.path @@ -34,9 +34,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/paths/30 Right to HP.path b/src/main/deploy/pathplanner/paths/30 Right to HP.path index f6a2c3e..b13eb80 100644 --- a/src/main/deploy/pathplanner/paths/30 Right to HP.path +++ b/src/main/deploy/pathplanner/paths/30 Right to HP.path @@ -41,9 +41,9 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/paths/330 Right to HP.path b/src/main/deploy/pathplanner/paths/330 Right to HP.path index 98576e0..e99b2be 100644 --- a/src/main/deploy/pathplanner/paths/330 Right to HP.path +++ b/src/main/deploy/pathplanner/paths/330 Right to HP.path @@ -46,9 +46,9 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/paths/H Backup.path b/src/main/deploy/pathplanner/paths/H Backup.path new file mode 100644 index 0000000..ce72262 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/H Backup.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.7, + "y": 4.3 + }, + "prevControl": null, + "nextControl": { + "x": 6.053790983606557, + "y": 4.312704918032787 + }, + "isLocked": false, + "linkedName": "H" + }, + { + "anchor": { + "x": 6.389446721311475, + "y": 4.3 + }, + "prevControl": { + "x": 6.1394487099079695, + "y": 4.300997143065429 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 400.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "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/HP to 330 Left.path b/src/main/deploy/pathplanner/paths/HP to 330 Left.path index ff41ed9..d311ea1 100644 --- a/src/main/deploy/pathplanner/paths/HP to 330 Left.path +++ b/src/main/deploy/pathplanner/paths/HP to 330 Left.path @@ -41,9 +41,9 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path index 24bafdf..dce0c13 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -45,9 +45,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, "unlimited": false }, 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 index 0b21d36..74672e0 100644 --- a/src/main/deploy/pathplanner/paths/Right Start to 150 Right.path +++ b/src/main/deploy/pathplanner/paths/Right Start to 150 Right.path @@ -41,9 +41,9 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/paths/Start to 30 Right.path b/src/main/deploy/pathplanner/paths/Start to 30 Right.path index f3d5d3f..3fdb040 100644 --- a/src/main/deploy/pathplanner/paths/Start to 30 Right.path +++ b/src/main/deploy/pathplanner/paths/Start to 30 Right.path @@ -41,9 +41,9 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/paths/Start to H.path b/src/main/deploy/pathplanner/paths/Start to H.path new file mode 100644 index 0000000..7585a51 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Start to H.path @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.588217213114754, + "y": 3.9890368852459024 + }, + "prevControl": null, + "nextControl": { + "x": 6.916905737704918, + "y": 4.036987704918033 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.7, + "y": 4.3 + }, + "prevControl": { + "x": 6.347336065573771, + "y": 4.2640368852459005 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "H" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Lift L4", + "waypointRelativePos": 0.08214624881291864, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 400.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "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/fein.path b/src/main/deploy/pathplanner/paths/fein.path index 9504997..dc9b1fa 100644 --- a/src/main/deploy/pathplanner/paths/fein.path +++ b/src/main/deploy/pathplanner/paths/fein.path @@ -16,28 +16,28 @@ }, { "anchor": { - "x": 4.459426229508197, - "y": 5.978995901639344 + "x": 6.209631147540984, + "y": 6.074897540983606 }, "prevControl": { - "x": 5.370491803278689, - "y": 5.978995901639344 + "x": 7.120696721311476, + "y": 6.074897540983606 }, "nextControl": { - "x": 2.995158619919281, - "y": 5.978995901639344 + "x": 4.745363537952068, + "y": 6.074897540983606 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.6543032786885246, - "y": 6.254713114754098 + "x": 4.8670081967213115, + "y": 6.973975409836065 }, "prevControl": { - "x": 2.613319672131147, - "y": 6.278688524590164 + "x": 5.826024590163935, + "y": 6.9979508196721305 }, "nextControl": null, "isLocked": false, @@ -50,9 +50,9 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularAcceleration": 400.0, "nominalVoltage": 12.0, "unlimited": false }, @@ -64,7 +64,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -90.0 + "rotation": 180.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 index 3f1ac7e..47744e8 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -5,11 +5,11 @@ "pathFolders": [], "autoFolders": [], "defaultMaxVel": 4.0, - "defaultMaxAccel": 4.0, + "defaultMaxAccel": 1.0, "defaultMaxAngVel": 540.0, - "defaultMaxAngAccel": 720.0, + "defaultMaxAngAccel": 400.0, "defaultNominalVoltage": 12.0, - "robotMass": 53.0, + "robotMass": 48.35, "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 e1879ea..c9e64fc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,6 +22,8 @@ import java.util.function.IntSupplier; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.events.EventTrigger; +import com.pathplanner.lib.path.EventMarker; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; @@ -70,6 +72,8 @@ public class RobotContainer { manipulatorPivot = new ManipulatorPivot(); + configureNamedCommands(); + driver = new CommandXboxController(OIConstants.kDriverControllerPort); operator = new CommandXboxController(OIConstants.kOperatorControllerPort); @@ -81,8 +85,6 @@ public class RobotContainer { //elevatorSysIDBindings(); //elevatorOnlyBindings(); - configureNamedCommands(); - configureShuffleboard(); } @@ -114,8 +116,8 @@ public class RobotContainer { drivetrain.setDefaultCommand( drivetrain.drive( - driver::getLeftY, - driver::getLeftX, + () -> driver.getLeftY(), + () -> driver.getLeftX(), driver::getRightX, () -> true ) @@ -148,7 +150,7 @@ public class RobotContainer { ); driver.leftTrigger().whileTrue( - manipulator.runUntilCollected(() -> 0.35) + manipulator.runUntilCollected(() -> 0.75) ); driver.start().and(driver.back()).onTrue( @@ -217,21 +219,27 @@ public class RobotContainer { operator.x().onTrue( safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition) - - ).and(driver.rightTrigger().whileFalse(manipulator.runManipulator(() -> 0.5, false))); + .alongWith(manipulator.runManipulator(() -> 0.5, false)) + .until(driver.rightTrigger()) + ); operator.b().onTrue( safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition) - ).and(driver.rightTrigger().whileFalse(manipulator.runManipulator(() -> 0.5, false))); + .alongWith(manipulator.runManipulator(() -> 0.5, false)) + .until(driver.rightTrigger()) + ); - operator.y().onTrue(moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)) - .and(driver.rightTrigger().whileFalse(manipulator.runManipulator(() -> 0.5, false))); + operator.y().onTrue(moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition) + .alongWith(manipulator.runManipulator(() -> 0.5, false)) + .until(driver.rightTrigger()) + ); } private void configureNamedCommands() { + new EventTrigger("Lift L4").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position)); NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand()); - NamedCommands.registerCommand("Shoot Coral L4", manipulator.runManipulator(() -> 0.4, true)); + NamedCommands.registerCommand("Shoot Coral L4", manipulator.runManipulator(() -> 0.4, true).withTimeout(2)); 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)); @@ -311,6 +319,9 @@ public class RobotContainer { sensorTab.addDouble("dt distance", drivetrain::driveDistance); + sensorTab.addDouble("velocity", drivetrain::getVelocity); + + //sensorTab.add("odometry", drivetrain::getPose); } diff --git a/src/main/java/frc/robot/constants/AutoConstants.java b/src/main/java/frc/robot/constants/AutoConstants.java index 2e30d65..724e7f0 100644 --- a/src/main/java/frc/robot/constants/AutoConstants.java +++ b/src/main/java/frc/robot/constants/AutoConstants.java @@ -12,13 +12,13 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; public class AutoConstants { public static final double kMaxSpeedMetersPerSecond = 4; - public static final double kMaxAccelerationMetersPerSecondSquared = 4; + public static final double kMaxAccelerationMetersPerSecondSquared = 1; public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; - public static final double kPXController = 15; - public static final double kPYController = 15; - public static final double kPThetaController = 5; + public static final double kPXController = 6; + public static final double kPYController = 6; + public static final double kPThetaController = 5.5; // Constraint for the motion profiled robot angle controller public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints( diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index aeb9173..2758076 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -45,7 +45,7 @@ public class ElevatorConstants { public static final double kL3Position = 23.0; public static final double kL4Position = 50.5; public static final double kL4TransitionPosition = 40.0; - public static final double kL2AlgaePosition = 21.0; + public static final double kL2AlgaePosition = 23.0; public static final double kL3AlgaePosition = 39.0; public static final double kProcessorPosition = 4.0; /**The position of the top of the elevator brace */ diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 52790dd..f3eb779 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -370,4 +370,8 @@ public class Drivetrain extends SubsystemBase { public double driveDistance(){ return m_frontLeft.getTotalDist(); } + + public double getVelocity(){ + return m_frontLeft.getState().speedMetersPerSecond; + } }