Lintake

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

@@ -29,6 +29,7 @@
 import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Superstructure.SuperState;
+import frc.robot.components.canrange.CANrangeIOReal;
 import frc.robot.components.rollers.RollerIO;
 import frc.robot.components.rollers.RollerIOSim;
 import frc.robot.subsystems.climber.ClimberIO;
@@ -39,6 +40,8 @@
 import frc.robot.subsystems.indexer.SpindexerSubsystem;
 import frc.robot.subsystems.intake.FintakeSubsystem;
 import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.intake.LinearRackIO;
+import frc.robot.subsystems.intake.LinearRackIOSim;
 import frc.robot.subsystems.intake.LintakeSubsystem;
 import frc.robot.subsystems.led.LEDIOReal;
 import frc.robot.subsystems.led.LEDSubsystem;
@@ -312,7 +315,27 @@ public Robot() {
                             DCMotor.getKrakenX44Foc(1)),
                         MotorType.KrakenX44,
                         canivore));
-        intake = new LintakeSubsystem();
+        // TODO: FOVs
+        intake =
+            (ROBOT_MODE == RobotMode.REAL)
+                ? new LintakeSubsystem(
+                    new LinearRackIO(14, canivore, LintakeSubsystem.getRackMotorConfig()),
+                    new RollerIO(8, LintakeSubsystem.getRollerMotorConfig(), canivore),
+                    new CANrangeIOReal(0, canivore, 10))
+                : new LintakeSubsystem(
+                    new LinearRackIOSim(14, canivore, LintakeSubsystem.getRackMotorConfig()),
+                    new RollerIOSim(
+                        8,
+                        LintakeSubsystem.getRollerMotorConfig(),
+                        new DCMotorSim(
+                            LinearSystemId.createDCMotorSystem(
+                                DCMotor.getKrakenX44Foc(1),
+                                0.001,
+                                LintakeSubsystem.ROLLER_GEAR_RATIO),
+                            DCMotor.getKrakenX44Foc(1)),
+                        MotorType.KrakenX44,
+                        canivore),
+                    new CANrangeIOReal(0, canivore, 10));
         shooter =
             new TurretSubsystem(
                 ROBOT_MODE == RobotMode.REAL

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

@@ -0,0 +1,94 @@
+package frc.robot.subsystems.intake;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+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.units.measure.Temperature;
+import edu.wpi.first.units.measure.Voltage;
+import org.littletonrobotics.junction.AutoLog;
+
+public class LinearRackIO {
+
+  @AutoLog
+  public static class LinearRackIOInputs {
+    public double positionMeters = 0.0;
+    public double velocityMetersPerSecond = 0.0;
+    public double voltage = 0.0;
+    public double statorCurrentAmps = 0.0;
+    public double supplyCurrentAmps = 0.0;
+    public double temperatureC = 0.0;
+  }
+
+  protected final TalonFX motor;
+
+  // Technically, these measure angle, but the conversion from angle to linear movement happens in
+  // the sensor-to-mechanism ratio
+  private final StatusSignal<Angle> positionMeters;
+  private final StatusSignal<AngularVelocity> velocityMetersPerSecond;
+  private final StatusSignal<Voltage> voltage;
+  private final StatusSignal<Current> statorCurrent;
+  private final StatusSignal<Current> supplyCurrent;
+  private final StatusSignal<Temperature> temperature;
+
+  private VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true);
+  // I think we might want to motion profile this so i'm using motion magic
+  private MotionMagicVoltage motionMagicVoltage = new MotionMagicVoltage(0.0).withEnableFOC(true);
+
+  private double setpointMeters = 0.0;
+
+  public LinearRackIO(int motorID, CANBus canBus, TalonFXConfiguration config) {
+    this.motor = new TalonFX(motorID, canBus);
+
+    positionMeters = motor.getPosition();
+    velocityMetersPerSecond = motor.getVelocity();
+    voltage = motor.getMotorVoltage();
+    statorCurrent = motor.getStatorCurrent();
+    supplyCurrent = motor.getSupplyCurrent();
+    temperature = motor.getDeviceTemp();
+
+    BaseStatusSignal.setUpdateFrequencyForAll(
+        50.0,
+        positionMeters,
+        velocityMetersPerSecond,
+        voltage,
+        statorCurrent,
+        supplyCurrent,
+        temperature);
+    motor.optimizeBusUtilization();
+
+    motor.getConfigurator().apply(config);
+  }
+
+  public void updateInputs(LinearRackIOInputs inputs) {
+    inputs.positionMeters = positionMeters.getValueAsDouble();
+    inputs.velocityMetersPerSecond = velocityMetersPerSecond.getValueAsDouble();
+    inputs.voltage = voltage.getValueAsDouble();
+    inputs.statorCurrentAmps = statorCurrent.getValueAsDouble();
+    inputs.supplyCurrentAmps = supplyCurrent.getValueAsDouble();
+    inputs.temperatureC = temperature.getValueAsDouble();
+  }
+
+  public void setVoltage(double volts) {
+    motor.setControl(voltageOut.withOutput(volts));
+  }
+
+  public void setPositionSetpoint(double setpointMeters) {
+    this.setpointMeters = setpointMeters;
+    motor.setControl(motionMagicVoltage.withPosition(setpointMeters));
+  }
+
+  public double getSetpointMeters() {
+    return setpointMeters;
+  }
+
+  public void resetEncoder(double positionMeters) {
+    motor.setPosition(positionMeters);
+  }
+}

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

