Skip to content

Commit

Permalink
Log power commands
Browse files Browse the repository at this point in the history
  • Loading branch information
@rbrott
rbrott committed Nov 26, 2023
1 parent 7ad781b commit 416cab6
Show file tree
Hide file tree
Showing 9 changed files with 106 additions and 91 deletions.
20 changes: 0 additions & 20 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DownsampledWriter.java
View file

This file was deleted.

37 changes: 28 additions & 9 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 @@ -23,6 +23,7 @@
import com.acmerobotics.roadrunner.Twist2dDual;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.VelConstraint;
import com.acmerobotics.roadrunner.ftc.DownsampledWriter;
import com.acmerobotics.roadrunner.ftc.Encoder;
import com.acmerobotics.roadrunner.ftc.FlightRecorder;
import com.acmerobotics.roadrunner.ftc.LynxFirmware;
Expand All @@ -39,6 +40,7 @@

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.teamcode.messages.DriveCommandMessage;
import org.firstinspires.ftc.teamcode.messages.MecanumCommandMessage;
import org.firstinspires.ftc.teamcode.messages.MecanumEncodersMessage;
import org.firstinspires.ftc.teamcode.messages.PoseMessage;

Expand Down Expand Up @@ -114,6 +116,7 @@ kinematics.new WheelVelConstraint(PARAMS.maxWheelVel),
private final DownsampledWriter estimatedPoseWriter = new DownsampledWriter("ESTIMATED_POSE", 50_000_000);
private final DownsampledWriter targetPoseWriter = new DownsampledWriter("TARGET_POSE", 50_000_000);
private final DownsampledWriter driveCommandWriter = new DownsampledWriter("DRIVE_COMMAND", 50_000_000);
private final DownsampledWriter mecanumCommandWriter = new DownsampledWriter("MECANUM_COMMAND", 50_000_000);

public class DriveLocalizer implements Localizer {
public final Encoder leftFront, leftBack, rightBack, rightFront;
Expand Down Expand Up @@ -268,6 +271,7 @@ public boolean run(@NonNull TelemetryPacket p) {
}

Pose2dDual<Time> txWorldTarget = timeTrajectory.get(t);
targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));

PoseVelocity2d robotVelRobot = updatePoseEstimate();

Expand All @@ -281,13 +285,20 @@ public boolean run(@NonNull TelemetryPacket p) {
MecanumKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
double voltage = voltageSensor.getVoltage();

final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS, PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
leftFront.setPower(feedforward.compute(wheelVels.leftFront) / voltage);
leftBack.setPower(feedforward.compute(wheelVels.leftBack) / voltage);
rightBack.setPower(feedforward.compute(wheelVels.rightBack) / voltage);
rightFront.setPower(feedforward.compute(wheelVels.rightFront) / voltage);
final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS,
PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
double leftFrontPower = feedforward.compute(wheelVels.leftFront) / voltage;
double leftBackPower = feedforward.compute(wheelVels.leftBack) / voltage;
double rightBackPower = feedforward.compute(wheelVels.rightBack) / voltage;
double rightFrontPower = feedforward.compute(wheelVels.rightFront) / voltage;
mecanumCommandWriter.write(new MecanumCommandMessage(
voltage, leftFrontPower, leftBackPower, rightBackPower, rightFrontPower
));

targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
leftFront.setPower(leftFrontPower);
leftBack.setPower(leftBackPower);
rightBack.setPower(rightBackPower);
rightFront.setPower(rightFrontPower);

p.put("x", pose.position.x);
p.put("y", pose.position.y);
Expand Down Expand Up @@ -352,6 +363,7 @@ public boolean run(@NonNull TelemetryPacket p) {
}

Pose2dDual<Time> txWorldTarget = turn.get(t);
targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));

PoseVelocity2d robotVelRobot = updatePoseEstimate();

Expand All @@ -364,14 +376,21 @@ public boolean run(@NonNull TelemetryPacket p) {

MecanumKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
double voltage = voltageSensor.getVoltage();
final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS, PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS,
PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
double leftFrontPower = feedforward.compute(wheelVels.leftFront) / voltage;
double leftBackPower = feedforward.compute(wheelVels.leftBack) / voltage;
double rightBackPower = feedforward.compute(wheelVels.rightBack) / voltage;
double rightFrontPower = feedforward.compute(wheelVels.rightFront) / voltage;
mecanumCommandWriter.write(new MecanumCommandMessage(
voltage, leftFrontPower, leftBackPower, rightBackPower, rightFrontPower
));

leftFront.setPower(feedforward.compute(wheelVels.leftFront) / voltage);
leftBack.setPower(feedforward.compute(wheelVels.leftBack) / voltage);
rightBack.setPower(feedforward.compute(wheelVels.rightBack) / voltage);
rightFront.setPower(feedforward.compute(wheelVels.rightFront) / voltage);

targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));

Canvas c = p.fieldOverlay();
drawPoseHistory(c);

