diff --git a/ChoreoProject.chor b/ChoreoProject.chor index 54f1070..2a875ac 100644 --- a/ChoreoProject.chor +++ b/ChoreoProject.chor @@ -1,5 +1,5 @@ { - "version": "v0.3", + "version": "v0.3.1", "robotConfiguration": { "mass": 65.1974197627209, "rotationalInertia": 15, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 58164e4..6c045c6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -12,7 +12,7 @@ import java.util.function.BooleanSupplier; public final class Constants { - public static final Mode kCurrentMode = Mode.kSim; + public static final Mode kCurrentMode = Mode.kReal; public static final RobotName kRobot = RobotName.kViper; public static final boolean kIsViper = kRobot.equals(RobotName.kViper); public static final boolean kIsSim = Constants.kCurrentMode.equals(Mode.kSim); @@ -51,6 +51,6 @@ public static enum Mode { public static enum RobotName { kViper, - kBackshot + kBlackMamba } } diff --git a/src/main/java/frc/robot/NoteVisualizer.java b/src/main/java/frc/robot/NoteVisualizer.java index 92c6b48..492f2d7 100644 --- a/src/main/java/frc/robot/NoteVisualizer.java +++ b/src/main/java/frc/robot/NoteVisualizer.java @@ -46,9 +46,9 @@ public static void showStagedNotes() { public static void showHeldNotes() { if (m_hasNote) { - Logger.recordOutput("NoteVisualizer/HeldNotes", new Pose3d[] {getNotePose()}); + Logger.recordOutput("NoteVisualizer/HeldNote", getNotePose()); } else { - Logger.recordOutput("NoteVisualizer/HeldNotes", new Pose3d()); + Logger.recordOutput("NoteVisualizer/HeldNote", new Pose3d()); } } diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index cba0ad2..0c18332 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -4,8 +4,10 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ScheduleCommand; import frc.robot.arm.Arm; import frc.robot.arm.Arm.ArmSetpoints; +import frc.robot.climber.Climber; import frc.robot.drivetrain.Drivetrain; import frc.robot.feeder.Feeder; import frc.robot.intake.Intake; @@ -63,4 +65,11 @@ public static Command aimAtGoal(Drivetrain drivetrain, Shooter shooter, Arm arm, .and(arm.atGoal))) .withInterruptBehavior(InterruptionBehavior.kCancelIncoming); } + + public static Command trapRoutine(Arm arm, Climber climber, Shooter shooter, Feeder feeder) { + return Commands.sequence( + new ScheduleCommand(arm.goToSetpoint(ArmSetpoints.kTrap)), + climber.goToTrapLimit(), + shooter.trapShot().alongWith(feeder.feed())); + } } diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java index e04673c..ba5fede 100644 --- a/src/main/java/frc/robot/arm/Arm.java +++ b/src/main/java/frc/robot/arm/Arm.java @@ -9,7 +9,6 @@ import edu.wpi.first.math.Vector; import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; @@ -30,6 +29,7 @@ public class Arm extends SubsystemBase { public static final double kCatchWristAngleRad = 2.264 - Units.degreesToRadians(5.0); public static final double kSubwooferWristAngleRad = 2.083; + public static final double kDeployGizmoAngleRad = Units.degreesToRadians(80.0); private final double kJointTolerenceDegrees = 1.0; private final ArmIO m_io; @@ -46,8 +46,8 @@ public class Arm extends SubsystemBase { new LoggedTunableNumber("Arm/Wrist/MaxAcceleration"); private Vector m_initialAngles; - private Rotation2d m_lastElbowSetpoint; - private Rotation2d m_lastWristSetpoint; + private double m_lastElbowSetpoint; + private double m_lastWristSetpoint; public Trigger atGoal = new Trigger(this::jointsAtGoal).debounce(0.1, DebounceType.kBoth); public Supplier wristPoseSupplier; private final SysIdRoutine m_sysIdRoutineElbow, m_sysIdRoutineWrist; @@ -136,8 +136,8 @@ private void runSetpoint( throw new IllegalArgumentException("Only one joint can be delayed at a time."); if (elbowDelay < 0.0 || wristDelay < 0.0) throw new IllegalArgumentException("Percent delay can't be negative."); - m_lastElbowSetpoint = new Rotation2d(elbowAngleRad); - m_lastWristSetpoint = new Rotation2d(wristAngleRad); + m_lastElbowSetpoint = elbowAngleRad; + m_lastWristSetpoint = wristAngleRad; Logger.recordOutput("Arm/Goal Elbow Angle", Units.radiansToDegrees(elbowAngleRad)); Logger.recordOutput("Arm/Goal Wrist Angle", Units.radiansToDegrees(wristAngleRad)); m_goalVisualizer.update(elbowAngleRad, wristAngleRad); @@ -159,17 +159,13 @@ private void runSetpoint( @AutoLogOutput(key = "Arm/JointsAtGoal") private boolean jointsAtGoal() { return (MathUtil.isNear( - 0.0, - m_lastElbowSetpoint - .minus(Rotation2d.fromRotations(m_inputs.elbowPositionRot)) - .getDegrees(), - kJointTolerenceDegrees) + m_lastElbowSetpoint, + Units.rotationsToRadians(m_inputs.elbowPositionRot), + Units.degreesToRadians(kJointTolerenceDegrees)) && MathUtil.isNear( - 0.0, - m_lastWristSetpoint - .minus(Rotation2d.fromRotations(m_inputs.wristPositionRot)) - .getDegrees(), - kJointTolerenceDegrees)); + m_lastWristSetpoint, + Units.rotationsToRadians(m_inputs.wristPositionRot), + Units.degreesToRadians(kJointTolerenceDegrees))); } public Command storeInitialAngles() { @@ -221,10 +217,11 @@ public Command aimElbowForTuning(DoubleSupplier driveEffort) { public Command aimWristForTuning(DoubleSupplier driveEffort) { return this.run( - () -> { - m_io.setElbowRotations(Units.radiansToRotations(ArmSetpoints.kStowed.elbowAngle)); - m_io.setWristVoltage(MathUtil.applyDeadband(driveEffort.getAsDouble(), 0.1) * 1.0); - }); + () -> { + m_io.setElbowRotations(Units.radiansToRotations(ArmSetpoints.kStowed.elbowAngle)); + m_io.setWristVoltage(MathUtil.applyDeadband(driveEffort.getAsDouble(), 0.1) * 1.0); + }) + .finallyDo(() -> holdSetpoint().schedule()); } public Command holdSetpoint() { @@ -237,7 +234,7 @@ public Command holdSetpoint() { Units.rotationsToRadians(m_initialAngles.get(1)), 0.0, 0.0))) - .withName("Hold Setpoint"); + .withName("Hold Setpoint (Arm)"); } public Command goToSetpoint(ArmSetpoints setpoint) { @@ -250,7 +247,7 @@ public Command goToSetpoint(ArmSetpoints setpoint) { setpoint.wristAngle, setpoint.elbowDelay, setpoint.wristDelay))) - .withName("Set Inverse Setpoint"); + .withName("Go To Setpoint (Arm)"); } public Command idle() { @@ -285,8 +282,7 @@ public static enum ArmSetpoints { kStowed(-0.548, 2.485, 0.15, 0.0), kAmp(1.49 + 0.0873, Units.degreesToRadians(230), 0.0, 0.0), kClimb(1.633, -2.371, 0.0, 0.0), - kTrap(Units.degreesToRadians(63), Units.degreesToRadians(97), 0.0, 0.0), - kBalance(Units.degreesToRadians(18), Units.degreesToRadians(84), 0.0, 0.0); + kTrap(Units.degreesToRadians(53.0), Units.degreesToRadians(80.0), 0.0, 0.0); public final double elbowAngle; public final double wristAngle; diff --git a/src/main/java/frc/robot/arm/ArmIOTalonFX.java b/src/main/java/frc/robot/arm/ArmIOTalonFX.java index 4dc2960..aa26dc9 100644 --- a/src/main/java/frc/robot/arm/ArmIOTalonFX.java +++ b/src/main/java/frc/robot/arm/ArmIOTalonFX.java @@ -5,7 +5,7 @@ import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.MotionMagicExpoVoltage; +import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.hardware.ParentDevice; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.GravityTypeValue; @@ -23,7 +23,7 @@ public class ArmIOTalonFX implements ArmIO { m_elbowRightFollowerMotor, m_wristLeftMotor, m_wristRightFollowerMotor; - private final MotionMagicExpoVoltage m_elbowControl, m_wristControl; + private final MotionMagicVoltage m_elbowControl, m_wristControl; private final StatusSignal m_elbowPositionSignal, m_elbowPositionSetpointSignal, m_elbowVelocitySignal, @@ -57,17 +57,18 @@ public ArmIOTalonFX() { elbowLeftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; elbowLeftMotorConfig.Feedback.SensorToMechanismRatio = ArmConstants.kElbowReduction; elbowLeftMotorConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine; - elbowLeftMotorConfig.Slot0.kP = 80.403; - elbowLeftMotorConfig.Slot0.kD = 14.119; - elbowLeftMotorConfig.Slot0.kS = 0.67; - elbowLeftMotorConfig.Slot0.kV = 14.11; - elbowLeftMotorConfig.Slot0.kA = 1.2204; + elbowLeftMotorConfig.Slot0.kP = 62.122; + elbowLeftMotorConfig.Slot0.kD = 10.118; + elbowLeftMotorConfig.Slot0.kS = 0.74887; + elbowLeftMotorConfig.Slot0.kV = 13.863; + elbowLeftMotorConfig.Slot0.kA = 1.2605; elbowLeftMotorConfig.Slot0.kG = 0.37766; elbowLeftMotorConfig.MotionMagic.MotionMagicCruiseVelocity = 0.5; + elbowLeftMotorConfig.MotionMagic.MotionMagicAcceleration = 2.0; elbowLeftMotorConfig.MotionMagic.MotionMagicExpo_kV = 16.0; elbowLeftMotorConfig.MotionMagic.MotionMagicExpo_kA = 6.0; - elbowLeftMotorConfig.Voltage.PeakForwardVoltage = 5.0; - elbowLeftMotorConfig.Voltage.PeakReverseVoltage = -5.0; + elbowLeftMotorConfig.Voltage.PeakForwardVoltage = 6.0; + elbowLeftMotorConfig.Voltage.PeakReverseVoltage = -6.0; m_elbowLeftMotor.getConfigurator().apply(elbowLeftMotorConfig, 1.0); final TalonFXConfiguration wristLeftMotorConfig = new TalonFXConfiguration(); @@ -78,14 +79,13 @@ public ArmIOTalonFX() { wristLeftMotorConfig.Feedback.SensorToMechanismRatio = ArmConstants.kWristReduction; wristLeftMotorConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine; wristLeftMotorConfig.Slot0.kP = 30.612; - wristLeftMotorConfig.Slot0.kD = 0.5071; + wristLeftMotorConfig.Slot0.kD = 5.5071; wristLeftMotorConfig.Slot0.kS = 0.57211; wristLeftMotorConfig.Slot0.kV = 2.5239; wristLeftMotorConfig.Slot0.kA = 0.10939; wristLeftMotorConfig.Slot0.kG = 0.45457; wristLeftMotorConfig.MotionMagic.MotionMagicCruiseVelocity = 0.5; - wristLeftMotorConfig.MotionMagic.MotionMagicExpo_kV = 4.0; - wristLeftMotorConfig.MotionMagic.MotionMagicExpo_kA = 1.0; + wristLeftMotorConfig.MotionMagic.MotionMagicAcceleration = 2.0; wristLeftMotorConfig.Voltage.PeakForwardVoltage = 3.0; wristLeftMotorConfig.Voltage.PeakReverseVoltage = -3.0; m_wristLeftMotor.getConfigurator().apply(wristLeftMotorConfig, 1.0); @@ -137,8 +137,10 @@ public ArmIOTalonFX() { ParentDevice.optimizeBusUtilizationForAll( m_elbowLeftMotor, m_elbowRightFollowerMotor, m_wristLeftMotor, m_wristRightFollowerMotor); - m_elbowControl = new MotionMagicExpoVoltage(0.0, true, 0.0, 0, false, false, false); - m_wristControl = new MotionMagicExpoVoltage(0.0, true, 0.0, 0, false, false, false); + m_elbowControl = + new MotionMagicVoltage(0.0, true, 0.0, 0, false, false, false).withUpdateFreqHz(0.0); + m_wristControl = + new MotionMagicVoltage(0.0, true, 0.0, 0, false, false, false).withUpdateFreqHz(0.0); } @Override diff --git a/src/main/java/frc/robot/climber/Climber.java b/src/main/java/frc/robot/climber/Climber.java index 835767a..f89c9db 100644 --- a/src/main/java/frc/robot/climber/Climber.java +++ b/src/main/java/frc/robot/climber/Climber.java @@ -1,6 +1,7 @@ package frc.robot.climber; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.Logger; @@ -8,6 +9,7 @@ public class Climber extends SubsystemBase { public static final double kWinchReduction = 1.0; private final ClimberIO m_io; private final ClimberIOInputsAutoLogged m_inputs = new ClimberIOInputsAutoLogged(); + private final double kTrapLimit = 200.0; public Climber(ClimberIO climberIO) { m_io = climberIO; @@ -43,4 +45,12 @@ public Command unwindWinch() { m_io.setRightVoltage(-4.0); }); } + + public Command goToTrapLimit() { + return Commands.parallel( + this.run(() -> m_io.setLeftVoltage(12.0)) + .until(() -> m_inputs.leftWinchPositionRot > kTrapLimit), + this.run(() -> m_io.setRightVoltage(12.0)) + .until(() -> m_inputs.leftWinchPositionRot > kTrapLimit)); + } } diff --git a/src/main/java/frc/robot/climber/ClimberIOSparkMAX.java b/src/main/java/frc/robot/climber/ClimberIOSparkMAX.java index 5b47240..582c18f 100644 --- a/src/main/java/frc/robot/climber/ClimberIOSparkMAX.java +++ b/src/main/java/frc/robot/climber/ClimberIOSparkMAX.java @@ -20,6 +20,14 @@ public ClimberIOSparkMAX() { m_leftWinchEncoder = m_leftWinchMotor.getEncoder(); m_rightWinchEncoder = m_rightWinchMotor.getEncoder(); + m_leftWinchEncoder.setPositionConversionFactor(1.0 / Climber.kWinchReduction); + m_leftWinchEncoder.setVelocityConversionFactor(1.0 / Climber.kWinchReduction); + m_rightWinchEncoder.setPositionConversionFactor(1.0 / Climber.kWinchReduction); + m_rightWinchEncoder.setVelocityConversionFactor(1.0 / Climber.kWinchReduction); + + m_leftWinchEncoder.setPosition(0.0); + m_rightWinchEncoder.setPosition(0.0); + m_leftWinchMotor.setIdleMode(IdleMode.kBrake); m_rightWinchMotor.setIdleMode(IdleMode.kBrake); } diff --git a/src/main/java/frc/robot/climber/ClimberIOTalonFX.java b/src/main/java/frc/robot/climber/ClimberIOTalonFX.java index d2bbf07..e32883d 100644 --- a/src/main/java/frc/robot/climber/ClimberIOTalonFX.java +++ b/src/main/java/frc/robot/climber/ClimberIOTalonFX.java @@ -8,6 +8,7 @@ import com.ctre.phoenix6.hardware.ParentDevice; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; +import frc.robot.Constants; public class ClimberIOTalonFX implements ClimberIO { @@ -23,8 +24,8 @@ public class ClimberIOTalonFX implements ClimberIO { public ClimberIOTalonFX() { - m_leftWinchMotor = new TalonFX(9); - m_rightWinchMotor = new TalonFX(10); + m_leftWinchMotor = new TalonFX(25, Constants.kSuperstructureCanBus); + m_rightWinchMotor = new TalonFX(26, Constants.kSuperstructureCanBus); final FeedbackConfigs feedbackConfigs = new FeedbackConfigs().withSensorToMechanismRatio(Climber.kWinchReduction); diff --git a/src/main/java/frc/robot/drivetrain/ImuIOPigeon2.java b/src/main/java/frc/robot/drivetrain/ImuIOPigeon2.java index 57ce5c5..d2b7967 100644 --- a/src/main/java/frc/robot/drivetrain/ImuIOPigeon2.java +++ b/src/main/java/frc/robot/drivetrain/ImuIOPigeon2.java @@ -24,7 +24,7 @@ public ImuIOPigeon2() { m_imu.optimizeBusUtilization(); m_yawPositionQueue = PhoenixOdometryThread.getInstance() - .registerSignals(m_imu, m_yawSignal, m_yawVelocitySignal); + .registerSignals(m_imu, m_yawSignal.clone(), m_yawVelocitySignal.clone()); m_yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); } diff --git a/src/main/java/frc/robot/drivetrain/ModuleIOTalonFX.java b/src/main/java/frc/robot/drivetrain/ModuleIOTalonFX.java index d4d116e..fa9a8f4 100644 --- a/src/main/java/frc/robot/drivetrain/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/drivetrain/ModuleIOTalonFX.java @@ -141,10 +141,12 @@ public ModuleIOTalonFX(int index) { m_drivePositionQueue = PhoenixOdometryThread.getInstance() - .registerSignals(m_driveMotor, m_drivePositionSignal, m_driveVelocitySignal); + .registerSignals( + m_driveMotor, m_drivePositionSignal.clone(), m_driveVelocitySignal.clone()); m_steerPositionQueue = PhoenixOdometryThread.getInstance() - .registerSignals(m_steerMotor, m_steerPositionSignal, m_steerVelocitySignal); + .registerSignals( + m_steerMotor, m_steerPositionSignal.clone(), m_steerVelocitySignal.clone()); m_timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); BaseStatusSignal.setUpdateFrequencyForAll( diff --git a/src/main/java/frc/robot/drivetrain/PhoenixOdometryThread.java b/src/main/java/frc/robot/drivetrain/PhoenixOdometryThread.java index 82786ff..7bc96d4 100644 --- a/src/main/java/frc/robot/drivetrain/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/drivetrain/PhoenixOdometryThread.java @@ -26,8 +26,7 @@ public class PhoenixOdometryThread extends Thread { new ArrayList<>(); private final List> m_positionQueues = new ArrayList<>(); private final List> m_timestampQueues = new ArrayList<>(); - private final MedianFilter m_peakRemover = new MedianFilter(3); - private final LinearFilter m_lowPassFilter = LinearFilter.movingAverage(50); + private double m_averageLoopTimeMs = 0.0; public final Supplier averageLoopTimeSupplier = () -> m_averageLoopTimeMs; @@ -83,6 +82,8 @@ public Queue makeTimestampQueue() { @Override public void run() { + final MedianFilter peakRemover = new MedianFilter(3); + final LinearFilter lowPassFilter = LinearFilter.movingAverage(50); double lastTime = 0.0; double currentTime = 0.0; while (true) { @@ -97,7 +98,7 @@ public void run() { lastTime = currentTime; currentTime = Utils.getCurrentTimeSeconds(); m_averageLoopTimeMs = - m_lowPassFilter.calculate(m_peakRemover.calculate(currentTime - lastTime)) * 1e3; + lowPassFilter.calculate(peakRemover.calculate(currentTime - lastTime)) * 1e3; double timestamp = Logger.getRealTimestamp() / 1e6; double totalLatency = 0.0; diff --git a/src/main/java/frc/robot/intake/IntakeIOTalonFX.java b/src/main/java/frc/robot/intake/IntakeIOTalonFX.java index ffc08d8..d0553ad 100644 --- a/src/main/java/frc/robot/intake/IntakeIOTalonFX.java +++ b/src/main/java/frc/robot/intake/IntakeIOTalonFX.java @@ -1,19 +1,20 @@ package frc.robot.intake; +import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; -import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.ctre.phoenix.motorcontrol.can.BaseTalon; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.ParentDevice; import com.ctre.phoenix6.hardware.TalonFX; +import frc.robot.Constants; public class IntakeIOTalonFX implements IntakeIO { private final TalonFX m_topRollerMotor, m_bottomRollerMotor; - private final TalonSRX m_centeringMotors; + private final BaseTalon m_centeringMotors; private final StatusSignal m_topRollerVelocitySignal, m_topRollerAppliedVoltageSignal, @@ -23,10 +24,9 @@ public class IntakeIOTalonFX implements IntakeIO { m_bottomRollerCurrentSignal; public IntakeIOTalonFX() { - m_topRollerMotor = new TalonFX(5); - m_bottomRollerMotor = new TalonFX(6); - m_centeringMotors = new TalonSRX(8); - + m_topRollerMotor = new TalonFX(22, Constants.kSuperstructureCanBus); + m_bottomRollerMotor = new TalonFX(23, Constants.kSuperstructureCanBus); + m_centeringMotors = new BaseTalon(24, "Talon SRX", "rio"); m_topRollerMotor.getConfigurator().apply(new TalonFXConfiguration()); m_bottomRollerMotor.getConfigurator().apply(new TalonFXConfiguration()); final CurrentLimitsConfigs currentLimits = @@ -35,7 +35,7 @@ public IntakeIOTalonFX() { m_bottomRollerMotor.getConfigurator().apply(currentLimits); m_centeringMotors.configSupplyCurrentLimit( - new SupplyCurrentLimitConfiguration(true, 40.0, 0.0, 1.0)); + new SupplyCurrentLimitConfiguration(true, 40.0, 0.0, 1.0), 1000); m_centeringMotors.configVoltageCompSaturation(12.0); m_centeringMotors.enableVoltageCompensation(true); @@ -94,13 +94,13 @@ public void setBottomRollerVoltage(double volts) { @Override public void setCenteringMotorsVoltage(double volts) { - m_centeringMotors.set(TalonSRXControlMode.PercentOutput, (volts / 12.0) * 100.0); + m_centeringMotors.set(ControlMode.PercentOutput, volts / 12.0); } @Override public void stopRollers() { m_topRollerMotor.stopMotor(); m_bottomRollerMotor.stopMotor(); - m_centeringMotors.set(TalonSRXControlMode.PercentOutput, 0.0); + m_centeringMotors.set(ControlMode.PercentOutput, 0.0); } } diff --git a/src/main/java/frc/robot/shooter/Shooter.java b/src/main/java/frc/robot/shooter/Shooter.java index 0f9c723..2102d3b 100644 --- a/src/main/java/frc/robot/shooter/Shooter.java +++ b/src/main/java/frc/robot/shooter/Shooter.java @@ -2,6 +2,7 @@ import static edu.wpi.first.units.Units.Volts; +import com.ctre.phoenix6.SignalLogger; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.wpilibj2.command.Command; @@ -10,26 +11,28 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; +import frc.robot.Constants; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class Shooter extends SubsystemBase { + public static final double kShooterStepUp = Constants.kIsViper ? 0.0 : 0.5; public static final double kFarShotVelocityRpm = 5600.0; private final double kTrapShot = 400.0; private final double kAmpShot = 5000.0; private final double kReadyToShootToleranceRps = 3.0; // Denominator for gains here are in rotations - public static final double topRollerkS = 0.18039; - public static final double topRollerkV = 0.11968; - public static final double topRollerkA = 0.0089044; - public static final double bottomRollerkS = 0.19936; - public static final double bottomRollerkV = 0.12041; - public static final double bottomRollerkA = 0.0071461; + public static final double topRollerkS = 0.081727; + public static final double topRollerkV = 0.11753; + public static final double topRollerkA = 0.015313; + public static final double bottomRollerkS = 0.11496; + public static final double bottomRollerkV = 0.11868; + public static final double bottomRollerkA = 0.0141; - public static final double topRollerkP = 0.13085; - public static final double bottomRollerkP = 0.11992; + public static final double topRollerkP = 0.17726; + public static final double bottomRollerkP = 0.17918; private final SysIdRoutine m_sysIdRoutine; private final ShooterIO m_io; @@ -49,7 +52,7 @@ public Shooter(ShooterIO io) { null, null, null, - state -> Logger.recordOutput("Shooter/SysIdState", state.toString())), + state -> SignalLogger.writeString("Shooter/SysIdState", state.toString())), new SysIdRoutine.Mechanism( voltage -> { m_io.setTopRollerVoltage(voltage.in(Volts)); diff --git a/src/main/java/frc/robot/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/shooter/ShooterIOTalonFX.java index c24d80f..01defb7 100644 --- a/src/main/java/frc/robot/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/shooter/ShooterIOTalonFX.java @@ -36,6 +36,7 @@ public ShooterIOTalonFX() { topRollerMotorConfig.Slot0.kA = Shooter.topRollerkA; topRollerMotorConfig.Slot0.kP = Shooter.topRollerkP; topRollerMotorConfig.MotionMagic = motionMagicConfigs; + topRollerMotorConfig.Feedback.SensorToMechanismRatio = Shooter.kShooterStepUp; m_topRoller.getConfigurator().apply(topRollerMotorConfig); final TalonFXConfiguration bottomRollerMotorConfig = new TalonFXConfiguration(); bottomRollerMotorConfig.Slot0.kS = Shooter.bottomRollerkS; @@ -43,6 +44,7 @@ public ShooterIOTalonFX() { bottomRollerMotorConfig.Slot0.kA = Shooter.bottomRollerkA; bottomRollerMotorConfig.Slot0.kP = Shooter.bottomRollerkP; bottomRollerMotorConfig.MotionMagic = motionMagicConfigs; + bottomRollerMotorConfig.Feedback.SensorToMechanismRatio = Shooter.kShooterStepUp; m_bottomRoller.getConfigurator().apply(bottomRollerMotorConfig); m_topRollerPositionSignal = m_topRoller.getPosition(); @@ -55,7 +57,7 @@ public ShooterIOTalonFX() { m_bottomRollerCurrentSignal = m_bottomRoller.getStatorCurrent(); BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, + 100.0, m_topRollerVelocitySignal, m_topRollerAppliedVoltageSignal, m_topRollerCurrentSignal,