SCDS/FTCRoadRunnerRobotics
7
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

added functionality to IMU methods in PoseUpdater

Browse Source
This commit is contained in:
Anyi Lin
2024-11-21 15:35:42 -05:00
parent ea381fa92c
commit 437635838b
4 changed files with 42 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/Localizer.java
View File

@ -1,5 +1,7 @@
package org.firstinspires.ftc.teamcode.pedroPathing.localization;
import com.qualcomm.robotcore.hardware.IMU;
import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
/**
@ -91,4 +93,13 @@ public abstract class Localizer {
* This resets the IMU of the localizer, if applicable.
*/
public abstract void resetIMU();
/**
* This is overridden to return the IMU, if there is one.
*
* @return returns the IMU if it exists
*/
public IMU getIMU() {
return null;
}
}

14
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/PoseUpdater.java
View File

@ -60,6 +60,7 @@ public class PoseUpdater {
}
this.localizer = localizer;
imu = localizer.getIMU();
}
/**
@ -300,7 +301,9 @@ public class PoseUpdater {
* This resets the heading of the robot to the IMU's heading, using Road Runner's pose reset.
*/
public void resetHeadingToIMU() {
localizer.setPose(new Pose(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading()));
if (imu != null) {
localizer.setPose(new Pose(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading()));
}
}
/**
@ -309,7 +312,9 @@ public class PoseUpdater {
* method.
*/
public void resetHeadingToIMUWithOffsets() {
setCurrentPoseWithOffset(new Pose(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading()));
if (imu != null) {
setCurrentPoseWithOffset(new Pose(getPose().getX(), getPose().getY(), getNormalizedIMUHeading() + startingPose.getHeading()));
}
}
/**
@ -318,7 +323,10 @@ public class PoseUpdater {
* @return returns the normalized IMU heading.
*/
public double getNormalizedIMUHeading() {
return MathFunctions.normalizeAngle(-imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
if (imu != null) {
return MathFunctions.normalizeAngle(-imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
}
return 0;
}
/**

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/ThreeWheelIMULocalizer.java
View File

@ -313,4 +313,14 @@ public class ThreeWheelIMULocalizer extends Localizer {
public void resetIMU() {
imu.resetYaw();
}
/**
* This is returns the IMU.
*
* @return returns the IMU
*/
@Override
public IMU getIMU() {
return imu;
}
}

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java
View File

@ -296,4 +296,14 @@ public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo w
public void resetIMU() {
imu.resetYaw();
}
/**
* This is returns the IMU.
*
* @return returns the IMU
*/
@Override
public IMU getIMU() {
return imu;
}
}