Naively implement hardware IO

team401 · Dec 10, 2023 · 1c2dd4c · 1c2dd4c
1 parent 1d45015
commit 1c2dd4c
Show file tree

Hide file tree

Showing 15 changed files with 549 additions and 326 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -23,6 +23,14 @@
 
 public final class Constants {
 
+    public enum Mode {
+        REAL,
+        SIM,
+        REPLAY,
+    }
+
+    public static final Mode mode = Mode.REAL;
+
     public static final double loopTime = 0.02;
 
     public static final class CANDevices {

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -5,11 +5,8 @@
 
 package frc.robot;
 
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -19,8 +16,13 @@
 import frc.robot.Constants.GamePieceMode;
 import frc.robot.Constants.Position;
 import frc.robot.subsystems.arm.ArmSubsystem;
-import frc.robot.subsystems.arm.ArmSubsystem.ActiveArmSide;
 import frc.robot.subsystems.arm.ArmSubsystem.ArmPosition;
+import frc.robot.subsystems.arm.pivot.PivotIO;
+import frc.robot.subsystems.arm.pivot.PivotIOFalcon;
+import frc.robot.subsystems.arm.telescope.TelescopeIO;
+import frc.robot.subsystems.arm.telescope.TelescopeIOFalcon;
+import frc.robot.subsystems.arm.wrist.WristIO;
+import frc.robot.subsystems.arm.wrist.WristIOFalcon;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.LEDManager;
 // import frc.robot.subsystems.drive.Drive;
@@ -32,11 +34,11 @@
 public class RobotContainer {
 
     // private final Drive drive = new Drive();
-    private final ArmSubsystem arm = new ArmSubsystem();
-    private final IntakeSubsystem intake = new IntakeSubsystem(() -> arm.getArmSide());
+    private final ArmSubsystem arm;
+    private final IntakeSubsystem intake;
     @SuppressWarnings("unused")
     private final Vision vision = new Vision();
-    private final LEDManager ledManager = new LEDManager(() -> arm.getArmSide());
+    private final LEDManager ledManager;
 
     private final Thrustmaster leftStick = new Thrustmaster(0);
     private final Thrustmaster rightStick = new Thrustmaster(1);
@@ -51,6 +53,22 @@ public class RobotContainer {
 
     /** The container for the robot. Contains subsystems, OI devices, and commands. */
     public RobotContainer() {
+        switch (Constants.mode) {
+            case REAL:
+                arm = new ArmSubsystem(
+                    new PivotIOFalcon(),
+                    new TelescopeIOFalcon(),
+                    new WristIOFalcon());
+                break;
+            default:
+                arm = new ArmSubsystem(
+                    new PivotIO() {},
+                    new TelescopeIO() {},
+                    new WristIO() {});
+        }
+
+        intake = new IntakeSubsystem(arm::getArmSide);
+        ledManager = new LEDManager(arm::getArmSide);
 
         configureSubsystems();
         configureCompBindings();

diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java
@@ -15,6 +15,12 @@
 import frc.robot.Constants.PivotConstants;
 import frc.robot.Constants.TelescopeConstants;
 import frc.robot.Constants.WristConstants;
+import frc.robot.subsystems.arm.pivot.Pivot;
+import frc.robot.subsystems.arm.pivot.PivotIO;
+import frc.robot.subsystems.arm.telescope.Telescope;
+import frc.robot.subsystems.arm.telescope.TelescopeIO;
+import frc.robot.subsystems.arm.wrist.Wrist;
+import frc.robot.subsystems.arm.wrist.WristIO;
 
 public class ArmSubsystem extends SubsystemBase {
 
@@ -59,18 +65,21 @@ public class ArmSubsystem extends SubsystemBase {
             new Color8Bit(Color.kCoral)));
 
 
-    public ArmSubsystem() {
+    public ArmSubsystem(PivotIO pivotIO, TelescopeIO telescopeIO, WristIO wristIO) {
         pivot = new Pivot(
+                pivotIO,
                 new TrapezoidProfile.Constraints(
                         Units.degreesToRadians(360),
                         Units.degreesToRadians(600)),
                 () -> telescope.getPosition());
 
         telescope = new Telescope(
+                telescopeIO,
                 new TrapezoidProfile.Constraints(3, 3),
                 () -> pivot.getPosition());
 
         wrist = new Wrist(
+                wristIO,
                 new TrapezoidProfile.Constraints(
                         Units.degreesToRadians(630),
                         Units.degreesToRadians(810)),

diff --git a/src/main/java/frc/robot/subsystems/arm/GenericArmJoint.java b/src/main/java/frc/robot/subsystems/arm/GenericArmJoint.java
@@ -41,7 +41,7 @@ public void setSetpoint(double setpoint) {
     }
 
     public void runControls() {
-        update();
+        updateInputs();
 
         if (!active) {
             setOutput(0.0);
@@ -67,11 +67,7 @@ public void runControls() {
 
     public abstract void setBrakeMode(boolean brake);
 
-    /**
-     * Optional Override: Update internal state periodically
-     */
-    protected void update() {
-    }
+    protected abstract void updateInputs();
 
     /**
      * Set the control input to this joint, in volts

diff --git a/src/main/java/frc/robot/subsystems/arm/Pivot.java b/src/main/java/frc/robot/subsystems/arm/Pivot.java