Skip to content
Open
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.follower;
import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.stopRobot;
import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.telemetryM;
import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.format;

import com.bylazar.configurables.PanelsConfigurables;
import com.bylazar.configurables.annotations.Configurable;
Expand Down Expand Up @@ -127,6 +128,10 @@ public static void stopRobot() {
follower.startTeleopDrive(true);
follower.setTeleOpDrive(0,0,0,true);
}

public static String format(double value, int decimalPlaces) {
return String.format("%." + decimalPlaces + "f", value);
}
}

/**
Expand Down Expand Up @@ -429,7 +434,7 @@ public void loop() {
average += velocity;
}
average /= velocities.size();
telemetryM.debug("Forward Velocity: " + average);
telemetryM.debug("Forward Velocity: " + format(average, 3));
telemetryM.debug("\n");
telemetryM.debug("Press A to set the Forward Velocity temporarily (while robot remains on).");

Expand Down Expand Up @@ -537,7 +542,7 @@ public void loop() {
}
average /= velocities.size();

telemetryM.debug("Strafe Velocity: " + average);
telemetryM.debug("Strafe Velocity: " + format(average, 3));
telemetryM.debug("\n");
telemetryM.debug("Press A to set the Lateral Velocity temporarily (while robot remains on).");
telemetryM.update(telemetry);
Expand Down Expand Up @@ -643,7 +648,7 @@ public void loop() {
}
average /= accelerations.size();

telemetryM.debug("Forward Zero Power Acceleration (Deceleration): " + average);
telemetryM.debug("Forward Zero Power Acceleration (Deceleration): " + format(average, 3));
telemetryM.debug("\n");
telemetryM.debug("Press A to set the Forward Zero Power Acceleration temporarily (while robot remains on).");
telemetryM.update(telemetry);
Expand Down Expand Up @@ -747,7 +752,7 @@ public void loop() {
}
average /= accelerations.size();

telemetryM.debug("Lateral Zero Power Acceleration (Deceleration): " + average);
telemetryM.debug("Lateral Zero Power Acceleration (Deceleration): " + format(average, 3));
telemetryM.debug("\n");
telemetryM.debug("Press A to set the Lateral Zero Power Acceleration temporarily (while robot remains on).");
telemetryM.update(telemetry);
Expand Down Expand Up @@ -919,13 +924,13 @@ public void loop() {

telemetryM.debug("Tuning Complete");
telemetryM.debug("Braking Profile:");
telemetryM.debug("kQuadratic", coefficients[1]);
telemetryM.debug("kLinear", coefficients[0]);
telemetryM.debug("kQuadratic", format(coefficients[1], 8)));
telemetryM.debug("kLinear", format(coefficients[0], 7));
telemetryM.update(telemetry);
telemetryM.debug("Tuning Complete");
telemetryM.debug("Braking Profile:");
telemetryM.debug("kQuadraticFriction", coefficients[1]);
telemetryM.debug("kLinearBraking", coefficients[0]);
telemetryM.debug("kQuadraticFriction", format(coefficients[1], 8));
telemetryM.debug("kLinearBraking", format(coefficients[0], 7));
for (BrakeRecord record : brakeData) {
Pose p = record.pose;
telemetryM.debug(String.format("t=%.0f ms, x=%.2f, y=%.2f, θ=%.2f, v=%.2f",
Expand Down Expand Up @@ -1790,4 +1795,4 @@ public static void drawPoseHistory(PoseHistory poseTracker) {
public static void sendPacket() {
panelsField.update();
}
}
}
Loading