synchro

Author: AvidCoder27

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

@@ -69,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() {

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;
@@ -81,7 +83,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() {