PS4 SW Mech binds + fully tested chain mech and kitbot shooter code (#12

)

* 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

* removed EXTENDING state

For testing chain mech, run the week_4 testing branch because that has extension capabilities.

* fixed timer for auto shooter + runs high motor before low when shooting

* tested

* Added config for PS4 controller for easier control rebinding

* uncommented subsystems

* removed extending state

* added mech PS4 controller binds

* fixed compiler error with autoIntake

* corrected chassis CAN IDs

* removed dupilcate FSM calls in robot.java

---------

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>

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

@@ -6,6 +6,11 @@
  */
 public final class HardwareMap {
 	// ID numbers for devices on the CAN bus
+	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;
 
 	// NEW Chassis
 
@@ -30,10 +35,4 @@ public final class HardwareMap {
 	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 CAN_ID_SPARK_SHOOTER_UPPER = 31;
-	public static final int CAN_ID_SPARK_SHOOTER_LOWER = 30;
-
-	public static final int CAN_ID_SPARK_LEFT_CLIMBER_MOTOR = 5;
-	public static final int CAN_ID_SPARK_RIGHT_CLIMBER_MOTOR = 32;
 }

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

@@ -20,10 +20,11 @@
 public class Robot extends TimedRobot {
 	private TeleopInput input;
 	// Systems
-	private DriveFSMSystem driveFSMSystem;
 	private KitBotShooterFSM shooterFSM;
 	private ClimberMechFSMLeft climberMechLeftFSM;
 	private ClimberMechFSMRight climberMechRightFSM;
+	private DriveFSMSystem driveFSMSystem;
+
 	private AutoHandlerSystem autoHandler;
 	private AutoPathChooser autoPathChooser;
 

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

@@ -1,7 +1,6 @@
 package frc.robot;
 
 // WPILib Imports
-import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.PS4Controller;
 
 /**
@@ -13,15 +12,12 @@
  */
 public class TeleopInput {
 	/* ======================== Constants ======================== */
-	private static final int LEFT_JOYSTICK_PORT = 1;
-	private static final int RETRACT_CLIMBER_BUTTON = 3;
-	private static final int INTAKE_BUTTON = 2;
-	private static final int OUTTAKE_BUTTON = 1;
-	public static final int DRIVER_CONTROLLER_PORT = 0;
+	private static final int MECH_CONTROLLER_PORT = 1;
+	private static final int DRIVER_CONTROLLER_PORT = 0;
 
 	/* ======================== Private variables ======================== */
 	// Input objects
-	private Joystick leftJoystick;
+	private PS4Controller mechController;
 	private PS4Controller driverController;
 
 	/* ======================== Constructor ======================== */
@@ -31,14 +27,10 @@ public class TeleopInput {
 	 * by WPILib until teleop mode.
 	 */
 	public TeleopInput() {
-		leftJoystick = new Joystick(LEFT_JOYSTICK_PORT);
+		mechController = new PS4Controller(MECH_CONTROLLER_PORT);
 		driverController = new PS4Controller(DRIVER_CONTROLLER_PORT);
-	}
 
-	/* ======================== Public methods ======================== */
-	// Getter methods for fetch input values should be defined here.
-	// Method names should be descriptive of the behavior, so the
-	// control mapping is hidden from other classes.
+	}
 
 	/* ------------------------ Driver Controller ------------------------ */
 	/**
@@ -119,43 +111,29 @@ public double getRightTrigger() {
 		return driverController.getR2Axis();
 	}
 
-	/* ------------------------ Left Joystick ------------------------ */
-	/**
-	 * Get X axis of Left Joystick.
-	 * @return Axis value
-	 */
-	public double getLeftJoystickX() {
-		return leftJoystick.getX();
-	}
-	/**
-	 * Get Y axis of Left Joystick.
-	 * @return Axis value
-	 */
-	public double getLeftJoystickY() {
-		return leftJoystick.getY();
-	}
+	/* ------------------------ Mech Controller ------------------------ */
 	/**
 	 * Get the value of the intake button.
 	 * @return True if button is pressed
 	 */
 	public boolean isIntakeButtonPressed() {
-		return leftJoystick.getRawButton(INTAKE_BUTTON);
+		return mechController.getCircleButton();
 	}
+
 	/**
 	 * Get the value of the outtake button.
 	 * @return True if button is pressed
 	 */
 	public boolean isOuttakeButtonPressed() {
-		return leftJoystick.getRawButton(OUTTAKE_BUTTON);
+		return mechController.getTriangleButton();
 	}
+
 	/**
 	 * Get the value of the retract button.
 	 * @return True if button is pressed
 	 */
 	public boolean isRetractClimberButtonPressed() {
-		return leftJoystick.getRawButton(RETRACT_CLIMBER_BUTTON);
+		return mechController.getCrossButton();
 	}
 
-	/* ======================== Private methods ======================== */
-
 }

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

@@ -13,7 +13,6 @@ public enum AutoFSMState {
 		PICK_UP_3,
 		PICK_UP_4,
 		SHOOTER_STATE,
-		INTAKE_STATE,
 		PENDING
 	}
 
@@ -133,15 +132,13 @@ public void update() {
 			case SHOOTER_STATE:
 				isCurrentStateFinished = shooterFSM.updateAutonomous(AutoFSMState.SHOOTER_STATE);
 				break;
-			case INTAKE_STATE:
-				isCurrentStateFinished = shooterFSM.updateAutonomous(AutoFSMState.INTAKE_STATE);
-				break;
 			case PENDING:
 				isCurrentStateFinished = driveSystem.updateAutonomous(AutoFSMState.PENDING);
 				break;
 			default:
 				throw new IllegalStateException("Invalid state: " + getCurrentState().toString());
 		}
+
 		if (isCurrentStateFinished) {
 			currentStateIndex++;
 			driveSystem.setCurrentPointInPath(0);

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

@@ -23,7 +23,6 @@ public enum ClimberMechFSMState {
 	private static final float MOTOR_RUN_POWER = -0.1f;
 	private boolean limitPressed = false;
 
-
 	/* ======================== Private variables ======================== */
 	private ClimberMechFSMState currentState;
 
@@ -69,6 +68,7 @@ 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);
@@ -99,8 +99,8 @@ public void update(TeleopInput input) {
 			default:
 				throw new IllegalStateException("Invalid state: " + currentState.toString());
 		}
-		SmartDashboard.putString("Current State", currentState.toString());
-		SmartDashboard.putBoolean("Bottom Limit Switch Pressed", peakLimitSwitchHit());
+		SmartDashboard.putString("Current State Left", currentState.toString());
+		SmartDashboard.putBoolean("Bottom Limit Left Switch Pressed", peakLimitSwitchHit());
 		SmartDashboard.putBoolean("Retract Button Pressed", input.isRetractClimberButtonPressed());
 		currentState = nextState(input);
 	}

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

@@ -68,7 +68,7 @@ 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);
 	}
