diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1d7f329..2ff5e31 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -8,7 +8,12 @@ import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; @@ -29,6 +34,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 +45,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; @@ -52,6 +60,7 @@ import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.subsystems.swerve.odometry.PhoenixOdometryThread; import frc.robot.utils.CommandXboxControllerSubsystem; +import frc.robot.utils.LoggedTunableNumber; import java.util.Optional; import java.util.Set; import org.ironmaple.simulation.SimulatedArena; @@ -171,6 +180,8 @@ public enum RobotEdition { private final LEDSubsystem leds; private final ClimberSubsystem climber; + private Intake intake = null; + // climber only exists for the comp bot - this is accounted for later private final Superstructure superstructure; @@ -216,7 +227,6 @@ public Robot() { // break // granted this would never actually happen but Indexer indexer = null; - Intake intake = null; Shooter shooter = null; // this looks at the ROBOT_EDITION variable and decides which version of each subsystem to @@ -312,7 +322,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 @@ -564,13 +594,80 @@ private void addAutos() { // autoChooser.addOption("Intake Roller Sysid", intake.runRollerSysid()); // autoChooser.addOption("Flywheel Sysid", shooter.runFlywheelSysid()); + private LoggedTunableNumber turretAngle = new LoggedTunableNumber("Turret Angle Rads", 0); + private LoggedTunableNumber hoodAngle = new LoggedTunableNumber("Hood angle rads", 0); + private LoggedTunableNumber intakeExtension = new LoggedTunableNumber("Intake extension", 0); + @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); superstructure.periodic(); - // TODO Log mechanism poses + // TODO: YAW VALUE FROM HARDWARE + Pose3d turretPose = + new Pose3d( + new Translation3d(-0.177413, -0.111702, 0.350341), + new Rotation3d(0, 0, turretAngle.getAsDouble())); + + // TODO: USE MEASURED EXTENSIONS AND ANGLES + Logger.recordOutput( + "Robot/Mechanism Poses", + new Pose3d[] { + // Turret + turretPose, + // Hood + turretPose + // First transform the hood out to the hood pivot, and rotate by the amount needed + .transformBy( + new Transform3d( + new Translation3d(-0.095638, 0, 0.095123), + new Rotation3d(0, hoodAngle.getAsDouble() * -1, 0))) + // Then, transform the hood back to the correct location relative to the turret + .transformBy( + new Transform3d( + new Translation3d(-0.095638, 0, 0.095123).times(-1), Rotation3d.kZero)), + // Intake + new Pose3d( + intake.getExtensionMeters() * LintakeSubsystem.INTAKE_ROTATION.getCos(), + 0, + -(intake.getExtensionMeters() * LintakeSubsystem.INTAKE_ROTATION.getSin()), + Rotation3d.kZero), + // Climber + new Pose3d(0, 0, climber.getClimberExtensionMeters(), Rotation3d.kZero) + }); + + // TODO: ACTUAL SETPOINT + Pose3d turretSetpoint = + new Pose3d( + new Translation3d(-0.177413, -0.111702, 0.350341), + new Rotation3d(0, 0, turretAngle.getAsDouble())); + // TODO: ACTUAL SETPOINTS + Logger.recordOutput( + "Robot/Mechanism Setpoints", + new Pose3d[] { + // Turret + turretSetpoint, + // Hood + turretSetpoint + // First transform the hood out to the hood pivot, and rotate by the amount needed + .transformBy( + new Transform3d( + new Translation3d(-0.095638, 0, 0.095123), + new Rotation3d(0, hoodAngle.getAsDouble() * -1, 0))) + // Then, transform the hood back to the correct location relative to the turret + .transformBy( + new Transform3d( + new Translation3d(-0.095638, 0, 0.095123).times(-1), Rotation3d.kZero)), + // Intake + new Pose3d( + intake.getExtensionSetpointMeters() * LintakeSubsystem.INTAKE_ROTATION.getCos(), + 0, + -(intake.getExtensionSetpointMeters() * LintakeSubsystem.INTAKE_ROTATION.getSin()), + Rotation3d.kZero), + // Climber + new Pose3d(0, 0, climber.getClimberSetpointMeters(), Rotation3d.kZero) + }); updateAlerts(); } @@ -634,7 +731,21 @@ public void simulationInit() { } @Override - public void simulationPeriodic() {} + public void simulationPeriodic() { + // Log zeroed poses for mechs and robot for debugging in sim + Logger.recordOutput( + "Robot/Zeroed Mechanism Poses", + new Pose3d[] { + // Turret + new Pose3d(), + // Hood + new Pose3d(), + new Pose3d(), + new Pose3d() + }); + + Logger.recordOutput("Robot/Zero Position", new Pose2d()); + } @Override public void disabledInit() {} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index 88b1b93..9e579b1 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -10,19 +10,19 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.geometry.Rotation2d; 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; +import org.littletonrobotics.junction.AutoLogOutput; public class ClimberIO { @AutoLog public static class ClimberIOInputs { - public Rotation2d motorPositionRotations = new Rotation2d(); + public double motorPositionMeters = 0.0; public double motorVelocityMetersPerSec = 0.0; public double motorStatorCurrentAmps = 0.0; public double motorSupplyCurrentAmps = 0.0; @@ -32,8 +32,9 @@ public static class ClimberIOInputs { protected final TalonFX climberMotor; - private final StatusSignal motorPositionRotations; - private final StatusSignal angularVelocityRotsPerSec; + // Rotation -> linear conversion happens in sensor to mech ratio + private final StatusSignal motorPositionMeters; + private final StatusSignal velocityMetersPerSec; private final StatusSignal statorCurrentAmps; private final StatusSignal supplyCurrentAmps; private final StatusSignal motorVoltage; @@ -50,21 +51,21 @@ public ClimberIO(CANBus canBus) { climberMotor = new TalonFX(30, canBus); climberMotor.getConfigurator().apply(ClimberIO.getClimberConfiguration()); - angularVelocityRotsPerSec = climberMotor.getVelocity(); + velocityMetersPerSec = climberMotor.getVelocity(); supplyCurrentAmps = climberMotor.getSupplyCurrent(); motorVoltage = climberMotor.getMotorVoltage(); statorCurrentAmps = climberMotor.getStatorCurrent(); motorTemp = climberMotor.getDeviceTemp(); - motorPositionRotations = climberMotor.getPosition(); + motorPositionMeters = climberMotor.getPosition(); BaseStatusSignal.setUpdateFrequencyForAll( 50.0, - angularVelocityRotsPerSec, + velocityMetersPerSec, supplyCurrentAmps, motorVoltage, statorCurrentAmps, motorTemp, - motorPositionRotations); + motorPositionMeters); climberMotor.optimizeBusUtilization(); } @@ -76,7 +77,8 @@ public static TalonFXConfiguration getClimberConfiguration() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; // todo: find and make climber gear ratio variable - config.Feedback.SensorToMechanismRatio = ClimberSubsystem.GEAR_RATIO; + config.Feedback.SensorToMechanismRatio = + ClimberSubsystem.GEAR_RATIO * (Math.PI * ClimberSubsystem.SPOOL_DIAMETER_METERS); // todo: tune config.Slot0.kS = 0.0; @@ -108,19 +110,23 @@ public void setClimberVelocity(double climberVelocity) { public void updateInputs(ClimberIOInputs inputs) { BaseStatusSignal.refreshAll( - motorPositionRotations, - angularVelocityRotsPerSec, + motorPositionMeters, + velocityMetersPerSec, statorCurrentAmps, supplyCurrentAmps, motorVoltage, motorTemp); - inputs.motorPositionRotations = - Rotation2d.fromRotations(motorPositionRotations.getValueAsDouble()); + inputs.motorPositionMeters = motorPositionMeters.getValueAsDouble(); inputs.motorVoltage = motorVoltage.getValueAsDouble(); inputs.motorTempC = motorTemp.getValueAsDouble(); inputs.motorSupplyCurrentAmps = supplyCurrentAmps.getValueAsDouble(); inputs.motorStatorCurrentAmps = statorCurrentAmps.getValueAsDouble(); - inputs.motorVelocityMetersPerSec = angularVelocityRotsPerSec.getValueAsDouble(); + inputs.motorVelocityMetersPerSec = velocityMetersPerSec.getValueAsDouble(); + } + + @AutoLogOutput(key = "Climber/Setpoint Meters") + public double getClimberSetpointMeters() { + return climberSetpoint; } } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java index 57c1c46..43910a2 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.climber; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger; @@ -8,6 +9,7 @@ public class ClimberSubsystem extends SubsystemBase { // todo: find actual constants public static double GEAR_RATIO = (45.0 / 1.0); public static double MAX_EXTENSION_METERS = 0.2413; + public static double SPOOL_DIAMETER_METERS = Units.inchesToMeters(0.668898); public static double MAX_ACCELERATION = 10.0; public static double MAX_VELOCITY = 2.0; @@ -39,4 +41,13 @@ public Command retractClimber() { climberIO.setClimberPosition(0.0); }); } + + public double getClimberExtensionMeters() { + // Convert rotations into linear motion + return climberInputs.motorPositionMeters; + } + + public double getClimberSetpointMeters() { + return climberIO.getClimberSetpointMeters(); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java index 3d3256c..2f7195f 100644 --- a/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java @@ -98,4 +98,14 @@ public static TalonFXConfiguration getIntakeConfig() { public boolean beambreak() { return canrangeInputs.isDetected; } + + @Override + public double getExtensionMeters() { + return 0; + } + + @Override + public double getExtensionSetpointMeters() { + return 0; + } } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index e193211..a65a19f 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -19,4 +19,8 @@ public interface Intake { /** for controller rumble */ public boolean beambreak(); + + public double getExtensionMeters(); + + public double getExtensionSetpointMeters(); } 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..2490af8 100644 --- a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java @@ -4,40 +4,172 @@ 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.geometry.Rotation2d; +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 + 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; + // From CAD + public static final Rotation2d INTAKE_ROTATION = Rotation2d.fromDegrees(17.329856); + 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.run(() -> 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 = 80.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; + } + + @Override + public double getExtensionMeters() { + return rackIOInputs.positionMeters; + } + + @Override + public double getExtensionSetpointMeters() { + return rackIO.getSetpointMeters(); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index eedea8c..2cd6c1f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -173,4 +173,14 @@ public boolean isFacingTarget() { // return MathUtil.isNear( // target.getRadians(), getPose().getRotation().getRadians(), 0.174533); // 10 degrees // } + + // @Override + // public double getHoodAngleRads() { + // return hoodInputs.hoodPositionRotations; + // } + + // @Override + // public double getTurretAngleRads() { + // return 0; + // } }