From 68433acac393fa87cb79f21de143d16198e38177 Mon Sep 17 00:00:00 2001 From: WindowsVistaisCool Date: Wed, 24 Jan 2024 20:33:34 -0500 Subject: [PATCH 1/2] [#88] basic system tests --- src/main/java/frc/robot/RobotContainer.java | 14 ++++++ .../frc/robot/command/tests/DriveTest.java | 46 +++++++++++++++++++ .../command/tests/DrivetrainSystemTest.java | 26 +++++++++++ .../robot/command/tests/TurnSystemTest.java | 43 +++++++++++++++++ 4 files changed, 129 insertions(+) create mode 100644 src/main/java/frc/robot/command/tests/DriveTest.java create mode 100644 src/main/java/frc/robot/command/tests/DrivetrainSystemTest.java create mode 100644 src/main/java/frc/robot/command/tests/TurnSystemTest.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 080404fe..77ca2b0a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,12 +10,16 @@ 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.wpilibj2.command.button.Trigger; import frc.robot.Constants.ControllerConstants; import frc.robot.Constants.DrivetrAinConstants; import frc.robot.Constants.TunerConstants; import frc.robot.command.PointAtTag; import frc.robot.command.Shoot; +import frc.robot.command.tests.DrivetrainSystemTest; +import frc.robot.command.tests.TurnSystemTest; import frc.robot.subsystems.Collector; import frc.robot.subsystems.Collision; import frc.robot.subsystems.Flywheel; @@ -24,7 +28,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; @@ -136,5 +142,13 @@ 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) + + )); } } diff --git a/src/main/java/frc/robot/command/tests/DriveTest.java b/src/main/java/frc/robot/command/tests/DriveTest.java new file mode 100644 index 00000000..b3f33ef5 --- /dev/null +++ b/src/main/java/frc/robot/command/tests/DriveTest.java @@ -0,0 +1,46 @@ +// 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.SwerveModule; +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; + + 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; + } +} diff --git a/src/main/java/frc/robot/command/tests/DrivetrainSystemTest.java b/src/main/java/frc/robot/command/tests/DrivetrainSystemTest.java new file mode 100644 index 00000000..d89f8122 --- /dev/null +++ b/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 + ); + } +} diff --git a/src/main/java/frc/robot/command/tests/TurnSystemTest.java b/src/main/java/frc/robot/command/tests/TurnSystemTest.java new file mode 100644 index 00000000..e7ea4d06 --- /dev/null +++ b/src/main/java/frc/robot/command/tests/TurnSystemTest.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; + +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; + + 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; + } +} From 7a4afc5ecada468d9934d48ce324b4809a121681 Mon Sep 17 00:00:00 2001 From: WindowsVistaisCool Date: Sat, 27 Jan 2024 13:49:10 -0500 Subject: [PATCH 2/2] [#88] comments n stuff --- src/main/java/frc/robot/RobotContainer.java | 1 - src/main/java/frc/robot/command/tests/DriveTest.java | 7 ++++++- src/main/java/frc/robot/command/tests/TurnSystemTest.java | 5 +++++ 3 files changed, 11 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index be995dd2..57ea49b4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -165,7 +165,6 @@ protected void configureSystemTests() { new TimedCommand(new TurnSystemTest(drivetrain, () -> 0.25), 2), new WaitCommand(0.5), new TimedCommand(new TurnSystemTest(drivetrain, () -> -0.25), 2) - )); } } diff --git a/src/main/java/frc/robot/command/tests/DriveTest.java b/src/main/java/frc/robot/command/tests/DriveTest.java index b3f33ef5..9cecdfb8 100644 --- a/src/main/java/frc/robot/command/tests/DriveTest.java +++ b/src/main/java/frc/robot/command/tests/DriveTest.java @@ -6,7 +6,6 @@ import java.util.function.DoubleSupplier; -import com.ctre.phoenix6.mechanisms.swerve.SwerveModule; import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; import edu.wpi.first.wpilibj2.command.Command; @@ -18,6 +17,12 @@ public class DriveTest extends Command { 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; diff --git a/src/main/java/frc/robot/command/tests/TurnSystemTest.java b/src/main/java/frc/robot/command/tests/TurnSystemTest.java index e7ea4d06..6820bca1 100644 --- a/src/main/java/frc/robot/command/tests/TurnSystemTest.java +++ b/src/main/java/frc/robot/command/tests/TurnSystemTest.java @@ -16,6 +16,11 @@ 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;