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

Add more debug messages

Browse Source
This commit is contained in:
Ryan Brott
2023-11-20 22:59:36 -08:00
parent 0e66ff3715
commit 79d6297a46
9 changed files with 192 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -0,0 +1,20 @@
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);
}
}
}

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

@ -38,6 +38,9 @@ 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.messages.DriveCommandMessage;
import org.firstinspires.ftc.teamcode.messages.MecanumEncodersMessage;
import org.firstinspires.ftc.teamcode.messages.PoseMessage;
import java.lang.Math;
import java.util.Arrays;
@ -47,6 +50,12 @@ import java.util.List;
@Config
public final class MecanumDrive {
public static class Params {
// IMU orientation
public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection =
RevHubOrientationOnRobot.LogoFacingDirection.UP;
public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection =
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
// drive model parameters
public double inPerTick = 0;
public double lateralInPerTick = 1;
@ -102,6 +111,10 @@ public final class MecanumDrive {
private final LinkedList<Pose2d> poseHistory = new LinkedList<>();
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);
public class DriveLocalizer implements Localizer {
public final Encoder leftFront, leftBack, rightBack, rightFront;
@ -129,6 +142,9 @@ public final class MecanumDrive {
PositionVelocityPair rightBackPosVel = rightBack.getPositionAndVelocity();
PositionVelocityPair rightFrontPosVel = rightFront.getPositionAndVelocity();
FlightRecorder.write("MECANUM_ENCODERS", new MecanumEncodersMessage(
leftFrontPosVel, leftBackPosVel, rightBackPosVel, rightFrontPosVel));
Rotation2d heading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
double headingDelta = heading.minus(lastHeading);
@ -186,8 +202,7 @@ public final class MecanumDrive {
imu = hardwareMap.get(IMU.class, "imu");
IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.UP,
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD));
PARAMS.logoFacingDirection, PARAMS.usbFacingDirection));
imu.initialize(parameters);
voltageSensor = hardwareMap.voltageSensor.iterator().next();
@ -261,6 +276,7 @@ public final class MecanumDrive {
PARAMS.axialVelGain, PARAMS.lateralVelGain, PARAMS.headingVelGain
)
.compute(txWorldTarget, pose, robotVelRobot);
driveCommandWriter.write(new DriveCommandMessage(command));
MecanumKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
double voltage = voltageSensor.getVoltage();
@ -271,7 +287,7 @@ public final class MecanumDrive {
rightBack.setPower(feedforward.compute(wheelVels.rightBack) / voltage);
rightFront.setPower(feedforward.compute(wheelVels.rightFront) / voltage);
FlightRecorder.write("TARGET_POSE", new PoseMessage(txWorldTarget.value()));
targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
p.put("x", pose.position.x);
p.put("y", pose.position.y);
@ -344,6 +360,7 @@ public final class MecanumDrive {
PARAMS.axialVelGain, PARAMS.lateralVelGain, PARAMS.headingVelGain
)
.compute(txWorldTarget, pose, robotVelRobot);
driveCommandWriter.write(new DriveCommandMessage(command));
MecanumKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
double voltage = voltageSensor.getVoltage();
@ -353,7 +370,7 @@ public final class MecanumDrive {
rightBack.setPower(feedforward.compute(wheelVels.rightBack) / voltage);
rightFront.setPower(feedforward.compute(wheelVels.rightFront) / voltage);
FlightRecorder.write("TARGET_POSE", new PoseMessage(txWorldTarget.value()));
targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
Canvas c = p.fieldOverlay();
drawPoseHistory(c);
@ -386,7 +403,7 @@ public final class MecanumDrive {
poseHistory.removeFirst();
}
FlightRecorder.write("ESTIMATED_POSE", new PoseMessage(pose));
estimatedPoseWriter.write(new PoseMessage(pose));
return twist.velocity().value();
}

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

@ -43,6 +43,11 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU;
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.TankEncodersMessage;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collections;
@ -52,6 +57,12 @@ import java.util.List;
@Config
public final class TankDrive {
public static class Params {
// IMU orientation
public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection =
RevHubOrientationOnRobot.LogoFacingDirection.UP;
public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection =
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
// drive model parameters
public double inPerTick = 0;
public double trackWidthTicks = 0;
@ -104,6 +115,10 @@ public final class TankDrive {
private final LinkedList<Pose2d> poseHistory = new LinkedList<>();
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);
public class DriveLocalizer implements Localizer {
public final List<Encoder> leftEncs, rightEncs;
@ -135,11 +150,13 @@ public final class TankDrive {
@Override
public Twist2dDual<Time> update() {
List<PositionVelocityPair> leftReadings = new ArrayList<>(), rightReadings = new ArrayList<>();
double meanLeftPos = 0.0, meanLeftVel = 0.0;
for (Encoder e : leftEncs) {
PositionVelocityPair p = e.getPositionAndVelocity();
meanLeftPos += p.position;
meanLeftVel += p.velocity;
leftReadings.add(p);
}
meanLeftPos /= leftEncs.size();
meanLeftVel /= leftEncs.size();
@ -149,10 +166,14 @@ public final class TankDrive {
PositionVelocityPair p = e.getPositionAndVelocity();
meanRightPos += p.position;
meanRightVel += p.velocity;
rightReadings.add(p);
}
meanRightPos /= rightEncs.size();
meanRightVel /= rightEncs.size();
FlightRecorder.write("TANK_ENCODERS",
new TankEncodersMessage(leftReadings, rightReadings));
TankKinematics.WheelIncrements<Time> twist = new TankKinematics.WheelIncrements<>(
new DualNum<Time>(new double[] {
meanLeftPos - lastLeftPos,
@ -192,8 +213,7 @@ public final class TankDrive {
imu = hardwareMap.get(IMU.class, "imu");
IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.UP,
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD));
PARAMS.logoFacingDirection, PARAMS.usbFacingDirection));
imu.initialize(parameters);
voltageSensor = hardwareMap.voltageSensor.iterator().next();
@ -270,6 +290,7 @@ public final class TankDrive {
PoseVelocity2dDual<Time> command = new RamseteController(kinematics.trackWidth, PARAMS.ramseteZeta, PARAMS.ramseteBBar)
.compute(x, txWorldTarget, pose);
driveCommandWriter.write(new DriveCommandMessage(command));
TankKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
double voltage = voltageSensor.getVoltage();
@ -290,7 +311,7 @@ public final class TankDrive {
p.put("yError", error.position.y);
p.put("headingError (deg)", Math.toDegrees(error.heading.log()));
FlightRecorder.write("TARGET_POSE", new PoseMessage(txWorldTarget.value()));
targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
// only draw when active; only one drive action should be active at a time
Canvas c = p.fieldOverlay();
@ -358,6 +379,7 @@ public final class TankDrive {
PARAMS.turnVelGain * (robotVelRobot.angVel - txWorldTarget.heading.velocity().value())
)
);
driveCommandWriter.write(new DriveCommandMessage(command));
TankKinematics.WheelVelocities<Time> wheelVels = kinematics.inverse(command);
double voltage = voltageSensor.getVoltage();
@ -369,7 +391,7 @@ public final class TankDrive {
m.setPower(feedforward.compute(wheelVels.right) / voltage);
}
FlightRecorder.write("TARGET_POSE", new PoseMessage(txWorldTarget.value()));
targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
Canvas c = p.fieldOverlay();
drawPoseHistory(c);
@ -402,7 +424,7 @@ public final class TankDrive {
poseHistory.removeFirst();
}
FlightRecorder.write("ESTIMATED_POSE", new PoseMessage(pose));
estimatedPoseWriter.write(new PoseMessage(pose));
return twist.velocity().value();
}

24
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/DriveCommandMessage.java Normal file
View File

@ -0,0 +1,24 @@
package org.firstinspires.ftc.teamcode.messages;
import com.acmerobotics.roadrunner.PoseVelocity2dDual;
import com.acmerobotics.roadrunner.Time;
public final class DriveCommandMessage {
public long timestamp;
public double forwardVelocity;
public double forwardAcceleration;
public double lateralVelocity;
public double lateralAcceleration;
public double angularVelocity;
public double angularAcceleration;
public DriveCommandMessage(PoseVelocity2dDual<Time> poseVelocity) {
this.timestamp = System.nanoTime();
this.forwardVelocity = poseVelocity.linearVel.x.get(0);
this.forwardAcceleration = poseVelocity.linearVel.x.get(1);
this.lateralVelocity = poseVelocity.linearVel.y.get(0);
this.lateralAcceleration = poseVelocity.linearVel.y.get(1);
this.angularVelocity = poseVelocity.angVel.get(0);
this.angularAcceleration = poseVelocity.angVel.get(1);
}
}

27
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/MecanumEncodersMessage.java Normal file
View File

@ -0,0 +1,27 @@
package org.firstinspires.ftc.teamcode.messages;
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 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;
}
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/PoseMessage.java
View File

@ -1,4 +1,4 @@
package org.firstinspires.ftc.teamcode;
package org.firstinspires.ftc.teamcode.messages;
import com.acmerobotics.roadrunner.Pose2d;

29
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/TankEncodersMessage.java Normal file
View File

@ -0,0 +1,29 @@
package org.firstinspires.ftc.teamcode.messages;
import com.acmerobotics.roadrunner.ftc.PositionVelocityPair;
import java.util.List;
public final class TankEncodersMessage {
public long timestamp;
public int[] rawLeftPositions;
public int[] rawLeftVelocities;
public int[] rawRightPositions;
public int[] rawRightVelocities;
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;
}
}
}

23
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/messages/ThreeDeadWheelEncodersMessage.java Normal file
View File

@ -0,0 +1,23 @@
package org.firstinspires.ftc.teamcode.messages;
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 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;
}
}

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

@ -0,0 +1,19 @@
package org.firstinspires.ftc.teamcode.messages;
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 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;
}
}