Merge branch 'coralrollerbar' into mechanism_testing

.vscode/settings.json

@@ -68,5 +68,6 @@
   },
   "[java]": {
     "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle"
-  }
+  },
+  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable"
 }

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

@@ -23,6 +23,9 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.commands.DriveCommands;
+import frc.robot.subsystems.coralrollerbar.CoralRoller;
+import frc.robot.subsystems.coralrollerbar.CoralRollerIOReal;
+import frc.robot.subsystems.coralrollerbar.CoralRollerIOSim;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.GyroIO;
 import frc.robot.subsystems.drive.GyroIOPigeon2;
@@ -43,6 +46,7 @@
 public class RobotContainer {
   // Subsystems
   private final Drive drive;
+  private final CoralRoller coralRoller;
   private final Pivot pivot;
   // Controller
   private final CommandXboxController controller = new CommandXboxController(0);
@@ -54,6 +58,7 @@ public class RobotContainer {
   public RobotContainer() {
     switch (Constants.currentMode) {
       case REAL:
+        coralRoller = CoralRoller.init(new CoralRollerIOReal());
         pivot = Pivot.initialize(new PivotIOReal());
         // Real robot, instantiate hardware IO implementations
         drive =
@@ -66,6 +71,7 @@ public RobotContainer() {
         break;
 
       case SIM:
+        coralRoller = CoralRoller.init(new CoralRollerIOSim());
         pivot = Pivot.initialize(new PivotIOSim());
         // Sim robot, instantiate physics sim IO implementations
         drive =
@@ -78,6 +84,7 @@ public RobotContainer() {
         break;
 
       default:
+        coralRoller = CoralRoller.init(new CoralRollerIOSim());
         pivot = Pivot.initialize(new PivotIOSim());
         // Replayed robot, disable IO implementations
         drive =
@@ -166,6 +173,12 @@ private void configureButtonBindings() {
                             new Pose2d(drive.getPose().getTranslation(), new Rotation2d())),
                     drive)
                 .ignoringDisable(true));
+    controller
+        .leftBumper()
+        .onTrue(Commands.runOnce(() -> coralRoller.setVoltage(6), coralRoller));
+    controller
+        .rightBumper()
+        .onTrue(Commands.runOnce(() -> coralRoller.setVoltage(-6), coralRoller));
   }
 
   /**

src/main/java/frc/robot/subsystems/coralrollerbar/CoralRoller.java

@@ -0,0 +1,50 @@
+package frc.robot.subsystems.coralrollerbar;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
+
+public class CoralRoller extends SubsystemBase {
+  private final CoralRollerIO io;
+  private final CoralRollerIOInputsAutoLogged inputs = new CoralRollerIOInputsAutoLogged();
+
+  private double velocity = 0;
+  private static CoralRoller instance;
+
+  public static CoralRoller getInstance() {
+    return instance;
+  }
+
+  public static CoralRoller init(CoralRollerIO io) {
+    if (instance == null) {
+      instance = new CoralRoller(io);
+    }
+    return instance;
+  }
+
+  private CoralRoller(CoralRollerIO io) {
+    this.io = io;
+    io.updateInputs(inputs);
+  }
+
+  public void setVoltage(double v) {
+    io.setCoralRollerVoltage(v);
+  }
+
+  public double getVelocity() {
+    return inputs.velocityRPM;
+  }
+
+  public double getCurrent() {
+    return inputs.current;
+  }
+
+  public double getVoltage() {
+    return inputs.voltage;
+  }
+
+  @Override
+  public void periodic() {
+    io.updateInputs(inputs);
+    Logger.processInputs("CoralRoller", inputs);
+  }
+}

src/main/java/frc/robot/subsystems/coralrollerbar/CoralRollerConstants.java

@@ -0,0 +1,6 @@
+package frc.robot.subsystems.coralrollerbar;
+
+public final class CoralRollerConstants {
+  public static final int currentLimit = 30;
+  public static final double CanID = 0;
+}

src/main/java/frc/robot/subsystems/coralrollerbar/CoralRollerIO.java

@@ -0,0 +1,18 @@
+package frc.robot.subsystems.coralrollerbar;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface CoralRollerIO {
+  @AutoLog
+  public static class CoralRollerIOInputs {
+    public double current = 0;
+    public double voltage = 0;
+    public double velocityRPM = 0;
+  }
+
+  public default void updateInputs(CoralRollerIOInputs inputs) {}
+
+  public default void setDesiredState(double speed) {}
+
+  public default void setCoralRollerVoltage(double volts) {}
+}

src/main/java/frc/robot/subsystems/coralrollerbar/CoralRollerIOReal.java

@@ -0,0 +1,28 @@
+package frc.robot.subsystems.coralrollerbar;
+
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.SparkMaxConfig;
+
+import edu.wpi.first.units.measure.Velocity;
+
+public class CoralRollerIOReal implements CoralRollerIO {
+  private final SparkMax CoralRollerMotor = new SparkMax(0, MotorType.kBrushless);
+
+  public CoralRollerIOReal() {
+    SparkMaxConfig coralRollerConfig = new SparkMaxConfig();
+    coralRollerConfig.smartCurrentLimit(CoralRollerConstants.currentLimit);
+    coralRollerConfig.voltageCompensation(12);
+  }
+
+  @Override
+  public void setCoralRollerVoltage(double volts) {
+    CoralRollerMotor.setVoltage(volts);
+  }
+  @Override
+  public void updateInputs(CoralRollerIOInputs inputs){
+    inputs.voltage = CoralRollerMotor.getBusVoltage() * CoralRollerMotor.getAppliedOutput();
+    inputs.velocityRPM = CoralRollerMotor.getEncoder().getVelocity();
+    inputs.current = CoralRollerMotor.getOutputCurrent();
+  } 
+}

src/main/java/frc/robot/subsystems/coralrollerbar/CoralRollerIOSim.java

@@ -0,0 +1,29 @@
+package frc.robot.subsystems.coralrollerbar;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+
+public class CoralRollerIOSim implements CoralRollerIO {
+  DCMotorSim motor =
+      new DCMotorSim(
+          LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), 0.5, 0.1),
+          DCMotor.getKrakenX60(1),
+          0.0,
+          0.0);
+  private double appliedVolts = 0;
+
+  @Override
+  public void setCoralRollerVoltage(double volts) {
+    appliedVolts = volts;
+    motor.setInputVoltage(appliedVolts);
+  }
+
+  @Override
+  public void updateInputs(CoralRollerIOInputs inputs) {
+    inputs.velocityRPM = motor.getAngularVelocityRPM();
+    inputs.voltage = motor.getInputVoltage();
+    inputs.current = motor.getCurrentDrawAmps();
+    motor.update(0.02);
+  }
+}