Expand Down
32 changes: 22 additions & 10 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.Vector2dDual;
import com.acmerobotics.roadrunner.VelConstraint;
import com.acmerobotics.roadrunner.ftc.DownsampledWriter;
import com.acmerobotics.roadrunner.ftc.Encoder;
import com.acmerobotics.roadrunner.ftc.FlightRecorder;
import com.acmerobotics.roadrunner.ftc.LynxFirmware;
Expand All @@ -46,6 +47,7 @@
import org.firstinspires.ftc.teamcode.messages.DriveCommandMessage;
import org.firstinspires.ftc.teamcode.messages.MecanumEncodersMessage;
import org.firstinspires.ftc.teamcode.messages.PoseMessage;
import org.firstinspires.ftc.teamcode.messages.TankCommandMessage;
import org.firstinspires.ftc.teamcode.messages.TankEncodersMessage;

import java.util.ArrayList;
Expand Down Expand Up @@ -119,6 +121,8 @@ kinematics.new WheelVelConstraint(PARAMS.maxWheelVel),
private final DownsampledWriter targetPoseWriter = new DownsampledWriter("TARGET_POSE", 50_000_000);
private final DownsampledWriter driveCommandWriter = new DownsampledWriter("DRIVE_COMMAND", 50_000_000);

private final DownsampledWriter tankCommandWriter = new DownsampledWriter("TANK_COMMAND", 50_000_000);

public class DriveLocalizer implements Localizer {
public final List<Encoder> leftEncs, rightEncs;

Expand Down Expand Up @@ -285,6 +289,7 @@ public boolean run(@NonNull TelemetryPacket p) {
DualNum<Time> x = timeTrajectory.profile.get(t);

Pose2dDual<Arclength> txWorldTarget = timeTrajectory.path.get(x.value(), 3);
targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));

updatePoseEstimate();

Expand All @@ -294,12 +299,17 @@ public boolean run(@NonNull TelemetryPacket p) {

TankKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
double voltage = voltageSensor.getVoltage();
final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS, PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS,
PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
double leftPower = feedforward.compute(wheelVels.left) / voltage;
double rightPower = feedforward.compute(wheelVels.right) / voltage;
tankCommandWriter.write(new TankCommandMessage(voltage, leftPower, rightPower));

for (DcMotorEx m : leftMotors) {
m.setPower(feedforward.compute(wheelVels.left) / voltage);
m.setPower(leftPower);
}
for (DcMotorEx m : rightMotors) {
m.setPower(feedforward.compute(wheelVels.right) / voltage);
m.setPower(rightPower);
}

p.put("x", pose.position.x);
Expand All @@ -311,8 +321,6 @@ public boolean run(@NonNull TelemetryPacket p) {
p.put("yError", error.position.y);
p.put("headingError (deg)", Math.toDegrees(error.heading.toDouble()));

targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));

// only draw when active; only one drive action should be active at a time
Canvas c = p.fieldOverlay();
drawPoseHistory(c);
Expand Down Expand Up @@ -369,6 +377,7 @@ public boolean run(@NonNull TelemetryPacket p) {
}

Pose2dDual<Time> txWorldTarget = turn.get(t);
targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));

PoseVelocity2d robotVelRobot = updatePoseEstimate();

Expand All @@ -383,16 +392,19 @@ public boolean run(@NonNull TelemetryPacket p) {

TankKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
double voltage = voltageSensor.getVoltage();
final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS, PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
final MotorFeedforward feedforward = new MotorFeedforward(PARAMS.kS,
PARAMS.kV / PARAMS.inPerTick, PARAMS.kA / PARAMS.inPerTick);
double leftPower = feedforward.compute(wheelVels.left) / voltage;
double rightPower = feedforward.compute(wheelVels.right) / voltage;
tankCommandWriter.write(new TankCommandMessage(voltage, leftPower, rightPower));

for (DcMotorEx m : leftMotors) {
m.setPower(feedforward.compute(wheelVels.left) / voltage);
m.setPower(leftPower);
}
for (DcMotorEx m : rightMotors) {
m.setPower(feedforward.compute(wheelVels.right) / voltage);
m.setPower(rightPower);
}

targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));

Canvas c = p.fieldOverlay();
drawPoseHistory(c);

Expand Down
19 changes: 19 additions & 0 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/MecanumCommandMessage.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,19 @@
package org.firstinspires.ftc.teamcode.messages;

public final class MecanumCommandMessage {
public long timestamp;
public double voltage;
public double leftFrontPower;
public double leftBackPower;
public double rightBackPower;
public double rightFrontPower;

public MecanumCommandMessage(double voltage, double leftFrontPower, double leftBackPower, double rightBackPower, double rightFrontPower) {
this.timestamp = System.nanoTime();
this.voltage = voltage;
this.leftFrontPower = leftFrontPower;
this.leftBackPower = leftBackPower;
this.rightBackPower = rightBackPower;
this.rightFrontPower = rightFrontPower;
}
}
24 changes: 8 additions & 16 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/MecanumEncodersMessage.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,24 +4,16 @@

