feat: drive commands

Author: vox-eternal

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

@@ -44,6 +44,8 @@ public static enum Mode {
 
     public static final byte BLOCK_ACTUATOR_ID = 9;
 
+    public static final byte LUNITE_SENSOR = 0;
+
     public static final byte LED_STRIP_PWM_ID = 0;
 
     public static final double LOOP_PERIOD_SECS = 0.02;

src/main/java/frc/team5115/DriverController.java

@@ -0,0 +1,84 @@
+package frc.team5115;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.team5115.Constants.AutoConstants.Side;
+import frc.team5115.commands.DriveCommands;
+import frc.team5115.subsystems.drive.Drivetrain;
+
+public class DriverController {
+    private final CommandXboxController joyDrive;
+    private final CommandXboxController joyManip;
+
+    private final Drivetrain drivetrain;
+
+    private boolean robotRelative = false;
+    private boolean slowMode = false;
+
+    public DriverController(
+            int port,
+            Drivetrain drivetrain) {
+        joyDrive = new CommandXboxController(port);
+        joyManip = null;
+
+        this.drivetrain = drivetrain;
+        
+    }
+
+    public DriverController(
+            int drivePort,
+            int manipPort,
+            Drivetrain drivetrain) {
+        joyDrive = new CommandXboxController(drivePort);
+        joyManip = new CommandXboxController(manipPort);
+
+        this.drivetrain = drivetrain;
+
+    }
+
+    private Command offsetGyro() {
+        return Commands.runOnce(() -> drivetrain.offsetGyro(), drivetrain).ignoringDisable(true);
+    }
+
+    public boolean isConnected() {
+        return joyDrive.isConnected() && (joyManip == null || joyManip.isConnected());
+    }
+
+    private void configureSingleMode() {}
+    private void configureDualMode() {}
+        
+
+    public void configureButtonBindings() {
+        // drive control
+        drivetrain.setDefaultCommand(
+                DriveCommands.joystickDrive(
+                        drivetrain,
+                        () -> robotRelative,
+                        () -> slowMode,
+                        () -> -joyDrive.getLeftY(),
+                        () -> -joyDrive.getLeftX(),
+                        () -> -joyDrive.getRightX()));
+        if (joyManip == null) {
+            configureSingleMode();
+        } else {
+            configureDualMode();
+        }
+    }
+
+    private Command setRobotRelative(boolean state) {
+        return Commands.runOnce(() -> robotRelative = state);
+    }
+
+    private Command setSlowMode(boolean state) {
+        return Commands.runOnce(() -> slowMode = state);
+    }
+
+    public boolean getRobotRelative() {
+        return robotRelative;
+    }
+
+    public boolean getSlowMode() {
+        return slowMode;
+    }
+}

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

@@ -8,12 +8,11 @@
 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 frc.team5115.subsystems.outtake.Outtake;
 
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
@@ -74,17 +73,20 @@ public static Command joystickDrive(
     }
 
     public static Command modeSwap(boolean intakeMode, IntakeWheel intakeWheel, Arm arm) {
-        return Commands.run(
-            () -> {
-                if (intakeMode) {
-                    intakeWheel.intake().execute();
-                    arm.deploy().execute();
+        return Commands.either(
+                Commands.parallel(intakeWheel.intake(), arm.deploy()),
+                Commands.parallel(intakeWheel.stop(), arm.stow()),
+                () -> intakeMode);
+    }
+
+    public static Command intakeMode(IntakeWheel intakeWheel, Arm arm) {
+        return Commands.repeatingSequence(
+                Commands.parallel(intakeWheel.intake(), arm.deploy()),
+                (arm.waitForSensorState(true, Double.POSITIVE_INFINITY)),
+                arm.stow());
+    }
 
-                } else {
-                    intakeWheel.stop().execute();
-                    arm.stow().execute();
-                }
-            }, 
-            intakeWheel);
+    public static Command scoringMode(Outtake outtake) {
+        return 
     }
 }

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

@@ -18,8 +18,6 @@ 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;
@@ -69,7 +67,7 @@ public void periodic() {
 
         io.setArmVoltage(voltage);
     }
-    //meow meow meow, meowwww
+    // meow meow meow, meowwww
 
     public Command waitForSetpoint(double timeout) {
         return Commands.waitUntil(() -> pid.atSetpoint()).withTimeout(timeout);
@@ -87,12 +85,12 @@ public Command stow() {
         return setAngle(Rotation2d.fromDegrees(Constants.ARM_STOW_ANGLE_DEG));
     }
 
-    public Command deploy(){
+    public Command deploy() {
         return setAngle(Rotation2d.fromDegrees(Constants.ARM_DEPLOY_ANGLE_DEG));
     }
 
-    public void freeze(){
-        frozen = true;
+    public Command waitForSensorState(boolean state, double timeout) {
+        return Commands.waitUntil(() -> inputs.luniteDetected == state).withTimeout(timeout);
     }
 
     public void stop() {

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

@@ -12,6 +12,8 @@ public static class ArmIOInputs {
         public double armVelocityRPM = 0;
         public double currentAmps = 0;
         public double appliedVolts = 0;
+
+        public boolean luniteDetected = true;
     }
 
     public default void updateInputs(ArmIOInputs inputs) {}

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

@@ -8,15 +8,19 @@
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.DigitalInput;
 import frc.team5115.Constants;
 import java.util.ArrayList;
 
 public class ArmIOSparkMax implements ArmIO {
     private final SparkMax motor;
-    ;
+
     private final AbsoluteEncoder absoluteEncoder;
 
+    private final DigitalInput sensor;
+
     public ArmIOSparkMax() {
+        sensor = new DigitalInput(Constants.LUNITE_SENSOR);
         motor = new SparkMax(Constants.ARM_MOTOR_ID, MotorType.kBrushless);
         absoluteEncoder = motor.getAbsoluteEncoder();
         final SparkMaxConfig motorConfig = new SparkMaxConfig();
@@ -46,5 +50,6 @@ public void updateInputs(ArmIOInputs inputs) {
         }
         inputs.currentAmps = motor.getOutputCurrent();
         inputs.appliedVolts = motor.getAppliedOutput() * motor.getBusVoltage();
+        inputs.luniteDetected = sensor.get();
     }
 }

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

@@ -5,7 +5,6 @@
 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;
@@ -52,7 +51,7 @@ public Command intakeIf(BooleanSupplier supplier) {
                         if (inputs.currentAmps > 25.0) {
                             io.setPercent(-0.22);
                         } else {
-                            io.setPercent(INTAKE_SPEED);
+                            io.setPercent(Constants.INTAKE_SPEED);
                         }
                     } else {
                         io.setPercent(0);