Integrated mech testing on robot + changed binds for shooter mech

* Added code for nextState from FSM

* adjustments to shooter mech

* Fix Checkstyles

Fixed All checkstyle issues with the code.
Please make sure you have the plugin
enabled when writing the code and please follow the checkstyles

* fixed checkstyles again

* Fixed Shooter Code for Testing

* finalized shooter code

* addressing svr sw mech pr adjustments

* chain mech complete

* restoring chain mech files

* added auto functions

* fixed merge conflicts

* fixed checkstyle

* resolving pr comments

* made climber mech changes

* working climber code

* checkstyle

* Added config for PS4 controller for easier control rebinding

* removed extending state

* tested some auto, fixed unused state, ran teleop with shooter and drive

* uncommented shooterFSM.reset()

* added amp button

* added amp button

* tested amp outtake button

* id changes + cmech test

* switched right and left climber CAN IDs

* switched HW Map CAN IDs for climber to 19,20

* removed amp shooting + added shooter rev state and button

* smoother state transitions for shooter

* checkstyle

---------

Co-authored-by: Araav <nayakaraav@gmail.com>
Co-authored-by: Bhargav Eranki <77950550+beranki@users.noreply.github.com>
Co-authored-by: Aaron <143990364+Adbritto@users.noreply.github.com>
Co-authored-by: Daiji <duchino292@student.fuhsd.org>
Co-authored-by: Prashant Kondayapalepu <prashant777k@gmail.com>
Co-authored-by: Mira <mpanigrahy607@student.fuhsd.org>

src/main/java/frc/robot/HardwareMap.java

@@ -9,30 +9,30 @@ public final class HardwareMap {
 	public static final int CAN_ID_SPARK_SHOOTER_UPPER = 33;
 	public static final int CAN_ID_SPARK_SHOOTER_LOWER = 34;
 
-	public static final int CAN_ID_SPARK_LEFT_CLIMBER_MOTOR = 6;
-	public static final int CAN_ID_SPARK_RIGHT_CLIMBER_MOTOR = 32;
+	public static final int CAN_ID_SPARK_LEFT_CLIMBER_MOTOR = 19;
+	public static final int CAN_ID_SPARK_RIGHT_CLIMBER_MOTOR = 20;
 
 	// NEW Chassis
 
-	// public static final int FRONT_LEFT_DRIVING_CAN_ID = 6;
-	// public static final int FRONT_RIGHT_DRIVING_CAN_ID = 4;
-	// public static final int REAR_LEFT_DRIVING_CAN_ID = 8;
-	// public static final int REAR_RIGHT_DRIVING_CAN_ID = 2;
+	public static final int FRONT_LEFT_DRIVING_CAN_ID = 6;
+	public static final int FRONT_RIGHT_DRIVING_CAN_ID = 4;
+	public static final int REAR_LEFT_DRIVING_CAN_ID = 8;
+	public static final int REAR_RIGHT_DRIVING_CAN_ID = 2;
 
-	// public static final int FRONT_LEFT_TURNING_CAN_ID = 5;
-	// public static final int FRONT_RIGHT_TURNING_CAN_ID = 3;
-	// public static final int REAR_LEFT_TURNING_CAN_ID = 7;
-	// public static final int REAR_RIGHT_TURNING_CAN_ID = 1;
+	public static final int FRONT_LEFT_TURNING_CAN_ID = 5;
+	public static final int FRONT_RIGHT_TURNING_CAN_ID = 3;
+	public static final int REAR_LEFT_TURNING_CAN_ID = 7;
+	public static final int REAR_RIGHT_TURNING_CAN_ID = 1;
 
 	// OLD Chassis
 
-	public static final int FRONT_LEFT_DRIVING_CAN_ID = 8;
-	public static final int FRONT_RIGHT_DRIVING_CAN_ID = 6;
-	public static final int REAR_LEFT_DRIVING_CAN_ID = 2;
-	public static final int REAR_RIGHT_DRIVING_CAN_ID = 4;
+	// public static final int FRONT_LEFT_DRIVING_CAN_ID = 8;
+	// public static final int FRONT_RIGHT_DRIVING_CAN_ID = 6;
+	// public static final int REAR_LEFT_DRIVING_CAN_ID = 2;
+	// public static final int REAR_RIGHT_DRIVING_CAN_ID = 4;
 
-	public static final int FRONT_LEFT_TURNING_CAN_ID = 7;
-	public static final int FRONT_RIGHT_TURNING_CAN_ID = 5;
-	public static final int REAR_LEFT_TURNING_CAN_ID = 1;
-	public static final int REAR_RIGHT_TURNING_CAN_ID = 3;
+	// public static final int FRONT_LEFT_TURNING_CAN_ID = 7;
+	// public static final int FRONT_RIGHT_TURNING_CAN_ID = 5;
+	// public static final int REAR_LEFT_TURNING_CAN_ID = 1;
+	// public static final int REAR_RIGHT_TURNING_CAN_ID = 3;
 }

