From b617f87bac9b86d00cb368181015f6170b0f34df Mon Sep 17 00:00:00 2001 From: Ryan Brott Date: Thu, 2 Feb 2023 01:57:04 -0800 Subject: [PATCH] Add logging behind the scenes --- .../ftc/teamcode/MecanumDrive.java | 7 + .../firstinspires/ftc/teamcode/TankDrive.java | 7 + .../ftc/teamcode/tuning/LogFiles.java | 224 ++++++++++++++++++ 3 files changed, 238 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LogFiles.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index f3073f6d0ff7..25792169ebe9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -35,6 +35,7 @@ import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.VoltageSensor; +import org.firstinspires.ftc.teamcode.tuning.LogFiles; import org.firstinspires.ftc.teamcode.util.Encoder; import org.firstinspires.ftc.teamcode.util.Localizer; import org.firstinspires.ftc.teamcode.util.LynxFirmwareVersion; @@ -252,6 +253,8 @@ public boolean run(@NonNull TelemetryPacket p) { rightBack.setPower(feedforward.compute(wheelVels.rightBack) / voltage); rightFront.setPower(feedforward.compute(wheelVels.rightFront) / voltage); + 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())); @@ -331,6 +334,8 @@ public boolean run(@NonNull TelemetryPacket p) { rightBack.setPower(feedforward.compute(wheelVels.rightBack) / voltage); rightFront.setPower(feedforward.compute(wheelVels.rightFront) / voltage); + LogFiles.recordTargetPose(txWorldTarget.value()); + Canvas c = p.fieldOverlay(); drawPoseHistory(c); @@ -362,6 +367,8 @@ private Twist2d updatePoseEstimateAndGetActualVel() { poseHistory.removeFirst(); } + LogFiles.recordPose(pose); + return incr.velocity().value(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java index b5f00d15f90e..4c3eb536c744 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java @@ -37,6 +37,7 @@ import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.VoltageSensor; +import org.firstinspires.ftc.teamcode.tuning.LogFiles; import org.firstinspires.ftc.teamcode.util.Encoder; import org.firstinspires.ftc.teamcode.util.Localizer; import org.firstinspires.ftc.teamcode.util.LynxFirmwareVersion; @@ -281,6 +282,8 @@ public boolean run(@NonNull TelemetryPacket p) { p.put("yError", error.trans.y); p.put("headingError (deg)", Math.toDegrees(error.rot.log())); + LogFiles.recordTargetPose(txWorldTarget.value()); + // only draw when active; only one drive action should be active at a time Canvas c = p.fieldOverlay(); drawPoseHistory(c); @@ -357,6 +360,8 @@ public boolean run(@NonNull TelemetryPacket p) { m.setPower(feedforward.compute(wheelVels.right) / voltage); } + LogFiles.recordTargetPose(txWorldTarget.value()); + Canvas c = p.fieldOverlay(); drawPoseHistory(c); @@ -388,6 +393,8 @@ public Twist2d updatePoseEstimateAndGetActualVel() { poseHistory.removeFirst(); } + LogFiles.recordPose(pose); + return incr.velocity().value(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LogFiles.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LogFiles.java new file mode 100644 index 000000000000..f9bff435dbc9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LogFiles.java @@ -0,0 +1,224 @@ +package org.firstinspires.ftc.teamcode.tuning; + +import android.annotation.SuppressLint; +import android.content.Context; + +import com.acmerobotics.roadrunner.Pose2d; +import com.fasterxml.jackson.core.JsonFactory; +import com.fasterxml.jackson.databind.ObjectMapper; +import com.fasterxml.jackson.databind.ObjectWriter; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerImpl; +import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerNotifier; +import com.qualcomm.robotcore.util.RobotLog; +import com.qualcomm.robotcore.util.WebHandlerManager; + +import org.firstinspires.ftc.ftccommon.external.WebHandlerRegistrar; +import org.firstinspires.ftc.robotcore.internal.system.AppUtil; +import org.firstinspires.ftc.teamcode.MecanumDrive; +import org.firstinspires.ftc.teamcode.TankDrive; +import org.firstinspires.ftc.teamcode.ThreeDeadWheelLocalizer; +import org.firstinspires.ftc.teamcode.TwoDeadWheelLocalizer; + +import java.io.File; +import java.io.FileInputStream; +import java.io.IOException; +import java.text.DateFormat; +import java.text.SimpleDateFormat; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.Date; +import java.util.List; +import java.util.Objects; + +import fi.iki.elonen.NanoHTTPD; + +public final class LogFiles { + private static final File ROOT = + new File(AppUtil.ROOT_FOLDER + "/RoadRunner/logs/"); + + public static LogFile log = new LogFile("uninitialized"); + + public static class LogFile { + public int version = 0; + + public String opModeName; + public long msInit = System.currentTimeMillis(); + public long nsInit = System.nanoTime(); + public long nsStart, nsStop; + + public String driveClassName = TuningOpModes.DRIVE_CLASS.getCanonicalName(); + + public double mecInPerTick = MecanumDrive.IN_PER_TICK; + public double mecLateralInPerTick = MecanumDrive.LATERAL_IN_PER_TICK; + public double mecTrackWidthTicks = MecanumDrive.TRACK_WIDTH_TICKS; + public double mecKS = MecanumDrive.kS; + public double mecKV = MecanumDrive.kV; + public double mecKA = MecanumDrive.kA; + public double mecMaxWheelVel = MecanumDrive.MAX_WHEEL_VEL; + public double mecMinProfileAccel = MecanumDrive.MIN_PROFILE_ACCEL; + public double mecMaxProfileAccel = MecanumDrive.MAX_PROFILE_ACCEL; + public double mecMaxAngVel = MecanumDrive.MAX_ANG_VEL; + public double mecMaxAngAccel = MecanumDrive.MAX_ANG_ACCEL; + public double mecAxialGain = MecanumDrive.AXIAL_GAIN; + public double mecLateralGain = MecanumDrive.LATERAL_GAIN; + public double mecHeadingGain = MecanumDrive.HEADING_GAIN; + public double mecAxialVelGain = MecanumDrive.AXIAL_VEL_GAIN; + public double mecLateralVelGain = MecanumDrive.LATERAL_VEL_GAIN; + public double mecHeadingVelGain = MecanumDrive.HEADING_VEL_GAIN; + + public double tankInPerTick = TankDrive.IN_PER_TICK; + public double tankTrackWidthTicks = TankDrive.TRACK_WIDTH_TICKS; + public double tankKS = TankDrive.kS; + public double tankKV = TankDrive.kV; + public double tankKA = TankDrive.kA; + public double tankMaxWheelVel = TankDrive.MAX_WHEEL_VEL; + public double tankMinProfileAccel = TankDrive.MIN_PROFILE_ACCEL; + public double tankMaxProfileAccel = TankDrive.MAX_PROFILE_ACCEL; + public double tankMaxAngVel = TankDrive.MAX_ANG_VEL; + public double tankMaxAngAccel = TankDrive.MAX_ANG_ACCEL; + public double tankRamseteZeta = TankDrive.RAMSETE_ZETA; + public double tankRamseteBbar = TankDrive.RAMSETE_BBAR; + public double tankTurnGain = TankDrive.TURN_GAIN; + public double tankTurnVelGain = TankDrive.TURN_VEL_GAIN; + + public double threePar0YTicks = ThreeDeadWheelLocalizer.PAR0_Y_TICKS; + public double threePar1YTicks = ThreeDeadWheelLocalizer.PAR1_Y_TICKS; + public double threePerpXTicks = ThreeDeadWheelLocalizer.PERP_X_TICKS; + + public double twoParYTicks = TwoDeadWheelLocalizer.PAR_Y_TICKS; + public double twoPerpXTicks = TwoDeadWheelLocalizer.PERP_X_TICKS; + + public List nsTimes = new ArrayList<>(); + + public List targetXs = new ArrayList<>(); + public List targetYs = new ArrayList<>(); + public List targetHeadings = new ArrayList<>(); + + public List xs = new ArrayList<>(); + public List ys = new ArrayList<>(); + public List headings = new ArrayList<>(); + + public LogFile(String opModeName) { + this.opModeName = opModeName; + } + } + + public static void recordTargetPose(Pose2d targetPose) { + log.targetXs.add(targetPose.trans.x); + log.targetYs.add(targetPose.trans.y); + log.targetHeadings.add(targetPose.rot.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()); + } + + private static final OpModeManagerNotifier.Notifications notifHandler = new OpModeManagerNotifier.Notifications() { + @SuppressLint("SimpleDateFormat") + final DateFormat dateFormat = new SimpleDateFormat("yyyy_MM_dd__HH_mm_ss_SSS"); + + final ObjectWriter jsonWriter = new ObjectMapper(new JsonFactory()) + .writerWithDefaultPrettyPrinter(); + + @Override + public void onOpModePreInit(OpMode opMode) { + log = new LogFile(opMode.getClass().getCanonicalName()); + + // clean up old files + File[] fs = Objects.requireNonNull(ROOT.listFiles()); + Arrays.sort(fs, (a, b) -> Long.compare(a.lastModified(), b.lastModified())); + long totalSizeBytes = 0; + for (File f : fs) { + totalSizeBytes += f.length(); + } + + int i = 0; + while (i < fs.length && totalSizeBytes >= 8 * 1000 * 1000) { + totalSizeBytes -= fs[i].length(); + if (!fs[i].delete()) { + RobotLog.setGlobalErrorMsg("Unable to delete file " + fs[i].getAbsolutePath()); + } + ++i; + } + } + + @Override + public void onOpModePreStart(OpMode opMode) { + log.nsStart = System.nanoTime(); + } + + @Override + public void onOpModePostStop(OpMode opMode) { + log.nsStop = System.nanoTime(); + + if (!(opMode instanceof OpModeManagerImpl.DefaultOpMode)) { + String filename = dateFormat.format(new Date(log.msInit)) + "__" + opMode.getClass().getSimpleName() + ".json"; + File file = new File(ROOT, filename); + try { + jsonWriter.writeValue(file, log); + } catch (IOException e) { + RobotLog.setGlobalErrorMsg(new RuntimeException(e), + "Unable to write data to " + file.getAbsolutePath()); + } + } + } + }; + + @WebHandlerRegistrar + public static void registerRoutes(Context context, WebHandlerManager manager) { + //noinspection ResultOfMethodCallIgnored + ROOT.mkdirs(); + + // op mode manager only stores a weak reference, so we need to keep notifHandler alive ourselves + // don't use @OnCreateEventLoop because it's unreliable + OpModeManagerImpl.getOpModeManagerOfActivity( + AppUtil.getInstance().getActivity() + ).registerListener(notifHandler); + + manager.register("/logs", session -> { + final StringBuilder sb = new StringBuilder(); + sb.append("Logs
    "); + File[] fs = Objects.requireNonNull(ROOT.listFiles()); + Arrays.sort(fs, (a, b) -> Long.compare(b.lastModified(), a.lastModified())); + for (File f : fs) { + sb.append("
  • "); + sb.append(f.getName()); + sb.append("
    • "); + } + sb.append("
"); + return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.OK, + NanoHTTPD.MIME_HTML, sb.toString()); + }); + + manager.register("/logs/download", session -> { + final String[] pairs = session.getQueryParameterString().split("&"); + if (pairs.length != 1) { + return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.BAD_REQUEST, + NanoHTTPD.MIME_PLAINTEXT, "expected one query parameter, got " + pairs.length); + } + + final String[] parts = pairs[0].split("="); + if (!parts[0].equals("file")) { + return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.BAD_REQUEST, + NanoHTTPD.MIME_PLAINTEXT, "expected file query parameter, got " + parts[0]); + } + + File f = new File(ROOT, parts[1]); + if (!f.exists()) { + return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.NOT_FOUND, + NanoHTTPD.MIME_PLAINTEXT, "file " + f + " doesn't exist"); + } + + return NanoHTTPD.newChunkedResponse(NanoHTTPD.Response.Status.OK, + "application/json", new FileInputStream(f)); + }); + } +}