Michael

src/main/java/frc/robot/Constants.java

@@ -8,13 +8,6 @@
 package frc.robot;
 
 import com.ctre.phoenix6.CANBus;
-import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
-import com.revrobotics.spark.FeedbackSensor;
-
-import edu.wpi.first.units.Units;
-import edu.wpi.first.units.measure.Angle;
-import edu.wpi.first.units.measure.AngularVelocity;
-import edu.wpi.first.units.measure.Current;
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
@@ -40,147 +33,5 @@ public static enum Mode {
     REPLAY
   }
 
-  public static class ShooterConstants {
-
-    public enum ShooterState {
-      SHOOTER,
-      REVERSE,
-      IDLE
-    }
-
-    public static final boolean attached = true;
-    public static final int id = 50;
-    public static final boolean inverted = false;
-    public static final boolean breakType = false;
-    public static final double gearRatio = 1 / 1;
-
-    public static final double p = 1;
-    public static final double i = 0;
-    public static final double d = 0;
-    // public static final double maxIAccum = 2 * i; //CTRE Doesn't have one? Might Add later
-
-    public static final Current stallLimit = Units.Amps.of(60);
-    public static final Current supplyLimit = Units.Amps.of(80);
-    public static final double maxForwardOutput = 1;
-    public static final double maxReverseOutput = 0.5;
-
-    public static final AngularVelocity shooterSpeed = Units.RPM.of(4000);
-    public static final AngularVelocity reverseSpeed = Units.RPM.of(-1000);
-    public static final AngularVelocity idleSpeed = Units.RPM.of(0);
-
-    public enum ShooterModes {
-      SHOOTER(shooterSpeed, 1.0),
-      REVERSE(reverseSpeed, -0.5),
-      IDLE(idleSpeed, 0.0);
-
-      public AngularVelocity speed;
-      public double output;
-
-      ShooterModes(AngularVelocity speed, double output) {
-        this.speed = speed;
-        this.output = output;
-      }
-    }
-  }
-
-  public static class IndexerConstants {
-
-    public enum IndexerState {
-      INDEXER,
-      REVERSE,
-      IDLE
-    }
-
-    public static final boolean attached = true;
-
-    public static final int id = -1;
-
-    public static final double p = 1;
-    public static final double i = 0;
-    public static final double d = 0;
-
-    public static final boolean invert = false;
-    public static final boolean breakType = false;
-    public static final double gearRatio = 1 / 1;
-
-
-    public static final Current stallLimit = Units.Amps.of(60);
-    public static final Current supplyLimit = Units.Amps.of(80);
-    public static final double maxForwardOutput = 0.5;
-    public static final double maxReverseOutput = -0.5;
-
-    public static final double indexerSpeed = 0.5;
-    public static final double reverseSpeed = -0.5;
-    public static final double idleSpeed = 0.0;
 
-    public enum IndexerModes {
-      INDEXER(indexerSpeed),
-      REVERSE(reverseSpeed),
-      IDLE(idleSpeed);
-
-      public double output;
-
-      IndexerModes(double output) {
-        this.output = output;
-      }
-    }
-  }
-
-  public static final class IntakeRollerIOConstants {
-    public static final boolean attached = true;
-    public static final boolean useRpm = false;
-
-    public static final int id = -1;
-
-    public static final double p = 1;
-    public static final double i = 0;
-    public static final double d = 0;
-    public static final double maxIAccum = 0.2;
-
-    public static final double gearRatio = 1 / 1;
-
-    public static final Current forwardTorqueLimit = Units.Amps.of(80);
-    public static final Current reverseTorqueLimit = Units.Amps.of(80);
-
-    public static final boolean invert = false;
-    public static final boolean gravityType = false;
-    public static final boolean breakType = false;
-
-    public static final FeedbackSensorSourceValue feedbackSensorCTRE =
-        FeedbackSensorSourceValue.FusedCANcoder;
-    public static final FeedbackSensor feedbackSensorREV = 
-        FeedbackSensor.kPrimaryEncoder;
-
-    public static final double maxForwardOutput = 0.5;
-    public static final double maxReverseOutput = -0.5;
-
-    public static final boolean useFMaxRotation = true;
-    public static final boolean useRMaxRotation = true;
-    public static final Angle maxReverseRotation = Units.Rotation.of(-0.1);
-    public static final Angle maxFowardRotation = Units.Rotation.of(5);
-
-    public static final boolean useStallLimit = true;
-    public static final boolean useSupplyLimit = true;
-    public static final Current stallLimit = Units.Amps.of(80);
-    public static final Current supplyLimit = Units.Amps.of(60);
-
-    public static final AngularVelocity rollerIOSpeed = Units.RPM.of(3000);
-    public static final AngularVelocity outtakeSpeed = Units.RPM.of(0);
-    public static final AngularVelocity idleSpeed = Units.RPM.of(0);
-
-    public enum RollerIOModes {
-      IDLE(idleSpeed, 0.0),
-      INTAKE(rollerIOSpeed, 8.4),
-      OUTTAKE(outtakeSpeed, -4.8);
-
-      public AngularVelocity speed;
-      // TODO: Make sure voltage is just output * 12 (for 12V)
-      public double voltage;
-
-      RollerIOModes(AngularVelocity speed, double voltage) {
-        this.speed = speed;
-        this.voltage = voltage;
-      }
-    }
-  }
-}
+}

