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 {
private HardwareMap hardwareMap;
private Pose startPose;
private GoBildaPinpointDriver odo;
private double previousHeading;
private double totalHeading;
@ -84,7 +83,7 @@ public class PinpointLocalizer extends Localizer {
setStartPose(setStartPose);
totalHeading = 0;
previousHeading = startPose.getHeading();
previousHeading = setStartPose.getHeading();
resetPinpoint();
}
@ -97,9 +96,7 @@ public class PinpointLocalizer extends Localizer {
@Override
public Pose getPose() {
Pose2D rawPose = odo.getPosition();
Pose pose = new Pose(rawPose.getX(DistanceUnit.INCH), rawPose.getY(DistanceUnit.INCH), rawPose.getHeading(AngleUnit.RADIANS));
return MathFunctions.addPoses(startPose, MathFunctions.rotatePose(pose, startPose.getHeading(), false));
return new Pose(rawPose.getX(DistanceUnit.INCH), rawPose.getY(DistanceUnit.INCH), rawPose.getHeading(AngleUnit.RADIANS));
}
/**
@ -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
* previous movements were displacing it from its new start pose.
* This sets the start pose. Since nobody should be using this after the robot has begun moving,
* and due to issues with the PinpointLocalizer, this is functionally the same as setPose(Pose).
*
* @param setStart the new start pose
*/
@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
@ -143,9 +142,8 @@ public class PinpointLocalizer extends Localizer {
*/
@Override
public void setPose(Pose setPose) {
resetPinpoint();
Pose setPinpointPose = MathFunctions.subtractPoses(setPose, startPose);
odo.setPosition(new Pose2D(DistanceUnit.INCH, setPinpointPose.getX(), setPinpointPose.getY(), AngleUnit.RADIANS, setPinpointPose.getHeading()));
resetPinpoint();
odo.setPosition(new Pose2D(DistanceUnit.INCH, setPose.getX(), setPose.getY(), AngleUnit.RADIANS, setPose.getHeading()));
}
/**
@ -154,8 +152,8 @@ public class PinpointLocalizer extends Localizer {
@Override
public void update() {
odo.update();
totalHeading += MathFunctions.getSmallestAngleDifference(odo.getHeading(),previousHeading);
previousHeading = odo.getHeading();
totalHeading += MathFunctions.getSmallestAngleDifference(odo.getHeading(), previousHeading);
previousHeading = odo.getHeading();
}
/**