Team4388/2026KPopRobotHunters
1
0
Fork 0
mirror of https://github.com/Team4388/2026KPopRobotHunters.git synced 2026-06-09 00:38:03 -06:00
Code Issues Packages Projects Releases Wiki Activity

Finish simulation point to hub

Browse Source
This commit is contained in:
mimigamin
2026-03-14 20:00:21 -06:00
parent 8c8ac26139
commit f5ecfd0be1
5 changed files with 68 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java
+45 -13
View File
@@ -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) {
}
}
src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java
+1 -8
View File
@@ -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);
}