src/main/java/frc/robot/RobotContainer.java

@@ -7,33 +7,47 @@
 
 package frc.robot;
 
-import static frc.robot.subsystems.vision.VisionConstants.*;
-
 import com.pathplanner.lib.auto.AutoBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
-import frc.robot.Constants.IntakeRollerIOConstants.RollerIOModes;
 import frc.robot.commands.DriveCommands;
 import frc.robot.generated.TunerConstants;
+import frc.robot.subsystems.climber.Climber;
+import frc.robot.subsystems.climber.ClimberIO;
+import frc.robot.subsystems.climber.ClimberIOSim;
+import frc.robot.subsystems.climber.ClimberIOSparkFlex;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.GyroIO;
 import frc.robot.subsystems.drive.GyroIOPigeon2;
 import frc.robot.subsystems.drive.ModuleIO;
 import frc.robot.subsystems.drive.ModuleIOSim;
 import frc.robot.subsystems.drive.ModuleIOTalonFX;
-import frc.robot.subsystems.intake.roller.Roller;
+import frc.robot.subsystems.hopper.Carpet;
+import frc.robot.subsystems.hopper.CarpetIO;
+import frc.robot.subsystems.hopper.CarpetIOSim;
+import frc.robot.subsystems.hopper.CarpetIOSparkFlex;
+import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.intake.IntakeConstants.IntakeMode;
+import frc.robot.subsystems.intake.pivot.PivotIO;
+import frc.robot.subsystems.intake.pivot.PivotIOSim;
+import frc.robot.subsystems.intake.pivot.PivotIOSparkFlex;
 import frc.robot.subsystems.intake.roller.RollerIO;
 import frc.robot.subsystems.intake.roller.RollerIOSim;
 import frc.robot.subsystems.intake.roller.RollerIOSparkFlex;
-import frc.robot.subsystems.vision.Vision;
-import frc.robot.subsystems.vision.VisionIO;
-import frc.robot.subsystems.vision.VisionIOLimelight;
-import frc.robot.subsystems.vision.VisionIOPhotonVisionSim;
+import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.shooter.angler.AnglerIO;
+import frc.robot.subsystems.shooter.angler.AnglerIOSim;
+import frc.robot.subsystems.shooter.angler.AnglerIOTalonFX;
+import frc.robot.subsystems.shooter.flywheel.FlywheelIO;
+import frc.robot.subsystems.shooter.flywheel.FlywheelIOSim;
+import frc.robot.subsystems.shooter.flywheel.FlywheelIOTalonFX;
 import frc.team5431.titan.core.joysticks.CommandXboxController;
 
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
@@ -47,8 +61,11 @@
 public class RobotContainer {
   // Subsystems
   private final Drive drive;
-  private final Vision vision;
-  private final Roller roller;
+//   private final Vision vision;
+  private final Intake intake;
+  private final Shooter shooter;
+  private final Carpet carpet;
+  private final Climber climber;
 
   // Controller
   private final CommandXboxController driver = new CommandXboxController(0);
@@ -73,15 +90,16 @@ public RobotContainer() {
                 new ModuleIOTalonFX(TunerConstants.BackRight));
 
         // Real robot, instantiate hardware IO implementations
-        vision =
-            new Vision(
-                drive::addVisionMeasurement,
-                new VisionIOLimelight(camera0Name, drive::getRotation),
-                new VisionIOLimelight(camera1Name, drive::getRotation));
-        roller =
-            new Roller(
-                new RollerIOSparkFlex());
-
+        // vision =
+        //     new Vision(
+        //         drive::addVisionMeasurement,
+        //         new VisionIOLimelight(camera0Name, drive::getRotation),
+        //         new VisionIOLimelight(camera1Name, drive::getRotation));
+
+        intake = new Intake(new RollerIOSparkFlex(), new PivotIOSparkFlex());
+        shooter = new Shooter(new AnglerIOTalonFX(), new FlywheelIOTalonFX());
+        carpet = new Carpet(new CarpetIOSparkFlex());
+        climber = new Climber(new ClimberIOSparkFlex());
         // vision =
         // new Vision(
         // demoDrive::addVisionMeasurement,
@@ -116,17 +134,21 @@ public RobotContainer() {
                 new ModuleIOSim(TunerConstants.BackLeft),
                 new ModuleIOSim(TunerConstants.BackRight));
 
-        vision =
-            new Vision(
-                drive::addVisionMeasurement,
-                new VisionIOPhotonVisionSim(camera0Name, robotToCamera0, drive::getPose),
-                new VisionIOPhotonVisionSim(camera1Name, robotToCamera1, drive::getPose));
+        // vision =
+        //     new Vision(
+        //         drive::addVisionMeasurement,
+        //         new VisionIOPhotonVisionSim(camera0Name, robotToCamera0, drive::getPose),
+        //         new VisionIOPhotonVisionSim(camera1Name, robotToCamera1, drive::getPose));
 
-        roller = 
-            new Roller(
-                new RollerIOSim() {});
+        intake = 
+            new Intake(new RollerIOSim(), new PivotIOSim());
+        shooter = 
+              new Shooter(new AnglerIOSim(), new FlywheelIOSim());
+        carpet = 
+              new Carpet(new CarpetIOSim());
+        climber = 
+              new Climber(new ClimberIOSim());
         break;
-
       default:
         // Replayed robot, disable IO implementations
         drive =
@@ -137,9 +159,11 @@ public RobotContainer() {
                 new ModuleIO() {},
                 new ModuleIO() {});
 
-        vision = new Vision(drive::addVisionMeasurement, new VisionIO() {}, new VisionIO() {});
-        roller = new Roller(new RollerIO() {});
-
+        // vision = new Vision(drive::addVisionMeasurement, new VisionIO() {}, new VisionIO() {});
+        intake = new Intake(new RollerIO() {}, new PivotIO() {});
+        shooter = new Shooter(new AnglerIO() {}, new FlywheelIO() {});
+        carpet = new Carpet(new CarpetIO() {});
+        climber = new Climber(new ClimberIO() {});
         break;
 
     }
