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

reversed odometry WIP

Browse Source
This commit is contained in:
Aarav Shah
2021-03-22 18:13:37 -06:00
parent b0f4857b0d
commit 1e7d224d5e
1 changed files with 11 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Drive.java
+11 -1
View File
@@ -331,10 +331,20 @@ public class Drive extends SubsystemBase {
m_totalRightDistanceInches += ticksToInches(m_currentRightPosTicks - m_lastRightPosTicks); m_totalRightDistanceInches += ticksToInches(m_currentRightPosTicks - m_lastRightPosTicks);
m_totalLeftDistanceInches += ticksToInches(m_currentLeftPosTicks - m_lastLeftPosTicks); m_totalLeftDistanceInches += ticksToInches(m_currentLeftPosTicks - m_lastLeftPosTicks);
updateOdometry(false);
}
m_odometry.update(Rotation2d.fromDegrees( getHeading()), public void updateOdometry(boolean reversed) {
if (reversed) {
m_odometry.update(Rotation2d.fromDegrees( getHeading()-180),
-inchesToMeters(getDistanceInches(m_rightFrontMotor)),
inchesToMeters(getDistanceInches(m_leftFrontMotor)));
}
else {
m_odometry.update(Rotation2d.fromDegrees( getHeading()),
inchesToMeters(getDistanceInches(m_leftFrontMotor)), inchesToMeters(getDistanceInches(m_leftFrontMotor)),
-inchesToMeters(getDistanceInches(m_rightFrontMotor))); -inchesToMeters(getDistanceInches(m_rightFrontMotor)));
}
} }
/** /**