@@ -0,0 +1,68 @@
+package frc.robot.subsystems.intake;
+
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.Utils;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.sim.ChassisReference;
+import com.ctre.phoenix6.sim.TalonFXSimState;
+import com.ctre.phoenix6.sim.TalonFXSimState.MotorType;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Notifier;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.simulation.ElevatorSim;
+
+public class LinearRackIOSim extends LinearRackIO {
+  // TODO: SHOULD THIS BE AN ELEVATOR?
+  ElevatorSim physicsSim =
+      new ElevatorSim(
+          LinearSystemId.createElevatorSystem(
+              DCMotor.getKrakenX44Foc(1),
+              Units.lbsToKilograms(10.0),
+              LintakeSubsystem.RACK_PINION_DIAMETER_METERS / 2,
+              LintakeSubsystem.RACK_GEAR_RATIO),
+          DCMotor.getKrakenX44Foc(1),
+          0.0,
+          LintakeSubsystem.MAX_EXTENSION_METERS,
+          false,
+          0.0);
+
+  private static final double SIM_LOOP_PERIOD = 0.002; // 2 ms
+  private Notifier notifier;
+  private TalonFXSimState talonSim;
+  private double lastLoopTime = 0.0;
+
+  public LinearRackIOSim(int motorId, CANBus canBus, TalonFXConfiguration config) {
+    super(motorId, canBus, config);
+
+    this.talonSim = motor.getSimState();
+    // Maybe try to make have these passed in? Maybe not needed tho
+    this.talonSim.setMotorType(MotorType.KrakenX44);
+    this.talonSim.Orientation = ChassisReference.Clockwise_Positive; // TODO
+
+    notifier =
+        new Notifier(
+            () -> {
+              double deltaTime = (Utils.getCurrentTimeSeconds() - lastLoopTime);
+              lastLoopTime = Utils.getCurrentTimeSeconds();
+
+              talonSim.setSupplyVoltage(RobotController.getBatteryVoltage());
+
+              physicsSim.setInputVoltage(talonSim.getMotorVoltage());
+              physicsSim.update(deltaTime);
+
+              // I think these should be multiplied?
+              talonSim.setRawRotorPosition(
+                  physicsSim.getPositionMeters()
+                      * (LintakeSubsystem.RACK_GEAR_RATIO
+                          * (Math.PI * LintakeSubsystem.RACK_PINION_DIAMETER_METERS)));
+              talonSim.setRotorVelocity(
+                  physicsSim.getVelocityMetersPerSecond()
+                      * (LintakeSubsystem.RACK_GEAR_RATIO
+                          * (Math.PI * LintakeSubsystem.RACK_PINION_DIAMETER_METERS)));
+            });
+
+    notifier.startPeriodic(SIM_LOOP_PERIOD);
+  }
+}

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