@@ -165,6 +189,10 @@ public RobotContainer() {
 
     configureDriverBindings();
     configureOperatorBindings();
+
+
+    SmartDashboard.putData("Scheduler", CommandScheduler.getInstance());
+
   }
 
   /**
@@ -182,18 +210,18 @@ private void configureDriverBindings() {
             () -> -driver.getLeftX(),
             () -> -driver.getRightX()));
 
-    // Lock to 0° when A button is held
-    driver
-        .a()
-        .whileTrue(
-            DriveCommands.joystickDriveAtAngle(
-                drive,
-                () -> -driver.getLeftY(),
-                () -> -driver.getLeftX(),
-                () -> Rotation2d.kZero));
+    // // Lock to 0° when A button is held
+    // driver
+    //     .a()
+    //     .whileTrue(
+    //         DriveCommands.joystickDriveAtAngle(
+    //             drive,
+    //             () -> -driver.getLeftY(),
+    //             () -> -driver.getLeftX(),
+    //             () -> Rotation2d.kZero));
 
-    // Switch to X pattern when X button is pressed
-    driver.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
+    // // Switch to X pattern when X button is pressed
+    // driver.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
 
     // Reset gyro to 0° when B button is pressed
     driver
@@ -224,7 +252,8 @@ private void configureDriverBindings() {
   }
 
   private void configureOperatorBindings() {
-    operator.a().onTrue(roller.runIntakeCommand(RollerIOModes.INTAKE, true));
+    operator.a().whileTrue(intake.runIntakeCommand(IntakeMode.STOW));
+    operator.b().whileTrue(intake.runIntakeCommand(IntakeMode.INTAKE));
   }
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.

src/main/java/frc/robot/subsystems/climber/Climber.java

@@ -0,0 +1,40 @@
+package frc.robot.subsystems.climber;
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.subsystems.climber.ClimberConstants.ClimberModes;
+
+public class Climber extends SubsystemBase {
+    private final ClimberIO climberIO;
+  private final ClimberIOInputsAutoLogged carpetInputs = new ClimberIOInputsAutoLogged();
+
+  private ClimberModes mode;
+
+  public Climber(ClimberIO climberIO) {
+    this.climberIO = climberIO;
+    this.mode = ClimberModes.STOW;
+  }
+
+  @Override
+  public void periodic() {
+    climberIO.updateInputs(carpetInputs);
+    Logger.processInputs("Climber/", carpetInputs);
+
+    Logger.recordOutput("Climber/Mode", mode);
+  }
+
+  public void runClimberEnum(ClimberModes climberMode) {
+    this.mode = climberMode;
+      climberIO.setClimberPosition(mode.positionAngle.baseUnitMagnitude());
+  }
+
+  public Command runCarpetCommand(ClimberModes climberModes) {
+    return new RunCommand(() -> {
+      this.runClimberEnum(climberModes);
+    }, this).withName("Carpet.runCarpetEnum");
+  }
+
+}