Autonomous Modes for July Heat

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

@@ -32,12 +32,14 @@
 import frc.robot.commands.hoodedShooterCommands.ConstantShootCommand;
 import frc.robot.commands.hoodedShooterCommands.MaxPowerShootCommand;
 import frc.robot.commands.hoodedShooterCommands.PidShootCommand;
+import frc.robot.commands.hoodedShooterCommands.SpinUpShooterCommand;
 import frc.robot.commands.drivingCommands.DisableRampingCommand;
 import frc.robot.commands.drivingCommands.DriveEncoders;
 import frc.robot.commands.drivingCommands.EnableBrakeModeCommand;
 import frc.robot.commands.drivingCommands.SetSpeedMode;
 import frc.robot.commands.drivingCommands.StopDrivetrainCommand;
 import frc.robot.commands.drivingCommands.TankDriveCommand;
+import frc.robot.commands.drivingCommands.TurnCommand;
 import frc.robot.commands.drivingCommands.GTADriveCommand;
 import frc.robot.subsystems.ClimberSubsystem;
 import frc.robot.subsystems.DriveTrainSubsystem;
@@ -77,6 +79,8 @@ public class RobotContainer {
   enum PossibleAutos {
     IN_FRONT_OF_TARGET_MAX_POWER,
     IN_FRONT_OF_TARGET_MAX_POWER_THEN_BACK,
+    SHOOT_THEN_DRIVE_JULY_HEAT,
+    DRIVE_JULY_HEAT,
   }
 
   SendableChooser<PossibleAutos> autoChoice = new SendableChooser<PossibleAutos>();
@@ -238,6 +242,10 @@ public Command getAutonomousCommand() {
         return getAutoInFrontOfTarget();
       case IN_FRONT_OF_TARGET_MAX_POWER_THEN_BACK:
         return getAutoInFrontOfTargetThenBack();
+      case SHOOT_THEN_DRIVE_JULY_HEAT:
+        return getAutoShootThenDriveJulyHEAT();
+      case DRIVE_JULY_HEAT:
+        return getAutoDriveJulyHEAT();
       default:
         return null;
     }
@@ -274,6 +282,41 @@ private Command getAutoInFrontOfTargetThenBack() {
         .andThen((shoot.withTimeout(5)).andThen(driveBack));
   }
 
+  private Command getAutoShootThenDriveJulyHEAT() {
+    // This is the speed the shooter will spin
+    double shooterSpeed = 0.37;
+    // This command drives forward 4 feet when run
+    Command driveForward = new DriveEncoders(1.2192, .5, driveTrain);
+    // Turns around 180 degrees
+    Command turnAround = new TurnCommand(175, .5, driveTrain, navigation);
+    // This spins up the shooter - note: it doesn't stop the shooter, which might be concerning
+    Command spinUp = new SpinUpShooterCommand(shooter, shooterSpeed);
+    // This shoots with PID - We should adjust the value to the setpoint at wherever we start the
+    // bot
+    Command shoot = new PidShootCommand(shooter, shooterSpeed);
+    // This aims the turret
+    Command aimTurret = new PointTurretAtTargetWithAngleCommand(turret);
+    // This indexes the horizontal indexer in
+    Command horizIn = new HorizontalIndexerIntakeCommand(horizontalIndexer);
+    // This indexes the vertical indexer up
+    Command vertUp = new VerticalIndexerUpCommand(verticalIndexer);
+
+    // This lets the shooter spin, then keeps it spinning and indexes up for 5 seconds to allow all
+    // balls to be shot, then drives forwards. While it does this the turret aims. Times can be
+    // adjusted as needed
+    return aimTurret.alongWith(
+        spinUp.andThen(
+            (shoot.alongWith(horizIn.alongWith(vertUp)).withTimeout(5))
+                .andThen(turnAround.andThen(driveForward))));
+  }
+
+  private Command getAutoDriveJulyHEAT() {
+    // This command drives forward 4 feet when run
+    Command driveForward = new DriveEncoders(1.2192, .5, driveTrain);
+
+    return driveForward;
+  }
+
   public Command getStopDriveTrainCommand() {
     return new StopDrivetrainCommand(driveTrain);
   }

src/main/java/frc/robot/commands/hoodedShooterCommands/SpinUpShooterCommand.java

@@ -0,0 +1,54 @@
+// 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.
+
+/*Note - this command works just like PidShootCommand with 3 changes: 1. It stops when the shooter is at the speed. 2. It doesn't stop the shooter when it ends
+3. It prints to smart dashboard with a different prefix. Change 2 might be problematic
+*/
+
+package frc.robot.commands.hoodedShooterCommands;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.HoodedShooterSubsystem;
+
+public class SpinUpShooterCommand extends CommandBase {
+  HoodedShooterSubsystem shooter;
+  double velocityFraction;
+  /** Creates a new SpinUpShooterCommand. */
+  public SpinUpShooterCommand(HoodedShooterSubsystem shooter, double velocityFraction) {
+    this.shooter = shooter;
+    this.velocityFraction = velocityFraction;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(shooter);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    double shootingVelocityFraction =
+        SmartDashboard.getNumber("Commands.SpinUpShooter.topSpeedFraction", this.velocityFraction);
+    SmartDashboard.putNumber("Commands.SpinUpShooter.topSpeedFraction", shootingVelocityFraction);
+
+    shooter.setShooterVelFraction(velocityFraction);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    // Update it every time to ensure that it keeps working - there was some weirdness this helped
+    // fix
+    SmartDashboard.putNumber("Commands.SpinUpShooter.speedFraction", velocityFraction);
+    shooter.setShooterVelFraction(velocityFraction);
+  }
+
+  // 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() {
+    return shooter.isShooterReady();
+  }
+}