99 shoot system tests

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

@@ -172,5 +172,7 @@ protected void configureFaultMonitors() {
 	protected void configureSystemTests() {
 		SystemTest.registerTest("Drive Test", new DrivetrainSystemTest(drivetrain, brake, DrivetrainConstants.SYS_TEST_SPEED_DRIVE));
 		SystemTest.registerTest("Azimuth Test", new TurnSystemTest(drivetrain, brake, DrivetrainConstants.SYS_TEST_SPEED_TURN));
+
+		// SystemTest.registerTest("Shooter Test", new ShooterSystemTest(shooter, flywheel, collector, indexer, pivot));
 	}
 }

src/main/java/frc/robot/command/tests/testCommands/CollectorTest.java

@@ -0,0 +1,40 @@
+// 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.testCommands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Collector;
+
+public class CollectorTest extends Command {
+
+  private Collector collector;
+  private double power;
+
+  public CollectorTest(Collector collector, double power) {
+    this.collector = collector;
+    this.power = power;
+
+    addRequirements(collector);
+  }
+
+  @Override
+  public void initialize() {
+    collector.setPower(power);
+  }
+
+  @Override
+  public void execute() {}
+
+  @Override
+  public void end(boolean interrupted) {
+    collector.setPower(0);
+  }
+
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+
+}

src/main/java/frc/robot/command/tests/testCommands/FlywheelTest.java

@@ -0,0 +1,43 @@
+// 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.testCommands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Flywheel;
+
+public class FlywheelTest extends Command {
+
+  private Flywheel flywheel;
+  private double motor2Speed;
+  private double motor1Speed;
+
+  public FlywheelTest(Flywheel flywheel, double motor1Speed, double motor2Speed) {
+    this.flywheel = flywheel;
+    this.motor1Speed = motor1Speed;
+    this.motor2Speed = motor2Speed;
+
+    addRequirements(flywheel);
+  }
+
+  @Override
+  public void initialize() {
+    flywheel.setMoter1RPM(motor1Speed);
+    flywheel.setMoter1RPM(motor2Speed);
+  }
+
+  @Override
+  public void execute() {}
+
+  @Override
+  public void end(boolean interrupted) {
+    flywheel.setAllMotorsRPM(0);
+  }
+
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+
+}

src/main/java/frc/robot/command/tests/testCommands/IndexerTest.java

@@ -0,0 +1,40 @@
+// 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.testCommands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Indexer;
+
+public class IndexerTest extends Command {
+
+  private Indexer indexer;
+  private double power;
+
+  public IndexerTest(Indexer indexer, double power) {
+    this.indexer = indexer;
+    this.power = power;
+
+    addRequirements(indexer);
+  }
+
+  @Override
+  public void initialize() {
+    indexer.setPower(power);
+  }
+
+  @Override
+  public void execute() {}
+
+  @Override
+  public void end(boolean interrupted) {
+    indexer.setPower(0);
+  }
+
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+
+}

src/main/java/frc/robot/command/tests/testCommands/PivotTest.java

@@ -0,0 +1,38 @@
+// 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.testCommands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Pivot;
+
+public class PivotTest extends Command {
+
+  private Pivot pivot;
+  private double angle;
+
+  public PivotTest(Pivot pivot, double angle) {
+    this.pivot = pivot;
+    this.angle = angle;
+
+    addRequirements(pivot);
+  }
+
+  @Override
+  public void initialize() {
+    pivot.setTargetAngle(angle);
+  }
+
+  @Override
+  public void execute() {}
+
+  @Override
+  public void end(boolean interrupted) {}
+
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+
+}