40 baseline auto (#58)

* Created baseline auto

* fixed something

* Updated the auto baseline

* small fix

* Update DriveTimeCommand.java

* Fixed end command in auto drive

* fixed driving time

* fixed formatting

* added sim support

* Fixed implimentation

* Add default option for baseline auto.

* Switch to fieldRelative false

---------

Co-authored-by: Luhan Wang <[email protected]>
Co-authored-by: m10653 <[email protected]>

src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java

@@ -10,6 +10,7 @@
 import org.jmhsrobotics.frc2024.subsystems.LED.commands.RainbowLEDCommand;
 import org.jmhsrobotics.frc2024.subsystems.drive.DriveSubsystem;
 import org.jmhsrobotics.frc2024.subsystems.drive.commands.DriveCommand;
+import org.jmhsrobotics.frc2024.subsystems.drive.commands.auto.DriveTimeCommand;
 import org.jmhsrobotics.frc2024.subsystems.drive.commands.LockAprilTag;
 import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem;
 import org.jmhsrobotics.frc2024.subsystems.vision.VisionSubsystem;
@@ -26,7 +27,6 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import monologue.Logged;
 
@@ -47,13 +47,14 @@ public RobotContainer() {
 
 		this.driveSubsystem.setDefaultCommand(new DriveCommand(this.driveSubsystem, this.control));
 		this.ledSubsystem.setDefaultCommand(new RainbowLEDCommand(this.ledSubsystem));
-		SmartDashboard.putData("Schedular", CommandScheduler.getInstance());
+		SmartDashboard.putData("Scheduler", CommandScheduler.getInstance());
 		SmartDashboard.putData("LockAprilTagCommand", new LockAprilTag(4, this.driveSubsystem, this.visionSubsystem));
 		configureBindings();
 
 		// Named commands must be added before building the chooser.
 		configurePathPlanner();
 		autoChooser = AutoBuilder.buildAutoChooser();
+		autoChooser.setDefaultOption("BaseLineAuto", new DriveTimeCommand(1.535, 0.3, driveSubsystem));
 		SmartDashboard.putData("Auto Chooser", autoChooser);
 	}
 
@@ -80,7 +81,8 @@ private void configureBindings() {
 	public Command getAutonomousCommand() {
 		Command picked = autoChooser.getSelected();
 		if (picked == null) {
-			return Commands.print("No autonomous command configured");
+			DriverStation.reportError("WARNING: No auto command detected, defaulting to baseline auto.", false);
+			return new DriveTimeCommand(1.535, 0.3, driveSubsystem);
 		} else {
 			return picked;
 		}

src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/auto/DriveTimeCommand.java

@@ -0,0 +1,52 @@
+package org.jmhsrobotics.frc2024.subsystems.drive.commands.auto;
+
+import org.jmhsrobotics.frc2024.subsystems.drive.DriveSubsystem;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class DriveTimeCommand extends Command {
+
+	private DriveSubsystem drive;
+
+	private double driveSeconds;
+	private double drivePower;
+
+	private Timer timer = new Timer();
+
+	public DriveTimeCommand(double seconds, double power, DriveSubsystem subsystem) {
+
+		driveSeconds = seconds;
+		drivePower = power;
+
+		drive = subsystem;
+
+	}
+
+	public void initialize() {
+
+		timer.start();
+		timer.reset();
+
+	}
+
+	@Override
+	public void execute() {
+
+		this.drive.drive(drivePower, 0, 0, false, false);
+
+	}
+
+	@Override
+	public boolean isFinished() {
+
+		return timer.get() >= driveSeconds;
+
+	}
+
+	public void end() {
+		this.drive.drive(0, 0, 0, false, false);
+
+	}
+
+}