Add bonk intake

src/main/deploy/pathplanner/BumpLeaveCom.path

@@ -24,12 +24,12 @@
     },
     {
       "anchorPoint": {
-        "x": 5.952165408604322,
-        "y": 0.9021142828610453
+        "x": 6.88172647773867,
+        "y": 0.9018288036576078
       },
       "prevControl": {
-        "x": 4.862761111168394,
-        "y": 0.8685941506322471
+        "x": 5.792322180302743,
+        "y": 0.8683086714288095
       },
       "nextControl": null,
       "holonomicAngle": 0,

src/main/deploy/pathplanner/FlatLeaveCom.path

@@ -24,12 +24,12 @@
     },
     {
       "anchorPoint": {
-        "x": 5.3093828752978185,
-        "y": 4.760247050185312
+        "x": 6.151674589063464,
+        "y": 4.781124133676839
       },
       "prevControl": {
-        "x": 3.9384317248444485,
-        "y": 4.769767544285683
+        "x": 4.780723438610093,
+        "y": 4.79064462777721
       },
       "nextControl": null,
       "holonomicAngle": 0.0,

src/main/java/frc/team2412/robot/Controls.java

@@ -1,6 +1,8 @@
 package frc.team2412.robot;
 
-import static frc.team2412.robot.Controls.ControlConstants.*;
+import static frc.team2412.robot.Controls.ControlConstants.CODRIVER_CONTROLLER_PORT;
+import static frc.team2412.robot.Controls.ControlConstants.CONTROLLER_PORT;
+import static frc.team2412.robot.Controls.ControlConstants.FAST_WRIST_EXTEND_TOLERANCE;
 import static frc.team2412.robot.commands.arm.SetWristCommand.WristPosition.WRIST_PRESCORE;
 import static frc.team2412.robot.commands.arm.SetWristCommand.WristPosition.WRIST_RETRACT;
 import static frc.team2412.robot.commands.arm.SetWristCommand.WristPosition.WRIST_SCORE;
@@ -24,8 +26,6 @@
 import frc.team2412.robot.commands.intake.IntakeSetOutCommand;
 import frc.team2412.robot.commands.led.LEDPurpleCommand;
 import frc.team2412.robot.commands.led.LEDYellowCommand;
-import frc.team2412.robot.subsystems.IntakeSubsystem.IntakeConstants.GamePieceType;
-import frc.team2412.robot.util.DriverAssist;
 
 public class Controls {
 	public static class ControlConstants {
@@ -66,6 +66,15 @@ public static class ControlConstants {
 	public final Trigger driveIntakeOutButton;
 	public final Trigger driveIntakeFastOutButton;
 
+	public final Trigger bonkIntakeWristUpTrigger;
+	public final Trigger bonkIntakeWristDownTrigger;
+	public final Trigger bonkIntakeOutButton;
+	public final Trigger bonkIntakeFastOutButton;
+	public final Trigger bonkMoveIntakeInButton;
+	public final Trigger bonkMoveIntakeHybridButton;
+	public final Trigger bonkMoveIntakeMidButton;
+	public final Trigger bonkMoveIntakeStowButton;
+
 	public final Trigger ledPurple;
 	public final Trigger ledYellow;
 
@@ -94,8 +103,17 @@ public Controls(Subsystems s) {
 		codriveIntakeInButton = codriveController.rightTrigger();
 		codriveIntakeOutButton = codriveController.leftTrigger();
 		driveIntakeInButton = driveController.a();
-		driveIntakeOutButton = driveController.y();
-		driveIntakeFastOutButton = driveController.b();
+		driveIntakeOutButton = driveController.b();
+		driveIntakeFastOutButton = driveController.y();
+
+		bonkIntakeWristUpTrigger = driveController.rightTrigger(0.1);
+		bonkIntakeWristDownTrigger = driveController.leftTrigger(0.1);
+		bonkIntakeOutButton = driveController.leftBumper();
+		bonkIntakeFastOutButton = driveController.rightBumper();
+		bonkMoveIntakeInButton = driveController.x();
+		bonkMoveIntakeHybridButton = driveController.y();
+		bonkMoveIntakeMidButton = driveController.a();
+		bonkMoveIntakeStowButton = driveController.b();
 
 		ledPurple = codriveController.rightBumper();
 		ledYellow = codriveController.leftBumper();
@@ -106,6 +124,9 @@ public Controls(Subsystems s) {
 		if (Subsystems.SubsystemConstants.INTAKE_ENABLED) {
 			bindIntakeControls();
 		}
+		if (Subsystems.SubsystemConstants.BONK_INTAKE_ENABLED) {
+			bindBonkIntakeControls();
+		}
 		if (Subsystems.SubsystemConstants.LED_ENABLED) {
 			bindLEDControls();
 		}
@@ -128,10 +149,10 @@ public void bindDrivebaseControls() {
 		driveController.back().onTrue(new InstantCommand(s.drivebaseSubsystem::resetPose));
 		driveController.leftStick().onTrue(new InstantCommand(s.drivebaseSubsystem::toggleXWheels));
 
-		triggerDriverAssistCube.whileTrue(
-				DriverAssist.alignRobotCommand(s.drivebaseSubsystem, GamePieceType.CUBE).repeatedly());
-		triggerDriverAssistCone.whileTrue(
-				DriverAssist.alignRobotCommand(s.drivebaseSubsystem, GamePieceType.CONE).repeatedly());
+		// triggerDriverAssistCube.whileTrue(
+		// 		DriverAssist.alignRobotCommand(s.drivebaseSubsystem, GamePieceType.CUBE).repeatedly());
+		// triggerDriverAssistCone.whileTrue(
+		// 		DriverAssist.alignRobotCommand(s.drivebaseSubsystem, GamePieceType.CONE).repeatedly());
 		// // CommandBase driverAssistCube =
 		// // 		new ProxyCommand(() -> DriverAssist.alignRobot(s.drivebaseSubsystem,
 		// GamePieceType.CUBE));
@@ -195,6 +216,37 @@ public void bindIntakeControls() {
 		// codriveIntakeStopButton.onTrue(new IntakeSetStopCommand(s.intakeSubsystem));
 	}
 
+	public void bindBonkIntakeControls() {
+		// Wrist move up
+		bonkIntakeWristUpTrigger.whileTrue(
+				s.bonkIntakeSubsystem.adjustWristCommand(driveController::getRightTriggerAxis));
+		// negative bc its going down
+		bonkIntakeWristDownTrigger.whileTrue(
+				s.bonkIntakeSubsystem.adjustWristCommand(() -> -driveController.getLeftTriggerAxis()));
+
+		// Stock Jonah Code
+		// bonkIntakeOutButton.onTrue(s.bonkIntakeSubsystem.intakeOutCommand());
+		// bonkIntakeFastOutButton.onTrue(s.bonkIntakeSubsystem.intakeFastOutCommand());
+
+		// Tristan's Intake Code
+		bonkIntakeOutButton.onTrue(s.bonkIntakeSubsystem.moveToIntakeCommand());
+		bonkIntakeOutButton.onFalse(s.bonkIntakeSubsystem.moveToStowCommand());
+		bonkIntakeFastOutButton.onTrue(s.bonkIntakeSubsystem.moveToHybridCommand());
+		bonkIntakeFastOutButton.onFalse(s.bonkIntakeSubsystem.moveToStowCommand());
+
+		// Stock Jonah Code
+		bonkMoveIntakeInButton.onTrue(s.bonkIntakeSubsystem.moveToIntakeCommand());
+
+		// Tristans Mid Command
+		bonkMoveIntakeHybridButton.onTrue(s.bonkIntakeSubsystem.moveToMidCommand());
+		bonkMoveIntakeHybridButton.onFalse(s.bonkIntakeSubsystem.moveToStowCommand());
+
+		// Stock Jonah Code
+		bonkMoveIntakeMidButton.onTrue(s.bonkIntakeSubsystem.moveToHighCommand());
+		bonkMoveIntakeMidButton.onFalse(s.bonkIntakeSubsystem.moveToStowCommand());
+		bonkMoveIntakeStowButton.onTrue(s.bonkIntakeSubsystem.moveToStowCommand());
+	}
+
 	public void bindLEDControls() {
 		ledPurple.onTrue(new LEDPurpleCommand(s.ledSubsystem));
 		ledYellow.onTrue(new LEDYellowCommand(s.ledSubsystem));

src/main/java/frc/team2412/robot/Hardware.java

@@ -54,6 +54,11 @@ public class Hardware {
 	public static final int INTAKE_MOTOR_1 = 55, INTAKE_MOTOR_2 = 56;
 	public static final int INTAKE_DISTANCE_SENSOR = 0;
 
+	// Bonk intake and wrist are from range 30-39
+	public static final int BONK_INTAKE_WRIST_MOTOR = 30;
+	public static final int BONK_INTAKE_MOTOR_1 = 31;
+	public static final int BONK_INTAKE_MOTOR_2 = 32;
+
 	// LED strip is PWM port 8
 	public static final int BLINKIN_LED = 8;
 }

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

@@ -60,10 +60,10 @@ protected Robot() {
 	}
 
 	public static final MACAddress COMPETITION_ADDRESS = MACAddress.of(0x33, 0x9d, 0xd1);
-	public static final MACAddress PRACTICE_ADDRESS = MACAddress.of(0x28, 0x40, 0x82);
+	public static final MACAddress PRACTICE_ADDRESS = MACAddress.of(0x33, 0x9D, 0xE7);
 
 	private static RobotType getTypeFromAddress() {
-		if (PRACTICE_ADDRESS.exists()) return RobotType.DRIVEBASE;
+		if (PRACTICE_ADDRESS.exists() || isSimulation()) return RobotType.DRIVEBASE;
 		else return RobotType.COMPETITION;
 	}
 
@@ -227,6 +227,9 @@ public void simulationInit() {
 		if (Subsystems.SubsystemConstants.ARM_ENABLED) {
 			subsystems.armSubsystem.simInit(sim);
 		}
+		if (Subsystems.SubsystemConstants.BONK_INTAKE_ENABLED && robotType == RobotType.DRIVEBASE) {
+			subsystems.bonkIntakeSubsystem.simInit(sim);
+		}
 	}
 
 	@Override

src/main/java/frc/team2412/robot/Subsystems.java

@@ -12,6 +12,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import frc.team2412.robot.subsystems.ArmLEDSubsystem;
 import frc.team2412.robot.subsystems.ArmSubsystem;
+import frc.team2412.robot.subsystems.BonkIntakeSubsystem;
 import frc.team2412.robot.subsystems.DrivebaseSubsystem;
 import frc.team2412.robot.subsystems.IntakeSubsystem;
 import frc.team2412.robot.subsystems.LEDSubsystem;
@@ -23,6 +24,7 @@ public static class SubsystemConstants {
 		public static final boolean DRIVEBASE_ENABLED = true;
 		public static final boolean ARM_ENABLED = IS_COMP && true;
 		public static final boolean INTAKE_ENABLED = IS_COMP && true;
+		public static final boolean BONK_INTAKE_ENABLED = !IS_COMP && true;
 		public static final boolean VISION_ENABLED = true;
 		public static final boolean LED_ENABLED = IS_COMP && true;
 		public static final boolean ARM_LED_ENABLED = IS_COMP && true;
@@ -32,6 +34,7 @@ public static class SubsystemConstants {
 	public DrivebaseSubsystem drivebaseSubsystem;
 	public ArmSubsystem armSubsystem;
 	public IntakeSubsystem intakeSubsystem;
+	public BonkIntakeSubsystem bonkIntakeSubsystem;
 	public VisionSubsystem visionSubsystem;
 	public LEDSubsystem ledSubsystem;
 	public ArmLEDSubsystem armLedSubsystem;
@@ -86,6 +89,9 @@ public Subsystems() {
 		if (INTAKE_ENABLED) {
 			intakeSubsystem = new IntakeSubsystem();
 		}
+		if (BONK_INTAKE_ENABLED) {
+			bonkIntakeSubsystem = new BonkIntakeSubsystem();
+		}
 		if (LED_ENABLED) {
 			ledSubsystem = new LEDSubsystem();
 		}