From 764cb64095861fe9a9c4fa53027799f53d3ccc42 Mon Sep 17 00:00:00 2001 From: Daniel Tsur Date: Thu, 28 May 2026 21:31:20 +0300 Subject: [PATCH 1/3] Updated a library Added some code to possible fix turret oscillation when it is very cose to the max and min degrees and its moving forward or back --- .../shooter/kinematics/LaunchCalculator.java | 45 +++++++++++- .../Phoenix6-replay-frc2026-latest.json | 68 +++++++++---------- 2 files changed, 76 insertions(+), 37 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/kinematics/LaunchCalculator.java b/src/main/java/frc/robot/subsystems/shooter/kinematics/LaunchCalculator.java index 177200b..667e6f1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/kinematics/LaunchCalculator.java +++ b/src/main/java/frc/robot/subsystems/shooter/kinematics/LaunchCalculator.java @@ -16,6 +16,8 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Time; +import frc.robot.subsystems.shooter.ShooterConstants; + import org.littletonrobotics.junction.Logger; public class LaunchCalculator { @@ -34,6 +36,11 @@ private LaunchCalculator() {} private final double maxDistance = 5.16; private final double phaseDelay = 0.03; + private double turretChangingTolerance = 0; + private final double turretCloseToEdgeDegrees = 2; + private final double turretMaxTolerance = 4; + private final double turretYRobotHighSpeedCap = 1; + private Transform2d robotToTurret; public void setRobotToTurret(Transform2d transform) { @@ -203,14 +210,46 @@ public ShotParams calculate( Rotation2d turretAngle = fieldTurretAngle.minus(robotPose.getRotation()); double deg = turretAngle.getDegrees(); + if (Math.abs(robotVelocityRobotRelative.vxMetersPerSecond) < turretYRobotHighSpeedCap) { + if ((deg < turretCloseToEdgeDegrees + && robotVelocityRobotRelative.vyMetersPerSecond + < 0) // at left side of hub and moving to the right + || (360 - deg < turretCloseToEdgeDegrees + && robotVelocityRobotRelative.vyMetersPerSecond + > 0)) { // at right side of hub and moving to the left + turretChangingTolerance = + turretMaxTolerance + * (Math.abs(robotVelocityRobotRelative.vxMetersPerSecond) + / turretYRobotHighSpeedCap); + } + } else { + turretChangingTolerance = 0; + } + if (m_turretAngleFormat.isCWPositive()) { deg *= -1; // CCW → CW } if (m_turretAngleFormat.is0To360()) { - if (deg < 0) { - deg += 360; + if (turretChangingTolerance < 0) { + if(deg < 0) { + deg += 360; + } + deg %= 360; + } + else { + if (deg < 360 + turretChangingTolerance && deg > 360) { //only if the tolerance is needed + deg = 360; //set setpoint to max angle + } + else { + deg %= 360; + } + if(deg > 0 - turretChangingTolerance && deg < 0) { //only if the tolerance is needed + deg = 0; //set setpoint to min angle + } + else { + deg += 360; + } } - deg %= 360; } Angle turretAngleRobotRelative = Degrees.of(deg); diff --git a/vendordeps/Phoenix6-replay-frc2026-latest.json b/vendordeps/Phoenix6-replay-frc2026-latest.json index 9a3d886..eb07c55 100644 --- a/vendordeps/Phoenix6-replay-frc2026-latest.json +++ b/vendordeps/Phoenix6-replay-frc2026-latest.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix6-replay-frc2026-latest.json", "name": "CTRE-Phoenix (v6) Replay", - "version": "26.2.0", + "version": "26.3.0", "frcYear": "2026", "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "26.2.0" + "version": "26.3.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "26.2.0", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.2.0", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -48,7 +48,7 @@ { "groupId": "com.ctre.phoenix6.replay", "artifactId": "api-cpp-replay", - "version": "26.2.0", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -62,7 +62,7 @@ { "groupId": "com.ctre.phoenix6.replay", "artifactId": "tools-replay", - "version": "26.2.0", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -76,7 +76,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "26.2.0", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -90,7 +90,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.2.0", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -104,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.2.0", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -118,7 +118,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.2.0", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -132,7 +132,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.2.0", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -146,7 +146,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.2.0", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -160,7 +160,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.2.0", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -174,7 +174,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.2.0", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -188,7 +188,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.2.0", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -202,7 +202,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.2.0", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -216,7 +216,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.2.0", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -230,7 +230,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.2.0", + "version": "26.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -246,7 +246,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "26.2.0", + "version": "26.3.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -259,7 +259,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "26.2.0", + "version": "26.3.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.replay", "artifactId": "wpiapi-cpp-replay", - "version": "26.2.0", + "version": "26.3.0", "libName": "CTRE_Phoenix6_WPIReplay", "headerClassifier": "headers", "sharedLibrary": true, @@ -288,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.replay", "artifactId": "tools-replay", - "version": "26.2.0", + "version": "26.3.0", "libName": "CTRE_PhoenixTools_Replay", "headerClassifier": "headers", "sharedLibrary": true, @@ -304,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "26.2.0", + "version": "26.3.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -320,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "26.2.0", + "version": "26.3.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -336,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "26.2.0", + "version": "26.3.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -352,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "26.2.0", + "version": "26.3.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -368,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "26.2.0", + "version": "26.3.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -384,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "26.2.0", + "version": "26.3.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -400,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "26.2.0", + "version": "26.3.0", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -416,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "26.2.0", + "version": "26.3.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -432,7 +432,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "26.2.0", + "version": "26.3.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -448,7 +448,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "26.2.0", + "version": "26.3.0", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -464,7 +464,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdi", - "version": "26.2.0", + "version": "26.3.0", "libName": "CTRE_SimProCANdi", "headerClassifier": "headers", "sharedLibrary": true, @@ -480,7 +480,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANdle", - "version": "26.2.0", + "version": "26.3.0", "libName": "CTRE_SimProCANdle", "headerClassifier": "headers", "sharedLibrary": true, From b65ce694253b3ed98d9cebf400173cef3725c427 Mon Sep 17 00:00:00 2001 From: Daniel Tsur Date: Sun, 31 May 2026 18:59:11 +0300 Subject: [PATCH 2/3] Apllied spotless --- .../shooter/kinematics/LaunchCalculator.java | 21 +++++++------------ 1 file changed, 8 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/kinematics/LaunchCalculator.java b/src/main/java/frc/robot/subsystems/shooter/kinematics/LaunchCalculator.java index 667e6f1..6a420ac 100644 --- a/src/main/java/frc/robot/subsystems/shooter/kinematics/LaunchCalculator.java +++ b/src/main/java/frc/robot/subsystems/shooter/kinematics/LaunchCalculator.java @@ -16,8 +16,6 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Time; -import frc.robot.subsystems.shooter.ShooterConstants; - import org.littletonrobotics.junction.Logger; public class LaunchCalculator { @@ -231,22 +229,19 @@ public ShotParams calculate( } if (m_turretAngleFormat.is0To360()) { if (turretChangingTolerance < 0) { - if(deg < 0) { + if (deg < 0) { deg += 360; } deg %= 360; - } - else { - if (deg < 360 + turretChangingTolerance && deg > 360) { //only if the tolerance is needed - deg = 360; //set setpoint to max angle - } - else { + } else { + if (deg < 360 + turretChangingTolerance && deg > 360) { // only if the tolerance is needed + deg = 360; // set setpoint to max angle + } else { deg %= 360; } - if(deg > 0 - turretChangingTolerance && deg < 0) { //only if the tolerance is needed - deg = 0; //set setpoint to min angle - } - else { + if (deg > 0 - turretChangingTolerance && deg < 0) { // only if the tolerance is needed + deg = 0; // set setpoint to min angle + } else { deg += 360; } } From 3a836702ace103c9a39dda1ab27db10a0d2cf9db Mon Sep 17 00:00:00 2001 From: Daniel Tsur Date: Thu, 4 Jun 2026 18:56:22 +0300 Subject: [PATCH 3/3] Moved the turret oscillation prevention to `ShootToTargetCmd` from `LaunchCalculator` --- .../subsystems/shooter/ShootToTargetCmd.java | 26 ++++++++++++ .../shooter/kinematics/LaunchCalculator.java | 40 ++----------------- 2 files changed, 29 insertions(+), 37 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShootToTargetCmd.java b/src/main/java/frc/robot/subsystems/shooter/ShootToTargetCmd.java index da0110a..ccb87c1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShootToTargetCmd.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShootToTargetCmd.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.shooter; +import static edu.wpi.first.units.Units.Degrees; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -17,6 +19,9 @@ public class ShootToTargetCmd extends Command { private final HoodSubsystem m_hood; private final FlywheelSubsystem m_flywheel; + private final double turretCloseToEdgeDegrees = 2; + private final double turretYRobotHighSpeedCap = 1; + private Supplier estimatedRobotPoseSupplier; private Supplier robotSpeedsSupplier; private Supplier fieldSpeedsSupplier; @@ -102,6 +107,27 @@ public void execute() { target); m_turret.runAngleDirect(params.turretAngle()); + + boolean shouldTurretTurn = false; + double deg = params.turretAngle().in(Degrees); + + if (Math.abs(robotSpeedsSupplier.get().vxMetersPerSecond) < turretYRobotHighSpeedCap) { + if ((deg < turretCloseToEdgeDegrees + && robotSpeedsSupplier.get().vyMetersPerSecond + < 0) // at left side of hub and moving to the right + || (360 - deg < turretCloseToEdgeDegrees + && robotSpeedsSupplier.get().vyMetersPerSecond + > 0)) { // at right side of hub and moving to the left + shouldTurretTurn = false; + } + } else { + shouldTurretTurn = true; + } + + if (shouldTurretTurn) { + m_turret.runAngleDirect(params.turretAngle()); + } + m_flywheel.runVelocityDirect(params.flywheelAngularVelocity()); m_hood.runAngleDirect(params.hoodAngle()); } diff --git a/src/main/java/frc/robot/subsystems/shooter/kinematics/LaunchCalculator.java b/src/main/java/frc/robot/subsystems/shooter/kinematics/LaunchCalculator.java index 6a420ac..177200b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/kinematics/LaunchCalculator.java +++ b/src/main/java/frc/robot/subsystems/shooter/kinematics/LaunchCalculator.java @@ -34,11 +34,6 @@ private LaunchCalculator() {} private final double maxDistance = 5.16; private final double phaseDelay = 0.03; - private double turretChangingTolerance = 0; - private final double turretCloseToEdgeDegrees = 2; - private final double turretMaxTolerance = 4; - private final double turretYRobotHighSpeedCap = 1; - private Transform2d robotToTurret; public void setRobotToTurret(Transform2d transform) { @@ -208,43 +203,14 @@ public ShotParams calculate( Rotation2d turretAngle = fieldTurretAngle.minus(robotPose.getRotation()); double deg = turretAngle.getDegrees(); - if (Math.abs(robotVelocityRobotRelative.vxMetersPerSecond) < turretYRobotHighSpeedCap) { - if ((deg < turretCloseToEdgeDegrees - && robotVelocityRobotRelative.vyMetersPerSecond - < 0) // at left side of hub and moving to the right - || (360 - deg < turretCloseToEdgeDegrees - && robotVelocityRobotRelative.vyMetersPerSecond - > 0)) { // at right side of hub and moving to the left - turretChangingTolerance = - turretMaxTolerance - * (Math.abs(robotVelocityRobotRelative.vxMetersPerSecond) - / turretYRobotHighSpeedCap); - } - } else { - turretChangingTolerance = 0; - } - if (m_turretAngleFormat.isCWPositive()) { deg *= -1; // CCW → CW } if (m_turretAngleFormat.is0To360()) { - if (turretChangingTolerance < 0) { - if (deg < 0) { - deg += 360; - } - deg %= 360; - } else { - if (deg < 360 + turretChangingTolerance && deg > 360) { // only if the tolerance is needed - deg = 360; // set setpoint to max angle - } else { - deg %= 360; - } - if (deg > 0 - turretChangingTolerance && deg < 0) { // only if the tolerance is needed - deg = 0; // set setpoint to min angle - } else { - deg += 360; - } + if (deg < 0) { + deg += 360; } + deg %= 360; } Angle turretAngleRobotRelative = Degrees.of(deg);