diff --git a/.vscode/settings.json b/.vscode/settings.json index 7399d64..50e3d8c 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -59,5 +59,6 @@ ], "java.dependency.enableDependencyCheckup": false, "wpilib.selectDefaultSimulateExtension": false, - "wpilib.skipSelectSimulateExtension": true + "wpilib.skipSelectSimulateExtension": true, + "wpilib.autoStartRioLog": false } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 036f351..10928ec 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -316,6 +316,8 @@ public Robot() { .alongWith(leds.blinkCmd(Color.kWhite, Color.kBlack, 20.0).withTimeout(1.0)) .ignoringDisable(true)); SmartDashboard.putData("Add autos", Commands.runOnce(this::addAutos).ignoringDisable(true)); + SmartDashboard.putData("Zero hood", shooter.zeroHood().ignoringDisable(true)); + SmartDashboard.putData("Test Shot", shooter.testShoot()); // Reset alert timers canInitialErrorTimer.restart(); diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 9be6815..f4e1895 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -67,8 +67,8 @@ public Trigger getTrigger() { @AutoLogOutput(key = "Superstructure/Feed Request") private Trigger feedReq; - @AutoLogOutput(key = "Superstructure/Flowstate Request") - private Trigger flowReq; + // @AutoLogOutput(key = "Superstructure/Flowstate Request") + // private Trigger flowReq; @AutoLogOutput(key = "Superstructure/Anti Jam Req") private Trigger antiJamReq; @@ -127,65 +127,81 @@ private void addTriggers() { .and(() -> shouldFeed == true) .or(Autos.autoFeedReq); - flowReq = driver.leftTrigger().and(driver.rightTrigger()); + // flowReq = driver.leftTrigger().and(driver.rightTrigger()); antiJamReq = driver.a().or(operator.a()); - isFull = new Trigger(indexer::isFull); + isFull = new Trigger(indexer::isFull).debounce(0.5); // TODO tune isEmpty = new Trigger(indexer::isEmpty); } private void addTransitions() { - bindTransition(SuperState.IDLE, SuperState.INTAKE, intakeReq); + bindTransition(SuperState.IDLE, SuperState.INTAKE, intakeReq.and(scoreReq.negate())); bindTransition(SuperState.INTAKE, SuperState.IDLE, intakeReq.negate().and(isEmpty)); bindTransition( - SuperState.INTAKE, SuperState.READY, (intakeReq.negate().and(isEmpty.negate())).or(isFull)); + SuperState.INTAKE, + SuperState.READY, + (intakeReq.negate().and(scoreReq.negate()).and(isEmpty.negate()))); + // .or(isFull)); - bindTransition(SuperState.INTAKE, SuperState.SPIN_UP_FEED, feedReq); + // bindTransition(SuperState.INTAKE, SuperState.SPIN_UP_FEED, feedReq); - bindTransition(SuperState.READY, SuperState.INTAKE, intakeReq.and(isFull.negate())); + bindTransition(SuperState.READY, SuperState.INTAKE, intakeReq); + // .and(isFull.negate())); bindTransition(SuperState.READY, SuperState.SPIN_UP_SCORE, scoreReq); bindTransition( SuperState.SPIN_UP_SCORE, SuperState.SCORE, - new Trigger(shooter::atFlywheelVelocitySetpoint)); + new Trigger(shooter::atFlywheelVelocitySetpoint) + .debounce(0.5) + .and(new Trigger(shooter::atHoodSetpoint).debounce(0.5)) + .and(() -> stateTimer.hasElapsed(0.5))); - bindTransition( - SuperState.SPIN_UP_FEED, SuperState.FEED, new Trigger(shooter::atFlywheelVelocitySetpoint)); + // bindTransition( + // SuperState.SPIN_UP_FEED, + // SuperState.FEED, + // new Trigger(shooter::atFlywheelVelocitySetpoint) + // .and(() -> stateTimer.hasElapsed(0.2)) + // .and(shooter::atHoodSetpoint)); - bindTransition(SuperState.FEED, SuperState.IDLE, isEmpty); + // bindTransition(SuperState.FEED, SuperState.IDLE, isEmpty); - bindTransition(SuperState.SCORE, SuperState.IDLE, isEmpty); + bindTransition(SuperState.SCORE, SuperState.IDLE, isEmpty.debounce(0.5).and(scoreReq.negate())); // FEED_FLOW transitions - { - bindTransition(SuperState.FEED, SuperState.FEED_FLOW, flowReq); + // { + // bindTransition(SuperState.FEED, SuperState.FEED_FLOW, intakeReq.and(feedReq)); - bindTransition(SuperState.FEED_FLOW, SuperState.FEED, flowReq.negate().and(feedReq)); + // bindTransition(SuperState.FEED_FLOW, SuperState.FEED, intakeReq.negate().and(feedReq)); - bindTransition( - SuperState.FEED_FLOW, SuperState.READY, flowReq.negate().and(isEmpty.negate())); + // bindTransition( + // SuperState.FEED_FLOW, SuperState.READY, flowReq.negate().and(isEmpty.negate())); - // No so sure about the end condition here. - bindTransition(SuperState.FEED_FLOW, SuperState.IDLE, flowReq.negate().and(isEmpty)); - } + // // No so sure about the end condition here. + // bindTransition(SuperState.FEED_FLOW, SuperState.IDLE, flowReq.negate().and(isEmpty)); + // } // SCORE_FLOW transitions { - bindTransition(SuperState.SCORE, SuperState.SCORE_FLOW, flowReq); + bindTransition(SuperState.SCORE, SuperState.SCORE_FLOW, scoreReq.and(intakeReq)); - bindTransition(SuperState.SCORE_FLOW, SuperState.SCORE, flowReq.negate().and(scoreReq)); + bindTransition(SuperState.SCORE_FLOW, SuperState.SCORE, intakeReq.negate().and(scoreReq)); bindTransition( - SuperState.SCORE_FLOW, SuperState.READY, flowReq.negate().and(isEmpty.negate())); + SuperState.SCORE_FLOW, + SuperState.READY, + intakeReq.negate().and(scoreReq.negate()).and(isEmpty.negate())); // No so sure about the end condition here. - bindTransition(SuperState.SCORE_FLOW, SuperState.IDLE, flowReq.negate().and(isEmpty)); + bindTransition( + SuperState.SCORE_FLOW, + SuperState.IDLE, + intakeReq.negate().and(scoreReq.negate()).and(isEmpty)); } // Transition from any state to SPIT for anti jamming @@ -206,11 +222,14 @@ private void addCommands() { bindCommands( SuperState.READY, intake.rest(), - indexer.index(), + indexer.rest(), shooter.rest()); // Maybe index at slower speed? bindCommands( - SuperState.SPIN_UP_SCORE, intake.rest(), indexer.rest(), shooter.shoot(swerve::getPose)); + SuperState.SPIN_UP_SCORE, + intake.rest(), + indexer.rest(), /*shooter.shoot(swerve::getPose)*/ + shooter.testShoot()); bindCommands( SuperState.SPIN_UP_FEED, @@ -220,10 +239,12 @@ private void addCommands() { swerve::getPose, () -> FeedTargets.BLUE_BACK_RIGHT.getPose())); // TODO: SELECTION LOGIC bindCommands( - SuperState.SCORE, intake.rest(), indexer.indexToShoot(), shooter.shoot(swerve::getPose)); + SuperState.SCORE, + intake.rest(), + indexer.kick(), /*shooter.shoot(swerve::getPose)*/ + shooter.testShoot()); - bindCommands( - SuperState.SCORE_FLOW, intake.intake(), indexer.index(), shooter.shoot(swerve::getPose)); + bindCommands(SuperState.SCORE_FLOW, intake.intake(), indexer.kick(), shooter.testShoot()); bindCommands( SuperState.FEED, @@ -239,7 +260,7 @@ private void addCommands() { indexer.index(), shooter.feed(swerve::getPose, () -> FeedTargets.BLUE_BACK_RIGHT.getPose())); - bindCommands(SuperState.SPIT, intake.outake(), indexer.outtake(), shooter.spit()); + bindCommands(SuperState.SPIT, intake.outake(), indexer.spit(), shooter.spit()); } public void periodic() { diff --git a/src/main/java/frc/robot/components/canrange/CANrangeIOReal.java b/src/main/java/frc/robot/components/canrange/CANrangeIOReal.java index 0548179..343ed4f 100644 --- a/src/main/java/frc/robot/components/canrange/CANrangeIOReal.java +++ b/src/main/java/frc/robot/components/canrange/CANrangeIOReal.java @@ -17,14 +17,15 @@ public class CANrangeIOReal implements CANrangeIO { private final StatusSignal distance; private final StatusSignal isDetected; - public CANrangeIOReal(int CANrangeID, CANBus canbus) { + public CANrangeIOReal(int CANrangeID, CANBus canbus, double xFovDegrees) { canrange = new CANrange(CANrangeID, canbus); distance = canrange.getDistance(); isDetected = canrange.getIsDetected(); final CANrangeConfiguration config = new CANrangeConfiguration(); config.ToFParams.UpdateFrequency = 50; // update frequency in Hz - config.ProximityParams.ProximityThreshold = 0.05; + config.ProximityParams.ProximityThreshold = 0.254; + config.FovParams.FOVRangeX = xFovDegrees; BaseStatusSignal.setUpdateFrequencyForAll(50.0, distance, isDetected); diff --git a/src/main/java/frc/robot/components/rollers/RollerIO.java b/src/main/java/frc/robot/components/rollers/RollerIO.java index 1575259..46cea56 100644 --- a/src/main/java/frc/robot/components/rollers/RollerIO.java +++ b/src/main/java/frc/robot/components/rollers/RollerIO.java @@ -7,6 +7,7 @@ import com.ctre.phoenix6.controls.VelocityVoltage; 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; @@ -23,6 +24,7 @@ public static class RollerIOInputs { public double appliedVoltage = 0.0; public double statorCurrentAmps = 0.0; public double motorTemperatureCelsius = 0.0; + public double motorPositionRotations = 0.0; } protected final TalonFX motor; @@ -32,6 +34,7 @@ public static class RollerIOInputs { private final StatusSignal appliedVoltage; private final StatusSignal statorCurrentAmps; private final StatusSignal motorTemperatureCelsius; + private final StatusSignal motorPosition; private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); private final VelocityVoltage velocityVoltage = @@ -47,6 +50,7 @@ public RollerIO(int motorID, TalonFXConfiguration config, CANBus canbus) { appliedVoltage = motor.getMotorVoltage(); statorCurrentAmps = motor.getStatorCurrent(); motorTemperatureCelsius = motor.getDeviceTemp(); + motorPosition = motor.getPosition(); BaseStatusSignal.setUpdateFrequencyForAll( 50.0, @@ -54,7 +58,8 @@ public RollerIO(int motorID, TalonFXConfiguration config, CANBus canbus) { supplyCurrentAmps, statorCurrentAmps, appliedVoltage, - motorTemperatureCelsius); + motorTemperatureCelsius, + motorPosition); motor.getConfigurator().apply(config); motor.optimizeBusUtilization(); @@ -66,13 +71,15 @@ public void updateInputs(RollerIOInputs inputs) { supplyCurrentAmps, appliedVoltage, statorCurrentAmps, - motorTemperatureCelsius); + motorTemperatureCelsius, + motorPosition); inputs.velocityRotsPerSec = angularVelocityRotsPerSec.getValueAsDouble(); inputs.supplyCurrentAmps = supplyCurrentAmps.getValueAsDouble(); inputs.appliedVoltage = appliedVoltage.getValueAsDouble(); inputs.statorCurrentAmps = statorCurrentAmps.getValueAsDouble(); inputs.motorTemperatureCelsius = motorTemperatureCelsius.getValueAsDouble(); + inputs.motorPositionRotations = motorPosition.getValueAsDouble(); } public void setRollerVoltage(double volts) { diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java index af761a4..1e8eebf 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java @@ -39,7 +39,7 @@ public class IndexerSubsystem extends SubsystemBase { null, null, null, - (state) -> Logger.recordOutput("Indexer/Roller/SysID State", state)), + (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; @@ -48,8 +48,8 @@ public class IndexerSubsystem extends SubsystemBase { public IndexerSubsystem(CANBus canbus, RollerIO indexRollerIO, RollerIO kickerIO) { this.kickerIO = kickerIO; - firstCANRangeIO = new CANrangeIOReal(0, canbus); - secondCANRangeIO = new CANrangeIOReal(1, canbus); + firstCANRangeIO = new CANrangeIOReal(0, canbus, 10); + secondCANRangeIO = new CANrangeIOReal(1, canbus, 10); this.indexRollerIO = indexRollerIO; } @@ -77,20 +77,20 @@ public boolean isPartiallyFull() { public Command index() { return this.run( () -> { - indexRollerIO.setRollerVoltage(5); - kickerIO.setRollerVoltage(-5); + indexRollerIO.setRollerVoltage(7); + kickerIO.setRollerVoltage(7); }); } - public Command indexToShoot() { + public Command kick() { return this.run( () -> { - indexRollerIO.setRollerVoltage(10); - kickerIO.setRollerVoltage(5); + indexRollerIO.setRollerVoltage(12); + kickerIO.setRollerVoltage(-7); }); } - public Command outtake() { + public Command spit() { return this.run( () -> { indexRollerIO.setRollerVoltage(-5); @@ -111,7 +111,7 @@ public static TalonFXConfiguration getIndexerConfigs() { config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; config.Feedback.SensorToMechanismRatio = GEAR_RATIO; @@ -136,7 +136,7 @@ public static TalonFXConfiguration getKickerConfigs() { config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; // Converts angular motion to linear motion config.Feedback.SensorToMechanismRatio = KICKER_GEAR_RATIO; @@ -166,7 +166,7 @@ public void periodic() { indexRollerIO.updateInputs(rollerInputs); Logger.processInputs("Indexer/Roller", rollerInputs); kickerIO.updateInputs(kickerInputs); - Logger.processInputs("Intake/Kicker", kickerInputs); + Logger.processInputs("Indexer/Kicker", kickerInputs); } public Command runRollerSysId() { diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index 89ab34d..f2e00f4 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -22,7 +22,11 @@ public class IntakeSubsystem extends SubsystemBase { private SysIdRoutine intakeRollerSysid = new SysIdRoutine( - new Config(null, null, null, (state) -> Logger.recordOutput("Intake/SysID State", state)), + new Config( + null, + null, + null, + (state) -> Logger.recordOutput("Intake/SysID State", state.toString())), new Mechanism((volts) -> io.setRollerVoltage(volts.in(Volts)), null, this)); public IntakeSubsystem(RollerIO io) { @@ -34,13 +38,12 @@ public void periodic() { Logger.processInputs("Intake", inputs); } - // TODO get actual values public Command intake() { - return this.run(() -> io.setRollerVoltage(5)); + return this.run(() -> io.setRollerVoltage(10)); } public Command outake() { - return this.run(() -> io.setRollerVoltage(-2)); + return this.run(() -> io.setRollerVoltage(-5)); } public Command rest() { @@ -62,14 +65,13 @@ public static TalonFXConfiguration getIntakeConfig() { config.Feedback.SensorToMechanismRatio = GEAR_RATIO; - config.Slot0.kS = 0.24; - config.Slot0.kV = 0.6; - config.Slot0.kP = 110.0; - config.Slot0.kD = 0.0; + config.Slot0.kS = 0.42; + config.Slot0.kV = 0.21; + config.Slot0.kA = 0.00347; - config.CurrentLimits.StatorCurrentLimit = 80.0; + config.CurrentLimits.StatorCurrentLimit = 40.0; config.CurrentLimits.StatorCurrentLimitEnable = true; - config.CurrentLimits.SupplyCurrentLimit = 60.0; + config.CurrentLimits.SupplyCurrentLimit = 40.0; config.CurrentLimits.SupplyCurrentLimitEnable = true; return config; diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java index ac355ed..304e65b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java @@ -16,10 +16,12 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.MotorAlignmentValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.units.measure.Angle; 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; /** Add your docs here. */ public class FlywheelIO { @@ -31,6 +33,7 @@ public static class FlywheelIOInputs { public double flywheelLeaderSupplyCurrentAmp = 0.0; public double flywheelLeaderVoltage = 0.0; public double flywheelLeaderTempC = 0.0; + public double flywheelLeaderPosition = 0.0; public double flywheelFollowerVelocityRotationsPerSecond = 0.0; public double flywheelFollowerStatorCurrentAmps = 0.0; @@ -47,6 +50,7 @@ public static class FlywheelIOInputs { private final StatusSignal flywheelLeaderStatorCurrent; private final StatusSignal flywheelLeaderSupplyCurrent; private final StatusSignal flywheelLeaderTemp; + private final StatusSignal flywheelLeaderPosition; private final BaseStatusSignal flywheelFollowerVelocity; private final StatusSignal flywheelFollowerVoltage; @@ -56,8 +60,7 @@ public static class FlywheelIOInputs { private VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); private VelocityVoltage velocityVoltage = new VelocityVoltage(0.0).withEnableFOC(true); - private MotionMagicVelocityVoltage motionMagicVelocityVoltage = - new MotionMagicVelocityVoltage(0.0).withEnableFOC(true).withAcceleration(100); + private MotionMagicVelocityVoltage motionMagicVelocityVoltage; private double velocitySetpointRotPerSec = 0.0; @@ -79,6 +82,7 @@ public FlywheelIO(TalonFXConfiguration config, CANBus canbus) { flywheelLeaderStatorCurrent = flywheelLeader.getStatorCurrent(); flywheelLeaderSupplyCurrent = flywheelLeader.getSupplyCurrent(); flywheelLeaderTemp = flywheelLeader.getDeviceTemp(); + flywheelLeaderPosition = flywheelLeader.getPosition(); flywheelFollowerVelocity = flywheelFollower.getVelocity(); flywheelFollowerVoltage = flywheelFollower.getMotorVoltage(); @@ -86,6 +90,11 @@ public FlywheelIO(TalonFXConfiguration config, CANBus canbus) { flywheelFollowerSupplyCurrent = flywheelFollower.getSupplyCurrent(); flywheelFollowerTemp = flywheelFollower.getDeviceTemp(); + motionMagicVelocityVoltage = + new MotionMagicVelocityVoltage(0.0) + .withAcceleration(config.MotionMagic.MotionMagicAcceleration) + .withEnableFOC(true); + BaseStatusSignal.setUpdateFrequencyForAll( 50.0, flywheelLeader.getVelocity(), @@ -97,7 +106,8 @@ public FlywheelIO(TalonFXConfiguration config, CANBus canbus) { flywheelFollower.getMotorVoltage(), flywheelFollower.getStatorCurrent(), flywheelFollower.getSupplyCurrent(), - flywheelFollower.getDeviceTemp()); + flywheelFollower.getDeviceTemp(), + flywheelLeaderPosition); flywheelLeader.optimizeBusUtilization(); flywheelFollower.optimizeBusUtilization(); @@ -111,15 +121,18 @@ public static TalonFXConfiguration getFlywheelConfiguration() { config.Feedback.SensorToMechanismRatio = ShooterSubsystem.FLYWHEEL_GEAR_RATIO; - config.Slot0.kS = 0.0; - config.Slot0.kV = 0.15; // From sim - config.Slot0.kP = 10.0; + config.Slot0.kS = 0.43477; + config.Slot0.kV = 0.144; + config.Slot0.kA = 0.016433; + config.Slot0.kP = 0.1; config.Slot0.kD = 0.0; config.CurrentLimits.StatorCurrentLimit = 120.0; config.CurrentLimits.StatorCurrentLimitEnable = true; config.CurrentLimits.SupplyCurrentLimit = 80.0; + config.MotionMagic.MotionMagicAcceleration = 100.0; + return config; } @@ -148,7 +161,8 @@ public void updateInputs(FlywheelIOInputs inputs) { flywheelFollowerVoltage, flywheelFollowerStatorCurrent, flywheelFollowerSupplyCurrent, - flywheelFollowerTemp); + flywheelFollowerTemp, + flywheelLeaderPosition); inputs.flywheelLeaderVelocityRotationsPerSecond = flywheelLeaderVelocity.getValueAsDouble(); inputs.flywheelLeaderVoltage = flywheelLeaderVoltage.getValueAsDouble(); @@ -156,6 +170,8 @@ public void updateInputs(FlywheelIOInputs inputs) { inputs.flywheelLeaderSupplyCurrentAmp = flywheelLeaderSupplyCurrent.getValueAsDouble(); inputs.flywheelLeaderTempC = flywheelLeaderTemp.getValueAsDouble(); + inputs.flywheelLeaderPosition = flywheelLeaderPosition.getValueAsDouble(); + inputs.flywheelFollowerVelocityRotationsPerSecond = flywheelFollowerVelocity.getValueAsDouble(); inputs.flywheelFollowerVoltage = flywheelFollowerVoltage.getValueAsDouble(); inputs.flywheelFollowerStatorCurrentAmps = flywheelFollowerStatorCurrent.getValueAsDouble(); @@ -163,6 +179,7 @@ public void updateInputs(FlywheelIOInputs inputs) { inputs.flywheelFollowerTempC = flywheelFollowerTemp.getValueAsDouble(); } + @AutoLogOutput(key = "Shooter/Setpoint") public double getSetpointRotPerSec() { return velocitySetpointRotPerSec; } diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java index e852e44..3c38920 100644 --- a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java @@ -14,13 +14,16 @@ import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.GravityTypeValue; 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.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 HoodIO { /** Creates a new HoodIOReal. */ @@ -32,11 +35,13 @@ public static class HoodIOInputs { public double hoodSupplyCurrentAmp = 0.0; public double hoodVoltage = 0.0; public double hoodTempC = 0.0; + // For sysid + public double hoodRotations = 0.0; } protected TalonFX hoodMotor; - private final BaseStatusSignal hoodPositionRotations; + private final StatusSignal hoodPositionRotations; private final BaseStatusSignal hoodAngularVelocity; private final StatusSignal hoodVoltage; private final StatusSignal hoodStatorCurrent; @@ -46,6 +51,8 @@ public static class HoodIOInputs { private PositionVoltage positionVoltage = new PositionVoltage(0.0).withEnableFOC(true); private VelocityVoltage velocityVoltage = new VelocityVoltage(0.0).withEnableFOC(true); + private Rotation2d hoodSetpoint = Rotation2d.kZero; + public HoodIO(TalonFXConfiguration talonFXConfiguration, CANBus canbus) { hoodMotor = new TalonFX(11, canbus); hoodMotor.getConfigurator().apply(HoodIO.getHoodConfiguration()); @@ -81,13 +88,13 @@ public static TalonFXConfiguration getHoodConfiguration() { config.Feedback.SensorToMechanismRatio = ShooterSubsystem.HOOD_GEAR_RATIO; - // config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; Potentially need, maybe not tho. + config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; - config.Slot0.kS = 0.0; - config.Slot0.kG = 0.0; - config.Slot0.kV = 1.1; - config.Slot0.kP = 5.0; - config.Slot0.kD = 0.0; + config.Slot0.kS = 0.055; + config.Slot0.kG = 0.445; + config.Slot0.kV = 1.45; + config.Slot0.kP = 35; + config.Slot0.kD = 0.25; config.CurrentLimits.StatorCurrentLimit = 80.0; config.CurrentLimits.StatorCurrentLimitEnable = true; @@ -101,6 +108,7 @@ public void setHoodVoltage(double hoodVoltage) { } public void setHoodPosition(Rotation2d hoodPosition) { + hoodSetpoint = hoodPosition; hoodMotor.setControl(positionVoltage.withPosition(hoodPosition.getRotations())); } @@ -117,11 +125,22 @@ public void updateInputs(HoodIOInputs inputs) { hoodSupplyCurrent, hoodTemp); - inputs.hoodPositionRotations = Rotation2d.fromRadians(hoodPositionRotations.getValueAsDouble()); + inputs.hoodPositionRotations = + Rotation2d.fromRotations(hoodPositionRotations.getValueAsDouble()); inputs.hoodAngularVelocity = hoodAngularVelocity.getValueAsDouble(); inputs.hoodVoltage = hoodVoltage.getValueAsDouble(); inputs.hoodStatorCurrentAmps = hoodStatorCurrent.getValueAsDouble(); inputs.hoodSupplyCurrentAmp = hoodSupplyCurrent.getValueAsDouble(); inputs.hoodTempC = hoodTemp.getValueAsDouble(); + inputs.hoodRotations = hoodPositionRotations.getValueAsDouble(); + } + + @AutoLogOutput(key = "Shooter/Hood/Setpoint") + public Rotation2d getHoodSetpoint() { + return hoodSetpoint; + } + + public void resetEncoder(Rotation2d rotations) { + hoodMotor.setPosition(rotations.getRotations()); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 9cfde0a..7a11881 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -16,19 +16,21 @@ 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.utils.LoggedTunableNumber; import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; import java.util.function.Supplier; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class ShooterSubsystem extends SubsystemBase { - public static double HOOD_GEAR_RATIO = 147.0 / 13.0; + public static double HOOD_GEAR_RATIO = 24.230769; public static Rotation2d HOOD_MAX_ROTATION = Rotation2d.fromDegrees(40); - public static Rotation2d HOOD_MIN_ROTATION = Rotation2d.fromDegrees(0); + public static Rotation2d HOOD_MIN_ROTATION = Rotation2d.fromDegrees(2); public static double FLYWHEEL_GEAR_RATIO = 28.0 / 24.0; - public static double FLYWHEEL_VELOCITY_TOLERANCE_ROTATIONS_PER_SECOND = 5.0; // TODO: TUNE + public static double FLYWHEEL_VELOCITY_TOLERANCE_ROTATIONS_PER_SECOND = 5.0; HoodIO hoodIO; HoodIOInputsAutoLogged hoodInputs = new HoodIOInputsAutoLogged(); @@ -39,7 +41,10 @@ public class ShooterSubsystem extends SubsystemBase { private SysIdRoutine hoodSysid = new SysIdRoutine( new Config( - null, null, null, (state) -> Logger.recordOutput("Shooter/Hood/SysID State", state)), + null, + null, + null, + (state) -> Logger.recordOutput("Shooter/Hood/SysID State", state.toString())), new Mechanism((voltage) -> hoodIO.setHoodVoltage(voltage.in(Volts)), null, this)); private SysIdRoutine flywheelSysid = @@ -48,15 +53,26 @@ public class ShooterSubsystem extends SubsystemBase { null, null, null, - (state) -> Logger.recordOutput("Shooter/Flywheel/SysID State", state)), + (state) -> Logger.recordOutput("Shooter/Flywheel/SysID State", state.toString())), new Mechanism((voltage) -> flywheelIO.setFlywheelVoltage(voltage.in(Volts)), null, this)); + private LoggedTunableNumber testDegrees = new LoggedTunableNumber("Shooter/Test Degrees", 10.0); + private LoggedTunableNumber testVelocity = new LoggedTunableNumber("Shooter/Test Velocity", 30.0); + /** Creates a new HoodSubsystem. */ public ShooterSubsystem(HoodIO hoodIO, FlywheelIO flywheelIO) { this.hoodIO = hoodIO; this.flywheelIO = flywheelIO; } + public Command testShoot() { + return this.run( + () -> { + hoodIO.setHoodPosition(Rotation2d.fromDegrees(testDegrees.get())); + flywheelIO.setMotionProfiledFlywheelVelocity(testVelocity.get()); + }); + } + public Command shoot(Supplier robotPoseSupplier) { return this.run( () -> { @@ -84,7 +100,7 @@ public Command feed(Supplier robotPoseSupplier, Supplier feedTar public Command rest() { return this.run( () -> { - hoodIO.setHoodPosition(Rotation2d.kZero); // TODO: TUNE TUCKED POSITION IF NEEDED + hoodIO.setHoodPosition(HOOD_MIN_ROTATION); // TODO: TUNE TUCKED POSITION IF NEEDED flywheelIO.setFlywheelVoltage(0.0); }); } @@ -146,10 +162,21 @@ public Command runFlywheelSysid() { flywheelSysid.dynamic(Direction.kReverse)); } + @AutoLogOutput(key = "Shooter/At Vel Setpoint") public boolean atFlywheelVelocitySetpoint() { return MathUtil.isNear( flywheelInputs.flywheelLeaderVelocityRotationsPerSecond, flywheelIO.getSetpointRotPerSec(), FLYWHEEL_VELOCITY_TOLERANCE_ROTATIONS_PER_SECOND); } + + @AutoLogOutput(key = "Shooter/Hood/At Setpoint") + public boolean atHoodSetpoint() { + return MathUtil.isNear( + hoodInputs.hoodPositionRotations.getDegrees(), hoodIO.getHoodSetpoint().getDegrees(), 1); + } + + public Command zeroHood() { + return this.runOnce(() -> hoodIO.resetEncoder(HOOD_MIN_ROTATION)); + } } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 220fa62..424a206 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -263,6 +263,8 @@ public void periodic() { Tracer.trace("Update odometry", this::updateOdometry); Tracer.trace("Update vision", this::updateVision); + + Logger.recordOutput("Current Hub Pose", FieldUtils.getCurrentHubPose()); }); } @@ -586,8 +588,11 @@ public Command faceHub(DoubleSupplier xVel, DoubleSupplier yVel) { () -> { Translation2d robotHubVec = FieldUtils.getCurrentHubTranslation().minus(getPose().getTranslation()); + // return FieldUtils.getCurrentHubPose().minus(getPose()).getRotation(); + // Logger.recordOutput("robot hub vec", robotHubVec); // atan2 takes y as the first arg (i think bc θ = atan(y/x) but idk) - return Rotation2d.fromRadians(Math.atan2(robotHubVec.getY(), robotHubVec.getX())); + return Rotation2d.fromRadians(Math.atan2(robotHubVec.getY(), robotHubVec.getX())) + .plus(Rotation2d.kCW_90deg); }, xVel, yVel); diff --git a/src/main/java/frc/robot/subsystems/swerve/constants/AlphaSwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/constants/AlphaSwerveConstants.java index ce122de..78470e3 100644 --- a/src/main/java/frc/robot/subsystems/swerve/constants/AlphaSwerveConstants.java +++ b/src/main/java/frc/robot/subsystems/swerve/constants/AlphaSwerveConstants.java @@ -181,27 +181,23 @@ public Mass getMass() { @Override public ModuleConstants getFrontLeftModuleConstants() { - // TODO update cancoder rotation2d return new ModuleConstants( 0, "Front Left", 0, 1, 0, Rotation2d.fromRotations(-0.29).plus(Rotation2d.k180deg)); } @Override public ModuleConstants getFrontRightModuleConstants() { - // TODO update cancoder rotation2d return new ModuleConstants(1, "Front Right", 2, 3, 1, Rotation2d.fromRotations(0.012)); } @Override public ModuleConstants getBackLeftModuleConstants() { - // TODO update cancoder rotation2d return new ModuleConstants( 2, "Back Left", 4, 5, 2, Rotation2d.fromRotations(0.229).plus(Rotation2d.k180deg)); } @Override public ModuleConstants getBackRightModuleConstants() { - // TODO update cancoder rotation2d return new ModuleConstants(3, "Back Right", 6, 7, 3, Rotation2d.fromRotations(-0.205)); } @@ -212,7 +208,6 @@ public int getGyroID() { @Override public Pigeon2Configuration getGyroConfig() { - // TODO getGyroConfig Pigeon2Configuration config = new Pigeon2Configuration(); config.MountPose.MountPosePitch = 0.18661323189735413; config.MountPose.MountPoseRoll = -0.706454336643219; @@ -222,7 +217,6 @@ public Pigeon2Configuration getGyroConfig() { @Override public TalonFXConfiguration getDriveConfig() { - // TODO getDriveConfig var driveConfig = new TalonFXConfiguration(); // Current limits driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0; @@ -255,7 +249,6 @@ public TalonFXConfiguration getDriveConfig() { @Override public TalonFXConfiguration getTurnConfig(int cancoderID) { - // TODO getTurnConfig var turnConfig = new TalonFXConfiguration(); // Current limits turnConfig.CurrentLimits.SupplyCurrentLimit = 20.0; diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 1426b50..d1f27a5 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -2,19 +2,37 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; import frc.robot.utils.FieldUtils; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; +import org.littletonrobotics.junction.Logger; public class AutoAim { public static final InterpolatingShotTree HUB_SHOT_TREE = new InterpolatingShotTree(); - // If we need other shot trees (i.e. for feeding) we can put them here - static { // For hub shot tree - // TODO: ADD SHOTS TO HUB SHOT HERE HUB_SHOT_TREE.put( - 1.0, new ShotData(Rotation2d.kCW_90deg, 10, 0.5)); // Placeholder to prevent crashes + Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 12), + new ShotData(Rotation2d.fromDegrees(6), 30)); + HUB_SHOT_TREE.put( + Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 3 * 12), + new ShotData(Rotation2d.fromDegrees(10.5), 30)); + HUB_SHOT_TREE.put( + Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 5 * 12), + new ShotData(Rotation2d.fromDegrees(14.5), 30)); + HUB_SHOT_TREE.put( + Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 7 * 12), + new ShotData(Rotation2d.fromDegrees(18), 30)); + HUB_SHOT_TREE.put( + Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 9 * 12), + new ShotData(Rotation2d.fromDegrees(21.5), 30)); + HUB_SHOT_TREE.put( + Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 11 * 12), + new ShotData(Rotation2d.fromDegrees(24.5), 30)); + HUB_SHOT_TREE.put( + Units.inchesToMeters(24 * Math.sqrt(2) + 6 + 13 * 12), + new ShotData(Rotation2d.fromDegrees(28), 30)); } // Ig we'll see if we need more than 1 feed shot tree @@ -23,12 +41,14 @@ public class AutoAim { static { // For feed shot tree // TODO: POPULATE FEED_SHOT_TREE.put( - 1.0, new ShotData(Rotation2d.kCW_90deg, 10, 0.5)); // Placeholder to prevent crashes + 1.0, new ShotData(Rotation2d.kCW_90deg, 10)); // Placeholder to prevent crashes } // TODO: SOTM public static double distanceToHub(Pose2d pose) { - return pose.getTranslation().getDistance(FieldUtils.getCurrentHubTranslation()); + double distance = pose.getTranslation().getDistance(FieldUtils.getCurrentHubTranslation()); + Logger.recordOutput("Autoaim/Distance To Hub", distance); + return distance; } } diff --git a/src/main/java/frc/robot/utils/autoaim/InterpolatingShotTree.java b/src/main/java/frc/robot/utils/autoaim/InterpolatingShotTree.java index deaebd9..a9e90c0 100644 --- a/src/main/java/frc/robot/utils/autoaim/InterpolatingShotTree.java +++ b/src/main/java/frc/robot/utils/autoaim/InterpolatingShotTree.java @@ -5,8 +5,7 @@ import java.util.TreeMap; public class InterpolatingShotTree { - public record ShotData( - Rotation2d hoodAngle, double flywheelVelocityRotPerSec, double flightTimeSec) {} + public record ShotData(Rotation2d hoodAngle, double flywheelVelocityRotPerSec) {} private final TreeMap map = new TreeMap<>(); @@ -90,8 +89,7 @@ private ShotData interpolate(ShotData startValue, ShotData endValue, double t) { MathUtil.interpolate( startValue.hoodAngle().getRadians(), endValue.hoodAngle().getRadians(), t)), MathUtil.interpolate( - startValue.flywheelVelocityRotPerSec(), endValue.flywheelVelocityRotPerSec(), t), - MathUtil.interpolate(startValue.flightTimeSec(), endValue.flightTimeSec(), t)); + startValue.flywheelVelocityRotPerSec(), endValue.flywheelVelocityRotPerSec(), t)); } /**