diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index acd4977..4e38b3e 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -6,10 +6,9 @@ "New Folder" ], "autoFolders": [], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 3.0, + "defaultMaxVel": 2.0, + "defaultMaxAccel": 1.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, - "maxModuleSpeed": 4.5, - "choreoProjectPath": null + "maxModuleSpeed": 4.5 } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/DoNothing.auto b/src/main/deploy/pathplanner/autos/DoNothing.auto index acf233e..07d4e33 100644 --- a/src/main/deploy/pathplanner/autos/DoNothing.auto +++ b/src/main/deploy/pathplanner/autos/DoNothing.auto @@ -10,14 +10,7 @@ "command": { "type": "sequential", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AimSpeakerCommand" - } - } - ] + "commands": [] } }, "folder": null, diff --git a/src/main/deploy/pathplanner/autos/ShootSpeakerClose.auto b/src/main/deploy/pathplanner/autos/ShootSpeakerClose.auto new file mode 100644 index 0000000..36e4957 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/ShootSpeakerClose.auto @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AimSpeakerCommand" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ArmCommand" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterCommand" + } + }, + { + "type": "wait", + "data": { + "waitTime": 5.0 + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AimLowerCommand" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "SpeakerToLine" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/ShootSpeakerFar.auto b/src/main/deploy/pathplanner/autos/ShootSpeakerFar.auto new file mode 100644 index 0000000..88315f5 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/ShootSpeakerFar.auto @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ShootSpeakerPath" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AimSpeakerCommand" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ArmCommand" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterCommand" + } + }, + { + "type": "wait", + "data": { + "waitTime": 5.0 + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AimLowerCommand" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "SpeakerToLine" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ShootSpeakerPath.path b/src/main/deploy/pathplanner/paths/ShootSpeakerPath.path new file mode 100644 index 0000000..308e73e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/ShootSpeakerPath.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.2, + "y": 1.75 + }, + "prevControl": null, + "nextControl": { + "x": 2.200000000000002, + "y": 1.75 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.4, + "y": 5.5 + }, + "prevControl": { + "x": 2.4, + "y": 5.5 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/SpeakerToLine.path similarity index 55% rename from src/main/deploy/pathplanner/paths/Example Path.path rename to src/main/deploy/pathplanner/paths/SpeakerToLine.path index a3fe7a4..14229a1 100644 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/SpeakerToLine.path @@ -3,41 +3,41 @@ "waypoints": [ { "anchor": { - "x": 2.0, - "y": 7.0 + "x": 2.118024005101129, + "y": 4.635503404956002 }, "prevControl": null, "nextControl": { - "x": 3.0, - "y": 6.5 + "x": 1.5654159830259637, + "y": 3.041080915880077 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.0, - "y": 5.0 + "x": 2.5, + "y": 2.199257239161131 }, "prevControl": { - "x": 4.0, - "y": 6.0 + "x": 1.3231422106608646, + "y": 3.5454931848085423 }, "nextControl": { - "x": 6.0, - "y": 4.0 + "x": 3.676857789339134, + "y": 0.85302129351372 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.0, - "y": 1.0 + "x": 7.957431157356543, + "y": 1.3650562174103558 }, "prevControl": { - "x": 6.75, - "y": 2.5 + "x": 6.957431157356543, + "y": 1.3650562174103558 }, "nextControl": null, "isLocked": false, @@ -48,18 +48,21 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { - "velocity": 0.0, - "rotation": 0, + "velocity": 0, + "rotation": -4.382714348011046, "rotateFast": false }, "reversed": false, "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TeleopAmpPath.path b/src/main/deploy/pathplanner/paths/TeleopAmpPath.path index 09f662d..dcfd3d5 100644 --- a/src/main/deploy/pathplanner/paths/TeleopAmpPath.path +++ b/src/main/deploy/pathplanner/paths/TeleopAmpPath.path @@ -3,25 +3,41 @@ "waypoints": [ { "anchor": { - "x": 5.0, - "y": 5.0 + "x": 8.298695211705768, + "y": 4.3985144783186625 }, "prevControl": null, "nextControl": { - "x": 6.0, - "y": 4.0 + "x": 7.438598433871163, + "y": 5.888245796954481 + }, + "isLocked": false, + "linkedName": "MidPoint" + }, + { + "anchor": { + "x": 6.222672214394183, + "y": 6.540894375087692 + }, + "prevControl": { + "x": 8.13247757990561, + "y": 5.745679746402094 + }, + "nextControl": { + "x": 4.3128668488827575, + "y": 7.336109003773291 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.0, - "y": 1.0 + "x": 1.8051986219412255, + "y": 7.422493182165216 }, "prevControl": { - "x": 6.75, - "y": 2.5 + "x": 1.8051986219412255, + "y": 5.90180254959067 }, "nextControl": null, "isLocked": false, @@ -39,7 +55,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": -3.1134387480333885, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/TeleopSourcePath.path b/src/main/deploy/pathplanner/paths/TeleopSourcePath.path index 9593bf1..14d7952 100644 --- a/src/main/deploy/pathplanner/paths/TeleopSourcePath.path +++ b/src/main/deploy/pathplanner/paths/TeleopSourcePath.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.0725321924577496, - "y": 1.0796586522082987 + "x": 15.398883453656053, + "y": 1.1470264049073142 }, "prevControl": { - "x": -0.4481584401168075, - "y": 1.0796586522082987 + "x": 14.995556428276537, + "y": 1.1470264049073142 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/TeleopSpeakerPath.path b/src/main/deploy/pathplanner/paths/TeleopSpeakerPath.path index 09f662d..c33b7a8 100644 --- a/src/main/deploy/pathplanner/paths/TeleopSpeakerPath.path +++ b/src/main/deploy/pathplanner/paths/TeleopSpeakerPath.path @@ -3,25 +3,41 @@ "waypoints": [ { "anchor": { - "x": 5.0, - "y": 5.0 + "x": 8.298695211705768, + "y": 4.3985144783186625 }, "prevControl": null, "nextControl": { - "x": 6.0, - "y": 4.0 + "x": 9.712908774078864, + "y": 4.3985144783186625 + }, + "isLocked": false, + "linkedName": "MidPoint" + }, + { + "anchor": { + "x": 5.303155179058627, + "y": 4.806135432132297 + }, + "prevControl": { + "x": 7.312495893755382, + "y": 4.221098166502594 + }, + "nextControl": { + "x": 3.293814464361872, + "y": 5.391172697762 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.0, - "y": 1.0 + "x": 0.9994362713898646, + "y": 5.545540883229574 }, "prevControl": { - "x": 6.75, - "y": 2.5 + "x": 1.681964380095043, + "y": 5.469704426706775 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ea94787..cc22487 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -33,6 +33,7 @@ import frc.robot.commands.UltrasonicShooterCommand; import frc.robot.commands.auto.AimAmpCommand; import frc.robot.commands.auto.AimSpeakerCommand; +import frc.robot.commands.auto.LowerArmCommand; import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.CameraSubsystem; import frc.robot.subsystems.DriveSubsystem; @@ -93,6 +94,8 @@ public class RobotContainer { // these commands are used by autonomous only private final AimAmpCommand m_AimAmpCommand = new AimAmpCommand(m_armSubsystem, m_shooterState); + private final LowerArmCommand m_AimLowerCommand = + new LowerArmCommand(m_armSubsystem, m_shooterState); private final AimSpeakerCommand m_AimSpeakerCommand = new AimSpeakerCommand(m_armSubsystem, m_shooterState); @@ -239,8 +242,8 @@ private void bindShooterSysIDCommands() { private void initializeAutonomous() { // Network Table Routine Options - autoDashboardChooser.setDefaultOption("ShootSpeaker", "SpeakerAuto"); - autoDashboardChooser.addOption("ShootAmp", "AmpAuto"); + autoDashboardChooser.setDefaultOption("ShootSpeakerClose", "ShootSpeakerClose"); + autoDashboardChooser.addOption("ShootSpeakerFar", "ShootSpeakerFar"); autoDashboardChooser.addOption("DriveForward", "DriveForward"); autoDashboardChooser.addOption("Do Nothing", "DoNothing"); SmartDashboard.putData(autoDashboardChooser); @@ -254,7 +257,9 @@ private void initializeAutonomous() { "BrakeCommand", new InstantCommand(() -> m_driveSubsystem.SetBrakemode())); NamedCommands.registerCommand("ShooterCommand", m_shooterCommand); NamedCommands.registerCommand("AimSpeakerCommand", m_AimSpeakerCommand); + NamedCommands.registerCommand("ArmCommand", m_armCommand); NamedCommands.registerCommand("AimAmpCommand", m_AimAmpCommand); + NamedCommands.registerCommand("AimLowerCommand", m_AimLowerCommand); // autoBuilder = // new RamseteAutoBuilder( diff --git a/src/main/java/frc/robot/commands/auto/LowerArmCommand.java b/src/main/java/frc/robot/commands/auto/LowerArmCommand.java new file mode 100644 index 0000000..11a52db --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/LowerArmCommand.java @@ -0,0 +1,58 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.auto; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.ShooterState; +import frc.robot.subsystems.ArmSubsystem; + +/** An example command that uses an example subsystem. */ +public class LowerArmCommand extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final ArmSubsystem m_ArmSubsystem; + + private final ShooterState m_shooterState; + + /** + * Creates a new ShootSpeakerCommand. + * + * @param subsystem The subsystem used by this command. + */ + public LowerArmCommand(ArmSubsystem a_subsystem, ShooterState shooterState) { + m_ArmSubsystem = a_subsystem; + m_shooterState = shooterState; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(m_ArmSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + // this bypasses the shooter state and just moves the arm to the speaker position, as the arm + // subsystem gets paused during the auto shoot command + m_shooterState.setMode(ShooterState.ShooterMode.DEFAULT); + m_ArmSubsystem.lowerArm(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + // if the arm is stopped, at goal, and the offset is set, then the command is finished, aka at + // correct angle + if (m_ArmSubsystem.ArmStopped() && m_ArmSubsystem.atGoal() && m_ArmSubsystem.isOffsetSet()) { + return true; + } else { + return false; + } + } +}