src/main/java/frc/robot/Robot.java

@@ -100,4 +100,3 @@ public void simulationPeriodic() { }
 	@Override
 	public void robotPeriodic() { }
 }
-

src/main/java/frc/robot/TeleopInput.java

@@ -121,11 +121,19 @@ public boolean isIntakeButtonPressed() {
 	}
 
 	/**
-	 * Get the value of the outtake button.
+	 * Get the value of the shoot button.
 	 * @return True if button is pressed
 	 */
-	public boolean isOuttakeButtonPressed() {
-		return mechController.getTriangleButton();
+	public boolean isShootButtonPressed() {
+		return mechController.getR2Button();
+	}
+
+	/**
+	 * Get the value of the rev button for the shooter.
+	 * @return True if button is pressed
+	 */
+	public boolean isRevOuttakeButtonPressed() {
+		return mechController.getL2Button();
 	}
 
 	/**

src/main/java/frc/robot/systems/AutoHandlerSystem.java

@@ -5,7 +5,6 @@ public class AutoHandlerSystem {
 	/* ======================== Constants ======================== */
 	// Auto FSM state definitions
 	public enum AutoFSMState {
-		TURN_TO_SCORE,
 		LEAVE,
 		DRIVE_TO_SCORE,
 		PICK_UP_1,
@@ -36,15 +35,15 @@ public enum AutoPath {
 	//Predefined auto paths
 
 	private static final AutoFSMState[] PATH1 = new AutoFSMState[]{
-		AutoFSMState.TURN_TO_SCORE, AutoFSMState.LEAVE};
+		AutoFSMState.DRIVE_TO_SCORE, AutoFSMState.SHOOTER_STATE, AutoFSMState.LEAVE};
 
 	private static final AutoFSMState[] PATH2 = new AutoFSMState[]{
-		AutoFSMState.DRIVE_TO_SCORE, AutoFSMState.PICK_UP_1,
-		AutoFSMState.DRIVE_TO_SCORE, AutoFSMState.PICK_UP_2,
-		AutoFSMState.DRIVE_TO_SCORE, AutoFSMState.PICK_UP_3,
-		AutoFSMState.DRIVE_TO_SCORE, AutoFSMState.PICK_UP_4};
+		AutoFSMState.DRIVE_TO_SCORE, AutoFSMState.SHOOTER_STATE, AutoFSMState.PICK_UP_1,
+		AutoFSMState.DRIVE_TO_SCORE, AutoFSMState.SHOOTER_STATE, AutoFSMState.PICK_UP_2,
+		AutoFSMState.DRIVE_TO_SCORE, AutoFSMState.SHOOTER_STATE, AutoFSMState.PICK_UP_3,
+		AutoFSMState.DRIVE_TO_SCORE, AutoFSMState.SHOOTER_STATE, AutoFSMState.PICK_UP_4};
 
-	private static final AutoFSMState[] PATH3 = new AutoFSMState[]{};
+	private static final AutoFSMState[] PATH3 = new AutoFSMState[]{AutoFSMState.SHOOTER_STATE};
 
 	/* ======================== Constructor ======================== */
 	/**
@@ -78,7 +77,7 @@ public AutoFSMState getCurrentState() {
 	 */
 	public void reset(AutoPath path) {
 		driveSystem.resetAutonomus();
-		//shooterFSM.reset();
+		shooterFSM.reset();
 
 		if (path == AutoPath.PATH1) {
 			currentStateList = PATH1;
@@ -100,11 +99,6 @@ public void update() {
 		boolean isCurrentStateFinished;
 		SmartDashboard.putString("In Auto State: ", "" + getCurrentState());
 		switch (getCurrentState()) {
-
-			case TURN_TO_SCORE:
-				isCurrentStateFinished = driveSystem.updateAutonomous(
-					AutoFSMState.TURN_TO_SCORE);
-				break;
 			case LEAVE:
 				isCurrentStateFinished = driveSystem.updateAutonomous(
 					AutoFSMState.LEAVE);

src/main/java/frc/robot/systems/ClimberMechFSMLeft.java

@@ -20,7 +20,7 @@ public enum ClimberMechFSMState {
 		RETRACTING
 	}
 
-	private static final float MOTOR_RUN_POWER = -0.1f;
+	private static final float MOTOR_RUN_POWER = -0.4f;
 	private boolean limitPressed = false;
 
 	/* ======================== Private variables ======================== */
@@ -69,7 +69,6 @@ public ClimberMechFSMState getCurrentState() {
 	public void reset() {
 		currentState = ClimberMechFSMState.IDLE_STOP;
 		limitPressed = false;
-
 		// Call one tick of update to ensure outputs reflect start state
 		update(null);
 	}
@@ -103,6 +102,8 @@ public void update(TeleopInput input) {
 		SmartDashboard.putBoolean("Bottom Limit Left Switch Pressed", peakLimitSwitchHit());
 		SmartDashboard.putBoolean("Retract Button Pressed", input.isRetractClimberButtonPressed());
 		currentState = nextState(input);
+		SmartDashboard.putNumber("left output", motor.getAppliedOutput());
+		SmartDashboard.putNumber("left motor applied", motor.get());
 	}
 
 	/**

src/main/java/frc/robot/systems/ClimberMechFSMRight.java

@@ -20,7 +20,7 @@ public enum ClimberMechFSMState {
 		RETRACTING
 	}
 
-	private static final float MOTOR_RUN_POWER = 0.1f;
+	private static final float MOTOR_RUN_POWER = 0.4f;
 	private boolean limitPressed = false;
 
 	/* ======================== Private variables ======================== */
@@ -103,6 +103,9 @@ public void update(TeleopInput input) {
 		SmartDashboard.putBoolean("Bottom Limit Right Switch Pressed", peakLimitSwitchHit());
 		SmartDashboard.putBoolean("Retract Button Pressed", input.isRetractClimberButtonPressed());
 		currentState = nextState(input);
+		SmartDashboard.putNumber("right output", motor.getAppliedOutput());
+		SmartDashboard.putNumber("right motor applied", motor.get());
+
 	}
 
 	/**

src/main/java/frc/robot/systems/KitBotShooterFSM.java

@@ -20,15 +20,14 @@ public class KitBotShooterFSM {
 	public enum ShooterFSMState {
 		IDLE_STOP,
 		INTAKING,
-		OUTTAKING
+		OUTTAKING_SPEAKER,
 	}
 
-	private static final float L_MOTOR_RUN_POWER = 0.8f;
-	private static final float U_MOTOR_RUN_POWER = -1.0f;
+	private static final float SPEAKER_L_MOTOR_RUN_POWER = 0.8f;
+	private static final float SPEAKER_U_MOTOR_RUN_POWER = -1.0f;
 	private static final float INTAKING_SPEED = -0.4f;
 	private static final float OUTTAKING_TIME = 2.0f;
 
-
 	/* ======================== Private variables ======================== */
 	private ShooterFSMState currentState;
 
@@ -113,17 +112,16 @@ public void update(TeleopInput input) {
 			case INTAKING:
 				handleIntakingState(input);
 				break;
-			case OUTTAKING:
-				handleOuttakingState(input);
+			case OUTTAKING_SPEAKER:
+				handleShootSpeakerState(input);
 				break;
 			default:
 				throw new IllegalStateException("Invalid state: " + currentState.toString());
 		}
 		SmartDashboard.putString("Current State", currentState.toString());
 		SmartDashboard.putBoolean("Bottom Limit Switch Pressed", bottomLimitSwitch.isPressed());
-		SmartDashboard.putBoolean("Outtake Button Pressed", input.isOuttakeButtonPressed());
 		SmartDashboard.putBoolean("Intake Button Pressed", input.isIntakeButtonPressed());
-		SmartDashboard.putNumber("Motor SPeed", highMotor.get());
+		SmartDashboard.putNumber("Motor Speed", highMotor.get());
 		SmartDashboard.putNumber("Get applied output", highMotor.getAppliedOutput());
 		currentState = nextState(input);
 	}
@@ -158,26 +156,28 @@ private ShooterFSMState nextState(TeleopInput input) {
 		}
 		switch (currentState) {
 			case IDLE_STOP:
-				if (input.isOuttakeButtonPressed() && !input.isIntakeButtonPressed()) {
-					outtakingTimerStart = timer.get();
-					return ShooterFSMState.OUTTAKING;
+				if ((input.isShootButtonPressed() || input.isRevOuttakeButtonPressed())
+					&& !input.isIntakeButtonPressed()) {
+					return ShooterFSMState.OUTTAKING_SPEAKER;
 				}
 				if (input.isIntakeButtonPressed() && !hasNote()
-					&& !input.isOuttakeButtonPressed()) {
+					&& !input.isShootButtonPressed()
+					&& !input.isRevOuttakeButtonPressed()) {
 					return ShooterFSMState.INTAKING;
-				} else {
-					return ShooterFSMState.IDLE_STOP;
 				}
+				return ShooterFSMState.IDLE_STOP;
 			case INTAKING:
 				if (input.isIntakeButtonPressed() && !hasNote()
-					&& !input.isOuttakeButtonPressed()) {
+					&& !input.isShootButtonPressed()
+					&& !input.isRevOuttakeButtonPressed()) {
 					return ShooterFSMState.INTAKING;
 				} else {
 					return ShooterFSMState.IDLE_STOP;
 				}
-			case OUTTAKING:
-				if (input.isOuttakeButtonPressed() && !input.isIntakeButtonPressed()) {
-					return ShooterFSMState.OUTTAKING;
+			case OUTTAKING_SPEAKER:
+				if ((input.isShootButtonPressed() || input.isRevOuttakeButtonPressed())
+					&& !input.isIntakeButtonPressed()) {
+					return ShooterFSMState.OUTTAKING_SPEAKER;
 				} else {
 					return ShooterFSMState.IDLE_STOP;
 				}
@@ -207,20 +207,19 @@ private void handleIntakingState(TeleopInput input) {
 	}
 
 	/**
-	 * Handle behavior in OUTTAKING state.
+	 * Handle behavior in OUTTAKING_SPEAKER state.
 	 * @param input Global TeleopInput if robot in teleop mode or null if
 	 *        the robot is in autonomous mode.
 	 */
-	private void handleOuttakingState(TeleopInput input) {
-		if (!timer.hasElapsed(outtakingTimerStart + OUTTAKING_TIME)) {
-			highMotor.set(U_MOTOR_RUN_POWER);
-			if (timer.hasElapsed(outtakingTimerStart + (OUTTAKING_TIME / 2))) {
-				lowMotor.set(L_MOTOR_RUN_POWER);
-			} else {
-				lowMotor.set(0);
-			}
+	private void handleShootSpeakerState(TeleopInput input) {
+		if (input.isRevOuttakeButtonPressed()) {
+			highMotor.set(SPEAKER_U_MOTOR_RUN_POWER);
 		} else {
 			highMotor.set(0);
+		}
+		if (input.isShootButtonPressed()) {
+			lowMotor.set(SPEAKER_L_MOTOR_RUN_POWER);
+		} else {
 			lowMotor.set(0);
 		}
 	}
@@ -232,9 +231,9 @@ private boolean handleAutoOuttakingState() {
 		}
 		if (autoOuttakingTimerStarted
 			&& !timer.hasElapsed(outtakingTimerStart + OUTTAKING_TIME)) {
-			highMotor.set(U_MOTOR_RUN_POWER);
+			highMotor.set(SPEAKER_U_MOTOR_RUN_POWER);
 			if (timer.hasElapsed(outtakingTimerStart + (OUTTAKING_TIME / 2))) {
-				lowMotor.set(L_MOTOR_RUN_POWER);
+				lowMotor.set(SPEAKER_L_MOTOR_RUN_POWER);
 			} else {
 				lowMotor.set(0);
 			}