diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d6d290e..ba5e31e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -219,7 +219,7 @@ public Robot() { MotorType.KrakenX44, canivore), (ROBOT_MODE == RobotMode.REAL) - ? new RollerIO(10, LindexerSubsystem.getIndexerConfigs(), canivore) + ? new RollerIO(10, LindexerSubsystem.getKickerConfigs(), canivore) : new RollerIOSim( 10, LindexerSubsystem.getKickerConfigs(), @@ -257,7 +257,33 @@ public Robot() { // note that the climber is not instantiated here break; case COMP: - indexer = new SpindexerSubsystem(); + indexer = + new SpindexerSubsystem( + canivore, + (ROBOT_MODE == RobotMode.REAL) + ? new RollerIO(9, SpindexerSubsystem.getIndexerConfigs(), canivore) + : new RollerIOSim( + 9, + SpindexerSubsystem.getIndexerConfigs(), + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + DCMotor.getKrakenX44Foc(1), 0.003, SpindexerSubsystem.GEAR_RATIO), + DCMotor.getKrakenX44Foc(1)), + MotorType.KrakenX44, + canivore), + (ROBOT_MODE == RobotMode.REAL) + ? new RollerIO(10, SpindexerSubsystem.getKickerConfigs(), canivore) + : new RollerIOSim( + 10, + SpindexerSubsystem.getKickerConfigs(), + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + DCMotor.getKrakenX44Foc(1), + 0.00001, + SpindexerSubsystem.KICKER_GEAR_RATIO), + DCMotor.getKrakenX44Foc(1)), + MotorType.KrakenX44, + canivore)); intake = new LintakeSubsystem(); shooter = new TurretSubsystem(); climber = new ClimberSubsystem(); // TODO climber diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 0ce8617..a4fcf50 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -80,9 +80,6 @@ public Trigger getTrigger() { @AutoLogOutput(key = "Superstructure/Anti Jam Req") private Trigger antiJamReq; - @AutoLogOutput(key = "Superstructure/Is Full") - private Trigger isFull; - @AutoLogOutput(key = "Superstructure/Is Empty") private Trigger isEmpty; @@ -133,8 +130,6 @@ private void addTriggers() { antiJamReq = driver.a().or(operator.a()); - isFull = new Trigger(indexer::isFull).debounce(0.5); // TODO tune - isEmpty = new Trigger(indexer::isEmpty); } diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index 6bd3118..0b35d8a 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -9,11 +9,9 @@ /** Add your docs here. */ public interface Indexer { - public boolean isFull(); - public boolean isEmpty(); - public boolean isPartiallyFull(); + public boolean isNotEmpty(); /** Run indexer towards shooter and kicker away from shooter */ public Command index(); diff --git a/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java index 44cceff..876e9c0 100644 --- a/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java @@ -54,19 +54,14 @@ public LindexerSubsystem(CANBus canbus, RollerIO indexRollerIO, RollerIO kickerI this.indexRollerIO = indexRollerIO; } - @Override - public boolean isFull() { - return firstCANRangeInputs.isDetected && secondCANRangeInputs.isDetected; - } - @Override public boolean isEmpty() { return !firstCANRangeInputs.isDetected && !secondCANRangeInputs.isDetected; } @Override - public boolean isPartiallyFull() { - return !firstCANRangeInputs.isDetected && secondCANRangeInputs.isDetected; + public boolean isNotEmpty() { + return secondCANRangeInputs.isDetected; } @Override diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java index 4346925..3ae7192 100644 --- a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java @@ -1,61 +1,168 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - package frc.robot.subsystems.indexer; +import static edu.wpi.first.units.Units.Volts; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.*; +import frc.robot.components.canrange.CANrangeIOInputsAutoLogged; +import frc.robot.components.canrange.CANrangeIOReal; +import frc.robot.components.rollers.RollerIO; +import frc.robot.components.rollers.RollerIOInputsAutoLogged; +import org.littletonrobotics.junction.Logger; -/** Spindexer = SPINning Indexer. !! COMP !! */ +/** Spindexer = Spinning Indexer. !! COMP !! */ public class SpindexerSubsystem extends SubsystemBase implements Indexer { - /** Creates a new SpindexerSubsystem. */ - public SpindexerSubsystem() {} - @Override - public void periodic() { - // This method will be called once per scheduler run - } + public static final double GEAR_RATIO = 2.0; + private CANrangeIOReal CANRangeIO; - @Override - public boolean isFull() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'isFull'"); + private RollerIO indexRollerIO; + + CANrangeIOInputsAutoLogged CANRangeInputs = new CANrangeIOInputsAutoLogged(); + + RollerIOInputsAutoLogged rollerInputs = new RollerIOInputsAutoLogged(); + + RollerIO kickerIO; + RollerIOInputsAutoLogged kickerInputs = new RollerIOInputsAutoLogged(); + + private SysIdRoutine indexRollerSysid = + new SysIdRoutine( + new Config( + null, + null, + null, + (state) -> Logger.recordOutput("Indexer/Roller/SysID State", state.toString())), + new Mechanism((volts) -> indexRollerIO.setRollerVoltage(volts.in(Volts)), null, this)); + + public static final double MAX_ACCELERATION = 10.0; + public static final double MAX_VELOCITY = 10.0; + public static final double KICKER_GEAR_RATIO = 2.0; + + public SpindexerSubsystem(CANBus canbus, RollerIO indexRollerIO, RollerIO kickerIO) { + this.kickerIO = kickerIO; + CANRangeIO = new CANrangeIOReal(1, canbus, 10); + this.indexRollerIO = indexRollerIO; } @Override public boolean isEmpty() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'isEmpty'"); + return !CANRangeInputs.isDetected; } @Override - public boolean isPartiallyFull() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'isPartiallyFull'"); + public boolean isNotEmpty() { + return CANRangeInputs.isDetected; } @Override public Command index() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'index'"); + return this.run( + () -> { + indexRollerIO.setRollerVoltage(7); + kickerIO.setRollerVoltage(7); + }); } @Override - public Command spit() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'spit'"); + public Command kick() { + return this.run( + () -> { + indexRollerIO.setRollerVoltage(12); + kickerIO.setRollerVoltage(-7); + }); } @Override - public Command kick() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'kick'"); + public Command spit() { + return this.run( + () -> { + indexRollerIO.setRollerVoltage(-5); + kickerIO.setRollerVoltage(-5); + }); } @Override public Command rest() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'rest'"); + return this.run( + () -> { + indexRollerIO.setRollerVoltage(0.0); + kickerIO.setRollerVoltage(0.0); + }); + } + + public static TalonFXConfiguration getIndexerConfigs() { + TalonFXConfiguration config = new TalonFXConfiguration(); + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + config.Feedback.SensorToMechanismRatio = GEAR_RATIO; + + config.Slot0.kS = 0; + config.Slot0.kG = 0; + config.Slot0.kV = 0; + config.Slot0.kP = 0; + config.Slot0.kD = 0; + + config.CurrentLimits.StatorCurrentLimit = 80.0; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.SupplyCurrentLimit = 60.0; + config.CurrentLimits.SupplyCurrentLimitEnable = true; + config.CurrentLimits.SupplyCurrentLowerLimit = 40.0; + config.CurrentLimits.SupplyCurrentLowerTime = 0.25; + + return config; + } + + public static TalonFXConfiguration getKickerConfigs() { + TalonFXConfiguration config = new TalonFXConfiguration(); + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + // Converts angular motion to linear motion + config.Feedback.SensorToMechanismRatio = KICKER_GEAR_RATIO; + + config.Slot0.kS = 0; + config.Slot0.kG = 0; + config.Slot0.kV = 0; + config.Slot0.kP = 0; + config.Slot0.kD = 0; + + config.CurrentLimits.StatorCurrentLimit = 80.0; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.SupplyCurrentLimit = 60.0; + config.CurrentLimits.SupplyCurrentLimitEnable = true; + config.CurrentLimits.SupplyCurrentLowerLimit = 40.0; + config.CurrentLimits.SupplyCurrentLowerTime = 0.25; + + return config; + } + + @Override + public void periodic() { + CANRangeIO.updateInputs(CANRangeInputs); + Logger.processInputs("Indexer/First Beambreak", CANRangeInputs); + indexRollerIO.updateInputs(rollerInputs); + Logger.processInputs("Indexer/Roller", rollerInputs); + kickerIO.updateInputs(kickerInputs); + Logger.processInputs("Indexer/Kicker", kickerInputs); + } + + public Command runRollerSysId() { + return Commands.sequence( + indexRollerSysid.quasistatic(Direction.kForward), + indexRollerSysid.quasistatic(Direction.kReverse), + indexRollerSysid.dynamic(Direction.kForward), + indexRollerSysid.dynamic(Direction.kReverse)); } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index e422313..e546a3c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -605,6 +605,18 @@ public Command faceHub(DoubleSupplier xVel, DoubleSupplier yVel) { yVel); } + public Command bumpAlign(DoubleSupplier xVel, DoubleSupplier yVel) { + return driveWithHeadingSnap( + () -> { + Translation2d robotHubVec = + FieldUtils.getCurrentHubTranslation().minus(getPose().getTranslation()); + // atan2 takes y as the first arg (i think bc θ = atan(y/x) but idk) + return Rotation2d.fromRadians(Math.atan2(robotHubVec.getY(), robotHubVec.getX())); + }, + xVel, + yVel); + } + public boolean isInAutoAimTolerance(Pose2d target) { return isInTolerance( target, AutoAlign.TRANSLATION_TOLERANCE_METERS, AutoAlign.ROTATION_TOLERANCE_RADIANS);