Merge branch 'main' of https://github.com/team5115/2025-Bunnybots

Author: vox-eternal

src/main/java/frc/team5115/Constants.java

@@ -36,7 +36,11 @@ public static enum Mode {
     public static final byte DEALGAE_REVERSE_CHANNEL = 3;
 
     public static final byte ARM_MOTOR_ID = 11;
+    public static final double ARM_STOW_ANGLE_DEG = 75.0;
+    public static final double ARM_DEPLOY_ANGLE_DEG = 0.0;
+
     public static final byte INTAKE_MOTOR_ID = 12;
+    public static final double INTAKE_SPEED = 0.15;
 
     public static final byte BLOCK_ACTUATOR_ID = 9;
 

src/main/java/frc/team5115/RobotContainer.java

@@ -12,6 +12,10 @@
 import frc.team5115.Constants.AutoConstants.Side;
 import frc.team5115.Constants.Mode;
 import frc.team5115.commands.DriveCommands;
+import frc.team5115.subsystems.arm.Arm;
+import frc.team5115.subsystems.arm.ArmIO;
+import frc.team5115.subsystems.arm.ArmIOSim;
+import frc.team5115.subsystems.arm.ArmIOSparkMax;
 import frc.team5115.subsystems.bling.Bling;
 import frc.team5115.subsystems.bling.BlingIO;
 import frc.team5115.subsystems.bling.BlingIOReal;
@@ -22,6 +26,10 @@
 import frc.team5115.subsystems.drive.ModuleIO;
 import frc.team5115.subsystems.drive.ModuleIOSim;
 import frc.team5115.subsystems.drive.ModuleIOSparkMax;
+import frc.team5115.subsystems.intakewheel.IntakeWheel;
+import frc.team5115.subsystems.intakewheel.IntakeWheelIO;
+import frc.team5115.subsystems.intakewheel.IntakeWheelIOSim;
+import frc.team5115.subsystems.intakewheel.IntakeWheelIOSparkMax;
 import frc.team5115.subsystems.outtake.Outtake;
 import frc.team5115.subsystems.outtake.OuttakeIO;
 import frc.team5115.subsystems.outtake.OuttakeIOReal;
@@ -46,6 +54,8 @@ public class RobotContainer {
     private final PhotonVision vision;
     private final Bling bling;
     private final Outtake outtake;
+    private final Arm arm;
+    private final IntakeWheel intakeWheel;
 
     // Controllers
     private final CommandXboxController joyDrive = new CommandXboxController(0);
@@ -59,6 +69,7 @@ public class RobotContainer {
     private boolean slowMode = false;
     private boolean hasFaults = true;
     private double faultPrintTimeout = 0;
+    private boolean intakeMode = false;
 
     /** The container for the robot. Contains subsystems, OI devices, and commands. */
     public RobotContainer() {
@@ -81,6 +92,8 @@ public RobotContainer() {
                 vision = new PhotonVision(new PhotonVisionIOReal(), drivetrain);
                 bling = new Bling(new BlingIOReal());
                 outtake = new Outtake(new OuttakeIOReal(hub));
+                arm = new Arm(new ArmIOSparkMax());
+                intakeWheel = new IntakeWheel(new IntakeWheelIOSparkMax());
                 break;
             case SIM:
                 // Sim robot, instantiate physics sim IO implementations
@@ -91,6 +104,8 @@ public RobotContainer() {
                 vision = new PhotonVision(new PhotonVisionIOSim(), drivetrain);
                 bling = new Bling(new BlingIOSim());
                 outtake = new Outtake(new OuttakeIOSim());
+                arm = new Arm(new ArmIOSim());
+                intakeWheel = new IntakeWheel(new IntakeWheelIOSim());
                 break;
 
             default:
@@ -102,6 +117,8 @@ public RobotContainer() {
                 vision = new PhotonVision(new PhotonVisionIO() {}, drivetrain);
                 bling = new Bling(new BlingIO() {});
                 outtake = new Outtake(new OuttakeIO() {});
+                arm = new Arm(new ArmIO() {});
+                intakeWheel = new IntakeWheel(new IntakeWheelIO() {});
                 break;
         }
 

src/main/java/frc/team5115/commands/DriveCommands.java

@@ -8,8 +8,13 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import frc.team5115.Constants;
 import frc.team5115.Constants.SwerveConstants;
+import frc.team5115.subsystems.arm.Arm;
 import frc.team5115.subsystems.drive.Drivetrain;
+import frc.team5115.subsystems.intakewheel.IntakeWheel;
+
+
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
@@ -68,20 +73,18 @@ public static Command joystickDrive(
                 drivetrain);
     }
 
-    // public static Command cleanStart(
-    //         Height height, Elevator elevator, Dealgaefacationinator5000 dealgae) {
-    //     return Commands.sequence(
-    //             elevator.setHeightAndWait(height, 5),
-    //             Commands.print("Cleaner at Height"),
-    //             Commands.waitSeconds(0.2),
-    //             elevator.waitForSetpoint(5),
-    //             dealgae.extend(),
-    //             elevator.setHeightAndWait((height == Height.L2 ? Height.CLEAN2 : Height.CLEAN3),
-    // 5));
-    // }
+    public static Command modeSwap(boolean intakeMode, IntakeWheel intakeWheel, Arm arm) {
+        return Commands.run(
+            () -> {
+                if (intakeMode) {
+                    intakeWheel.intake().execute();
+                    arm.deploy().execute();
 
-    // public static Command cleanEnd(Elevator elevator, Dealgaefacationinator5000 dealgae) {
-    //     return Commands.sequence(
-    //             dealgae.retract(), Commands.waitSeconds(0.5), elevator.setHeight(Height.INTAKE));
-    // }
+                } else {
+                    intakeWheel.stop().execute();
+                    arm.stow().execute();
+                }
+            }, 
+            intakeWheel);
+    }
 }

src/main/java/frc/team5115/subsystems/arm/Arm.java

@@ -18,6 +18,8 @@ public class Arm extends SubsystemBase {
     private final ArmFeedforward feedforward;
     private final PIDController pid;
     private final double ks;
+    
+    private boolean frozen = false;
 
     public Arm(ArmIO io) {
         this.io = io;
@@ -82,7 +84,15 @@ public Command goToAngle(Rotation2d setpoint, double timeout) {
     }
 
     public Command stow() {
-        return setAngle(Rotation2d.fromDegrees(75.0));
+        return setAngle(Rotation2d.fromDegrees(Constants.ARM_STOW_ANGLE_DEG));
+    }
+
+    public Command deploy(){
+        return setAngle(Rotation2d.fromDegrees(Constants.ARM_DEPLOY_ANGLE_DEG));
+    }
+
+    public void freeze(){
+        frozen = true;
     }
 
     public void stop() {

src/main/java/frc/team5115/subsystems/intakewheel/IntakeWheel.java

@@ -4,12 +4,13 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.team5115.Constants;
+
 import java.util.ArrayList;
 import java.util.function.BooleanSupplier;
 import org.littletonrobotics.junction.Logger;
 
 public class IntakeWheel extends SubsystemBase {
-    private static final double INTAKE_SPEED = 0.15;
     private final IntakeWheelIO io;
     private final IntakeWheelIOInputsAutoLogged inputs = new IntakeWheelIOInputsAutoLogged();
 
@@ -28,7 +29,7 @@ public Command setSpeed(double speed) {
     }
 
     public Command intake() {
-        return setSpeed(INTAKE_SPEED);
+        return setSpeed(Constants.INTAKE_SPEED);
     }
 
     public Command vomit() {