Skip to content

Commit

Permalink
Bump RR
Browse files Browse the repository at this point in the history
  • Loading branch information
@rbrott
rbrott committed Jul 20, 2023
1 parent 1ae2be3 commit 0c14630
Show file tree
Hide file tree
Showing 12 changed files with 105 additions and 102 deletions.
4 changes: 2 additions & 2 deletions TeamCode/build.gradle
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 changes: 35 additions & 36 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,9 +20,7 @@
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;
Expand Down Expand Up @@ -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
Expand All @@ -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);

Expand Down Expand Up @@ -126,7 +124,7 @@ public DriveLocalizer() {
}

@Override
public Twist2dIncrDual<Time> update() {
public Twist2dDual<Time> update() {
Encoder.PositionVelocityPair leftFrontPosVel = leftFront.getPositionAndVelocity();
Encoder.PositionVelocityPair leftRearPosVel = leftRear.getPositionAndVelocity();
Encoder.PositionVelocityPair rightRearPosVel = rightRear.getPositionAndVelocity();
Expand All @@ -135,7 +133,7 @@ public Twist2dIncrDual<Time> update() {
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,
Expand All @@ -161,9 +159,9 @@ public Twist2dIncrDual<Time> update() {

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))
);
}
}
Expand Down Expand Up @@ -199,8 +197,9 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) {
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()) {
Expand Down Expand Up @@ -229,8 +228,8 @@ public FollowTrajectoryAction(TimeTrajectory t) {
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;
}
}

Expand All @@ -255,9 +254,9 @@ public boolean run(@NonNull TelemetryPacket p) {

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
)
Expand All @@ -272,14 +271,14 @@ public boolean run(@NonNull TelemetryPacket p) {

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();
Expand Down Expand Up @@ -336,9 +335,9 @@ public boolean run(@NonNull TelemetryPacket p) {

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
)
Expand All @@ -363,21 +362,21 @@ public boolean run(@NonNull TelemetryPacket p) {
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;
}

@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) {
Expand All @@ -386,7 +385,7 @@ public Twist2d updatePoseEstimate() {

LogFiles.recordPose(pose);

return incr.velocity().value();
return twist.velocity().value();
}

private void drawPoseHistory(Canvas c) {
Expand All @@ -395,8 +394,8 @@ private void drawPoseHistory(Canvas c) {

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++;
}
Expand All @@ -410,10 +409,10 @@ private static void drawRobot(Canvas c, Pose2d t) {
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);
}
Expand All @@ -422,10 +421,10 @@ public TrajectoryActionBuilder actionBuilder(Pose2d beginPose) {
return new TrajectoryActionBuilder(
TurnAction::new,
FollowTrajectoryAction::new,
beginPose, 1e-6,
beginPose, 1e-6, 0.0,
defaultTurnConstraints,
defaultVelConstraint, defaultAccelConstraint,
0.25
0.25, 0.1
);
}
}
Loading

0 comments on commit 0c14630

Please sign in to comment.