Merge branch 'main' into 29-auto-align-to-tag-using-pathfinding

frc-862 · Mar 2, 2024 · 6120e25 · 6120e25
2 parents f39deb0 + da3041a
commit 6120e25
Show file tree

Hide file tree

Showing 22 changed files with 147 additions and 112 deletions.
diff --git 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
@@ -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<SystemTestCommand> songChooser = new
 		// SendableChooser<>();

diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -22,5 +22,6 @@ public IndexerSystemTest(Indexer indexer, double speed) {
                 new TimedCommand(new IndexerTest(indexer, -speed), 2) // Indexer in
             )
         );
+        addRequirements(indexer);
     }
 }