Team2890/YAGSL
4
0
Fork 0
mirror of https://github.com/BroncBotz3481/YAGSL synced 2026-06-19 06:21:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Upgrading to 2025.1.0

Browse Source
This commit is contained in:
thenetworkgrinch
2024-12-09 23:26:04 +00:00
parent 9fe6551d88
commit 4bc6978a20
35 changed files with 1902 additions and 1122 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
swervelib/simulation/SwerveIMUSimulation.java
View File

@@ -6,9 +6,9 @@ import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import java.util.Optional;
import org.ironmaple.simulation.drivesims.GyroSimulation;
/**
* Simulation for {@link swervelib.SwerveDrive} IMU.
@@ -16,27 +16,16 @@ import java.util.Optional;
public class SwerveIMUSimulation
{
/**
* Main timer to control movement estimations.
*/
private final Timer timer;
/**
* The last time the timer was read, used to determine position changes.
*/
private double lastTime;
/**
* Heading of the robot.
*/
private double angle;
private final GyroSimulation gyroSimulation;
/**
* Create the swerve drive IMU simulation.
*
* @param gyroSimulation Gyro simulation from MapleSim.
*/
public SwerveIMUSimulation()
public SwerveIMUSimulation(GyroSimulation gyroSimulation)
{
timer = new Timer();
timer.start();
lastTime = timer.get();
this.gyroSimulation = gyroSimulation;
}
/**
@@ -46,7 +35,7 @@ public class SwerveIMUSimulation
*/
public Rotation2d getYaw()
{
return new Rotation2d(angle);
return gyroSimulation.getGyroReading();
}
/**
@@ -76,7 +65,7 @@ public class SwerveIMUSimulation
*/
public Rotation3d getGyroRotation3d()
{
return new Rotation3d(0, 0, angle);
return new Rotation3d(0, 0, getYaw().getRadians());
}
/**
@@ -104,9 +93,6 @@ public class SwerveIMUSimulation
Pose2d[] modulePoses,
Field2d field)
{
angle += kinematics.toChassisSpeeds(states).omegaRadiansPerSecond * (timer.get() - lastTime);
lastTime = timer.get();
field.getObject("XModules").setPoses(modulePoses);
}
@@ -117,6 +103,6 @@ public class SwerveIMUSimulation
*/
public void setAngle(double angle)
{
this.angle = angle;
this.gyroSimulation.setRotation(Rotation2d.fromRadians(angle));
}
}