Speedrun

Signed-off-by: Jade Turner <[email protected]>

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

@@ -5,7 +5,7 @@
 package frc.robot;
 
 public final class Constants {
-  public static final Mode currentMode = Mode.REAL;
+  public static final Mode currentMode = Mode.SIM;
 
   public static enum Mode {
     /** Running on a real robotokent. */

src/main/java/frc/robot/Robot.java

@@ -32,6 +32,16 @@
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.DriveIO;
 import frc.robot.subsystems.drive.DriveIOCTRE;
+import frc.robot.subsystems.index.Index;
+import frc.robot.subsystems.index.IndexIO;
+import frc.robot.subsystems.index.IndexIOSparkMax;
+import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.intake.IntakeIO;
+import frc.robot.subsystems.intake.IntakeIOSparkMax;
+import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.shooter.ShooterIO;
+import frc.robot.subsystems.shooter.ShooterIOSim;
+import frc.robot.subsystems.shooter.ShooterIOSparkMax;
 import frc.robot.subsystems.vision.VisionIO;
 import frc.robot.subsystems.vision.VisionIOLimelight;
 import frc.robot.subsystems.vision.VisionIOSim;
@@ -57,6 +67,10 @@ public class Robot extends LoggedRobot {
   private final CommandXboxController driver = new CommandXboxController(0);
 
   private final Drive drive;
+  private final Shooter m_shooter;
+  private final Intake m_intake;
+  private final Index m_index;
+  private final Superstructure superstructure;
   private final VisionSource limelight_back;
   private final VisionSource limelight_front;
 
@@ -111,6 +125,10 @@ public Robot() {
                     TunerConstants.BackLeft,
                     TunerConstants.BackRight));
 
+        m_shooter = new Shooter(new ShooterIOSparkMax());
+        m_index = new Index(new IndexIOSparkMax());
+        m_intake = new Intake(new IntakeIOSparkMax());
+
         limelight_back = new VisionSource(new VisionIOLimelight("limelight-back"));
         limelight_front = new VisionSource(new VisionIOLimelight("limelight-front"));
         break;
@@ -129,6 +147,10 @@ public Robot() {
                     TunerConstants.BackLeft,
                     TunerConstants.BackRight));
 
+        m_shooter = new Shooter(new ShooterIOSim());
+        m_index = new Index(new IndexIO() {});
+        m_intake = new Intake(new IntakeIO() {});
+
         limelight_back = new VisionSource(new VisionIOSim("limelight-back"));
         limelight_front = new VisionSource(new VisionIOSim("limelight-front"));
         break;
@@ -143,6 +165,9 @@ public Robot() {
 
         // inputs come from log file
         drive = new Drive(new DriveIO() {});
+        m_shooter = new Shooter(new ShooterIO() {});
+        m_index = new Index(new IndexIO() {});
+        m_intake = new Intake(new IntakeIO() {});
         limelight_back = new VisionSource(new VisionIO() {});
         limelight_front = new VisionSource(new VisionIO() {});
         break;
@@ -152,6 +177,8 @@ public Robot() {
     Logger.registerURCL(URCL.startExternal());
     Logger.start();
 
+    superstructure = new Superstructure(m_shooter, m_intake, m_index);
+
     // TODO remove me and use LL LED abstraction
     NetworkTableInstance.getDefault().getTable("limelight-front").getEntry("ledMode").setNumber(1);
 
@@ -169,6 +196,10 @@ public Robot() {
             Millimeters.of(0),
             Rotation3d.kZero)); // we only care about 2d translation data
 
+    m_intake.setDefaultCommand(superstructure.intake());
+    m_shooter.setDefaultCommand(m_shooter.stop());
+    m_index.setDefaultCommand(m_index.stop());
+
     drive.setDefaultCommand(
         // Drivetrain will execute this command periodically
         drive.applyRequest(
@@ -180,21 +211,22 @@ public Robot() {
                         driver.getRightX()
                             * MaxAngularRate) // Drive counterclockwise with negative X (left)
             ));
-    driver
-        .leftBumper()
-        .whileTrue(
-            drive.driverAssistance(
-                new Pose2d(2, 5.6, new Rotation2d()),
-                () -> -driver.getLeftY() * MaxSpeed,
-                () -> -driver.getLeftX() * MaxSpeed));
+    // driver
+    //     .leftBumper()
+    //     .whileTrue(
+    //         drive.driverAssistance(
+    //             new Pose2d(2, 5.6, new Rotation2d()),
+    //             () -> -driver.getLeftY() * MaxSpeed,
+    //             () -> -driver.getLeftX() * MaxSpeed));
+    driver.rightBumper().whileTrue(superstructure.shoot());
     driver.a().whileTrue(drive.brake());
     driver.y().onTrue(drive.seedFieldCentric());
-    driver
-        .x()
-        .whileTrue(
-            drive
-                .moveToPosition(new Pose2d(2, 5.6, new Rotation2d()))
-                .until(drive::isTeleopAtSetpoint));
+    // driver
+    //     .x()
+    //     .whileTrue(
+    //         drive
+    //             .moveToPosition(new Pose2d(2, 5.6, new Rotation2d()))
+    //             .until(drive::isTeleopAtSetpoint));
 
     // Run SysId routines when holding back/start and X/Y.
     // Note that each routine should be run exactly once in a single log.

src/main/java/frc/robot/Superstructure.java

@@ -0,0 +1,62 @@
+// Copyright (c) 2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+package frc.robot;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.subsystems.index.Index;
+import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.shooter.Shooter;
+
+public class Superstructure {
+  private static final int FRONT_BEAMBREAK_PORT = 5;
+  private static final int BACK_BEAMBREAK_PORT = 6;
+
+  // TODO should be logged akit style
+  private final DigitalInput frontBeambreak = new DigitalInput(FRONT_BEAMBREAK_PORT);
+  private final DigitalInput backBeamBreak = new DigitalInput(BACK_BEAMBREAK_PORT);
+
+  private final Shooter m_shooter;
+  private final Intake m_intake;
+  private final Index m_index;
+
+  public final Trigger m_hasNote = new Trigger(backBeamBreak::get);
+  public final Trigger m_intaking = new Trigger(frontBeambreak::get).negate();
+
+  public Superstructure(Shooter shooter, Intake intake, Index index) {
+    m_shooter = shooter;
+    m_intake = intake;
+    m_index = index;
+
+    m_intaking.onTrue(
+        Commands.parallel(
+            m_intake.setVoltage(5).until(m_hasNote).andThen(m_intake.stop()),
+            m_index
+                .setVoltage(-3.5)
+                .until(m_hasNote)
+                .andThen(new WaitCommand(0.5))
+                .andThen(m_index.setVoltage(2))
+                .until(m_hasNote)
+                .andThen(m_index.stop())));
+  }
+
+  public Command intake() {
+    return m_intake.setVoltage(10).until(m_intaking);
+  }
+
+  public Command shoot() {
+    return m_shooter
+        .spinup(250)
+        .withTimeout(1.5)
+        .andThen(Commands.parallel(m_shooter.maintain(), m_index.setVoltage(8)));
+  }
+
+  public Command stop() {
+    return Commands.parallel(m_shooter.stop(), m_intake.stop(), m_index.stop());
+  }
+}

src/main/java/frc/robot/subsystems/index/Index.java

@@ -0,0 +1,29 @@
+package frc.robot.subsystems.index;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
+
+public class Index extends SubsystemBase {
+  private final IndexIO io;
+  private final IndexIOInputsAutoLogged inputs;
+
+  public Index(IndexIO io) {
+    this.io = io;
+    inputs = new IndexIOInputsAutoLogged();
+  }
+
+  @Override
+  public void periodic() {
+    io.updateInputs(inputs);
+    Logger.processInputs("Index", inputs);
+  }
+
+  public Command stop() {
+    return runOnce(() -> io.stop());
+  }
+
+  public Command setVoltage(double volts) {
+    return run(() -> io.setVoltage(volts));
+  }
+}

src/main/java/frc/robot/subsystems/index/IndexIO.java

@@ -0,0 +1,22 @@
+package frc.robot.subsystems.index;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface IndexIO {
+  @AutoLog
+  public static class IndexIOInputs {
+    public double position;
+    public double velocity;
+    public double appliedVolts;
+    public double currentAmps;
+  }
+
+  /** Updates the set of loggable inputs. */
+  public default void updateInputs(IndexIOInputs inputs) {}
+
+  /** Run open loop at the specified voltage. */
+  public default void setVoltage(double volts) {}
+
+  /** Stop in open loop. */
+  public default void stop() {}
+}

src/main/java/frc/robot/subsystems/index/IndexIOSparkMax.java

@@ -0,0 +1,29 @@
+package frc.robot.subsystems.index;
+
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+import edu.wpi.first.wpilibj.RobotController;
+
+public class IndexIOSparkMax implements IndexIO {
+  private static final int PORT = 21;
+
+  SparkMax spark = new SparkMax(PORT, MotorType.kBrushless);
+
+  @Override
+  public void updateInputs(IndexIOInputs inputs) {
+    inputs.appliedVolts = spark.getAppliedOutput() * RobotController.getBatteryVoltage();
+    inputs.currentAmps = spark.getOutputCurrent();
+    inputs.position = spark.getEncoder().getPosition();
+    inputs.velocity = spark.getEncoder().getVelocity();
+  }
+
+  @Override
+  public void setVoltage(double volts) {
+    spark.setVoltage(volts);
+  }
+
+  @Override
+  public void stop() {
+    spark.stopMotor();
+  }
+}

src/main/java/frc/robot/subsystems/intake/Intake.java

@@ -0,0 +1,29 @@
+package frc.robot.subsystems.intake;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
+
+public class Intake extends SubsystemBase {
+  private final IntakeIO io;
+  private final IntakeIOInputsAutoLogged inputs;
+
+  public Intake(IntakeIO io) {
+    this.io = io;
+    inputs = new IntakeIOInputsAutoLogged();
+  }
+
+  @Override
+  public void periodic() {
+    io.updateInputs(inputs);
+    Logger.processInputs("Intake", inputs);
+  }
+
+  public Command stop() {
+    return runOnce(() -> io.stop());
+  }
+
+  public Command setVoltage(double volts) {
+    return run(() -> io.setVoltage(volts));
+  }
+}

src/main/java/frc/robot/subsystems/intake/IntakeIO.java

@@ -0,0 +1,22 @@
+package frc.robot.subsystems.intake;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface IntakeIO {
+  @AutoLog
+  public static class IntakeIOInputs {
+    public double position;
+    public double velocity;
+    public double appliedVolts;
+    public double currentAmps;
+  }
+
+  /** Updates the set of loggable inputs. */
+  public default void updateInputs(IntakeIOInputs inputs) {}
+
+  /** Run open loop at the specified voltage. */
+  public default void setVoltage(double volts) {}
+
+  /** Stop in open loop. */
+  public default void stop() {}
+}

src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java

@@ -0,0 +1,29 @@
+package frc.robot.subsystems.intake;
+
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+import edu.wpi.first.wpilibj.RobotController;
+
+public class IntakeIOSparkMax implements IntakeIO {
+  private static final int PORT = 32;
+
+  SparkMax spark = new SparkMax(PORT, MotorType.kBrushless);
+
+  @Override
+  public void updateInputs(IntakeIOInputs inputs) {
+    inputs.appliedVolts = spark.getAppliedOutput() * RobotController.getBatteryVoltage();
+    inputs.currentAmps = spark.getOutputCurrent();
+    inputs.position = spark.getEncoder().getPosition();
+    inputs.velocity = spark.getEncoder().getVelocity();
+  }
+
+  @Override
+  public void setVoltage(double volts) {
+    spark.setVoltage(volts);
+  }
+
+  @Override
+  public void stop() {
+    spark.stopMotor();
+  }
+}

src/main/java/frc/robot/subsystems/shooter/Shooter.java

@@ -14,11 +14,11 @@
 import org.littletonrobotics.junction.Logger;
 
 public class Shooter extends SubsystemBase {
-  private static final double P = 0.1;
+  private static final double P = 0.00035273;
   private static final double D = 0;
-  private static final double S = 0;
-  private static final double V = 0;
-  private static final double A = 0;
+  private static final double S = 0.090624;
+  private static final double V = 0.0021896;
+  private static final double A = 0.00070176;
 
   private final ShooterIO io;
   private final ShooterIOInputsAutoLogged inputs;

src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java

@@ -9,7 +9,7 @@
 import edu.wpi.first.wpilibj.RobotController;
 
 public class ShooterIOSparkMax implements ShooterIO {
-  private static final int PORT = 99;
+  private static final int PORT = 31;
 
   SparkMax spark = new SparkMax(PORT, MotorType.kBrushless);