diff --git a/FtcRobotController/src/main/AndroidManifest.xml b/FtcRobotController/src/main/AndroidManifest.xml index c873221..143c1f1 100644 --- a/FtcRobotController/src/main/AndroidManifest.xml +++ b/FtcRobotController/src/main/AndroidManifest.xml @@ -1,8 +1,8 @@ + android:versionCode="62" + android:versionName="11.2"> diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java index f2bb14f..b4e313d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/AutonomousConstants.java @@ -65,8 +65,24 @@ public Coordinates(double x, double y, double heading) { public static Coordinates IntakeZoneBack11Blue = new Coordinates(5, -20, 1.5); public static Coordinates ParkBackBlue = new Coordinates(30, -50, 0); + + /// Replay Correction + public static double kP_translation = 0.022; + public static double kD_translation = 0.013; + public static double kP_rotation = 0.30; + public static double kD_rotation = 0.08; + + // Constraints + public static double BLEND_K = 0.40; + public static double MIN_CORRECTION_CAP = 0.20; + public static double MAX_CORRECTION_CAP = 0.60; + public static double CORRECTION_ERROR_SCALE = 7.0; + public static double D_FILTER_ALPHA = 0.25; + + + public static Pose coordinates(Coordinates coord) { return new Pose(coord.x, coord.y, coord.heading); } -} \ No newline at end of file +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/FinalReplayOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/FinalReplayOp.java new file mode 100644 index 0000000..55d0dc3 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/FinalReplayOp.java @@ -0,0 +1,501 @@ +package org.firstinspires.ftc.teamcode.kronbot.autonomous; + +import static org.firstinspires.ftc.teamcode.kronbot.autonomous.AutonomousConstants.BLEND_K; +import static org.firstinspires.ftc.teamcode.kronbot.autonomous.AutonomousConstants.MAX_CORRECTION_CAP; +import static org.firstinspires.ftc.teamcode.kronbot.autonomous.AutonomousConstants.MIN_CORRECTION_CAP; +import static org.firstinspires.ftc.teamcode.kronbot.autonomous.AutonomousConstants.CORRECTION_ERROR_SCALE; +import static org.firstinspires.ftc.teamcode.kronbot.autonomous.AutonomousConstants.D_FILTER_ALPHA; +import static org.firstinspires.ftc.teamcode.kronbot.autonomous.AutonomousConstants.kD_rotation; +import static org.firstinspires.ftc.teamcode.kronbot.autonomous.AutonomousConstants.kP_rotation; +import static org.firstinspires.ftc.teamcode.kronbot.autonomous.AutonomousConstants.kP_translation; +import static org.firstinspires.ftc.teamcode.kronbot.autonomous.AutonomousConstants.kD_translation; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.INTAKE_REVERSE; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; +import com.pedropathing.geometry.Pose; + +import org.firstinspires.ftc.teamcode.kronbot.Robot; + +import java.io.BufferedReader; +import java.io.File; +import java.io.FileReader; +import java.io.IOException; +import java.util.ArrayList; +import java.util.List; + +/** + * Replay Autonomous — V3.4 (Feedforward + Odometry Correction) + * + * Reads the V3.4 CSV format produced by FinalRecorderOp. Older V3.3 CSVs + * still load; drive feedforward is approximated from recorded wheel powers. + * + * This version uses the recorded servo positions (turretPos, anglePos) + * directly during replay instead of re-calculating them with live auto-aim. + * Drive playback uses the recorded TeleOp commands as feedforward and blends + * in Pedro odometry PD correction when the robot drifts from the recording. + */ +@Autonomous(name = "Replay Vlad", group = "Replay") +public class FinalReplayOp extends LinearOpMode { + + private final Robot robot = Robot.getInstance(); + + // ------------------------------------------------------------------------- + // Configuration + // ------------------------------------------------------------------------- + private static final String CSV_PATH = "/sdcard/robot_data_Vlad.csv"; + + // Playback tuning + private static final double LOOKAHEAD_TIME = 0.060; // 60ms lookahead while moving + private static final double LOOKAHEAD_DISABLE_THRESH = 0.04; + private static final double VOLTAGE_REFRESH_SEC = 0.10; + private static final double TIME_SCALING_FACTOR = 0.5; // adjust replay speed for voltage drops + + private static final double MAX_SLEW_RATE = 5.0; + private static final double MIN_DT = 0.008; + private static final double MAX_DT = 0.050; + + // ------------------------------------------------------------------------- + // State + // ------------------------------------------------------------------------- + private final List recordedFrames = new ArrayList<>(); + private final ElapsedTime runtime = new ElapsedTime(); + + private double recordedVoltage = 13.0; + private double cachedVoltage = 13.0; + private double lastVoltageReadTime = -10; + private double timeScalingFactor = 1.0; + + private double prevTime; + private double prevErrorX, prevErrorY, prevErrorHeading; + private double filteredDx, filteredDy, filteredDh; + + private double prevRobotFwd, prevRobotStr, prevRobotTurn; + + private int prevShootRange = -2; + private boolean prevAutoAim = false; + + @Override + public void runOpMode() { + robot.init(hardwareMap); + robot.initFollower(hardwareMap, false); + + telemetry.addData("Status", "Loading CSV..."); + telemetry.update(); + + try { + loadRecordedData(); + calculateRecordedVoltage(); + + if (recordedFrames.isEmpty()) { + telemetry.addData("ERROR", "CSV is empty!"); + telemetry.update(); + return; + } + + telemetry.addData("Status", "Loaded %d frames. Ready.", recordedFrames.size()); + telemetry.update(); + + // Wait for start + while (!isStarted() && !isStopRequested()) { + idle(); + } + + if (isStopRequested()) return; + + // Settle localizer + robot.follower.update(); + sleep(250); + robot.follower.setPose(new Pose(recordedFrames.get(0).x, recordedFrames.get(0).y, recordedFrames.get(0).heading)); + robot.follower.update(); + + applyInitialMechanismState(recordedFrames.get(0)); + + executePlayback(); + + robot.follower.setTeleOpDrive(0, 0, 0, true); + robot.follower.update(); + stopMechanisms(); + + } catch (Exception e) { + telemetry.addData("ERROR", e.toString()); + telemetry.update(); + robot.follower.setTeleOpDrive(0, 0, 0, true); + robot.follower.update(); + stopMechanisms(); + sleep(3000); + } + } + + // ------------------------------------------------------------------------- + // Main loop + // ------------------------------------------------------------------------- + private void executePlayback() { + runtime.reset(); + + double startTs = recordedFrames.get(0).timestamp; + double endTs = recordedFrames.get(recordedFrames.size() - 1).timestamp; + double duration = endTs - startTs; + + int idx = 0; + prevTime = 0; + prevErrorX = 0; + prevErrorY = 0; + prevErrorHeading = 0; + filteredDx = 0; + filteredDy = 0; + filteredDh = 0; + prevShootRange = -2; + prevAutoAim = false; + + prevRobotFwd = 0; + prevRobotStr = 0; + prevRobotTurn = 0; + + double posErrorSum = 0; + double posErrorPeak = 0; + int posErrorSamples = 0; + + robot.follower.startTeleopDrive(); + + while (opModeIsActive() && idx < recordedFrames.size() - 1) { + double now = runtime.seconds(); + + refreshVoltage(now); + updateTimeScaling(); + + double recordingTime = now / timeScalingFactor; + RobotFrame currentBaseFrame = recordedFrames.get(Math.min(idx, recordedFrames.size() - 1)); + double currentInputMag = Math.abs(currentBaseFrame.driveFwd) + + Math.abs(currentBaseFrame.driveStr) + + Math.abs(currentBaseFrame.driveTurn); + double lookahead = currentInputMag > LOOKAHEAD_DISABLE_THRESH ? LOOKAHEAD_TIME : 0.0; + recordingTime += lookahead; + double targetTs = startTs + recordingTime; + + while (idx < recordedFrames.size() - 1 + && recordedFrames.get(idx + 1).timestamp <= targetTs) { + idx++; + } + + RobotFrame fA = recordedFrames.get(idx); + RobotFrame fB = (idx + 1 < recordedFrames.size()) + ? recordedFrames.get(idx + 1) : fA; + + double t = 0; + if (fB.timestamp > fA.timestamp) { + t = (targetTs - fA.timestamp) / (fB.timestamp - fA.timestamp); + t = Range.clip(t, 0.0, 1.0); + } + + double targetX = lerp(fA.x, fB.x, t); + double targetY = lerp(fA.y, fB.y, t); + double targetH = lerpAngle(fA.heading, fB.heading, t); + + double ffFwd = Range.clip(lerp(fA.driveFwd, fB.driveFwd, t), -1.0, 1.0); + double ffStr = Range.clip(lerp(fA.driveStr, fB.driveStr, t), -1.0, 1.0); + double ffTurn = Range.clip(lerp(fA.driveTurn, fB.driveTurn, t), -1.0, 1.0); + + robot.follower.update(); + Pose cur = robot.follower.getPose(); + double curH = cur.getHeading(); + + double dt = Range.clip(now - prevTime, MIN_DT, MAX_DT); + + double exField = targetX - cur.getX(); + double eyField = targetY - cur.getY(); + double eh = normalizeAngle(targetH - curH); + + double cosH = Math.cos(curH); + double sinH = Math.sin(curH); + double exRobot = cosH * exField + sinH * eyField; + double eyRobot = -sinH * exField + cosH * eyField; + + // Do not differentiate the initial lookahead error. That produced + // a one-loop correction spike at the start of every replay. + double rawDx = prevTime > 0 ? (exRobot - prevErrorX) / dt : 0; + double rawDy = prevTime > 0 ? (eyRobot - prevErrorY) / dt : 0; + double rawDh = prevTime > 0 + ? normalizeAngle(eh - prevErrorHeading) / dt : 0; + filteredDx = filteredDx + D_FILTER_ALPHA * (rawDx - filteredDx); + filteredDy = filteredDy + D_FILTER_ALPHA * (rawDy - filteredDy); + filteredDh = filteredDh + D_FILTER_ALPHA * (rawDh - filteredDh); + + double corrFwd = exRobot * kP_translation + filteredDx * kD_translation; + double corrStr = eyRobot * kP_translation + filteredDy * kD_translation; + double corrTurn = eh * kP_rotation + filteredDh * kD_rotation; + + double posError = Math.hypot(exField, eyField); + posErrorSum += posError; + posErrorSamples++; + posErrorPeak = Math.max(posErrorPeak, posError); + double posErrorMean = posErrorSum / posErrorSamples; + + double corrScale = Math.min(posError / CORRECTION_ERROR_SCALE, 1.0); + double dynamicMaxCorr = lerp(MIN_CORRECTION_CAP, MAX_CORRECTION_CAP, corrScale); + + double corrMag = Math.hypot(corrFwd, corrStr); + if (corrMag > dynamicMaxCorr) { + corrFwd *= dynamicMaxCorr / corrMag; + corrStr *= dynamicMaxCorr / corrMag; + } + corrTurn = Range.clip(corrTurn, -dynamicMaxCorr, dynamicMaxCorr); + + double corrWeight = Range.clip(1.0 - Math.exp(-BLEND_K * posError), 0.0, 1.0); + + double robotFwd = ffFwd + corrWeight * corrFwd; + double robotStr = ffStr + corrWeight * corrStr; + // Heading error is independent of position error. Weighting this + // by corrWeight disabled heading correction whenever XY tracking + // happened to be good. + double robotTurn = ffTurn + corrTurn; + + double driveMag = Math.hypot(robotFwd, robotStr); + if (driveMag > 1.0) { + robotFwd /= driveMag; + robotStr /= driveMag; + } + robotTurn = Range.clip(robotTurn, -1.0, 1.0); + + double maxDelta = MAX_SLEW_RATE * dt; + robotFwd = prevRobotFwd + Range.clip(robotFwd - prevRobotFwd, -maxDelta, maxDelta); + robotStr = prevRobotStr + Range.clip(robotStr - prevRobotStr, -maxDelta, maxDelta); + robotTurn = prevRobotTurn + Range.clip(robotTurn - prevRobotTurn, -maxDelta, maxDelta); + + prevRobotFwd = robotFwd; + prevRobotStr = robotStr; + prevRobotTurn = robotTurn; + prevErrorX = exRobot; + prevErrorY = eyRobot; + prevErrorHeading = eh; + prevTime = now; + + robot.follower.setTeleOpDrive(robotFwd, robotStr, robotTurn, true); + + applyMechanismCommands(fA, fB, t); + robot.updateAllSystems(); + + // Hardware Overrides: Ensure servos follow the recording exactly, bypassing live auto-aim. + robot.turretServo.setPosition(lerp(fA.turretPos, fB.turretPos, t)); + robot.angleServo.setPosition(lerp(fA.anglePos, fB.anglePos, t)); + robot.flapsServo.setPosition(lerp(fA.flapPos, fB.flapPos, t)); + + // Telemetry + telemetry.addData("Time", "%.2f / %.2f s", now, duration); + telemetry.addData("Lookahead", "%.3f s", lookahead); + telemetry.addData("PosErr", "%.2f cm", posError); + telemetry.addData("PosErr Mean", "%.2f cm", posErrorMean); + telemetry.addData("PosErr Peak", "%.2f cm", posErrorPeak); + telemetry.addData("HeadErr", "%.1f °", Math.toDegrees(Math .abs(eh))); + telemetry.addData("FF", "fwd=%.2f str=%.2f turn=%.2f", ffFwd, ffStr, ffTurn); + telemetry.addData("Corr", "fwd=%.2f str=%.2f turn=%.2f w=%.0f%%", + corrFwd, corrStr, corrTurn, corrWeight * 100); + telemetry.addData("Cmd", "fwd=%.2f str=%.2f turn=%.2f", robotFwd, robotStr, robotTurn); + telemetry.addData("Mech", "rng=%d aa=%s flap=%s", + (int) Math.round(lerp(fA.shootRange, fB.shootRange, t)), + (t < 0.5 ? fA.autoAim : fB.autoAim) ? "Y" : "N", + lerpBool(fA.flapOpen, fB.flapOpen, t) ? "Y" : "N"); + telemetry.update(); + + idle(); + } + } + + // ------------------------------------------------------------------------- + // Mechanism application + // ------------------------------------------------------------------------- + private void applyInitialMechanismState(RobotFrame f) { + robot.intake.reversed = INTAKE_REVERSE; + robot.turret.driverOffset = f.turretOffset; + robot.Blue_Target = f.blueTarget; + robot.flap.open = f.flapOpen; + robot.intake.speed = f.intakeCmd; + robot.loader.speed = f.loaderCmd; + + // Force auto-aim off for Replay; we use recorded positions. + robot.turret.autoAimEnabled = false; + prevAutoAim = false; + + if (f.shootRange >= 0) { + if (f.shootRange == 0) { + // Recorded as Auto-Aim: get a live kS fallback, then force the + // recorded interpolated velocity and angle. + robot.shoot.activateRange(0); + robot.outtake.activeConfig.velocity = f.leftShtrVel; + robot.outtake.activeConfig.angle = f.anglePos; + if (f.outtakeKs > 0) { + robot.outtake.activeConfig.kS = f.outtakeKs; + } + } else { + robot.shoot.activateRange(f.shootRange); + } + prevShootRange = f.shootRange; + } else if (f.shootRange == -1) { + robot.shoot.deactivate(); + prevShootRange = -1; + } + } + + private void applyMechanismCommands(RobotFrame a, RobotFrame b, double t) { + robot.intake.speed = lerp(a.intakeCmd, b.intakeCmd, t); + robot.loader.speed = lerp(a.loaderCmd, b.loaderCmd, t); + robot.flap.open = lerpBool(a.flapOpen, b.flapOpen, t); + robot.turret.driverOffset = lerp(a.turretOffset, b.turretOffset, t); + robot.Blue_Target = t < 0.5 ? a.blueTarget : b.blueTarget; + + // Ensure auto-aim is off. + robot.turret.autoAimEnabled = false; + + int curRange = (int) Math.round(lerp(a.shootRange, b.shootRange, t)); + if (curRange == 0) { + // Recorded as Auto-Aim: live kS fallback, recorded velocity/angle. + robot.shoot.activateRange(0); + robot.outtake.activeConfig.velocity = lerp(a.leftShtrVel, b.leftShtrVel, t); + robot.outtake.activeConfig.angle = lerp(a.anglePos, b.anglePos, t); + double ks = lerp(a.outtakeKs, b.outtakeKs, t); + if (ks > 0) { + robot.outtake.activeConfig.kS = ks; + } + } else if (curRange != prevShootRange) { + if (curRange > 0) { + robot.shoot.activateRange(curRange); + } else if (curRange == -1 && prevShootRange > 0) { + robot.shoot.deactivate(); + } + } + prevShootRange = curRange; + } + + private void stopMechanisms() { + robot.intake.speed = 0; + robot.loader.speed = 0; + robot.flap.open = false; + robot.shoot.deactivate(); + robot.updateAllSystems(); + } + + // ------------------------------------------------------------------------- + // Helpers + // ------------------------------------------------------------------------- + private void loadRecordedData() throws IOException { + File f = new File(CSV_PATH); + if (!f.exists()) throw new IOException("CSV not found: " + CSV_PATH); + try (BufferedReader r = new BufferedReader(new FileReader(f))) { + r.readLine(); // skip header + String line; + while ((line = r.readLine()) != null) { + String[] d = line.split(","); + if (d.length >= 16) { + recordedFrames.add(new RobotFrame(d)); + } + } + } + } + + private void calculateRecordedVoltage() { + double total = 0; int n = 0; + for (RobotFrame f : recordedFrames) { + if (f.voltage > 0) { total += f.voltage; n++; } + } + if (n > 0) recordedVoltage = total / n; + } + + private void refreshVoltage(double now) { + if (now - lastVoltageReadTime >= VOLTAGE_REFRESH_SEC) { + cachedVoltage = hardwareMap.voltageSensor.iterator().next().getVoltage(); + lastVoltageReadTime = now; + } + } + + private void updateTimeScaling() { + double ratio = cachedVoltage / recordedVoltage; + timeScalingFactor = ratio < 1.0 + ? Range.clip(1.0 + TIME_SCALING_FACTOR * (1.0 - ratio), 1.0, 2.0) + : 1.0; + } + + private static double lerp(double a, double b, double t) { + return a + (b - a) * t; + } + + private static boolean lerpBool(boolean a, boolean b, double t) { + return t < 0.5 ? a : b; + } + + private static double lerpAngle(double a, double b, double t) { + return normalizeAngle(a + normalizeAngle(b - a) * t); + } + + private static double normalizeAngle(double a) { + while (a > Math.PI) a -= 2 * Math.PI; + while (a < -Math.PI) a += 2 * Math.PI; + return a; + } + + private static class RobotFrame { + double timestamp; + double lrPow, rrPow, lfPow, rfPow; + double x, y, heading; + double voltage; + double intakeVel, loaderVel, leftShtrVel, rightShtrVel; + double turretPos, anglePos, flapPos; + double intakeCmd = 0; + double loaderCmd = 0; + boolean flapOpen = false; + int shootRange = -2; + double turretOffset = 0; + boolean blueTarget = false; + boolean autoAim = false; + double driveFwd = 0; + double driveStr = 0; + double driveTurn = 0; + double outtakeKs = 0; + + RobotFrame(String[] d) { + timestamp = Double.parseDouble(d[0]); + lrPow = Double.parseDouble(d[1]); + rrPow = Double.parseDouble(d[2]); + lfPow = Double.parseDouble(d[3]); + rfPow = Double.parseDouble(d[4]); + x = Double.parseDouble(d[5]); + y = Double.parseDouble(d[6]); + heading = Double.parseDouble(d[7]); + voltage = Double.parseDouble(d[8]); + intakeVel = Double.parseDouble(d[9]); + loaderVel = Double.parseDouble(d[10]); + leftShtrVel = Double.parseDouble(d[11]); + rightShtrVel = Double.parseDouble(d[12]); + turretPos = Double.parseDouble(d[13]); + anglePos = Double.parseDouble(d[14]); + flapPos = Double.parseDouble(d[15]); + if (d.length >= 22) { + intakeCmd = Double.parseDouble(d[16]); + loaderCmd = Double.parseDouble(d[17]); + flapOpen = d[18].equals("1") || d[18].equalsIgnoreCase("true"); + shootRange = (int) Math.round(Double.parseDouble(d[19])); + turretOffset = Double.parseDouble(d[20]); + blueTarget = d[21].equals("1") || d[21].equalsIgnoreCase("true"); + } + if (d.length >= 23) { + autoAim = d[22].equals("1") || d[22].equalsIgnoreCase("true"); + } + inferDriveCommandsFromWheelPowers(); + if (d.length >= 26) { + driveFwd = Double.parseDouble(d[23]); + driveStr = Double.parseDouble(d[24]); + driveTurn = Double.parseDouble(d[25]); + } + if (d.length >= 27) { + outtakeKs = Double.parseDouble(d[26]); + } + } + + private void inferDriveCommandsFromWheelPowers() { + driveFwd = (lfPow + rfPow + lrPow + rrPow) / 4.0; + driveStr = (lfPow - rfPow - lrPow + rrPow) / 4.0; + driveTurn = (lfPow - rfPow + lrPow - rrPow) / 4.0; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/OldReplayOp2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/OldReplayOp2.java deleted file mode 100644 index 32b9df0..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/OldReplayOp2.java +++ /dev/null @@ -1,341 +0,0 @@ -package org.firstinspires.ftc.teamcode.kronbot.autonomous; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.util.ElapsedTime; -import com.qualcomm.robotcore.util.Range; -import com.pedropathing.follower.Follower; -import com.pedropathing.geometry.Pose; -import android.os.Environment; -import org.firstinspires.ftc.teamcode.kronbot.Robot; - -import java.io.BufferedReader; -import java.io.File; -import java.io.FileReader; -import java.util.ArrayList; -import java.util.List; - -/** - * Replay autonomous that follows a path recorded by DataRecordingOp2. - * - * Simplified approach (matching the old working ImprovedAuto pattern): - * - PedroPathing follower is used ONLY for odometry (getPoseTracker().update()). - * We never call follower.update() because that also drives the motors. - * - We set the starting pose to the RECORDED start pose, so all coordinates are - * in the same absolute frame. No coordinate transforms needed. - * - A simple PD controller drives toward each interpolated target pose. - * - Mecanum mixing converts robot-relative PD output to wheel powers. - */ -@Autonomous(name = "Drive & Mech Replay", group = "Replay") -public class OldReplayOp2 extends LinearOpMode { - - private static final String CSV_PATH = Environment.getExternalStorageDirectory().getPath() + "/robot_data.csv"; - - // PD Constants - private static final double kP_translation = 0.08; - private static final double kD_translation = 0.02; - private static final double kP_rotation = 1.5; - private static final double kD_rotation = 0.1; - private static final double MAX_POWER = 0.8; - private static final double TIME_SCALING_FACTOR = 0.7; - - private Robot robot; - private Follower follower; - private final List recordedFrames = new ArrayList<>(); - private final ElapsedTime runtime = new ElapsedTime(); - - // PD state - private double prevErrorX = 0, prevErrorY = 0, prevErrorHeading = 0, prevTime = 0; - - // Battery compensation - private double recordedVoltageAvg = 12.0; - private double timeScalingFactor = 1.0; - - // Drive motors - private DcMotorEx leftFront, rightFront, leftRear, rightRear; - - /** - * A single recorded frame. - * Heading is in RADIANS (from follower.getHeading() in DataRecordingOp2). - */ - private static class ReplayFrame { - double timestamp; - double x, y, headingRad; - double voltage; - - // Mechanism data - double intakePwr, loaderPwr, leftShtrPwr, rightShtrPwr; - double turretPos, anglePos, flapPos; - - ReplayFrame(String[] data) { - // CSV: Time,LR,RR,LF,RF,X,Y,Heading,Voltage,IntakePwr,LoaderPwr,LeftShtrPwr,RightShtrPwr,TurretPos,AnglePos,FlapPos - this.timestamp = Double.parseDouble(data[0]); - this.x = Double.parseDouble(data[5]); - this.y = Double.parseDouble(data[6]); - this.headingRad = Double.parseDouble(data[7]); // radians - this.voltage = Double.parseDouble(data[8]); - - if (data.length > 9) { - this.intakePwr = Double.parseDouble(data[9]); - this.loaderPwr = Double.parseDouble(data[10]); - this.leftShtrPwr = Double.parseDouble(data[11]); - this.rightShtrPwr = Double.parseDouble(data[12]); - this.turretPos = Double.parseDouble(data[13]); - this.anglePos = Double.parseDouble(data[14]); - this.flapPos = Double.parseDouble(data[15]); - } - } - } - - @Override - public void runOpMode() { - robot = Robot.getInstance(); - robot.initFollower(hardwareMap, true); // add this - robot.init(hardwareMap); - follower = robot.follower; - - // Get drive motors and match PedroPathing's MecanumConstants directions. - leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); - rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); - leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); - rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); - - leftFront.setDirection(DcMotorSimple.Direction.REVERSE); - leftRear.setDirection(DcMotorSimple.Direction.REVERSE); - rightFront.setDirection(DcMotorSimple.Direction.REVERSE); - rightRear.setDirection(DcMotorSimple.Direction.FORWARD); - - leftFront.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - rightFront.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - leftRear.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - rightRear.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - - if (!loadRecordedData()) { - telemetry.addData("Error", "Failed to load data from " + CSV_PATH); - telemetry.update(); - return; - } - - // Set the starting pose to the RECORDED start pose so both coordinate - // systems match — no transform needed during playback. - ReplayFrame startFrame = recordedFrames.get(0); - follower.setStartingPose(new Pose(startFrame.x, startFrame.y, startFrame.headingRad)); - - telemetry.addData("Status", "Ready to Replay (Drive + Mechs)"); - telemetry.addData("Frames", recordedFrames.size()); - telemetry.addData("Duration", "%.2f s", - recordedFrames.get(recordedFrames.size() - 1).timestamp - startFrame.timestamp); - telemetry.addData("Start Pose", "(%.1f, %.1f) h=%.1f°", - startFrame.x, startFrame.y, Math.toDegrees(startFrame.headingRad)); - telemetry.addData("Recorded Voltage", "%.1f V", recordedVoltageAvg); - telemetry.update(); - - waitForStart(); - if (isStopRequested()) return; - - runtime.reset(); - prevTime = 0; - executePlayback(); - } - - private void executePlayback() { - double startTs = recordedFrames.get(0).timestamp; - double endTs = recordedFrames.get(recordedFrames.size() - 1).timestamp; - double duration = endTs - startTs; - int idx = 0; - - while (opModeIsActive() && idx < recordedFrames.size() - 1) { - double now = runtime.seconds(); - updateTimeScaling(); - double adjustedTime = now / timeScalingFactor; - double targetTs = startTs + adjustedTime; - - // Advance to the correct frame - while (idx < recordedFrames.size() - 1 && recordedFrames.get(idx + 1).timestamp <= targetTs) { - idx++; - } - if (idx >= recordedFrames.size() - 1) break; - - // Interpolate between frame[idx] and frame[idx+1] - ReplayFrame frameA = recordedFrames.get(idx); - ReplayFrame frameB = recordedFrames.get(idx + 1); - double segDur = frameB.timestamp - frameA.timestamp; - double t = (segDur > 0.0001) - ? Range.clip((targetTs - frameA.timestamp) / segDur, 0.0, 1.0) - : 0.0; - - double targetX = lerp(frameA.x, frameB.x, t); - double targetY = lerp(frameA.y, frameB.y, t); - double targetHeading = lerpAngle(frameA.headingRad, frameB.headingRad, t); - - // Update odometry only (NOT follower.update() which also drives motors) - follower.getPoseTracker().update(); - Pose curPose = follower.getPose(); - double curHeading = curPose.getHeading(); - - // PD drive control — all in absolute coordinates, no transform needed - applyDrivePD(curPose.getX(), curPose.getY(), curHeading, - targetX, targetY, targetHeading, now); - - // Replay mechanism states - setMechanismStates(frameA, frameB, t); - - // Telemetry - double posErr = Math.hypot(targetX - curPose.getX(), targetY - curPose.getY()); - double headErr = Math.toDegrees(Math.abs(normalizeAngle(targetHeading - curHeading))); - double curVoltage = hardwareMap.voltageSensor.iterator().next().getVoltage(); - - telemetry.addData("Time", "%.2f/%.2f s (%.2fx)", now, duration * timeScalingFactor, timeScalingFactor); - telemetry.addData("Frame", "%d/%d (lerp %.2f)", idx, recordedFrames.size(), t); - telemetry.addData("PosErr", "%.2f in", posErr); - telemetry.addData("HeadErr", "%.1f°", headErr); - telemetry.addData("Target", "(%.1f, %.1f) h=%.1f°", targetX, targetY, Math.toDegrees(targetHeading)); - telemetry.addData("Current", "(%.1f, %.1f) h=%.1f°", curPose.getX(), curPose.getY(), Math.toDegrees(curHeading)); - telemetry.addData("Voltage", "%.1f V (rec: %.1f V)", curVoltage, recordedVoltageAvg); - telemetry.update(); - - idle(); - } - stopRobot(); - requestOpModeStop(); - } - - /** - * PD controller — field-relative error rotated into robot frame, then mecanum mixed. - * Same structure as the old working ImprovedAuto. - */ - private void applyDrivePD(double curX, double curY, double curHeading, - double tgtX, double tgtY, double tgtHeading, - double now) { - double errorX = tgtX - curX; - double errorY = tgtY - curY; - double dt = now - prevTime; - - double dX = (dt > 0) ? (errorX - prevErrorX) / dt : 0; - double dY = (dt > 0) ? (errorY - prevErrorY) / dt : 0; - - // Field-relative PD output - double fieldX = errorX * kP_translation + dX * kD_translation; - double fieldY = errorY * kP_translation + dY * kD_translation; - - // Rotate into robot-relative frame - double cos = Math.cos(curHeading); - double sin = Math.sin(curHeading); - double robotX = cos * fieldX + sin * fieldY; - double robotY = -sin * fieldX + cos * fieldY; - - // Clamp magnitude - double mag = Math.hypot(robotX, robotY); - if (mag > MAX_POWER) { - robotX *= MAX_POWER / mag; - robotY *= MAX_POWER / mag; - } - - prevErrorX = errorX; - prevErrorY = errorY; - prevTime = now; - - // Heading PD - double errorH = normalizeAngle(tgtHeading - curHeading); - double dH = (dt > 0) ? (errorH - prevErrorHeading) / dt : 0; - double rotPower = Range.clip(errorH * kP_rotation + dH * kD_rotation, -MAX_POWER, MAX_POWER); - prevErrorHeading = errorH; - - // Mecanum mixing: robotY = forward/back, robotX = strafe, rotPower = turn - double fl = robotY + robotX + rotPower; - double fr = robotY - robotX - rotPower; - double bl = robotY - robotX + rotPower; - double br = robotY + robotX - rotPower; - - // Normalize so no wheel exceeds 1.0 - double maxPwr = Math.max(1.0, Math.max(Math.abs(fl), - Math.max(Math.abs(fr), Math.max(Math.abs(bl), Math.abs(br))))); - leftFront.setPower(fl / maxPwr); - rightFront.setPower(fr / maxPwr); - leftRear.setPower(bl / maxPwr); - rightRear.setPower(br / maxPwr); - } - - /** - * Set mechanism outputs. Servo positions are interpolated; motor powers snap to nearest frame. - */ - private void setMechanismStates(ReplayFrame a, ReplayFrame b, double t) { - ReplayFrame src = (t < 0.5) ? a : b; - if (robot.intakeMotor != null) robot.intakeMotor.setPower(src.intakePwr); - if (robot.loaderMotor != null) robot.loaderMotor.setPower(src.loaderPwr); - if (robot.leftOuttake != null) robot.leftOuttake.setPower(src.leftShtrPwr); - if (robot.rightOuttake != null) robot.rightOuttake.setPower(src.rightShtrPwr); - - if (robot.turretServo != null) robot.turretServo.setPosition(lerp(a.turretPos, b.turretPos, t)); - if (robot.angleServo != null) robot.angleServo.setPosition(lerp(a.anglePos, b.anglePos, t)); - if (robot.flapsServo != null) robot.flapsServo.setPosition(lerp(a.flapPos, b.flapPos, t)); - } - - private void updateTimeScaling() { - double currentVoltage = hardwareMap.voltageSensor.iterator().next().getVoltage(); - double voltageRatio = currentVoltage / recordedVoltageAvg; - - if (voltageRatio < 1.0) { - timeScalingFactor = 1.0 + TIME_SCALING_FACTOR * (1.0 - voltageRatio); - } else { - timeScalingFactor = 1.0; - } - timeScalingFactor = Range.clip(timeScalingFactor, 1.0, 2.0); - } - - private boolean loadRecordedData() { - File file = new File(CSV_PATH); - if (!file.exists()) return false; - - try (BufferedReader br = new BufferedReader(new FileReader(file))) { - br.readLine(); // Skip header - double totalV = 0; - int count = 0; - String line; - while ((line = br.readLine()) != null) { - String[] parts = line.split(","); - if (parts.length >= 16) { - ReplayFrame frame = new ReplayFrame(parts); - recordedFrames.add(frame); - if (frame.voltage > 5) { totalV += frame.voltage; count++; } - } - } - if (count > 0) recordedVoltageAvg = totalV / count; - return !recordedFrames.isEmpty(); - } catch (Exception e) { - return false; - } - } - - private void stopRobot() { - leftFront.setPower(0); - rightFront.setPower(0); - leftRear.setPower(0); - rightRear.setPower(0); - - if (robot.intakeMotor != null) robot.intakeMotor.setPower(0); - if (robot.loaderMotor != null) robot.loaderMotor.setPower(0); - if (robot.leftOuttake != null) robot.leftOuttake.setPower(0); - if (robot.rightOuttake != null) robot.rightOuttake.setPower(0); - } - - // ---- Utility helpers ---- - - private static double lerp(double a, double b, double t) { - return a + (b - a) * t; - } - - private static double lerpAngle(double a, double b, double t) { - double diff = normalizeAngle(b - a); - return normalizeAngle(a + diff * t); - } - - private static double normalizeAngle(double angle) { - while (angle > Math.PI) angle -= 2.0 * Math.PI; - while (angle < -Math.PI) angle += 2.0 * Math.PI; - return angle; - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAutoOp3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAutoOp3.java deleted file mode 100644 index 6d787ca..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAutoOp3.java +++ /dev/null @@ -1,451 +0,0 @@ -package org.firstinspires.ftc.teamcode.kronbot.autonomous; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.util.ElapsedTime; -import com.qualcomm.robotcore.util.Range; -import com.pedropathing.geometry.Pose; - -import org.firstinspires.ftc.teamcode.kronbot.Robot; - -import java.io.BufferedReader; -import java.io.File; -import java.io.FileReader; -import java.io.IOException; -import java.util.ArrayList; -import java.util.List; - -/** - * Replay Autonomous — fixed version of the original simple architecture. - * - * Reads the V3 CSV format produced by DataRecordingOp3: - * Time, LR, RR, LF, RF, X, Y, Heading, Voltage, - * IntakeVel, LoaderVel, LeftShtrVel, RightShtrVel, - * TurretPos, AnglePos, FlapPos - * - * Fixes applied over the original ReplayAutoOp: - * 1. sleep(20) removed — replaced with idle() so loop runs at hardware speed - * 2. D-term prevTime bug fixed — prevTime updated once per loop after ALL PD calculations - * 3. Frame interpolation — lerps between adjacent frames for smooth pose targets - * 4. Mechanism velocities — reads IntakeVel/LoaderVel columns and converts to power - * 5. Voltage cached — sensor read at most every 100ms, not every loop - * - * Architecture is intentionally kept identical to the original: - * setTeleOpDrive() is used for all motor output, PedroPathing handles localization. - * No direct motor writes — avoids any conflict with PedroPathing's internal update(). - */ -@Autonomous(name = "Replay Auto V3", group = "Autonomous") -public class ReplayAutoOp3 extends LinearOpMode { - - private static final String CSV_PATH = "/sdcard/robot_data.csv"; - - // PD gains — same as original - private static final double MAX_POWER = 0.8; - private static final double kP_translation = 0.08; - private static final double kD_translation = 0.02; - private static final double kP_rotation = 1.5; - private static final double kD_rotation = 0.1; - - // Lookahead: follow a target this many seconds ahead in the recording - // Keeps the robot moving smoothly instead of stalling at near-zero error - private static final double LOOKAHEAD_TIME = 0.15; - - // Derivative smoothing — prevents D-term spikes at start/stop transitions - private static final double D_FILTER_ALPHA = 0.5; // low-pass filter (0 = ignore D, 1 = raw) - private static final double MAX_D_CONTRIBUTION = 0.15; // max power the D-term alone can add - - // Velocity-transition ramp — ramps output 0→1 over this period whenever the - // target transitions from stationary to moving (fires at every stop→start, not just opmode start) - private static final double RAMP_UP_TIME = 0.3; // seconds - private static final double TARGET_STILL_THRESH = 5.0; // cm/sec — target speed below this = "still" - - // Battery compensation - private static final double NOMINAL_VOLTAGE = 12.0; - private static final double TIME_SCALING_FACTOR = 0.7; - - // Voltage sensor is slow (I2C) — only re-read every 100ms - private static final double VOLTAGE_REFRESH_SEC = 0.1; - - // Intake/loader velocity → power scaling - // Tune this if intake feels too weak or too strong during replay - private static final double INTAKE_VEL_SCALE = 500.0; // ticks/sec that maps to power 1.0 - private static final double LOADER_VEL_SCALE = 500.0; - - // Shooter - private static final double SHOOTER_VEL_THRESHOLD = 50.0; // below this = off - private static final double SHOOTER_MAX_VEL = 2000.0; // tune to your motor's free-spin vel - - private final Robot robot = Robot.getInstance(); - private final ElapsedTime runtime = new ElapsedTime(); - private final List recordedFrames = new ArrayList<>(); - - // PD state - private double prevErrorX = 0; - private double prevErrorY = 0; - private double prevErrorHeading = 0; - private double prevTime = 0; - // Filtered derivatives — smoothed across loops to prevent spikes - private double filteredDx = 0; - private double filteredDy = 0; - private double filteredDh = 0; - - // Velocity-transition ramp state - private double prevTargetX = 0; - private double prevTargetY = 0; - private boolean targetWasStill = true; // was the target stationary last loop? - private double motionStartTime = 0; // timestamp when target started moving - - // Voltage caching - private double cachedVoltage = NOMINAL_VOLTAGE; - private double lastVoltageReadTime = -999; - - // Battery compensation result - private double recordedVoltage = NOMINAL_VOLTAGE; - private double timeScalingFactor = 1.0; - - // ------------------------------------------------------------------------- - // Data model — matches the V3 CSV written by DataRecordingOp3 - // ------------------------------------------------------------------------- - private static class RobotFrame { - double timestamp; - // Drive motor powers (columns 1-4, used as feedforward reference only — not replayed directly) - double lrPow, rrPow, lfPow, rfPow; - // Pose - double x, y, heading; - double voltage; - // Mechanism velocities - double intakeVel, loaderVel, leftShtrVel, rightShtrVel; - // Servo positions - double turretPos, anglePos, flapPos; - - RobotFrame(String[] d) { - timestamp = Double.parseDouble(d[0]); - lrPow = Double.parseDouble(d[1]); - rrPow = Double.parseDouble(d[2]); - lfPow = Double.parseDouble(d[3]); - rfPow = Double.parseDouble(d[4]); - x = Double.parseDouble(d[5]); - y = Double.parseDouble(d[6]); - heading = Double.parseDouble(d[7]); - voltage = Double.parseDouble(d[8]); - intakeVel = Double.parseDouble(d[9]); - loaderVel = Double.parseDouble(d[10]); - leftShtrVel = Double.parseDouble(d[11]); - rightShtrVel= Double.parseDouble(d[12]); - turretPos = Double.parseDouble(d[13]); - anglePos = Double.parseDouble(d[14]); - flapPos = Double.parseDouble(d[15]); - } - } - - // ------------------------------------------------------------------------- - // OpMode - // ------------------------------------------------------------------------- - @Override - public void runOpMode() { - telemetry.addLine("Initializing Replay Auto..."); - telemetry.update(); - - robot.initFollower(hardwareMap, true); - robot.init(hardwareMap); - - try { - robot.follower.getPoseTracker().resetIMU(); - } catch (InterruptedException e) { - telemetry.addLine("IMU Reset Interrupted"); - } - - try { - loadRecordedData(); - - if (recordedFrames.isEmpty()) { - telemetry.addLine("ERROR: No recorded data found!"); - telemetry.update(); - return; - } - - calculateRecordedVoltage(); - - RobotFrame first = recordedFrames.get(0); - robot.follower.setPose(new Pose(first.x, first.y, first.heading)); - - telemetry.addLine("Ready."); - telemetry.addData("Frames", recordedFrames.size()); - telemetry.addData("Duration", "%.2f s", - recordedFrames.get(recordedFrames.size() - 1).timestamp - first.timestamp); - telemetry.addData("Recorded voltage", "%.1f V", recordedVoltage); - telemetry.update(); - - waitForStart(); - if (isStopRequested()) return; - - executePlayback(); - - robot.follower.setTeleOpDrive(0, 0, 0, true); - robot.follower.update(); - stopMechanisms(); - - } catch (Exception e) { - telemetry.addData("ERROR", e.toString()); - telemetry.update(); - robot.follower.setTeleOpDrive(0, 0, 0, true); - robot.follower.update(); - stopMechanisms(); - sleep(3000); - } - } - - // ------------------------------------------------------------------------- - // Main loop - // ------------------------------------------------------------------------- - private void executePlayback() { - runtime.reset(); - - double startTs = recordedFrames.get(0).timestamp; - double endTs = recordedFrames.get(recordedFrames.size() - 1).timestamp; - double duration = endTs - startTs; - - int idx = 0; - prevTime = 0; - prevErrorX = 0; - prevErrorY = 0; - prevErrorHeading = 0; - filteredDx = 0; - filteredDy = 0; - filteredDh = 0; - - // Init velocity-transition ramp with first frame's position - RobotFrame firstFrame = recordedFrames.get(0); - prevTargetX = firstFrame.x; - prevTargetY = firstFrame.y; - targetWasStill = true; - motionStartTime = 0; - - robot.follower.startTeleopDrive(); - - while (opModeIsActive() && idx < recordedFrames.size() - 1) { - double now = runtime.seconds(); - - // Refresh voltage sensor at most every 100ms - refreshVoltage(now); - updateTimeScaling(); - - // Current position in the recording, with lookahead added - double recordingTime = (now / timeScalingFactor) + LOOKAHEAD_TIME; - double targetTs = startTs + recordingTime; - - // Advance index to the frame just before targetTs - while (idx < recordedFrames.size() - 1 - && recordedFrames.get(idx + 1).timestamp <= targetTs) { - idx++; - } - - // Interpolate between frame[idx] and frame[idx+1] - RobotFrame fA = recordedFrames.get(idx); - RobotFrame fB = (idx + 1 < recordedFrames.size()) - ? recordedFrames.get(idx + 1) : fA; - - double t = 0; - if (fB.timestamp > fA.timestamp) { - t = (targetTs - fA.timestamp) / (fB.timestamp - fA.timestamp); - t = Range.clip(t, 0.0, 1.0); - } - - // Smooth interpolated target pose - double targetX = lerp(fA.x, fB.x, t); - double targetY = lerp(fA.y, fB.y, t); - double targetH = lerpAngle(fA.heading, fB.heading, t); - Pose target = new Pose(targetX, targetY, targetH); - - // Update localizer - robot.follower.update(); - Pose cur = robot.follower.getPose(); - - // --- PD with filtered derivative --- - double dt = now - prevTime; - - // Translation PD (field-centric) - double ex = target.getX() - cur.getX(); - double ey = target.getY() - cur.getY(); - - double rawDx = dt > 1e-6 ? (ex - prevErrorX) / dt : 0; - double rawDy = dt > 1e-6 ? (ey - prevErrorY) / dt : 0; - - // Low-pass filter the derivatives to eliminate spikes - filteredDx = filteredDx + D_FILTER_ALPHA * (rawDx - filteredDx); - filteredDy = filteredDy + D_FILTER_ALPHA * (rawDy - filteredDy); - - // Clamp derivative contribution - double dxClamped = Range.clip(filteredDx * kD_translation, -MAX_D_CONTRIBUTION, MAX_D_CONTRIBUTION); - double dyClamped = Range.clip(filteredDy * kD_translation, -MAX_D_CONTRIBUTION, MAX_D_CONTRIBUTION); - - double fx = ex * kP_translation + dxClamped; - double fy = ey * kP_translation + dyClamped; - - // Rotate field-centric correction into robot frame - double cosH = Math.cos(cur.getHeading()); - double sinH = Math.sin(cur.getHeading()); - double fwdCmd = cosH * fx + sinH * fy; - double strCmd = -sinH * fx + cosH * fy; - - // Clamp translation magnitude - double norm = Math.hypot(fwdCmd, strCmd); - if (norm > MAX_POWER) { - fwdCmd *= MAX_POWER / norm; - strCmd *= MAX_POWER / norm; - } - - // Rotation PD with filtered derivative - double eh = normalizeAngle(target.getHeading() - cur.getHeading()); - double rawDh = dt > 1e-6 ? (eh - prevErrorHeading) / dt : 0; - filteredDh = filteredDh + D_FILTER_ALPHA * (rawDh - filteredDh); - double dhClamped = Range.clip(filteredDh * kD_rotation, -MAX_D_CONTRIBUTION, MAX_D_CONTRIBUTION); - - double turnCmd = Range.clip( - eh * kP_rotation + dhClamped, - -MAX_POWER, MAX_POWER - ); - - // Velocity-transition ramp — detect when target goes from still → moving - double targetDist = Math.hypot(targetX - prevTargetX, targetY - prevTargetY); - double targetSpeed = dt > 1e-6 ? targetDist / dt : 0; // cm/sec, independent of loop rate - boolean targetIsStill = targetSpeed < TARGET_STILL_THRESH; - - if (targetWasStill && !targetIsStill) { - // Target just started moving — begin a new ramp and reset stale derivatives - motionStartTime = now; - filteredDx = 0; - filteredDy = 0; - filteredDh = 0; - } - targetWasStill = targetIsStill; - prevTargetX = targetX; - prevTargetY = targetY; - - double timeSinceMotionStart = now - motionStartTime; - double ramp = Range.clip(timeSinceMotionStart / RAMP_UP_TIME, 0.0, 1.0); - fwdCmd *= ramp; - strCmd *= ramp; - turnCmd *= ramp; - - // Update PD state — ONCE, after all derivative calculations - prevErrorX = ex; - prevErrorY = ey; - prevErrorHeading = eh; - prevTime = now; - - // Drive via PedroPathing (same as original — no motor bypass) - robot.follower.setTeleOpDrive(fwdCmd, strCmd, turnCmd, false); - - // Mechanisms - controlMechanisms(fA, fB, t); - - // Telemetry - telemetry.addData("Time", "%.2f / %.2f s (scale %.2f)", now, duration, timeScalingFactor); - telemetry.addData("Frame", "%d / %d (t=%.2f)", idx, recordedFrames.size(), t); - telemetry.addData("PosErr", "%.2f cm", Math.hypot(ex, ey)); - telemetry.addData("HeadErr", "%.1f °", Math.toDegrees(Math.abs(eh))); - telemetry.addData("Cmd", "fwd=%.2f str=%.2f turn=%.2f", fwdCmd, strCmd, turnCmd); - telemetry.addData("Voltage", "%.1f V (rec %.1f V)", cachedVoltage, recordedVoltage); - telemetry.update(); - - // FIX #1: no sleep(20) — yield to SDK scheduler only - idle(); - } - } - - // ------------------------------------------------------------------------- - // Mechanisms - // ------------------------------------------------------------------------- - private void controlMechanisms(RobotFrame a, RobotFrame b, double t) { - // Servos — interpolated - robot.turretServo.setPosition(lerp(a.turretPos, b.turretPos, t)); - robot.angleServo.setPosition( lerp(a.anglePos, b.anglePos, t)); - robot.flapsServo.setPosition( lerp(a.flapPos, b.flapPos, t)); - - // Shooter — simple FF+P velocity controller - double targetVel = lerp(a.leftShtrVel, b.leftShtrVel, t); - if (Math.abs(targetVel) > SHOOTER_VEL_THRESHOLD) { - double ffPower = targetVel / SHOOTER_MAX_VEL; - double velError = targetVel - robot.leftOuttake.getVelocity(); - double shootPower = Range.clip(ffPower + velError * 0.0008, 0.0, 1.0); - robot.leftOuttake.setPower(shootPower); - robot.rightOuttake.setPower(shootPower); - } else { - // Gentle brake, same as original TeleOp behaviour - double braking = robot.leftOuttake.getVelocity() > 21 ? -0.1 : 0.0; - robot.leftOuttake.setPower(braking); - robot.rightOuttake.setPower(braking); - } - - // Intake and loader — velocity → power - double intakePow = velToPower(lerp(a.intakeVel, b.intakeVel, t), INTAKE_VEL_SCALE); - double loaderPow = velToPower(lerp(a.loaderVel, b.loaderVel, t), LOADER_VEL_SCALE); - robot.intakeMotor.setPower(intakePow); - robot.loaderMotor.setPower(loaderPow); - } - - private void stopMechanisms() { - robot.intakeMotor.setPower(0); - robot.loaderMotor.setPower(0); - robot.leftOuttake.setPower(0); - robot.rightOuttake.setPower(0); - } - - // ------------------------------------------------------------------------- - // Helpers - // ------------------------------------------------------------------------- - private void loadRecordedData() throws IOException { - File f = new File(CSV_PATH); - if (!f.exists()) throw new IOException("CSV not found: " + CSV_PATH); - try (BufferedReader r = new BufferedReader(new FileReader(f))) { - r.readLine(); // skip header - String line; - while ((line = r.readLine()) != null) { - String[] d = line.split(","); - if (d.length >= 16) recordedFrames.add(new RobotFrame(d)); - } - } - } - - private void calculateRecordedVoltage() { - double total = 0; int n = 0; - for (RobotFrame f : recordedFrames) { - if (f.voltage > 0) { total += f.voltage; n++; } - } - if (n > 0) recordedVoltage = total / n; - } - - private void refreshVoltage(double now) { - if (now - lastVoltageReadTime >= VOLTAGE_REFRESH_SEC) { - cachedVoltage = hardwareMap.voltageSensor.iterator().next().getVoltage(); - lastVoltageReadTime = now; - } - } - - private void updateTimeScaling() { - double ratio = cachedVoltage / recordedVoltage; - timeScalingFactor = ratio < 1.0 - ? Range.clip(1.0 + TIME_SCALING_FACTOR * (1.0 - ratio), 1.0, 2.0) - : 1.0; - } - - /** Converts a recorded velocity to a clamped power value, preserving sign. */ - private static double velToPower(double vel, double scale) { - return Range.clip(Math.signum(vel) * Math.min(Math.abs(vel) / scale, 1.0), -1.0, 1.0); - } - - private static double lerp(double a, double b, double t) { - return a + (b - a) * t; - } - - private static double lerpAngle(double a, double b, double t) { - return normalizeAngle(a + normalizeAngle(b - a) * t); - } - - private static double normalizeAngle(double a) { - while (a > Math.PI) a -= 2 * Math.PI; - while (a < -Math.PI) a += 2 * Math.PI; - return a; - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayOpRob.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayOpRob.java new file mode 100644 index 0000000..315270d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayOpRob.java @@ -0,0 +1,567 @@ +package org.firstinspires.ftc.teamcode.kronbot.autonomous; + +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.INTAKE_REVERSE; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; +import com.pedropathing.geometry.Pose; + +import org.firstinspires.ftc.teamcode.kronbot.Robot; + +import java.io.BufferedReader; +import java.io.File; +import java.io.FileReader; +import java.io.IOException; +import java.util.ArrayList; +import java.util.List; + +/** + * Replay Autonomous — V3.3 + * + * Reads the V3.3 CSV format produced by DataRecordingOp (V3.3): + * Time, LR, RR, LF, RF, X, Y, Heading, Voltage, + * IntakeVel, LoaderVel, LeftShtrVel, RightShtrVel, + * TurretPos, AnglePos, FlapPos, + * IntakeCmd, LoaderCmd, FlapOpen, ShootRange, TurretOffset, BlueTarget, + * AutoAim + * + * V3.3 changes over V3.2: + * - INTAKE_REVERSE is now applied during replay (V3.2 missed it; the + * intake direction defaulted to whatever the Robot class had at init, + * which is almost certainly wrong for matching TeleOp behavior). + * - activateRange() is now driven by the recorded "LastActivateRange" + * column directly (V3.2 reverse-engineered it from activeConfig.velocity, + * which is brittle and breaks for the auto-aim interpolated range). + * - autoAimEnabled is now a recorded column (column 23) and is replayed + * frame-by-frame, matching the TeleOp's dpadUp toggle. + * - applyInitialMechanismState now fires activateRange() even when the + * first frame has LastActivateRange == 0 (auto-aim). Previously the + * guard `shootRange >= 0` correctly handled 0, but the activateRange(0) + * code path in Shoot is also gated on autoAimEnabled being true, so + * we now also set autoAimEnabled before calling it. + * - Deactivate transitions (LastActivateRange: any positive → -1) are + * now replayed by calling robot.shoot.deactivate(), matching the + * TeleOp's leftBumper.justPressed() → deactivate() edge. + * - Camera stop is NOT called on exit (matches the recorder's stop(), + * which doesn't call robot.webcam.stop() either — the original TeleOp + * does, but the recorder is a copy without webcam init). + * + * Position-accuracy structural fixes (carried from V3): + * 1. MAX_POWER 0.8 → 1.0. + * 2. Velocity-transition ramp applies to TRANSLATION ONLY. + * 3. Lookahead scales with time scaling. + * 4. Settling period (200 ms) after setPose. + * 5. CSV loader requires ≥16 columns (warns and skips if fewer). + * For full mechanism parity, the CSV needs 23 columns (V3.3 format); + * with only 16 columns the high-level commands default to safe + * values and only servo positions + velocities are replayed. + * + * Battery handling: time scaling ONLY. Motor commands are not power-scaled + * because the cap is 1.0 and a weaker battery cannot produce more torque. + */ +@Autonomous(name = "Replay Robert", group = "Autonomous") +public class ReplayOpRob extends LinearOpMode { + + private static final String CSV_PATH = "/sdcard/robot_data_Robert.csv"; + + // PD gains — V3 values + private static final double MAX_POWER = 1.0; + private static final double kP_translation = 0.08; + private static final double kD_translation = 0.02; + private static final double kP_rotation = 1.5; + private static final double kD_rotation = 0.1; + + // ROBOT-time lookahead. Scales with timeScalingFactor so the effective + // robot-time offset stays constant at LOOKAHEAD_TIME regardless of battery. + private static final double LOOKAHEAD_TIME = 0.15; + + // Derivative smoothing — V3 values + private static final double D_FILTER_ALPHA = 0.5; + private static final double MAX_D_CONTRIBUTION = 0.15; + + // Velocity-transition ramp — applies to translation only + private static final double RAMP_UP_TIME = 0.3; + private static final double TARGET_STILL_THRESH = 5.0; + + // Battery compensation (time scaling only) + private static final double NOMINAL_VOLTAGE = 12.0; + private static final double TIME_SCALING_FACTOR = 0.7; + + // Voltage sensor refresh + private static final double VOLTAGE_REFRESH_SEC = 0.1; + + // Settling period after setPose, before playback begins + private static final long SETTLE_NANOS = 200_000_000L; + + private final Robot robot = Robot.getInstance(); + private final ElapsedTime runtime = new ElapsedTime(); + private final List recordedFrames = new ArrayList<>(); + + // PD state + private double prevErrorX = 0; + private double prevErrorY = 0; + private double prevErrorHeading = 0; + private double prevTime = 0; + private double filteredDx = 0; + private double filteredDy = 0; + private double filteredDh = 0; + + // Ramp state + private double prevTargetX = 0; + private double prevTargetY = 0; + private boolean targetWasStill = true; + private double motionStartTime = 0; + + // Voltage caching + private double cachedVoltage = NOMINAL_VOLTAGE; + private double lastVoltageReadTime = -999; + + // Battery compensation result + private double recordedVoltage = NOMINAL_VOLTAGE; + private double timeScalingFactor = 1.0; + + // Mechanism state from the previous frame — used to detect "shoot range + // was just activated" and call robot.shoot.activateRange() only on + // transitions, matching the TeleOp's edge-triggered behavior. + private int prevShootRange = -2; // sentinel: never set + private boolean prevAutoAim = false; + + // ------------------------------------------------------------------------- + // Data model — V3.3 CSV (16, 22, or 23 columns) + // ------------------------------------------------------------------------- + private static class RobotFrame { + double timestamp; + double lrPow, rrPow, lfPow, rfPow; + double x, y, heading; + double voltage; + double intakeVel, loaderVel, leftShtrVel, rightShtrVel; + double turretPos, anglePos, flapPos; + // V3.2 high-level mechanism commands — default to safe values + // so a V3 (16-column) CSV still loads. + double intakeCmd = 0; + double loaderCmd = 0; + boolean flapOpen = false; + int shootRange = -2; // -2 = never activated, -1 = deactivated, 0..4 = last call + double turretOffset = 0; + boolean blueTarget = false; + // V3.3 + boolean autoAim = false; + + RobotFrame(String[] d) { + timestamp = Double.parseDouble(d[0]); + lrPow = Double.parseDouble(d[1]); + rrPow = Double.parseDouble(d[2]); + lfPow = Double.parseDouble(d[3]); + rfPow = Double.parseDouble(d[4]); + x = Double.parseDouble(d[5]); + y = Double.parseDouble(d[6]); + heading = Double.parseDouble(d[7]); + voltage = Double.parseDouble(d[8]); + intakeVel = Double.parseDouble(d[9]); + loaderVel = Double.parseDouble(d[10]); + leftShtrVel = Double.parseDouble(d[11]); + rightShtrVel = Double.parseDouble(d[12]); + turretPos = Double.parseDouble(d[13]); + anglePos = Double.parseDouble(d[14]); + flapPos = Double.parseDouble(d[15]); + // V3.2 columns (optional — defaults used if absent) + if (d.length >= 22) { + intakeCmd = Double.parseDouble(d[16]); + loaderCmd = Double.parseDouble(d[17]); + flapOpen = d[18].equals("1") || d[18].equalsIgnoreCase("true"); + shootRange = (int) Math.round(Double.parseDouble(d[19])); + turretOffset = Double.parseDouble(d[20]); + blueTarget = d[21].equals("1") || d[21].equalsIgnoreCase("true"); + } + // V3.3 column (optional — default false) + if (d.length >= 23) { + autoAim = d[22].equals("1") || d[22].equalsIgnoreCase("true"); + } + } + } + + // ------------------------------------------------------------------------- + // OpMode + // ------------------------------------------------------------------------- + @Override + public void runOpMode() { + telemetry.addLine("Initializing Replay Auto V3.3..."); + telemetry.update(); + + robot.initFollower(hardwareMap, true); + robot.init(hardwareMap); + + try { + robot.follower.getPoseTracker().resetIMU(); + } catch (InterruptedException e) { + telemetry.addLine("IMU Reset Interrupted"); + } + + try { + loadRecordedData(); + + if (recordedFrames.isEmpty()) { + telemetry.addLine("ERROR: No recorded data found!"); + telemetry.update(); + return; + } + + calculateRecordedVoltage(); + + RobotFrame first = recordedFrames.get(0); + robot.follower.setPose(new Pose(first.x, first.y, first.heading)); + + // Apply the recorded initial mechanism state so the first frame + // of replay starts from the same configuration the driver had + // (turret offset, blue target, flap, intake reverse, auto-aim, + // shooter range, etc.) + applyInitialMechanismState(first); + + telemetry.addLine("Ready."); + telemetry.addData("Frames", recordedFrames.size()); + telemetry.addData("Duration", "%.2f s", + recordedFrames.get(recordedFrames.size() - 1).timestamp - first.timestamp); + telemetry.addData("Recorded voltage", "%.1f V", recordedVoltage); + telemetry.update(); + + waitForStart(); + if (isStopRequested()) return; + + // Settle: let IMU/localizer stabilize on the initial pose + long settleEnd = System.nanoTime() + SETTLE_NANOS; + while (opModeIsActive() && System.nanoTime() < settleEnd) { + robot.follower.update(); + idle(); + } + robot.follower.setPose(new Pose(first.x, first.y, first.heading)); + + executePlayback(); + + robot.follower.setTeleOpDrive(0, 0, 0, true); + robot.follower.update(); + stopMechanisms(); + + } catch (Exception e) { + telemetry.addData("ERROR", e.toString()); + telemetry.update(); + robot.follower.setTeleOpDrive(0, 0, 0, true); + robot.follower.update(); + stopMechanisms(); + sleep(3000); + } + } + + // ------------------------------------------------------------------------- + // Main loop + // ------------------------------------------------------------------------- + private void executePlayback() { + runtime.reset(); + + double startTs = recordedFrames.get(0).timestamp; + double endTs = recordedFrames.get(recordedFrames.size() - 1).timestamp; + double duration = endTs - startTs; + + int idx = 0; + prevTime = 0; + prevErrorX = 0; + prevErrorY = 0; + prevErrorHeading = 0; + filteredDx = 0; + filteredDy = 0; + filteredDh = 0; + prevShootRange = -2; // sentinel: no transitions fire on frame 0 + prevAutoAim = false; + + RobotFrame firstFrame = recordedFrames.get(0); + prevTargetX = firstFrame.x; + prevTargetY = firstFrame.y; + targetWasStill = true; + motionStartTime = 0; + + robot.follower.startTeleopDrive(); + + while (opModeIsActive() && idx < recordedFrames.size() - 1) { + double now = runtime.seconds(); + + refreshVoltage(now); + updateTimeScaling(); + + // Constant ROBOT-time lookahead + double recordingTime = (now / timeScalingFactor) + LOOKAHEAD_TIME * timeScalingFactor; + double targetTs = startTs + recordingTime; + + while (idx < recordedFrames.size() - 1 + && recordedFrames.get(idx + 1).timestamp <= targetTs) { + idx++; + } + + RobotFrame fA = recordedFrames.get(idx); + RobotFrame fB = (idx + 1 < recordedFrames.size()) + ? recordedFrames.get(idx + 1) : fA; + + double t = 0; + if (fB.timestamp > fA.timestamp) { + t = (targetTs - fA.timestamp) / (fB.timestamp - fA.timestamp); + t = Range.clip(t, 0.0, 1.0); + } + + double targetX = lerp(fA.x, fB.x, t); + double targetY = lerp(fA.y, fB.y, t); + double targetH = lerpAngle(fA.heading, fB.heading, t); + + // Update localizer + robot.follower.update(); + Pose cur = robot.follower.getPose(); + + // --- PD (no feedforward) --- + double dt = now - prevTime; + if (dt <= 0) dt = 1e-6; + + double ex = targetX - cur.getX(); + double ey = targetY - cur.getY(); + + double rawDx = (ex - prevErrorX) / dt; + double rawDy = (ey - prevErrorY) / dt; + filteredDx = filteredDx + D_FILTER_ALPHA * (rawDx - filteredDx); + filteredDy = filteredDy + D_FILTER_ALPHA * (rawDy - filteredDy); + + double dxClamped = Range.clip(filteredDx * kD_translation, -MAX_D_CONTRIBUTION, MAX_D_CONTRIBUTION); + double dyClamped = Range.clip(filteredDy * kD_translation, -MAX_D_CONTRIBUTION, MAX_D_CONTRIBUTION); + + double fx = ex * kP_translation + dxClamped; + double fy = ey * kP_translation + dyClamped; + + double cosH = Math.cos(cur.getHeading()); + double sinH = Math.sin(cur.getHeading()); + double fwdCmd = cosH * fx + sinH * fy; + double strCmd = -sinH * fx + cosH * fy; + + double norm = Math.hypot(fwdCmd, strCmd); + if (norm > MAX_POWER) { + fwdCmd *= MAX_POWER / norm; + strCmd *= MAX_POWER / norm; + } + + double eh = normalizeAngle(targetH - cur.getHeading()); + double rawDh = (eh - prevErrorHeading) / dt; + filteredDh = filteredDh + D_FILTER_ALPHA * (rawDh - filteredDh); + double dhClamped = Range.clip(filteredDh * kD_rotation, -MAX_D_CONTRIBUTION, MAX_D_CONTRIBUTION); + double turnCmd = Range.clip( + eh * kP_rotation + dhClamped, + -MAX_POWER, MAX_POWER + ); + + // --- Velocity-transition ramp --- + double targetDist = Math.hypot(targetX - prevTargetX, targetY - prevTargetY); + double targetSpeed = targetDist / dt; + boolean targetIsStill = targetSpeed < TARGET_STILL_THRESH; + + if (targetWasStill && !targetIsStill) { + motionStartTime = now; + filteredDx = 0; + filteredDy = 0; + filteredDh = 0; + } + targetWasStill = targetIsStill; + prevTargetX = targetX; + prevTargetY = targetY; + + double timeSinceMotionStart = now - motionStartTime; + double ramp = Range.clip(timeSinceMotionStart / RAMP_UP_TIME, 0.0, 1.0); + + fwdCmd *= ramp; + strCmd *= ramp; + + // Update PD state + prevErrorX = ex; + prevErrorY = ey; + prevErrorHeading = eh; + prevTime = now; + + robot.follower.setTeleOpDrive(fwdCmd, strCmd, turnCmd, false); + + // --- Apply recorded mechanism commands the same way the TeleOp does --- + applyMechanismCommands(fA, fB, t); + robot.updateAllSystems(); + + // Telemetry + telemetry.addData("Time", "%.2f / %.2f s (scale %.2f)", now, duration, timeScalingFactor); + telemetry.addData("Frame", "%d / %d (t=%.2f)", idx, recordedFrames.size(), t); + telemetry.addData("PosErr", "%.2f cm", Math.hypot(ex, ey)); + telemetry.addData("HeadErr", "%.1f °", Math.toDegrees(Math.abs(eh))); + telemetry.addData("Cmd", "fwd=%.2f str=%.2f turn=%.2f (ramp %.2f)", fwdCmd, strCmd, turnCmd, ramp); + telemetry.addData("Voltage", "%.1f V (rec %.1f V)", cachedVoltage, recordedVoltage); + telemetry.addData("Mech", "rng=%d aa=%s flap=%s intk=%.2f load=%.2f", + (int) Math.round(lerp(fA.shootRange, fB.shootRange, t)), + (t < 0.5 ? fA.autoAim : fB.autoAim) ? "Y" : "N", + lerpBool(fA.flapOpen, fB.flapOpen, t) ? "Y" : "N", + lerp(fA.intakeCmd, fB.intakeCmd, t), + lerp(fA.loaderCmd, fB.loaderCmd, t)); + telemetry.update(); + + idle(); + } + } + + // ------------------------------------------------------------------------- + // Mechanism application — same code path as MainDrivingOp + // ------------------------------------------------------------------------- + private void applyInitialMechanismState(RobotFrame f) { + // Set the persistent state from the first recorded frame. + // Order matters: set intake.reversed BEFORE intake.speed, so the + // first updateAllSystems() call applies the correct direction. + robot.intake.reversed = INTAKE_REVERSE; + robot.turret.driverOffset = f.turretOffset; + robot.Blue_Target = f.blueTarget; + robot.flap.open = f.flapOpen; + robot.intake.speed = f.intakeCmd; + robot.loader.speed = f.loaderCmd; + + // Replay the initial auto-aim flag. The TeleOp sets + // autoAimEnabled via dpadUp.justPressed() — we mirror the result + // of that toggle here, not the action. The replay's mechanism + // loop keeps it in sync thereafter. + prevAutoAim = f.autoAim; + + // Fire the initial shoot range. Use -2 as a "never set" sentinel + // so we only call activateRange() / deactivate() if the first + // frame actually contains a real range value (>= 0 or == -1). + if (f.shootRange >= 0) { + // activateRange(0) requires autoAimEnabled to be true (per + // Shoot.activateRange). Set the flag before calling it so + // the interpolated velocity path actually engages. + if (f.shootRange == 0 && f.autoAim) { + robot.shoot.activateRange(0); + } else if (f.shootRange > 0) { + robot.shoot.activateRange(f.shootRange); + } + prevShootRange = f.shootRange; + } else if (f.shootRange == -1) { + robot.shoot.deactivate(); + prevShootRange = -1; + } + // -2 means "never activated" — leave outtake alone. + } + + private void applyMechanismCommands(RobotFrame a, RobotFrame b, double t) { + // Interpolate the high-level commands and apply them to the Robot + // exactly the way MainDrivingOp applies them — then call + // updateAllSystems() so the same internal control loop runs. + robot.intake.speed = lerp(a.intakeCmd, b.intakeCmd, t); + robot.loader.speed = lerp(a.loaderCmd, b.loaderCmd, t); + robot.flap.open = lerpBool(a.flapOpen, b.flapOpen, t); + robot.turret.driverOffset = lerp(a.turretOffset, b.turretOffset, t); + robot.Blue_Target = t < 0.5 ? a.blueTarget : b.blueTarget; + + // autoAimEnabled: in the TeleOp this is toggled by dpadUp.justPressed(). + // The replay treats it as a recorded state and mirrors it (uses the + // later of the two frames to avoid chatter on the toggle frame). + boolean curAutoAim = t < 0.5 ? a.autoAim : b.autoAim; + // (We do not call robot.shoot.activateRange(0) here on auto-aim + // toggle, because the TeleOp only fires it inside the loop body + // when autoAimEnabled is true. The activateRange(0) call on + // every loop re-interpolates the velocity based on distance, so + // re-firing it from the mechanism applier would actually be + // MORE faithful to the TeleOp. We opt to re-fire it here, + // guarded by the autoAim flag.) + if (curAutoAim) { + robot.shoot.activateRange(0); + } + prevAutoAim = curAutoAim; + + // shoot.activateRange is edge-triggered in the TeleOp (only fires + // on a button press). Detect when the recorded range changes and + // call it on the transition, matching the TeleOp behavior. + // + // Special case: when the recorded range is 0 (auto-aim), the + // TeleOp's "if (autoAimEnabled) robot.shoot.activateRange(0);" + // line fires every loop, so re-firing per-frame is correct + // (handled above). We only need edge detection for ranges 1-4 + // and for the -1 → positive (or positive → -1) deactivate + // transitions. + int curRange = (int) Math.round(lerp(a.shootRange, b.shootRange, t)); + if (curRange == 0) { + // Already handled by the curAutoAim block above; do not + // re-fire here as an "edge" (it isn't an edge in the TeleOp). + } else if (curRange != prevShootRange) { + if (curRange > 0) { + robot.shoot.activateRange(curRange); + } else if (curRange == -1 && prevShootRange > 0) { + // positive → -1 transition: matches the TeleOp's + // leftBumper.justPressed() → robot.shoot.deactivate(). + robot.shoot.deactivate(); + } + } + prevShootRange = curRange; + } + + private void stopMechanisms() { + robot.intake.speed = 0; + robot.loader.speed = 0; + robot.shoot.deactivate(); + } + + // ------------------------------------------------------------------------- + // Helpers + // ------------------------------------------------------------------------- + private void loadRecordedData() throws IOException { + File f = new File(CSV_PATH); + if (!f.exists()) throw new IOException("CSV not found: " + CSV_PATH); + try (BufferedReader r = new BufferedReader(new FileReader(f))) { + r.readLine(); // skip header + String line; + int lineNum = 1; + while ((line = r.readLine()) != null) { + lineNum++; + String[] d = line.split(","); + if (d.length >= 16) { + recordedFrames.add(new RobotFrame(d)); + } else { + telemetry.addData("Skip line", "%d (cols=%d, need ≥16)", lineNum, d.length); + } + } + } + } + + private void calculateRecordedVoltage() { + double total = 0; int n = 0; + for (RobotFrame f : recordedFrames) { + if (f.voltage > 0) { total += f.voltage; n++; } + } + if (n > 0) recordedVoltage = total / n; + } + + private void refreshVoltage(double now) { + if (now - lastVoltageReadTime >= VOLTAGE_REFRESH_SEC) { + cachedVoltage = hardwareMap.voltageSensor.iterator().next().getVoltage(); + lastVoltageReadTime = now; + } + } + + private void updateTimeScaling() { + double ratio = cachedVoltage / recordedVoltage; + timeScalingFactor = ratio < 1.0 + ? Range.clip(1.0 + TIME_SCALING_FACTOR * (1.0 - ratio), 1.0, 2.0) + : 1.0; + } + + private static double lerp(double a, double b, double t) { + return a + (b - a) * t; + } + + private static boolean lerpBool(boolean a, boolean b, double t) { + return t < 0.5 ? a : b; + } + + private static double lerpAngle(double a, double b, double t) { + return normalizeAngle(a + normalizeAngle(b - a) * t); + } + + private static double normalizeAngle(double a) { + while (a > Math.PI) a -= 2 * Math.PI; + while (a < -Math.PI) a += 2 * Math.PI; + return a; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp2.java deleted file mode 100644 index b895b27..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp2.java +++ /dev/null @@ -1,326 +0,0 @@ -package org.firstinspires.ftc.teamcode.kronbot.manual; - -import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.INTAKE_DRIVER_POWER; -import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.INTAKE_DRIVER_REVERSE; -import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.INTAKE_REVERSE; -import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RedTowerCoords; - -import android.os.Environment; - -import com.acmerobotics.dashboard.FtcDashboard; -import com.qualcomm.robotcore.eventloop.opmode.OpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.util.ElapsedTime; - -import org.firstinspires.ftc.teamcode.kronbot.Robot; -import org.firstinspires.ftc.teamcode.kronbot.utils.Constants; -import org.firstinspires.ftc.teamcode.kronbot.utils.Controls; -import org.firstinspires.ftc.teamcode.kronbot.utils.components.TurretAligner; -import org.firstinspires.ftc.teamcode.kronbot.utils.misc.LpsCounter; - -import java.io.FileWriter; -import java.io.IOException; -import java.util.Locale; - -/** - * The main TeleOP program for the driving period of the game, with data recording. - * - * @version 1.0 - */ -@TeleOp(name = "Data Recorder", group = "Replay") -public class DataRecordingOp2 extends OpMode { - private final Robot robot = Robot.getInstance(); - private Controls drivingGP; - private Controls utilityGP; - - private TurretAligner turretAligner; - - private FtcDashboard dashboard; - - private boolean autoAimEnabled = false; - - ElapsedTime turretTimer = new ElapsedTime(); - - LpsCounter lpsCounter; - - boolean rumbled = false; - - // Data Recording Fields - private FileWriter dataRecorder; - private static final long RECORD_INTERVAL_MS = 20; - private long startTime; - private long lastRecordTime = 0; - - // Direct access to motors for recording since MainDrivingOp uses follower which hides them - private DcMotorEx leftFront, rightFront, leftRear, rightRear; - - @Override - public void init() { - lpsCounter = new LpsCounter(); - lpsCounter.getLoopTime(); - robot.initFollower(hardwareMap, true); - robot.init(hardwareMap); - - dashboard = FtcDashboard.getInstance(); - robot.webcam.init(hardwareMap, telemetry); - - if (robot.webcam.getVisionPortal() != null) { - dashboard.startCameraStream(robot.webcam.getVisionPortal(), 30); - } - - // Initialize the new coordinate aligner - turretAligner = new TurretAligner(robot); - turretAligner.setTarget(RedTowerCoords.x, RedTowerCoords.y); - - drivingGP = new Controls(gamepad1); - utilityGP = new Controls(gamepad2); - - try { - robot.follower.getPoseTracker().resetIMU(); - } catch (InterruptedException e) { - throw new RuntimeException(e); - } - - // Initialize motors for recording - leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); - rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); - leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); - rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); - - // Initialize Data Recorder - String filePath = Environment.getExternalStorageDirectory().getPath() + "/robot_data.csv"; - try { - dataRecorder = new FileWriter(filePath); - // Header updated to include mechanism data - dataRecorder.write("Time,LR,RR,LF,RF,X,Y,Heading,Voltage,IntakePwr,LoaderPwr,LeftShtrPwr,RightShtrPwr,TurretPos,AnglePos,FlapPos\n"); - } catch (IOException e) { - telemetry.addData("Error initializing recorder", e.getMessage()); - } - } - - @Override - public void init_loop() { - lpsCounter.getLoopTime(); - - telemetry.addLine("Initialization Ready (Recording Enabled)"); - telemetry.update(); - } - - @Override - public void start() { - robot.follower.startTeleopDrive(); - startTime = System.currentTimeMillis(); - } - - @Override - public void loop() { - long now = System.currentTimeMillis(); - - // Update Loops/s delta - lpsCounter.getLoopTime(); - - //Update controller inputs - drivingGP.update(); - utilityGP.update(); - - robot.follower.update(); - - //Intake - robot.intake.speed = utilityGP.rightStick.y; - robot.intake.reversed = INTAKE_REVERSE; - - //Aliniere - turretAligner.update(); - - //Loader - if (!drivingGP.rightBumper.pressed()) { - robot.loader.speed = utilityGP.leftStick.y; - robot.flap.open = false; - } else { - robot.loader.speed = drivingGP.rightTrigger - drivingGP.leftTrigger; - robot.flap.open = true; - if (robot.loader.speed > 0.1) - robot.intake.speed = INTAKE_DRIVER_POWER; - else if (robot.loader.speed < -0.2) - robot.intake.speed = INTAKE_DRIVER_REVERSE; - else - robot.intake.speed = 0; - } - - //AprilTagDetection tag = robot.webcam.getTowerTags(); - //autoAim.telemetry(telemetry, tag); - - //Turret/Angle aiming - if (autoAimEnabled) { - //To do -// robot.webcam.update(); - - //robot.turret.angle = autoAim.calculateServoPosition(tag); - - } else { - //Turret aiming - if (drivingGP.dpadLeft.pressed()) { - - //if button is pressed for longer, increase increment - if (turretTimer.seconds() == 0) { - turretTimer.reset(); - } - double increment = 0.03; - - if (turretTimer.seconds() > 1) { - increment = 0.07; - } - - if (turretTimer.seconds() > 1.5) { - increment = 0.1; - } - robot.turret.driverOffset += increment; - - } else if (drivingGP.dpadRight.pressed()) { - - double decrement = 0.03; - - if (turretTimer.seconds() == 0) { - turretTimer.reset(); - } - - if (turretTimer.seconds() > 1) { - decrement = 0.07; - } - - if (turretTimer.seconds() > 1.5) { - decrement = 0.1; - } - - robot.turret.driverOffset -= decrement; - } else { - turretTimer.reset(); - } - - //Angle aiming - if (drivingGP.dpadUp.pressed()) - robot.outtake.activeConfig.angle+= 0.01; - else if (drivingGP.dpadDown.pressed()) - robot.outtake.activeConfig.angle -= 0.01; - } - - //Shoot Close/Far - if (drivingGP.triangle.justPressed()) { - robot.turret.autoAimEnabled = false; - robot.shoot.activateRange(1); - } - if (drivingGP.square.justPressed()) { - robot.turret.autoAimEnabled = false; - robot.shoot.activateRange(2); - } - if (drivingGP.cross.justPressed()) { - robot.turret.autoAimEnabled = false; - robot.shoot.activateRange(3); - } - if (drivingGP.circle.justPressed()) { - robot.turret.autoAimEnabled = false; - robot.shoot.activateRange(4); - } - - if (robot.outtake.on && - robot.leftOuttake.getVelocity() >= robot.outtake.activeConfig.velocity - 30 && - robot.leftOuttake.getVelocity() <= robot.outtake.activeConfig.velocity + 90) { - gamepad1.rumble(1, 0, 150); - rumbled = true; - } - - if (!autoAimEnabled && drivingGP.leftBumper.justPressed()) { - robot.turret.autoAimEnabled = true; - if (robot.outtake.on) { - robot.shoot.deactivate(); - gamepad1.rumble(1, 1, 100); - rumbled = false; - } - } - - //Update robot systems status - robot.follower.setTeleOpDrive(-drivingGP.leftStick.y, -drivingGP.leftStick.x, -drivingGP.rightStick.x, true); - robot.updateAllSystems(); - - // Record Data - if (now - lastRecordTime >= RECORD_INTERVAL_MS) { - try { - recordData(); - } catch (IOException e) { - telemetry.addData("Recording Error", e.getMessage()); - } - lastRecordTime = now; - } - - _telemetry(); - //robot.webcam.update(); - } - - - @Override - public void stop() { - robot.webcam.stop(); - if (dataRecorder != null) { - try { - dataRecorder.flush(); - dataRecorder.close(); - } catch (IOException ignored) { - } - } - } - - public void _telemetry() { - telemetry.addData("LPS", "%.1f", 1 / lpsCounter.delta); - telemetry.addData("Recording", "ACTIVE"); - telemetry.addData("x", robot.follower.getPose().getX()); - telemetry.addData("y", robot.follower.getPose().getY()); - telemetry.addData("heading", robot.follower.getPose().getHeading()); - telemetry.addData("Heading", robot.follower.getHeading()); - telemetry.addData("shooter motor vel:", robot.leftOuttake.getVelocity()); - telemetry.addData("angle servo pos:", robot.turretServo.getPosition()); - telemetry.addData("turret angle:", robot.turret.angle); - robot.intake.telemetry(telemetry); - robot.loader.telemetry(telemetry); - robot.outtake.telemetry(telemetry); - robot.heading.telemetry(telemetry); - robot.turret.telemetry(telemetry); - drivingGP.telemetry(telemetry); - telemetry.update(); - } - - private void recordData() throws IOException { - double t = (System.currentTimeMillis() - startTime) / 1000.0; - // Get pose from follower (PedroPathing) - // com.pedropathing.geometry.Pose pose = robot.follower.getPose(); - // However accessing getPose() might require checking if follower is initialized, which it should be. - // Assuming robot.follower.getPose() returns the pose. - - double x = robot.follower.getPose().getX(); - double y = robot.follower.getPose().getY(); - double heading = robot.follower.getHeading(); // or getPose().getHeading() - - double voltage = hardwareMap.voltageSensor.iterator().next().getVoltage(); - - // Writing columns: Time, Powers(LR,RR,LF,RF), X, Y, Heading, Voltage, Mech Powers, Servo Pos - dataRecorder.write(String.format(Locale.US, - "%.3f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.2f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n", - t, - leftRear.getPower(), - rightRear.getPower(), - leftFront.getPower(), - rightFront.getPower(), - x, - y, - heading, - voltage, - robot.intakeMotor.getPower(), - robot.loaderMotor.getPower(), - robot.leftOuttake.getPower(), - robot.rightOuttake.getPower(), - robot.turretServo.getPosition(), - robot.angleServo.getPosition(), - robot.flapsServo.getPosition() - )); - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/FinalRecorderOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/FinalRecorderOp.java new file mode 100644 index 0000000..b478bd4 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/FinalRecorderOp.java @@ -0,0 +1,378 @@ +package org.firstinspires.ftc.teamcode.kronbot.manual; + +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.INTAKE_DRIVER_POWER; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.INTAKE_DRIVER_REVERSE; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.INTAKE_REVERSE; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RedTowerCoords; + +import android.os.Environment; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.teamcode.kronbot.Robot; +import org.firstinspires.ftc.teamcode.kronbot.utils.Controls; +import org.firstinspires.ftc.teamcode.kronbot.utils.components.TurretAligner; +import org.firstinspires.ftc.teamcode.kronbot.utils.misc.LpsCounter; + +import java.io.FileWriter; +import java.io.IOException; +import java.util.Locale; + +/** + * TeleOp data recorder (V3.4). + * + * This OpMode is a faithful copy of MainDrivingOp with data-recording added. + * Every mechanism control line is identical to MainDrivingOp so the driver + * experiences exactly the same robot behavior while recording. + * + * CSV columns (27 total): + * Time, LR, RR, LF, RF, X, Y, Heading, Voltage, + * IntakeVel, LoaderVel, LeftShtrVel, RightShtrVel, + * TurretPos, AnglePos, FlapPos, + * IntakeCmd, LoaderCmd, FlapOpen, ShootRange, TurretOffset, BlueTarget, + * AutoAimEnabled, DriveFwd, DriveStr, DriveTurn, OuttakeKs + * + * V3.3 changes over V3.2: + * - Removed resetIMU() from init() to match MainDrivingOp exactly. The + * recorded path now starts from the same IMU state the driver had at + * init, not from a freshly-reset one. The replay still does setPose() + * with the recorded X/Y/heading, so localizer drift is reset on replay. + * - Added AutoAimEnabled column. Previously auto-aim was only inferable + * from the brittle "velocity matches RANGE_X_VELOCITY" check, which + * breaks for the interpolated auto-aim range (activateRange(0)). The + * recorder now mirrors MainDrivingOp's dpadUp toggle explicitly and + * records the boolean. + * - LastActivateRange tracking: the recorder now wraps each + * robot.shoot.activateRange(N) call to remember the last discrete N + * that was passed in. The replay uses this to fire activateRange() + * with the exact same arguments the driver used, instead of trying + * to reverse-engineer the range from activeConfig.velocity. + * + * V3.4 changes over V3.3: + * - Records the exact drive commands passed to Pedro's setTeleOpDrive(). + * Replay can now use driver feedforward plus odometry correction instead + * of relying only on pose PD or trying to infer intent from wheel powers. + * - Records auto-aim as ShootRange 0 and stores the active shooter kS, so + * replay can keep the recorded interpolated shooter velocity stable. + * + * @version 3.4 + */ +@TeleOp(name = "Recorder Vlad", group = "Replay") +public class FinalRecorderOp extends OpMode { + private final Robot robot = Robot.getInstance(); + private Controls drivingGP; + private Controls utilityGP; + + private TurretAligner turretAligner; + + private FtcDashboard dashboard; + + private boolean autoAimEnabled = false; + + ElapsedTime turretTimer = new ElapsedTime(); + + LpsCounter lpsCounter; + + boolean rumbled = false; + + // Data Recording Fields + private FileWriter dataRecorder; + private static final double RECORD_INTERVAL_SEC = 0.020; // 20ms in seconds + private ElapsedTime recordTimer = new ElapsedTime(); + private double lastRecordTime = 0; + + // Direct access to drive motors for recording (follower hides them) + private DcMotorEx leftFront, rightFront, leftRear, rightRear; + + // Last discrete range passed to robot.shoot.activateRange(N). -2 means + // "never called this session" (used by replay to avoid an initial-state + // fire if no range was ever activated). -1 means deactivate was called + // or outtake was never on. + private int lastActivateRange = -2; + + @Override + public void init() { + lpsCounter = new LpsCounter(); + lpsCounter.getLoopTime(); + robot.initFollower(hardwareMap, true); + robot.init(hardwareMap); + + dashboard = FtcDashboard.getInstance(); + // Webcam init intentionally NOT done here — matches MainDrivingOp, + // which has it commented out. Driver doesn't see camera stream + // during normal play, so we don't show it during recording either. + + // Initialize the new coordinate aligner + turretAligner = new TurretAligner(robot); + turretAligner.setTarget(RedTowerCoords.x, RedTowerCoords.y); + + drivingGP = new Controls(gamepad1); + utilityGP = new Controls(gamepad2); + + // Initialize drive motors for recording + leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); + rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); + rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); + + // NOTE: MainDrivingOp does NOT call resetIMU() in init(). The + // recorder used to (V3.2), which made the recorded path start from + // a freshly-zeroed IMU while the actual TeleOp didn't. Removed + // for parity. The replay still does its own setPose() at the + // first recorded frame, so this is fine. + + // Initialize Data Recorder — V3.4 header (27 columns) + String filePath = Environment.getExternalStorageDirectory().getPath() + "/robot_data_Vlad.csv"; + try { + dataRecorder = new FileWriter(filePath); + dataRecorder.write("Time,LR,RR,LF,RF,X,Y,Heading,Voltage,IntakeVel,LoaderVel,LeftShtrVel,RightShtrVel,TurretPos,AnglePos,FlapPos,IntakeCmd,LoaderCmd,FlapOpen,ShootRange,TurretOffset,BlueTarget,AutoAim,DriveFwd,DriveStr,DriveTurn,OuttakeKs\n"); + } catch (IOException e) { + telemetry.addData("Error initializing recorder", e.getMessage()); + } + } + + @Override + public void init_loop() { + lpsCounter.getLoopTime(); + + telemetry.addLine("Initialization Ready (Recording V3.4 Enabled)"); + telemetry.update(); + } + + @Override + public void start() { + robot.follower.startTeleopDrive(); + recordTimer.reset(); + } + + @Override + public void loop() { + double now = recordTimer.seconds(); + + lpsCounter.getLoopTime(); + + drivingGP.update(); + utilityGP.update(); + + robot.follower.update(); + + double driveFwd = -drivingGP.leftStick.y; + double driveStr = -drivingGP.leftStick.x; + double driveTurn = -drivingGP.rightStick.x; + + // ----- Everything below this point is identical to MainDrivingOp ----- + + //Intake + robot.intake.speed = utilityGP.rightStick.y; + robot.intake.reversed = INTAKE_REVERSE; + + //Loader + if (!drivingGP.rightBumper.pressed()) { + robot.loader.speed = utilityGP.leftStick.y; + robot.flap.open = false; + } else { + robot.loader.speed = (drivingGP.rightTrigger - drivingGP.leftTrigger) * 0.8; + robot.flap.open = true; + if (robot.loader.speed > 0.1) + robot.intake.speed = INTAKE_DRIVER_POWER; + else if (robot.loader.speed < -0.2) + robot.intake.speed = INTAKE_DRIVER_REVERSE; + else + robot.intake.speed = 0; + } + + //Turret aiming + if (drivingGP.dpadLeft.pressed()) { + if (turretTimer.seconds() == 0) { + turretTimer.reset(); + } + double increment = 0.03; + if (turretTimer.seconds() > 1) { + increment = 0.07; + } + if (turretTimer.seconds() > 1.5) { + increment = 0.1; + } + robot.turret.driverOffset += increment; + + } else if (drivingGP.dpadRight.pressed()) { + double decrement = 0.03; + if (turretTimer.seconds() == 0) { + turretTimer.reset(); + } + if (turretTimer.seconds() > 1) { + decrement = 0.07; + } + if (turretTimer.seconds() > 1.5) { + decrement = 0.1; + } + robot.turret.driverOffset -= decrement; + } else { + turretTimer.reset(); + } + + if(drivingGP.dpadDown.justPressed()) + robot.turret.autoAimEnabled = !robot.turret.autoAimEnabled; + + if(drivingGP.dpadUp.justPressed()) + autoAimEnabled=!autoAimEnabled; + + if(autoAimEnabled) { + robot.shoot.activateRange(0); + lastActivateRange = 0; + } + //Shoot Close/Far + if (drivingGP.triangle.justPressed()) { + robot.shoot.activateRange(1); + lastActivateRange = 1; + } + if (drivingGP.square.justPressed()) { + robot.shoot.activateRange(2); + lastActivateRange = 2; + } + if (drivingGP.cross.justPressed()) { + robot.shoot.activateRange(3); + lastActivateRange = 3; + } + if (drivingGP.circle.justPressed()) { + robot.shoot.activateRange(4); + lastActivateRange = 4; + } + + if (robot.outtake.on && + robot.leftOuttake.getVelocity() >= robot.outtake.activeConfig.velocity - 30 && + robot.leftOuttake.getVelocity() <= robot.outtake.activeConfig.velocity + 90) { + gamepad1.rumble(1, 0, 150); + rumbled = true; + } + + if (!autoAimEnabled && drivingGP.leftBumper.justPressed()) { + robot.turret.autoAimEnabled = true; + if (robot.outtake.on) { + robot.shoot.deactivate(); + lastActivateRange = -1; // explicit: deactivated + gamepad1.rumble(1, 1, 100); + rumbled = false; + } + } + + if(drivingGP.rightStick.button.justPressed()) + robot.Blue_Target = !robot.Blue_Target; + + //Update robot systems status + robot.follower.setTeleOpDrive(driveFwd, driveStr, driveTurn, true); + robot.updateAllSystems(); + + // ----- End of MainDrivingOp-identical block ----- + + // Record Data + if (now - lastRecordTime >= RECORD_INTERVAL_SEC) { + try { + recordData(now, driveFwd, driveStr, driveTurn); + } catch (IOException e) { + telemetry.addData("Recording Error", e.getMessage()); + } + lastRecordTime = now; + } + + _telemetry(); + } + + @Override + public void stop() { + if (dataRecorder != null) { + try { + dataRecorder.flush(); + dataRecorder.close(); + } catch (IOException ignored) { + } + } + } + + public void _telemetry() { + telemetry.addData("LPS", "%.1f", 1 / lpsCounter.delta); + telemetry.addData("Recording V3.4", "ACTIVE"); + telemetry.addData("x", robot.follower.getPose().getX()); + telemetry.addData("y", robot.follower.getPose().getY()); + telemetry.addData("heading", robot.follower.getPose().getHeading()); + telemetry.addData("Heading", robot.follower.getHeading()); + telemetry.addData("Drive Powers", "LF:%.2f RF:%.2f LR:%.2f RR:%.2f", + leftFront.getPower(), rightFront.getPower(), leftRear.getPower(), rightRear.getPower()); + telemetry.addData("shooter motor vel:", robot.leftOuttake.getVelocity()); + telemetry.addData("angle servo pos:", robot.turretServo.getPosition()); + telemetry.addData("turret angle:", robot.turret.angle); + robot.intake.telemetry(telemetry); + robot.loader.telemetry(telemetry); + robot.outtake.telemetry(telemetry); + robot.heading.telemetry(telemetry); + robot.turret.telemetry(telemetry); + drivingGP.telemetry(telemetry); + telemetry.update(); + } + + private void recordData(double t, double driveFwd, double driveStr, double driveTurn) throws IOException { + + double x = robot.follower.getPose().getX(); + double y = robot.follower.getPose().getY(); + double heading = robot.follower.getHeading(); + + double voltage = hardwareMap.voltageSensor.iterator().next().getVoltage(); + + // Mechanism states (V3 columns) + double intakeVel = robot.intakeMotor.getVelocity(); + double loaderVel = robot.loaderMotor.getVelocity(); + double leftShtrVel = robot.leftOuttake.getVelocity(); + double rightShtrVel = robot.rightOuttake.getVelocity(); + double turretPos = robot.turretServo.getPosition(); + double anglePos = robot.angleServo.getPosition(); + double flapPos = robot.flapsServo.getPosition(); + + // High-level mechanism commands (V3.3 columns) — what the TeleOp + // applied to robot.intake / robot.loader / robot.shoot / etc. + // The replay uses these to call the same high-level API instead + // of setting motor powers directly. + double intakeCmd = robot.intake.speed; + double loaderCmd = robot.loader.speed; + int flapOpen = robot.flap.open ? 1 : 0; + int shootRange = lastActivateRange; // -2, -1, 0, 1, 2, 3, 4 + double turretOffset = robot.turret.driverOffset; + int blueTarget = robot.Blue_Target ? 1 : 0; + int autoAim = autoAimEnabled ? 1 : 0; + double outtakeKs = robot.outtake.activeConfig.kS; + + dataRecorder.write(String.format(Locale.US, + "%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.2f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%d,%d,%.4f,%d,%d,%.4f,%.4f,%.4f,%.4f\n", + t, + leftRear.getPower(), + rightRear.getPower(), + leftFront.getPower(), + rightFront.getPower(), + x, + y, + heading, + voltage, + intakeVel, + loaderVel, + leftShtrVel, + rightShtrVel, + turretPos, + anglePos, + flapPos, + intakeCmd, + loaderCmd, + flapOpen, + shootRange, + turretOffset, + blueTarget, + autoAim, + driveFwd, + driveStr, + driveTurn, + outtakeKs + )); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/RecorderOpRob.java similarity index 64% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp3.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/RecorderOpRob.java index d5bee1a..1ee9b4a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/RecorderOpRob.java @@ -23,24 +23,39 @@ import java.util.Locale; /** - * Enhanced TeleOP data recorder (V3). - * Records drive motor powers (for feedforward replay) and mechanism velocities - * (for faithful shooter reproduction). + * TeleOp data recorder (V3.3). * - * CSV columns (17 total): - * Time, LR, RR, LF, RF, X, Y, Heading, Voltage, - * IntakeVel, LoaderVel, LeftShtrVel, RightShtrVel, - * TurretPos, AnglePos, FlapPos + * This OpMode is a faithful copy of MainDrivingOp with data-recording added. + * Every mechanism control line is identical to MainDrivingOp so the driver + * experiences exactly the same robot behavior while recording. * - * Key differences from DataRecordingOp2: - * - Mechanism columns record velocities instead of raw powers (better for shooter replay) - * - Uses ElapsedTime (nanosecond resolution) for timestamps, matching the replay timer exactly - * - Otherwise identical format for drive motors (LR, RR, LF, RF powers) + * CSV columns (23 total): + * Time, LR, RR, LF, RF, X, Y, Heading, Voltage, + * IntakeVel, LoaderVel, LeftShtrVel, RightShtrVel, + * TurretPos, AnglePos, FlapPos, + * IntakeCmd, LoaderCmd, FlapOpen, ShootRange, TurretOffset, BlueTarget, + * AutoAimEnabled * - * @version 3.0 + * V3.3 changes over V3.2: + * - Removed resetIMU() from init() to match MainDrivingOp exactly. The + * recorded path now starts from the same IMU state the driver had at + * init, not from a freshly-reset one. The replay still does setPose() + * with the recorded X/Y/heading, so localizer drift is reset on replay. + * - Added AutoAimEnabled column. Previously auto-aim was only inferable + * from the brittle "velocity matches RANGE_X_VELOCITY" check, which + * breaks for the interpolated auto-aim range (activateRange(0)). The + * recorder now mirrors MainDrivingOp's dpadUp toggle explicitly and + * records the boolean. + * - LastActivateRange tracking: the recorder now wraps each + * robot.shoot.activateRange(N) call to remember the last discrete N + * that was passed in. The replay uses this to fire activateRange() + * with the exact same arguments the driver used, instead of trying + * to reverse-engineer the range from activeConfig.velocity. + * + * @version 3.3 */ -@TeleOp(name = "Data Recorder V3", group = "Replay") -public class DataRecordingOp3 extends OpMode { +@TeleOp(name = "Recorder Robert", group = "Replay") +public class RecorderOpRob extends OpMode { private final Robot robot = Robot.getInstance(); private Controls drivingGP; private Controls utilityGP; @@ -66,6 +81,12 @@ public class DataRecordingOp3 extends OpMode { // Direct access to drive motors for recording (follower hides them) private DcMotorEx leftFront, rightFront, leftRear, rightRear; + // Last discrete range passed to robot.shoot.activateRange(N). -2 means + // "never called this session" (used by replay to avoid an initial-state + // fire if no range was ever activated). -1 means deactivate was called + // or outtake was never on. + private int lastActivateRange = -2; + @Override public void init() { lpsCounter = new LpsCounter(); @@ -74,35 +95,34 @@ public void init() { robot.init(hardwareMap); dashboard = FtcDashboard.getInstance(); - robot.webcam.init(hardwareMap, telemetry); - - if (robot.webcam.getVisionPortal() != null) { - dashboard.startCameraStream(robot.webcam.getVisionPortal(), 30); - } + // Webcam init intentionally NOT done here — matches MainDrivingOp, + // which has it commented out. Driver doesn't see camera stream + // during normal play, so we don't show it during recording either. + // Initialize the new coordinate aligner turretAligner = new TurretAligner(robot); turretAligner.setTarget(RedTowerCoords.x, RedTowerCoords.y); drivingGP = new Controls(gamepad1); utilityGP = new Controls(gamepad2); - try { - robot.follower.getPoseTracker().resetIMU(); - } catch (InterruptedException e) { - throw new RuntimeException(e); - } - // Initialize drive motors for recording leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); - // Initialize Data Recorder - String filePath = Environment.getExternalStorageDirectory().getPath() + "/robot_data.csv"; + // NOTE: MainDrivingOp does NOT call resetIMU() in init(). The + // recorder used to (V3.2), which made the recorded path start from + // a freshly-zeroed IMU while the actual TeleOp didn't. Removed + // for parity. The replay still does its own setPose() at the + // first recorded frame, so this is fine. + + // Initialize Data Recorder — V3.3 header (23 columns) + String filePath = Environment.getExternalStorageDirectory().getPath() + "/robot_data_Robert.csv"; try { dataRecorder = new FileWriter(filePath); - dataRecorder.write("Time,LR,RR,LF,RF,X,Y,Heading,Voltage,IntakeVel,LoaderVel,LeftShtrVel,RightShtrVel,TurretPos,AnglePos,FlapPos\n"); + dataRecorder.write("Time,LR,RR,LF,RF,X,Y,Heading,Voltage,IntakeVel,LoaderVel,LeftShtrVel,RightShtrVel,TurretPos,AnglePos,FlapPos,IntakeCmd,LoaderCmd,FlapOpen,ShootRange,TurretOffset,BlueTarget,AutoAim\n"); } catch (IOException e) { telemetry.addData("Error initializing recorder", e.getMessage()); } @@ -112,7 +132,7 @@ public void init() { public void init_loop() { lpsCounter.getLoopTime(); - telemetry.addLine("Initialization Ready (Recording V3 Enabled)"); + telemetry.addLine("Initialization Ready (Recording V3.3 Enabled)"); telemetry.update(); } @@ -133,19 +153,18 @@ public void loop() { robot.follower.update(); - // Intake + // ----- Everything below this point is identical to MainDrivingOp ----- + + //Intake robot.intake.speed = utilityGP.rightStick.y; robot.intake.reversed = INTAKE_REVERSE; - // Alignment - turretAligner.update(); - - // Loader + //Loader if (!drivingGP.rightBumper.pressed()) { robot.loader.speed = utilityGP.leftStick.y; robot.flap.open = false; } else { - robot.loader.speed = (drivingGP.rightTrigger - drivingGP.leftTrigger) * 0.9; + robot.loader.speed = (drivingGP.rightTrigger - drivingGP.leftTrigger) * 0.8; robot.flap.open = true; if (robot.loader.speed > 0.1) robot.intake.speed = INTAKE_DRIVER_POWER; @@ -155,42 +174,31 @@ else if (robot.loader.speed < -0.2) robot.intake.speed = 0; } - // Turret/Angle aiming - - // Turret aiming + //Turret aiming if (drivingGP.dpadLeft.pressed()) { - - //if button is pressed for longer, increase increment if (turretTimer.seconds() == 0) { turretTimer.reset(); } double increment = 0.03; - if (turretTimer.seconds() > 1) { increment = 0.07; } - if (turretTimer.seconds() > 1.5) { increment = 0.1; } robot.turret.driverOffset += increment; } else if (drivingGP.dpadRight.pressed()) { - double decrement = 0.03; - if (turretTimer.seconds() == 0) { turretTimer.reset(); } - if (turretTimer.seconds() > 1) { decrement = 0.07; } - if (turretTimer.seconds() > 1.5) { decrement = 0.1; } - robot.turret.driverOffset -= decrement; } else { turretTimer.reset(); @@ -204,18 +212,22 @@ else if (robot.loader.speed < -0.2) if(autoAimEnabled) robot.shoot.activateRange(0); - // Shoot ranges + //Shoot Close/Far if (drivingGP.triangle.justPressed()) { robot.shoot.activateRange(1); + lastActivateRange = 1; } if (drivingGP.square.justPressed()) { robot.shoot.activateRange(2); + lastActivateRange = 2; } if (drivingGP.cross.justPressed()) { robot.shoot.activateRange(3); + lastActivateRange = 3; } if (drivingGP.circle.justPressed()) { robot.shoot.activateRange(4); + lastActivateRange = 4; } if (robot.outtake.on && @@ -229,16 +241,22 @@ else if (robot.loader.speed < -0.2) robot.turret.autoAimEnabled = true; if (robot.outtake.on) { robot.shoot.deactivate(); + lastActivateRange = -1; // explicit: deactivated gamepad1.rumble(1, 1, 100); rumbled = false; } } - // Update robot systems + if(drivingGP.rightStick.button.justPressed()) + robot.Blue_Target = !robot.Blue_Target; + + //Update robot systems status robot.follower.setTeleOpDrive(-drivingGP.leftStick.y, -drivingGP.leftStick.x, -drivingGP.rightStick.x, true); robot.updateAllSystems(); - // Record Data (Fix #5: uses same ElapsedTime as replay for consistent timestamps) + // ----- End of MainDrivingOp-identical block ----- + + // Record Data if (now - lastRecordTime >= RECORD_INTERVAL_SEC) { try { recordData(now); @@ -253,7 +271,6 @@ else if (robot.loader.speed < -0.2) @Override public void stop() { - robot.webcam.stop(); if (dataRecorder != null) { try { dataRecorder.flush(); @@ -265,7 +282,7 @@ public void stop() { public void _telemetry() { telemetry.addData("LPS", "%.1f", 1 / lpsCounter.delta); - telemetry.addData("Recording V3", "ACTIVE"); + telemetry.addData("Recording V3.3", "ACTIVE"); telemetry.addData("x", robot.follower.getPose().getX()); telemetry.addData("y", robot.follower.getPose().getY()); telemetry.addData("heading", robot.follower.getPose().getHeading()); @@ -292,15 +309,29 @@ private void recordData(double t) throws IOException { double voltage = hardwareMap.voltageSensor.iterator().next().getVoltage(); - // Record mechanism velocities instead of raw powers for better reproduction - double intakeVel = robot.intakeMotor.getVelocity(); - double loaderVel = robot.loaderMotor.getVelocity(); - double leftShtrVel = robot.leftOuttake.getVelocity(); + // Mechanism states (V3 columns) + double intakeVel = robot.intakeMotor.getVelocity(); + double loaderVel = robot.loaderMotor.getVelocity(); + double leftShtrVel = robot.leftOuttake.getVelocity(); double rightShtrVel = robot.rightOuttake.getVelocity(); + double turretPos = robot.turretServo.getPosition(); + double anglePos = robot.angleServo.getPosition(); + double flapPos = robot.flapsServo.getPosition(); + + // High-level mechanism commands (V3.3 columns) — what the TeleOp + // applied to robot.intake / robot.loader / robot.shoot / etc. + // The replay uses these to call the same high-level API instead + // of setting motor powers directly. + double intakeCmd = robot.intake.speed; + double loaderCmd = robot.loader.speed; + int flapOpen = robot.flap.open ? 1 : 0; + int shootRange = lastActivateRange; // -2, -1, 0, 1, 2, 3, 4 + double turretOffset = robot.turret.driverOffset; + int blueTarget = robot.Blue_Target ? 1 : 0; + int autoAim = autoAimEnabled ? 1 : 0; - // CSV: Time, LR, RR, LF, RF, X, Y, Heading, Voltage, IntakeVel, LoaderVel, LeftShtrVel, RightShtrVel, TurretPos, AnglePos, FlapPos dataRecorder.write(String.format(Locale.US, - "%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.2f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n", + "%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.2f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%d,%d,%.4f,%d,%d\n", t, leftRear.getPower(), rightRear.getPower(), @@ -314,9 +345,16 @@ private void recordData(double t) throws IOException { loaderVel, leftShtrVel, rightShtrVel, - robot.turretServo.getPosition(), - robot.angleServo.getPosition(), - robot.flapsServo.getPosition() + turretPos, + anglePos, + flapPos, + intakeCmd, + loaderCmd, + flapOpen, + shootRange, + turretOffset, + blueTarget, + autoAim )); } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java index fcdd2d0..3b0d2b2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/utils/Constants.java @@ -52,8 +52,8 @@ public class Constants { public static double ANGLE_SERVO_FAR = 0.72; public static double ANGLE_SERVO_MIN = 0; - public static double FLAP_CLOSED = 0.55; - public static double FLAP_OPEN = 0.9; + public static double FLAP_CLOSED = 0.35; + public static double FLAP_OPEN = 0.6; public static double INTAKE_DRIVER_POWER = 0.55; public static double INTAKE_DRIVER_REVERSE = -0.55;