Merge pull request #61 from frc937/auto-number-zero

Auto number zero

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

@@ -80,6 +80,12 @@ 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);
+
+    /** 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;
   }
 
   /** Constants for the Intake System */

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

@@ -21,6 +21,7 @@
 import frc.robot.commands.DeployUrMom;
 import frc.robot.commands.EnterXMode;
 import frc.robot.commands.RunIntake;
+import frc.robot.commands.auto.TaxiAuto;
 import frc.robot.commands.drive.DriveFieldOriented;
 import frc.robot.commands.drive.DriveRobotOriented;
 import frc.robot.commands.mailbox.DeindexNote;
@@ -104,6 +105,9 @@ public class RobotContainer {
           Constants.Limelight.AimingLimelight.AMP_APRILTAG_HEIGHT);
   private DeployUrMom deployUrMom = new DeployUrMom();
 
+  /* Autos */
+  private TaxiAuto taxiAuto = new TaxiAuto();
+
   /*
    * ***********************
    * * OTHER INSTANCE VARS *
@@ -135,6 +139,7 @@ private void configureAuto() {
     /* autoChooser = AutoBuilder.buildAutoChooser("My Default Auto"); */
 
     /* This is where you put auto commands. Call autoChooser.addOption() to add autos. */
+    autoChooser.addOption("Taxi", taxiAuto);
 
     SmartDashboard.putData("choose auto", autoChooser);
   }

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

@@ -0,0 +1,31 @@
+// 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.WaitCommand;
+import frc.robot.Constants;
+
+/**
+ * Auto that just taxis (or more accurately, moves us out of the auto starting box.) Taxi time and
+ * velocity can be tuned in Constants.Drive.
+ */
+public class TaxiAuto extends ParallelDeadlineGroup {
+  /** Creates a new TaxiAuto. */
+  public TaxiAuto() {
+    super(new WaitCommand(Constants.Drive.TAXI_AUTO_DURATION_SECONDS));
+    addCommands(
+        new DriveAutoRobotOriented(
+            new Translation2d(0, Constants.Drive.TAXI_AUTO_METERS_PER_SECOND), 0));
+  }
+}