Sim + Elevator and Wrist Integration + some name changes

team20 · Feb 8, 2025 · f3218e8 · f3218e8
1 parent f0f8260
commit f3218e8
Show file tree

Hide file tree

Showing 8 changed files with 204 additions and 30 deletions.
diff --git a/.vscode/settings.json b/.vscode/settings.json
@@ -68,5 +68,6 @@
 	"editor.detectIndentation": false,
 	"editor.insertSpaces": false,
 	"java.format.settings.url": "eclipse-formatter.xml",
-	"java.sources.organizeImports.staticStarThreshold": 1
+	"java.sources.organizeImports.staticStarThreshold": 1,
+	"java.debug.settings.onBuildFailureProceed": true
 }
diff --git a/simgui-ds.json b/simgui-ds.json
@@ -0,0 +1,100 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "030000004c050000cc09000000000000"
+    },
+    {
+      "guid": "030000004c050000cc09000000000000"
+    }
+  ]
+}
diff --git a/src/main/java/frc/robot/CommandComposer.java b/src/main/java/frc/robot/CommandComposer.java
@@ -0,0 +1,63 @@
+package frc.robot;
+
+import static edu.wpi.first.wpilibj2.command.Commands.*;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.AlgaeGrabberSubsystem;
+import frc.robot.subsystems.CheeseStickSubsystem;
+import frc.robot.subsystems.ClimberSubsystem;
+import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.ElevatorSubsystem;
+import frc.robot.subsystems.WristSubsystem;
+
+public class CommandComposer {
+	private static DriveSubsystem m_driveSubsystem;
+	private static AlgaeGrabberSubsystem m_algaeGrabberSubsystem;
+	private static CheeseStickSubsystem m_cheeseStickSubsystem;
+	private static ClimberSubsystem m_climberSubsystem;
+	private static ElevatorSubsystem m_elevatorSubsystem;
+	private static WristSubsystem m_wristSubsystem;
+
+	private static Command scoreLevel(Supplier<Command> levelCommand) {
+		return sequence(
+				levelCommand.get(),
+				m_wristSubsystem.goToAngle(35),
+				m_elevatorSubsystem.lowerToScore(),
+				m_cheeseStickSubsystem.goLeft(), // TODO: Left is release?
+				m_cheeseStickSubsystem.goRight(),
+				levelCommand.get(),
+				m_wristSubsystem.goToAngle(-90));
+	}
+
+	public static Command scoreLevelFour() {
+		return scoreLevel(m_elevatorSubsystem::goToLevelFourHeight);
+	}
+
+	public static Command scoreLevelThree() {
+		return scoreLevel(m_elevatorSubsystem::goToLevelThreeHeight);
+	}
+
+	public static Command scoreLevelTwo() {
+		return scoreLevel(m_elevatorSubsystem::goToLevelTwoHeight);
+	}
+
+	public static Command scoreLevelOne() {
+		return scoreLevel(m_elevatorSubsystem::goToLevelOneHeight);
+	}
+
+	public static Command prepareForCoralPickup() {
+		return sequence(
+				m_elevatorSubsystem.goToCoralStationHeight(),
+				m_wristSubsystem.goToAngle(-90));
+	}
+
+	public static Command pickupAtCoralStation() {
+		return sequence(
+				m_cheeseStickSubsystem.goLeft(),
+				m_elevatorSubsystem.goToCoralStationHeight(),
+				m_cheeseStickSubsystem.goRight());
+	}
+
+}
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -119,6 +119,8 @@ public static final class ElevatorConstants {
 		public static final double kLevelTwoHeight = kLevelOneHeight; // same as level 1
 		public static final double kLevelThreeHeight = Units.inchesToMeters(60 - 24);
 		public static final double kLevelFourHeight = Units.inchesToMeters(90 - 24);
+		public static final double kToScoreHeightDecrease = 0; // The amount that the elevator decreases in order to
+																// score
 		public static final double kCoralStationHeight = 0;
 	}
 

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -4,6 +4,7 @@
 
 package frc.robot;
 
+import static frc.robot.CommandComposer.*;
 import static frc.robot.Constants.AlgaeConstants.*;
 import static frc.robot.Constants.ClimberConstants.*;
 import static frc.robot.Constants.ControllerConstants.*;
