From 955682b1ec3a51cca84b3bd699bedfad94d63b04 Mon Sep 17 00:00:00 2001 From: clarkmilbert Date: Mon, 23 Feb 2026 19:39:45 -0600 Subject: [PATCH 1/9] Add an initializing state to teleop that makes the shooter calibrate before setting to default state. --- .../team1816/season/subsystems/Shooter.java | 41 +++++++++---------- .../season/subsystems/Superstructure.java | 22 +++++++--- src/main/resources/yaml/ztldr.yml | 1 + 3 files changed, 38 insertions(+), 26 deletions(-) diff --git a/src/main/java/com/team1816/season/subsystems/Shooter.java b/src/main/java/com/team1816/season/subsystems/Shooter.java index 9a14a0d..35fe71a 100644 --- a/src/main/java/com/team1816/season/subsystems/Shooter.java +++ b/src/main/java/com/team1816/season/subsystems/Shooter.java @@ -2,6 +2,7 @@ import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; import com.pathplanner.lib.util.FlippingUtil; import com.team1816.lib.BaseRobotState; import com.team1816.lib.hardware.components.motor.IMotor; @@ -63,7 +64,10 @@ public class Shooter extends SubsystemBase implements ITestableSubsystem { private final double CALIBRATION_THRESHOLD; private final Rotation2d CALIBRATION_POSITION_ARC_ANGLE; private final Rotation2d ROTATION_OFFSET_FROM_CALIBRATION_ZERO; - private static final double HALF_FIELD_WIDTH = FlippingUtil.fieldSizeY/2; + private final double HALF_FIELD_WIDTH = FlippingUtil.fieldSizeY/2; + private final double DISTANCE_BETWEEN_BEAM_BREAKS; + private double leftLimit = 0; + private double rightLimit = 0; //CALIBRATION private Double[] calibrationPositions = new Double[]{0.0, 0.0}; @@ -104,7 +108,6 @@ public Translation3d getPosition(){ public enum SHOOTER_STATE { // TODO: figure out what default angles and velocities should be for manual mode CALIBRATING(0, 0, 0), - CALIBRATED(0, 0, 0), DISTANCE_ONE(factory.getConstant(NAME,"distanceOneLaunchAngle",0), factory.getConstant(NAME,"distanceOneRotationAngle",0), factory.getConstant(NAME,"distanceOneLaunchVelocity",0)), DISTANCE_TWO(factory.getConstant(NAME,"distanceTwoLaunchAngle",0), factory.getConstant(NAME,"distanceTwoRotationAngle",0), factory.getConstant(NAME,"distanceTwoLaunchVelocity",0)), DISTANCE_THREE(factory.getConstant(NAME,"distanceThreeLaunchAngle",0), factory.getConstant(NAME,"distanceThreeRotationAngle",0), factory.getConstant(NAME,"distanceThreeLaunchVelocity",0)), @@ -143,6 +146,7 @@ public Shooter(){ CALIBRATION_THRESHOLD = factory.getConstant(NAME, "calibrationThreshold",10); //TODO WHEN PHYSICAL SUBSYSTEM EXISTS, set this. CALIBRATION_POSITION_ARC_ANGLE = Rotation2d.fromRotations(factory.getConstant(NAME, "calibrationPositionArcAngle", 0.75)); //should always be less than 1 rotation //TODO WHEN PHYSICAL SUBSYSTEM EXISTS, set this. ROTATION_OFFSET_FROM_CALIBRATION_ZERO = Rotation2d.fromDegrees(factory.getConstant(NAME, "rotationOffsetFromCalibrationZero", 70)); //as a note, the rotation motor should move clockwise on positive dutycycle, otherwise directions will be flipped //TODO WHEN PHYSICAL SUBSYSTEM EXISTS, set this. + DISTANCE_BETWEEN_BEAM_BREAKS = factory.getConstant(NAME,"distanceBetweenBeamBreaks",0); launcherTranslation = new Translation3d(0,0,0).plus(SHOOTER_OFFSET); @@ -242,23 +246,17 @@ public void setCurrentAutoAimTarget(AUTO_AIM_TARGETS target) { } private void calibratePeriodic(){ - if (!rightSensorValue && !leftSensorValue){ - if (java.util.Arrays.stream(calibrationPositions).filter(java.util.Objects::nonNull).noneMatch(rotation -> Math.abs(rotation - currentRotationPosition) <= CALIBRATION_THRESHOLD)) { - if (calibrationPositions[0] == null) { - calibrationPositions[0] = currentRotationPosition; - } else if (calibrationPositions[1] == null) { - calibrationPositions[1] = currentRotationPosition; - if (calibrationPositions[0] > calibrationPositions[1]){ - double temp = calibrationPositions[0]; - calibrationPositions[0] = calibrationPositions[1]; - calibrationPositions[1] = temp; - } - } else { - GreenLogger.log("Shooter.calibrate() has logged more calibration positions than expected"); - } - } + // TODO: figure out which sensor value is a lower number of ticks + if (!leftSensorValue) { + double currentMotorPosition = rotationAngleMotor.getMotorPosition(); + leftLimit = currentMotorPosition; + rightLimit = currentMotorPosition + DISTANCE_BETWEEN_BEAM_BREAKS; + isCalibrated = true; } - if (calibrationPositions[0] != null && calibrationPositions[1] != null && wantedState == SHOOTER_STATE.CALIBRATING){ + if (!rightSensorValue) { + double currentMotorPosition = rotationAngleMotor.getMotorPosition(); + leftLimit = currentMotorPosition - DISTANCE_BETWEEN_BEAM_BREAKS; + rightLimit = currentMotorPosition; isCalibrated = true; } } @@ -293,12 +291,13 @@ private void setRotationAngle(double wantedAngleDegrees) { rotations = wrapMotorRotations(rotations); - if(calibrationPositions[0] != null && calibrationPositions[1] != null){ - if (calibrationPositions[0] > rotations || calibrationPositions[1] < rotations){ + if(isCalibrated){ + // TODO: fix this based on if left or right is lower + if (rotations < leftLimit || rotations > rightLimit){ GreenLogger.log("Wanted Shooter rotation is out of bounds of the calibrated positions"); } - double output = MathUtil.clamp(rotations, calibrationPositions[0], calibrationPositions[1]); + double output = MathUtil.clamp(rotations, leftLimit, rightLimit); rotationAngleMotor.setControl(positionControl.withPosition(output)); } else { diff --git a/src/main/java/com/team1816/season/subsystems/Superstructure.java b/src/main/java/com/team1816/season/subsystems/Superstructure.java index 0e069ca..8d649f5 100644 --- a/src/main/java/com/team1816/season/subsystems/Superstructure.java +++ b/src/main/java/com/team1816/season/subsystems/Superstructure.java @@ -4,6 +4,7 @@ import com.team1816.lib.subsystems.drivetrain.Swerve; import com.team1816.lib.util.GreenLogger; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -31,6 +32,7 @@ public enum WantedSuperState { STORAGE_INTAKE, STORAGE_SHOOTER, INDEX_AGITATE, + INITIALIZING, SHOOTER_CALIBRATE, SHOOTER_AUTOMATIC_HUB, @@ -78,6 +80,7 @@ public enum ActualSuperState { STORAGE_INTAKING, STORAGE_SHOOTING, INDEX_AGITATING, + INITIALIZING, SHOOTER_CALIBRATING, SHOOTING_AUTOMATIC_HUB, @@ -136,7 +139,6 @@ public enum WantedClimbState { public enum WantedShooterState { CALIBRATING, - CALIBRATED, DISTANCE_ONE, DISTANCE_TWO, DISTANCE_THREE, @@ -175,10 +177,9 @@ public enum FeederControlState { DEFAULTING } - private WantedSuperState wantedSuperState = WantedSuperState.DEFAULT; + private WantedSuperState wantedSuperState = WantedSuperState.INITIALIZING; private ActualSuperState actualSuperState = ActualSuperState.DEFAULTING; - // TODO: Add calibration state, maybe as the default here private WantedShooterState wantedShooterState = WantedShooterState.IDLE; private WantedGatekeeperState wantedGatekeeperState = WantedGatekeeperState.CLOSED; private WantedClimbState wantedClimbState = WantedClimbState.IDLING; @@ -243,6 +244,9 @@ private ActualSuperState handleStateTransitions() { case INDEX_AGITATE: actualSuperState = ActualSuperState.INDEX_AGITATING; break; + case INITIALIZING: + actualSuperState = ActualSuperState.INITIALIZING; + break; case IDLE: default: actualSuperState = ActualSuperState.IDLING; @@ -281,6 +285,8 @@ private void applyStates() { case INDEX_AGITATING: agitate(); break; + case INITIALIZING: + initializing(); case IDLING: default: defaulting(); @@ -456,10 +462,16 @@ private void agitate() { } } + private void initializing() { + setWantedShooterState(WantedShooterState.CALIBRATING); + if (shooter.isCalibrated()) { + setWantedSuperState(WantedSuperState.DEFAULT); + } + } + private void defaulting() { switch (wantedShooterState) { - case CALIBRATING -> {if (shooter.isCalibrated()) {shooter.setWantedState(Shooter.SHOOTER_STATE.CALIBRATED);} else {shooter.setWantedState(Shooter.SHOOTER_STATE.CALIBRATING);}} - case CALIBRATED -> {GreenLogger.log("Shooter is calibrated"); shooter.setWantedState(Shooter.SHOOTER_STATE.IDLE);} + case CALIBRATING -> shooter.setWantedState(Shooter.SHOOTER_STATE.CALIBRATING); case DISTANCE_ONE -> shooter.setWantedState(Shooter.SHOOTER_STATE.DISTANCE_ONE); case DISTANCE_TWO -> shooter.setWantedState(Shooter.SHOOTER_STATE.DISTANCE_TWO); case DISTANCE_THREE -> shooter.setWantedState(Shooter.SHOOTER_STATE.DISTANCE_THREE); diff --git a/src/main/resources/yaml/ztldr.yml b/src/main/resources/yaml/ztldr.yml index 8f8fc43..ae4266e 100644 --- a/src/main/resources/yaml/ztldr.yml +++ b/src/main/resources/yaml/ztldr.yml @@ -204,6 +204,7 @@ subsystems: distanceThreeLaunchAngle: 86 distanceThreeRotationAngle: 0 distanceThreeLaunchVelocity: 30 + distanceBetweenBeamBreaks: 0 # TODO: Set this gatekeeper: devices: topMotor: From 46ca40fec453cc1bca51c595dabc3f08a024b1c1 Mon Sep 17 00:00:00 2001 From: clarkmilbert Date: Mon, 23 Feb 2026 19:42:30 -0600 Subject: [PATCH 2/9] Remove unused calibration threshold --- src/main/java/com/team1816/season/subsystems/Shooter.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/com/team1816/season/subsystems/Shooter.java b/src/main/java/com/team1816/season/subsystems/Shooter.java index 35fe71a..bb1aea8 100644 --- a/src/main/java/com/team1816/season/subsystems/Shooter.java +++ b/src/main/java/com/team1816/season/subsystems/Shooter.java @@ -61,7 +61,6 @@ public class Shooter extends SubsystemBase implements ITestableSubsystem { private final double MOTOR_ROTATIONS_PER_LAUNCH_ANGLE_DEGREE; private final double MOTOR_ROTATIONS_PER_ROTATION_ANGLE_DEGREE; private final Translation3d SHOOTER_OFFSET; - private final double CALIBRATION_THRESHOLD; private final Rotation2d CALIBRATION_POSITION_ARC_ANGLE; private final Rotation2d ROTATION_OFFSET_FROM_CALIBRATION_ZERO; private final double HALF_FIELD_WIDTH = FlippingUtil.fieldSizeY/2; @@ -143,7 +142,6 @@ public Shooter(){ MOTOR_ROTATIONS_PER_LAUNCH_ANGLE_DEGREE = factory.getConstant(NAME, "motorRotationsPerLaunchAngleDegree", 0); //TODO WHEN PHYSICAL SUBSYSTEM EXISTS, set this. MOTOR_ROTATIONS_PER_ROTATION_ANGLE_DEGREE = factory.getConstant(NAME, "motorRotationsPerRotationAngleDegree", 0); //TODO WHEN PHYSICAL SUBSYSTEM EXISTS, set this. SHOOTER_OFFSET = new Translation3d(factory.getConstant(NAME, "initialShooterOffsetX",0), factory.getConstant(NAME, "initialShooterOffsetY",0), factory.getConstant(NAME, "initialShooterOffsetZ",0)); //TODO WHEN PHYSICAL SUBSYSTEM EXISTS, set this. - CALIBRATION_THRESHOLD = factory.getConstant(NAME, "calibrationThreshold",10); //TODO WHEN PHYSICAL SUBSYSTEM EXISTS, set this. CALIBRATION_POSITION_ARC_ANGLE = Rotation2d.fromRotations(factory.getConstant(NAME, "calibrationPositionArcAngle", 0.75)); //should always be less than 1 rotation //TODO WHEN PHYSICAL SUBSYSTEM EXISTS, set this. ROTATION_OFFSET_FROM_CALIBRATION_ZERO = Rotation2d.fromDegrees(factory.getConstant(NAME, "rotationOffsetFromCalibrationZero", 70)); //as a note, the rotation motor should move clockwise on positive dutycycle, otherwise directions will be flipped //TODO WHEN PHYSICAL SUBSYSTEM EXISTS, set this. DISTANCE_BETWEEN_BEAM_BREAKS = factory.getConstant(NAME,"distanceBetweenBeamBreaks",0); From 2e3ee53973ee3c175a2a80641f19719762625b4d Mon Sep 17 00:00:00 2001 From: clarkmilbert Date: Tue, 24 Feb 2026 17:59:51 -0600 Subject: [PATCH 3/9] Up to date superstructure added --- .../season/subsystems/Superstructure.java | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/src/main/java/com/team1816/season/subsystems/Superstructure.java b/src/main/java/com/team1816/season/subsystems/Superstructure.java index 173161f..1259ebe 100644 --- a/src/main/java/com/team1816/season/subsystems/Superstructure.java +++ b/src/main/java/com/team1816/season/subsystems/Superstructure.java @@ -24,7 +24,7 @@ public class Superstructure extends SubsystemBase { public enum WantedSuperState { DEFAULT, - SHOOTER_CALIBRATE, + INITIALIZING, SHOOTER_AUTOMATIC_HUB, @@ -54,7 +54,7 @@ public enum WantedSuperState { public enum ActualSuperState { DEFAULTING, - SHOOTING_CALIBRATING, + INITIALIZING, SHOOTING_AUTOMATIC_HUB, @@ -107,8 +107,8 @@ public enum FeederControlState { DEFAULTING } - private WantedSuperState wantedSuperState = WantedSuperState.DEFAULT; - private ActualSuperState actualSuperState = ActualSuperState.DEFAULTING; + private WantedSuperState wantedSuperState = WantedSuperState.INITIALIZING; + private ActualSuperState actualSuperState = ActualSuperState.INITIALIZING; // TODO: Add calibration state, maybe as the default here @@ -142,7 +142,7 @@ private ActualSuperState handleStateTransitions() { switch (wantedSuperState) { case DEFAULT -> actualSuperState = ActualSuperState.DEFAULTING; - case SHOOTER_CALIBRATE -> actualSuperState = ActualSuperState.SHOOTING_CALIBRATING; + case INITIALIZING -> actualSuperState = ActualSuperState.INITIALIZING; case SHOOTER_AUTOMATIC_HUB -> actualSuperState = ActualSuperState.SHOOTING_AUTOMATIC_HUB; @@ -175,7 +175,7 @@ private void applyStates() { switch (actualSuperState) { case DEFAULTING -> defaulting(); - case SHOOTING_CALIBRATING -> shootCalibrating(); + case INITIALIZING -> initializing(); case SHOOTING_AUTOMATIC_HUB -> shootingAutomaticHub(); @@ -228,8 +228,11 @@ private void defaulting() { swerve.setWantedState(Swerve.ActualState.MANUAL_DRIVING); } - private void shootCalibrating() { + private void initializing() { shooter.setWantedState(Shooter.SHOOTER_STATE.CALIBRATING); + if (shooter.isCalibrated()) { + setWantedSuperState(WantedSuperState.DEFAULT); + } } private void shootingAutomaticHub() { @@ -334,7 +337,7 @@ public void setClimbSide(ClimbSide climbSide) { } public void autonomousInit() { - //What is supposed to be here??? + //What is supposed to be here??? } public void teleopInit() { From 416062424748dd6b104c1724c99e185206f38825 Mon Sep 17 00:00:00 2001 From: clarkmilbert Date: Tue, 24 Feb 2026 18:00:57 -0600 Subject: [PATCH 4/9] Shooter stuff from my branch added --- .../team1816/season/subsystems/Shooter.java | 53 ++++++++----------- 1 file changed, 23 insertions(+), 30 deletions(-) diff --git a/src/main/java/com/team1816/season/subsystems/Shooter.java b/src/main/java/com/team1816/season/subsystems/Shooter.java index 1994e16..e46f4b6 100644 --- a/src/main/java/com/team1816/season/subsystems/Shooter.java +++ b/src/main/java/com/team1816/season/subsystems/Shooter.java @@ -2,6 +2,7 @@ import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; import com.pathplanner.lib.util.FlippingUtil; import com.team1816.lib.BaseRobotState; import com.team1816.lib.hardware.components.motor.IMotor; @@ -60,10 +61,12 @@ public class Shooter extends SubsystemBase implements ITestableSubsystem { private final double MOTOR_ROTATIONS_PER_LAUNCH_ANGLE_DEGREE; private final double MOTOR_ROTATIONS_PER_ROTATION_ANGLE_DEGREE; private final Translation3d SHOOTER_OFFSET; - private final double CALIBRATION_THRESHOLD; private final Rotation2d CALIBRATION_POSITION_ARC_ANGLE; private final Rotation2d ROTATION_OFFSET_FROM_CALIBRATION_ZERO; - private static final double HALF_FIELD_WIDTH = FlippingUtil.fieldSizeY/2; + private final double HALF_FIELD_WIDTH = FlippingUtil.fieldSizeY/2; + private final double DISTANCE_BETWEEN_BEAM_BREAKS; + private double leftLimit = 0; + private double rightLimit = 0; //CALIBRATION private Double[] calibrationPositions = new Double[]{0.0, 0.0}; @@ -104,7 +107,6 @@ public Translation3d getPosition(){ public enum SHOOTER_STATE { // TODO: figure out what default angles and velocities should be for manual mode CALIBRATING(0, 0, 0), - CALIBRATED(0, 0, 0), DISTANCE_ONE(factory.getConstant(NAME,"distanceOneLaunchAngle",0), factory.getConstant(NAME,"distanceOneRotationAngle",0), factory.getConstant(NAME,"distanceOneLaunchVelocity",0)), DISTANCE_TWO(factory.getConstant(NAME,"distanceTwoLaunchAngle",0), factory.getConstant(NAME,"distanceTwoRotationAngle",0), factory.getConstant(NAME,"distanceTwoLaunchVelocity",0)), DISTANCE_THREE(factory.getConstant(NAME,"distanceThreeLaunchAngle",0), factory.getConstant(NAME,"distanceThreeRotationAngle",0), factory.getConstant(NAME,"distanceThreeLaunchVelocity",0)), @@ -140,9 +142,9 @@ public Shooter(){ MOTOR_ROTATIONS_PER_LAUNCH_ANGLE_DEGREE = factory.getConstant(NAME, "motorRotationsPerLaunchAngleDegree", 0); //TODO WHEN PHYSICAL SUBSYSTEM EXISTS, set this. MOTOR_ROTATIONS_PER_ROTATION_ANGLE_DEGREE = factory.getConstant(NAME, "motorRotationsPerRotationAngleDegree", 0); //TODO WHEN PHYSICAL SUBSYSTEM EXISTS, set this. SHOOTER_OFFSET = new Translation3d(factory.getConstant(NAME, "initialShooterOffsetX",0), factory.getConstant(NAME, "initialShooterOffsetY",0), factory.getConstant(NAME, "initialShooterOffsetZ",0)); //TODO WHEN PHYSICAL SUBSYSTEM EXISTS, set this. - CALIBRATION_THRESHOLD = factory.getConstant(NAME, "calibrationThreshold",10); //TODO WHEN PHYSICAL SUBSYSTEM EXISTS, set this. CALIBRATION_POSITION_ARC_ANGLE = Rotation2d.fromRotations(factory.getConstant(NAME, "calibrationPositionArcAngle", 0.75)); //should always be less than 1 rotation //TODO WHEN PHYSICAL SUBSYSTEM EXISTS, set this. ROTATION_OFFSET_FROM_CALIBRATION_ZERO = Rotation2d.fromDegrees(factory.getConstant(NAME, "rotationOffsetFromCalibrationZero", 70)); //as a note, the rotation motor should move clockwise on positive dutycycle, otherwise directions will be flipped //TODO WHEN PHYSICAL SUBSYSTEM EXISTS, set this. + DISTANCE_BETWEEN_BEAM_BREAKS = factory.getConstant(NAME,"distanceBetweenBeamBreaks",0); launcherTranslation = new Translation3d(0,0,0).plus(SHOOTER_OFFSET); @@ -187,8 +189,9 @@ private void applyState() { double rotationAngle = wantedState.getRotationAngle(); double launchPower = wantedState.getLaunchPower(); - if ((wantedState == SHOOTER_STATE.AUTOMATIC) || (wantedState == SHOOTER_STATE.SNOWBLOWING)) { - if (wantedState == SHOOTER_STATE.AUTOMATIC) { //Does this work??? + if (wantedState == SHOOTER_STATE.AUTOMATIC || wantedState == SHOOTER_STATE.SNOWBLOWING) { + + if (wantedState == SHOOTER_STATE.AUTOMATIC) { if (DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) == DriverStation.Alliance.Red) { setCurrentAutoAimTarget(AUTO_AIM_TARGETS.RED_HUB); } @@ -230,11 +233,6 @@ private void applyState() { GreenLogger.log("Launch Angle: " + launchAngle); GreenLogger.log("Launch Power: " + launchPower); GreenLogger.log("Rotation Angle: " + rotationAngle); - - SmartDashboard.putString("Shooter state: ", wantedState.toString()); - SmartDashboard.putNumber("Launch Angle: ", launchAngle); - SmartDashboard.putNumber("Launch Power: ", launchPower); - SmartDashboard.putNumber("Rotation Angle: ", rotationAngle); } public void setWantedState(SHOOTER_STATE state) { @@ -246,23 +244,17 @@ public void setCurrentAutoAimTarget(AUTO_AIM_TARGETS target) { } private void calibratePeriodic(){ - if (!rightSensorValue && !leftSensorValue){ - if (java.util.Arrays.stream(calibrationPositions).filter(java.util.Objects::nonNull).noneMatch(rotation -> Math.abs(rotation - currentRotationPosition) <= CALIBRATION_THRESHOLD)) { - if (calibrationPositions[0] == null) { - calibrationPositions[0] = currentRotationPosition; - } else if (calibrationPositions[1] == null) { - calibrationPositions[1] = currentRotationPosition; - if (calibrationPositions[0] > calibrationPositions[1]){ - double temp = calibrationPositions[0]; - calibrationPositions[0] = calibrationPositions[1]; - calibrationPositions[1] = temp; - } - } else { - GreenLogger.log("Shooter.calibrate() has logged more calibration positions than expected"); - } - } + // TODO: figure out which sensor value is a lower number of ticks + if (!leftSensorValue) { + double currentMotorPosition = rotationAngleMotor.getMotorPosition(); + leftLimit = currentMotorPosition; + rightLimit = currentMotorPosition + DISTANCE_BETWEEN_BEAM_BREAKS; + isCalibrated = true; } - if (calibrationPositions[0] != null && calibrationPositions[1] != null && wantedState == SHOOTER_STATE.CALIBRATING){ + if (!rightSensorValue) { + double currentMotorPosition = rotationAngleMotor.getMotorPosition(); + leftLimit = currentMotorPosition - DISTANCE_BETWEEN_BEAM_BREAKS; + rightLimit = currentMotorPosition; isCalibrated = true; } } @@ -297,12 +289,13 @@ private void setRotationAngle(double wantedAngleDegrees) { rotations = wrapMotorRotations(rotations); - if(calibrationPositions[0] != null && calibrationPositions[1] != null){ - if (calibrationPositions[0] > rotations || calibrationPositions[1] < rotations){ + if(isCalibrated){ + // TODO: fix this based on if left or right is lower + if (rotations < leftLimit || rotations > rightLimit){ GreenLogger.log("Wanted Shooter rotation is out of bounds of the calibrated positions"); } - double output = MathUtil.clamp(rotations, calibrationPositions[0], calibrationPositions[1]); + double output = MathUtil.clamp(rotations, leftLimit, rightLimit); rotationAngleMotor.setControl(positionControl.withPosition(output)); } else { From 9cde4d364d11219ca6b7d412caa4d3a90106d115 Mon Sep 17 00:00:00 2001 From: clarkmilbert Date: Tue, 24 Feb 2026 18:10:05 -0600 Subject: [PATCH 5/9] Fix todo --- src/main/java/com/team1816/season/subsystems/Shooter.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/com/team1816/season/subsystems/Shooter.java b/src/main/java/com/team1816/season/subsystems/Shooter.java index e46f4b6..37e24f3 100644 --- a/src/main/java/com/team1816/season/subsystems/Shooter.java +++ b/src/main/java/com/team1816/season/subsystems/Shooter.java @@ -244,7 +244,7 @@ public void setCurrentAutoAimTarget(AUTO_AIM_TARGETS target) { } private void calibratePeriodic(){ - // TODO: figure out which sensor value is a lower number of ticks + // TODO: figure out which sensor value is a lower number of ticks and change left/right if (!leftSensorValue) { double currentMotorPosition = rotationAngleMotor.getMotorPosition(); leftLimit = currentMotorPosition; From bebb036684abdca831e2695c610c2d23c3c24b9f Mon Sep 17 00:00:00 2001 From: Shishir Ghate Date: Wed, 25 Feb 2026 13:48:15 -0600 Subject: [PATCH 6/9] Refactor based on discussion where the beam breaks will be. --- .../team1816/season/subsystems/Shooter.java | 24 +++++++++++-------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/src/main/java/com/team1816/season/subsystems/Shooter.java b/src/main/java/com/team1816/season/subsystems/Shooter.java index 256c04b..e6a0ead 100644 --- a/src/main/java/com/team1816/season/subsystems/Shooter.java +++ b/src/main/java/com/team1816/season/subsystems/Shooter.java @@ -42,6 +42,7 @@ public class Shooter extends SubsystemBase implements ITestableSubsystem { private final IMotor rotationAngleMotor = (IMotor) factory.getDevice(NAME, "rotationAngleMotor"); private final VelocityVoltage velocityControl = new VelocityVoltage(0); + private VelocityVoltage rotationVelocityControl = new VelocityVoltage(0); private final PositionVoltage positionControl = new PositionVoltage(0); //AUTO AIM @@ -71,7 +72,8 @@ public class Shooter extends SubsystemBase implements ITestableSubsystem { //CALIBRATION private Double[] calibrationPositions = new Double[]{0.0, 0.0}; - private boolean isCalibrated; + private boolean isCalibrated = false; + private boolean rotatingRight = false; //MECHANISMS private final NetworkTable networkTable; @@ -156,11 +158,9 @@ public Shooter(){ } public void periodic() { - SmartDashboard.putString("Shooter state: ", wantedState.toString()); - readFromHardware(); - if (wantedState == SHOOTER_STATE.CALIBRATING) { + if (!isCalibrated) { calibratePeriodic(); } else { applyState(); @@ -254,18 +254,22 @@ public void setCurrentAutoAimTarget(AUTO_AIM_TARGETS target) { private void calibratePeriodic(){ // TODO: figure out which sensor value is a lower number of ticks and change left/right + if (!rotatingRight) { + rotationAngleMotor.setControl(rotationVelocityControl.withVelocity(.2)); + rotatingRight = true; + } + + if (!rightSensorValue) { + rotationAngleMotor.setControl(rotationVelocityControl.withVelocity(.1)); + } + if (!leftSensorValue) { + rotationAngleMotor.setControl(rotationVelocityControl.withVelocity(0)); double currentMotorPosition = rotationAngleMotor.getMotorPosition(); leftLimit = currentMotorPosition; rightLimit = currentMotorPosition + DISTANCE_BETWEEN_BEAM_BREAKS; isCalibrated = true; } - if (!rightSensorValue) { - double currentMotorPosition = rotationAngleMotor.getMotorPosition(); - leftLimit = currentMotorPosition - DISTANCE_BETWEEN_BEAM_BREAKS; - rightLimit = currentMotorPosition; - isCalibrated = true; - } } private void setAutomaticRotationAngle(){ From 3ffee8ac05de225c104443b60870de2f585a0da9 Mon Sep 17 00:00:00 2001 From: Shishir Ghate Date: Wed, 25 Feb 2026 16:52:29 -0600 Subject: [PATCH 7/9] Added device crash check --- src/main/java/com/team1816/season/subsystems/Shooter.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/java/com/team1816/season/subsystems/Shooter.java b/src/main/java/com/team1816/season/subsystems/Shooter.java index e6a0ead..24a82f8 100644 --- a/src/main/java/com/team1816/season/subsystems/Shooter.java +++ b/src/main/java/com/team1816/season/subsystems/Shooter.java @@ -160,6 +160,10 @@ public Shooter(){ public void periodic() { readFromHardware(); + if (rotationAngleMotor.hasDeviceCrashed()) { + isCalibrated = false; + } + if (!isCalibrated) { calibratePeriodic(); } else { From 237cc8a36db192c3727ab8e1717ed9e1bcf9dc1e Mon Sep 17 00:00:00 2001 From: clarkmilbert Date: Fri, 27 Feb 2026 19:51:58 -0600 Subject: [PATCH 8/9] Add shooter automatic calibration. Needs some constants and doesn't currently have a manual mode. --- .../team1816/season/subsystems/Shooter.java | 82 ++++++++++--------- .../season/subsystems/Superstructure.java | 8 +- 2 files changed, 46 insertions(+), 44 deletions(-) diff --git a/src/main/java/com/team1816/season/subsystems/Shooter.java b/src/main/java/com/team1816/season/subsystems/Shooter.java index 24a82f8..ae3735b 100644 --- a/src/main/java/com/team1816/season/subsystems/Shooter.java +++ b/src/main/java/com/team1816/season/subsystems/Shooter.java @@ -1,8 +1,8 @@ package com.team1816.season.subsystems; +import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.controls.VoltageOut; import com.pathplanner.lib.util.FlippingUtil; import com.team1816.lib.BaseRobotState; import com.team1816.lib.hardware.components.motor.IMotor; @@ -12,13 +12,13 @@ import com.team1816.lib.util.ShooterTableCalculator; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.networktables.DoubleArrayPublisher; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; @@ -63,17 +63,18 @@ public class Shooter extends SubsystemBase implements ITestableSubsystem { private final double MOTOR_ROTATIONS_PER_LAUNCH_ANGLE_DEGREE; private final double MOTOR_ROTATIONS_PER_ROTATION_ANGLE_DEGREE; private final Translation3d SHOOTER_OFFSET; - private final Rotation2d CALIBRATION_POSITION_ARC_ANGLE; - private final Rotation2d ROTATION_OFFSET_FROM_CALIBRATION_ZERO; private final double HALF_FIELD_WIDTH = FlippingUtil.fieldSizeY/2; - private final double DISTANCE_BETWEEN_BEAM_BREAKS; - private double leftLimit = 0; - private double rightLimit = 0; //CALIBRATION - private Double[] calibrationPositions = new Double[]{0.0, 0.0}; - private boolean isCalibrated = false; - private boolean rotatingRight = false; + private final double FAST_CALIBRATION_SPEED = .2; + private final double SLOW_CALIBRATION_SPEED = .1; + private final double EXTRA_SLOW_CALIBRATION_SPEED = .05; + private final double CALIBRATION_STALL_SECONDS = .5; + private final double ONE_TURRET_ROTATION = 1; + private double initialCalibrationTimestamp = -1; + private CALIBRATION_STATE calibrationState; + private double rotationAngleMotorOffsetRotations; + private final DutyCycleOut dutyCycleOutRequest = new DutyCycleOut(0); //MECHANISMS private final NetworkTable networkTable; @@ -139,18 +140,22 @@ public enum SHOOTER_STATE { } } + public enum CALIBRATION_STATE { + CALIBRATING, + CALIBRATED + } + public Shooter(){ super(); MOTOR_ROTATIONS_PER_LAUNCH_ANGLE_DEGREE = factory.getConstant(NAME, "motorRotationsPerLaunchAngleDegree", 0); //TODO WHEN PHYSICAL SUBSYSTEM EXISTS, set this. MOTOR_ROTATIONS_PER_ROTATION_ANGLE_DEGREE = factory.getConstant(NAME, "motorRotationsPerRotationAngleDegree", 0); //TODO WHEN PHYSICAL SUBSYSTEM EXISTS, set this. SHOOTER_OFFSET = new Translation3d(factory.getConstant(NAME, "initialShooterOffsetX",0), factory.getConstant(NAME, "initialShooterOffsetY",0), factory.getConstant(NAME, "initialShooterOffsetZ",0)); //TODO WHEN PHYSICAL SUBSYSTEM EXISTS, set this. - CALIBRATION_POSITION_ARC_ANGLE = Rotation2d.fromRotations(factory.getConstant(NAME, "calibrationPositionArcAngle", 0.75)); //should always be less than 1 rotation //TODO WHEN PHYSICAL SUBSYSTEM EXISTS, set this. - ROTATION_OFFSET_FROM_CALIBRATION_ZERO = Rotation2d.fromDegrees(factory.getConstant(NAME, "rotationOffsetFromCalibrationZero", 70)); //as a note, the rotation motor should move clockwise on positive dutycycle, otherwise directions will be flipped //TODO WHEN PHYSICAL SUBSYSTEM EXISTS, set this. - DISTANCE_BETWEEN_BEAM_BREAKS = factory.getConstant(NAME,"distanceBetweenBeamBreaks",0); launcherTranslation = new Translation3d(0,0,0).plus(SHOOTER_OFFSET); + calibrationState = CALIBRATION_STATE.CALIBRATING; + networkTable = NetworkTableInstance.getDefault().getTable(""); turretFieldPose = networkTable.getDoubleArrayTopic("Field/Turret").publish(); SmartDashboard.putData("Shooter Incline", launchMech); @@ -161,10 +166,10 @@ public void periodic() { readFromHardware(); if (rotationAngleMotor.hasDeviceCrashed()) { - isCalibrated = false; + calibrationState = CALIBRATION_STATE.CALIBRATING; } - if (!isCalibrated) { + if (!(calibrationState == CALIBRATION_STATE.CALIBRATED)) { calibratePeriodic(); } else { applyState(); @@ -257,22 +262,22 @@ public void setCurrentAutoAimTarget(AUTO_AIM_TARGETS target) { } private void calibratePeriodic(){ - // TODO: figure out which sensor value is a lower number of ticks and change left/right - if (!rotatingRight) { - rotationAngleMotor.setControl(rotationVelocityControl.withVelocity(.2)); - rotatingRight = true; - } - - if (!rightSensorValue) { - rotationAngleMotor.setControl(rotationVelocityControl.withVelocity(.1)); - } - - if (!leftSensorValue) { - rotationAngleMotor.setControl(rotationVelocityControl.withVelocity(0)); - double currentMotorPosition = rotationAngleMotor.getMotorPosition(); - leftLimit = currentMotorPosition; - rightLimit = currentMotorPosition + DISTANCE_BETWEEN_BEAM_BREAKS; - isCalibrated = true; + // TODO: figure out if left, right, and clockwise are used correctly + if (calibrationState == CALIBRATION_STATE.CALIBRATING) { + if (!rightSensorValue && !leftSensorValue) { + rotationAngleMotor.setControl(dutyCycleOutRequest.withOutput(EXTRA_SLOW_CALIBRATION_SPEED)); + if (initialCalibrationTimestamp == -1) { + initialCalibrationTimestamp = Timer.getFPGATimestamp(); + } + else if (Timer.getFPGATimestamp() - initialCalibrationTimestamp > CALIBRATION_STALL_SECONDS) { + rotationAngleMotorOffsetRotations = rotationAngleMotor.getMotorPosition(); + calibrationState = CALIBRATION_STATE.CALIBRATED; + } + } + else { + initialCalibrationTimestamp = -1; + rotationAngleMotor.setControl(dutyCycleOutRequest.withOutput(leftSensorValue ? FAST_CALIBRATION_SPEED : SLOW_CALIBRATION_SPEED)); + } } } @@ -302,17 +307,17 @@ private void setLaunchAngle(double wantedAngleDegrees) { } private void setRotationAngle(double wantedAngleDegrees) { - double rotations = (wantedAngleDegrees + ROTATION_OFFSET_FROM_CALIBRATION_ZERO.getDegrees()) * MOTOR_ROTATIONS_PER_ROTATION_ANGLE_DEGREE; + double rotations = wantedAngleDegrees * MOTOR_ROTATIONS_PER_ROTATION_ANGLE_DEGREE + rotationAngleMotorOffsetRotations; rotations = wrapMotorRotations(rotations); - if(isCalibrated){ + if(calibrationState == CALIBRATION_STATE.CALIBRATED){ // TODO: fix this based on if left or right is lower - if (rotations < leftLimit || rotations > rightLimit){ + if (rotations < rotationAngleMotorOffsetRotations || rotations > rotationAngleMotorOffsetRotations + ONE_TURRET_ROTATION){ GreenLogger.log("Wanted Shooter rotation is out of bounds of the calibrated positions"); } - double output = MathUtil.clamp(rotations, leftLimit, rightLimit); + double output = MathUtil.clamp(rotations, rotationAngleMotorOffsetRotations, rotationAngleMotorOffsetRotations + ONE_TURRET_ROTATION); rotationAngleMotor.setControl(positionControl.withPosition(output)); } else { @@ -322,13 +327,12 @@ private void setRotationAngle(double wantedAngleDegrees) { //this method technically causes inoptimal behavior in turret dead zones, but it shouldn't matter bc private double wrapMotorRotations(double rotations) { - double scale = (calibrationPositions[1] - calibrationPositions[0]) / CALIBRATION_POSITION_ARC_ANGLE.getRotations(); - double scaledRotations = (rotations - calibrationPositions[0]) % scale; + double scaledRotations = (rotations - rotationAngleMotorOffsetRotations) % ONE_TURRET_ROTATION; - return scaledRotations + calibrationPositions[0]; + return scaledRotations + rotationAngleMotorOffsetRotations; } public boolean isCalibrated() { - return isCalibrated; + return calibrationState == CALIBRATION_STATE.CALIBRATED; } } diff --git a/src/main/java/com/team1816/season/subsystems/Superstructure.java b/src/main/java/com/team1816/season/subsystems/Superstructure.java index 43b415a..27b44f0 100644 --- a/src/main/java/com/team1816/season/subsystems/Superstructure.java +++ b/src/main/java/com/team1816/season/subsystems/Superstructure.java @@ -107,11 +107,9 @@ public enum FeederControlState { DEFAULTING } - private WantedSuperState wantedSuperState = WantedSuperState.DEFAULT; - private WantedSuperState previousWantedSuperState = WantedSuperState.DEFAULT; - private ActualSuperState actualSuperState = ActualSuperState.DEFAULTING; - - // TODO: Add calibration state, maybe as the default here + private WantedSuperState wantedSuperState = WantedSuperState.INITIALIZING; + private WantedSuperState previousWantedSuperState = WantedSuperState.INITIALIZING; + private ActualSuperState actualSuperState = ActualSuperState.INITIALIZING; private WantedSwerveState wantedSwerveState = WantedSwerveState.MANUAL_DRIVING; //Do we need this?? private FeederControlState feederControlState = FeederControlState.DEFAULTING; //What to do with this? From 75211a4a490a431ed93435baef2cfa6f3ef7a792 Mon Sep 17 00:00:00 2001 From: clarkmilbert Date: Fri, 27 Feb 2026 19:52:41 -0600 Subject: [PATCH 9/9] Reset initial superstructure states to default --- .../java/com/team1816/season/subsystems/Superstructure.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/com/team1816/season/subsystems/Superstructure.java b/src/main/java/com/team1816/season/subsystems/Superstructure.java index 27b44f0..29c54b4 100644 --- a/src/main/java/com/team1816/season/subsystems/Superstructure.java +++ b/src/main/java/com/team1816/season/subsystems/Superstructure.java @@ -107,9 +107,9 @@ public enum FeederControlState { DEFAULTING } - private WantedSuperState wantedSuperState = WantedSuperState.INITIALIZING; - private WantedSuperState previousWantedSuperState = WantedSuperState.INITIALIZING; - private ActualSuperState actualSuperState = ActualSuperState.INITIALIZING; + private WantedSuperState wantedSuperState = WantedSuperState.DEFAULT; + private WantedSuperState previousWantedSuperState = WantedSuperState.DEFAULT; + private ActualSuperState actualSuperState = ActualSuperState.DEFAULTING; private WantedSwerveState wantedSwerveState = WantedSwerveState.MANUAL_DRIVING; //Do we need this?? private FeederControlState feederControlState = FeederControlState.DEFAULTING; //What to do with this?