@@ -99,8 +99,8 @@ public void update(TeleopInput input) {
 			default:
 				throw new IllegalStateException("Invalid state: " + currentState.toString());
 		}
-		SmartDashboard.putString("Current State", currentState.toString());
-		SmartDashboard.putBoolean("Bottom Limit Switch Pressed", peakLimitSwitchHit());
+		SmartDashboard.putString("Current State Right", currentState.toString());
+		SmartDashboard.putBoolean("Bottom Limit Right Switch Pressed", peakLimitSwitchHit());
 		SmartDashboard.putBoolean("Retract Button Pressed", input.isRetractClimberButtonPressed());
 		currentState = nextState(input);
 	}

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

@@ -23,10 +23,11 @@ public enum ShooterFSMState {
 		OUTTAKING
 	}
 
-	private static final float L_MOTOR_RUN_POWER = 0.05f;
-	private static final float U_MOTOR_RUN_POWER = 0.1f;
-	private static final float AUTO_OUTTAKING_TIME = 2.0f;
-	private static final float AUTO_INTAKING_TIME = 2.0f;
+	private static final float L_MOTOR_RUN_POWER = 0.8f;
+	private static final float 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;
@@ -38,10 +39,8 @@ public enum ShooterFSMState {
 	private SparkLimitSwitch bottomLimitSwitch;
 
 	private boolean autoOuttakingTimerStarted;
-	private double autoOuttakingTimerStart;
-	private boolean autoIntakingTimerStarted;
-	private double autoIntakingTimerStart;
-	private Timer timer;
+	private double outtakingTimerStart;
+	private Timer timer = new Timer();
 
 	/* ======================== Constructor ======================== */
 	/**
@@ -63,7 +62,6 @@ public KitBotShooterFSM() {
 		highMotor.setIdleMode(CANSparkMax.IdleMode.kBrake);
 
 		autoOuttakingTimerStarted = false;
-		autoIntakingTimerStarted = false;
 
 		timer = new Timer();
 
@@ -89,7 +87,9 @@ public ShooterFSMState getCurrentState() {
 	 */
 	public void reset() {
 		currentState = ShooterFSMState.IDLE_STOP;
-
+		timer = new Timer();
+		timer.start();
+		autoOuttakingTimerStarted = false;
 		// Call one tick of update to ensure outputs reflect start state
 		update(null);
 	}
@@ -123,6 +123,8 @@ public void update(TeleopInput input) {
 		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("Get applied output", highMotor.getAppliedOutput());
 		currentState = nextState(input);
 	}
 
@@ -135,8 +137,6 @@ public boolean updateAutonomous(AutoFSMState autoState) {
 		switch (autoState) {
 			case SHOOTER_STATE:
 				return handleAutoOuttakingState();
-			case INTAKE_STATE:
-				return handleAutoIntakingState();
 			default:
 				return true;
 		}
@@ -159,6 +159,7 @@ private ShooterFSMState nextState(TeleopInput input) {
 		switch (currentState) {
 			case IDLE_STOP:
 				if (input.isOuttakeButtonPressed() && !input.isIntakeButtonPressed()) {
+					outtakingTimerStart = timer.get();
 					return ShooterFSMState.OUTTAKING;
 				}
 				if (input.isIntakeButtonPressed() && !hasNote()
@@ -201,8 +202,8 @@ private void handleIdleState(TeleopInput input) {
 	 *        the robot is in autonomous mode.
 	 */
 	private void handleIntakingState(TeleopInput input) {
-		lowMotor.set(-L_MOTOR_RUN_POWER);
-		highMotor.set(-U_MOTOR_RUN_POWER);
+		lowMotor.set(INTAKING_SPEED);
+		highMotor.set(-INTAKING_SPEED);
 	}
 
 	/**
@@ -211,48 +212,38 @@ private void handleIntakingState(TeleopInput input) {
 	 *        the robot is in autonomous mode.
 	 */
 	private void handleOuttakingState(TeleopInput input) {
-		lowMotor.set(L_MOTOR_RUN_POWER);
-		highMotor.set(U_MOTOR_RUN_POWER);
+		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);
+			}
+		} else {
+			highMotor.set(0);
+			lowMotor.set(0);
+		}
 	}
 
 	private boolean handleAutoOuttakingState() {
 		if (!autoOuttakingTimerStarted) {
 			autoOuttakingTimerStarted = true;
-			autoOuttakingTimerStart = timer.get();
+			outtakingTimerStart = timer.get();
 		}
-
 		if (autoOuttakingTimerStarted
-			&& !timer.hasElapsed(autoOuttakingTimerStart + AUTO_OUTTAKING_TIME)) {
-			lowMotor.set(L_MOTOR_RUN_POWER);
+			&& !timer.hasElapsed(outtakingTimerStart + OUTTAKING_TIME)) {
 			highMotor.set(U_MOTOR_RUN_POWER);
-		} else {
-			lowMotor.set(0);
-			highMotor.set(0);
+			if (timer.hasElapsed(outtakingTimerStart + (OUTTAKING_TIME / 2))) {
+				lowMotor.set(L_MOTOR_RUN_POWER);
+			} else {
+				lowMotor.set(0);
+			}
 
-			autoOuttakingTimerStarted = false;
-
-			return true;
-		}
-
-		return false;
-	}
-
-	private boolean handleAutoIntakingState() {
-		if (!autoIntakingTimerStarted) {
-			autoIntakingTimerStarted = true;
-			autoIntakingTimerStart = timer.get();
-		}
-
-		if (autoIntakingTimerStarted
-			&& !timer.hasElapsed(autoIntakingTimerStart + AUTO_INTAKING_TIME)) {
-			lowMotor.set(-L_MOTOR_RUN_POWER);
-			highMotor.set(-U_MOTOR_RUN_POWER);
 		} else {
 			lowMotor.set(0);
 			highMotor.set(0);
 
-			autoIntakingTimerStarted = false;
-
+			autoOuttakingTimerStarted = false;
 			return true;
 		}