From b351180435a6b7ed03e1b567ce5cc3425b91bb33 Mon Sep 17 00:00:00 2001 From: SR1899 Date: Fri, 15 Mar 2024 18:24:32 -0700 Subject: [PATCH] feat: tested auton paths with spinup times --- .../autos/0-Anywhere-Leave-StraightBack.auto | 25 +++++++++ ...s.auto => 1-AmpSide-Leave-Delayed-3s.auto} | 0 ...s.auto => 1-AmpSide-Leave-Delayed-8s.auto} | 0 ...ide-Leave-4s.auto => 1-AmpSide-Leave.auto} | 0 ...Side-Stays-8s.auto => 2-Center-Stays.auto} | 0 .../pathplanner/paths/Straight-Back.path | 52 +++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 8 +-- .../robot/commands/NoteOuttakeCommand.java | 13 ++++- 8 files changed, 92 insertions(+), 6 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/0-Anywhere-Leave-StraightBack.auto rename src/main/deploy/pathplanner/autos/{1-AmpSide-Leave-Delayed-6s.auto => 1-AmpSide-Leave-Delayed-3s.auto} (100%) rename src/main/deploy/pathplanner/autos/{1-AmpSide-Leave-Delayed-12s.auto => 1-AmpSide-Leave-Delayed-8s.auto} (100%) rename src/main/deploy/pathplanner/autos/{1-AmpSide-Leave-4s.auto => 1-AmpSide-Leave.auto} (100%) rename src/main/deploy/pathplanner/autos/{2-AmpSide-Stays-8s.auto => 2-Center-Stays.auto} (100%) create mode 100644 src/main/deploy/pathplanner/paths/Straight-Back.path diff --git a/src/main/deploy/pathplanner/autos/0-Anywhere-Leave-StraightBack.auto b/src/main/deploy/pathplanner/autos/0-Anywhere-Leave-StraightBack.auto new file mode 100644 index 0000000..398d77b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/0-Anywhere-Leave-StraightBack.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.55744644822623, + "y": 6.0 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Straight-Back" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/1-AmpSide-Leave-Delayed-6s.auto b/src/main/deploy/pathplanner/autos/1-AmpSide-Leave-Delayed-3s.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/1-AmpSide-Leave-Delayed-6s.auto rename to src/main/deploy/pathplanner/autos/1-AmpSide-Leave-Delayed-3s.auto diff --git a/src/main/deploy/pathplanner/autos/1-AmpSide-Leave-Delayed-12s.auto b/src/main/deploy/pathplanner/autos/1-AmpSide-Leave-Delayed-8s.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/1-AmpSide-Leave-Delayed-12s.auto rename to src/main/deploy/pathplanner/autos/1-AmpSide-Leave-Delayed-8s.auto diff --git a/src/main/deploy/pathplanner/autos/1-AmpSide-Leave-4s.auto b/src/main/deploy/pathplanner/autos/1-AmpSide-Leave.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/1-AmpSide-Leave-4s.auto rename to src/main/deploy/pathplanner/autos/1-AmpSide-Leave.auto diff --git a/src/main/deploy/pathplanner/autos/2-AmpSide-Stays-8s.auto b/src/main/deploy/pathplanner/autos/2-Center-Stays.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/2-AmpSide-Stays-8s.auto rename to src/main/deploy/pathplanner/autos/2-Center-Stays.auto diff --git a/src/main/deploy/pathplanner/paths/Straight-Back.path b/src/main/deploy/pathplanner/paths/Straight-Back.path new file mode 100644 index 0000000..2eddfae --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Straight-Back.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.55744644822623, + "y": 6.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.557446448226229, + "y": 6.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 6.0 + }, + "prevControl": { + "x": 3.0, + "y": 6.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.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/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8a8c56c..3886c34 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -67,7 +67,7 @@ public RobotContainer() { NamedCommands.registerCommand("Shoot", new SequentialCommandGroup( new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, ShooterConstants.kShooterOnTime), - new ParallelDeadlineGroup(new WaitCommand(0.8), new NoteOuttakeCommand(m_intakeSubsystem)), + new ParallelDeadlineGroup(new WaitCommand(0.8), new NoteOuttakeCommand(m_intakeSubsystem, 1)), new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, ShooterConstants.kShooterOffTime))); NamedCommands.registerCommand("Intake", @@ -85,7 +85,7 @@ public RobotContainer() { NamedCommands.registerCommand("Spin down Shooter", new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, 0.01)); - NamedCommands.registerCommand("Outtake", new NoteOuttakeCommand(m_intakeSubsystem)); + NamedCommands.registerCommand("Outtake", new NoteOuttakeCommand(m_intakeSubsystem, 1)); NamedCommands.registerCommand("Intake in", new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted)); @@ -174,7 +174,7 @@ private void configureBindings() { new JoystickButton(m_driverController, Button.kX.value) .onTrue(new SequentialCommandGroup(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, ShooterConstants.kShooterOnTime), - new NoteOuttakeCommand(m_intakeSubsystem))) + new NoteOuttakeCommand(m_intakeSubsystem, 30))) .onFalse(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, ShooterConstants.kShooterOffTime)); new JoystickButton(m_driverController, Button.kRightBumper.value) @@ -194,7 +194,7 @@ private void configureBindings() { // Outtake, Operator Controller Right Trigger new Trigger(() -> { return m_operatorController.getRightTriggerAxis() > 0.5; - }).whileTrue(new NoteOuttakeCommand(m_intakeSubsystem)); + }).whileTrue(new NoteOuttakeCommand(m_intakeSubsystem, 30)); new Trigger(() -> { return m_operatorController.getLeftTriggerAxis() > 0.5; diff --git a/src/main/java/frc/robot/commands/NoteOuttakeCommand.java b/src/main/java/frc/robot/commands/NoteOuttakeCommand.java index 2cddff1..658f949 100644 --- a/src/main/java/frc/robot/commands/NoteOuttakeCommand.java +++ b/src/main/java/frc/robot/commands/NoteOuttakeCommand.java @@ -4,16 +4,25 @@ package frc.robot.commands; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.IntakeSubsystem; public class NoteOuttakeCommand extends Command { private IntakeSubsystem m_intakeSubsystem; + private Timer m_timer = new Timer(); + private double deadline; + /** Creates a new intakeCommand. */ - public NoteOuttakeCommand(IntakeSubsystem subsystem) { + public NoteOuttakeCommand(IntakeSubsystem subsystem, double time) { m_intakeSubsystem = subsystem; addRequirements(m_intakeSubsystem); + + deadline = time; + + m_timer.reset(); + m_timer.start(); } // Called when the command is initially scheduled. @@ -37,6 +46,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return m_timer.get() > deadline; } }