Program Simple Auto

src/main/java/frc4388/robot/Constants.java

@@ -171,7 +171,7 @@ public final class Constants {
         public static final int STORAGE_CAN_ID = 11;
         public static final double STORAGE_PARTIAL_BALL = 2;
         public static final double STORAGE_FULL_BALL = 7;
-        public static final double STORAGE_SPEED = 1.0;
+        public static final double STORAGE_SPEED = 0.5;
         public static final double STORAGE_TIMEOUT = 3000;
 
         /* Storage Characteristics */

src/main/java/frc4388/robot/RobotContainer.java

@@ -269,6 +269,8 @@ public class RobotContainer {
     }
 
     public void buildAutos() {
+        resetOdometry(new Pose2d(0, 0, new Rotation2d(180)));
+
         String[] sixBallAutoMiddlePaths = new String[]{
             "SixBallMidComplete"
         };
@@ -300,10 +302,12 @@ public class RobotContainer {
         m_fiveBallAutoMiddle = new FiveBallAutoMiddle(m_robotDrive, buildPaths(fiveBallAutoMiddlePaths));
 
         String[] tenBallAutoMiddlePaths = new String[]{
+            "SixBallMidComplete",
             "TenBallMidComplete"
         };
 
-        m_tenBallAutoMiddle = new TenBallAutoMiddle(m_robotDrive, buildPaths(tenBallAutoMiddlePaths));
+        m_tenBallAutoMiddle = new TenBallAutoMiddle(m_robotShooterHood, m_robotStorage, m_robotIntake, m_robotShooter,
+                m_robotShooterAim, m_robotDrive,buildPaths(tenBallAutoMiddlePaths));
     }
 
     /**
@@ -321,12 +325,12 @@ public class RobotContainer {
         try {
             SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto);
 
-            return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
+            //return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
             //return m_eightBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
             //return m_driveOffLineForward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
-            //return m_driveOffLineBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
+            //return m_driveOffLinfeBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
             //return m_fiveBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
-            //return m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
+            return m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
 
         } catch (Exception e) {
             System.err.println("ERROR");

src/main/java/frc4388/robot/commands/auto/TenBallAutoMiddle.java

@@ -7,9 +7,21 @@
 
 package frc4388.robot.commands.auto;
 
+import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
 import edu.wpi.first.wpilibj2.command.RamseteCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.Constants.IntakeConstants;
+import frc4388.robot.commands.shooter.CalibrateShooter;
+import frc4388.robot.commands.shooter.PrepChecker;
+import frc4388.robot.commands.shooter.ShootPrepGroup;
+import frc4388.robot.commands.storage.RunStorage;
 import frc4388.robot.subsystems.Drive;
+import frc4388.robot.subsystems.Intake;
+import frc4388.robot.subsystems.Shooter;
+import frc4388.robot.subsystems.ShooterAim;
+import frc4388.robot.subsystems.ShooterHood;
+import frc4388.robot.subsystems.Storage;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
@@ -18,11 +30,30 @@ public class TenBallAutoMiddle extends SequentialCommandGroup {
   /**
    * Creates a new TenBallAutoMiddle.
    */
-  public TenBallAutoMiddle(Drive drive, RamseteCommand[] paths) {
+  public TenBallAutoMiddle(ShooterHood shooterHood, Storage storage, Intake intake, Shooter shooter, ShooterAim shooterAim, Drive drive, RamseteCommand[] paths) {
     // Add your commands in the super() call, e.g.
     // super(new FooCommand(), new BarCommand());
     addCommands(
-      paths[0]
+      new ParallelDeadlineGroup(
+        new Wait(drive, 0.1, 0),
+        new CalibrateShooter(shooter, shooterAim, shooterHood)
+      ),
+      new ParallelDeadlineGroup(
+        new Wait(drive, 1, 0),
+        new RunCommand(() -> shooterAim.runShooterWithInput(-0.75), shooterAim)
+      ),
+      new ParallelDeadlineGroup(
+        new Wait(drive, 4, 0),
+        new PrepChecker(shooter, shooterAim),
+        new RunCommand(() -> intake.runExtender(IntakeConstants.EXTENDER_SPEED), intake),
+        new ShootPrepGroup(shooter, shooterAim, shooterHood, storage)
+      ),
+      new ParallelDeadlineGroup(
+        new ShootPrepGroup(shooter, shooterAim, shooterHood, storage),
+        new RunStorage(storage)
+      )
+      //paths[0],
+      //paths[1]
     );
   }
 }

src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java

@@ -7,6 +7,7 @@
 
 package frc4388.robot.commands.shooter;
 
+import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
 import com.revrobotics.CANSparkMax.SoftLimitDirection;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
@@ -59,6 +60,10 @@ public class CalibrateShooter extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
+    if (m_shooterAim.m_shooterRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen).get() &&
+      m_shooterHood.m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen).get()) {
+      return true;
+    }
     return false;
   }
 }

src/main/java/frc4388/robot/commands/shooter/PrepChecker.java

@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands.shooter;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.Shooter;
+import frc4388.robot.subsystems.ShooterAim;
+
+public class PrepChecker extends CommandBase {
+  Shooter m_shooter;
+  ShooterAim m_shooterAim;
+
+  /**
+   * Creates a new PrepChecker.
+   */
+  public PrepChecker(Shooter shooter, ShooterAim shooterAim) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    m_shooter = shooter;
+    m_shooterAim = shooterAim;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    if (m_shooterAim.m_isAimReady && m_shooter.m_isDrumReady) {
+      return true;
+    }
+
+    return false;
+  }
+}