diff --git a/src/main/java/frc/robot/subsystems/shooter/ShootToTargetCmd.java b/src/main/java/frc/robot/subsystems/shooter/ShootToTargetCmd.java index 2b97283..f869bef 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.filter.Debouncer; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -28,6 +30,12 @@ private enum FeedState { 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; private Supplier m_robotPoseSupplier = Pose2d::new; private Supplier m_robotRelativeSpeedsSupplier = ChassisSpeeds::new; private Supplier m_fieldRelativeSpeedsSupplier = ChassisSpeeds::new; @@ -117,6 +125,27 @@ private void updateShooter() { 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/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,