diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0efc8ac0..4055c3e9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -66,7 +66,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.7d; public static final Translation2d SPEAKER_POSE = new Translation2d(0d, 5.547393); } @@ -433,7 +433,7 @@ public class FlywheelConstants { // TODO: get real public static final double BIAS_INCREMENT = 1.25; // RPS to bias by per button press 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 @@ -448,6 +448,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.25d; } public class PivotConstants { // TODO: get real @@ -482,6 +484,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 a7df8497..cefec697 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -53,8 +53,11 @@ import frc.robot.command.shoot.AutonCand.PointBlankShotAuton; 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.IndexerSystemTest; +import frc.robot.command.tests.PivotAngleTest; import frc.robot.command.tests.SingSystemTest; import frc.robot.command.tests.TurnSystemTest; import frc.robot.command.Climb; @@ -135,9 +138,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)); @@ -151,7 +154,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! @@ -229,9 +232,10 @@ protected void configureButtonBindings() { .onTrue(new InstantCommand(() -> flywheel.decreaseBias())); // LEFT new Trigger(coPilot::getRightBumper) - .whileTrue(new Index(indexer, () -> IndexerConstants.INDEXER_MANUAL_POWER)); + .whileTrue(new Index(() -> IndexerConstants.INDEXER_DEFAULT_POWER, indexer)); new Trigger(coPilot::getLeftBumper) - .whileTrue(new Index(indexer, () -> -IndexerConstants.INDEXER_MANUAL_POWER)); + .whileTrue(new Index(() -> -IndexerConstants.INDEXER_DEFAULT_POWER, indexer)); + /* Other */ new Trigger( @@ -287,22 +291,28 @@ protected void configureFaultMonitors() { @Override protected void configureSystemTests() { - 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("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("Collector Test", new CollectorSystemTest(collector, + Constants.CollectorConstants.COLLECTOR_SYSTEST_POWER)); - // SystemTest.registerTest("Collector Test", new CollectorSystemTest(collector, - // Constants.CollectorConstants.COLLECTOR_SYSTEST_POWER)); + // SystemTest.registerTest("Pivot 90 Degrees", new PivotAngleTest(pivot, + // Constants.PivotConstants.PIVOT_SYSTEST_ANGLE)); - // TODO make pivot system test + SystemTest.registerTest("Flywheel Test", new FlywheelSystemTest(flywheel, collector, + indexer, pivot, Constants.FlywheelConstants.FLYWHEEL_SYSTEST_RPM)); - // SystemTest.registerTest("Flywheel Test", new FlywheelSystemTest(flywheel, - // collector, - // indexer, pivot, Constants.FlywheelConstants.SYS_TEST_SPEED)); + 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/Collect.java b/src/main/java/frc/robot/command/Collect.java index 503c07b5..35d34565 100644 --- a/src/main/java/frc/robot/command/Collect.java +++ b/src/main/java/frc/robot/command/Collect.java @@ -10,9 +10,9 @@ public class Collect extends Command { // Declares collector + private DoubleSupplier powerSupplier; private Collector collector; private Indexer indexer; - private DoubleSupplier powerSupplier; /** * Creates a new Collect. @@ -21,9 +21,9 @@ public class Collect extends Command { * @param indexer subsystem */ public Collect(DoubleSupplier powerSupplier, Collector collector, Indexer indexer) { + this.powerSupplier = powerSupplier; this.collector = collector; this.indexer = indexer; - this.powerSupplier = powerSupplier; 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/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/ClimbSystemTest.java b/src/main/java/frc/robot/command/tests/ClimbSystemTest.java index 1ba66441..6c670ce5 100644 --- a/src/main/java/frc/robot/command/tests/ClimbSystemTest.java +++ b/src/main/java/frc/robot/command/tests/ClimbSystemTest.java @@ -17,9 +17,9 @@ 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 4e571fde..e0398ef5 100644 --- a/src/main/java/frc/robot/command/tests/CollectorSystemTest.java +++ b/src/main/java/frc/robot/command/tests/CollectorSystemTest.java @@ -13,14 +13,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), 2), // Collector out + new TimedCommand(new CollectorTest(collector, () -> power), 2), // Collector out new WaitCommand(1), - new TimedCommand(new CollectorTest(collector, -speed), 2) // Collector in + new TimedCommand(new CollectorTest(collector, () -> -power), 2) // Collector in + ) ); + addRequirements(collector); } } 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..09611d46 --- /dev/null +++ b/src/main/java/frc/robot/command/tests/CycleSytemTest.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 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), // Run smart collect for 3 + new WaitCommand(1), + 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/FlywheelSystemTest.java b/src/main/java/frc/robot/command/tests/FlywheelSystemTest.java index ebea2e14..c6638b4c 100644 --- a/src/main/java/frc/robot/command/tests/FlywheelSystemTest.java +++ b/src/main/java/frc/robot/command/tests/FlywheelSystemTest.java @@ -16,21 +16,23 @@ 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 TimedCommand(new FlywheelTest(flywheel, speed, 0), 2), // Motor 1 out + new TimedCommand(new FlywheelTest(flywheel, speed, 0d), 2), // Top out new WaitCommand(1), - new TimedCommand(new FlywheelTest(flywheel, -speed, 0), 2), // Motor 1 in + new TimedCommand(new FlywheelTest(flywheel, 0d, speed), 2), // Bottom out new WaitCommand(1), - new TimedCommand(new FlywheelTest(flywheel, 0, speed), 2), // Motor 2 out + new TimedCommand(new FlywheelTest(flywheel, -speed, 0d), 2), // Top in new WaitCommand(1), - new TimedCommand(new FlywheelTest(flywheel, 0, -speed), 2), // Motor 2 in + 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(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 32aa1dec..205ba4e6 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), 2) // Indexer in ) ); + addRequirements(indexer); } } diff --git a/src/main/java/frc/robot/command/tests/testcmds/PivotTest.java b/src/main/java/frc/robot/command/tests/PivotAngleTest.java similarity index 50% rename from src/main/java/frc/robot/command/tests/testcmds/PivotTest.java rename to src/main/java/frc/robot/command/tests/PivotAngleTest.java index d98dfc5d..8ede9163 100644 --- a/src/main/java/frc/robot/command/tests/testcmds/PivotTest.java +++ b/src/main/java/frc/robot/command/tests/PivotAngleTest.java @@ -2,17 +2,23 @@ // 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; +package frc.robot.command.tests; -import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.PivotConstants; import frc.robot.subsystems.Pivot; +import frc.thunder.testing.SystemTestCommand; -public class PivotTest extends Command { +public class PivotAngleTest extends SystemTestCommand { private Pivot pivot; private double angle; - public PivotTest(Pivot pivot, double angle) { + /** + * 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; @@ -20,15 +26,17 @@ public PivotTest(Pivot pivot, double angle) { } @Override - public void initialize() { + public void initializeTest() { pivot.setTargetAngle(angle); } @Override - public void execute() {} + public void executeTest() {} @Override - public void end(boolean interrupted) {} + public void endTest(boolean interrupted) { + pivot.setTargetAngle(PivotConstants.STOW_ANGLE); + } @Override public boolean isFinished() { 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/TurnSystemTest.java b/src/main/java/frc/robot/command/tests/TurnSystemTest.java index 83986db0..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), 1), + new TimedCommand(new TurnTest(drivetrain, () -> speed), 1.5), new WaitCommand(1), - new TimedCommand(new TurnTest(drivetrain, () -> -speed), 1), + new TimedCommand(new TurnTest(drivetrain, () -> -speed), 1.5), 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 c2830db7..7708e295 100644 --- a/src/main/java/frc/robot/command/tests/testcmds/ClimbTest.java +++ b/src/main/java/frc/robot/command/tests/testcmds/ClimbTest.java @@ -4,20 +4,22 @@ package frc.robot.command.tests.testcmds; +import java.util.function.DoubleSupplier; + import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Climber; 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; @@ -29,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 3fd6aff8..30b1a845 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; + private DoubleSupplier power; - public CollectorTest(Collector collector, double power) { + public CollectorTest(Collector collector, DoubleSupplier power) { this.collector = collector; this.power = power; @@ -21,11 +23,12 @@ public CollectorTest(Collector collector, double power) { @Override public void initialize() { - collector.setPower(power); + collector.setPower(power.getAsDouble()); } @Override - public void execute() {} + public void execute() { + } @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/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/TurnTest.java b/src/main/java/frc/robot/command/tests/testcmds/TurnTest.java index ab50d03f..eca0941e 100644 --- a/src/main/java/frc/robot/command/tests/testcmds/TurnTest.java +++ b/src/main/java/frc/robot/command/tests/testcmds/TurnTest.java @@ -27,7 +27,7 @@ public void initialize() {} @Override public void execute() { - drivetrain.applyPercentRequestRobot(() -> 0d, () -> 0d, speed); + drivetrain.setRobot(0d, 0d, speed.getAsDouble()); } @Override 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;