diff --git a/notes/canIDs.md b/notes/canIDs.md index 9675c0bb..ca6ba0a6 100644 --- a/notes/canIDs.md +++ b/notes/canIDs.md @@ -27,7 +27,7 @@ Gyro (Pigeon2) | 0 CANivore | 0 -# Comp (total devices: 25 + canivore) +# Comp (total devices: 26 + canivore) Device | CAN ID | Code ------ | ------ | ------ Front Left Drive | 0 | X60-044 @@ -40,13 +40,14 @@ Back Right Drive | 6 | X60-042 Back Right Steer | 7 | X44-015 Intake Rollers | 8 | X44-012 Spindexer Agitator | 9 | X60-046 -Kicker Roller | 10 | X60-051 +1st Kicker Roller | 10 | X44-004 Shooter Hood | 11 | X44-008 Shooter Flywheel Leader (Left) | 12 | X60-048 Shooter Flywheel Follower (Right) | 13 | X60-050 intake Pivot | 14 | X44-017 Turret Pivot | 15 | X44-001 Climber | 16 | X60-049 +2nd Kicker Roller | 17 | X60-051 Front Left Encoder (CANcoder) | 0 Front Right Encoder (CANcoder) | 1 Back Left Encoder (CANcoder) | 2 diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2610e5e3..e138fa98 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -207,6 +207,8 @@ public enum RobotEdition { private Intake intake = null; private Shooter shooter = null; + + private Indexer indexer = null; private final CANdleSubsystem candle = new CANdleSubsystem(new CANdleIOReal(0, CANdleSubsystem.getCandleConfig(), canivore)); @@ -258,7 +260,6 @@ public Robot() { // accounted for, it wouldn't be able to pass anything to the superstructure below and it would // break // granted this would never actually happen but - Indexer indexer = null; // this looks at the ROBOT_EDITION variable and decides which version of each subsystem to // create based on that @@ -348,17 +349,30 @@ public Robot() { MotorType.KrakenX44, canivore), (ROBOT_MODE == RobotMode.REAL) - ? new RollerIO(10, SpindexerSubsystem.getKickerConfig(), canivore) + ? new RollerIO(10, SpindexerSubsystem.getX44KickerConfig(), canivore) : new RollerIOSim( 10, - SpindexerSubsystem.getKickerConfig(), + SpindexerSubsystem.getX44KickerConfig(), new DCMotorSim( LinearSystemId.createDCMotorSystem( DCMotor.getKrakenX44Foc(1), 0.00001, - SpindexerSubsystem.KICKER_GEAR_RATIO), + SpindexerSubsystem.X44_KICKER_GEAR_RATIO), DCMotor.getKrakenX44Foc(1)), MotorType.KrakenX44, + canivore), + (ROBOT_MODE == RobotMode.REAL) + ? new RollerIO(17, SpindexerSubsystem.getX60KickerConfig(), canivore) + : new RollerIOSim( + 17, + SpindexerSubsystem.getX60KickerConfig(), + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + DCMotor.getKrakenX60Foc(1), + 0.00001, + SpindexerSubsystem.X60_KICKER_GEAR_RATIO), + DCMotor.getKrakenX60Foc(1)), + MotorType.KrakenX60, canivore)); intake = (ROBOT_MODE == RobotMode.REAL) @@ -811,6 +825,9 @@ private void addAutos() { autoChooser.addOption("Flywheel Sysid", shooter.runFlywheelSysid()); autoChooser.addOption("Hood Sysid", shooter.runHoodSysid()); + autoChooser.addOption("X60 Sysid", indexer.runX60Sysid()); + + autoChooser.addOption("X44 Sysid", indexer.runX44Sysid()); autoChooser.addOption("Right Neutral Outpost Score", autos.getRightNeutralOutpostScore()); diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 8d2a32f3..e4bbf472 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -500,7 +500,15 @@ private void addCommands() { bindCommands( SuperState.FEED, intake.agitate(), - indexer.kick(), + indexer.kick( + () -> + NewAutoAim.getParametersMechA( + swerve.getPose(), + swerve.getVelocityRobotRelative(), + FeedTargets.getFeedTarget(feedTarget).getTranslation(), + AutoAim.FEED_SHOT_TREE) + .shotData() + .flywheelVelocityRotPerSec()), shooter.feed( () -> NewAutoAim.getParametersMechA( @@ -530,7 +538,15 @@ private void addCommands() { bindCommands( SuperState.FEED_FLOW, intake.intake(), - indexer.kick(), + indexer.kick( + () -> + NewAutoAim.getParametersMechA( + swerve.getPose(), + swerve.getVelocityRobotRelative(), + FeedTargets.getFeedTarget(feedTarget).getTranslation(), + AutoAim.FEED_SHOT_TREE) + .shotData() + .flywheelVelocityRotPerSec()), shooter.feed( () -> NewAutoAim.getParametersMechA( @@ -561,7 +577,17 @@ private void addCommands() { SuperState.SCORE, intake.agitate(), // intake.restExtended(), - indexer.kick(), + indexer.kick( + () -> + NewAutoAim.getParametersMechA( + swerve.getPose(), + swerve.getVelocityRobotRelative(), + FieldUtils.getCurrentHubTranslation(), + Robot.ROBOT_EDITION == RobotEdition.ALPHA + ? AutoAim.ALPHA_HUB_SHOT_TREE + : AutoAim.COMP_HUB_SHOT_TREE) + .shotData() + .flywheelVelocityRotPerSec()), shooter.score( () -> NewAutoAim.getParametersMechA( @@ -591,7 +617,17 @@ private void addCommands() { bindCommands( SuperState.SCORE_FLOW, intake.intake(), - indexer.kick(), + indexer.kick( + () -> + NewAutoAim.getParametersMechA( + swerve.getPose(), + swerve.getVelocityRobotRelative(), + FieldUtils.getCurrentHubTranslation(), + Robot.ROBOT_EDITION == RobotEdition.ALPHA + ? AutoAim.ALPHA_HUB_SHOT_TREE + : AutoAim.COMP_HUB_SHOT_TREE) + .shotData() + .flywheelVelocityRotPerSec()), shooter.score( () -> NewAutoAim.getParametersMechA( @@ -654,7 +690,17 @@ private void addCommands() { bindCommands( SuperState.SCORE_PRE_CLIMB, intake.restRetracted(), - indexer.kick(), + indexer.kick( + () -> + NewAutoAim.getParametersMechA( + swerve.getPose(), + swerve.getVelocityRobotRelative(), + FieldUtils.getCurrentHubTranslation(), + Robot.ROBOT_EDITION == RobotEdition.ALPHA + ? AutoAim.ALPHA_HUB_SHOT_TREE + : AutoAim.COMP_HUB_SHOT_TREE) + .shotData() + .flywheelVelocityRotPerSec()), shooter.score( () -> NewAutoAim.getParametersMechA( diff --git a/src/main/java/frc/robot/components/rollers/RollerIO.java b/src/main/java/frc/robot/components/rollers/RollerIO.java index 4bf1607b..556d08fc 100644 --- a/src/main/java/frc/robot/components/rollers/RollerIO.java +++ b/src/main/java/frc/robot/components/rollers/RollerIO.java @@ -36,6 +36,8 @@ public static class RollerIOInputs { private final StatusSignal temperatureCelsius; private final StatusSignal positionRotations; + private double setpoint; + private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); private final VelocityVoltage velocityVoltage = new VelocityVoltage(0.0).withEnableFOC(true).withSlot(0); @@ -95,6 +97,11 @@ public void setRollerVoltage(double volts) { } public void setRollerVelocity(double velocityRPS) { + setpoint = velocityRPS; motor.setControl(velocityVoltage.withVelocity(velocityRPS)); } + + public double getVelocitySetpoint() { + return setpoint; + } } diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index 017b0c18..fb73240c 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Subsystem; +import java.util.function.DoubleSupplier; /** Add your docs here. */ public interface Indexer extends Subsystem { @@ -18,7 +19,7 @@ public interface Indexer extends Subsystem { public Command spit(); /** Run both indexer and kicker towards the shooter */ - public Command kick(); + public Command kick(DoubleSupplier flywheelRPS); /** Not running (set spinner to 0 but idle kicker) */ public Command rest(); @@ -26,4 +27,12 @@ public interface Indexer extends Subsystem { public default Command stop() { return Commands.none(); } + + public default Command runX60Sysid() { + return Commands.none(); + } + + public default Command runX44Sysid() { + return Commands.none(); + } } diff --git a/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java index c17d8d62..24aef0b3 100644 --- a/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java @@ -15,6 +15,7 @@ import frc.robot.components.canrange.CANrangeIOReal; import frc.robot.components.rollers.RollerIO; import frc.robot.components.rollers.RollerIOInputsAutoLogged; +import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; /** Lindexer = Linear Indexer. !! ALPHA !! */ @@ -61,7 +62,7 @@ public Command index() { } @Override - public Command kick() { + public Command kick(DoubleSupplier flywheelRPS) { return this.run( () -> { // if (shooterAtSetpoint.getAsBoolean()) { diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java index 5a4bbc1b..9b87bffc 100644 --- a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java @@ -1,5 +1,7 @@ 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; @@ -10,9 +12,15 @@ 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.Config; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; import frc.robot.components.rollers.RollerIO; import frc.robot.components.rollers.RollerIOInputsAutoLogged; +import frc.robot.subsystems.shooter.TurretSubsystem; import frc.robot.utils.LoggedTunableNumber; +import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -20,27 +28,36 @@ public class SpindexerSubsystem extends SubsystemBase implements Indexer { public static final double SPINNER_GEAR_RATIO = 67.0 / 15.0; - public static final double KICKER_GEAR_RATIO = 24.0 / 18.0; + public static final double X44_KICKER_GEAR_RATIO = 24.0 / 18.0; + public static final double X60_KICKER_GEAR_RATIO = 36.0 / 12.0; + // i don't really know if i should be using the sushi or the stealth wheels but the sushi wheels // are 1" in diameter and the stealth wheels are 3" in diameter - public static final double KICKER_DIAMETER_INCHES = 3; + public static final double X44_KICKER_DIAMETER_INCHES = 3; + public static final double X60_KICKER_DIAMETER_INCHES = 1; // biggest wheel (smallest wheel is 2") - public static final double SPINNER_DIAMETER_INCHES = 8; + public static final double SPINNER_DIAMETER_INCHES = 13.875; private RollerIO spinnerIO; private RollerIOInputsAutoLogged spinnerInputs = new RollerIOInputsAutoLogged(); - private RollerIO kickerIO; - private RollerIOInputsAutoLogged kickerInputs = new RollerIOInputsAutoLogged(); + private RollerIO x44KickerIO; + private RollerIOInputsAutoLogged x44kickerInputs = new RollerIOInputsAutoLogged(); + + private RollerIO x60KickerIO; + private RollerIOInputsAutoLogged x60kickerInputs = new RollerIOInputsAutoLogged(); public static final double MAX_ACCELERATION = 10.0; public static final double MAX_VELOCITY = 10.0; private final Alert spinnerDisconnectedAlert = new Alert("Disconnected spinner motor!", AlertType.kError); - private final Alert kickerDisconnectedAlert = - new Alert("Disconnected kicker motor!", AlertType.kError); + private final Alert x44KickerDisconnectedAlert = + new Alert("Disconnected x44 kicker motor!", AlertType.kError); + + private final Alert x60KickerDisconnectedAlert = + new Alert("Disconnected x60 kicker motor!", AlertType.kError); @AutoLogOutput(key = "Kicker/Current Filter Value") private double currentFilterValue = 0.0; @@ -49,12 +66,35 @@ public class SpindexerSubsystem extends SubsystemBase implements Indexer { public static final double KICKER_CURRENT_THRESHOLD = 20; // TODO - private LoggedTunableNumber kickerSpeed = new LoggedTunableNumber("Kicker Speed", 100); + private LoggedTunableNumber kickerSpeed = new LoggedTunableNumber("Kicker Speed", 70); private LoggedTunableNumber spinnerSpeed = new LoggedTunableNumber("Spinner Speed", 13); - public SpindexerSubsystem(CANBus canbus, RollerIO indexRollerIO, RollerIO kickerIO) { - this.kickerIO = kickerIO; + private LoggedTunableNumber x60KickSpeed = new LoggedTunableNumber("X60 Kick Speed", 20); + + private SysIdRoutine x60Sysid = + new SysIdRoutine( + new Config( + null, + null, + null, + (state) -> Logger.recordOutput("Indexer/Kicker X60/SysID State", state.toString())), + new Mechanism((voltage) -> x60KickerIO.setRollerVoltage(voltage.in(Volts)), null, this)); + + private SysIdRoutine x44Sysid = + new SysIdRoutine( + new Config( + null, + null, + null, + (state) -> Logger.recordOutput("Indexer/Kicker X44/SysID State", state.toString())), + new Mechanism((voltage) -> x44KickerIO.setRollerVoltage(voltage.in(Volts)), null, this)); + + public SpindexerSubsystem( + CANBus canbus, RollerIO indexRollerIO, RollerIO x44KickerIO, RollerIO x60KickerIO) { + this.x44KickerIO = x44KickerIO; + this.x60KickerIO = x60KickerIO; + this.spinnerIO = indexRollerIO; } @@ -63,23 +103,25 @@ public Command index() { return this.run( () -> { spinnerIO.setRollerVoltage(7); - kickerIO.setRollerVoltage(-7); + x44KickerIO.setRollerVoltage(-7); + x60KickerIO.setRollerVoltage(-7); }); } @Override - public Command kick() { + public Command kick(DoubleSupplier flywheelRPS) { return Commands.sequence( this.run( () -> { - // spinnerIO.setRollerVoltage(12); - // kickerIO.setRollerVoltage(12); - // }) - // .withTimeout(3), - // this.run( - // () -> { - spinnerIO.setRollerVelocity(spinnerSpeed.get()); - kickerIO.setRollerVelocity(kickerSpeed.get()); + double flywheelLinearSpeed = + flywheelRPS.getAsDouble() * Math.PI * TurretSubsystem.FLYWHEEL_DIAMETER_INCHES; + double x44Speed = flywheelLinearSpeed * 1 / (Math.PI * X44_KICKER_DIAMETER_INCHES); + double x60Speed = flywheelLinearSpeed * 0.9 / (Math.PI * X60_KICKER_DIAMETER_INCHES); + double spinnerSpeed = + flywheelLinearSpeed * 0.85 / (Math.PI * SPINNER_DIAMETER_INCHES); + spinnerIO.setRollerVelocity(spinnerSpeed); + x44KickerIO.setRollerVelocity(x44Speed); + x60KickerIO.setRollerVelocity(x60Speed); })); } @@ -88,7 +130,7 @@ public Command spit() { return this.run( () -> { spinnerIO.setRollerVoltage(-7); - kickerIO.setRollerVoltage(-7); + x44KickerIO.setRollerVoltage(-7); }); } @@ -97,7 +139,8 @@ public Command rest() { return this.run( () -> { spinnerIO.setRollerVoltage(0.0); - kickerIO.setRollerVoltage(0.0); + x44KickerIO.setRollerVoltage(0.0); + x60KickerIO.setRollerVoltage(0.0); }); } @@ -106,7 +149,7 @@ public Command stop() { return this.run( () -> { spinnerIO.setRollerVoltage(0.0); - kickerIO.setRollerVoltage(0.0); + x44KickerIO.setRollerVoltage(0.0); }); } @@ -114,12 +157,19 @@ public Command stop() { public void periodic() { spinnerIO.updateInputs(spinnerInputs); Logger.processInputs("Indexer/Spinner", spinnerInputs); - kickerIO.updateInputs(kickerInputs); - Logger.processInputs("Indexer/Kicker", kickerInputs); + x44KickerIO.updateInputs(x44kickerInputs); + Logger.processInputs("Indexer/Kicker X44", x44kickerInputs); + x60KickerIO.updateInputs(x60kickerInputs); + Logger.processInputs("Indexer/Kicker X60", x60kickerInputs); spinnerDisconnectedAlert.set(!spinnerInputs.connected); - kickerDisconnectedAlert.set(!kickerInputs.connected); + x44KickerDisconnectedAlert.set(!x44kickerInputs.connected); + x60KickerDisconnectedAlert.set(!x60kickerInputs.connected); - currentFilterValue = kickerCurrentFilter.calculate(kickerInputs.statorCurrentAmps); + currentFilterValue = kickerCurrentFilter.calculate(x44kickerInputs.statorCurrentAmps); + + Logger.recordOutput("Indexer/Kicker X44 Velocity Setpoint", x44KickerIO.getVelocitySetpoint()); + Logger.recordOutput("Indexer/Kicker X60 Velocity Setpoint", x60KickerIO.getVelocitySetpoint()); + Logger.recordOutput("Indexer/Spinner Velocity Setpoint", spinnerIO.getVelocitySetpoint()); } public static TalonFXConfiguration getIndexerConfig() { @@ -147,7 +197,7 @@ public static TalonFXConfiguration getIndexerConfig() { return config; } - public static TalonFXConfiguration getKickerConfig() { + public static TalonFXConfiguration getX44KickerConfig() { TalonFXConfiguration config = new TalonFXConfiguration(); config.MotorOutput.NeutralMode = NeutralModeValue.Brake; @@ -155,12 +205,12 @@ public static TalonFXConfiguration getKickerConfig() { config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; // Converts angular motion to linear motion - config.Feedback.SensorToMechanismRatio = KICKER_GEAR_RATIO; + config.Feedback.SensorToMechanismRatio = X44_KICKER_GEAR_RATIO; - config.Slot0.kS = 0.22251; - config.Slot0.kV = 0.17199; - config.Slot0.kA = 0.024802; - config.Slot0.kP = 21; + config.Slot0.kS = 0.50822; + config.Slot0.kV = 0.13022; + config.Slot0.kA = 0.0051671; + config.Slot0.kP = 0.5; config.Slot0.kD = 0; config.CurrentLimits.StatorCurrentLimit = 80.0; @@ -173,7 +223,50 @@ public static TalonFXConfiguration getKickerConfig() { return config; } + public static TalonFXConfiguration getX60KickerConfig() { + TalonFXConfiguration config = new TalonFXConfiguration(); + + config.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + // Converts angular motion to linear motion + config.Feedback.SensorToMechanismRatio = X60_KICKER_GEAR_RATIO; + + config.Slot0.kS = 0.22539; + config.Slot0.kV = 0.35529; + config.Slot0.kA = 0.0078104; + config.Slot0.kP = 0.5; + + config.CurrentLimits.StatorCurrentLimit = 80.0; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.SupplyCurrentLimit = 40.0; + config.CurrentLimits.SupplyCurrentLimitEnable = true; + config.CurrentLimits.SupplyCurrentLowerLimit = 40.0; + config.CurrentLimits.SupplyCurrentLowerTime = 0.25; + + return config; + } + public boolean isEmpty() { return currentFilterValue < KICKER_CURRENT_THRESHOLD; } + + @Override + public Command runX60Sysid() { + return Commands.sequence( + x60Sysid.quasistatic(Direction.kForward), + x60Sysid.quasistatic(Direction.kReverse), + x60Sysid.dynamic(Direction.kForward), + x60Sysid.dynamic(Direction.kReverse)); + } + + @Override + public Command runX44Sysid() { + return Commands.sequence( + x44Sysid.quasistatic(Direction.kForward), + x44Sysid.quasistatic(Direction.kReverse), + x44Sysid.dynamic(Direction.kForward), + x44Sysid.dynamic(Direction.kReverse)); + } }