public final class MecanumEncodersMessage {
public long timestamp;
public int rawLeftFrontPosition;
public int rawLeftFrontVelocity;
public int rawLeftBackPosition;
public int rawLeftBackVelocity;
public int rawRightBackPosition;
public int rawRightBackVelocity;
public int rawRightFrontPosition;
public int rawRightFrontVelocity;
public PositionVelocityPair leftFront;
public PositionVelocityPair leftBack;
public PositionVelocityPair rightBack;
public PositionVelocityPair rightFront;

public MecanumEncodersMessage(PositionVelocityPair leftFront, PositionVelocityPair leftBack, PositionVelocityPair rightBack, PositionVelocityPair rightFront) {
this.timestamp = System.nanoTime();
this.rawLeftFrontPosition = leftFront.rawPosition;
this.rawLeftFrontVelocity = leftFront.rawVelocity;
this.rawLeftBackPosition = leftBack.rawPosition;
this.rawLeftBackVelocity = leftBack.rawVelocity;
this.rawRightBackPosition = rightBack.rawPosition;
this.rawRightBackVelocity = rightBack.rawVelocity;
this.rawRightFrontPosition = rightFront.rawPosition;
this.rawRightFrontVelocity = rightFront.rawVelocity;
this.leftFront = leftFront;
this.leftBack = leftBack;
this.rightBack = rightBack;
this.rightFront = rightFront;
}
}
15 changes: 15 additions & 0 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TankCommandMessage.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,15 @@
package org.firstinspires.ftc.teamcode.messages;

public final class TankCommandMessage {
public long timestamp;
public double voltage;
public double leftPower;
public double rightPower;

public TankCommandMessage(double voltage, double leftPower, double rightPower) {
this.timestamp = System.nanoTime();
this.voltage = voltage;
this.leftPower = leftPower;
this.rightPower = rightPower;
}
}
20 changes: 4 additions & 16 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TankEncodersMessage.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,24 +6,12 @@

public final class TankEncodersMessage {
public long timestamp;
public int[] rawLeftPositions;
public int[] rawLeftVelocities;
public int[] rawRightPositions;
public int[] rawRightVelocities;
public PositionVelocityPair[] left;
public PositionVelocityPair[] right;

public TankEncodersMessage(List<PositionVelocityPair> left, List<PositionVelocityPair> right) {
this.timestamp = System.nanoTime();
this.rawLeftPositions = new int[left.size()];
this.rawLeftVelocities = new int[left.size()];
this.rawRightPositions = new int[right.size()];
this.rawRightVelocities = new int[right.size()];
for (int i = 0; i < left.size(); i++) {
this.rawLeftPositions[i] = left.get(i).rawPosition;
this.rawLeftVelocities[i] = left.get(i).rawVelocity;
}
for (int i = 0; i < right.size(); i++) {
this.rawRightPositions[i] = right.get(i).rawPosition;
this.rawRightVelocities[i] = right.get(i).rawVelocity;
}
this.left = left.toArray(new PositionVelocityPair[0]);
this.right = right.toArray(new PositionVelocityPair[0]);
}
}
18 changes: 6 additions & 12 deletions .../src/main/java/org/firstinspires/ftc/teamcode/messages/ThreeDeadWheelEncodersMessage.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,20 +4,14 @@

public final class ThreeDeadWheelEncodersMessage {
public long timestamp;
public int rawPar0Position;
public int rawPar0Velocity;
public int rawPar1Position;
public int rawPar1Velocity;
public int rawPerpPosition;
public int rawPerpVelocity;
public PositionVelocityPair par0;
public PositionVelocityPair par1;
public PositionVelocityPair perp;

public ThreeDeadWheelEncodersMessage(PositionVelocityPair par0, PositionVelocityPair par1, PositionVelocityPair perp) {
this.timestamp = System.nanoTime();
this.rawPar0Position = par0.rawPosition;
this.rawPar0Velocity = par0.rawVelocity;
this.rawPar1Position = par1.rawPosition;
this.rawPar1Velocity = par1.rawVelocity;
this.rawPerpPosition = perp.rawPosition;
this.rawPerpVelocity = perp.rawVelocity;
this.par0 = par0;
this.par1 = par1;
this.perp = perp;
}
}
12 changes: 4 additions & 8 deletions ...de/src/main/java/org/firstinspires/ftc/teamcode/messages/TwoDeadWheelEncodersMessage.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,16 +4,12 @@

public final class TwoDeadWheelEncodersMessage {
public long timestamp;
public int rawParPosition;
public int rawParVelocity;
public int rawPerpPosition;
public int rawPerpVelocity;
public PositionVelocityPair par;
public PositionVelocityPair perp;

public TwoDeadWheelEncodersMessage(PositionVelocityPair par, PositionVelocityPair perp) {
this.timestamp = System.nanoTime();
this.rawParPosition = par.rawPosition;
this.rawParVelocity = par.rawVelocity;
this.rawPerpPosition = perp.rawPosition;
this.rawPerpVelocity = perp.rawVelocity;
this.par = par;
this.perp = perp;
}
}

0 comments on commit 416cab6

Please sign in to comment.