Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 08:48:07 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'swerve' of https://github.com/Team4388/2022NoWayHome into swerve

Browse Source
This commit is contained in:
Ryan Manley
2022-02-19 11:19:15 -07:00
parent a75e98d911 91ad9521c1
commit 9530cbdaa7
1 changed files with 13 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+13 -4
View File
@@ -56,7 +56,7 @@ public class SwerveDrive extends SubsystemBase {
public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
public boolean ignoreAngles;
public Rotation2d rotTarget = new Rotation2d();;
public Rotation2d rotTarget = new Rotation2d();
private final Field2d m_field = new Field2d();
@@ -204,6 +204,15 @@ public class SwerveDrive extends SubsystemBase {
return m_poseEstimator.getEstimatedPosition();
}
/**
* Gets the current gyro using regression formula.
* @return Rotation2d object holding current gyro in radians
*/
public Rotation2d getRegGyro() {
double regCur = 0.6552670369 + m_gyro.getRotation2d().getDegrees() * 0.9926871527;
return new Rotation2d(regCur * Math.PI / 180);
}
/**
* Resets the odometry of the robot to the given pose.
*/
@@ -211,10 +220,10 @@ public class SwerveDrive extends SubsystemBase {
m_poseEstimator.resetPosition(pose, m_gyro.getRotation2d());
}
/** Updates the field relative position of the robot. */
/** Updates the field relative position of the robot.
*/
public void updateOdometry() {
m_poseEstimator.update( m_gyro.getRotation2d(),
m_poseEstimator.update( getRegGyro(),
modules[0].getState(),
modules[1].getState(),
modules[2].getState(),