[#118] Tested sys tests and modified them

frc-862 · Jan 27, 2024 · 634d713 · 634d713
1 parent 8cbe57e
commit 634d713
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 9 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -38,7 +38,8 @@ public class DrivetrainConstants { // TODO Get new for new robot
         public static final double SLOW_ROT_MULT = 0.007; // TODO Tune for Driver
         public static final double SLOW_SPEED_MULT = 0.4; // TODO Tune for Driver
 
-        public static final double SYS_TEST_SPEED = 0.25;
+        public static final double SYS_TEST_SPEED_DRIVE = 0.5;
+        public static final double SYS_TEST_SPEED_TURN = 1d;
     }
 
     public class RobotMap {

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -169,12 +169,13 @@ protected void configureFaultMonitors() {
 
 	@Override
 	protected void configureSystemTests() {
-		SystemTest.registerTest("Drive All Directions", new DrivetrainSystemTest(drivetrain, Constants.DrivetrainConstants.SYS_TEST_SPEED));
+		SystemTest.registerTest("Drive Test", new DrivetrainSystemTest(drivetrain, brake, Constants.DrivetrainConstants.SYS_TEST_SPEED_DRIVE));
 
 		SystemTest.registerTest("Azimuth Test", new SequentialCommandGroup(
-			new TimedCommand(new TurnSystemTest(drivetrain, () -> Constants.DrivetrainConstants.SYS_TEST_SPEED), 2),
+			new TimedCommand(new TurnSystemTest(drivetrain, () -> Constants.DrivetrainConstants.SYS_TEST_SPEED_TURN), 1),
 			new WaitCommand(0.5),
-			new TimedCommand(new TurnSystemTest(drivetrain, () -> -Constants.DrivetrainConstants.SYS_TEST_SPEED), 2)
+			new TimedCommand(new TurnSystemTest(drivetrain, () -> -Constants.DrivetrainConstants.SYS_TEST_SPEED_TURN), 1),
+			drivetrain.applyRequest(() -> brake)
 		));
 	}
 }
diff --git a/src/main/java/frc/robot/command/tests/DrivetrainSystemTest.java b/src/main/java/frc/robot/command/tests/DrivetrainSystemTest.java
@@ -4,23 +4,28 @@
 
 package frc.robot.command.tests;
 
+import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 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) {
+  public DrivetrainSystemTest(Swerve drivetrain, SwerveRequest brake, double speed) {
     addCommands(
       new WaitCommand(0.5),
-      new TimedCommand(new DriveTest(drivetrain, () -> 0d, () -> speed), 2), // Forward
+      new TimedCommand(new DriveTest(drivetrain, () -> speed, () -> 0d), 1), // Forward
       new WaitCommand(1),
-      new TimedCommand(new DriveTest(drivetrain, () -> 0d, () -> -speed), 2), // Backward
+      new TimedCommand(new DriveTest(drivetrain, () -> -speed, () -> 0d), 1), // Backward
       new WaitCommand(1),
-      new TimedCommand(new DriveTest(drivetrain, () -> -speed, () -> 0), 2), // Left
+      new TimedCommand(new DriveTest(drivetrain, () -> 0d, () -> speed), 1), // Left
       new WaitCommand(1),
-      new TimedCommand(new DriveTest(drivetrain, () -> speed, () -> 0), 2) // Right
+      new TimedCommand(new DriveTest(drivetrain, () -> 0d, () -> -speed), 1), // Right
+      new WaitCommand(0.5),
+      drivetrain.applyRequest(() -> brake) // Brake
     );
   }
 }