diff --git a/src/main/deploy/pathplanner/paths/Shoot driving across.path b/src/main/deploy/pathplanner/paths/Shoot driving across.path index 9d737a3..4af7874 100644 --- a/src/main/deploy/pathplanner/paths/Shoot driving across.path +++ b/src/main/deploy/pathplanner/paths/Shoot driving across.path @@ -53,11 +53,11 @@ }, "prevControl": { "x": 1.9337461053582308, - "y": 1.9128436544318115 + "y": 1.912843654431812 }, "nextControl": { "x": 1.9315103048981825, - "y": 1.4128486532604976 + "y": 1.4128486532604971 }, "isLocked": false, "linkedName": null @@ -81,7 +81,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.5, + "maxVelocity": 0.8, "maxAcceleration": 10.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, @@ -95,7 +95,7 @@ "reversed": false, "folder": null, "idealStartingState": { - "velocity": 0, + "velocity": 0.0, "rotation": 180.0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/Shoot driving vertical.path b/src/main/deploy/pathplanner/paths/Shoot driving vertical.path deleted file mode 100644 index 59da45c..0000000 --- a/src/main/deploy/pathplanner/paths/Shoot driving vertical.path +++ /dev/null @@ -1,118 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.956, - "y": 4.0 - }, - "prevControl": null, - "nextControl": { - "x": 2.707930530821583, - "y": 4.031008683647302 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.3, - "y": 4.0 - }, - "prevControl": { - "x": 1.5499999999999998, - "y": 4.0 - }, - "nextControl": { - "x": 1.05, - "y": 4.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.9559102564102573, - "y": 4.0 - }, - "prevControl": { - "x": 2.7118639603594925, - "y": 3.9457652751099213 - }, - "nextControl": { - "x": 3.199956552461022, - "y": 4.054234724890079 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.3, - "y": 4.0 - }, - "prevControl": { - "x": 1.5434594129947146, - "y": 4.0568112156556175 - }, - "nextControl": { - "x": 1.0565405870052855, - "y": 3.9431887843443825 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.0, - "y": 4.0 - }, - "prevControl": { - "x": 2.7532300973825357, - "y": 3.9599423520140395 - }, - "nextControl": { - "x": 3.2467699026174643, - "y": 4.040057647985961 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.3, - "y": 4.0 - }, - "prevControl": { - "x": 1.526511041060335, - "y": 4.105795785727803 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 1.5, - "maxAcceleration": 10.0, - "maxAngularVelocity": 600.0, - "maxAngularAcceleration": 750.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 8126120..b6a032f 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 180; - public static final String GIT_SHA = "89a1f34a4a4751acadc1509890fed15d6d870930"; - public static final String GIT_DATE = "2026-03-20 23:50:03 MDT"; + public static final int GIT_REVISION = 181; + public static final String GIT_SHA = "3c1b94a2d86a624a9320493a0240ee552491beca"; + public static final String GIT_DATE = "2026-03-21 13:04:40 MDT"; public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-21 13:00:47 MDT"; - public static final long BUILD_UNIX_TIME = 1774119647384L; + public static final String BUILD_DATE = "2026-03-21 13:30:10 MDT"; + public static final long BUILD_UNIX_TIME = 1774121410165L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 6c1b4a9..fdf0af5 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -39,7 +39,7 @@ public class ShooterConstants { public static final ConfigurableDouble POS_OFFSET = new ConfigurableDouble("Positive offset", 8.); - public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", -1); + public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", -1.1); // Shoot mode tolerances public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 1.8); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index b7e43d4..f92a760 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -141,6 +141,8 @@ public class ShooterReal implements ShooterIO { // Math.abs(currentLimit.in(Amps)) > current && // Math.abs(targetVelocity.in(RotationsPerSecond)) > velocity // ) { + state.motor1TargetVelocity = RotationsPerSecond.of(percentOutput); + state.motor2TargetVelocity = RotationsPerSecond.of(percentOutput); m_shooter1Motor.set(percentOutput); m_shooter2Motor.set(percentOutput); // } else { diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index b771081..29a3686 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -416,19 +416,20 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // Drive with the robot facing towards a specific position public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos, double aimLeadTime) { - // Get the current speed of the robot - Translation2d robotSpeed = new Translation2d( - chassisSpeeds.vxMetersPerSecond, - chassisSpeeds.vyMetersPerSecond - ); - - // Calculate a point to aim ahead of the actual position. - Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos); - - // Calculate the angle between the current position and the lead position + Translation2d robotSpeedYOnly = new Translation2d(0, chassisSpeeds.vyMetersPerSecond); + double yDistance = Math.abs(getPose2d().getTranslation().getY() - fieldPos.getY()); + if ((chassisSpeeds.vyMetersPerSecond >0 &&getPose2d().getTranslation().getY() >4)||(chassisSpeeds.vyMetersPerSecond <0 &&getPose2d().getTranslation().getY() <4)){ + if (Math.abs(chassisSpeeds.vyMetersPerSecond) > 0.2) { + if (TimesNegativeOne.isRed){ + robotSpeedYOnly = new Translation2d(-SwerveDriveConstants.FAR_OFFSET.get() * yDistance * (getPose2d().getTranslation().getX()-7.28989525), chassisSpeeds.vyMetersPerSecond); + } else { + robotSpeedYOnly = new Translation2d((getPose2d().getTranslation().getX())* yDistance* SwerveDriveConstants.FAR_OFFSET.get(), chassisSpeeds.vyMetersPerSecond); + } + } } + Translation2d fieldPosLead = robotSpeedYOnly.times(aimLeadTime).plus(fieldPos); Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle(); - - + Pose2d fieldPosLeadLog = new Pose2d(fieldPosLead, new Rotation2d()); + Logger.recordOutput("Lead Aim", fieldPosLeadLog); driveFieldAngle(leftStick, ang); } @@ -541,13 +542,9 @@ public class SwerveDrive extends SubsystemBase implements Queryable { } public void enableRotationOverride(Translation2d fieldTarget, double aimLeadTime, Translation2d fieldPos) { - Translation2d robotSpeed = new Translation2d( - chassisSpeeds.vxMetersPerSecond, - chassisSpeeds.vyMetersPerSecond - ); - Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos); - System.out.println("field pos lead: " + fieldPosLead); - Logger.recordOutput("Auto Aim", fieldPosLead); + Translation2d robotSpeedYOnly = new Translation2d(0, chassisSpeeds.vyMetersPerSecond); + + Translation2d fieldPosLead = robotSpeedYOnly.times(aimLeadTime).plus(fieldPos); m_rotationOverrideTarget = fieldPosLead; m_useRotationOverride = true; } diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java index 5bd6369..597b7a3 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java @@ -73,8 +73,7 @@ public final class SwerveDriveConstants { public static final boolean INVERT_Y = true; public static final boolean INVERT_ROTATION = false; - public static ConfigurableDouble distanceTolerence = new ConfigurableDouble("Distance Tolerence", 0.5); - + public static ConfigurableDouble FAR_OFFSET = new ConfigurableDouble("Far Offset", 0.05); // public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0); public static final class ModuleSpecificConstants { //2026