[#88] create drivetrain system test

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

@@ -10,6 +10,10 @@
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.MathUtil;
 import frc.robot.subsystems.Flywheel;
 import frc.robot.subsystems.Pivot;
 import frc.robot.subsystems.Shooter;
@@ -26,6 +30,8 @@
 import frc.robot.command.SetLED;
 import frc.robot.command.TipDetection;
 import frc.robot.command.Shoot;
+import frc.robot.command.tests.DrivetrainSystemTest;
+import frc.robot.command.tests.TurnSystemTest;
 import frc.robot.command.ChasePieces;
 import frc.robot.command.Climb;
 import frc.robot.command.ManualClimb;
@@ -38,7 +44,9 @@
 import frc.robot.subsystems.Shooter;
 import frc.robot.subsystems.Swerve;
 import frc.thunder.LightningContainer;
+import frc.thunder.command.TimedCommand;
 import frc.thunder.shuffleboard.LightningShuffleboard;
+import frc.thunder.testing.SystemTest;
 
 public class RobotContainer extends LightningContainer {
 	XboxController driver;
@@ -161,5 +169,12 @@ protected void configureFaultMonitors() {
 
 	@Override
 	protected void configureSystemTests() {
+		SystemTest.registerTest("Drive All Directions", new DrivetrainSystemTest(drivetrain, 0.25));
+
+		SystemTest.registerTest("Azimuth Test", new SequentialCommandGroup(
+			new TimedCommand(new TurnSystemTest(drivetrain, () -> 0.25), 2),
+			new WaitCommand(0.5),
+			new TimedCommand(new TurnSystemTest(drivetrain, () -> -0.25), 2)
+		));
 	}
 }

src/main/java/frc/robot/command/tests/DriveTest.java

@@ -0,0 +1,51 @@
+// 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.
+
+package frc.robot.command.tests;
+
+import java.util.function.DoubleSupplier;
+
+import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Swerve;
+
+public class DriveTest extends Command {
+
+  private Swerve drivetrain;
+  private DoubleSupplier speedX;
+  private DoubleSupplier speedY;
+
+  /**
+   * Creates a new drive test
+   * @param drivetrain swerve subsystem
+   * @param speedX X velocity
+   * @param speedY Y velocity
+   */
+  public DriveTest(Swerve drivetrain, DoubleSupplier speedX, DoubleSupplier speedY) {
+    this.drivetrain = drivetrain;
+    this.speedX = speedX;
+    this.speedY = speedY;
+
+    addRequirements(drivetrain);
+  }
+
+  @Override
+  public void initialize() {}
+
+  @Override
+  public void execute() {
+    drivetrain.setControl(new SwerveRequest.FieldCentric().withVelocityX(speedX.getAsDouble()).withVelocityY(speedY.getAsDouble()));
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    drivetrain.setControl(new SwerveRequest.FieldCentric().withVelocityX(0).withVelocityY(0).withRotationalRate(0));
+  }
+
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/command/tests/DrivetrainSystemTest.java

@@ -0,0 +1,26 @@
+// 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.
+
+package frc.robot.command.tests;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import frc.robot.subsystems.Swerve;
+import frc.thunder.command.TimedCommand;
+
+public class DrivetrainSystemTest extends SequentialCommandGroup {
+
+  public DrivetrainSystemTest(Swerve drivetrain, double speed) {
+    addCommands(
+      new WaitCommand(0.5),
+      new TimedCommand(new DriveTest(drivetrain, () -> 0d, () -> speed), 2), // Forward
+      new WaitCommand(1),
+      new TimedCommand(new DriveTest(drivetrain, () -> 0d, () -> -speed), 2), // Backward
+      new WaitCommand(1),
+      new TimedCommand(new DriveTest(drivetrain, () -> -speed, () -> 0), 2), // Left
+      new WaitCommand(1),
+      new TimedCommand(new DriveTest(drivetrain, () -> speed, () -> 0), 2) // Right
+    );
+  }
+}

src/main/java/frc/robot/command/tests/TurnSystemTest.java

@@ -0,0 +1,48 @@
+// 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.
+
+package frc.robot.command.tests;
+
+import java.util.function.DoubleSupplier;
+
+import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Swerve;
+
+public class TurnSystemTest extends Command {
+
+  private Swerve drivetrain;
+  private DoubleSupplier speed;
+
+  /**
+   * System test for testing azimuth motors
+   * @param drivetrain swerve subsystem
+   * @param speed rotational rate
+   */
+  public TurnSystemTest(Swerve drivetrain, DoubleSupplier speed) {
+    this.drivetrain = drivetrain;
+    this.speed = speed;
+
+    addRequirements(drivetrain);
+  }
+
+  @Override
+  public void initialize() {}
+
+  @Override
+  public void execute() {
+    drivetrain.setControl(new SwerveRequest.FieldCentric().withVelocityX(0).withVelocityY(0).withRotationalRate(speed.getAsDouble()));
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    drivetrain.setControl(new SwerveRequest.FieldCentric().withVelocityX(0).withVelocityY(0).withRotationalRate(0));
+  }
+
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}