From e6dc6c5b45fad5b674c2ed5a821c624fdb022ab0 Mon Sep 17 00:00:00 2001 From: WindowsVistaisCool Date: Sat, 24 Feb 2024 15:20:07 -0500 Subject: [PATCH 01/13] [#312] system test cleanup + cycle test --- src/main/java/frc/robot/RobotContainer.java | 11 ++++-- src/main/java/frc/robot/command/Collect.java | 5 +-- src/main/java/frc/robot/command/Index.java | 15 +++---- .../robot/command/tests/CycleSytemTest.java | 38 ++++++++++++++++++ .../robot/command/tests/PivotAngleTest.java | 39 +++++++++++++++++++ .../robot/command/tests/PivotSystemTest.java | 26 ------------- .../command/tests/testcmds/FlywheelTest.java | 14 +++---- .../command/tests/testcmds/IndexerTest.java | 2 +- .../command/tests/testcmds/PivotTest.java | 37 ------------------ 9 files changed, 101 insertions(+), 86 deletions(-) create mode 100644 src/main/java/frc/robot/command/tests/CycleSytemTest.java create mode 100644 src/main/java/frc/robot/command/tests/PivotAngleTest.java delete mode 100644 src/main/java/frc/robot/command/tests/PivotSystemTest.java delete mode 100644 src/main/java/frc/robot/command/tests/testcmds/PivotTest.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d5cce249..61e5307c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -44,8 +44,10 @@ import frc.robot.command.shoot.Stow; import frc.robot.command.tests.ClimbSystemTest; import frc.robot.command.tests.CollectorSystemTest; +import frc.robot.command.tests.CycleSytemTest; import frc.robot.command.tests.DrivetrainSystemTest; import frc.robot.command.tests.FlywheelSystemTest; +import frc.robot.command.tests.PivotAngleTest; import frc.robot.command.tests.SingSystemTest; import frc.robot.command.tests.TurnSystemTest; import frc.robot.command.Climb; @@ -225,9 +227,9 @@ protected void configureButtonBindings() { // flywheel.decreaseBias())); // LEFT new Trigger(coPilot::getRightBumper) - .whileTrue(new Index(indexer, () -> IndexerConstants.INDEXER_DEFAULT_POWER)); + .whileTrue(new Index(() -> IndexerConstants.INDEXER_DEFAULT_POWER, indexer)); new Trigger(coPilot::getLeftBumper) - .whileTrue(new Index(indexer, () -> -IndexerConstants.INDEXER_DEFAULT_POWER)); + .whileTrue(new Index(() -> -IndexerConstants.INDEXER_DEFAULT_POWER, indexer)); /* Other */ @@ -288,10 +290,13 @@ protected void configureSystemTests() { SystemTest.registerTest("Azimuth Test", new TurnSystemTest(drivetrain, DrivetrainConstants.SYS_TEST_SPEED_TURN)); + SystemTest.registerTest("Single Note Cycle", + new CycleSytemTest(collector, indexer, pivot, flywheel, () -> 0.5d, () -> 0.6d, () -> 250)); + // SystemTest.registerTest("Collector Test", new CollectorSystemTest(collector, // Constants.CollectorConstants.COLLECTOR_SYSTEST_POWER)); - // TODO make pivot system test + // SystemTest.registerTest("Pivot 90 Degrees", new PivotAngleTest(pivot, 90d)); // SystemTest.registerTest("Flywheel Test", new FlywheelSystemTest(flywheel, collector, // indexer, pivot, Constants.FlywheelConstants.SYS_TEST_SPEED)); diff --git a/src/main/java/frc/robot/command/Collect.java b/src/main/java/frc/robot/command/Collect.java index dc0b5688..4e21ce58 100644 --- a/src/main/java/frc/robot/command/Collect.java +++ b/src/main/java/frc/robot/command/Collect.java @@ -8,9 +8,8 @@ public class Collect extends Command { - // Declares collector - private Collector collector; private DoubleSupplier powerSupplier; + private Collector collector; /** * Creates a new Collect. @@ -18,8 +17,8 @@ public class Collect extends Command { * @param collector subsystem */ public Collect(DoubleSupplier powerSupplier, Collector collector) { - this.collector = collector; this.powerSupplier = powerSupplier; + this.collector = collector; addRequirements(collector); } diff --git a/src/main/java/frc/robot/command/Index.java b/src/main/java/frc/robot/command/Index.java index 17827bee..c2d0a44e 100644 --- a/src/main/java/frc/robot/command/Index.java +++ b/src/main/java/frc/robot/command/Index.java @@ -5,40 +5,37 @@ import frc.robot.subsystems.Indexer; public class Index extends Command { - Indexer indexer; - DoubleSupplier power; + + private DoubleSupplier power; + private Indexer indexer; /** * Creates a new Index. - * @param indexer subsystem * @param power supplier for motor power + * @param indexer subsystem */ - public Index(Indexer indexer, DoubleSupplier power) { - this.indexer = indexer; + public Index(DoubleSupplier power, Indexer indexer) { this.power = power; + this.indexer = indexer; addRequirements(indexer); } - // Called when the command is initially scheduled. @Override public void initialize() { indexer.setPower(power.getAsDouble()); } - // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { indexer.setPower(power.getAsDouble()); } - // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { indexer.stop(); } - // Returns true when the command should end. @Override public boolean isFinished() { return false; diff --git a/src/main/java/frc/robot/command/tests/CycleSytemTest.java b/src/main/java/frc/robot/command/tests/CycleSytemTest.java new file mode 100644 index 00000000..b66a64c4 --- /dev/null +++ b/src/main/java/frc/robot/command/tests/CycleSytemTest.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; + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.StartEndCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.command.SmartCollect; +import frc.robot.subsystems.Collector; +import frc.robot.subsystems.Flywheel; +import frc.robot.subsystems.Indexer; +import frc.robot.subsystems.Pivot; +import frc.thunder.command.TimedCommand; +import frc.thunder.testing.SystemTestCommandGroup; + +public class CycleSytemTest extends SystemTestCommandGroup { + + public CycleSytemTest(Collector collector, Indexer indexer, Pivot pivot, Flywheel flywheel, DoubleSupplier collectorPower, DoubleSupplier indexerPower, DoubleSupplier flywheelRPM) { + super( + new SequentialCommandGroup( + new WaitCommand(0.5), + new TimedCommand(new SmartCollect(collectorPower, indexerPower, collector, indexer, pivot), 3), + new WaitCommand(1), + new TimedCommand(new ParallelCommandGroup( + new StartEndCommand(() -> flywheel.setAllMotorsRPM(flywheelRPM.getAsDouble()), () -> flywheel.coast(true)), + new RunCommand(() -> pivot.setTargetAngle(50)) + ), 2), + new WaitCommand(0.5) + ) + ); + } +} diff --git a/src/main/java/frc/robot/command/tests/PivotAngleTest.java b/src/main/java/frc/robot/command/tests/PivotAngleTest.java new file mode 100644 index 00000000..f2b8edff --- /dev/null +++ b/src/main/java/frc/robot/command/tests/PivotAngleTest.java @@ -0,0 +1,39 @@ +// 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 frc.robot.subsystems.Pivot; +import frc.thunder.testing.SystemTestCommand; + +public class PivotAngleTest extends SystemTestCommand { + + private Pivot pivot; + private double angle; + + public PivotAngleTest(Pivot pivot, double angle) { + this.pivot = pivot; + this.angle = angle; + + addRequirements(pivot); + } + + @Override + public void initializeTest() { + pivot.setTargetAngle(angle); + } + + @Override + public void executeTest() {} + + @Override + public void endTest(boolean interrupted) { + pivot.setPower(0d); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/command/tests/PivotSystemTest.java b/src/main/java/frc/robot/command/tests/PivotSystemTest.java deleted file mode 100644 index 04dc80f7..00000000 --- a/src/main/java/frc/robot/command/tests/PivotSystemTest.java +++ /dev/null @@ -1,26 +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.command.tests.testcmds.PivotTest; -import frc.robot.subsystems.Pivot; -import frc.thunder.command.TimedCommand; -import frc.thunder.testing.SystemTestCommandGroup; - -public class PivotSystemTest extends SystemTestCommandGroup { - - public PivotSystemTest(Pivot pivot, double speed) { - super( - new SequentialCommandGroup( - new WaitCommand(0.5), - new TimedCommand(new PivotTest(pivot, speed), 0.5), // Pivot up - new WaitCommand(1), - new TimedCommand(new PivotTest(pivot, -speed), 0.5) // Pivot down - ) - ); - } -} diff --git a/src/main/java/frc/robot/command/tests/testcmds/FlywheelTest.java b/src/main/java/frc/robot/command/tests/testcmds/FlywheelTest.java index eb831c31..729c3a1c 100644 --- a/src/main/java/frc/robot/command/tests/testcmds/FlywheelTest.java +++ b/src/main/java/frc/robot/command/tests/testcmds/FlywheelTest.java @@ -10,21 +10,21 @@ public class FlywheelTest extends Command { private Flywheel flywheel; - private double motor2Speed; - private double motor1Speed; + private double topMotorSpeed; + private double bottomMotorSpeed; - public FlywheelTest(Flywheel flywheel, double motor1Speed, double motor2Speed) { + public FlywheelTest(Flywheel flywheel, double topMotorSpeed, double bottomMotorSpeed) { this.flywheel = flywheel; - this.motor1Speed = motor1Speed; - this.motor2Speed = motor2Speed; + this.topMotorSpeed = topMotorSpeed; + this.bottomMotorSpeed = bottomMotorSpeed; addRequirements(flywheel); } @Override public void initialize() { - flywheel.setTopMoterRPM(motor1Speed); - flywheel.setTopMoterRPM(motor2Speed); + flywheel.setTopMoterRPM(topMotorSpeed); + flywheel.setBottomMoterRPM(bottomMotorSpeed); } @Override diff --git a/src/main/java/frc/robot/command/tests/testcmds/IndexerTest.java b/src/main/java/frc/robot/command/tests/testcmds/IndexerTest.java index 1983140e..524074fa 100644 --- a/src/main/java/frc/robot/command/tests/testcmds/IndexerTest.java +++ b/src/main/java/frc/robot/command/tests/testcmds/IndexerTest.java @@ -29,7 +29,7 @@ public void execute() {} @Override public void end(boolean interrupted) { - indexer.setPower(0); + indexer.stop(); } @Override diff --git a/src/main/java/frc/robot/command/tests/testcmds/PivotTest.java b/src/main/java/frc/robot/command/tests/testcmds/PivotTest.java deleted file mode 100644 index d98dfc5d..00000000 --- a/src/main/java/frc/robot/command/tests/testcmds/PivotTest.java +++ /dev/null @@ -1,37 +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.testcmds; - -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; - } -} From be581b43739ff388ea870f5eca2344d97ea00649 Mon Sep 17 00:00:00 2001 From: Paul J Date: Mon, 26 Feb 2024 21:10:14 -0500 Subject: [PATCH 02/13] [#312] - systest check --- .../java/frc/robot/command/tests/CycleSytemTest.java | 9 +++++---- .../java/frc/robot/command/tests/PivotAngleTest.java | 10 ++++++++-- .../java/frc/robot/command/tests/TurnSystemTest.java | 6 +++--- src/main/java/frc/thunder | 2 +- 4 files changed, 17 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/command/tests/CycleSytemTest.java b/src/main/java/frc/robot/command/tests/CycleSytemTest.java index b66a64c4..2ca7f857 100644 --- a/src/main/java/frc/robot/command/tests/CycleSytemTest.java +++ b/src/main/java/frc/robot/command/tests/CycleSytemTest.java @@ -25,11 +25,12 @@ public CycleSytemTest(Collector collector, Indexer indexer, Pivot pivot, Flywhee super( new SequentialCommandGroup( new WaitCommand(0.5), - new TimedCommand(new SmartCollect(collectorPower, indexerPower, collector, indexer, pivot), 3), + new TimedCommand(new SmartCollect(collectorPower, indexerPower, collector, indexer, pivot), 3), // Run smart collect for 3 new WaitCommand(1), - new TimedCommand(new ParallelCommandGroup( - new StartEndCommand(() -> flywheel.setAllMotorsRPM(flywheelRPM.getAsDouble()), () -> flywheel.coast(true)), - new RunCommand(() -> pivot.setTargetAngle(50)) + new TimedCommand(new ParallelCommandGroup( // Run two commands for 2 seconds + new StartEndCommand(() -> flywheel.setAllMotorsRPM(flywheelRPM.getAsDouble()), // Starts by setting flywheel RPM to flywheel RPM + () -> flywheel.coast(true)), // Slows down flywheel thru coasting + new RunCommand(() -> pivot.setTargetAngle(50)) // Meanwhile Moves pivot angle to 50 deg ), 2), new WaitCommand(0.5) ) diff --git a/src/main/java/frc/robot/command/tests/PivotAngleTest.java b/src/main/java/frc/robot/command/tests/PivotAngleTest.java index f2b8edff..c59c4454 100644 --- a/src/main/java/frc/robot/command/tests/PivotAngleTest.java +++ b/src/main/java/frc/robot/command/tests/PivotAngleTest.java @@ -12,6 +12,12 @@ public class PivotAngleTest extends SystemTestCommand { private Pivot pivot; private double angle; +/** + * Pivot Angle System test + * @param pivot used to get access to pivot + * @param angle angle to set pivot to + */ + public PivotAngleTest(Pivot pivot, double angle) { this.pivot = pivot; this.angle = angle; @@ -21,7 +27,7 @@ public PivotAngleTest(Pivot pivot, double angle) { @Override public void initializeTest() { - pivot.setTargetAngle(angle); + pivot.setTargetAngle(angle); // Gives pivot an angle to move to } @Override @@ -29,7 +35,7 @@ public void executeTest() {} @Override public void endTest(boolean interrupted) { - pivot.setPower(0d); + pivot.setPower(0d); // Stops giving pivot power } @Override diff --git a/src/main/java/frc/robot/command/tests/TurnSystemTest.java b/src/main/java/frc/robot/command/tests/TurnSystemTest.java index 83986db0..81cb00ae 100644 --- a/src/main/java/frc/robot/command/tests/TurnSystemTest.java +++ b/src/main/java/frc/robot/command/tests/TurnSystemTest.java @@ -18,11 +18,11 @@ public TurnSystemTest(Swerve drivetrain, double speed) { super( new SequentialCommandGroup( new WaitCommand(0.5), - new TimedCommand(new TurnTest(drivetrain, () -> speed), 1), + new TimedCommand(new TurnTest(drivetrain, () -> speed), 1), // Rotates at speed (-1, 1) for 1 second new WaitCommand(1), - new TimedCommand(new TurnTest(drivetrain, () -> -speed), 1), + new TimedCommand(new TurnTest(drivetrain, () -> -speed), 1), // Rotates at the opposite speed for 1 second new WaitCommand(0.5), - new InstantCommand(() -> drivetrain.brake()) + new InstantCommand(() -> drivetrain.brake()) // Brakes the robot robot ) ); } diff --git a/src/main/java/frc/thunder b/src/main/java/frc/thunder index 96746a51..761965e3 160000 --- a/src/main/java/frc/thunder +++ b/src/main/java/frc/thunder @@ -1 +1 @@ -Subproject commit 96746a51fe0772dbda637b0ae2cc2afb42751888 +Subproject commit 761965e3977ed7ef0b6cda0952bceed2359f862f From 6f3eda126751927545f03e5762513094ff5fbfec Mon Sep 17 00:00:00 2001 From: HeroSoLos Date: Wed, 28 Feb 2024 18:20:57 -0500 Subject: [PATCH 03/13] [#312]paulj trustthrsut --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 8 +++---- .../robot/command/tests/ClimbSystemTest.java | 2 +- .../robot/command/tests/PivotAngleTest.java | 3 ++- .../robot/command/tests/TurnSystemTest.java | 21 +++++++++++-------- .../command/tests/testcmds/TurnTest.java | 4 +++- 6 files changed, 23 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 169f8a57..52cd72b3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -65,7 +65,7 @@ public class DrivetrainConstants { // TODO Get new for new robot public static final double SLOW_SPEED_MULT = 0.4; // TODO Tune for Driver public static final double SYS_TEST_SPEED_DRIVE = 0.5; - public static final double SYS_TEST_SPEED_TURN = 1d; + public static final double SYS_TEST_SPEED_TURN = 0.5d; public static final Translation2d SPEAKER_POSE = new Translation2d(0d, 5.547393); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0f7e316f..e718dacd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -285,13 +285,13 @@ protected void configureFaultMonitors() {} @Override protected void configureSystemTests() { - SystemTest.registerTest("Drive Test", - new DrivetrainSystemTest(drivetrain, DrivetrainConstants.SYS_TEST_SPEED_DRIVE)); + //SystemTest.registerTest("Drive Test", + // new DrivetrainSystemTest(drivetrain, DrivetrainConstants.SYS_TEST_SPEED_DRIVE)); SystemTest.registerTest("Azimuth Test", new TurnSystemTest(drivetrain, DrivetrainConstants.SYS_TEST_SPEED_TURN)); - SystemTest.registerTest("Single Note Cycle", - new CycleSytemTest(collector, indexer, pivot, flywheel, () -> 0.5d, () -> 0.6d, () -> 250)); + //SystemTest.registerTest("Single Note Cycle", + // new CycleSytemTest(collector, indexer, pivot, flywheel, () -> 0.5d, () -> 0.6d, () -> 250)); // SystemTest.registerTest("Collector Test", new CollectorSystemTest(collector, // Constants.CollectorConstants.COLLECTOR_SYSTEST_POWER)); diff --git a/src/main/java/frc/robot/command/tests/ClimbSystemTest.java b/src/main/java/frc/robot/command/tests/ClimbSystemTest.java index 1ba66441..a27fbe40 100644 --- a/src/main/java/frc/robot/command/tests/ClimbSystemTest.java +++ b/src/main/java/frc/robot/command/tests/ClimbSystemTest.java @@ -20,7 +20,7 @@ public ClimbSystemTest(Climber climber, double speed) { new TimedCommand(new ClimbTest(climber, speed), 1), // UP new WaitCommand(1), new TimedCommand(new ClimbTest(climber, -speed), 1) // DOWN - ) + ) ); } } diff --git a/src/main/java/frc/robot/command/tests/PivotAngleTest.java b/src/main/java/frc/robot/command/tests/PivotAngleTest.java index c59c4454..6f41286a 100644 --- a/src/main/java/frc/robot/command/tests/PivotAngleTest.java +++ b/src/main/java/frc/robot/command/tests/PivotAngleTest.java @@ -13,7 +13,8 @@ public class PivotAngleTest extends SystemTestCommand { private double angle; /** - * Pivot Angle System test + * please note that this removes all usage of the drivetrain until it is ended. + * use with caution, and only when the drivetrain is not in use. * @param pivot used to get access to pivot * @param angle angle to set pivot to */ diff --git a/src/main/java/frc/robot/command/tests/TurnSystemTest.java b/src/main/java/frc/robot/command/tests/TurnSystemTest.java index 81cb00ae..0060f63f 100644 --- a/src/main/java/frc/robot/command/tests/TurnSystemTest.java +++ b/src/main/java/frc/robot/command/tests/TurnSystemTest.java @@ -16,14 +16,17 @@ public class TurnSystemTest extends SystemTestCommandGroup { public TurnSystemTest(Swerve drivetrain, double speed) { super( - new SequentialCommandGroup( - new WaitCommand(0.5), - new TimedCommand(new TurnTest(drivetrain, () -> speed), 1), // Rotates at speed (-1, 1) for 1 second - new WaitCommand(1), - new TimedCommand(new TurnTest(drivetrain, () -> -speed), 1), // Rotates at the opposite speed for 1 second - new WaitCommand(0.5), - new InstantCommand(() -> drivetrain.brake()) // Brakes the robot robot - ) - ); + new SequentialCommandGroup( + new WaitCommand(0.5), + new TimedCommand(new TurnTest(drivetrain, () -> speed), 1), // Rotates at speed 10% max speed + // for 3 second + // new WaitCommand(1), + // new TimedCommand(new TurnTest(drivetrain, () -> -speed), 3), // Rotates in + // the opposite dir for 3 second + // new WaitCommand(0.5), + new InstantCommand(() -> drivetrain.brake()) // Brakes the robot, no need imo cuz it should + // break after the TurnTest command + + )); } } diff --git a/src/main/java/frc/robot/command/tests/testcmds/TurnTest.java b/src/main/java/frc/robot/command/tests/testcmds/TurnTest.java index ab50d03f..afaa0f66 100644 --- a/src/main/java/frc/robot/command/tests/testcmds/TurnTest.java +++ b/src/main/java/frc/robot/command/tests/testcmds/TurnTest.java @@ -27,12 +27,14 @@ public void initialize() {} @Override public void execute() { - drivetrain.applyPercentRequestRobot(() -> 0d, () -> 0d, speed); + drivetrain.applyPercentRequestRobot(() -> 0d, () -> 0d, speed);//speed); + System.out.println("ExecutingTurntest"); } @Override public void end(boolean interrupted) { drivetrain.brake(); + System.out.println("Ending"); } @Override From 2714f3fb42b264c31d707d005ca93e09c1626984 Mon Sep 17 00:00:00 2001 From: Paul J Date: Wed, 28 Feb 2024 20:44:33 -0500 Subject: [PATCH 04/13] [#312] - Turn test now works. collector, flywheel, and indexer don't --- .../java/frc/robot/command/tests/CollectorSystemTest.java | 4 ++-- .../java/frc/robot/command/tests/IndexerSystemTest.java | 4 ++-- src/main/java/frc/robot/command/tests/TurnSystemTest.java | 8 ++++---- .../frc/robot/command/tests/testcmds/CollectorTest.java | 4 +++- .../java/frc/robot/command/tests/testcmds/DriveTest.java | 2 +- .../java/frc/robot/command/tests/testcmds/TurnTest.java | 4 ++-- 6 files changed, 14 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/command/tests/CollectorSystemTest.java b/src/main/java/frc/robot/command/tests/CollectorSystemTest.java index 4e571fde..7a141010 100644 --- a/src/main/java/frc/robot/command/tests/CollectorSystemTest.java +++ b/src/main/java/frc/robot/command/tests/CollectorSystemTest.java @@ -17,9 +17,9 @@ public CollectorSystemTest(Collector collector, double speed) { super( new SequentialCommandGroup( new WaitCommand(0.5), - new TimedCommand(new CollectorTest(collector, speed), 2), // Collector out + new TimedCommand(new CollectorTest(collector, speed), 4), // Collector out new WaitCommand(1), - new TimedCommand(new CollectorTest(collector, -speed), 2) // Collector in + new TimedCommand(new CollectorTest(collector, -speed), 4) // Collector in ) ); } diff --git a/src/main/java/frc/robot/command/tests/IndexerSystemTest.java b/src/main/java/frc/robot/command/tests/IndexerSystemTest.java index 32aa1dec..f2d5bfea 100644 --- a/src/main/java/frc/robot/command/tests/IndexerSystemTest.java +++ b/src/main/java/frc/robot/command/tests/IndexerSystemTest.java @@ -17,9 +17,9 @@ public IndexerSystemTest(Indexer indexer, double speed) { super( new SequentialCommandGroup( new WaitCommand(0.5), - new TimedCommand(new IndexerTest(indexer, speed), 2), // Indexer out + new TimedCommand(new IndexerTest(indexer, speed), 3), // Indexer out new WaitCommand(1), - new TimedCommand(new IndexerTest(indexer, -speed), 2) // Indexer in + new TimedCommand(new IndexerTest(indexer, -speed), 3) // Indexer in ) ); } diff --git a/src/main/java/frc/robot/command/tests/TurnSystemTest.java b/src/main/java/frc/robot/command/tests/TurnSystemTest.java index 0060f63f..9cb71759 100644 --- a/src/main/java/frc/robot/command/tests/TurnSystemTest.java +++ b/src/main/java/frc/robot/command/tests/TurnSystemTest.java @@ -18,12 +18,12 @@ public TurnSystemTest(Swerve drivetrain, double speed) { super( new SequentialCommandGroup( new WaitCommand(0.5), - new TimedCommand(new TurnTest(drivetrain, () -> speed), 1), // Rotates at speed 10% max speed + new TimedCommand(new TurnTest(drivetrain, () -> speed), 6), // Rotates at speed 10% max speed // for 3 second - // new WaitCommand(1), - // new TimedCommand(new TurnTest(drivetrain, () -> -speed), 3), // Rotates in + new WaitCommand(1), + new TimedCommand(new TurnTest(drivetrain, () -> -speed), 6), // Rotates in // the opposite dir for 3 second - // new WaitCommand(0.5), + new WaitCommand(0.5), new InstantCommand(() -> drivetrain.brake()) // Brakes the robot, no need imo cuz it should // break after the TurnTest command diff --git a/src/main/java/frc/robot/command/tests/testcmds/CollectorTest.java b/src/main/java/frc/robot/command/tests/testcmds/CollectorTest.java index 3fd6aff8..b761d84e 100644 --- a/src/main/java/frc/robot/command/tests/testcmds/CollectorTest.java +++ b/src/main/java/frc/robot/command/tests/testcmds/CollectorTest.java @@ -25,7 +25,9 @@ public void initialize() { } @Override - public void execute() {} + public void execute() { + collector.setPower(power); + } @Override public void end(boolean interrupted) { diff --git a/src/main/java/frc/robot/command/tests/testcmds/DriveTest.java b/src/main/java/frc/robot/command/tests/testcmds/DriveTest.java index 6ebd0c55..8dbe09d7 100644 --- a/src/main/java/frc/robot/command/tests/testcmds/DriveTest.java +++ b/src/main/java/frc/robot/command/tests/testcmds/DriveTest.java @@ -30,7 +30,7 @@ public void initialize() {} @Override public void execute() { - drivetrain.applyPercentRequestRobot(speedX, speedY, () -> 0d); + drivetrain.setRobot(speedX.getAsDouble(), speedY.getAsDouble(), 0d); } @Override diff --git a/src/main/java/frc/robot/command/tests/testcmds/TurnTest.java b/src/main/java/frc/robot/command/tests/testcmds/TurnTest.java index afaa0f66..0c0d8834 100644 --- a/src/main/java/frc/robot/command/tests/testcmds/TurnTest.java +++ b/src/main/java/frc/robot/command/tests/testcmds/TurnTest.java @@ -8,7 +8,7 @@ public class TurnTest extends Command { private Swerve drivetrain; - private DoubleSupplier speed; + public DoubleSupplier speed; /** * System test command for testing azimuth motors @@ -27,7 +27,7 @@ public void initialize() {} @Override public void execute() { - drivetrain.applyPercentRequestRobot(() -> 0d, () -> 0d, speed);//speed); + drivetrain.setRobot(0d, 0d, speed.getAsDouble());//speed); System.out.println("ExecutingTurntest"); } From 285186b057a2d57c81beb1027a6836b6861b6ec3 Mon Sep 17 00:00:00 2001 From: Paul J Date: Thu, 29 Feb 2024 20:53:33 -0500 Subject: [PATCH 05/13] [#312] systest check Should work. Turn test, drivetrain test, and collecter work. Index and flywheel should work but needs to be tested --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 14 +++++++------- src/main/java/frc/robot/command/Collect.java | 1 + src/main/java/frc/robot/command/MoveToPose.java | 2 +- src/main/java/frc/robot/command/PointAtTag.java | 2 +- .../robot/command/tests/CollectorSystemTest.java | 10 +++++++--- .../robot/command/tests/FlywheelSystemTest.java | 3 ++- .../robot/command/tests/IndexerSystemTest.java | 1 + .../robot/command/tests/testcmds/ClimbTest.java | 2 ++ .../command/tests/testcmds/CollectorTest.java | 9 +++++---- .../java/frc/robot/subsystems/Collector.java | 1 + src/main/java/frc/robot/subsystems/Swerve.java | 16 ++++++++++------ 12 files changed, 39 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 52cd72b3..1668ec55 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -65,7 +65,7 @@ public class DrivetrainConstants { // TODO Get new for new robot public static final double SLOW_SPEED_MULT = 0.4; // TODO Tune for Driver public static final double SYS_TEST_SPEED_DRIVE = 0.5; - public static final double SYS_TEST_SPEED_TURN = 0.5d; + public static final double SYS_TEST_SPEED_TURN = 0.7d; public static final Translation2d SPEAKER_POSE = new Translation2d(0d, 5.547393); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e718dacd..e755391d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -285,21 +285,21 @@ protected void configureFaultMonitors() {} @Override protected void configureSystemTests() { - //SystemTest.registerTest("Drive Test", - // new DrivetrainSystemTest(drivetrain, DrivetrainConstants.SYS_TEST_SPEED_DRIVE)); + SystemTest.registerTest("Drive Test", + new DrivetrainSystemTest(drivetrain, DrivetrainConstants.SYS_TEST_SPEED_DRIVE));//to be tested SystemTest.registerTest("Azimuth Test", - new TurnSystemTest(drivetrain, DrivetrainConstants.SYS_TEST_SPEED_TURN)); + new TurnSystemTest(drivetrain, DrivetrainConstants.SYS_TEST_SPEED_TURN));// works //SystemTest.registerTest("Single Note Cycle", // new CycleSytemTest(collector, indexer, pivot, flywheel, () -> 0.5d, () -> 0.6d, () -> 250)); - // SystemTest.registerTest("Collector Test", new CollectorSystemTest(collector, - // Constants.CollectorConstants.COLLECTOR_SYSTEST_POWER)); + SystemTest.registerTest("Collector Test", new CollectorSystemTest(collector,//to be tested + Constants.CollectorConstants.COLLECTOR_SYSTEST_POWER)); // SystemTest.registerTest("Pivot 90 Degrees", new PivotAngleTest(pivot, 90d)); - // SystemTest.registerTest("Flywheel Test", new FlywheelSystemTest(flywheel, collector, - // indexer, pivot, Constants.FlywheelConstants.SYS_TEST_SPEED)); + SystemTest.registerTest("Flywheel Test", new FlywheelSystemTest(flywheel, collector,//to be tested + indexer, pivot, Constants.FlywheelConstants.FLYWHEEL_SYSTEST_POWER)); // SystemTest.registerTest("Climb Test", new ClimbSystemTest(climber, // Constants.ClimbConstants.CLIMB_SYSTEST_POWER)); diff --git a/src/main/java/frc/robot/command/Collect.java b/src/main/java/frc/robot/command/Collect.java index 4e21ce58..001be8aa 100644 --- a/src/main/java/frc/robot/command/Collect.java +++ b/src/main/java/frc/robot/command/Collect.java @@ -26,6 +26,7 @@ public Collect(DoubleSupplier powerSupplier, Collector collector) { @Override public void initialize() { collector.setPower(powerSupplier.getAsDouble()); + System.out.println("Collect.init"); LightningShuffleboard.setDoubleSupplier("Collector", "Power", () -> powerSupplier.getAsDouble()); } diff --git a/src/main/java/frc/robot/command/MoveToPose.java b/src/main/java/frc/robot/command/MoveToPose.java index 6c5b3013..a45d3e6c 100644 --- a/src/main/java/frc/robot/command/MoveToPose.java +++ b/src/main/java/frc/robot/command/MoveToPose.java @@ -48,7 +48,7 @@ private void initLogging() { @Override public void execute() { - current = drivetrain.getPose().get(); + current = drivetrain.getPose(); dx = target.getTranslation().getX() - current.getTranslation().getX(); dy = target.getTranslation().getY() - current.getTranslation().getY(); diff --git a/src/main/java/frc/robot/command/PointAtTag.java b/src/main/java/frc/robot/command/PointAtTag.java index d093faa2..be06eb3a 100644 --- a/src/main/java/frc/robot/command/PointAtTag.java +++ b/src/main/java/frc/robot/command/PointAtTag.java @@ -76,7 +76,7 @@ public void execute() { // headingController.setI(LightningShuffleboard.getDouble("PointAtTag", "I", 0)); // headingController.setD(LightningShuffleboard.getDouble("PointAtTag", "D", 1)); - Pose2d pose = drivetrain.getPose().get(); + Pose2d pose = drivetrain.getPose(); deltaX = targetPose.getX() - pose.getX(); deltaY = targetPose.getY() - pose.getY(); diff --git a/src/main/java/frc/robot/command/tests/CollectorSystemTest.java b/src/main/java/frc/robot/command/tests/CollectorSystemTest.java index 7a141010..f82d7fd9 100644 --- a/src/main/java/frc/robot/command/tests/CollectorSystemTest.java +++ b/src/main/java/frc/robot/command/tests/CollectorSystemTest.java @@ -4,6 +4,8 @@ package frc.robot.command.tests; +import java.util.function.DoubleSupplier; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.command.tests.testcmds.CollectorTest; @@ -13,14 +15,16 @@ public class CollectorSystemTest extends SystemTestCommandGroup { - public CollectorSystemTest(Collector collector, double speed) { + public CollectorSystemTest(Collector collector, Double power) { super( new SequentialCommandGroup( new WaitCommand(0.5), - new TimedCommand(new CollectorTest(collector, speed), 4), // Collector out + new TimedCommand(new CollectorTest(collector,() -> power), 4), // Collector out new WaitCommand(1), - new TimedCommand(new CollectorTest(collector, -speed), 4) // Collector in + new TimedCommand(new CollectorTest(collector,() -> -power), 4) // Collector in + ) ); + addRequirements(collector); } } diff --git a/src/main/java/frc/robot/command/tests/FlywheelSystemTest.java b/src/main/java/frc/robot/command/tests/FlywheelSystemTest.java index ebea2e14..4ebc8c72 100644 --- a/src/main/java/frc/robot/command/tests/FlywheelSystemTest.java +++ b/src/main/java/frc/robot/command/tests/FlywheelSystemTest.java @@ -30,7 +30,8 @@ public FlywheelSystemTest(Flywheel flywheel, Collector collector, Indexer indexe new TimedCommand(new FlywheelTest(flywheel, speed, speed), 2), // Both out new WaitCommand(1), new TimedCommand(new FlywheelTest(flywheel, -speed, -speed), 2) // Both in - ) + ) ); + addRequirements(flywheel); } } diff --git a/src/main/java/frc/robot/command/tests/IndexerSystemTest.java b/src/main/java/frc/robot/command/tests/IndexerSystemTest.java index f2d5bfea..4560c903 100644 --- a/src/main/java/frc/robot/command/tests/IndexerSystemTest.java +++ b/src/main/java/frc/robot/command/tests/IndexerSystemTest.java @@ -22,5 +22,6 @@ public IndexerSystemTest(Indexer indexer, double speed) { new TimedCommand(new IndexerTest(indexer, -speed), 3) // Indexer in ) ); + addRequirements(indexer); } } diff --git a/src/main/java/frc/robot/command/tests/testcmds/ClimbTest.java b/src/main/java/frc/robot/command/tests/testcmds/ClimbTest.java index c2830db7..8f7c3af4 100644 --- a/src/main/java/frc/robot/command/tests/testcmds/ClimbTest.java +++ b/src/main/java/frc/robot/command/tests/testcmds/ClimbTest.java @@ -4,6 +4,8 @@ package frc.robot.command.tests.testcmds; +import java.util.function.DoubleSupplier; + import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Climber; diff --git a/src/main/java/frc/robot/command/tests/testcmds/CollectorTest.java b/src/main/java/frc/robot/command/tests/testcmds/CollectorTest.java index b761d84e..7b8e722c 100644 --- a/src/main/java/frc/robot/command/tests/testcmds/CollectorTest.java +++ b/src/main/java/frc/robot/command/tests/testcmds/CollectorTest.java @@ -4,15 +4,17 @@ package frc.robot.command.tests.testcmds; +import java.util.function.DoubleSupplier; + import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Collector; public class CollectorTest extends Command { private Collector collector; - private double power; + public DoubleSupplier power; - public CollectorTest(Collector collector, double power) { + public CollectorTest(Collector collector, DoubleSupplier power) { this.collector = collector; this.power = power; @@ -21,12 +23,11 @@ public CollectorTest(Collector collector, double power) { @Override public void initialize() { - collector.setPower(power); + collector.setPower(power.getAsDouble()); } @Override public void execute() { - collector.setPower(power); } @Override diff --git a/src/main/java/frc/robot/subsystems/Collector.java b/src/main/java/frc/robot/subsystems/Collector.java index e694ef27..6d1e25fb 100644 --- a/src/main/java/frc/robot/subsystems/Collector.java +++ b/src/main/java/frc/robot/subsystems/Collector.java @@ -67,6 +67,7 @@ public boolean hasPiece() { * Stops the collector */ public void stop() { + System.out.println("Collector.stop"); setPower(0d); } } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 80f710b0..98bef7b2 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -187,8 +187,8 @@ private void initLogging() { // TODO Remove the unecessary shuffleboard stuff eventually LightningShuffleboard.setDoubleSupplier("Swerve", "Timer", () -> Timer.getFPGATimestamp()); LightningShuffleboard.setDoubleSupplier("Swerve", "Robot Heading", () -> getPigeon2().getAngle()); - LightningShuffleboard.setDoubleSupplier("Swerve", "Odo X", () -> getState().Pose.getX()); - LightningShuffleboard.setDoubleSupplier("Swerve", "Odo Y", () -> getState().Pose.getY()); + LightningShuffleboard.setDoubleSupplier("Swerve", "Odo X", () -> getPose().getX()); + LightningShuffleboard.setDoubleSupplier("Swerve", "Odo Y", () -> getPose().getY()); LightningShuffleboard.setBoolSupplier("Swerve", "Slow mode", () -> slowMode); LightningShuffleboard.setBoolSupplier("Swerve", "Robot Centric", () -> isRobotCentricControl()); @@ -225,8 +225,12 @@ AutonomousConstants.DRIVE_BASE_RADIUS, new ReplanningConfig(), }, this); // Subsystem for requirements } - public Supplier getPose() { - return () -> getState().Pose; + public Pose2d getPose() { + var pose = getState().Pose; + if (pose == null) { + pose = new Pose2d(); + } + return pose; } public ChassisSpeeds getCurrentRobotChassisSpeeds() { @@ -302,7 +306,7 @@ public void swap(XboxControllerFilter driverC, XboxControllerFilter copilotC) { * @return boolean if the robot is in the wing to start aiming STATE priming */ public boolean inWing() { - return (getPose().get().getX() < ShooterConstants.FAR_WING_X); + return (getPose().getX() < ShooterConstants.FAR_WING_X); } public void disableVision() { @@ -316,6 +320,6 @@ public void enableVision() { } public double distanceToSpeaker() { - return DrivetrainConstants.SPEAKER_POSE.getDistance(getPose().get().getTranslation()); + return DrivetrainConstants.SPEAKER_POSE.getDistance(getPose().getTranslation()); } } From 187b11e2caf8852a299cff2aecdad919a6532dbe Mon Sep 17 00:00:00 2001 From: HeroSoLos Date: Sat, 2 Mar 2024 09:36:56 -0500 Subject: [PATCH 06/13] [#312] fixed flywheel + added indexer David deleted easy lines, i strong (dis)like Savid Dandburg Prolly works btw --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 5 ++++- .../frc/robot/command/tests/CollectorSystemTest.java | 2 -- .../frc/robot/command/tests/FlywheelSystemTest.java | 11 +++++++---- .../frc/robot/command/tests/IndexerSystemTest.java | 4 ++-- .../robot/command/tests/testcmds/FlywheelTest.java | 1 + 6 files changed, 15 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1668ec55..30b73400 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -389,7 +389,7 @@ public class FlywheelConstants { // TODO: get real public static final double COAST_VOLTAGE = 0.1; - public static final double FLYWHEEL_SYSTEST_POWER = 0.5; + public static final double FLYWHEEL_SYSTEST_RPM = 1000; } public class IndexerConstants { // TODO: get real diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e755391d..9cbcae6b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -47,6 +47,7 @@ import frc.robot.command.tests.CycleSytemTest; import frc.robot.command.tests.DrivetrainSystemTest; import frc.robot.command.tests.FlywheelSystemTest; +import frc.robot.command.tests.IndexerSystemTest; import frc.robot.command.tests.PivotAngleTest; import frc.robot.command.tests.SingSystemTest; import frc.robot.command.tests.TurnSystemTest; @@ -299,7 +300,9 @@ protected void configureSystemTests() { // SystemTest.registerTest("Pivot 90 Degrees", new PivotAngleTest(pivot, 90d)); SystemTest.registerTest("Flywheel Test", new FlywheelSystemTest(flywheel, collector,//to be tested - indexer, pivot, Constants.FlywheelConstants.FLYWHEEL_SYSTEST_POWER)); + indexer, pivot, Constants.FlywheelConstants.FLYWHEEL_SYSTEST_RPM)); + + SystemTest.registerTest("Indexer Test", new IndexerSystemTest(indexer, 0.1)); // SystemTest.registerTest("Climb Test", new ClimbSystemTest(climber, // Constants.ClimbConstants.CLIMB_SYSTEST_POWER)); diff --git a/src/main/java/frc/robot/command/tests/CollectorSystemTest.java b/src/main/java/frc/robot/command/tests/CollectorSystemTest.java index f82d7fd9..50f31999 100644 --- a/src/main/java/frc/robot/command/tests/CollectorSystemTest.java +++ b/src/main/java/frc/robot/command/tests/CollectorSystemTest.java @@ -4,8 +4,6 @@ package frc.robot.command.tests; -import java.util.function.DoubleSupplier; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.command.tests.testcmds.CollectorTest; diff --git a/src/main/java/frc/robot/command/tests/FlywheelSystemTest.java b/src/main/java/frc/robot/command/tests/FlywheelSystemTest.java index 4ebc8c72..7133673b 100644 --- a/src/main/java/frc/robot/command/tests/FlywheelSystemTest.java +++ b/src/main/java/frc/robot/command/tests/FlywheelSystemTest.java @@ -3,6 +3,7 @@ // 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. +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.command.tests.testcmds.FlywheelTest; @@ -19,13 +20,15 @@ public FlywheelSystemTest(Flywheel flywheel, Collector collector, Indexer indexe super( new SequentialCommandGroup( new WaitCommand(0.5), - new TimedCommand(new FlywheelTest(flywheel, speed, 0), 2), // Motor 1 out + // new InstantCommand(() -> { System.out.println("Start top"); }), + new TimedCommand(new FlywheelTest(flywheel, speed, 0.0), 2), // Motor 1 out + // new InstantCommand(() -> { System.out.println("Stop top"); }), new WaitCommand(1), - new TimedCommand(new FlywheelTest(flywheel, -speed, 0), 2), // Motor 1 in + new TimedCommand(new FlywheelTest(flywheel, -speed, 0.0), 2), // Motor 1 in new WaitCommand(1), - new TimedCommand(new FlywheelTest(flywheel, 0, speed), 2), // Motor 2 out + new TimedCommand(new FlywheelTest(flywheel, 0.0, speed), 2), // Motor 2 out new WaitCommand(1), - new TimedCommand(new FlywheelTest(flywheel, 0, -speed), 2), // Motor 2 in + new TimedCommand(new FlywheelTest(flywheel, 0.0, -speed), 2), // Motor 2 in new WaitCommand(1), new TimedCommand(new FlywheelTest(flywheel, speed, speed), 2), // Both out new WaitCommand(1), diff --git a/src/main/java/frc/robot/command/tests/IndexerSystemTest.java b/src/main/java/frc/robot/command/tests/IndexerSystemTest.java index 4560c903..1ee7c0c8 100644 --- a/src/main/java/frc/robot/command/tests/IndexerSystemTest.java +++ b/src/main/java/frc/robot/command/tests/IndexerSystemTest.java @@ -16,9 +16,9 @@ public class IndexerSystemTest extends SystemTestCommandGroup { public IndexerSystemTest(Indexer indexer, double speed) { super( new SequentialCommandGroup( - new WaitCommand(0.5), + new WaitCommand(0.50), new TimedCommand(new IndexerTest(indexer, speed), 3), // Indexer out - new WaitCommand(1), + new WaitCommand(1.00), new TimedCommand(new IndexerTest(indexer, -speed), 3) // Indexer in ) ); diff --git a/src/main/java/frc/robot/command/tests/testcmds/FlywheelTest.java b/src/main/java/frc/robot/command/tests/testcmds/FlywheelTest.java index 729c3a1c..fc9387c4 100644 --- a/src/main/java/frc/robot/command/tests/testcmds/FlywheelTest.java +++ b/src/main/java/frc/robot/command/tests/testcmds/FlywheelTest.java @@ -23,6 +23,7 @@ public FlywheelTest(Flywheel flywheel, double topMotorSpeed, double bottomMotorS @Override public void initialize() { + System.out.println("Flywheeltest: " + topMotorSpeed); flywheel.setTopMoterRPM(topMotorSpeed); flywheel.setBottomMoterRPM(bottomMotorSpeed); } From b47fa66abb3bda406242789980085a528d08a1e0 Mon Sep 17 00:00:00 2001 From: HeroSoLos Date: Sat, 2 Mar 2024 09:49:57 -0500 Subject: [PATCH 07/13] [#312] done hehehar don! --- .../frc/robot/command/tests/FlywheelSystemTest.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/command/tests/FlywheelSystemTest.java b/src/main/java/frc/robot/command/tests/FlywheelSystemTest.java index 7133673b..1bca6746 100644 --- a/src/main/java/frc/robot/command/tests/FlywheelSystemTest.java +++ b/src/main/java/frc/robot/command/tests/FlywheelSystemTest.java @@ -23,15 +23,15 @@ public FlywheelSystemTest(Flywheel flywheel, Collector collector, Indexer indexe // new InstantCommand(() -> { System.out.println("Start top"); }), new TimedCommand(new FlywheelTest(flywheel, speed, 0.0), 2), // Motor 1 out // new InstantCommand(() -> { System.out.println("Stop top"); }), - new WaitCommand(1), - new TimedCommand(new FlywheelTest(flywheel, -speed, 0.0), 2), // Motor 1 in - new WaitCommand(1), + new WaitCommand(2), // Top out first, then bottom out, then top in, bottom in new TimedCommand(new FlywheelTest(flywheel, 0.0, speed), 2), // Motor 2 out - new WaitCommand(1), + new WaitCommand(2), + new TimedCommand(new FlywheelTest(flywheel, -speed, 0.0), 2), // Motor 1 in + new WaitCommand(2), new TimedCommand(new FlywheelTest(flywheel, 0.0, -speed), 2), // Motor 2 in - new WaitCommand(1), + new WaitCommand(2), new TimedCommand(new FlywheelTest(flywheel, speed, speed), 2), // Both out - new WaitCommand(1), + new WaitCommand(2), new TimedCommand(new FlywheelTest(flywheel, -speed, -speed), 2) // Both in ) ); From fae61c5ab6912352632aae0a1b008b3790de837c Mon Sep 17 00:00:00 2001 From: WindowsVistaisCool Date: Sat, 2 Mar 2024 11:37:04 -0500 Subject: [PATCH 08/13] [#312] Fix --- src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/command/MoveToPose.java | 2 +- src/main/java/frc/robot/subsystems/Swerve.java | 16 ++++++---------- 3 files changed, 8 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9b7f3fcf..ba4194c1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -151,7 +151,7 @@ protected void initializeNamedCommands() { NamedCommands.registerCommand("Collect", new SmartCollect(() -> .5d, () -> .6d, collector, indexer, pivot) .alongWith(leds.enableState(LED_STATES.COLLECTING).withTimeout(1))); - NamedCommands.registerCommand("Index-Up", new Index(indexer, () -> IndexerConstants.INDEXER_DEFAULT_POWER)); + NamedCommands.registerCommand("Index-Up", new Index(() -> IndexerConstants.INDEXER_DEFAULT_POWER, indexer)); NamedCommands.registerCommand("PathFind", new MoveToPose(AutonomousConstants.TARGET_POSE, drivetrain)); // make sure named commands are initialized before autobuilder! diff --git a/src/main/java/frc/robot/command/MoveToPose.java b/src/main/java/frc/robot/command/MoveToPose.java index a45d3e6c..6c5b3013 100644 --- a/src/main/java/frc/robot/command/MoveToPose.java +++ b/src/main/java/frc/robot/command/MoveToPose.java @@ -48,7 +48,7 @@ private void initLogging() { @Override public void execute() { - current = drivetrain.getPose(); + current = drivetrain.getPose().get(); dx = target.getTranslation().getX() - current.getTranslation().getX(); dy = target.getTranslation().getY() - current.getTranslation().getY(); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index ee7e65ae..62251b3c 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -189,8 +189,8 @@ private void initLogging() { // TODO Remove the unecessary shuffleboard stuff eventually LightningShuffleboard.setDoubleSupplier("Swerve", "Timer", () -> Timer.getFPGATimestamp()); LightningShuffleboard.setDoubleSupplier("Swerve", "Robot Heading", () -> getPigeon2().getAngle()); - LightningShuffleboard.setDoubleSupplier("Swerve", "Odo X", () -> getPose().getX()); - LightningShuffleboard.setDoubleSupplier("Swerve", "Odo Y", () -> getPose().getY()); + LightningShuffleboard.setDoubleSupplier("Swerve", "Odo X", () -> getPose().get().getX()); + LightningShuffleboard.setDoubleSupplier("Swerve", "Odo Y", () -> getPose().get().getY()); LightningShuffleboard.setBoolSupplier("Swerve", "Slow mode", () -> slowMode); LightningShuffleboard.setBoolSupplier("Swerve", "Robot Centric", () -> isRobotCentricControl()); @@ -227,12 +227,8 @@ AutonomousConstants.DRIVE_BASE_RADIUS, new ReplanningConfig(), }, this); // Subsystem for requirements } - public Pose2d getPose() { - var pose = getState().Pose; - if (pose == null) { - pose = new Pose2d(); - } - return pose; + public Supplier getPose() { + return () -> getState().Pose; } public ChassisSpeeds getCurrentRobotChassisSpeeds() { @@ -308,7 +304,7 @@ public void swap(XboxControllerFilter driverC, XboxControllerFilter copilotC) { * @return boolean if the robot is in the wing to start aiming STATE priming */ public boolean inWing() { - return (getPose().getX() < ShooterConstants.FAR_WING_X); + return (getPose().get().getX() < ShooterConstants.FAR_WING_X); } public void disableVision() { @@ -322,6 +318,6 @@ public void enableVision() { } public double distanceToSpeaker() { - return DrivetrainConstants.SPEAKER_POSE.getDistance(getPose().getTranslation()); + return DrivetrainConstants.SPEAKER_POSE.getDistance(getPose().get().getTranslation()); } } From 1c936f5bb64ce3da1bb1e12bc9616e3c7b9d27d4 Mon Sep 17 00:00:00 2001 From: WindowsVistaisCool Date: Sat, 2 Mar 2024 11:46:39 -0500 Subject: [PATCH 09/13] [#312] More cleanup --- .../robot/command/tests/ClimbSystemTest.java | 6 +- .../command/tests/CollectorSystemTest.java | 6 +- .../command/tests/FlywheelSystemTest.java | 16 +++-- .../command/tests/IndexerSystemTest.java | 8 +-- .../robot/command/tests/PivotAngleTest.java | 63 +++++++++---------- .../robot/command/tests/TurnSystemTest.java | 21 +++---- .../command/tests/testcmds/ClimbTest.java | 6 +- .../command/tests/testcmds/CollectorTest.java | 2 +- .../command/tests/testcmds/FlywheelTest.java | 1 - .../command/tests/testcmds/TurnTest.java | 6 +- .../java/frc/robot/subsystems/Collector.java | 1 - 11 files changed, 63 insertions(+), 73 deletions(-) diff --git a/src/main/java/frc/robot/command/tests/ClimbSystemTest.java b/src/main/java/frc/robot/command/tests/ClimbSystemTest.java index a27fbe40..6c670ce5 100644 --- a/src/main/java/frc/robot/command/tests/ClimbSystemTest.java +++ b/src/main/java/frc/robot/command/tests/ClimbSystemTest.java @@ -17,10 +17,10 @@ public ClimbSystemTest(Climber climber, double speed) { super( new SequentialCommandGroup( new WaitCommand(0.5), - new TimedCommand(new ClimbTest(climber, speed), 1), // UP + new TimedCommand(new ClimbTest(climber, () -> speed), 1), // UP new WaitCommand(1), - new TimedCommand(new ClimbTest(climber, -speed), 1) // DOWN - ) + new TimedCommand(new ClimbTest(climber, () -> -speed), 1) // DOWN + ) ); } } diff --git a/src/main/java/frc/robot/command/tests/CollectorSystemTest.java b/src/main/java/frc/robot/command/tests/CollectorSystemTest.java index 50f31999..e0398ef5 100644 --- a/src/main/java/frc/robot/command/tests/CollectorSystemTest.java +++ b/src/main/java/frc/robot/command/tests/CollectorSystemTest.java @@ -13,13 +13,13 @@ public class CollectorSystemTest extends SystemTestCommandGroup { - public CollectorSystemTest(Collector collector, Double power) { + public CollectorSystemTest(Collector collector, double power) { super( new SequentialCommandGroup( new WaitCommand(0.5), - new TimedCommand(new CollectorTest(collector,() -> power), 4), // Collector out + new TimedCommand(new CollectorTest(collector, () -> power), 2), // Collector out new WaitCommand(1), - new TimedCommand(new CollectorTest(collector,() -> -power), 4) // Collector in + new TimedCommand(new CollectorTest(collector, () -> -power), 2) // Collector in ) ); diff --git a/src/main/java/frc/robot/command/tests/FlywheelSystemTest.java b/src/main/java/frc/robot/command/tests/FlywheelSystemTest.java index 1bca6746..54f01eb8 100644 --- a/src/main/java/frc/robot/command/tests/FlywheelSystemTest.java +++ b/src/main/java/frc/robot/command/tests/FlywheelSystemTest.java @@ -3,7 +3,6 @@ // 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. -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.command.tests.testcmds.FlywheelTest; @@ -17,23 +16,22 @@ public class FlywheelSystemTest extends SystemTestCommandGroup { public FlywheelSystemTest(Flywheel flywheel, Collector collector, Indexer indexer, Pivot pivot, double speed) { + // Top out first, then bottom out, then top in, bottom in super( new SequentialCommandGroup( new WaitCommand(0.5), - // new InstantCommand(() -> { System.out.println("Start top"); }), - new TimedCommand(new FlywheelTest(flywheel, speed, 0.0), 2), // Motor 1 out - // new InstantCommand(() -> { System.out.println("Stop top"); }), - new WaitCommand(2), // Top out first, then bottom out, then top in, bottom in - new TimedCommand(new FlywheelTest(flywheel, 0.0, speed), 2), // Motor 2 out + new TimedCommand(new FlywheelTest(flywheel, speed, 0d), 2), // Motor 1 out new WaitCommand(2), - new TimedCommand(new FlywheelTest(flywheel, -speed, 0.0), 2), // Motor 1 in + new TimedCommand(new FlywheelTest(flywheel, 0d, speed), 2), // Motor 2 out new WaitCommand(2), - new TimedCommand(new FlywheelTest(flywheel, 0.0, -speed), 2), // Motor 2 in + new TimedCommand(new FlywheelTest(flywheel, -speed, 0d), 2), // Motor 1 in + new WaitCommand(2), + new TimedCommand(new FlywheelTest(flywheel, 0d, -speed), 2), // Motor 2 in new WaitCommand(2), new TimedCommand(new FlywheelTest(flywheel, speed, speed), 2), // Both out new WaitCommand(2), new TimedCommand(new FlywheelTest(flywheel, -speed, -speed), 2) // Both in - ) + ) ); addRequirements(flywheel); } diff --git a/src/main/java/frc/robot/command/tests/IndexerSystemTest.java b/src/main/java/frc/robot/command/tests/IndexerSystemTest.java index 1ee7c0c8..126c0585 100644 --- a/src/main/java/frc/robot/command/tests/IndexerSystemTest.java +++ b/src/main/java/frc/robot/command/tests/IndexerSystemTest.java @@ -16,10 +16,10 @@ public class IndexerSystemTest extends SystemTestCommandGroup { public IndexerSystemTest(Indexer indexer, double speed) { super( new SequentialCommandGroup( - new WaitCommand(0.50), - new TimedCommand(new IndexerTest(indexer, speed), 3), // Indexer out - new WaitCommand(1.00), - new TimedCommand(new IndexerTest(indexer, -speed), 3) // Indexer in + new WaitCommand(0.5), + new TimedCommand(new IndexerTest(indexer, speed), 2), // Indexer out + new WaitCommand(1d), + new TimedCommand(new IndexerTest(indexer, -speed), 1) // Indexer in ) ); addRequirements(indexer); diff --git a/src/main/java/frc/robot/command/tests/PivotAngleTest.java b/src/main/java/frc/robot/command/tests/PivotAngleTest.java index 6f41286a..8ede9163 100644 --- a/src/main/java/frc/robot/command/tests/PivotAngleTest.java +++ b/src/main/java/frc/robot/command/tests/PivotAngleTest.java @@ -4,43 +4,42 @@ package frc.robot.command.tests; +import frc.robot.Constants.PivotConstants; import frc.robot.subsystems.Pivot; import frc.thunder.testing.SystemTestCommand; public class PivotAngleTest extends SystemTestCommand { - private Pivot pivot; - private double angle; + private Pivot pivot; + private double angle; -/** - * please note that this removes all usage of the drivetrain until it is ended. - * use with caution, and only when the drivetrain is not in use. - * @param pivot used to get access to pivot - * @param angle angle to set pivot to + /** + * Sets pivot to a certain angle + * @param pivot subsystem + * @param angle angle to set pivot to (degrees) */ - - public PivotAngleTest(Pivot pivot, double angle) { - this.pivot = pivot; - this.angle = angle; - - addRequirements(pivot); - } - - @Override - public void initializeTest() { - pivot.setTargetAngle(angle); // Gives pivot an angle to move to - } - - @Override - public void executeTest() {} - - @Override - public void endTest(boolean interrupted) { - pivot.setPower(0d); // Stops giving pivot power - } - - @Override - public boolean isFinished() { - return false; - } + public PivotAngleTest(Pivot pivot, double angle) { + this.pivot = pivot; + this.angle = angle; + + addRequirements(pivot); + } + + @Override + public void initializeTest() { + pivot.setTargetAngle(angle); + } + + @Override + public void executeTest() {} + + @Override + public void endTest(boolean interrupted) { + pivot.setTargetAngle(PivotConstants.STOW_ANGLE); + } + + @Override + public boolean isFinished() { + return false; + } } diff --git a/src/main/java/frc/robot/command/tests/TurnSystemTest.java b/src/main/java/frc/robot/command/tests/TurnSystemTest.java index 9cb71759..e7767e9e 100644 --- a/src/main/java/frc/robot/command/tests/TurnSystemTest.java +++ b/src/main/java/frc/robot/command/tests/TurnSystemTest.java @@ -16,17 +16,14 @@ public class TurnSystemTest extends SystemTestCommandGroup { public TurnSystemTest(Swerve drivetrain, double speed) { super( - new SequentialCommandGroup( - new WaitCommand(0.5), - new TimedCommand(new TurnTest(drivetrain, () -> speed), 6), // Rotates at speed 10% max speed - // for 3 second - new WaitCommand(1), - new TimedCommand(new TurnTest(drivetrain, () -> -speed), 6), // Rotates in - // the opposite dir for 3 second - new WaitCommand(0.5), - new InstantCommand(() -> drivetrain.brake()) // Brakes the robot, no need imo cuz it should - // break after the TurnTest command - - )); + new SequentialCommandGroup( + new WaitCommand(0.5), + new TimedCommand(new TurnTest(drivetrain, () -> speed), 2), + new WaitCommand(1), + new TimedCommand(new TurnTest(drivetrain, () -> -speed), 2), + new WaitCommand(0.5), + new InstantCommand(() -> drivetrain.brake()) + ) + ); } } diff --git a/src/main/java/frc/robot/command/tests/testcmds/ClimbTest.java b/src/main/java/frc/robot/command/tests/testcmds/ClimbTest.java index 8f7c3af4..7708e295 100644 --- a/src/main/java/frc/robot/command/tests/testcmds/ClimbTest.java +++ b/src/main/java/frc/robot/command/tests/testcmds/ClimbTest.java @@ -12,14 +12,14 @@ public class ClimbTest extends Command { private Climber climber; - private double power; + private DoubleSupplier power; /** * System test command for testing climb motors. * @param climber climber subsystem * @param power power to control up or down */ - public ClimbTest(Climber climber, double power) { + public ClimbTest(Climber climber, DoubleSupplier power) { this.climber = climber; this.power = power; @@ -31,7 +31,7 @@ public void initialize() {} @Override public void execute() { - climber.setPower(power); + climber.setPower(power.getAsDouble()); } @Override diff --git a/src/main/java/frc/robot/command/tests/testcmds/CollectorTest.java b/src/main/java/frc/robot/command/tests/testcmds/CollectorTest.java index 7b8e722c..30b1a845 100644 --- a/src/main/java/frc/robot/command/tests/testcmds/CollectorTest.java +++ b/src/main/java/frc/robot/command/tests/testcmds/CollectorTest.java @@ -12,7 +12,7 @@ public class CollectorTest extends Command { private Collector collector; - public DoubleSupplier power; + private DoubleSupplier power; public CollectorTest(Collector collector, DoubleSupplier power) { this.collector = collector; diff --git a/src/main/java/frc/robot/command/tests/testcmds/FlywheelTest.java b/src/main/java/frc/robot/command/tests/testcmds/FlywheelTest.java index fc9387c4..729c3a1c 100644 --- a/src/main/java/frc/robot/command/tests/testcmds/FlywheelTest.java +++ b/src/main/java/frc/robot/command/tests/testcmds/FlywheelTest.java @@ -23,7 +23,6 @@ public FlywheelTest(Flywheel flywheel, double topMotorSpeed, double bottomMotorS @Override public void initialize() { - System.out.println("Flywheeltest: " + topMotorSpeed); flywheel.setTopMoterRPM(topMotorSpeed); flywheel.setBottomMoterRPM(bottomMotorSpeed); } diff --git a/src/main/java/frc/robot/command/tests/testcmds/TurnTest.java b/src/main/java/frc/robot/command/tests/testcmds/TurnTest.java index 0c0d8834..eca0941e 100644 --- a/src/main/java/frc/robot/command/tests/testcmds/TurnTest.java +++ b/src/main/java/frc/robot/command/tests/testcmds/TurnTest.java @@ -8,7 +8,7 @@ public class TurnTest extends Command { private Swerve drivetrain; - public DoubleSupplier speed; + private DoubleSupplier speed; /** * System test command for testing azimuth motors @@ -27,14 +27,12 @@ public void initialize() {} @Override public void execute() { - drivetrain.setRobot(0d, 0d, speed.getAsDouble());//speed); - System.out.println("ExecutingTurntest"); + drivetrain.setRobot(0d, 0d, speed.getAsDouble()); } @Override public void end(boolean interrupted) { drivetrain.brake(); - System.out.println("Ending"); } @Override diff --git a/src/main/java/frc/robot/subsystems/Collector.java b/src/main/java/frc/robot/subsystems/Collector.java index 576a3fc2..5957fb69 100644 --- a/src/main/java/frc/robot/subsystems/Collector.java +++ b/src/main/java/frc/robot/subsystems/Collector.java @@ -84,7 +84,6 @@ public boolean hasPiece() { * Stops the collector */ public void stop() { - System.out.println("Collector.stop"); setPower(0d); } } From 8e2bd061a7e62b03e856f4a6d9174c64ef5c7d43 Mon Sep 17 00:00:00 2001 From: WindowsVistaisCool Date: Sat, 2 Mar 2024 11:49:51 -0500 Subject: [PATCH 10/13] [#312] Adjust timings --- src/main/java/frc/robot/command/tests/IndexerSystemTest.java | 2 +- src/main/java/frc/robot/command/tests/TurnSystemTest.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/command/tests/IndexerSystemTest.java b/src/main/java/frc/robot/command/tests/IndexerSystemTest.java index 126c0585..a2cafc9a 100644 --- a/src/main/java/frc/robot/command/tests/IndexerSystemTest.java +++ b/src/main/java/frc/robot/command/tests/IndexerSystemTest.java @@ -19,7 +19,7 @@ public IndexerSystemTest(Indexer indexer, double speed) { new WaitCommand(0.5), new TimedCommand(new IndexerTest(indexer, speed), 2), // Indexer out new WaitCommand(1d), - new TimedCommand(new IndexerTest(indexer, -speed), 1) // Indexer in + new TimedCommand(new IndexerTest(indexer, -speed), 2) // Indexer in ) ); addRequirements(indexer); diff --git a/src/main/java/frc/robot/command/tests/TurnSystemTest.java b/src/main/java/frc/robot/command/tests/TurnSystemTest.java index e7767e9e..8d1c653d 100644 --- a/src/main/java/frc/robot/command/tests/TurnSystemTest.java +++ b/src/main/java/frc/robot/command/tests/TurnSystemTest.java @@ -18,9 +18,9 @@ public TurnSystemTest(Swerve drivetrain, double speed) { super( new SequentialCommandGroup( new WaitCommand(0.5), - new TimedCommand(new TurnTest(drivetrain, () -> speed), 2), + new TimedCommand(new TurnTest(drivetrain, () -> speed), 1.5), new WaitCommand(1), - new TimedCommand(new TurnTest(drivetrain, () -> -speed), 2), + new TimedCommand(new TurnTest(drivetrain, () -> -speed), 1.5), new WaitCommand(0.5), new InstantCommand(() -> drivetrain.brake()) ) From 4ba50efe8ca1d2e73d5e1e3db1604e30efff85ba Mon Sep 17 00:00:00 2001 From: WindowsVistaisCool Date: Sat, 2 Mar 2024 11:51:13 -0500 Subject: [PATCH 11/13] [#312] Fix swerve --- src/main/java/frc/robot/subsystems/Swerve.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 62251b3c..a9182591 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -189,8 +189,8 @@ private void initLogging() { // TODO Remove the unecessary shuffleboard stuff eventually LightningShuffleboard.setDoubleSupplier("Swerve", "Timer", () -> Timer.getFPGATimestamp()); LightningShuffleboard.setDoubleSupplier("Swerve", "Robot Heading", () -> getPigeon2().getAngle()); - LightningShuffleboard.setDoubleSupplier("Swerve", "Odo X", () -> getPose().get().getX()); - LightningShuffleboard.setDoubleSupplier("Swerve", "Odo Y", () -> getPose().get().getY()); + LightningShuffleboard.setDoubleSupplier("Swerve", "Odo X", () -> getState().Pose.getX()); + LightningShuffleboard.setDoubleSupplier("Swerve", "Odo Y", () -> getState().Pose.getY()); LightningShuffleboard.setBoolSupplier("Swerve", "Slow mode", () -> slowMode); LightningShuffleboard.setBoolSupplier("Swerve", "Robot Centric", () -> isRobotCentricControl()); From c1f80552a35c6c9a905388d09bfd9eaf4ebf1a70 Mon Sep 17 00:00:00 2001 From: WindowsVistaisCool Date: Sat, 2 Mar 2024 11:56:57 -0500 Subject: [PATCH 12/13] [#312] final cleanup --- src/main/java/frc/robot/Constants.java | 4 ++++ src/main/java/frc/robot/RobotContainer.java | 20 ++++++++++--------- .../command/tests/FlywheelSystemTest.java | 18 ++++++++--------- .../command/tests/IndexerSystemTest.java | 2 +- 4 files changed, 25 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3f7f8013..2277f76c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -442,6 +442,8 @@ public enum PieceState { public static final double INDEXER_DEFAULT_POWER = 0.6d; public static final double INDEXER_MANUAL_POWER = 0.75d; public static final double INDEXER_DEBOUNCE_TIME = 0.1d; + + public static final double INDEXER_SYSTEST_POWER = 0.25; } public class PivotConstants { // TODO: get real @@ -476,6 +478,8 @@ public class PivotConstants { // TODO: get real public static final double MIN_ANGLE = 25d; public static final double MAX_ANGLE = 105d; + + public static final double PIVOT_SYSTEST_ANGLE = 90d; } public class ShooterConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ba4194c1..ccc37ef6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -285,26 +285,28 @@ protected void configureFaultMonitors() { @Override protected void configureSystemTests() { - SystemTest.registerTest("Drive Test", - new DrivetrainSystemTest(drivetrain, DrivetrainConstants.SYS_TEST_SPEED_DRIVE)); // to be tested - SystemTest.registerTest("Azimuth Test", - new TurnSystemTest(drivetrain, DrivetrainConstants.SYS_TEST_SPEED_TURN)); + SystemTest.registerTest("Drive Test", new DrivetrainSystemTest(drivetrain, + DrivetrainConstants.SYS_TEST_SPEED_DRIVE)); // to be tested + SystemTest.registerTest("Azimuth Test", new TurnSystemTest(drivetrain, + DrivetrainConstants.SYS_TEST_SPEED_TURN)); - //SystemTest.registerTest("Single Note Cycle", - // new CycleSytemTest(collector, indexer, pivot, flywheel, () -> 0.5d, () -> 0.6d, () -> 250)); + // SystemTest.registerTest("Single Note Cycle", + // new CycleSytemTest(collector, indexer, pivot, flywheel, () -> 0.5d, () -> 0.6d, () -> 250)); SystemTest.registerTest("Collector Test", new CollectorSystemTest(collector, Constants.CollectorConstants.COLLECTOR_SYSTEST_POWER)); - // SystemTest.registerTest("Pivot 90 Degrees", new PivotAngleTest(pivot, 90d)); + // SystemTest.registerTest("Pivot 90 Degrees", new PivotAngleTest(pivot, + // Constants.PivotConstants.PIVOT_SYSTEST_ANGLE)); SystemTest.registerTest("Flywheel Test", new FlywheelSystemTest(flywheel, collector, indexer, pivot, Constants.FlywheelConstants.FLYWHEEL_SYSTEST_RPM)); - SystemTest.registerTest("Indexer Test", new IndexerSystemTest(indexer, 0.1)); + SystemTest.registerTest("Indexer Test", new IndexerSystemTest(indexer, + Constants.IndexerConstants.INDEXER_SYSTEST_POWER)); // SystemTest.registerTest("Climb Test", new ClimbSystemTest(climber, - // Constants.ClimbConstants.CLIMB_SYSTEST_POWER)); + // Constants.ClimbConstants.CLIMB_SYSTEST_POWER)); // Sing chooser SendableChooser songChooser = new // SendableChooser<>(); diff --git a/src/main/java/frc/robot/command/tests/FlywheelSystemTest.java b/src/main/java/frc/robot/command/tests/FlywheelSystemTest.java index 54f01eb8..c6638b4c 100644 --- a/src/main/java/frc/robot/command/tests/FlywheelSystemTest.java +++ b/src/main/java/frc/robot/command/tests/FlywheelSystemTest.java @@ -20,16 +20,16 @@ public FlywheelSystemTest(Flywheel flywheel, Collector collector, Indexer indexe super( new SequentialCommandGroup( new WaitCommand(0.5), - new TimedCommand(new FlywheelTest(flywheel, speed, 0d), 2), // Motor 1 out - new WaitCommand(2), - new TimedCommand(new FlywheelTest(flywheel, 0d, speed), 2), // Motor 2 out - new WaitCommand(2), - new TimedCommand(new FlywheelTest(flywheel, -speed, 0d), 2), // Motor 1 in - new WaitCommand(2), - new TimedCommand(new FlywheelTest(flywheel, 0d, -speed), 2), // Motor 2 in - new WaitCommand(2), + new TimedCommand(new FlywheelTest(flywheel, speed, 0d), 2), // Top out + new WaitCommand(1), + new TimedCommand(new FlywheelTest(flywheel, 0d, speed), 2), // Bottom out + new WaitCommand(1), + new TimedCommand(new FlywheelTest(flywheel, -speed, 0d), 2), // Top in + new WaitCommand(1), + new TimedCommand(new FlywheelTest(flywheel, 0d, -speed), 2), // Bottom in + new WaitCommand(1), new TimedCommand(new FlywheelTest(flywheel, speed, speed), 2), // Both out - new WaitCommand(2), + new WaitCommand(1), new TimedCommand(new FlywheelTest(flywheel, -speed, -speed), 2) // Both in ) ); diff --git a/src/main/java/frc/robot/command/tests/IndexerSystemTest.java b/src/main/java/frc/robot/command/tests/IndexerSystemTest.java index a2cafc9a..205ba4e6 100644 --- a/src/main/java/frc/robot/command/tests/IndexerSystemTest.java +++ b/src/main/java/frc/robot/command/tests/IndexerSystemTest.java @@ -18,7 +18,7 @@ public IndexerSystemTest(Indexer indexer, double speed) { new SequentialCommandGroup( new WaitCommand(0.5), new TimedCommand(new IndexerTest(indexer, speed), 2), // Indexer out - new WaitCommand(1d), + new WaitCommand(1), new TimedCommand(new IndexerTest(indexer, -speed), 2) // Indexer in ) ); From fdb68adde107c6eb40691b2cd340a8fead26f686 Mon Sep 17 00:00:00 2001 From: WindowsVistaisCool Date: Sat, 2 Mar 2024 12:02:53 -0500 Subject: [PATCH 13/13] [#312] linter things --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 8 ++++---- src/main/java/frc/robot/command/PointAtPoint.java | 10 +++------- src/main/java/frc/robot/command/PointAtTag.java | 6 +++--- .../frc/robot/command/tests/CycleSytemTest.java | 2 +- src/main/java/frc/robot/subsystems/Collector.java | 15 ++++++--------- src/main/java/frc/robot/subsystems/Indexer.java | 6 +++--- 7 files changed, 21 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2277f76c..74bb713b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -443,7 +443,7 @@ public enum PieceState { public static final double INDEXER_MANUAL_POWER = 0.75d; public static final double INDEXER_DEBOUNCE_TIME = 0.1d; - public static final double INDEXER_SYSTEST_POWER = 0.25; + public static final double INDEXER_SYSTEST_POWER = 0.25d; } public class PivotConstants { // TODO: get real diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ccc37ef6..f8a30f62 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -135,9 +135,9 @@ protected void initializeNamedCommands() { NamedCommands.registerCommand("led-Shoot", leds.enableState(LED_STATES.SHOOTING).withTimeout(0.5)); - NamedCommands.registerCommand("Cand-Sub", + NamedCommands.registerCommand("Cand-Sub", new PointBlankShotAuton(flywheel, pivot, indexer) - .alongWith(leds.enableState(LED_STATES.SHOOTING).withTimeout(0.5))); + .alongWith(leds.enableState(LED_STATES.SHOOTING).withTimeout(0.5))); NamedCommands.registerCommand("Cand-C1", new CandC1(flywheel, pivot, indexer)); NamedCommands.registerCommand("Cand-C2", new CandC2(flywheel, pivot, indexer)); NamedCommands.registerCommand("Cand-C3", new CandC3(flywheel, pivot, indexer)); @@ -290,8 +290,8 @@ protected void configureSystemTests() { SystemTest.registerTest("Azimuth Test", new TurnSystemTest(drivetrain, DrivetrainConstants.SYS_TEST_SPEED_TURN)); - // SystemTest.registerTest("Single Note Cycle", - // new CycleSytemTest(collector, indexer, pivot, flywheel, () -> 0.5d, () -> 0.6d, () -> 250)); + // SystemTest.registerTest("Single Note Cycle", new CycleSytemTest(collector, + // indexer, pivot, flywheel, () -> 0.5d, () -> 0.6d, () -> 250)); SystemTest.registerTest("Collector Test", new CollectorSystemTest(collector, Constants.CollectorConstants.COLLECTOR_SYSTEST_POWER)); diff --git a/src/main/java/frc/robot/command/PointAtPoint.java b/src/main/java/frc/robot/command/PointAtPoint.java index 8b98ea18..04432459 100644 --- a/src/main/java/frc/robot/command/PointAtPoint.java +++ b/src/main/java/frc/robot/command/PointAtPoint.java @@ -13,7 +13,7 @@ public class PointAtPoint extends Command { private Swerve drivetrain; private XboxController driver; - + private double pidOutput; private double targetHeading; private Translation2d targetPose; @@ -24,7 +24,7 @@ public class PointAtPoint extends Command { * Creates a new PointAtTag. * @param targetX the x coordinate of the target * @param targetY the y coordinate of the target - * @param drivetrain to request movement + * @param drivetrain to request movement * @param driver the driver's controller, used for drive input */ public PointAtPoint(double targetX, double targetY, Swerve drivetrain, XboxController driver) { @@ -34,14 +34,12 @@ public PointAtPoint(double targetX, double targetY, Swerve drivetrain, XboxContr addRequirements(drivetrain); } - - // Called when the command is initially scheduled. + @Override public void initialize() { headingController.enableContinuousInput(-180, 180); } - // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { Pose2d pose = drivetrain.getPose().get(); @@ -62,11 +60,9 @@ public void execute() { drivetrain.setField(-driver.getLeftY(), -driver.getLeftX(), pidOutput); } - // 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; diff --git a/src/main/java/frc/robot/command/PointAtTag.java b/src/main/java/frc/robot/command/PointAtTag.java index d776fab3..4d52f7b6 100644 --- a/src/main/java/frc/robot/command/PointAtTag.java +++ b/src/main/java/frc/robot/command/PointAtTag.java @@ -17,7 +17,7 @@ public class PointAtTag extends Command { private Swerve drivetrain; private Limelight limelight; private XboxController driver; - + private int limelightPrevPipeline = 0; private double pidOutput; private double targetHeading; @@ -27,7 +27,7 @@ public class PointAtTag extends Command { /** * Creates a new PointAtTag. - * @param drivetrain to request movement + * @param drivetrain to request movement * @param limelights to get the limelight from * @param driver the driver's controller, used for drive input */ @@ -65,7 +65,7 @@ public void execute() { targetHeading = limelight.getTargetX(); pidOutput = headingController.calculate(0, targetHeading); - + drivetrain.setFieldDriver( driver.getLeftY(), driver.getLeftX(), diff --git a/src/main/java/frc/robot/command/tests/CycleSytemTest.java b/src/main/java/frc/robot/command/tests/CycleSytemTest.java index 2ca7f857..09611d46 100644 --- a/src/main/java/frc/robot/command/tests/CycleSytemTest.java +++ b/src/main/java/frc/robot/command/tests/CycleSytemTest.java @@ -20,7 +20,7 @@ import frc.thunder.testing.SystemTestCommandGroup; public class CycleSytemTest extends SystemTestCommandGroup { - + public CycleSytemTest(Collector collector, Indexer indexer, Pivot pivot, Flywheel flywheel, DoubleSupplier collectorPower, DoubleSupplier indexerPower, DoubleSupplier flywheelRPM) { super( new SequentialCommandGroup( diff --git a/src/main/java/frc/robot/subsystems/Collector.java b/src/main/java/frc/robot/subsystems/Collector.java index 5957fb69..5bd227b4 100644 --- a/src/main/java/frc/robot/subsystems/Collector.java +++ b/src/main/java/frc/robot/subsystems/Collector.java @@ -15,10 +15,11 @@ public class Collector extends SubsystemBase { // Declare collector hardware private ThunderBird motor; - private DigitalInput beamBreak; - private final VelocityVoltage velocityVoltage = new VelocityVoltage(0, 0, true, CollectorConstants.MOTOR_KV, 0, - false, false, false); + + private final VelocityVoltage velocityVoltage = new VelocityVoltage( + 0, 0, true, CollectorConstants.MOTOR_KV, + 0, false, false, false); private boolean hasPiece; @@ -30,12 +31,11 @@ public Collector() { CollectorConstants.COLLECTOR_MOTOR_BRAKE_MODE); motor.configPIDF(0, CollectorConstants.MOTOR_KP, CollectorConstants.MOTOR_KI, - CollectorConstants.MOTOR_KD, CollectorConstants.MOTOR_KS, CollectorConstants.MOTOR_KV); + CollectorConstants.MOTOR_KD, CollectorConstants.MOTOR_KS, CollectorConstants.MOTOR_KV); - beamBreak = new DigitalInput(DIO.COLLECTOR_BEAMBREAK); motor.applyConfig(); - + initLogging(); } @@ -47,7 +47,6 @@ private void initLogging() { /** * Entrance of Collector Beam Break - * * @return When an object is present, returns true, otherwise returns false */ public boolean getEntryBeamBreakState() { @@ -56,7 +55,6 @@ public boolean getEntryBeamBreakState() { /** * Sets the power of both collector motors - * * @param power Double value from -1.0 to 1.0 (positive collects inwards) */ public void setPower(double power) { @@ -73,7 +71,6 @@ public void periodic() { /** * Has piece - * * @return boolean, true if collector has piece */ public boolean hasPiece() { diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java index 464efeb5..1f095b9a 100644 --- a/src/main/java/frc/robot/subsystems/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer.java @@ -1,7 +1,5 @@ package frc.robot.subsystems; -import com.ctre.phoenix6.Utils; - import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -19,6 +17,7 @@ public class Indexer extends SubsystemBase { private ThunderBird indexerMotor; private DigitalInput indexerSensorEntry = new DigitalInput(DIO.INDEXER_ENTER_BEAMBREAK); private DigitalInput indexerSensorExit = new DigitalInput(DIO.INDEXER_EXIT_BEAMBREAK); + private int exitIndexerIteration = 0; private double timeLastTriggered = 0d; @@ -142,7 +141,8 @@ public void periodic() { } else { setPieceState(PieceState.NONE); - } + } + // reset exitIndexerIteration if (!getEntryBeamBreakState()){ exitIndexerIteration = 0;