Merge pull request #63 from frc937/auto-number-one

Auto number one

src/main/java/frc/robot/Constants.java

@@ -83,12 +83,24 @@ public static class Drive {
 
     /** The Rotation Drive PID for the robot. */
     public static PIDConstants ROTATION_DRIVE_PID = new PIDConstants(1.0, 0.0, 0.0);
+  }
 
+  /** Holds constants specfically related to autonomous. */
+  public static class Auto {
     /** The number of meters per second that we want to move forward during the taxi auto */
     public static double TAXI_AUTO_METERS_PER_SECOND = 1.0;
 
     /** The number of seconds that we want to taxi for */
     public static double TAXI_AUTO_DURATION_SECONDS = 4.0;
+
+    /** The amount of time we want/need to drive away from the amp in auto. */
+    public static double DRIVE_AWAY_FROM_AMP_TIME = 2.0;
+
+    /** The time we use to back away from the amp in auto. */
+    public static double BACK_UP_FROM_AMP_TIME = 0.5;
+
+    /** The amount of time that we want to run the fire note command in auto. */
+    public static final double FIRE_NOTE_FOR_TIME = 4.0;
   }
 
   /** Constants for the Intake System */
@@ -144,6 +156,15 @@ public static class AimingLimelight {
        * Limelight's drive routine "done"
        */
       public static final double DISTANCE_DONE_THRESHOLD = 0;
+
+      /** Holds pipeline numbers for this Limelight. */
+      public static class PipelineNumbers {
+        /** The Limelight pipeline number for amp AprilTags. */
+        public static final double AMP_PIPELINE_NUMBER = 6;
+      }
     }
+
+    /** Number of radians per second that we want to turn while seeking an unseen target. */
+    public static double LIMELIGHT_SEEKING_RADIANS_PER_SECOND = Math.PI / 2;
   }
 }

src/main/java/frc/robot/commands/AimAndFireRoutine.java

@@ -0,0 +1,43 @@
+// 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.
+
+/*
+ * Asimov's Laws:
+ * The First Law: A robot may not injure a human being or, through inaction, allow a human being to come to harm.
+ * The Second Law: A robot must obey the orders given it by human beings except where such orders would conflict with the First Law.
+ * The Third Law: A robot must protect its own existence as long as such protection does not conflict with the First or Second Law.
+ */
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import frc.robot.Constants;
+import frc.robot.RobotContainer;
+import frc.robot.commands.mailbox.FireNoteRoutine;
+
+/**
+ * Uses the Limelight to aim the bot to the Amp and then deposits the note into the Amp. Has a
+ * timeout on the Note deopsit.
+ */
+public class AimAndFireRoutine extends SequentialCommandGroup {
+  /** Creates a new AimAndFireRoutine. */
+  public AimAndFireRoutine() {
+    addCommands(
+        new AimWithLimelight(
+            RobotContainer.limelight,
+            Constants.Limelight.AimingLimelight.STEER_STRENGTH,
+            Constants.Limelight.AimingLimelight.DISTANCE_FROM_TARGET,
+            Constants.Limelight.AimingLimelight.MOUNT_HEIGHT,
+            Constants.Limelight.AimingLimelight.MOUNT_ANGLE,
+            Constants.Limelight.AimingLimelight.DRIVE_STRENGTH,
+            Constants.Limelight.AimingLimelight.SPEED_LIMIT,
+            Constants.Limelight.AimingLimelight.TURN_DONE_THRESHOLD,
+            Constants.Limelight.AimingLimelight.DISTANCE_DONE_THRESHOLD,
+            Constants.Limelight.AimingLimelight.AMP_APRILTAG_HEIGHT),
+        new ParallelDeadlineGroup(
+            new WaitCommand(Constants.Auto.FIRE_NOTE_FOR_TIME), new FireNoteRoutine()));
+  }
+}

src/main/java/frc/robot/commands/SeekTargetWithLimelight.java

