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

Use IMU heading in mecanum drive localizer (fixes #215)

Browse Source
This commit is contained in:
Ryan Brott
2023-03-01 21:46:32 -08:00
parent 77dbd7f5e2
commit b4af81739a
1 changed files with 20 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
View File

@ -5,9 +5,7 @@ import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.canvas.Canvas; import com.acmerobotics.dashboard.canvas.Canvas;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.AccelConstraint; import com.acmerobotics.roadrunner.*;
import com.acmerobotics.roadrunner.Action;
import com.acmerobotics.roadrunner.Actions;
import com.acmerobotics.roadrunner.AngularVelConstraint; import com.acmerobotics.roadrunner.AngularVelConstraint;
import com.acmerobotics.roadrunner.DualNum; import com.acmerobotics.roadrunner.DualNum;
import com.acmerobotics.roadrunner.HolonomicController; import com.acmerobotics.roadrunner.HolonomicController;
@ -35,6 +33,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.VoltageSensor; import com.qualcomm.robotcore.hardware.VoltageSensor;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.teamcode.util.LogFiles; import org.firstinspires.ftc.teamcode.util.LogFiles;
import org.firstinspires.ftc.teamcode.util.Encoder; import org.firstinspires.ftc.teamcode.util.Encoder;
import org.firstinspires.ftc.teamcode.util.Localizer; import org.firstinspires.ftc.teamcode.util.Localizer;
@ -109,6 +108,7 @@ public final class MecanumDrive {
public final Encoder leftFront, leftRear, rightRear, rightFront; public final Encoder leftFront, leftRear, rightRear, rightFront;
private int lastLeftFrontPos, lastLeftRearPos, lastRightRearPos, lastRightFrontPos; private int lastLeftFrontPos, lastLeftRearPos, lastRightRearPos, lastRightFrontPos;
private Rotation2d lastHeading;
public DriveLocalizer() { public DriveLocalizer() {
leftFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftFront)); leftFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftFront));
@ -120,6 +120,8 @@ public final class MecanumDrive {
lastLeftRearPos = leftRear.getPositionAndVelocity().position; lastLeftRearPos = leftRear.getPositionAndVelocity().position;
lastRightRearPos = rightRear.getPositionAndVelocity().position; lastRightRearPos = rightRear.getPositionAndVelocity().position;
lastRightFrontPos = rightFront.getPositionAndVelocity().position; lastRightFrontPos = rightFront.getPositionAndVelocity().position;
lastHeading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
} }
@Override @Override
@ -129,31 +131,39 @@ public final class MecanumDrive {
Encoder.PositionVelocityPair rightRearPosVel = rightRear.getPositionAndVelocity(); Encoder.PositionVelocityPair rightRearPosVel = rightRear.getPositionAndVelocity();
Encoder.PositionVelocityPair rightFrontPosVel = rightFront.getPositionAndVelocity(); Encoder.PositionVelocityPair rightFrontPosVel = rightFront.getPositionAndVelocity();
MecanumKinematics.WheelIncrements<Time> incrs = new MecanumKinematics.WheelIncrements<>( Rotation2d heading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
double headingDelta = heading.minus(lastHeading);
Twist2dIncrDual<Time> twist = kinematics.forward(new MecanumKinematics.WheelIncrements<>(
new DualNum<Time>(new double[]{ new DualNum<Time>(new double[]{
leftFrontPosVel.position - lastLeftFrontPos, (leftFrontPosVel.position - lastLeftFrontPos) + kinematics.trackWidth * headingDelta,
leftFrontPosVel.velocity, leftFrontPosVel.velocity,
}).times(inPerTick), }).times(inPerTick),
new DualNum<Time>(new double[]{ new DualNum<Time>(new double[]{
leftRearPosVel.position - lastLeftRearPos, (leftRearPosVel.position - lastLeftRearPos) + kinematics.trackWidth * headingDelta,
leftRearPosVel.velocity, leftRearPosVel.velocity,
}).times(inPerTick), }).times(inPerTick),
new DualNum<Time>(new double[]{ new DualNum<Time>(new double[]{
rightRearPosVel.position - lastRightRearPos, (rightRearPosVel.position - lastRightRearPos) - kinematics.trackWidth * headingDelta,
rightRearPosVel.velocity, rightRearPosVel.velocity,
}).times(inPerTick), }).times(inPerTick),
new DualNum<Time>(new double[]{ new DualNum<Time>(new double[]{
rightFrontPosVel.position - lastRightFrontPos, (rightFrontPosVel.position - lastRightFrontPos) - kinematics.trackWidth * headingDelta,
rightFrontPosVel.velocity, rightFrontPosVel.velocity,
}).times(inPerTick) }).times(inPerTick)
); ));
lastLeftFrontPos = leftFrontPosVel.position; lastLeftFrontPos = leftFrontPosVel.position;
lastLeftRearPos = leftRearPosVel.position; lastLeftRearPos = leftRearPosVel.position;
lastRightRearPos = rightRearPosVel.position; lastRightRearPos = rightRearPosVel.position;
lastRightFrontPos = rightFrontPosVel.position; lastRightFrontPos = rightFrontPosVel.position;
return kinematics.forward(incrs); lastHeading = heading;
return new Twist2dIncrDual<>(
twist.transIncr,
twist.rotIncr.drop(1).addFront(headingDelta)
);
} }
} }