diff --git a/src/main/deploy/pathplanner/New Path.path b/src/main/deploy/pathplanner/New Path.path new file mode 100644 index 0000000..74c48a8 --- /dev/null +++ b/src/main/deploy/pathplanner/New Path.path @@ -0,0 +1,74 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.0, + "y": 3.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.0, + "y": 3.0 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 3.0, + "y": 5.0 + }, + "prevControl": { + "x": 3.0, + "y": 4.0 + }, + "nextControl": { + "x": 3.0, + "y": 4.0 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.0, + "y": 3.0 + }, + "prevControl": { + "x": 4.0, + "y": 3.0 + }, + "nextControl": null, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "markers": [] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SourceSpeaker1NoteMobility.auto b/src/main/deploy/pathplanner/autos/SourceSpeaker1NoteMobility.auto new file mode 100644 index 0000000..e2cf6d9 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/SourceSpeaker1NoteMobility.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.74, + "y": 4.48 + }, + "rotation": -59.45431809626428 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AutoStart" + } + }, + { + "type": "named", + "data": { + "name": "ShootNote" + } + }, + { + "type": "path", + "data": { + "pathName": "SourceSpeaker-Mobility" + } + } + ] + } + }, + "folder": "Source Speaker", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SourceSpeaker-Mobility.path b/src/main/deploy/pathplanner/paths/SourceSpeaker-Mobility.path new file mode 100644 index 0000000..f1cb1fd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SourceSpeaker-Mobility.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7352600952201659, + "y": 4.484967101535422 + }, + "prevControl": null, + "nextControl": { + "x": 1.1905904713351545, + "y": 1.3049178174275693 + }, + "isLocked": false, + "linkedName": "SourceSpeaker" + }, + { + "anchor": { + "x": 3.858855262194511, + "y": 1.022510230073842 + }, + "prevControl": { + "x": 3.2258727388154664, + "y": 1.0419866154085826 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.8, + "maxAcceleration": 4.0, + "maxAngularVelocity": 500.0, + "maxAngularAcceleration": 2000.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -60.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Source Speaker", + "previewStartingState": { + "rotation": -60.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1374ce7..c14471d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -229,7 +229,7 @@ public static class WristConstants { public static final double WRIST_GEAR_RATIO = 25.0 * 42.0 / 18.0; public static final Rotation2d WRIST_MAX = new Rotation2d(Units.degreesToRadians(60)); - public static final Rotation2d WRIST_MIN = Rotation2d.fromRadians(0.12799356352873126); + public static final Rotation2d WRIST_MIN = Rotation2d.fromRadians(0.10695095737072258); public static final double WRIST_OFFSET = 0.805 + Math.PI / 2.0; public static final int WRIST_ENCODER_PORT = 7; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1f80896..86f3fc0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -323,10 +323,13 @@ private void evaluateDriveStyle(Command newCommand) { if (oldCommand != null) { // We have to cancel the command so the new default one will run oldCommand.cancel(); + } } public Command getAutonomousCommand() { return autos.getAutoChooser().getSelected(); + + } } diff --git a/src/main/java/frc/robot/commands/ScoringCommands.java b/src/main/java/frc/robot/commands/ScoringCommands.java index 6dc7b89..9b62d9c 100644 --- a/src/main/java/frc/robot/commands/ScoringCommands.java +++ b/src/main/java/frc/robot/commands/ScoringCommands.java @@ -30,7 +30,7 @@ public static Command shootSetpointIdleCommand(ShooterSubsystem shooterSubsystem public static Command shootSetpointAmpCommand(ShooterSubsystem shooterSubsystem) { // return shooterSubsystem.setVoltageCommand(8.0); - return shooterSubsystem.runVelocityCommand(Units.rotationsPerMinuteToRadiansPerSecond(1000.0)); + return shooterSubsystem.runVelocityCommand(Units.rotationsPerMinuteToRadiansPerSecond(2000.0)); } public static Command transportCloseSpeakerCommand(TransportSubsystem transportSubsystem) {