From 3ef0a876f602cd296356b4ae63e82c63a1ebb960 Mon Sep 17 00:00:00 2001 From: mimigamin Date: Thu, 12 Mar 2026 18:05:14 -0600 Subject: [PATCH] not working --- .../paths/CPlayerStation-CShoot.path | 2 +- .../paths/HubCenter-PlayerStation.path | 2 +- .../paths/HubFarLeft-NeutralZone 1-2.path | 2 +- .../paths/HubFarLeft-NeutralZone 2-2.path | 2 +- .../paths/HubFarRight-NeutralZone 2-2.path | 2 +- .../paths/HubRight-PlayerStation.path | 2 +- .../paths/PlayerStation-Shoot.path | 2 +- .../deploy/pathplanner/paths/ReadyPush.path | 2 +- .../java/frc4388/robot/RobotContainer.java | 21 ++++++--- .../robot/constants/BuildConstants.java | 10 ++--- .../subsystems/swerve/SimpleSwerveSim.java | 43 +------------------ .../robot/subsystems/swerve/SwerveDrive.java | 1 - 12 files changed, 28 insertions(+), 63 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/CPlayerStation-CShoot.path b/src/main/deploy/pathplanner/paths/CPlayerStation-CShoot.path index 7325176..dd06473 100644 --- a/src/main/deploy/pathplanner/paths/CPlayerStation-CShoot.path +++ b/src/main/deploy/pathplanner/paths/CPlayerStation-CShoot.path @@ -58,7 +58,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 12.0, + "nominalVoltage": 10.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/HubCenter-PlayerStation.path b/src/main/deploy/pathplanner/paths/HubCenter-PlayerStation.path index cbd5c03..3307f61 100644 --- a/src/main/deploy/pathplanner/paths/HubCenter-PlayerStation.path +++ b/src/main/deploy/pathplanner/paths/HubCenter-PlayerStation.path @@ -104,7 +104,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 12.0, + "nominalVoltage": 10.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 1-2.path b/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 1-2.path index cf8bda0..c2d5d1a 100644 --- a/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 1-2.path +++ b/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 1-2.path @@ -48,7 +48,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 12.0, + "nominalVoltage": 10.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 2-2.path b/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 2-2.path index 2cab886..ae1d06a 100644 --- a/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 2-2.path +++ b/src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 2-2.path @@ -88,7 +88,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 12.0, + "nominalVoltage": 10.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone 2-2.path b/src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone 2-2.path index ddc66c0..79a2251 100644 --- a/src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone 2-2.path +++ b/src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone 2-2.path @@ -88,7 +88,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 12.0, + "nominalVoltage": 10.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/HubRight-PlayerStation.path b/src/main/deploy/pathplanner/paths/HubRight-PlayerStation.path index 0d931a0..ab1ab91 100644 --- a/src/main/deploy/pathplanner/paths/HubRight-PlayerStation.path +++ b/src/main/deploy/pathplanner/paths/HubRight-PlayerStation.path @@ -64,7 +64,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 12.0, + "nominalVoltage": 10.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/PlayerStation-Shoot.path b/src/main/deploy/pathplanner/paths/PlayerStation-Shoot.path index 61b8049..992c038 100644 --- a/src/main/deploy/pathplanner/paths/PlayerStation-Shoot.path +++ b/src/main/deploy/pathplanner/paths/PlayerStation-Shoot.path @@ -58,7 +58,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 12.0, + "nominalVoltage": 10.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/deploy/pathplanner/paths/ReadyPush.path b/src/main/deploy/pathplanner/paths/ReadyPush.path index 3c8bced..4d01629 100644 --- a/src/main/deploy/pathplanner/paths/ReadyPush.path +++ b/src/main/deploy/pathplanner/paths/ReadyPush.path @@ -48,7 +48,7 @@ "maxAcceleration": 3.0, "maxAngularVelocity": 600.0, "maxAngularAcceleration": 750.0, - "nominalVoltage": 12.0, + "nominalVoltage": 10.0, "unlimited": false }, "goalEndState": { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 00ab2c5..11e2a8e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -40,6 +40,7 @@ import frc4388.robot.subsystems.intake.Intake.IntakeMode; import frc4388.robot.subsystems.led.LED; import frc4388.robot.subsystems.shooter.Shooter; import frc4388.robot.subsystems.shooter.ShooterConstants; +import frc4388.robot.subsystems.swerve.SimpleSwerveSim; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Lidar; import frc4388.robot.subsystems.vision.Vision; @@ -69,6 +70,7 @@ public class RobotContainer { /* Subsystems */ // public final Lidar m_lidar = new Lidar(); public final LED m_robotLED = new LED(Constants.LEDConstants.LED_SPARK_ID); + public final SimpleSwerveSim m_robotSwerveSIM = new SimpleSwerveSim(); //Testing of Colors public final Vision m_vision = new Vision(m_robotMap.rightCamera, m_robotMap.leftCamera); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); @@ -243,14 +245,19 @@ public class RobotContainer { // IF the driver is holding the aim button, aim the robot towards the hub and shooter ready new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5) - .whileTrue(new RunCommand( + .onTrue(new InstantCommand( () -> { - m_robotSwerveDrive.driveFacingPosition( - getDeadbandedDriverController().getLeft(), - FieldPositions.HUB_POSITION, - ShooterConstants.AIM_LEAD_TIME.get() - ); - }, m_robotSwerveDrive) + m_robotSwerveSIM.driveFacingPosition( + FieldPositions.HUB_POSITION + ); + }) + // () -> { + // m_robotSwerveDrive.driveFacingPosition( + // getDeadbandedDriverController().getLeft(), + // FieldPositions.HUB_POSITION, + // ShooterConstants.AIM_LEAD_TIME.get() + // ); + // }, m_robotSwerveDrive) // () -> { // m_robotSwerveDrive.driveFacingVelocity( // getDeadbandedDriverController().getLeft(), diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 1a1f2b3..0f4ec98 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 = 152; - public static final String GIT_SHA = "4f361a805deb8cb48e62bc0a6c3a7406d6aa5f72"; - public static final String GIT_DATE = "2026-03-11 22:49:27 MDT"; + public static final int GIT_REVISION = 153; + public static final String GIT_SHA = "c4b8e2972e7be654bd544019760a174afc4871ae"; + public static final String GIT_DATE = "2026-03-12 00:08:36 MDT"; public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-12 00:02:41 MDT"; - public static final long BUILD_UNIX_TIME = 1773295361724L; + public static final String BUILD_DATE = "2026-03-12 18:03:03 MDT"; + public static final long BUILD_UNIX_TIME = 1773360183045L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java b/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java index 78fcd01..a79794c 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java @@ -132,48 +132,7 @@ public class SimpleSwerveSim implements SwerveIO { } - public synchronized void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos, double aimLeadTime) { - System.out.println("It has worked!"); - System.out.println("It has worked!"); - System.out.println("It has worked!"); - System.out.println("It has worked!"); - System.out.println("It has worked!"); - System.out.println("It has worked!"); - System.out.println("It has worked!"); - if (leftStick == null || fieldPos == null) return; - - // current robot speed (robot-relative) - Translation2d robotSpeed = new Translation2d(vx, vy); - - // lead the target by robot motion over aimLeadTime - Translation2d fieldPosLead = fieldPos.plus(robotSpeed.times(aimLeadTime)); - - // compute angle from robot to lead point (field frame) - Rotation2d toLead = fieldPosLead.minus(pose.getTranslation()).getAngle(); - // Rotation2d ang = fieldPosLead.minus(getPose2d().getTranslation()).getAngle(); - - // compute shortest angle error (ang_error = desired - current) normalized to [-pi,pi] - double currentYaw = pose.getRotation().getRadians(); - double desiredYaw = toLead.getRadians(); - double error = desiredYaw - currentYaw; - // normalize - while (error > Math.PI) error -= 2 * Math.PI; - while (error < -Math.PI) error += 2 * Math.PI; - - // simple P controller for rotation - final double KP_ROT = 2.0; // tune as needed - final double MAX_OMEGA = 6.0; // rad/s cap - double commandedOmega = KP_ROT * error; - if (commandedOmega > MAX_OMEGA) commandedOmega = MAX_OMEGA; - if (commandedOmega < -MAX_OMEGA) commandedOmega = -MAX_OMEGA; - this.omega = commandedOmega; - - // apply translational command from leftStick (assume stick in [-1,1]) - final double STICK_SPEED_MPS = 3.0; // 3 m/s at full stick; tune as needed - this.vx = leftStick.getX() * STICK_SPEED_MPS; - this.vy = leftStick.getY() * STICK_SPEED_MPS; - } - + @Override public synchronized void resetPose(Pose2d p) { if (p == null) return; diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 3ee51e6..071a21d 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -362,7 +362,6 @@ 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,