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

hopefully fixed the PinpointLocalizer

Browse Source
This commit is contained in:
Anyi Lin
2024-11-21 17:09:44 -05:00
parent 437635838b
commit 9711a268e5
1 changed files with 11 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -44,7 +44,6 @@ import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Vector;
*/ */
public class PinpointLocalizer extends Localizer { public class PinpointLocalizer extends Localizer {
private HardwareMap hardwareMap; private HardwareMap hardwareMap;
private Pose startPose;
private GoBildaPinpointDriver odo; private GoBildaPinpointDriver odo;
private double previousHeading; private double previousHeading;
private double totalHeading; private double totalHeading;
@ -84,7 +83,7 @@ public class PinpointLocalizer extends Localizer {
setStartPose(setStartPose); setStartPose(setStartPose);
totalHeading = 0; totalHeading = 0;
previousHeading = startPose.getHeading(); previousHeading = setStartPose.getHeading();
resetPinpoint(); resetPinpoint();
} }
@ -97,9 +96,7 @@ public class PinpointLocalizer extends Localizer {
@Override @Override
public Pose getPose() { public Pose getPose() {
Pose2D rawPose = odo.getPosition(); Pose2D rawPose = odo.getPosition();
Pose pose = new Pose(rawPose.getX(DistanceUnit.INCH), rawPose.getY(DistanceUnit.INCH), rawPose.getHeading(AngleUnit.RADIANS)); return new Pose(rawPose.getX(DistanceUnit.INCH), rawPose.getY(DistanceUnit.INCH), rawPose.getHeading(AngleUnit.RADIANS));
return MathFunctions.addPoses(startPose, MathFunctions.rotatePose(pose, startPose.getHeading(), false));
} }
/** /**
@ -127,13 +124,15 @@ public class PinpointLocalizer extends Localizer {
} }
/** /**
* This sets the start pose. Changing the start pose should move the robot as if all its * This sets the start pose. Since nobody should be using this after the robot has begun moving,
* previous movements were displacing it from its new start pose. * and due to issues with the PinpointLocalizer, this is functionally the same as setPose(Pose).
* *
* @param setStart the new start pose * @param setStart the new start pose
*/ */
@Override @Override
public void setStartPose(Pose setStart) {startPose = setStart;} public void setStartPose(Pose setStart) {
odo.setPosition(new Pose2D(DistanceUnit.INCH, setStart.getX(), setStart.getY(), AngleUnit.RADIANS, setStart.getHeading()));
}
/** /**
* This sets the current pose estimate. Changing this should just change the robot's current * This sets the current pose estimate. Changing this should just change the robot's current
@ -143,9 +142,8 @@ public class PinpointLocalizer extends Localizer {
*/ */
@Override @Override
public void setPose(Pose setPose) { public void setPose(Pose setPose) {
resetPinpoint(); resetPinpoint();
Pose setPinpointPose = MathFunctions.subtractPoses(setPose, startPose); odo.setPosition(new Pose2D(DistanceUnit.INCH, setPose.getX(), setPose.getY(), AngleUnit.RADIANS, setPose.getHeading()));
odo.setPosition(new Pose2D(DistanceUnit.INCH, setPinpointPose.getX(), setPinpointPose.getY(), AngleUnit.RADIANS, setPinpointPose.getHeading()));
} }
/** /**
@ -154,8 +152,8 @@ public class PinpointLocalizer extends Localizer {
@Override @Override
public void update() { public void update() {
odo.update(); odo.update();
totalHeading += MathFunctions.getSmallestAngleDifference(odo.getHeading(),previousHeading); totalHeading += MathFunctions.getSmallestAngleDifference(odo.getHeading(), previousHeading);
previousHeading = odo.getHeading(); previousHeading = odo.getHeading();
} }
/** /**