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

Log power commands

Browse Source
This commit is contained in:
Ryan Brott
2023-11-26 13:54:10 -08:00
parent 7ad781b8ac
commit 416cab6f51
9 changed files with 106 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DownsampledWriter.java
View File

@ -1,20 +0,0 @@
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.roadrunner.ftc.FlightRecorder;
public class DownsampledWriter {
public final String channel;
public final long maxPeriod;
private long nextWriteTimestamp = System.nanoTime();
public DownsampledWriter(String channel, long maxPeriod) {
this.channel = channel;
this.maxPeriod = maxPeriod;
}
public void write(Object msg) {
long now = System.nanoTime();
if (now >= nextWriteTimestamp) {
nextWriteTimestamp = now + maxPeriod;
FlightRecorder.write(channel, msg);
}
}
}

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

@ -23,6 +23,7 @@ import com.acmerobotics.roadrunner.TurnConstraints;
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;
@ -39,6 +40,7 @@ import com.qualcomm.robotcore.hardware.VoltageSensor;
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;
@ -114,6 +116,7 @@ public final class MecanumDrive {
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;
@ -268,6 +271,7 @@ public final class MecanumDrive {
}
Pose2dDual<Time> txWorldTarget = timeTrajectory.get(t);
targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
PoseVelocity2d robotVelRobot = updatePoseEstimate();
@ -281,13 +285,20 @@ public final class MecanumDrive {
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);
@ -352,6 +363,7 @@ public final class MecanumDrive {
}
Pose2dDual<Time> txWorldTarget = turn.get(t);
targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
PoseVelocity2d robotVelRobot = updatePoseEstimate();
@ -364,14 +376,21 @@ public final class MecanumDrive {
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);

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

@ -29,6 +29,7 @@ import com.acmerobotics.roadrunner.Twist2dDual;
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;
@ -46,6 +47,7 @@ import com.qualcomm.robotcore.hardware.VoltageSensor;
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;
@ -119,6 +121,8 @@ public final class TankDrive {
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;
@ -285,6 +289,7 @@ public final class TankDrive {
DualNum<Time> x = timeTrajectory.profile.get(t);
Pose2dDual<Arclength> txWorldTarget = timeTrajectory.path.get(x.value(), 3);
targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
updatePoseEstimate();
@ -294,12 +299,17 @@ public final class TankDrive {
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);
@ -311,8 +321,6 @@ public final class TankDrive {
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);
@ -369,6 +377,7 @@ public final class TankDrive {
}
Pose2dDual<Time> txWorldTarget = turn.get(t);
targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
PoseVelocity2d robotVelRobot = updatePoseEstimate();
@ -383,16 +392,19 @@ public final class TankDrive {
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);

19
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/MecanumCommandMessage.java Normal file
View File

@ -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
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/MecanumEncodersMessage.java
View File

@ -4,24 +4,16 @@ import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
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
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TankCommandMessage.java Normal file
View File

@ -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
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TankEncodersMessage.java
View File

@ -6,24 +6,12 @@ import java.util.List;
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
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/ThreeDeadWheelEncodersMessage.java
View File

@ -4,20 +4,14 @@ import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
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
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TwoDeadWheelEncodersMessage.java
View File

@ -4,16 +4,12 @@ import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
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;
}
}