diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1d7f329..969c521 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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 diff --git a/src/main/java/frc/robot/subsystems/intake/LinearRackIO.java b/src/main/java/frc/robot/subsystems/intake/LinearRackIO.java new file mode 100644 index 0000000..6b40c6a --- /dev/null +++ b/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 positionMeters; + private final StatusSignal velocityMetersPerSecond; + private final StatusSignal voltage; + private final StatusSignal statorCurrent; + private final StatusSignal supplyCurrent; + private final StatusSignal 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); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/LinearRackIOSim.java b/src/main/java/frc/robot/subsystems/intake/LinearRackIOSim.java new file mode 100644 index 0000000..567b624 --- /dev/null +++ b/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); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java index 425f6b0..0eb9a61 100644 --- a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java +++ b/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; } }