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