Scoral

simgui.json

@@ -11,6 +11,8 @@
       "/FMSInfo": "FMSInfo",
       "/Faults/Active Faults": "Alerts",
       "/Faults/Total Faults": "Alerts",
+      "/Faults/Active Faults": "Alerts",
+      "/Faults/Total Faults": "Alerts",
       "/Robot/autos": "String Chooser",
       "/Robot/drive/field2d": "Field2d",
       "/Robot/drive/frontLeft/driveFeedback": "PIDController",
@@ -23,10 +25,6 @@
       "/Robot/drive/rearRight/turnFeedback": "PIDController",
       "/Robot/drive/rotationController": "PIDController",
       "/Robot/drive/translationController": "ProfiledPIDController",
-      "/Robot/elevator/measurement/mech": "Mechanism2d",
-      "/Robot/elevator/pid": "ProfiledPIDController",
-      "/Robot/elevator/setpoint/mech": "Mechanism2d",
-      "/SmartDashboard/Alerts": "Alerts",
       "/SmartDashboard/Scheduler": "Scheduler",
       "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d",
       "/SmartDashboard/drive dynamic backward": "Command",
@@ -162,18 +160,18 @@
               "open": true
             },
             "open": true
-          }
-        },
-        "elevator": {
+          },
           "open": true
         },
-        "open": true,
         "vision": {
           "open": true
         }
       },
       "Shuffleboard": {
         "open": true
+      },
+      "SmartDashboard": {
+        "open": true
       }
     }
   },

src/main/java/org/sciborgs1155/robot/Ports.java

@@ -26,8 +26,7 @@ public static final class Elevator {
   }
 
   public static final class Scoral {
-    public static final int TOP_ROLLER = 21;
-    public static final int BOTTOM_ROLLER = 22;
+    public static final int ROLLER = 21;
     public static final int BEAMBREAK = 23;
   }
 

src/main/java/org/sciborgs1155/robot/Robot.java

@@ -33,6 +33,7 @@
 import org.sciborgs1155.robot.commands.Autos;
 import org.sciborgs1155.robot.drive.Drive;
 import org.sciborgs1155.robot.elevator.Elevator;
+import org.sciborgs1155.robot.scoral.Scoral;
 import org.sciborgs1155.robot.vision.Vision;
 
 /**
@@ -52,6 +53,7 @@ public class Robot extends CommandRobot implements Logged {
   private final Drive drive = Drive.create();
   private final Vision vision = Vision.create();
   private final Elevator elevator = Elevator.create();
+  private final Scoral scoral = Scoral.create();
 
   // COMMANDS
   @Log.NT private final SendableChooser<Command> autos = Autos.configureAutos(drive);
@@ -171,7 +173,8 @@ public Command rumble(RumbleType rumbleType, double strength) {
   }
 
   public Command systemsCheck() {
-    return Test.toCommand(drive.systemsCheck()).withName("Test Mechanisms");
+    return Test.toCommand(drive.systemsCheck(), Test.fromCommand(scoral.outwards().withTimeout(2)))
+        .withName("Test Mechanisms");
   }
 
   @Override

src/main/java/org/sciborgs1155/robot/scoral/Scoral.java

@@ -1,39 +1,71 @@
 package org.sciborgs1155.robot.scoral;
 
+import static edu.wpi.first.units.Units.Amps;
+import static org.sciborgs1155.robot.Ports.Scoral.*;
 import static org.sciborgs1155.robot.scoral.ScoralConstants.*;
 
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import java.util.Optional;
+import monologue.Annotations.Log;
+import monologue.Logged;
+import org.sciborgs1155.lib.SimpleMotor;
 import org.sciborgs1155.robot.Robot;
 
-public class Scoral extends SubsystemBase {
+public class Scoral extends SubsystemBase implements Logged, AutoCloseable {
+  private final SimpleMotor hardware;
 
-  private ScoralIO scoral;
+  private final DigitalInput beambreak = new DigitalInput(BEAMBREAK);
 
-  public Scoral(ScoralIO scoral) {
-    this.scoral = scoral;
+  public static Scoral create() {
+    return new Scoral(Robot.isReal() ? realMotor() : SimpleMotor.none());
   }
 
-  public static Scoral create() {
-    return Robot.isReal() ? new Scoral(new RealScoral()) : new Scoral(new SimScoral());
+  private static SimpleMotor realMotor() {
+    TalonFXConfiguration config = new TalonFXConfiguration();
+
+    config.CurrentLimits.StatorCurrentLimit = STATOR_LIMIT.in(Amps);
+    config.CurrentLimits.SupplyCurrentLimit = CURRENT_LIMIT.in(Amps);
+    config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+
+    return SimpleMotor.talon(new TalonFX(ROLLER), config);
   }
 
   public static Scoral none() {
-    return new Scoral(new NoScoral());
+    return new Scoral(SimpleMotor.none());
+  }
+
+  public Scoral(SimpleMotor hardware) {
+    this.hardware = hardware;
   }
 
-  /** Runs the motor to outtake, as in pushing out, a coral. */
-  public Command outtake() {
-    return run(() -> scoral.setPower(POWER));
+  /** Runs the motor to move a coral outwards. */
+  public Command outwards() {
+    return run(() -> hardware.set(POWER)).withName("outwards");
   }
 
-  /** Runs the motor to intake, as in pulling in, a coral. */
-  public Command intake() {
-    return run(() -> scoral.setPower(-POWER));
+  /** Runs the motor to move a coral inwards. */
+  public Command inwards() {
+    return run(() -> hardware.set(-POWER)).withName("inwards");
   }
 
   /** Returns the value of the beambreak. */
+  @Log.NT
   public boolean beambreak() {
-    return scoral.beambreak();
+    return beambreak.get();
+  }
+
+  @Override
+  public void periodic() {
+    log("command", Optional.ofNullable(getCurrentCommand()).map(Command::getName).orElse("none"));
+  }
+
+  @Override
+  public void close() throws Exception {
+    hardware.close();
   }
 }

src/main/java/org/sciborgs1155/robot/scoral/ScoralConstants.java

@@ -1,5 +1,18 @@
 package org.sciborgs1155.robot.scoral;
 
+import static edu.wpi.first.units.Units.Amps;
+import static edu.wpi.first.units.Units.Milliseconds;
+
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.Time;
+
 public class ScoralConstants {
   public static final double POWER = 0.5;
+  public static final Current STATOR_LIMIT = Amps.of(90);
+  public static final Current CURRENT_LIMIT = Amps.of(45);
+  public static final Time RAMP_TIME = Milliseconds.of(50);
+
+  public static final double kS = 0;
+  public static final double kV = 0.2;
+  public static final double kA = 0.5;
 }

src/test/java/org/sciborgs1155/robot/ElevatorTest.java

@@ -16,7 +16,7 @@
 
 public class ElevatorTest {
   private Elevator elevator;
-  private final double DELTA = .05;
+  private final double DELTA = 0.05;
 
   @BeforeEach
   public void initialize() {