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

Bump RR

Browse Source
This commit is contained in:
Ryan Brott
2023-07-19 23:13:58 -07:00
parent 1ae2be3dcf
commit 0c14630d40
12 changed files with 105 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
TeamCode/build.gradle
View File

@ -34,8 +34,8 @@ dependencies {
annotationProcessor files('lib/OpModeAnnotationProcessor.jar')
implementation 'com.acmerobotics.dashboard:dashboard:0.4.9'
implementation 'com.acmerobotics.roadrunner:core:1.0.0-beta1'
implementation 'com.acmerobotics.roadrunner:actions:1.0.0-beta1'
implementation 'com.acmerobotics.roadrunner:core:1.0.0-beta2'
implementation 'com.acmerobotics.roadrunner:actions:1.0.0-beta2'
implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7'
}

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

@ -20,9 +20,7 @@ import com.acmerobotics.roadrunner.TimeTrajectory;
import com.acmerobotics.roadrunner.TimeTurn;
import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
import com.acmerobotics.roadrunner.TurnConstraints;
import com.acmerobotics.roadrunner.Twist2d;
import com.acmerobotics.roadrunner.Twist2dDual;
import com.acmerobotics.roadrunner.Twist2dIncrDual;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.VelConstraint;
import com.qualcomm.hardware.lynx.LynxModule;
@ -51,6 +49,7 @@ public final class MecanumDrive {
// drive model parameters
public static double IN_PER_TICK = 0;
public static double LATERAL_IN_PER_TICK = 1;
public static double LATERAL_MULTIPLIER = IN_PER_TICK / LATERAL_IN_PER_TICK;
public static double TRACK_WIDTH_TICKS = 0;
// feedforward parameters in tick units
@ -77,8 +76,7 @@ public final class MecanumDrive {
public static double HEADING_VEL_GAIN = 0.0; // shared with turn
public final MecanumKinematics kinematics = new MecanumKinematics(
IN_PER_TICK * TRACK_WIDTH_TICKS,
IN_PER_TICK / LATERAL_IN_PER_TICK);
IN_PER_TICK * TRACK_WIDTH_TICKS, LATERAL_MULTIPLIER);
public final MotorFeedforward feedforward = new MotorFeedforward(kS, kV / IN_PER_TICK, kA / IN_PER_TICK);
@ -126,7 +124,7 @@ public final class MecanumDrive {
}
@Override
public Twist2dIncrDual<Time> update() {
public Twist2dDual<Time> update() {
Encoder.PositionVelocityPair leftFrontPosVel = leftFront.getPositionAndVelocity();
Encoder.PositionVelocityPair leftRearPosVel = leftRear.getPositionAndVelocity();
Encoder.PositionVelocityPair rightRearPosVel = rightRear.getPositionAndVelocity();
@ -135,7 +133,7 @@ public final class MecanumDrive {
Rotation2d heading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
double headingDelta = heading.minus(lastHeading);
Twist2dIncrDual<Time> twist = kinematics.forward(new MecanumKinematics.WheelIncrements<>(
Twist2dDual<Time> twist = kinematics.forward(new MecanumKinematics.WheelIncrements<>(
new DualNum<Time>(new double[]{
(leftFrontPosVel.position - lastLeftFrontPos) + kinematics.trackWidth * headingDelta,
leftFrontPosVel.velocity,
@ -161,9 +159,9 @@ public final class MecanumDrive {
lastHeading = heading;
return new Twist2dIncrDual<>(
twist.transIncr,
twist.rotIncr.drop(1).addFront(headingDelta)
return new Twist2dDual<>(
twist.line,
DualNum.cons(headingDelta, twist.angle.drop(1))
);
}
}
@ -199,8 +197,9 @@ public final class MecanumDrive {
localizer = new DriveLocalizer();
}
public void setDrivePowers(Twist2d powers) {
MecanumKinematics.WheelVelocities<Time> wheelVels = new MecanumKinematics(1).inverse(Twist2dDual.constant(powers, 1));
public void setDrivePowers(PoseVelocity2d powers) {
MecanumKinematics.WheelVelocities<Time> wheelVels = new MecanumKinematics(1).inverse(
PoseVelocity2dDual.constant(powers, 1));
double maxPowerMag = 1;
for (DualNum<Time> power : wheelVels.all()) {
@ -229,8 +228,8 @@ public final class MecanumDrive {
yPoints = new double[disps.size()];
for (int i = 0; i < disps.size(); i++) {
Pose2d p = t.path.get(disps.get(i), 1).value();
xPoints[i] = p.trans.x;
yPoints[i] = p.trans.y;
xPoints[i] = p.position.x;
yPoints[i] = p.position.y;
}
}
@ -255,9 +254,9 @@ public final class MecanumDrive {
Pose2dDual<Time> txWorldTarget = timeTrajectory.get(t);
Twist2d robotVelRobot = updatePoseEstimate();
PoseVelocity2d robotVelRobot = updatePoseEstimate();
Twist2dDual<Time> command = new HolonomicController(
PoseVelocity2dDual<Time> command = new HolonomicController(
AXIAL_GAIN, LATERAL_GAIN, HEADING_GAIN,
AXIAL_VEL_GAIN, LATERAL_VEL_GAIN, HEADING_VEL_GAIN
)
@ -272,14 +271,14 @@ public final class MecanumDrive {
LogFiles.recordTargetPose(txWorldTarget.value());
p.put("x", pose.trans.x);
p.put("y", pose.trans.y);
p.put("heading (deg)", Math.toDegrees(pose.rot.log()));
p.put("x", pose.position.x);
p.put("y", pose.position.y);
p.put("heading (deg)", Math.toDegrees(pose.heading.log()));
Pose2d error = txWorldTarget.value().minusExp(pose);
p.put("xError", error.trans.x);
p.put("yError", error.trans.y);
p.put("headingError (deg)", Math.toDegrees(error.rot.log()));
p.put("xError", error.position.x);
p.put("yError", error.position.y);
p.put("headingError (deg)", Math.toDegrees(error.heading.log()));
// only draw when active; only one drive action should be active at a time
Canvas c = p.fieldOverlay();
@ -336,9 +335,9 @@ public final class MecanumDrive {
Pose2dDual<Time> txWorldTarget = turn.get(t);
Twist2d robotVelRobot = updatePoseEstimate();
PoseVelocity2d robotVelRobot = updatePoseEstimate();
Twist2dDual<Time> command = new HolonomicController(
PoseVelocity2dDual<Time> command = new HolonomicController(
AXIAL_GAIN, LATERAL_GAIN, HEADING_GAIN,
AXIAL_VEL_GAIN, LATERAL_VEL_GAIN, HEADING_VEL_GAIN
)
@ -363,7 +362,7 @@ public final class MecanumDrive {
drawRobot(c, pose);
c.setStroke("#7C4DFFFF");
c.fillCircle(turn.beginPose.trans.x, turn.beginPose.trans.y, 2);
c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2);
return true;
}
@ -371,13 +370,13 @@ public final class MecanumDrive {
@Override
public void preview(Canvas c) {
c.setStroke("#7C4DFF7A");
c.fillCircle(turn.beginPose.trans.x, turn.beginPose.trans.y, 2);
c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2);
}
}
public Twist2d updatePoseEstimate() {
Twist2dIncrDual<Time> incr = localizer.update();
pose = pose.plus(incr.value());
public PoseVelocity2d updatePoseEstimate() {
Twist2dDual<Time> twist = localizer.update();
pose = pose.plus(twist.value());
poseHistory.add(pose);
while (poseHistory.size() > 100) {
@ -386,7 +385,7 @@ public final class MecanumDrive {
LogFiles.recordPose(pose);
return incr.velocity().value();
return twist.velocity().value();
}
private void drawPoseHistory(Canvas c) {
@ -395,8 +394,8 @@ public final class MecanumDrive {
int i = 0;
for (Pose2d t : poseHistory) {
xPoints[i] = t.trans.x;
yPoints[i] = t.trans.y;
xPoints[i] = t.position.x;
yPoints[i] = t.position.y;
i++;
}
@ -410,10 +409,10 @@ public final class MecanumDrive {
final double ROBOT_RADIUS = 9;
c.setStrokeWidth(1);
c.strokeCircle(t.trans.x, t.trans.y, ROBOT_RADIUS);
c.strokeCircle(t.position.x, t.position.y, ROBOT_RADIUS);
Vector2d halfv = t.rot.vec().times(0.5 * ROBOT_RADIUS);
Vector2d p1 = t.trans.plus(halfv);
Vector2d halfv = t.heading.vec().times(0.5 * ROBOT_RADIUS);
Vector2d p1 = t.position.plus(halfv);
Vector2d p2 = p1.plus(halfv);
c.strokeLine(p1.x, p1.y, p2.x, p2.y);
}
@ -422,10 +421,10 @@ public final class MecanumDrive {
return new TrajectoryActionBuilder(
TurnAction::new,
FollowTrajectoryAction::new,
beginPose, 1e-6,
beginPose, 1e-6, 0.0,
defaultTurnConstraints,
defaultVelConstraint, defaultAccelConstraint,
0.25
0.25, 0.1
);
}
}

68
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
View File

@ -15,6 +15,8 @@ import com.acmerobotics.roadrunner.MinVelConstraint;
import com.acmerobotics.roadrunner.MotorFeedforward;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.Pose2dDual;
import com.acmerobotics.roadrunner.PoseVelocity2d;
import com.acmerobotics.roadrunner.PoseVelocity2dDual;
import com.acmerobotics.roadrunner.ProfileAccelConstraint;
import com.acmerobotics.roadrunner.RamseteController;
import com.acmerobotics.roadrunner.TankKinematics;
@ -25,7 +27,6 @@ import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
import com.acmerobotics.roadrunner.TurnConstraints;
import com.acmerobotics.roadrunner.Twist2d;
import com.acmerobotics.roadrunner.Twist2dDual;
import com.acmerobotics.roadrunner.Twist2dIncrDual;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.Vector2dDual;
import com.acmerobotics.roadrunner.VelConstraint;
@ -135,7 +136,7 @@ public final class TankDrive {
}
@Override
public Twist2dIncrDual<Time> update() {
public Twist2dDual<Time> update() {
double meanLeftPos = 0.0, meanLeftVel = 0.0;
for (Encoder e : leftEncs) {
Encoder.PositionVelocityPair p = e.getPositionAndVelocity();
@ -154,7 +155,7 @@ public final class TankDrive {
meanRightPos /= rightEncs.size();
meanRightVel /= rightEncs.size();
TankKinematics.WheelIncrements<Time> incrs = new TankKinematics.WheelIncrements<>(
TankKinematics.WheelIncrements<Time> twist = new TankKinematics.WheelIncrements<>(
new DualNum<Time>(new double[] {
meanLeftPos - lastLeftPos,
meanLeftVel
@ -168,7 +169,7 @@ public final class TankDrive {
lastLeftPos = meanLeftPos;
lastRightPos = meanRightPos;
return kinematics.forward(incrs);
return kinematics.forward(twist);
}
}
@ -203,8 +204,9 @@ public final class TankDrive {
localizer = new TankDrive.DriveLocalizer();
}
public void setDrivePowers(Twist2d powers) {
TankKinematics.WheelVelocities<Time> wheelVels = new TankKinematics(2).inverse(Twist2dDual.constant(powers, 1));
public void setDrivePowers(PoseVelocity2d powers) {
TankKinematics.WheelVelocities<Time> wheelVels = new TankKinematics(2).inverse(
PoseVelocity2dDual.constant(powers, 1));
double maxPowerMag = 1;
for (DualNum<Time> power : wheelVels.all()) {
@ -235,8 +237,8 @@ public final class TankDrive {
yPoints = new double[disps.size()];
for (int i = 0; i < disps.size(); i++) {
Pose2d p = t.path.get(disps.get(i), 1).value();
xPoints[i] = p.trans.x;
yPoints[i] = p.trans.y;
xPoints[i] = p.position.x;
yPoints[i] = p.position.y;
}
}
@ -267,7 +269,7 @@ public final class TankDrive {
updatePoseEstimate();
Twist2dDual<Time> command = new RamseteController(kinematics.trackWidth, RAMSETE_ZETA, RAMSETE_BBAR)
PoseVelocity2dDual<Time> command = new RamseteController(kinematics.trackWidth, RAMSETE_ZETA, RAMSETE_BBAR)
.compute(x, txWorldTarget, pose);
TankKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
@ -279,14 +281,14 @@ public final class TankDrive {
m.setPower(feedforward.compute(wheelVels.right) / voltage);
}
p.put("x", pose.trans.x);
p.put("y", pose.trans.y);
p.put("heading (deg)", Math.toDegrees(pose.rot.log()));
p.put("x", pose.position.x);
p.put("y", pose.position.y);
p.put("heading (deg)", Math.toDegrees(pose.heading.log()));
Pose2d error = txWorldTarget.value().minusExp(pose);
p.put("xError", error.trans.x);
p.put("yError", error.trans.y);
p.put("headingError (deg)", Math.toDegrees(error.rot.log()));
p.put("xError", error.position.x);
p.put("yError", error.position.y);
p.put("headingError (deg)", Math.toDegrees(error.heading.log()));
LogFiles.recordTargetPose(txWorldTarget.value());
@ -347,13 +349,13 @@ public final class TankDrive {
Pose2dDual<Time> txWorldTarget = turn.get(t);
Twist2d robotVelRobot = updatePoseEstimate();
PoseVelocity2d robotVelRobot = updatePoseEstimate();
Twist2dDual<Time> command = new Twist2dDual<>(
PoseVelocity2dDual<Time> command = new PoseVelocity2dDual<>(
Vector2dDual.constant(new Vector2d(0, 0), 3),
txWorldTarget.rot.velocity().plus(
TURN_GAIN * pose.rot.minus(txWorldTarget.rot.value()) +
TURN_VEL_GAIN * (robotVelRobot.rotVel - txWorldTarget.rot.velocity().value())
txWorldTarget.heading.velocity().plus(
TURN_GAIN * pose.heading.minus(txWorldTarget.heading.value()) +
TURN_VEL_GAIN * (robotVelRobot.angVel - txWorldTarget.heading.velocity().value())
)
);
@ -378,7 +380,7 @@ public final class TankDrive {
drawRobot(c, pose);
c.setStroke("#7C4DFFFF");
c.fillCircle(turn.beginPose.trans.x, turn.beginPose.trans.y, 2);
c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2);
return true;
}
@ -386,13 +388,13 @@ public final class TankDrive {
@Override
public void preview(Canvas c) {
c.setStroke("#7C4DFF7A");
c.fillCircle(turn.beginPose.trans.x, turn.beginPose.trans.y, 2);
c.fillCircle(turn.beginPose.position.x, turn.beginPose.position.y, 2);
}
}
public Twist2d updatePoseEstimate() {
Twist2dIncrDual<Time> incr = localizer.update();
pose = pose.plus(incr.value());
public PoseVelocity2d updatePoseEstimate() {
Twist2dDual<Time> twist = localizer.update();
pose = pose.plus(twist.value());
poseHistory.add(pose);
while (poseHistory.size() > 100) {
@ -401,7 +403,7 @@ public final class TankDrive {
LogFiles.recordPose(pose);
return incr.velocity().value();
return twist.velocity().value();
}
private void drawPoseHistory(Canvas c) {
@ -410,8 +412,8 @@ public final class TankDrive {
int i = 0;
for (Pose2d t : poseHistory) {
xPoints[i] = t.trans.x;
yPoints[i] = t.trans.y;
xPoints[i] = t.position.x;
yPoints[i] = t.position.y;
i++;
}
@ -425,10 +427,10 @@ public final class TankDrive {
final double ROBOT_RADIUS = 9;
c.setStrokeWidth(1);
c.strokeCircle(t.trans.x, t.trans.y, ROBOT_RADIUS);
c.strokeCircle(t.position.x, t.position.y, ROBOT_RADIUS);
Vector2d halfv = t.rot.vec().times(0.5 * ROBOT_RADIUS);
Vector2d p1 = t.trans.plus(halfv);
Vector2d halfv = t.heading.vec().times(0.5 * ROBOT_RADIUS);
Vector2d p1 = t.position.plus(halfv);
Vector2d p2 = p1.plus(halfv);
c.strokeLine(p1.x, p1.y, p2.x, p2.y);
}
@ -437,10 +439,10 @@ public final class TankDrive {
return new TrajectoryActionBuilder(
TurnAction::new,
FollowTrajectoryAction::new,
beginPose, 1e-6,
beginPose, 1e-6, 0.0,
defaultTurnConstraints,
defaultVelConstraint, defaultAccelConstraint,
0.25
0.25, 0.1
);
}
}

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java
View File

@ -3,7 +3,7 @@ package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.DualNum;
import com.acmerobotics.roadrunner.Time;
import com.acmerobotics.roadrunner.Twist2dIncrDual;
import com.acmerobotics.roadrunner.Twist2dDual;
import com.acmerobotics.roadrunner.Vector2dDual;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
@ -36,7 +36,7 @@ public final class ThreeDeadWheelLocalizer implements Localizer {
this.inPerTick = inPerTick;
}
public Twist2dIncrDual<Time> update() {
public Twist2dDual<Time> update() {
Encoder.PositionVelocityPair par0PosVel = par0.getPositionAndVelocity();
Encoder.PositionVelocityPair par1PosVel = par1.getPositionAndVelocity();
Encoder.PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();
@ -45,7 +45,7 @@ public final class ThreeDeadWheelLocalizer implements Localizer {
int par1PosDelta = par1PosVel.position - lastPar1Pos;
int perpPosDelta = perpPosVel.position - lastPerpPos;
Twist2dIncrDual<Time> twistIncr = new Twist2dIncrDual<>(
Twist2dDual<Time> twist = new Twist2dDual<>(
new Vector2dDual<>(
new DualNum<Time>(new double[] {
(PAR0_Y_TICKS * par1PosDelta - PAR1_Y_TICKS * par0PosDelta) / (PAR0_Y_TICKS - PAR1_Y_TICKS),
@ -66,6 +66,6 @@ public final class ThreeDeadWheelLocalizer implements Localizer {
lastPar1Pos = par1PosVel.position;
lastPerpPos = perpPosVel.position;
return twistIncr;
return twist;
}
}

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

@ -4,7 +4,7 @@ import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.DualNum;
import com.acmerobotics.roadrunner.Rotation2d;
import com.acmerobotics.roadrunner.Time;
import com.acmerobotics.roadrunner.Twist2dIncrDual;
import com.acmerobotics.roadrunner.Twist2dDual;
import com.acmerobotics.roadrunner.Vector2dDual;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
@ -40,7 +40,7 @@ public final class TwoDeadWheelLocalizer implements Localizer {
this.inPerTick = inPerTick;
}
public Twist2dIncrDual<Time> update() {
public Twist2dDual<Time> update() {
Encoder.PositionVelocityPair parPosVel = par.getPositionAndVelocity();
Encoder.PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();
Rotation2d heading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
@ -51,7 +51,7 @@ public final class TwoDeadWheelLocalizer implements Localizer {
double headingVel = imu.getRobotAngularVelocity(AngleUnit.RADIANS).zRotationRate;
Twist2dIncrDual<Time> twistIncr = new Twist2dIncrDual<>(
Twist2dDual<Time> twist = new Twist2dDual<>(
new Vector2dDual<>(
new DualNum<Time>(new double[] {
parPosDelta - PAR_Y_TICKS * headingDelta,
@ -72,6 +72,6 @@ public final class TwoDeadWheelLocalizer implements Localizer {
lastPerpPos = perpPosVel.position;
lastHeading = heading;
return twistIncr;
return twist;
}
}

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/DriveView.java
View File

@ -2,7 +2,7 @@ package org.firstinspires.ftc.teamcode.tuning;
import com.acmerobotics.roadrunner.MotorFeedforward;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.Twist2d;
import com.acmerobotics.roadrunner.PoseVelocity2d;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
@ -176,7 +176,7 @@ final class DriveView {
throw new AssertionError();
}
public void setDrivePowers(Twist2d powers) {
public void setDrivePowers(PoseVelocity2d powers) {
if (md != null) {
md.setDrivePowers(powers);
return;

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ForwardRampLogger.java
View File

@ -13,7 +13,7 @@ import java.util.ArrayList;
import java.util.List;
import java.util.Map;
public final class ForwardRampLogger extends LinearOpMode {
public final class ForwardRampLogger extends LinearOpMode {
private static double power(double seconds) {
return Math.min(0.1 * seconds, 0.9);
}

17
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java
View File

@ -1,6 +1,7 @@
package org.firstinspires.ftc.teamcode.tuning;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.PoseVelocity2d;
import com.acmerobotics.roadrunner.Twist2d;
import com.acmerobotics.roadrunner.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
@ -17,7 +18,7 @@ public class LocalizationTest extends LinearOpMode {
waitForStart();
while (opModeIsActive()) {
drive.setDrivePowers(new Twist2d(
drive.setDrivePowers(new PoseVelocity2d(
new Vector2d(
-gamepad1.left_stick_y,
-gamepad1.left_stick_x
@ -27,9 +28,9 @@ public class LocalizationTest extends LinearOpMode {
drive.updatePoseEstimate();
telemetry.addData("x", drive.pose.trans.x);
telemetry.addData("y", drive.pose.trans.y);
telemetry.addData("heading", drive.pose.rot);
telemetry.addData("x", drive.pose.position.x);
telemetry.addData("y", drive.pose.position.y);
telemetry.addData("heading", drive.pose.heading);
telemetry.update();
}
} else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) {
@ -38,7 +39,7 @@ public class LocalizationTest extends LinearOpMode {
waitForStart();
while (opModeIsActive()) {
drive.setDrivePowers(new Twist2d(
drive.setDrivePowers(new PoseVelocity2d(
new Vector2d(
-gamepad1.left_stick_y,
0.0
@ -48,9 +49,9 @@ public class LocalizationTest extends LinearOpMode {
drive.updatePoseEstimate();
telemetry.addData("x", drive.pose.trans.x);
telemetry.addData("y", drive.pose.trans.y);
telemetry.addData("heading", drive.pose.rot);
telemetry.addData("x", drive.pose.position.x);
telemetry.addData("y", drive.pose.position.y);
telemetry.addData("heading", drive.pose.heading);
telemetry.update();
}
} else {

5
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedforwardTuner.java
View File

@ -4,6 +4,7 @@ import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.DualNum;
import com.acmerobotics.roadrunner.PoseVelocity2d;
import com.acmerobotics.roadrunner.Profiles;
import com.acmerobotics.roadrunner.Time;
import com.acmerobotics.roadrunner.TimeProfile;
@ -72,7 +73,7 @@ public class ManualFeedforwardTuner extends LinearOpMode {
telemetry.addData("vref", v.get(0));
double power = view.feedforward().compute(v) / view.voltageSensor.getVoltage();
view.setDrivePowers(new Twist2d(new Vector2d(power, 0), 0));
view.setDrivePowers(new PoseVelocity2d(new Vector2d(power, 0), 0));
break;
}
@ -84,7 +85,7 @@ public class ManualFeedforwardTuner extends LinearOpMode {
}
view.setDrivePowers(
new Twist2d(
new PoseVelocity2d(
new Vector2d(
-gamepad1.left_stick_y,
-gamepad1.left_stick_x

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Localizer.java
View File

@ -1,8 +1,8 @@
package org.firstinspires.ftc.teamcode.util;
import com.acmerobotics.roadrunner.Time;
import com.acmerobotics.roadrunner.Twist2dIncrDual;
import com.acmerobotics.roadrunner.Twist2dDual;
public interface Localizer {
Twist2dIncrDual<Time> update();
Twist2dDual<Time> update();
}

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java
View File

@ -106,18 +106,18 @@ public final class LogFiles {
}
public static void recordTargetPose(Pose2d targetPose) {
log.targetXs.add(targetPose.trans.x);
log.targetYs.add(targetPose.trans.y);
log.targetHeadings.add(targetPose.rot.log());
log.targetXs.add(targetPose.position.x);
log.targetYs.add(targetPose.position.y);
log.targetHeadings.add(targetPose.heading.log());
}
public static void recordPose(Pose2d pose) {
// arbitrarily add time here
log.nsTimes.add(System.nanoTime());
log.xs.add(pose.trans.x);
log.ys.add(pose.trans.y);
log.headings.add(pose.rot.log());
log.xs.add(pose.position.x);
log.ys.add(pose.position.y);
log.headings.add(pose.heading.log());
}
private static final OpModeManagerNotifier.Notifications notifHandler = new OpModeManagerNotifier.Notifications() {

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Paths.java
View File

@ -16,11 +16,11 @@ public final class Paths {
List<Double> xs = com.acmerobotics.roadrunner.Math.range(
0.0, path.length(), (int) Math.ceil(path.length() / 6.0));
double champDisp = Curves.project(path, query, xs.get(0));
final Vector2d initVec = path.get(champDisp, 1).trans.value();
final Vector2d initVec = path.get(champDisp, 1).position.value();
double champSqrNorm = initVec.minus(query).sqrNorm();
for (int i = 1; i < xs.size(); i++) {
double disp = Curves.project(path, query, xs.get(i));
Vector2d vec = path.get(disp, 1).trans.value();
Vector2d vec = path.get(disp, 1).position.value();
double sqrNorm = vec.minus(query).sqrNorm();
if (sqrNorm < champSqrNorm) {