Autonomous

src/main/java/frc/robot/RobotContainer.java

@@ -100,7 +100,16 @@ public RobotContainer() {
                     new Translation2d(2, -1)
                 ),
                 new Pose2d(3, 0, new Rotation2d())
-            );
+            ).andThen(new FollowPathCommand(
+                tankSubsystem,
+                new Pose2d(3, 0, new Rotation2d()),
+                List.of(
+                    new Translation2d(1, 1),
+                    new Translation2d(2, -1)
+                ),
+                new Pose2d(),
+                true
+            )).andThen(new InstantCommand(() -> tankSubsystem.setTankDriveVoltages(0, 0)));
         } else {
             autonCommand = new InstantCommand();
         }

src/main/java/frc/robot/commands/internals/RequestShotCommand.java

@@ -3,6 +3,11 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.subsystems.internals.InternalSubsystem;
 
+/**
+ * Asynchronously requests a shot be fired. The shot will *actually* be fired when a ball is in staging and
+ * the turret is lined up. Calling requestShot repeatedly has no effect; this merely requests that the next 
+ * opportunity internals has to shoot a ball is taken.
+ */
 public class RequestShotCommand extends InstantCommand {
     private final InternalSubsystem internals;
 

src/main/java/frc/robot/commands/internals/ShootCommand.java

@@ -0,0 +1,33 @@
+package frc.robot.commands.internals;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.internals.InternalSubsystem;
+
+/**
+ * Requests a shot to be fired, and waits until it is. This is used in autonomous command groups to
+ * make sure shots are fired when they are supposed to.
+ */
+public class ShootCommand extends CommandBase {
+    private final InternalSubsystem internals;
+
+    private boolean shotRequested = false;
+    private boolean finished = false;
+
+    public ShootCommand(InternalSubsystem internals) {
+        this.internals = internals;
+        addRequirements(internals);
+    }
+
+    @Override
+    public void initialize() {
+        internals.requestShot();
+        shotRequested = true;
+    }
+
+    @Override
+    public boolean isFinished() {
+        boolean doneShooting = shotRequested && !internals.shotRequested();
+        this.finished = finished || doneShooting; // in case someone ever queues a new shot before successive isFinished calls (???)
+        return finished;
+    }
+}

src/main/java/frc/robot/commands/tank/AutonMiddleSequence.java

@@ -0,0 +1,39 @@
+package frc.robot.commands.tank;
+
+import java.util.List;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+import frc.robot.commands.internals.ShootCommand;
+import frc.robot.subsystems.internals.InternalSubsystem;
+import frc.robot.subsystems.tank.TankSubsystem;
+
+/**
+ * The middle autonomous sequence. This assumes we start in the middle position, facing away from the hub.
+ * For all autonomous sequences, we drive forward and intake a ball, then shoot both cargo into the hub.
+ * For the middle sequence specifically, after shooting both balls off the tarmac, drive to and shoot
+ * the ball from the human player terminal.
+ */
+public class AutonMiddleSequence extends SequentialCommandGroup {
+    // TODO: measure these
+    private static Pose2d initialPose = new Pose2d(0, 0, Rotation2d.fromDegrees(-66));
+    private static Pose2d ballOnePose = new Pose2d(
+        Units.inchesToMeters(88.3000339775), 
+        Units.inchesToMeters(124.94840535), 
+        Rotation2d.fromDegrees(-54.7514582647));
+    private static Pose2d ballTwoPose = new Pose2d(0, 0, Rotation2d.fromDegrees(-45));
+
+    public AutonMiddleSequence(TankSubsystem tankSubsystem, InternalSubsystem internalSubsystem) {
+        addRequirements(tankSubsystem, internalSubsystem);
+        addCommands(
+            new FollowPathCommand(tankSubsystem, initialPose, List.of(), ballOnePose),
+            new ShootCommand(internalSubsystem),
+            new ShootCommand(internalSubsystem),
+            new FollowPathCommand(tankSubsystem, ballOnePose, List.of(), ballTwoPose),
+            new ShootCommand(internalSubsystem)
+        );
+    }
+}

src/main/java/frc/robot/commands/tank/AutonTopSequence.java

@@ -0,0 +1,30 @@
+package frc.robot.commands.tank;
+
+import java.util.List;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+import frc.robot.commands.internals.ShootCommand;
+import frc.robot.subsystems.internals.InternalSubsystem;
+import frc.robot.subsystems.tank.TankSubsystem;
+
+/**
+ * The top autonomous sequence. This assumes we start in the top position, facing away from the hub.
+ * For all autonomous sequences, we drive forward and intake a ball, then shoot both cargo into the hub.
+ */
+public class AutonTopSequence extends SequentialCommandGroup {
+    // TODO: measure these
+    public static final Pose2d initialPose = new Pose2d(0, 0, new Rotation2d());
+    public static final Pose2d ballOnePose = new Pose2d(0, 0, new Rotation2d());
+
+    public AutonTopSequence(TankSubsystem tankSubsystem, InternalSubsystem internalSubsystem) {
+        addRequirements(tankSubsystem, internalSubsystem);
+        addCommands(
+            new FollowPathCommand(tankSubsystem, initialPose, List.of(), ballOnePose),
+            new ShootCommand(internalSubsystem),
+            new ShootCommand(internalSubsystem)
+        );
+    }
+}

src/main/java/frc/robot/commands/tank/FollowPathCommand.java

@@ -74,20 +74,23 @@ public FollowPathCommand(TankSubsystem tankSubsystem, Trajectory trajectory) {
     }
 
     /**
-     * Creates a FollowPathCommand from a given start point, list of waypoints, and end point.
+     * Creates a FollowPathCommand from a given start point, list of waypoints, end point, and boolean representing whether
+     * the path should be reversed (if the robot should drive backwards through the trajectory).
      * 
      * @param tankSubsystem The tank subsystem.
      * @param start The start point of the trajectory as a Pose2d.
      * @param waypoints A list of waypoints the robot must pass through as a List<Translation2d>.
      * @param end The end point of the trajectory as a Pose2d.
+     * @param reversed Whether the trajectory is reversed.
      */
-    public FollowPathCommand(TankSubsystem tankSubsystem, Pose2d start, List<Translation2d> waypoints, Pose2d end) {
+    public FollowPathCommand(TankSubsystem tankSubsystem, Pose2d start, List<Translation2d> waypoints, Pose2d end, boolean reversed) {
         this(
             tankSubsystem,
             // Target trajectory
             TrajectoryGenerator.generateTrajectory(
                 start, waypoints, end, 
                 new TrajectoryConfig(MAX_VEL, MAX_ACCEL)
+                    .setReversed(reversed)
                     .setKinematics(KINEMATICS)
                     .addConstraint(new DifferentialDriveVoltageConstraint(
                         new SimpleMotorFeedforward(Ks, Kv, Ka), 
@@ -96,4 +99,16 @@ public FollowPathCommand(TankSubsystem tankSubsystem, Pose2d start, List<Transla
             )
         );
     }
+
+    /**
+     * Creates a FollowPathCommand from a given start point, list of waypoints, and end point.
+     * 
+     * @param tankSubsystem The tank subsystem.
+     * @param start The start point of the trajectory as a Pose2d.
+     * @param waypoints A list of waypoints the robot must pass through as a List<Translation2d>.
+     * @param end The end point of the trajectory as a Pose2d.
+     */
+    public FollowPathCommand(TankSubsystem tankSubsystem, Pose2d start, List<Translation2d> waypoints, Pose2d end) {
+        this(tankSubsystem, start, waypoints, end, false);
+    }
 }

src/main/java/frc/robot/subsystems/internals/InternalSubsystem.java

@@ -1,9 +1,13 @@
 package frc.robot.subsystems.internals;
 
+import static frc.robot.Constants.InternalConstants.entranceIRPort;
+import static frc.robot.Constants.InternalConstants.motorPortBottom;
+import static frc.robot.Constants.InternalConstants.motorPortTop;
+import static frc.robot.Constants.InternalConstants.stagingIRPort;
+
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
-
 import com.revrobotics.ColorMatch;
 import com.revrobotics.ColorMatchResult;
 import com.revrobotics.ColorSensorV3;
@@ -13,13 +17,10 @@
 import edu.wpi.first.wpilibj.I2C;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.util.Color;
-
 import frc.robot.GRTSubsystem;
 import frc.robot.brownout.PowerController;
 import frc.robot.subsystems.TurretSubsystem;
 
-import static frc.robot.Constants.InternalConstants.*;
-
 public class InternalSubsystem extends GRTSubsystem {
 
     private final TurretSubsystem turretSubsystem;
@@ -289,4 +290,8 @@ public void setCurrentLimit(double limit) {
     public void setDriverOverride(boolean override) {
         this.driverOverride = override;
     }
+
+    public boolean shotRequested() {
+        return shotRequested;
+    }
 }