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

Merge pull request #9 from TBHGodPro/master

Browse Source 
Improve OTOSLocalizer and PinpointLocalizer performance, add rotate Pose method
This commit is contained in:
Logan Nash
2024-10-27 16:23:53 -04:00
committed by GitHub
parent 6830a82403 4e18dae336
commit 85b8daed3d
3 changed files with 18 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -111,10 +111,7 @@ public class OTOSLocalizer extends Localizer {
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()));
return MathFunctions.addPoses(startPose, MathFunctions.rotatePose(pose, startPose.getHeading(), false));
}
/**

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

@ -99,10 +99,7 @@ public class PinpointLocalizer extends Localizer {
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()));
return MathFunctions.addPoses(startPose, MathFunctions.rotatePose(pose, startPose.getHeading(), false));
}
/**

16
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java
View File

@ -172,6 +172,22 @@ public class MathFunctions {
return new Pose(one.getX() - two.getX(), one.getY() - two.getY(), one.getHeading() - two.getHeading());
}
/**
* This rotates the given pose by the given theta,
*
* @param pose the Pose to rotate.
* @param theta the angle to rotate by.
* @param rotateHeading whether to adjust the Pose heading too.
* @return the rotated Pose.
*/
public static Pose rotatePose(Pose pose, double theta, boolean rotateHeading) {
double x = pose.getX() * Math.cos(theta) - pose.getY() * Math.sin(theta);
double y = pose.getX() * Math.sin(theta) + pose.getY() * Math.cos(theta);
double heading = rotateHeading ? normalizeAngle(pose.getHeading() + theta) : pose.getHeading();
return new Pose(x, y, heading);
}
/**
* This multiplies a Point by a scalar and returns the result as a Point
*