From f5ecfd0be19548215558eee7217cf6eb1b8c487b Mon Sep 17 00:00:00 2001 From: mimigamin Date: Sat, 14 Mar 2026 20:00:21 -0600 Subject: [PATCH] Finish simulation point to hub --- simgui-ds.json | 5 ++ .../robot/commands/Swerve/StayInPosition.java | 21 ++++--- .../robot/constants/BuildConstants.java | 10 ++-- .../subsystems/swerve/SimpleSwerveSim.java | 58 ++++++++++++++----- .../robot/subsystems/swerve/SwerveDrive.java | 9 +-- 5 files changed, 68 insertions(+), 35 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..4a63cc1 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "System Joysticks": { + "window": { + "enabled": false + } + }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java b/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java index 4b66de2..b306a27 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java +++ b/src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java @@ -4,6 +4,9 @@ package frc4388.robot.commands.Swerve; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.AutoLogOutput; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; import frc4388.robot.commands.PID; @@ -11,6 +14,7 @@ import frc4388.robot.subsystems.swerve.SwerveDrive; public class StayInPosition extends PID { SwerveDrive drive; + Translation2d driveTranslation = new Translation2d(); public StayInPosition(SwerveDrive drive) { super(0.3, 0.0, 0.0, 0.0, 1); @@ -20,24 +24,23 @@ public class StayInPosition extends PID { public void goToTargetPose(Pose2d targetPose) { Pose2d currentPose = drive.getCurrentPose(); - double translationX = targetPose.getX() - currentPose.getX(); double translationY = targetPose.getY() - currentPose.getY(); - - double magnitude = Math.sqrt(translationX * translationX + translationY * translationY); - if (magnitude > 1.0) { - translationX /= magnitude; - translationY /= magnitude; + if (translationX > 0.8){ + translationX = 0.8; } - - Translation2d driveTranslation; - if (magnitude < 0.05) { + if (translationY > 0.8){ + translationY = 0.8; + } + if (Math.abs(translationX) < 0.01 && Math.abs(translationY) < 0.01) { driveTranslation = new Translation2d(); } else { driveTranslation = new Translation2d(translationX, translationY); } drive.driveFieldAngleSIP(driveTranslation, targetPose.getRotation()); + // If above doesn't work + //drive.driveFieldAngle(driveTranslation, targetPose.getRotation()); } @Override diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 44b0323..16f7820 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 = 159; - public static final String GIT_SHA = "ebab028818fc0362daa4d7eb1010131819edd1cb"; - public static final String GIT_DATE = "2026-03-14 18:29:15 MDT"; + public static final int GIT_REVISION = 160; + public static final String GIT_SHA = "8c8ac261397954f1b5a118cb96d4e5f60d70da81"; + public static final String GIT_DATE = "2026-03-14 18:57:04 MDT"; public static final String GIT_BRANCH = "MiraOrg"; - public static final String BUILD_DATE = "2026-03-14 18:46:53 MDT"; - public static final long BUILD_UNIX_TIME = 1773535613285L; + public static final String BUILD_DATE = "2026-03-14 19:59:14 MDT"; + public static final long BUILD_UNIX_TIME = 1773539954157L; 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 a374ac8..3a177bb 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java @@ -24,27 +24,59 @@ public class SimpleSwerveSim implements SwerveIO { public SimpleSwerveSim() { } - @Override public synchronized void setControl(SwerveRequest ctrl) { if (ctrl == null) return; + // Handle FieldCentricFacingAngle — compute omega from target direction + if (ctrl instanceof SwerveRequest.FieldCentricFacingAngle facingAngle) { + vx = facingAngle.VelocityX; + vy = facingAngle.VelocityY; + + // Simple P controller to rotate toward target + double currentAngle = pose.getRotation().getRadians(); + double targetAngle = facingAngle.TargetDirection.getRadians(); + double error = targetAngle - currentAngle; + + // Wrap error to [-pi, pi] + error = Math.atan2(Math.sin(error), Math.cos(error)); + + double kP = 5.0; // tune this — matches PathPlanner's rotation PID + omega = error * kP; + return; + } + + // Handle FieldCentric (normal driving with explicit rotational rate) + if (ctrl instanceof SwerveRequest.FieldCentric fc) { + vx = fc.VelocityX; + vy = fc.VelocityY; + omega = fc.RotationalRate; + return; + } + + if (ctrl instanceof SwerveRequest.RobotCentric rc) { + // rotate velocity into field frame + double cos = pose.getRotation().getCos(); + double sin = pose.getRotation().getSin(); + double vxRobot = rc.VelocityX; + double vyRobot = rc.VelocityY; + vx = vxRobot * cos - vyRobot * sin; + vy = vxRobot * sin + vyRobot * cos; + omega = rc.RotationalRate; + return; + } + + // Handle brake + if (ctrl instanceof SwerveRequest.SwerveDriveBrake) { + vx = 0; vy = 0; omega = 0; + return; + } + + // Fallback: your original reflection approach ChassisSpeeds cs = tryGetSpeedsField(ctrl); if (cs != null) { vx = cs.vxMetersPerSecond; vy = cs.vyMetersPerSecond; omega = cs.omegaRadiansPerSecond; - return; - } - - try { - Class cls = ctrl.getClass(); - double vxF = tryGetDoubleField(ctrl, cls, "VelocityX", "velocityX", "velocityx", "VelX"); - double vyF = tryGetDoubleField(ctrl, cls, "VelocityY", "velocityY", "velocityy", "VelY"); - double rotF = tryGetDoubleField(ctrl, cls, "RotationalRate", "rotationalRate", "rotationalrate", "omega", "Omega"); - vx = vxF; - vy = vyF; - omega = rotF; - } catch (Exception e) { } } diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index e90be1b..d3664f3 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -297,11 +297,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { public void driveFieldAngleSIP(Translation2d leftStick, Rotation2d heading) { - leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset); - leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis); - - rotTarget = heading.getDegrees(); - + rotTarget = heading.getDegrees(); var ctrl = new SwerveRequest.FieldCentricFacingAngle() .withVelocityX(leftStick.getX() * speedAdjust) .withVelocityY(leftStick.getY() * speedAdjust) @@ -310,9 +306,6 @@ public class SwerveDrive extends SubsystemBase implements Queryable { SwerveDriveConstants.PIDConstants.AIM_kP.get(), SwerveDriveConstants.PIDConstants.AIM_kI.get(), SwerveDriveConstants.PIDConstants.AIM_kD.get() - // SwerveDriveConstants.PIDConstants.AIM_GAINS.kP, - // SwerveDriveConstants.PIDConstants.AIM_GAINS.kI, - // SwerveDriveConstants.PIDConstants.AIM_GAINS.kD ); io.setControl(ctrl); }