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

Apply heading velocity correction in localizer

Browse Source
This commit is contained in:
Ryan Brott
2023-09-24 15:29:43 -07:00
parent 629c103d77
commit 7cce1c43a9
1 changed files with 13 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java
View File

@ -34,6 +34,8 @@ public final class TwoDeadWheelLocalizer implements Localizer {
private final double inPerTick; private final double inPerTick;
private double lastRawHeadingVel, headingVelOffset;
public TwoDeadWheelLocalizer(HardwareMap hardwareMap, IMU imu, double inPerTick) { public TwoDeadWheelLocalizer(HardwareMap hardwareMap, IMU imu, double inPerTick) {
par = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "par"))); par = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "par")));
perp = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "perp"))); perp = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "perp")));
@ -48,6 +50,16 @@ public final class TwoDeadWheelLocalizer implements Localizer {
FlightRecorder.write("TWO_DEAD_WHEEL_PARAMS", PARAMS); FlightRecorder.write("TWO_DEAD_WHEEL_PARAMS", PARAMS);
} }
// see https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/617
private double getHeadingVelocity() {
double rawHeadingVel = imu.getRobotAngularVelocity(AngleUnit.RADIANS).zRotationRate;
if (Math.abs(rawHeadingVel - lastRawHeadingVel) > Math.PI) {
headingVelOffset -= Math.signum(rawHeadingVel) * 2 * Math.PI;
}
lastRawHeadingVel = rawHeadingVel;
return headingVelOffset + rawHeadingVel;
}
public Twist2dDual<Time> update() { public Twist2dDual<Time> update() {
PositionVelocityPair parPosVel = par.getPositionAndVelocity(); PositionVelocityPair parPosVel = par.getPositionAndVelocity();
PositionVelocityPair perpPosVel = perp.getPositionAndVelocity(); PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();
@ -57,7 +69,7 @@ public final class TwoDeadWheelLocalizer implements Localizer {
int perpPosDelta = perpPosVel.position - lastPerpPos; int perpPosDelta = perpPosVel.position - lastPerpPos;
double headingDelta = heading.minus(lastHeading); double headingDelta = heading.minus(lastHeading);
double headingVel = imu.getRobotAngularVelocity(AngleUnit.RADIANS).zRotationRate; double headingVel = getHeadingVelocity();
Twist2dDual<Time> twist = new Twist2dDual<>( Twist2dDual<Time> twist = new Twist2dDual<>(
new Vector2dDual<>( new Vector2dDual<>(