@@ -0,0 +1,75 @@
+// 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.
+
+/*
+ * Asimov's Laws:
+ * The First Law: A robot may not injure a human being or, through inaction, allow a human being to come to harm.
+ * The Second Law: A robot must obey the orders given it by human beings except where such orders would conflict with the First Law.
+ * The Third Law: A robot must protect its own existence as long as such protection does not conflict with the First or Second Law.
+ */
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.Drive;
+import frc.robot.subsystems.Limelight;
+
+/**
+ * Seeks an unseen target with the Limelight by spinning the robot in a circle until the Limelight
+ * sees a valid target.
+ */
+public class SeekTargetWithLimelight extends Command {
+  private Limelight limelight;
+  private Drive drive;
+  private double oldPipelineNumber;
+  private double seekingPipelineNumber;
+  private double rotationRadiansPerSecond;
+
+  /**
+   * Creates a new SeekTargetWithLimelight.
+   *
+   * @param limelight The Limelight to seek the target with.
+   * @param pipelineNumber The pipeline to set the Limelight to while seeking.
+   * @param rotationRadiansPerSecond The number of radians per second to rotate while seeking.
+   */
+  public SeekTargetWithLimelight(
+      Limelight limelight, double pipelineNumber, double rotationRadiansPerSecond) {
+    this.limelight = limelight;
+    this.drive = RobotContainer.drive;
+    this.seekingPipelineNumber = pipelineNumber;
+    this.rotationRadiansPerSecond = rotationRadiansPerSecond;
+
+    addRequirements(limelight, drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    this.oldPipelineNumber = limelight.getLimelightPipeline();
+    limelight.setLimelightPipeline(seekingPipelineNumber);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if (!limelight.hasValidTarget()) {
+      drive.driveRobotOriented(Constants.Drive.EMPTY_TRANSLATION, rotationRadiansPerSecond);
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    limelight.setLimelightPipeline(oldPipelineNumber);
+    drive.stop();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return limelight.hasValidTarget();
+  }
+}

src/main/java/frc/robot/commands/auto/MoveAwayFromAmp.java

@@ -0,0 +1,37 @@
+// 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.
+
+/*
+ * Asimov's Laws:
+ * The First Law: A robot may not injure a human being or, through inaction, allow a human being to come to harm.
+ * The Second Law: A robot must obey the orders given it by human beings except where such orders would conflict with the First Law.
+ * The Third Law: A robot must protect its own existence as long as such protection does not conflict with the First or Second Law.
+ */
+
+package frc.robot.commands.auto;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import frc.robot.Constants;
+
+/**
+ * Moves the robot away from the Amp, then slams us towards the opposing alliance wall to taxi and
+ * get out of the way of the Amp.
+ */
+public class MoveAwayFromAmp extends SequentialCommandGroup {
+  /** Creates a new MoveAwayFromAmp. */
+  public MoveAwayFromAmp() {
+    addCommands(
+        new ParallelDeadlineGroup(
+            new WaitCommand(Constants.Auto.BACK_UP_FROM_AMP_TIME),
+            new DriveAutoFieldOriented(
+                new Translation2d(Constants.Auto.TAXI_AUTO_METERS_PER_SECOND, 0), 0)),
+        new ParallelDeadlineGroup(
+            new WaitCommand(Constants.Auto.DRIVE_AWAY_FROM_AMP_TIME),
+            new DriveAutoFieldOriented(
+                new Translation2d(0, Constants.Auto.TAXI_AUTO_METERS_PER_SECOND), 0)));
+  }
+}

src/main/java/frc/robot/commands/auto/OnePieceAuto.java

@@ -0,0 +1,36 @@
+// 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.
+
+/*
+ * Asimov's Laws:
+ * The First Law: A robot may not injure a human being or, through inaction, allow a human being to come to harm.
+ * The Second Law: A robot must obey the orders given it by human beings except where such orders would conflict with the First Law.
+ * The Third Law: A robot must protect its own existence as long as such protection does not conflict with the First or Second Law.
+ */
+
+package frc.robot.commands.auto;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.Constants;
+import frc.robot.RobotContainer;
+import frc.robot.commands.AimAndFireRoutine;
+import frc.robot.commands.SeekTargetWithLimelight;
+
+/**
+ * Auto that deposits a proloaded Note into the Amp. Does so by spinning until the Limelight sees an
+ * AprilTag for one of the Amps, moving the bot towards the Amp with the Limelight, depositing the
+ * note in the Amp, and finally taxiing away from the Amp.
+ */
+public class OnePieceAuto extends SequentialCommandGroup {
+  /** Creates a new OnePieceAuto. */
+  public OnePieceAuto() {
+    addCommands(
+        new SeekTargetWithLimelight(
+            RobotContainer.limelight,
+            Constants.Limelight.AimingLimelight.PipelineNumbers.AMP_PIPELINE_NUMBER,
+            Constants.Limelight.LIMELIGHT_SEEKING_RADIANS_PER_SECOND),
+        new AimAndFireRoutine(),
+        new MoveAwayFromAmp());
+  }
+}

src/main/java/frc/robot/commands/auto/TaxiAuto.java

@@ -23,9 +23,9 @@
 public class TaxiAuto extends ParallelDeadlineGroup {
   /** Creates a new TaxiAuto. */
   public TaxiAuto() {
-    super(new WaitCommand(Constants.Drive.TAXI_AUTO_DURATION_SECONDS));
+    super(new WaitCommand(Constants.Auto.TAXI_AUTO_DURATION_SECONDS));
     addCommands(
         new DriveAutoRobotOriented(
-            new Translation2d(0, Constants.Drive.TAXI_AUTO_METERS_PER_SECOND), 0));
+            new Translation2d(0, Constants.Auto.TAXI_AUTO_METERS_PER_SECOND), 0));
   }
 }