@@ -4,40 +4,159 @@
 
 package frc.robot.subsystems.intake;
 
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.signals.GravityTypeValue;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.math.filter.LinearFilter;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.components.canrange.CANrangeIO;
+import frc.robot.components.canrange.CANrangeIOInputsAutoLogged;
+import frc.robot.components.rollers.RollerIO;
+import frc.robot.components.rollers.RollerIOInputsAutoLogged;
+import org.littletonrobotics.junction.Logger;
 
 /** Lintake = Linear Intake. !! COMP !! */
 public class LintakeSubsystem extends SubsystemBase implements Intake {
+  // I'm calling zero fully retracted and 1 fully extended (so that kG works if its needed)
+  public static final double MAX_EXTENSION_METERS = Units.inchesToMeters(16.0);
+  public static final double EXTENDED_POSITION_METERS = MAX_EXTENSION_METERS;
+  public static final double RACK_GEAR_RATIO = 8.0;
+  public static final double RACK_PINION_DIAMETER_METERS = Units.inchesToMeters(0.975);
+  public static final double ROLLER_GEAR_RATIO = 34 / 15;
+  public static final double CURRENT_ZEROING_THRESHOLD = 30; // TODO: TUNE
+
+  private final LinearRackIO rackIO;
+  private LinearRackIOInputsAutoLogged rackIOInputs = new LinearRackIOInputsAutoLogged();
+
+  private final RollerIO rollerIO;
+  private RollerIOInputsAutoLogged rollerIOInputs = new RollerIOInputsAutoLogged();
+
+  private final CANrangeIO canRangeIO;
+  private CANrangeIOInputsAutoLogged canRangeIOInputs = new CANrangeIOInputsAutoLogged();
+
+  private LinearFilter rackCurrentFilter = LinearFilter.movingAverage(10);
+  private double rackCurrentFilterValue = 0.0;
+
   /** Creates a new LintakeSubsystem. */
-  public LintakeSubsystem() {}
+  public LintakeSubsystem(LinearRackIO rackIO, RollerIO rollerIO, CANrangeIO canRangeIO) {
+    this.rackIO = rackIO;
+    this.rollerIO = rollerIO;
+    this.canRangeIO = canRangeIO;
+  }
 
   @Override
   public void periodic() {
-    // This method will be called once per scheduler run
+    rackIO.updateInputs(rackIOInputs);
+    Logger.processInputs("Intake/Rack", rackIOInputs);
+
+    rollerIO.updateInputs(rollerIOInputs);
+    Logger.processInputs("Intake/Rollers", rollerIOInputs);
+
+    canRangeIO.updateInputs(canRangeIOInputs);
+    Logger.processInputs("Intake/CANRange", canRangeIOInputs);
+
+    rackCurrentFilterValue = rackCurrentFilter.calculate(rackIOInputs.statorCurrentAmps);
   }
 
   @Override
   public Command intake() {
-    // TODO Auto-generated method stub
-    throw new UnsupportedOperationException("Unimplemented method 'intake'");
+    return this.run(
+        () -> {
+          rackIO.setPositionSetpoint(EXTENDED_POSITION_METERS);
+          rollerIO.setRollerVoltage(10.0);
+        });
   }
 
   @Override
   public Command outtake() {
-    // TODO Auto-generated method stub
-    throw new UnsupportedOperationException("Unimplemented method 'outtake'");
+    return this.run(
+        () -> {
+          // Oscillate between 0.5x extension pos and 1x extension pos
+          rackIO.setPositionSetpoint(
+              (0.25 * Math.sin(Timer.getFPGATimestamp()) + 0.75) * EXTENDED_POSITION_METERS);
+          rollerIO.setRollerVoltage(10.0);
+        });
   }
 
   @Override
   public Command rest() {
-    // TODO Auto-generated method stub
-    throw new UnsupportedOperationException("Unimplemented method 'rest'");
+    return this.run(
+        () -> {
+          rackIO.setPositionSetpoint(EXTENDED_POSITION_METERS);
+          rollerIO.setRollerVoltage(0.0);
+        });
+  }
+
+  public Command runCurrentZeroing() {
+    return this.run(() -> rackIO.setVoltage(3))
+        .until(() -> rackCurrentFilterValue > CURRENT_ZEROING_THRESHOLD)
+        .andThen(Commands.parallel(Commands.print("Intake Zeroed"), zeroRack()));
+  }
+
+  public Command zeroRack() {
+    return this.runOnce(() -> rackIO.resetEncoder(MAX_EXTENSION_METERS));
+  }
+
+  public static TalonFXConfiguration getRackMotorConfig() {
+    TalonFXConfiguration config = new TalonFXConfiguration();
+
+    config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; // TODO
+
+    // Converts rotational motion to linear motion
+    config.Feedback.SensorToMechanismRatio =
+        RACK_GEAR_RATIO * (Math.PI * RACK_PINION_DIAMETER_METERS);
+
+    config.Slot0.GravityType = GravityTypeValue.Elevator_Static; // Maybe don't need this?
+    config.Slot0.kG = 0.0;
+    config.Slot0.kS = 0.0;
+    config.Slot0.kV = 0.0;
+    config.Slot0.kP = 0.0;
+    config.Slot0.kD = 0.0;
+
+    // TODO: TUNE
+    config.CurrentLimits.StatorCurrentLimit = 30.0;
+    config.CurrentLimits.StatorCurrentLimitEnable = true;
+    config.CurrentLimits.SupplyCurrentLimit = 40.0;
+    config.CurrentLimits.SupplyCurrentLimitEnable = true;
+
+    // TODO: TUNE
+    config.MotionMagic.MotionMagicCruiseVelocity = 10.0;
+    config.MotionMagic.MotionMagicAcceleration = 30.0;
+
+    return config;
+  }
+
+  public static TalonFXConfiguration getRollerMotorConfig() {
+    TalonFXConfiguration config = new TalonFXConfiguration();
+
+    config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+    config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; // TODO
+
+    // Converts rotational motion to linear motion
+    config.Feedback.SensorToMechanismRatio = ROLLER_GEAR_RATIO;
+
+    config.Slot0.kS = 0.0;
+    config.Slot0.kV = 0.0;
+    config.Slot0.kP = 0.0;
+    config.Slot0.kD = 0.0;
+
+    // TODO: TUNE
+    config.CurrentLimits.StatorCurrentLimit = 80.0;
+    config.CurrentLimits.StatorCurrentLimitEnable = true;
+    config.CurrentLimits.SupplyCurrentLimit = 40.0;
+    config.CurrentLimits.SupplyCurrentLimitEnable = true;
+
+    return config;
   }
 
   @Override
   public boolean beambreak() {
-    // TODO Auto-generated method stub
-    throw new UnsupportedOperationException("Unimplemented method 'beambreak'");
+    return canRangeIOInputs.isDetected;
   }
 }