@@ -45,7 +46,6 @@ public class Robot extends TimedRobot {
 	private final ElevatorSubsystem m_elevatorSubsystem = new ElevatorSubsystem(
 			m_mechanism.getRoot("anchor", Units.inchesToMeters(23), 0));
 	private final WristSubsystem m_wristSubsystem = new WristSubsystem(m_elevatorSubsystem.getWristMount());
-	// Changed to PS5
 	private final CommandPS5Controller m_driverController = new CommandPS5Controller(kDriverControllerPort);
 	private final CommandPS5Controller m_operatorController = new CommandPS5Controller(kOperatorControllerPort);
 	private final PowerDistribution m_pdh = new PowerDistribution();
@@ -81,27 +81,28 @@ public void bindDriveControls() {
 	}
 
 	public void bindElevatorControls() {
-		m_operatorController.circle().onTrue(m_elevatorSubsystem.goToLevelFourCommand());
-		m_operatorController.triangle().onTrue(m_elevatorSubsystem.goToLevelThreeCommand());
-		m_operatorController.square().onTrue(m_elevatorSubsystem.goToLevelTwoCommand());
-		m_operatorController.cross().onTrue(m_elevatorSubsystem.goToLevelOneCommand());
-		m_operatorController.povLeft().onTrue(m_elevatorSubsystem.goToCoralStationCommand());
+		m_operatorController.circle().onTrue(scoreLevelFour());
+		m_operatorController.triangle().onTrue(scoreLevelThree());
+		m_operatorController.square().onTrue(scoreLevelTwo());
+		m_operatorController.cross().onTrue(scoreLevelOne());
+		m_operatorController.povLeft().onTrue(m_elevatorSubsystem.goToCoralStationHeight());
+		m_operatorController.povRight().onTrue(m_elevatorSubsystem.goToBaseHeight());
 		// TODO: Add manual movement
 	}
 
 	public void bindAlgaeControls() {
 		m_operatorController.R1().onTrue(
-				m_algaeGrabberSubsystem.deployGrabberCommand(GrabberState.DOWN)
-						.andThen(m_algaeGrabberSubsystem.runFlywheelCommand())); // TODO: Come up after?
+				m_algaeGrabberSubsystem.deployGrabber(GrabberState.DOWN)
+						.andThen(m_algaeGrabberSubsystem.runFlywheel())); // TODO: Come up after?
 		m_operatorController.L1().onTrue(
-				m_algaeGrabberSubsystem.runFlywheelReverseCommand()
+				m_algaeGrabberSubsystem.runFlywheelReverse()
 						.until(m_algaeGrabberSubsystem::checkCurrentOnFlywheel)
-						.andThen(m_algaeGrabberSubsystem.stopFlywheelCommand()));
+						.andThen(m_algaeGrabberSubsystem.stopFlywheel()));
 	}
 
 	public void bindWristControls() {
-		m_driverController.circle().onTrue(m_wristSubsystem.reverseMotor());
-		m_driverController.square().onTrue(m_wristSubsystem.forwardMotor());
+		// m_driverController.circle().onTrue(m_wristSubsystem.reverseMotor());
+		// m_driverController.square().onTrue(m_wristSubsystem.forwardMotor());
 	}
 
 	public void bindCheeseStickControls() {
@@ -112,7 +113,7 @@ public void bindCheeseStickControls() {
 
 	public void bindClimberControls() {
 		m_operatorController.L2().whileTrue(m_climberSubsystem.moveForward())
-				.onFalse(m_climberSubsystem.moveBackward());
+				.onFalse(m_climberSubsystem.moveBackward()); // TODO: will this run forever? (no)
 	}
 
 	@Override

diff --git a/src/main/java/frc/robot/subsystems/AlgaeGrabberSubsystem.java b/src/main/java/frc/robot/subsystems/AlgaeGrabberSubsystem.java
@@ -99,7 +99,7 @@ public boolean checkCurrentOnFlywheel() {
 	 *
 	 * @return (setVelocity(0)) sets the flywheel velocity to 0
 	 */
-	public Command stopFlywheelCommand() {
+	public Command stopFlywheel() {
 		return runOnce(() -> {
 			setVelocity(0);
 		});
@@ -110,7 +110,7 @@ public Command stopFlywheelCommand() {
 	 *
 	 * @return (setVelocity(.75)) sets the flywheel velocity to 75%
 	 */
-	public Command runFlywheelCommand() {
+	public Command runFlywheel() {
 		return runOnce(() -> {
 			setVelocity(.75);
 		});
@@ -121,7 +121,7 @@ public Command runFlywheelCommand() {
 	 *
 	 * @return (setVelocity(-.75)) sets the flywheel velocity to -75%
 	 */
-	public Command runFlywheelReverseCommand() {
+	public Command runFlywheelReverse() {
 		return runOnce(() -> {
 			setVelocity(-.75);
 		});
@@ -136,7 +136,7 @@ public Command runFlywheelReverseCommand() {
 	 * @return moves the whole grabber setup using a PID based on the grabberState
 	 *         enum given
 	 */
-	public Command deployGrabberCommand(GrabberState state) {
+	public Command deployGrabber(GrabberState state) {
 		return runOnce(() -> {
 			if (GrabberState.DOWN == state) {
 				m_grabberClosedLoopController.setReference(2, ControlType.kPosition);

diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
@@ -230,11 +230,11 @@ public Command driveCommand(DoubleSupplier forwardSpeed, DoubleSupplier strafeSp
 	 * 
 	 * @return A command to reset the gyro heading.
 	 */
-	public Command resetHeadingCommand() {
+	public Command resetHeading() {
 		return runOnce(m_gyro::zeroYaw).withName("ResetHeadingCommand");
 	}
 
-	public Command resetOdometryCommand(Pose2d pose) {
+	public Command resetOdometry(Pose2d pose) {
 		return runOnce(() -> m_odometry.resetPosition(getHeading(), getModulePositions(), pose))
 				.withName("ResetOdometryCommand");
 	}

diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
@@ -108,8 +108,7 @@ public boolean atSetpoint() {
 	/**
 	 * Sets the position of the motor (using PID)
 	 * 
-	 * @param position Position to set the master motor to (the follower
-	 *        will...follow)
+	 * @param position Position to set the master motor
 	 */
 	public void setPosition(double position) {
 		m_setPosition = position;
@@ -153,7 +152,7 @@ public void periodic() {
 	 * 
 	 * @return Sets the speed to 0
 	 */
-	public Command stopMotorCommand() {
+	public Command stopMotor() {
 		return runOnce(() -> setSpeed(0)).withName("Elevator motor stop");
 	}
 
@@ -162,7 +161,7 @@ public Command stopMotorCommand() {
 	 * 
 	 * @return the command to set the speed
 	 */
-	public Command reverseMotorCommand() {
+	public Command reverseMotor() {
 		return runOnce(() -> setSpeed(-1)).withName("Elevator motor backwards");
 	}
 
@@ -171,28 +170,36 @@ public Command reverseMotorCommand() {
 	 * 
 	 * @return the command to set the speed
 	 */
-	public Command forwardMotorCommand() {
+	public Command forwardMotor() {
 		return runOnce(() -> setSpeed(1)).withName("Elevator motor forward");
 	}
 
-	public Command goToLevelOneCommand() {
+	public Command goToLevelOneHeight() {
 		return runOnce(() -> setPosition(kLevelOneHeight)).withName("Elevator to Level 1");
 	}
 
-	public Command goToLevelTwoCommand() {
+	public Command goToLevelTwoHeight() {
 		return runOnce(() -> setPosition(kLevelTwoHeight)).withName("Elevator to Level 2");
 	}
 
-	public Command goToLevelThreeCommand() {
+	public Command goToLevelThreeHeight() {
 		return runOnce(() -> setPosition(kLevelThreeHeight)).withName("Elevator to Level 3");
 	}
 
-	public Command goToLevelFourCommand() {
+	public Command goToLevelFourHeight() {
 		return runOnce(() -> setPosition(kLevelFourHeight)).withName("Elevator to Level 4");
 	}
 
-	public Command goToCoralStationCommand() {
+	public Command goToCoralStationHeight() {
 		return runOnce(() -> setPosition(kCoralStationHeight)).withName("Elevator to Coral Station");
 	}
 
+	public Command goToBaseHeight() {
+		return runOnce(() -> setPosition(0)).withName("Go to Base Height");
+	}
+
+	public Command lowerToScore() {
+		return runOnce(() -> setPosition(getPosition() - kToScoreHeightDecrease)).withName("Lower Elevator to Score");
+	}
+
 }