feat: tested auton paths with spinup times

SaintsRobotics · Mar 16, 2024 · b351180 · b351180
1 parent a6388fd
commit b351180
Show file tree

Hide file tree

Showing 8 changed files with 92 additions and 6 deletions.
diff --git a/src/main/deploy/pathplanner/autos/0-Anywhere-Leave-StraightBack.auto 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
+}
diff --git a/...ner/autos/1-AmpSide-Leave-Delayed-6s.auto → ...ner/autos/1-AmpSide-Leave-Delayed-3s.auto b/...ner/autos/1-AmpSide-Leave-Delayed-6s.auto → ...ner/autos/1-AmpSide-Leave-Delayed-3s.auto
diff --git a/...er/autos/1-AmpSide-Leave-Delayed-12s.auto → ...ner/autos/1-AmpSide-Leave-Delayed-8s.auto b/...er/autos/1-AmpSide-Leave-Delayed-12s.auto → ...ner/autos/1-AmpSide-Leave-Delayed-8s.auto
diff --git a/...pathplanner/autos/1-AmpSide-Leave-4s.auto → ...oy/pathplanner/autos/1-AmpSide-Leave.auto b/...pathplanner/autos/1-AmpSide-Leave-4s.auto → ...oy/pathplanner/autos/1-AmpSide-Leave.auto
diff --git a/...pathplanner/autos/2-AmpSide-Stays-8s.auto → ...loy/pathplanner/autos/2-Center-Stays.auto b/...pathplanner/autos/2-AmpSide-Stays-8s.auto → ...loy/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
@@ -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
+}
diff --git 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
@@ -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;
   }
 }