diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3b426ccf..0ecc876f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -31,6 +31,7 @@ import frc.robot.command.TipDetection; import frc.robot.command.Shoot; import frc.robot.command.tests.DrivetrainSystemTest; +import frc.robot.command.tests.ShooterSystemTest; import frc.robot.command.tests.TurnSystemTest; import frc.robot.command.tests.testCommands.TurnTest; import frc.robot.command.ChasePieces; @@ -172,5 +173,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)); } } diff --git a/src/main/java/frc/robot/command/tests/ShooterSystemTest.java b/src/main/java/frc/robot/command/tests/ShooterSystemTest.java new file mode 100644 index 00000000..328cfcd7 --- /dev/null +++ b/src/main/java/frc/robot/command/tests/ShooterSystemTest.java @@ -0,0 +1,54 @@ +// 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.command.tests.testCommands.CollectorTest; +import frc.robot.command.tests.testCommands.FlywheelTest; +import frc.robot.command.tests.testCommands.IndexerTest; +import frc.robot.command.tests.testCommands.PivotTest; +import frc.robot.subsystems.Collector; +import frc.robot.subsystems.Flywheel; +import frc.robot.subsystems.Indexer; +import frc.robot.subsystems.Pivot; +import frc.robot.subsystems.Shooter; +import frc.thunder.command.TimedCommand; +import frc.thunder.testing.SystemTestCommandGroup; + +public class ShooterSystemTest extends SystemTestCommandGroup { + + public ShooterSystemTest(Shooter shooter, Flywheel flywheel, Collector collector, Indexer indexer, Pivot pivot, double speed) { + super( + new SequentialCommandGroup( + new WaitCommand(0.5), + new TimedCommand(new FlywheelTest(flywheel, speed, 0), 2), // Motor 1 out + new WaitCommand(1), + new TimedCommand(new FlywheelTest(flywheel, -speed, 0), 2), // Motor 1 in + new WaitCommand(1), + new TimedCommand(new FlywheelTest(flywheel, 0, speed), 2), // Motor 2 out + new WaitCommand(1), + new TimedCommand(new FlywheelTest(flywheel, 0, -speed), 2), // Motor 2 in + new WaitCommand(1), + new TimedCommand(new FlywheelTest(flywheel, speed, speed), 2), // Both out + new WaitCommand(1), + new TimedCommand(new FlywheelTest(flywheel, -speed, -speed), 2), // Both in + new WaitCommand(1), + new TimedCommand(new PivotTest(pivot, speed), 2), // Pivot up + new WaitCommand(1), + new TimedCommand(new PivotTest(pivot, -speed), 2), // Pivot down + new WaitCommand(1), + new TimedCommand(new IndexerTest(indexer, speed), 2), // Indexer out + new WaitCommand(1), + new TimedCommand(new IndexerTest(indexer, -speed), 2), // Indexer in + new WaitCommand(1), + new TimedCommand(new CollectorTest(collector, speed), 2), // Collector out + new WaitCommand(1), + new TimedCommand(new CollectorTest(collector, -speed), 2) // Collector in + ) + ); + } + +} diff --git a/src/main/java/frc/robot/command/tests/ShooterTest.java b/src/main/java/frc/robot/command/tests/ShooterTest.java deleted file mode 100644 index 11da8c6c..00000000 --- a/src/main/java/frc/robot/command/tests/ShooterTest.java +++ /dev/null @@ -1,64 +0,0 @@ -// 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.Collector; -import frc.robot.subsystems.Flywheel; -import frc.robot.subsystems.Indexer; -import frc.robot.subsystems.Pivot; -import frc.robot.subsystems.Shooter; -import frc.thunder.command.TimedCommand; - - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class ShooterTest extends SequentialCommandGroup { - - private Pivot pivot; - private Indexer indexer; - private Flywheel flywheel; - private Shooter shooter; - private Collector collector; - - public ShooterTest(Shooter shooter, Flywheel flywheel, Collector collector, Indexer indexer, Pivot pivot) { - this.shooter = shooter; - this.flywheel = flywheel; - this.collector = collector; - this.indexer = indexer; - this.pivot = pivot; - - // Add your commands in the addCommands() call, e.g. - // addCommands(new FooCommand(), new BarCommand()); - addCommands( - new WaitCommand(0.5), - new TimedCommand(new FlywheelTest(flywheel, 0.5, 0), 2), // Motor 1 out - new WaitCommand(1), - new TimedCommand(new FlywheelTest(flywheel, -0.5, 0), 2), // Motor 1 in - new WaitCommand(1), - new TimedCommand(new FlywheelTest(flywheel, 0, 0.5), 2), // Motor 2 out - new WaitCommand(1), - new TimedCommand(new FlywheelTest(flywheel, 0, -0.5), 2), // Motor 2 in - new WaitCommand(1), - new TimedCommand(new FlywheelTest(flywheel, 0.5, 0.5), 2), // Both out - new WaitCommand(1), - new TimedCommand(new FlywheelTest(flywheel, -0.5, -0.5), 2), // Both in - new WaitCommand(1), - new TimedCommand(new PivotTest(pivot, 0.5), 2), // Pivot up - new WaitCommand(1), - new TimedCommand(new PivotTest(pivot, -0.5), 2), // Pivot down - new WaitCommand(1), - new TimedCommand(new IndexerTest(indexer, 0.5), 2), // Indexer out - new WaitCommand(1), - new TimedCommand(new IndexerTest(indexer, -0.5), 2), // Indexer in - new WaitCommand(1), - new TimedCommand(new CollectorTest(collector, 0.5), 2), // Collector out - new WaitCommand(1), - new TimedCommand(new CollectorTest(collector, -0.5), 2) // Collector in - ); - } -} diff --git a/src/main/java/frc/robot/command/tests/CollectorTest.java b/src/main/java/frc/robot/command/tests/testCommands/CollectorTest.java similarity index 66% rename from src/main/java/frc/robot/command/tests/CollectorTest.java rename to src/main/java/frc/robot/command/tests/testCommands/CollectorTest.java index 2e9f66c8..809e8c81 100644 --- a/src/main/java/frc/robot/command/tests/CollectorTest.java +++ b/src/main/java/frc/robot/command/tests/testCommands/CollectorTest.java @@ -2,41 +2,39 @@ // 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; +package frc.robot.command.tests.testCommands; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Collector; public class CollectorTest extends Command { - Collector collector; + + private Collector collector; private double power; - /** Creates a new CollectorTest. */ + public CollectorTest(Collector collector, double power) { this.collector = collector; this.power = power; - // Use addRequirements() here to declare subsystem dependencies. + addRequirements(collector); } - // Called when the command is initially scheduled. @Override public void initialize() { collector.setPower(power); } - // Called every time the scheduler runs while the command is scheduled. @Override public void execute() {} - // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { collector.setPower(0); } - // Returns true when the command should end. @Override public boolean isFinished() { return false; } + } diff --git a/src/main/java/frc/robot/command/tests/FlywheelTest.java b/src/main/java/frc/robot/command/tests/testCommands/FlywheelTest.java similarity index 76% rename from src/main/java/frc/robot/command/tests/FlywheelTest.java rename to src/main/java/frc/robot/command/tests/testCommands/FlywheelTest.java index bda2c078..c5e76016 100644 --- a/src/main/java/frc/robot/command/tests/FlywheelTest.java +++ b/src/main/java/frc/robot/command/tests/testCommands/FlywheelTest.java @@ -2,17 +2,17 @@ // 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; +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; - /** Creates a new ShootTest. */ public FlywheelTest(Flywheel flywheel, double motor1Speed, double motor2Speed) { this.flywheel = flywheel; this.motor1Speed = motor1Speed; @@ -21,27 +21,23 @@ public FlywheelTest(Flywheel flywheel, double motor1Speed, double motor2Speed) { addRequirements(flywheel); } - // Called when the command is initially scheduled. @Override public void initialize() { flywheel.setMoter1RPM(motor1Speed); flywheel.setMoter1RPM(motor2Speed); - } - // Called every time the scheduler runs while the command is scheduled. @Override public void execute() {} - // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { flywheel.setAllMotorsRPM(0); } - // Returns true when the command should end. @Override public boolean isFinished() { return false; } + } diff --git a/src/main/java/frc/robot/command/tests/IndexerTest.java b/src/main/java/frc/robot/command/tests/testCommands/IndexerTest.java similarity index 65% rename from src/main/java/frc/robot/command/tests/IndexerTest.java rename to src/main/java/frc/robot/command/tests/testCommands/IndexerTest.java index 4632a170..dba96b08 100644 --- a/src/main/java/frc/robot/command/tests/IndexerTest.java +++ b/src/main/java/frc/robot/command/tests/testCommands/IndexerTest.java @@ -2,42 +2,39 @@ // 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; +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; - /** Creates a new IndexerTest. */ public IndexerTest(Indexer indexer, double power) { this.indexer = indexer; this.power = power; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(indexer); + + addRequirements(indexer); } - // Called when the command is initially scheduled. @Override public void initialize() { indexer.setPower(power); } - // Called every time the scheduler runs while the command is scheduled. @Override public void execute() {} - // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { indexer.setPower(0); } - // Returns true when the command should end. @Override public boolean isFinished() { return false; } + } diff --git a/src/main/java/frc/robot/command/tests/PivotTest.java b/src/main/java/frc/robot/command/tests/testCommands/PivotTest.java similarity index 67% rename from src/main/java/frc/robot/command/tests/PivotTest.java rename to src/main/java/frc/robot/command/tests/testCommands/PivotTest.java index 28f307e1..954f1752 100644 --- a/src/main/java/frc/robot/command/tests/PivotTest.java +++ b/src/main/java/frc/robot/command/tests/testCommands/PivotTest.java @@ -2,40 +2,37 @@ // 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; +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; - /** Creates a new PivotTest. */ public PivotTest(Pivot pivot, double angle) { this.pivot = pivot; this.angle = angle; - // Use addRequirements() here to declare subsystem dependencies. + addRequirements(pivot); } - // Called when the command is initially scheduled. @Override public void initialize() { pivot.setTargetAngle(angle); } - // Called every time the scheduler runs while the command is scheduled. @Override public void execute() {} - // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) {} - // Returns true when the command should end. @Override public boolean isFinished() { return false; } + }