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

Added fixes to external odometry processors.

Browse Source 
Signed-off-by: Logan-Nash <logannash@Mac.attlocal.net>
This commit is contained in:
Logan-Nash
2024-10-25 11:54:29 -04:00
committed by Logan-Nash
parent c5f33af419
commit 6830a82403
2 changed files with 14 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java
View File

@ -108,7 +108,13 @@ public class OTOSLocalizer extends Localizer {
*/
@Override
public Pose getPose() {
return MathFunctions.addPoses(startPose, new Pose(otosPose.x, otosPose.y, otosPose.h));
SparkFunOTOS.Pose2D rawPose = otos.getPosition();
Pose pose = new Pose(rawPose.x, rawPose.y, rawPose.h);
Vector vec = pose.getVector();
vec.rotateVector(startPose.getHeading());
return MathFunctions.addPoses(startPose, new Pose(vec.getXComponent(), vec.getYComponent(), pose.getHeading()));
}
/**

9
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java
View File

@ -96,8 +96,13 @@ public class PinpointLocalizer extends Localizer {
*/
@Override
public Pose getPose() {
Pose2D pose = odo.getPosition();
return new Pose(pose.getX(DistanceUnit.INCH), pose.getY(DistanceUnit.INCH), pose.getHeading(AngleUnit.RADIANS));
Pose2D rawPose = odo.getPosition();
Pose pose = new Pose(rawPose.getX(DistanceUnit.INCH), rawPose.getY(DistanceUnit.INCH), rawPose.getHeading(AngleUnit.RADIANS));
Vector vec = pose.getVector();
vec.rotateVector(startPose.getHeading());
return MathFunctions.addPoses(startPose, new Pose(vec.getXComponent(), vec.getYComponent(), pose.getHeading()));
}
/**