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

fix simulation auto

Browse Source
This commit is contained in:
mimigamin
2026-03-14 15:41:27 -06:00
parent 2de8c60597
commit 07ec609b01
3 changed files with 27 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java
+22 -2
View File
@@ -8,6 +8,8 @@ import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotBase;
import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
public class SimpleSwerveSim implements SwerveIO {
@@ -100,9 +102,27 @@ public class SimpleSwerveSim implements SwerveIO {
lastTimeNs = now;
lastPose = pose;
double dxField;
double dyField;
if (DriverStation.isAutonomous()) {
double dxRobot = vx * dt;
double dyRobot = vy * dt;
Rotation2d r = pose.getRotation();
double cos = r.getCos();
double sin = r.getSin();
dxField = dxRobot * cos - dyRobot * sin;
dyField = dxRobot * sin + dyRobot * cos;
} else {
dxField = vx * dt;
dyField = vy * dt;
}
double dxField = vx * dt;
double dyField = vy * dt;
double dTheta = omega * dt;
Translation2d newTrans = pose.getTranslation().plus(new Translation2d(dxField, dyField));