From 3e4fe2bfa440010cc1fb980fe7d23822b328b9b6 Mon Sep 17 00:00:00 2001 From: Robi2903 <113847997+Robi2903@users.noreply.github.com> Date: Tue, 14 Jul 2026 19:16:34 +0300 Subject: [PATCH 1/6] CODE DUMP RAAAAH --- .../kronbot/autonomous/ReplayAutoOp4.java | 528 ++++++++++++++++ .../kronbot/autonomous/ReplayAutoOp7.java | 495 +++++++++++++++ .../kronbot/autonomous/ReplayAutoOp8.java | 576 ++++++++++++++++++ .../kronbot/manual/DataRecordingOp4.java | 319 ++++++++++ .../kronbot/manual/DataRecordingOp7.java | 283 +++++++++ .../kronbot/manual/DataRecordingOp8.java | 302 +++++++++ 6 files changed, 2503 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAutoOp4.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAutoOp7.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAutoOp8.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp4.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp7.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp8.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAutoOp4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAutoOp4.java new file mode 100644 index 0000000..957967b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAutoOp4.java @@ -0,0 +1,528 @@ +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.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.Collections; +import java.util.List; + +/** + * Replay Auto V5 — Corrected Trajectory Follower + * + * FIXES from V4: + * 1. Velocities are recorded in ROBOT FRAME during TeleOp — no rotation needed in replay + * 2. Time scaling ONLY (no double velocity scaling) + * 3. Lookahead + feedforward use same time base (target frame velocity) + * 4. Slew rate limiting on motor commands (acceleration limits) + * 5. Correction is rotated into robot frame before application + * 6. Both feedforward and correction are in robot frame throughout + * + * ARCHITECTURE: + * 1. Recorded (vxRobot, vyRobot, omega) are smoothed real-time robot-frame velocities + * 2. Auto-calibrate maxLinearVel / maxAngularVel from recording + * 3. Scale velocities to [-1, 1] power units BEFORE mecanum mixing + * 4. Blend: 75% feedforward + 25% small PD correction (both in robot frame) + * 5. Normalize wheel powers ONLY if saturated (preserves ratios) + * 6. Time scaling ONLY for battery compensation (no velocity scaling) + * 7. Slew rate limiter prevents command jumps + * 8. Lookahead for smooth tracking + * + * CSV Format (from DataRecordingOp5): + * Time,X,Y,Heading,Voltage,VxRobot,VyRobot,Omega,IntakePwr,LoaderPwr,LShooterPwr,RShooterPwr,TurretPos,AnglePos,FlapPos + */ +@Autonomous(name = "Replay Auto V4", group = "Replay") +public class ReplayAutoOp4 extends LinearOpMode { + + private static final String CSV_PATH = "/sdcard/robot_data_v4.csv"; + + // ========== TUNABLE CONSTANTS ========== + + /** Feedforward weight: how much we trust recorded velocity vs correction. + * 0.75 = 75% feedforward, 25% correction. Tune 0.70–0.85. */ + private static final double FF_WEIGHT = 0.75; + + /** PD gains — LOW because feedforward does the heavy lifting. */ + private static final double kP_TRANSLATION = 0.015; + private static final double kD_TRANSLATION = 0.008; + private static final double kP_ROTATION = 0.22; + private static final double kD_ROTATION = 0.05; + + /** Max power the correction term can add. */ + private static final double MAX_CORRECTION = 0.30; + + /** Lookahead: follow a point this many seconds ahead in the recording. */ + private static final double LOOKAHEAD_TIME = 0.08; + + /** Time scaling for battery compensation. + * Lower battery → stretch time so robot has more time to execute. + * NO velocity scaling — time scaling handles everything. */ + private static final double TIME_SCALE_FACTOR = 0.55; + private static final double MAX_TIME_SCALE = 1.6; + private static final double MIN_TIME_SCALE = 0.85; + + /** D-term low-pass filter. */ + private static final double D_FILTER_ALPHA = 0.45; + + /** Error recovery: reduce FF weight when drift exceeds threshold. */ + private static final double ERROR_RECOVERY_THRESH = 6.0; + private static final double ERROR_RECOVERY_FF_WEIGHT = 0.45; + + /** Slew rate limit: max change in robot-frame command per second. + * Prevents command jumps that cause wheel slip. */ + private static final double MAX_SLEW_RATE = 4.0; // per second + + /** Voltage sensor refresh rate. */ + private static final double VOLTAGE_REFRESH_SEC = 0.1; + + /** Fallback max velocities if auto-calibration fails. */ + private static final double FALLBACK_MAX_LINEAR_VEL = 72.0; + private static final double FALLBACK_MAX_ANGULAR_VEL = 2.8; + + // ========== MOTOR DIRECTIONS ========== + // COPY THESE EXACTLY FROM THE TELEMETRY OUTPUT OF DataRecordingOp5 + private static final DcMotorSimple.Direction LF_DIR = DcMotorSimple.Direction.REVERSE; + private static final DcMotorSimple.Direction RF_DIR = DcMotorSimple.Direction.REVERSE; + private static final DcMotorSimple.Direction LR_DIR = DcMotorSimple.Direction.REVERSE; + private static final DcMotorSimple.Direction RR_DIR = DcMotorSimple.Direction.FORWARD; + + // ========== STATE ========== + private final Robot robot = Robot.getInstance(); + private final ElapsedTime runtime = new ElapsedTime(); + private final List recordedFrames = new ArrayList<>(); + + private DcMotorEx leftFront, rightFront, leftRear, rightRear; + + // Auto-calibrated max velocities + private double maxLinearVel = FALLBACK_MAX_LINEAR_VEL; + private double maxAngularVel = FALLBACK_MAX_ANGULAR_VEL; + + // PD state (all in ROBOT FRAME) + private double prevErrorX = 0, prevErrorY = 0, prevErrorHeading = 0; + private double prevTime = 0; + private double filteredDx = 0, filteredDy = 0, filteredDh = 0; + + // Slew rate limiter state + private double prevRobotFwd = 0, prevRobotStr = 0, prevRobotTurn = 0; + + // Voltage + private double cachedVoltage = 12.0; + private double lastVoltageReadTime = -999; + private double recordedVoltage = 12.0; + private double timeScale = 1.0; + + // Telemetry stats + private double maxWheelPowerSeen = 0; + private int clipCount = 0; + + // ------------------------------------------------------------------------- + // DATA MODEL + // ------------------------------------------------------------------------- + private static class RobotFrame { + double timestamp; + double x, y, heading; + double voltage; + double vxRobot, vyRobot, omega; // ROBOT-FRAME velocities (already rotated!) + double intakePwr, loaderPwr, leftShtrPwr, rightShtrPwr; + double turretPos, anglePos, flapPos; + + RobotFrame(String[] d) { + timestamp = Double.parseDouble(d[0]); + x = Double.parseDouble(d[1]); + y = Double.parseDouble(d[2]); + heading = Double.parseDouble(d[3]); + voltage = Double.parseDouble(d[4]); + vxRobot = Double.parseDouble(d[5]); + vyRobot = Double.parseDouble(d[6]); + omega = Double.parseDouble(d[7]); + intakePwr = Double.parseDouble(d[8]); + loaderPwr = Double.parseDouble(d[9]); + leftShtrPwr = Double.parseDouble(d[10]); + rightShtrPwr= Double.parseDouble(d[11]); + turretPos = Double.parseDouble(d[12]); + anglePos = Double.parseDouble(d[13]); + flapPos = Double.parseDouble(d[14]); + } + } + + // ------------------------------------------------------------------------- + // MAIN + // ------------------------------------------------------------------------- + @Override + public void runOpMode() { + telemetry.addLine("Initializing Replay Auto V4..."); + telemetry.update(); + + robot.initFollower(hardwareMap, true); + robot.init(hardwareMap); + + try { + robot.follower.getPoseTracker().resetIMU(); + } catch (InterruptedException e) { + telemetry.addLine("IMU Reset Interrupted"); + } + + // Drive motors with verified 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(LF_DIR); + rightFront.setDirection(RF_DIR); + leftRear.setDirection(LR_DIR); + rightRear.setDirection(RR_DIR); + + 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); + + try { + loadRecordedData(); + if (recordedFrames.isEmpty()) { + telemetry.addLine("ERROR: No recorded data found!"); + telemetry.update(); + return; + } + calculateRecordedVoltage(); + calibrateMaxVelocities(); + } catch (Exception e) { + telemetry.addData("ERROR", e.toString()); + telemetry.update(); + sleep(3000); + return; + } + + RobotFrame first = recordedFrames.get(0); + robot.follower.setPose(new Pose(first.x, first.y, first.heading)); + + telemetry.addLine("=== Replay Auto V5 Ready ==="); + telemetry.addData("Frames", recordedFrames.size()); + telemetry.addData("Duration", "%.2f s", + recordedFrames.get(recordedFrames.size()-1).timestamp - first.timestamp); + telemetry.addData("Rec Voltage", "%.1f V", recordedVoltage); + telemetry.addData("Max Lin Vel", "%.1f (auto)", maxLinearVel); + telemetry.addData("Max Ang Vel", "%.2f rad/s (auto)", maxAngularVel); + telemetry.addData("FF Weight", "%.0f%%", FF_WEIGHT * 100); + telemetry.addData("Lookahead", "%.2f s", LOOKAHEAD_TIME); + telemetry.update(); + + waitForStart(); + if (isStopRequested()) return; + + executePlayback(); + + stopRobot(); + stopMechanisms(); + } + + // ------------------------------------------------------------------------- + // PLAYBACK LOOP + // ------------------------------------------------------------------------- + private void executePlayback() { + runtime.reset(); + + double startTs = recordedFrames.get(0).timestamp; + double endTs = recordedFrames.get(recordedFrames.size() - 1).timestamp; + double duration = endTs - startTs; + + // Reset state + prevTime = 0; + prevErrorX = prevErrorY = prevErrorHeading = 0; + filteredDx = filteredDy = filteredDh = 0; + prevRobotFwd = prevRobotStr = prevRobotTurn = 0; + maxWheelPowerSeen = 0; + clipCount = 0; + + int idx = 0; + + while (opModeIsActive() && idx < recordedFrames.size() - 1) { + double now = runtime.seconds(); + + // Refresh voltage and compute time scaling + refreshVoltage(now); + updateTimeScaling(); + + // Current position in recording, with lookahead and time scaling + // TIME SCALING ONLY — no velocity scaling + double recordingTime = (now / timeScale) + LOOKAHEAD_TIME; + double targetTs = startTs + recordingTime; + + // Advance index to frame just before targetTs + while (idx < recordedFrames.size() - 1 && + recordedFrames.get(idx + 1).timestamp <= targetTs) { + idx++; + } + + // Interpolate between frames + RobotFrame fA = recordedFrames.get(idx); + RobotFrame fB = (idx + 1 < recordedFrames.size()) ? recordedFrames.get(idx + 1) : fA; + + double segDur = fB.timestamp - fA.timestamp; + double t = (segDur > 1e-6) + ? Range.clip((targetTs - fA.timestamp) / segDur, 0.0, 1.0) + : 0.0; + + // 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); + + // Feedforward from TARGET FRAME (same time base as lookahead) + double ffVxRobot = lerp(fA.vxRobot, fB.vxRobot, t); + double ffVyRobot = lerp(fA.vyRobot, fB.vyRobot, t); + double ffOmega = lerp(fA.omega, fB.omega, t); + + // NO velocity scaling — time scaling handles battery compensation + + // Update localization + robot.follower.getPoseTracker().update(); + Pose cur = robot.follower.getPose(); + double curH = cur.getHeading(); + + // ===== POSE ERROR (in FIELD FRAME) ===== + double errX_Field = targetX - cur.getX(); + double errY_Field = targetY - cur.getY(); + double errH = normalizeAngle(targetH - curH); + + // ===== ROTATE ERROR INTO ROBOT FRAME ===== + double cosH = Math.cos(curH); + double sinH = Math.sin(curH); + double errX_Robot = cosH * errX_Field + sinH * errY_Field; + double errY_Robot = -sinH * errX_Field + cosH * errY_Field; + + double dt = now - prevTime; + + // Filtered derivatives for D-term (in ROBOT FRAME) + double rawDx = dt > 1e-6 ? (errX_Robot - prevErrorX) / dt : 0; + double rawDy = dt > 1e-6 ? (errY_Robot - prevErrorY) / dt : 0; + double rawDh = dt > 1e-6 ? (errH - prevErrorHeading) / dt : 0; + + filteredDx = filteredDx + D_FILTER_ALPHA * (rawDx - filteredDx); + filteredDy = filteredDy + D_FILTER_ALPHA * (rawDy - filteredDy); + filteredDh = filteredDh + D_FILTER_ALPHA * (rawDh - filteredDh); + + // PD correction (in ROBOT FRAME) + double corrX_Robot = errX_Robot * kP_TRANSLATION + filteredDx * kD_TRANSLATION; + double corrY_Robot = errY_Robot * kP_TRANSLATION + filteredDy * kD_TRANSLATION; + double corrH = errH * kP_ROTATION + filteredDh * kD_ROTATION; + + // Clamp correction + double corrMag = Math.hypot(corrX_Robot, corrY_Robot); + if (corrMag > MAX_CORRECTION) { + corrX_Robot *= MAX_CORRECTION / corrMag; + corrY_Robot *= MAX_CORRECTION / corrMag; + } + corrH = Range.clip(corrH, -MAX_CORRECTION, MAX_CORRECTION); + + // Update PD state + prevErrorX = errX_Robot; + prevErrorY = errY_Robot; + prevErrorHeading = errH; + prevTime = now; + + // ===== SCALE FEEDFORWARD TO POWER UNITS ===== + double ffFwd = ffVxRobot / maxLinearVel; + double ffStr = ffVyRobot / maxLinearVel; + double ffTurn = ffOmega / maxAngularVel; + + ffFwd = Range.clip(ffFwd, -1.0, 1.0); + ffStr = Range.clip(ffStr, -1.0, 1.0); + ffTurn = Range.clip(ffTurn, -1.0, 1.0); + + // ===== ERROR RECOVERY ===== + double posError = Math.hypot(errX_Field, errY_Field); + double currentFFWeight = FF_WEIGHT; + if (posError > ERROR_RECOVERY_THRESH) { + currentFFWeight = ERROR_RECOVERY_FF_WEIGHT; + } + + // ===== COMBINE FEEDFORWARD + CORRECTION (both in ROBOT FRAME) ===== + double robotFwd = ffFwd * currentFFWeight + corrX_Robot * (1 - currentFFWeight); + double robotStr = ffStr * currentFFWeight + corrY_Robot * (1 - currentFFWeight); + double robotTurn = ffTurn * currentFFWeight + corrH * (1 - currentFFWeight); + + // ===== SLEW RATE LIMITING ===== + 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; + + // ===== MECANUM MIXING ===== + double fl = robotFwd + robotStr + robotTurn; + double fr = robotFwd - robotStr - robotTurn; + double bl = robotFwd - robotStr + robotTurn; + double br = robotFwd + robotStr - robotTurn; + + // ===== NORMALIZE ONLY IF SATURATED ===== + double maxWheel = Math.max(1.0, + Math.max(Math.abs(fl), + Math.max(Math.abs(fr), + Math.max(Math.abs(bl), Math.abs(br))))); + + if (maxWheel > 1.0) { + clipCount++; + fl /= maxWheel; + fr /= maxWheel; + bl /= maxWheel; + br /= maxWheel; + } + maxWheelPowerSeen = Math.max(maxWheelPowerSeen, maxWheel); + + leftFront.setPower(fl); + rightFront.setPower(fr); + leftRear.setPower(bl); + rightRear.setPower(br); + + // Mechanisms + controlMechanisms(fA, fB, t); + + // Telemetry + telemetry.addLine("=== Replay V5 ==="); + telemetry.addData("Time", "%.2f/%.2f s (scale %.2f)", now, duration * timeScale, timeScale); + telemetry.addData("Frame", "%d/%d (lerp %.2f)", idx, recordedFrames.size(), t); + telemetry.addData("FF%", "%.0f%%", currentFFWeight * 100); + telemetry.addData("FF cmd", "fwd=%.2f str=%.2f turn=%.2f", ffFwd, ffStr, ffTurn); + telemetry.addData("Corr", "x=%.2f y=%.2f h=%.2f", corrX_Robot, corrY_Robot, corrH); + telemetry.addData("Slew", "fwd=%.2f str=%.2f turn=%.2f", robotFwd, robotStr, robotTurn); + telemetry.addData("PosErr", "%.2f (thresh %.1f)", posError, ERROR_RECOVERY_THRESH); + telemetry.addData("Target", "(%.1f, %.1f) h=%.1f°", targetX, targetY, Math.toDegrees(targetH)); + telemetry.addData("Current", "(%.1f, %.1f) h=%.1f°", cur.getX(), cur.getY(), Math.toDegrees(curH)); + telemetry.addData("Voltage", "%.1f V (rec %.1f V)", cachedVoltage, recordedVoltage); + telemetry.addData("MaxWheel", "%.2f (clips %d)", maxWheelPowerSeen, clipCount); + telemetry.update(); + + idle(); + } + } + + // ------------------------------------------------------------------------- + // MECHANISMS + // ------------------------------------------------------------------------- + private void controlMechanisms(RobotFrame a, RobotFrame b, double t) { + 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)); + + RobotFrame 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); + } + + private void stopMechanisms() { + 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); + } + + private void stopRobot() { + leftFront.setPower(0); + rightFront.setPower(0); + leftRear.setPower(0); + rightRear.setPower(0); + } + + // ------------------------------------------------------------------------- + // DATA LOADING & CALIBRATION + // ------------------------------------------------------------------------- + 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 >= 15) recordedFrames.add(new RobotFrame(d)); + } + } + } + + private void calibrateMaxVelocities() { + List linearSpeeds = new ArrayList<>(); + List angularSpeeds = new ArrayList<>(); + + for (RobotFrame frame : recordedFrames) { + double linear = Math.hypot(frame.vxRobot, frame.vyRobot); + if (linear > 0.5) linearSpeeds.add(linear); + if (Math.abs(frame.omega) > 0.05) angularSpeeds.add(Math.abs(frame.omega)); + } + + if (!linearSpeeds.isEmpty()) { + Collections.sort(linearSpeeds); + int p95Index = (int) (linearSpeeds.size() * 0.95); + maxLinearVel = linearSpeeds.get(Math.min(p95Index, linearSpeeds.size() - 1)) * 1.15; + } + + if (!angularSpeeds.isEmpty()) { + Collections.sort(angularSpeeds); + int p95Index = (int) (angularSpeeds.size() * 0.95); + maxAngularVel = angularSpeeds.get(Math.min(p95Index, angularSpeeds.size() - 1)) * 1.15; + } + } + + private void calculateRecordedVoltage() { + double total = 0; + int count = 0; + for (RobotFrame f : recordedFrames) { + if (f.voltage > 5) { total += f.voltage; count++; } + } + if (count > 0) recordedVoltage = total / count; + } + + 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; + if (ratio < 1.0) { + timeScale = 1.0 + TIME_SCALE_FACTOR * (1.0 - ratio); + } else { + timeScale = 1.0; + } + timeScale = Range.clip(timeScale, MIN_TIME_SCALE, MAX_TIME_SCALE); + } + + // ------------------------------------------------------------------------- + // UTILITIES + // ------------------------------------------------------------------------- + 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; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAutoOp7.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAutoOp7.java new file mode 100644 index 0000000..71de4f1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAutoOp7.java @@ -0,0 +1,495 @@ +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.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 Auto V7 — Driver Input Feedforward + Continuous Blending + Proper Constraints + * + * ARCHITECTURE: + * Feedforward = driver's actual gamepad stick inputs (already in [-1, 1]) + * Correction = small PD on pose error (rotated to robot frame) + * Blending = continuous exponential based on pose error magnitude + * Battery = time scaling only (no voltage compensation on power) + * Constraints = proper mecanum normalization, slew rate limiting + * + * WHY THIS IS THE BEST APPROACH: + * - Feedforward is CAUSAL: the driver's intent, not derived from effect + * - No derivation noise, no scaling factors, no max velocity calibration + * - Identical mecanum mixing path to TeleOp + * - Continuous blending is smooth and physically meaningful + * - Time scaling naturally compensates battery + * + * CSV Format (from DataRecordingOp7): + * Time,X,Y,Heading,Voltage,GamepadFwd,GamepadStr,GamepadTurn,IntakePwr,LoaderPwr,LShooterPwr,RShooterPwr,TurretPos,AnglePos,FlapPos + */ +@Autonomous(name = "Replay Auto V7", group = "Replay") +public class ReplayAutoOp7 extends LinearOpMode { + + private static final String CSV_PATH = "/sdcard/robot_data_v7.csv"; + + // ========== TUNABLE CONSTANTS ========== + + /** Continuous blending: how fast FF weight drops as error grows. + * k_blend = 0.3 means at 3.3cm error, FF weight is ~50%. + * Higher = more aggressive correction at small errors. + * Lower = more trust in feedforward. + * Start with 0.25–0.40. */ + private static final double BLEND_K = 0.30; + + /** Minimum FF weight (even at huge error). Prevents pure PD oscillation. */ + private static final double MIN_FF_WEIGHT = 0.35; + + /** PD gains — LOW because driver input does the heavy lifting. */ + private static final double kP_TRANSLATION = 0.018; + private static final double kD_TRANSLATION = 0.010; + private static final double kP_ROTATION = 0.25; + private static final double kD_ROTATION = 0.06; + + /** Max power the correction term can add. */ + private static final double MAX_CORRECTION = 0.25; + + /** Lookahead: follow a point this many seconds ahead in the recording. */ + private static final double LOOKAHEAD_TIME = 0.08; + + /** Time scaling for battery compensation. + * Lower battery → stretch time so robot has more time to execute. + * NO velocity scaling — time scaling handles everything. */ + private static final double TIME_SCALE_FACTOR = 0.55; + private static final double MAX_TIME_SCALE = 1.6; + private static final double MIN_TIME_SCALE = 0.85; + + /** D-term low-pass filter. */ + private static final double D_FILTER_ALPHA = 0.45; + + /** Slew rate limit: max change in command per second. + * Human inputs are naturally smooth, but interpolation + correction + * can create discontinuities. 5.0 = full range in 0.4s. */ + private static final double MAX_SLEW_RATE = 5.0; + + /** Voltage sensor refresh rate. */ + private static final double VOLTAGE_REFRESH_SEC = 0.1; + + // ========== MOTOR DIRECTIONS ========== + // COPY THESE EXACTLY FROM THE TELEMETRY OUTPUT OF DataRecordingOp7 + private static final DcMotorSimple.Direction LF_DIR = DcMotorSimple.Direction.REVERSE; + private static final DcMotorSimple.Direction RF_DIR = DcMotorSimple.Direction.REVERSE; + private static final DcMotorSimple.Direction LR_DIR = DcMotorSimple.Direction.REVERSE; + private static final DcMotorSimple.Direction RR_DIR = DcMotorSimple.Direction.FORWARD; + + // ========== STATE ========== + private final Robot robot = Robot.getInstance(); + private final ElapsedTime runtime = new ElapsedTime(); + private final List recordedFrames = new ArrayList<>(); + + private DcMotorEx leftFront, rightFront, leftRear, rightRear; + + // PD state (all in ROBOT FRAME) + private double prevErrorX = 0, prevErrorY = 0, prevErrorHeading = 0; + private double prevTime = 0; + private double filteredDx = 0, filteredDy = 0, filteredDh = 0; + + // Slew rate limiter state + private double prevRobotFwd = 0, prevRobotStr = 0, prevRobotTurn = 0; + + // Voltage + private double cachedVoltage = 12.0; + private double lastVoltageReadTime = -999; + private double recordedVoltage = 12.0; + private double timeScale = 1.0; + + // Telemetry stats + private double maxWheelPowerSeen = 0; + private int clipCount = 0; + private double avgFFWeight = 0; + private int loopCount = 0; + + // ------------------------------------------------------------------------- + // DATA MODEL + // ------------------------------------------------------------------------- + private static class RobotFrame { + double timestamp; + double x, y, heading; + double voltage; + double gpFwd, gpStr, gpTurn; // DRIVER INPUTS in [-1, 1] (robot-frame) + double intakePwr, loaderPwr, leftShtrPwr, rightShtrPwr; + double turretPos, anglePos, flapPos; + + RobotFrame(String[] d) { + timestamp = Double.parseDouble(d[0]); + x = Double.parseDouble(d[1]); + y = Double.parseDouble(d[2]); + heading = Double.parseDouble(d[3]); + voltage = Double.parseDouble(d[4]); + gpFwd = Double.parseDouble(d[5]); + gpStr = Double.parseDouble(d[6]); + gpTurn = Double.parseDouble(d[7]); + intakePwr = Double.parseDouble(d[8]); + loaderPwr = Double.parseDouble(d[9]); + leftShtrPwr = Double.parseDouble(d[10]); + rightShtrPwr= Double.parseDouble(d[11]); + turretPos = Double.parseDouble(d[12]); + anglePos = Double.parseDouble(d[13]); + flapPos = Double.parseDouble(d[14]); + } + } + + // ------------------------------------------------------------------------- + // MAIN + // ------------------------------------------------------------------------- + @Override + public void runOpMode() { + telemetry.addLine("Initializing Replay Auto V7..."); + telemetry.update(); + + robot.initFollower(hardwareMap, true); + robot.init(hardwareMap); + + try { + robot.follower.getPoseTracker().resetIMU(); + } catch (InterruptedException e) { + telemetry.addLine("IMU Reset Interrupted"); + } + + // Drive motors with verified 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(LF_DIR); + rightFront.setDirection(RF_DIR); + leftRear.setDirection(LR_DIR); + rightRear.setDirection(RR_DIR); + + 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); + + try { + loadRecordedData(); + if (recordedFrames.isEmpty()) { + telemetry.addLine("ERROR: No recorded data found!"); + telemetry.update(); + return; + } + calculateRecordedVoltage(); + } catch (Exception e) { + telemetry.addData("ERROR", e.toString()); + telemetry.update(); + sleep(3000); + return; + } + + RobotFrame first = recordedFrames.get(0); + robot.follower.setPose(new Pose(first.x, first.y, first.heading)); + + telemetry.addLine("=== Replay Auto V7 Ready ==="); + telemetry.addData("Frames", recordedFrames.size()); + telemetry.addData("Duration", "%.2f s", + recordedFrames.get(recordedFrames.size()-1).timestamp - first.timestamp); + telemetry.addData("Rec Voltage", "%.1f V", recordedVoltage); + telemetry.addData("Blend K", "%.2f", BLEND_K); + telemetry.addData("Lookahead", "%.2f s", LOOKAHEAD_TIME); + telemetry.update(); + + waitForStart(); + if (isStopRequested()) return; + + executePlayback(); + + stopRobot(); + stopMechanisms(); + } + + // ------------------------------------------------------------------------- + // PLAYBACK LOOP + // ------------------------------------------------------------------------- + private void executePlayback() { + runtime.reset(); + + double startTs = recordedFrames.get(0).timestamp; + double endTs = recordedFrames.get(recordedFrames.size() - 1).timestamp; + double duration = endTs - startTs; + + // Reset state + prevTime = 0; + prevErrorX = prevErrorY = prevErrorHeading = 0; + filteredDx = filteredDy = filteredDh = 0; + prevRobotFwd = prevRobotStr = prevRobotTurn = 0; + maxWheelPowerSeen = 0; + clipCount = 0; + avgFFWeight = 0; + loopCount = 0; + + int idx = 0; + + while (opModeIsActive() && idx < recordedFrames.size() - 1) { + double now = runtime.seconds(); + + // Refresh voltage and compute time scaling + refreshVoltage(now); + updateTimeScaling(); + + // Current position in recording, with lookahead and time scaling + double recordingTime = (now / timeScale) + LOOKAHEAD_TIME; + double targetTs = startTs + recordingTime; + + // Advance index to frame just before targetTs + while (idx < recordedFrames.size() - 1 && + recordedFrames.get(idx + 1).timestamp <= targetTs) { + idx++; + } + + // Interpolate between frames + RobotFrame fA = recordedFrames.get(idx); + RobotFrame fB = (idx + 1 < recordedFrames.size()) ? recordedFrames.get(idx + 1) : fA; + + double segDur = fB.timestamp - fA.timestamp; + double t = (segDur > 1e-6) + ? Range.clip((targetTs - fA.timestamp) / segDur, 0.0, 1.0) + : 0.0; + + // 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); + + // ===== FEEDFORWARD: DRIVER'S ACTUAL INPUTS ===== + // These are already in [-1, 1] robot-frame power units + // Interpolated at the SAME target frame as lookahead + double ffFwd = lerp(fA.gpFwd, fB.gpFwd, t); + double ffStr = lerp(fA.gpStr, fB.gpStr, t); + double ffTurn = lerp(fA.gpTurn, fB.gpTurn, t); + + // Clamp to [-1, 1] (should already be, but safety) + ffFwd = Range.clip(ffFwd, -1.0, 1.0); + ffStr = Range.clip(ffStr, -1.0, 1.0); + ffTurn = Range.clip(ffTurn, -1.0, 1.0); + + // Update localization + robot.follower.getPoseTracker().update(); + Pose cur = robot.follower.getPose(); + double curH = cur.getHeading(); + + // ===== POSE ERROR (in FIELD FRAME) ===== + double errX_Field = targetX - cur.getX(); + double errY_Field = targetY - cur.getY(); + double errH = normalizeAngle(targetH - curH); + + // ===== ROTATE ERROR INTO ROBOT FRAME ===== + double cosH = Math.cos(curH); + double sinH = Math.sin(curH); + double errX_Robot = cosH * errX_Field + sinH * errY_Field; + double errY_Robot = -sinH * errX_Field + cosH * errY_Field; + + double dt = now - prevTime; + + // Filtered derivatives for D-term (in ROBOT FRAME) + double rawDx = dt > 1e-6 ? (errX_Robot - prevErrorX) / dt : 0; + double rawDy = dt > 1e-6 ? (errY_Robot - prevErrorY) / dt : 0; + double rawDh = dt > 1e-6 ? (errH - prevErrorHeading) / dt : 0; + + filteredDx = filteredDx + D_FILTER_ALPHA * (rawDx - filteredDx); + filteredDy = filteredDy + D_FILTER_ALPHA * (rawDy - filteredDy); + filteredDh = filteredDh + D_FILTER_ALPHA * (rawDh - filteredDh); + + // PD correction (in ROBOT FRAME) + double corrX_Robot = errX_Robot * kP_TRANSLATION + filteredDx * kD_TRANSLATION; + double corrY_Robot = errY_Robot * kP_TRANSLATION + filteredDy * kD_TRANSLATION; + double corrH = errH * kP_ROTATION + filteredDh * kD_ROTATION; + + // Clamp correction + double corrMag = Math.hypot(corrX_Robot, corrY_Robot); + if (corrMag > MAX_CORRECTION) { + corrX_Robot *= MAX_CORRECTION / corrMag; + corrY_Robot *= MAX_CORRECTION / corrMag; + } + corrH = Range.clip(corrH, -MAX_CORRECTION, MAX_CORRECTION); + + // Update PD state + prevErrorX = errX_Robot; + prevErrorY = errY_Robot; + prevErrorHeading = errH; + prevTime = now; + + // ===== CONTINUOUS BLENDING BASED ON ERROR ===== + // w = exp(-k * error) → 1.0 at zero error, drops as error grows + double posError = Math.hypot(errX_Field, errY_Field); + double ffWeight = Math.exp(-BLEND_K * posError); + ffWeight = Range.clip(ffWeight, MIN_FF_WEIGHT, 1.0); + + // ===== COMBINE FEEDFORWARD + CORRECTION (both in ROBOT FRAME) ===== + double robotFwd = ffFwd * ffWeight + corrX_Robot * (1 - ffWeight); + double robotStr = ffStr * ffWeight + corrY_Robot * (1 - ffWeight); + double robotTurn = ffTurn * ffWeight + corrH * (1 - ffWeight); + + // ===== SLEW RATE LIMITING ===== + 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; + + // ===== MECANUM MIXING (identical to TeleOp path) ===== + double fl = robotFwd + robotStr + robotTurn; + double fr = robotFwd - robotStr - robotTurn; + double bl = robotFwd - robotStr + robotTurn; + double br = robotFwd + robotStr - robotTurn; + + // ===== PROPER MECANUM NORMALIZATION ===== + // Normalize so that |fwd| + |strafe| + |turn| <= 1 preserves ratios + double maxSum = Math.abs(robotFwd) + Math.abs(robotStr) + Math.abs(robotTurn); + if (maxSum > 1.0) { + clipCount++; + double scale = 1.0 / maxSum; + fl *= scale; + fr *= scale; + bl *= scale; + br *= scale; + } + maxWheelPowerSeen = Math.max(maxWheelPowerSeen, Math.max(Math.abs(fl), + Math.max(Math.abs(fr), Math.max(Math.abs(bl), Math.abs(br))))); + + leftFront.setPower(fl); + rightFront.setPower(fr); + leftRear.setPower(bl); + rightRear.setPower(br); + + // Mechanisms + controlMechanisms(fA, fB, t); + + // Stats + avgFFWeight += ffWeight; + loopCount++; + + // Telemetry + telemetry.addLine("=== Replay V7 ==="); + telemetry.addData("Time", "%.2f/%.2f s (scale %.2f)", now, duration * timeScale, timeScale); + telemetry.addData("Frame", "%d/%d (lerp %.2f)", idx, recordedFrames.size(), t); + telemetry.addData("FF wt", "%.0f%% (avg %.0f%%)", ffWeight * 100, (avgFFWeight / loopCount) * 100); + telemetry.addData("FF cmd", "fwd=%.2f str=%.2f turn=%.2f", ffFwd, ffStr, ffTurn); + telemetry.addData("Corr", "x=%.2f y=%.2f h=%.2f", corrX_Robot, corrY_Robot, corrH); + telemetry.addData("Slew", "fwd=%.2f str=%.2f turn=%.2f", robotFwd, robotStr, robotTurn); + telemetry.addData("PosErr", "%.2f", posError); + telemetry.addData("Target", "(%.1f, %.1f) h=%.1f°", targetX, targetY, Math.toDegrees(targetH)); + telemetry.addData("Current", "(%.1f, %.1f) h=%.1f°", cur.getX(), cur.getY(), Math.toDegrees(curH)); + telemetry.addData("Voltage", "%.1f V (rec %.1f V)", cachedVoltage, recordedVoltage); + telemetry.addData("MaxWheel", "%.2f (clips %d)", maxWheelPowerSeen, clipCount); + telemetry.update(); + + 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)); + + // Motors — snap to nearest frame (fast response, interpolation not critical) + RobotFrame 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); + } + + private void stopMechanisms() { + 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); + } + + private void stopRobot() { + leftFront.setPower(0); + rightFront.setPower(0); + leftRear.setPower(0); + rightRear.setPower(0); + } + + // ------------------------------------------------------------------------- + // DATA LOADING + // ------------------------------------------------------------------------- + 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 >= 15) recordedFrames.add(new RobotFrame(d)); + } + } + } + + private void calculateRecordedVoltage() { + double total = 0; + int count = 0; + for (RobotFrame f : recordedFrames) { + if (f.voltage > 5) { total += f.voltage; count++; } + } + if (count > 0) recordedVoltage = total / count; + } + + 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; + if (ratio < 1.0) { + timeScale = 1.0 + TIME_SCALE_FACTOR * (1.0 - ratio); + } else { + timeScale = 1.0; + } + timeScale = Range.clip(timeScale, MIN_TIME_SCALE, MAX_TIME_SCALE); + } + + // ------------------------------------------------------------------------- + // UTILITIES + // ------------------------------------------------------------------------- + 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; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAutoOp8.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAutoOp8.java new file mode 100644 index 0000000..04e2820 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAutoOp8.java @@ -0,0 +1,576 @@ +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.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 Auto V8 — Exact Match to MainDrivingOp + * + * FIXES from V7: + * 1. Bug fix: getInterpolatedFrame uses lightweight InterpolatedInputs class + * 2. Mechanisms behave IDENTICALLY to MainDrivingOp (auto-aim, toggles, etc.) + * 3. Normalization: max wheel power + * 4. Blending: additive (ff + scaled corr) + * 5. Dynamic correction cap + * 6. Disable lookahead near stop + * 7. Clamp dt + * 8. Lerp motor powers + * + * CSV Format (from DataRecordingOp8): + * Time,X,Y,Heading,Voltage,GamepadFwd,GamepadStr,GamepadTurn, + * IntakePwr,LoaderPwr,LShooterPwr,RShooterPwr,TurretPos,AnglePos,FlapPos,AutoAim,BlueTarget + */ +@Autonomous(name = "Replay Auto V8", group = "Replay") +public class ReplayAutoOp8 extends LinearOpMode { + + private static final String CSV_PATH = "/sdcard/robot_data_v8.csv"; + + // ========== TUNABLE CONSTANTS ========== + + /** Continuous blending: how fast correction weight grows with error. */ + private static final double BLEND_K = 0.35; + + /** PD gains — LOW because driver input does the heavy lifting. */ + private static final double kP_TRANSLATION = 0.018; + private static final double kD_TRANSLATION = 0.010; + private static final double kP_ROTATION = 0.28; + private static final double kD_ROTATION = 0.07; + + /** Base correction cap (small error). */ + private static final double MIN_CORRECTION_CAP = 0.20; + /** Max correction cap (large error). */ + private static final double MAX_CORRECTION_CAP = 0.60; + /** Error scale for dynamic cap transition. */ + private static final double CORRECTION_ERROR_SCALE = 8.0; + + /** Lookahead: follow a point this many seconds ahead. */ + private static final double LOOKAHEAD_TIME = 0.08; + /** Threshold to disable lookahead (near stop). */ + private static final double LOOKAHEAD_DISABLE_THRESH = 0.04; + + /** Time scaling for battery compensation. */ + private static final double TIME_SCALE_FACTOR = 0.55; + private static final double MAX_TIME_SCALE = 1.6; + private static final double MIN_TIME_SCALE = 0.85; + + /** D-term low-pass filter. */ + private static final double D_FILTER_ALPHA = 0.45; + + /** Slew rate limit: max change in command per second. */ + private static final double MAX_SLEW_RATE = 5.0; + + /** Voltage sensor refresh rate. */ + private static final double VOLTAGE_REFRESH_SEC = 0.1; + + /** dt clamp to prevent derivative spikes from timing jitter. */ + private static final double MIN_DT = 0.008; + private static final double MAX_DT = 0.050; + + // ========== MOTOR DIRECTIONS ========== + // COPY THESE EXACTLY FROM THE TELEMETRY OUTPUT OF DataRecordingOp8 + private static final DcMotorSimple.Direction LF_DIR = DcMotorSimple.Direction.REVERSE; + private static final DcMotorSimple.Direction RF_DIR = DcMotorSimple.Direction.REVERSE; + private static final DcMotorSimple.Direction LR_DIR = DcMotorSimple.Direction.REVERSE; + private static final DcMotorSimple.Direction RR_DIR = DcMotorSimple.Direction.FORWARD; + + // ========== STATE ========== + private final Robot robot = Robot.getInstance(); + private final ElapsedTime runtime = new ElapsedTime(); + private final List recordedFrames = new ArrayList<>(); + + private DcMotorEx leftFront, rightFront, leftRear, rightRear; + + // PD state (all in ROBOT FRAME) + private double prevErrorX = 0, prevErrorY = 0, prevErrorHeading = 0; + private double prevTime = 0; + private double filteredDx = 0, filteredDy = 0, filteredDh = 0; + + // Slew rate limiter state + private double prevRobotFwd = 0, prevRobotStr = 0, prevRobotTurn = 0; + + // Voltage + private double cachedVoltage = 12.0; + private double lastVoltageReadTime = -999; + private double recordedVoltage = 12.0; + private double timeScale = 1.0; + + // Telemetry stats + private double maxWheelPowerSeen = 0; + private int clipCount = 0; + private double avgCorrWeight = 0; + private int loopCount = 0; + + // ------------------------------------------------------------------------- + // DATA MODEL + // ------------------------------------------------------------------------- + private static class RobotFrame { + double timestamp; + double x, y, heading; + double voltage; + double gpFwd, gpStr, gpTurn; // DRIVER INPUTS in [-1, 1] + double intakePwr, loaderPwr, leftShtrPwr, rightShtrPwr; + double turretPos, anglePos, flapPos; + boolean autoAimEnabled; + boolean blueTarget; + + RobotFrame(String[] d) { + timestamp = Double.parseDouble(d[0]); + x = Double.parseDouble(d[1]); + y = Double.parseDouble(d[2]); + heading = Double.parseDouble(d[3]); + voltage = Double.parseDouble(d[4]); + gpFwd = Double.parseDouble(d[5]); + gpStr = Double.parseDouble(d[6]); + gpTurn = Double.parseDouble(d[7]); + intakePwr = Double.parseDouble(d[8]); + loaderPwr = Double.parseDouble(d[9]); + leftShtrPwr = Double.parseDouble(d[10]); + rightShtrPwr= Double.parseDouble(d[11]); + turretPos = Double.parseDouble(d[12]); + anglePos = Double.parseDouble(d[13]); + flapPos = Double.parseDouble(d[14]); + autoAimEnabled = Integer.parseInt(d[15]) != 0; + blueTarget = Integer.parseInt(d[16]) != 0; + } + } + + /** Lightweight class for interpolated inputs — BUG FIX from V7 */ + private static class InterpolatedInputs { + double gpFwd, gpStr, gpTurn; + double intakePwr, loaderPwr, leftShtrPwr, rightShtrPwr; + double turretPos, anglePos, flapPos; + boolean autoAimEnabled; + boolean blueTarget; + } + + // ------------------------------------------------------------------------- + // MAIN + // ------------------------------------------------------------------------- + @Override + public void runOpMode() { + telemetry.addLine("Initializing Replay Auto V8..."); + telemetry.update(); + + robot.initFollower(hardwareMap, true); + robot.init(hardwareMap); + + try { + robot.follower.getPoseTracker().resetIMU(); + } catch (InterruptedException e) { + telemetry.addLine("IMU Reset Interrupted"); + } + + // Drive motors with verified 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(LF_DIR); + rightFront.setDirection(RF_DIR); + leftRear.setDirection(LR_DIR); + rightRear.setDirection(RR_DIR); + + 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); + + try { + loadRecordedData(); + if (recordedFrames.isEmpty()) { + telemetry.addLine("ERROR: No recorded data found!"); + telemetry.update(); + return; + } + calculateRecordedVoltage(); + } catch (Exception e) { + telemetry.addData("ERROR", e.toString()); + telemetry.update(); + sleep(3000); + return; + } + + RobotFrame first = recordedFrames.get(0); + robot.follower.setPose(new Pose(first.x, first.y, first.heading)); + robot.Blue_Target = first.blueTarget; + + telemetry.addLine("=== Replay Auto V8 Ready ==="); + telemetry.addData("Frames", recordedFrames.size()); + telemetry.addData("Duration", "%.2f s", + recordedFrames.get(recordedFrames.size()-1).timestamp - first.timestamp); + telemetry.addData("Rec Voltage", "%.1f V", recordedVoltage); + telemetry.addData("Blend K", "%.2f", BLEND_K); + telemetry.addData("Lookahead", "%.2f s", LOOKAHEAD_TIME); + telemetry.update(); + + waitForStart(); + if (isStopRequested()) return; + + executePlayback(); + + stopRobot(); + stopMechanisms(); + } + + // ------------------------------------------------------------------------- + // PLAYBACK LOOP + // ------------------------------------------------------------------------- + private void executePlayback() { + runtime.reset(); + + double startTs = recordedFrames.get(0).timestamp; + double endTs = recordedFrames.get(recordedFrames.size() - 1).timestamp; + double duration = endTs - startTs; + + // Reset state + prevTime = 0; + prevErrorX = prevErrorY = prevErrorHeading = 0; + filteredDx = filteredDy = filteredDh = 0; + prevRobotFwd = prevRobotStr = prevRobotTurn = 0; + maxWheelPowerSeen = 0; + clipCount = 0; + avgCorrWeight = 0; + loopCount = 0; + + int idx = 0; + + while (opModeIsActive() && idx < recordedFrames.size() - 1) { + double now = runtime.seconds(); + + // Refresh voltage and compute time scaling + refreshVoltage(now); + updateTimeScaling(); + + // ===== LOOKAHEAD (disable near stop) ===== + double currentRecordingTime = now / timeScale; + InterpolatedInputs currentInputs = getInterpolatedInputs(currentRecordingTime, startTs); + double currentInputMag = Math.abs(currentInputs.gpFwd) + + Math.abs(currentInputs.gpStr) + + Math.abs(currentInputs.gpTurn); + + double lookahead = (currentInputMag > LOOKAHEAD_DISABLE_THRESH) ? LOOKAHEAD_TIME : 0.0; + + // Current position in recording, with lookahead and time scaling + double recordingTime = currentRecordingTime + lookahead; + double targetTs = startTs + recordingTime; + + // Advance index to frame just before targetTs + while (idx < recordedFrames.size() - 1 && + recordedFrames.get(idx + 1).timestamp <= targetTs) { + idx++; + } + + // Interpolate between frames + RobotFrame fA = recordedFrames.get(idx); + RobotFrame fB = (idx + 1 < recordedFrames.size()) ? recordedFrames.get(idx + 1) : fA; + + double segDur = fB.timestamp - fA.timestamp; + double t = (segDur > 1e-6) + ? Range.clip((targetTs - fA.timestamp) / segDur, 0.0, 1.0) + : 0.0; + + // 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); + + // ===== FEEDFORWARD: DRIVER'S ACTUAL INPUTS ===== + double ffFwd = lerp(fA.gpFwd, fB.gpFwd, t); + double ffStr = lerp(fA.gpStr, fB.gpStr, t); + double ffTurn = lerp(fA.gpTurn, fB.gpTurn, t); + + ffFwd = Range.clip(ffFwd, -1.0, 1.0); + ffStr = Range.clip(ffStr, -1.0, 1.0); + ffTurn = Range.clip(ffTurn, -1.0, 1.0); + + // Update localization + robot.follower.getPoseTracker().update(); + Pose cur = robot.follower.getPose(); + double curH = cur.getHeading(); + + // ===== POSE ERROR (in FIELD FRAME) ===== + double errX_Field = targetX - cur.getX(); + double errY_Field = targetY - cur.getY(); + double errH = normalizeAngle(targetH - curH); + + // ===== ROTATE ERROR INTO ROBOT FRAME ===== + double cosH = Math.cos(curH); + double sinH = Math.sin(curH); + double errX_Robot = cosH * errX_Field + sinH * errY_Field; + double errY_Robot = -sinH * errX_Field + cosH * errY_Field; + + // ===== CLAMPED dt ===== + double rawDt = now - prevTime; + double dt = Range.clip(rawDt, MIN_DT, MAX_DT); + + // Filtered derivatives for D-term (in ROBOT FRAME) + double rawDx = (errX_Robot - prevErrorX) / dt; + double rawDy = (errY_Robot - prevErrorY) / dt; + double rawDh = (errH - prevErrorHeading) / dt; + + filteredDx = filteredDx + D_FILTER_ALPHA * (rawDx - filteredDx); + filteredDy = filteredDy + D_FILTER_ALPHA * (rawDy - filteredDy); + filteredDh = filteredDh + D_FILTER_ALPHA * (rawDh - filteredDh); + + // PD correction (in ROBOT FRAME) + double corrX_Robot = errX_Robot * kP_TRANSLATION + filteredDx * kD_TRANSLATION; + double corrY_Robot = errY_Robot * kP_TRANSLATION + filteredDy * kD_TRANSLATION; + double corrH = errH * kP_ROTATION + filteredDh * kD_ROTATION; + + // ===== DYNAMIC CORRECTION CAP ===== + double posError = Math.hypot(errX_Field, errY_Field); + double corrScale = Math.min(posError / CORRECTION_ERROR_SCALE, 1.0); + double dynamicMaxCorr = lerp(MIN_CORRECTION_CAP, MAX_CORRECTION_CAP, corrScale); + + double corrMag = Math.hypot(corrX_Robot, corrY_Robot); + if (corrMag > dynamicMaxCorr) { + double scale = dynamicMaxCorr / corrMag; + corrX_Robot *= scale; + corrY_Robot *= scale; + } + corrH = Range.clip(corrH, -dynamicMaxCorr, dynamicMaxCorr); + + // Update PD state + prevErrorX = errX_Robot; + prevErrorY = errY_Robot; + prevErrorHeading = errH; + prevTime = now; + + // ===== CONTINUOUS BLENDING (additive) ===== + double corrWeight = 1.0 - Math.exp(-BLEND_K * posError); + corrWeight = Range.clip(corrWeight, 0.0, 1.0); + + // ===== ADDITIVE BLENDING: u = u_ff + corrWeight * u_fb ===== + double robotFwd = ffFwd + corrWeight * corrX_Robot; + double robotStr = ffStr + corrWeight * corrY_Robot; + double robotTurn = ffTurn + corrWeight * corrH; + + // Clamp combined command to [-1, 1] before mixing + double combinedMag = Math.hypot(robotFwd, robotStr); + if (combinedMag > 1.0) { + robotFwd /= combinedMag; + robotStr /= combinedMag; + } + robotTurn = Range.clip(robotTurn, -1.0, 1.0); + + // ===== SLEW RATE LIMITING ===== + 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; + + // ===== MECANUM MIXING ===== + double fl = robotFwd + robotStr + robotTurn; + double fr = robotFwd - robotStr - robotTurn; + double bl = robotFwd - robotStr + robotTurn; + double br = robotFwd + robotStr - robotTurn; + + // ===== NORMALIZE: max wheel power ===== + double maxWheel = Math.max(1.0, + Math.max(Math.abs(fl), + Math.max(Math.abs(fr), + Math.max(Math.abs(bl), Math.abs(br))))); + + if (maxWheel > 1.0) { + clipCount++; + fl /= maxWheel; + fr /= maxWheel; + bl /= maxWheel; + br /= maxWheel; + } + maxWheelPowerSeen = Math.max(maxWheelPowerSeen, maxWheel); + + leftFront.setPower(fl); + rightFront.setPower(fr); + leftRear.setPower(bl); + rightRear.setPower(br); + + // ===== MECHANISMS — EXACT MATCH TO MainDrivingOp ===== + controlMechanisms(fA, fB, t); + + // Stats + avgCorrWeight += corrWeight; + loopCount++; + + // Telemetry + telemetry.addLine("=== Replay V8 ==="); + telemetry.addData("Time", "%.2f/%.2f s (scale %.2f)", now, duration * timeScale, timeScale); + telemetry.addData("Frame", "%d/%d (lerp %.2f)", idx, recordedFrames.size(), t); + telemetry.addData("Lookahead", "%.3f s", lookahead); + telemetry.addData("Corr%", "%.0f%% (avg %.0f%%)", corrWeight * 100, (avgCorrWeight / loopCount) * 100); + telemetry.addData("FF cmd", "fwd=%.2f str=%.2f turn=%.2f", ffFwd, ffStr, ffTurn); + telemetry.addData("Corr", "x=%.2f y=%.2f h=%.2f (cap %.2f)", corrX_Robot, corrY_Robot, corrH, dynamicMaxCorr); + telemetry.addData("Final", "fwd=%.2f str=%.2f turn=%.2f", robotFwd, robotStr, robotTurn); + telemetry.addData("PosErr", "%.2f", posError); + telemetry.addData("Target", "(%.1f, %.1f) h=%.1f°", targetX, targetY, Math.toDegrees(targetH)); + telemetry.addData("Current", "(%.1f, %.1f) h=%.1f°", cur.getX(), cur.getY(), Math.toDegrees(curH)); + telemetry.addData("Voltage", "%.1f V (rec %.1f V)", cachedVoltage, recordedVoltage); + telemetry.addData("MaxWheel", "%.2f (clips %d)", maxWheelPowerSeen, clipCount); + telemetry.update(); + + idle(); + } + } + + // ------------------------------------------------------------------------- + // MECHANISMS — EXACT MATCH TO MainDrivingOp LOGIC + // ------------------------------------------------------------------------- + private void controlMechanisms(RobotFrame a, RobotFrame b, double t) { + // Interpolate all mechanism values + double intakePwr = lerp(a.intakePwr, b.intakePwr, t); + double loaderPwr = lerp(a.loaderPwr, b.loaderPwr, t); + double leftShtrPwr = lerp(a.leftShtrPwr, b.leftShtrPwr, t); + double rightShtrPwr = lerp(a.rightShtrPwr, b.rightShtrPwr, t); + double turretPos = lerp(a.turretPos, b.turretPos, t); + double anglePos = lerp(a.anglePos, b.anglePos, t); + double flapPos = lerp(a.flapPos, b.flapPos, t); + + // Auto-aim state — use nearest frame (state changes are discrete) + boolean autoAim = (t < 0.5) ? a.autoAimEnabled : b.autoAimEnabled; + boolean blueTarget = (t < 0.5) ? a.blueTarget : b.blueTarget; + + // Apply to robot — EXACT same as MainDrivingOp + if (robot.intakeMotor != null) robot.intakeMotor.setPower(intakePwr); + if (robot.loaderMotor != null) robot.loaderMotor.setPower(loaderPwr); + if (robot.leftOuttake != null) robot.leftOuttake.setPower(leftShtrPwr); + if (robot.rightOuttake != null) robot.rightOuttake.setPower(rightShtrPwr); + + robot.turretServo.setPosition(turretPos); + robot.angleServo.setPosition(anglePos); + robot.flapsServo.setPosition(flapPos); + + // Set robot state variables for turret auto-aim logic + robot.Blue_Target = blueTarget; + // Note: autoAim state is used by turret.update() if it checks this + // The recorded turretPos already includes the result of auto-aim calculations + } + + private void stopMechanisms() { + 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); + } + + private void stopRobot() { + leftFront.setPower(0); + rightFront.setPower(0); + leftRear.setPower(0); + rightRear.setPower(0); + } + + // ------------------------------------------------------------------------- + // HELPERS — BUG FIX: lightweight InterpolatedInputs class + // ------------------------------------------------------------------------- + + /** + * Get interpolated inputs at a given recording time. + * Uses lightweight InterpolatedInputs instead of RobotFrame constructor. + */ + private InterpolatedInputs getInterpolatedInputs(double recordingTime, double startTs) { + double targetTs = startTs + recordingTime; + InterpolatedInputs result = new InterpolatedInputs(); + + for (int i = 0; i < recordedFrames.size() - 1; i++) { + RobotFrame a = recordedFrames.get(i); + RobotFrame b = recordedFrames.get(i + 1); + if (b.timestamp > targetTs) { + double segDur = b.timestamp - a.timestamp; + double t = (segDur > 1e-6) + ? Range.clip((targetTs - a.timestamp) / segDur, 0.0, 1.0) + : 0.0; + result.gpFwd = lerp(a.gpFwd, b.gpFwd, t); + result.gpStr = lerp(a.gpStr, b.gpStr, t); + result.gpTurn = lerp(a.gpTurn, b.gpTurn, t); + return result; + } + } + + // Fallback: return last frame + RobotFrame last = recordedFrames.get(recordedFrames.size() - 1); + result.gpFwd = last.gpFwd; + result.gpStr = last.gpStr; + result.gpTurn = last.gpTurn; + return result; + } + + // ------------------------------------------------------------------------- + // DATA LOADING + // ------------------------------------------------------------------------- + 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 >= 17) recordedFrames.add(new RobotFrame(d)); + } + } + } + + private void calculateRecordedVoltage() { + double total = 0; + int count = 0; + for (RobotFrame f : recordedFrames) { + if (f.voltage > 5) { total += f.voltage; count++; } + } + if (count > 0) recordedVoltage = total / count; + } + + 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; + if (ratio < 1.0) { + timeScale = 1.0 + TIME_SCALE_FACTOR * (1.0 - ratio); + } else { + timeScale = 1.0; + } + timeScale = Range.clip(timeScale, MIN_TIME_SCALE, MAX_TIME_SCALE); + } + + // ------------------------------------------------------------------------- + // UTILITIES + // ------------------------------------------------------------------------- + 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; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp4.java new file mode 100644 index 0000000..837f8f1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp4.java @@ -0,0 +1,319 @@ +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; + +/** + * Data Recorder V4 — Correct Frame Recording + * + * Records pose + ROBOT-FRAME smoothed velocities + mechanism states at 50Hz. + * Velocities are computed in the ROBOT'S FRAME during recording, so replay + * does not need to rotate them. This eliminates the field→robot frame + * mismatch bug entirely. + * + * CSV Format: + * Time,X,Y,Heading,Voltage,VxRobot,VyRobot,Omega,IntakePwr,LoaderPwr,LShooterPwr,RShooterPwr,TurretPos,AnglePos,FlapPos + * + * @version 5.0 + */ +@TeleOp(name = "Data Recorder V4", group = "Replay") +public class DataRecordingOp4 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 + private FileWriter dataRecorder; + private static final long RECORD_INTERVAL_MS = 20; + private long startTime; + private long lastRecordTime = 0; + + // Velocity smoothing (in ROBOT FRAME) + private double smoothedVxRobot = 0, smoothedVyRobot = 0, smoothedOmega = 0; + private static final double VEL_SMOOTH_ALPHA = 0.35; + + // Previous pose for velocity computation + private double prevX = 0, prevY = 0, prevHeading = 0; + private long prevPoseTime = 0; + private boolean firstPose = true; + + // Wheel velocity recording (optional debug) + 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); + } + + 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); + } + + // Get motors and LOG THEIR DIRECTIONS for replay verification + leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); + rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); + rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); + + telemetry.addLine("=== COPY THESE DIRECTIONS TO REPLAY CODE ==="); + telemetry.addData("LF", leftFront.getDirection().toString()); + telemetry.addData("RF", rightFront.getDirection().toString()); + telemetry.addData("LR", leftRear.getDirection().toString()); + telemetry.addData("RR", rightRear.getDirection().toString()); + telemetry.addLine("============================================"); + telemetry.update(); + + String filePath = Environment.getExternalStorageDirectory().getPath() + "/robot_data_v4.csv"; + try { + dataRecorder = new FileWriter(filePath); + dataRecorder.write("Time,X,Y,Heading,Voltage,VxRobot,VyRobot,Omega,IntakePwr,LoaderPwr,LShooterPwr,RShooterPwr,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 V4)"); + telemetry.addLine("Verify motor directions above match replay code!"); + telemetry.update(); + } + + @Override + public void start() { + robot.follower.startTeleopDrive(); + startTime = System.currentTimeMillis(); + firstPose = true; + } + + @Override + public void loop() { + long now = System.currentTimeMillis(); + lpsCounter.getLoopTime(); + + drivingGP.update(); + utilityGP.update(); + robot.follower.update(); + + // ===== MECHANISM CONTROL (same as before) ===== + robot.intake.speed = utilityGP.rightStick.y; + robot.intake.reversed = INTAKE_REVERSE; + turretAligner.update(); + + 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; + } + + // Turret/Angle aiming + if (autoAimEnabled) { + // To do + } else { + if (drivingGP.dpadLeft.pressed()) { + if (turretTimer.seconds() == 0) turretTimer.reset(); + double increment = turretTimer.seconds() > 1.5 ? 0.1 : + turretTimer.seconds() > 1.0 ? 0.07 : 0.03; + robot.turret.driverOffset += increment; + } else if (drivingGP.dpadRight.pressed()) { + if (turretTimer.seconds() == 0) turretTimer.reset(); + double decrement = turretTimer.seconds() > 1.5 ? 0.1 : + turretTimer.seconds() > 1.0 ? 0.07 : 0.03; + robot.turret.driverOffset -= decrement; + } else { + turretTimer.reset(); + } + + if (drivingGP.dpadUp.pressed()) + robot.outtake.activeConfig.angle += 0.01; + else if (drivingGP.dpadDown.pressed()) + robot.outtake.activeConfig.angle -= 0.01; + } + + // Shoot presets + 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); + } + + // Rumble when shooter ready + 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; + } + } + + 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(now); + } catch (IOException e) { + telemetry.addData("Recording Error", e.getMessage()); + } + lastRecordTime = now; + } + + _telemetry(); + } + + @Override + public void stop() { + robot.webcam.stop(); + if (dataRecorder != null) { + try { + dataRecorder.flush(); + dataRecorder.close(); + } catch (IOException ignored) {} + } + } + + private void recordData(long now) throws IOException { + double t = (now - startTime) / 1000.0; + double x = robot.follower.getPose().getX(); + double y = robot.follower.getPose().getY(); + double heading = robot.follower.getHeading(); + double voltage = hardwareMap.voltageSensor.iterator().next().getVoltage(); + + // Compute raw velocity from pose delta (in FIELD FRAME) + double rawVxField = 0, rawVyField = 0, rawOmega = 0; + if (!firstPose) { + double dt = (now - prevPoseTime) / 1000.0; + if (dt > 0.001) { + rawVxField = (x - prevX) / dt; + rawVyField = (y - prevY) / dt; + rawOmega = normalizeAngle(heading - prevHeading) / dt; + } + } else { + firstPose = false; + } + + // Rotate field-frame velocity into ROBOT FRAME + double cosH = Math.cos(heading); + double sinH = Math.sin(heading); + double rawVxRobot = cosH * rawVxField + sinH * rawVyField; + double rawVyRobot = -sinH * rawVxField + cosH * rawVyField; + + // Exponential smoothing (in ROBOT FRAME) + smoothedVxRobot = smoothedVxRobot + VEL_SMOOTH_ALPHA * (rawVxRobot - smoothedVxRobot); + smoothedVyRobot = smoothedVyRobot + VEL_SMOOTH_ALPHA * (rawVyRobot - smoothedVyRobot); + smoothedOmega = smoothedOmega + VEL_SMOOTH_ALPHA * (rawOmega - smoothedOmega); + + // Update previous pose + prevX = x; + prevY = y; + prevHeading = heading; + prevPoseTime = now; + + dataRecorder.write(String.format(Locale.US, + "%.3f,%.4f,%.4f,%.4f,%.2f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n", + t, x, y, heading, voltage, + smoothedVxRobot, smoothedVyRobot, smoothedOmega, + robot.intakeMotor.getPower(), + robot.loaderMotor.getPower(), + robot.leftOuttake.getPower(), + robot.rightOuttake.getPower(), + robot.turretServo.getPosition(), + robot.angleServo.getPosition(), + robot.flapsServo.getPosition() + )); + } + + private static double normalizeAngle(double a) { + while (a > Math.PI) a -= 2 * Math.PI; + while (a < -Math.PI) a += 2 * Math.PI; + return a; + } + + public void _telemetry() { + telemetry.addData("LPS", "%.1f", 1 / lpsCounter.delta); + telemetry.addData("Recording", "V4 ACTIVE (robot-frame velocities)"); + telemetry.addData("x", robot.follower.getPose().getX()); + telemetry.addData("y", robot.follower.getPose().getY()); + telemetry.addData("heading", Math.toDegrees(robot.follower.getPose().getHeading())); + telemetry.addData("robot V", "%.1f, %.1f, %.1f°/s", smoothedVxRobot, smoothedVyRobot, Math.toDegrees(smoothedOmega)); + telemetry.addData("shooter vel", robot.leftOuttake.getVelocity()); + telemetry.addData("turret pos", robot.turretServo.getPosition()); + robot.intake.telemetry(telemetry); + robot.loader.telemetry(telemetry); + robot.outtake.telemetry(telemetry); + drivingGP.telemetry(telemetry); + telemetry.update(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp7.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp7.java new file mode 100644 index 0000000..d0b15c9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp7.java @@ -0,0 +1,283 @@ +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; + +/** + * Data Recorder V7 — Driver Input Feedforward + Continuous Blending Replay + * + * Records the ACTUAL DRIVER INPUTS (gamepad stick values) that caused the motion, + * plus pose for correction reference. + * + * CSV Format: + * Time,X,Y,Heading,Voltage, + * GamepadFwd,GamepadStr,GamepadTurn, // [-1, 1] robot-frame stick values + * IntakePwr,LoaderPwr,LShooterPwr,RShooterPwr, + * TurretPos,AnglePos,FlapPos + * + * @version 7.0 + */ +@TeleOp(name = "Data Recorder V7", group = "Replay") +public class DataRecordingOp7 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 + private FileWriter dataRecorder; + private static final long RECORD_INTERVAL_MS = 20; + private long startTime; + private long lastRecordTime = 0; + + // Wheel velocity recording (optional debug) + 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); + } + + 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); + } + + // Get motors and LOG THEIR DIRECTIONS for replay verification + leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); + rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); + rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); + + telemetry.addLine("=== COPY THESE DIRECTIONS TO REPLAY CODE ==="); + telemetry.addData("LF", leftFront.getDirection().toString()); + telemetry.addData("RF", rightFront.getDirection().toString()); + telemetry.addData("LR", leftRear.getDirection().toString()); + telemetry.addData("RR", rightRear.getDirection().toString()); + telemetry.addLine("============================================"); + telemetry.update(); + + String filePath = Environment.getExternalStorageDirectory().getPath() + "/robot_data_v7.csv"; + try { + dataRecorder = new FileWriter(filePath); + dataRecorder.write("Time,X,Y,Heading,Voltage,GamepadFwd,GamepadStr,GamepadTurn,IntakePwr,LoaderPwr,LShooterPwr,RShooterPwr,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 V7)"); + telemetry.addLine("Verify motor directions above match replay code!"); + telemetry.update(); + } + + @Override + public void start() { + robot.follower.startTeleopDrive(); + startTime = System.currentTimeMillis(); + } + + @Override + public void loop() { + long now = System.currentTimeMillis(); + lpsCounter.getLoopTime(); + + drivingGP.update(); + utilityGP.update(); + robot.follower.update(); + + // ===== CAPTURE DRIVER INPUTS (the feedforward signal) ===== + // These are the EXACT values passed to setTeleOpDrive + // Note: setTeleOpDrive takes (forward, strafe, turn, robotCentric=true) + // We negate Y because gamepad Y is up=negative in FTC + double gamepadFwd = -drivingGP.leftStick.y; // forward/back + double gamepadStr = -drivingGP.leftStick.x; // strafe + double gamepadTurn = -drivingGP.rightStick.x; // turn + + // ===== MECHANISM CONTROL (same as before) ===== + robot.intake.speed = utilityGP.rightStick.y; + robot.intake.reversed = INTAKE_REVERSE; + turretAligner.update(); + + 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; + } + + // Turret/Angle aiming + if (autoAimEnabled) { + // To do + } else { + if (drivingGP.dpadLeft.pressed()) { + if (turretTimer.seconds() == 0) turretTimer.reset(); + double increment = turretTimer.seconds() > 1.5 ? 0.1 : + turretTimer.seconds() > 1.0 ? 0.07 : 0.03; + robot.turret.driverOffset += increment; + } else if (drivingGP.dpadRight.pressed()) { + if (turretTimer.seconds() == 0) turretTimer.reset(); + double decrement = turretTimer.seconds() > 1.5 ? 0.1 : + turretTimer.seconds() > 1.0 ? 0.07 : 0.03; + robot.turret.driverOffset -= decrement; + } else { + turretTimer.reset(); + } + + if (drivingGP.dpadUp.pressed()) + robot.outtake.activeConfig.angle += 0.01; + else if (drivingGP.dpadDown.pressed()) + robot.outtake.activeConfig.angle -= 0.01; + } + + // Shoot presets + 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); + } + + // Rumble when shooter ready + 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; + } + } + + // Pass inputs to PedroPathing (same as always) + robot.follower.setTeleOpDrive(gamepadFwd, gamepadStr, gamepadTurn, true); + robot.updateAllSystems(); + + // ===== RECORD DATA ===== + if (now - lastRecordTime >= RECORD_INTERVAL_MS) { + try { + recordData(now, gamepadFwd, gamepadStr, gamepadTurn); + } catch (IOException e) { + telemetry.addData("Recording Error", e.getMessage()); + } + lastRecordTime = now; + } + + _telemetry(gamepadFwd, gamepadStr, gamepadTurn); + } + + @Override + public void stop() { + robot.webcam.stop(); + if (dataRecorder != null) { + try { + dataRecorder.flush(); + dataRecorder.close(); + } catch (IOException ignored) {} + } + } + + private void recordData(long now, double gpFwd, double gpStr, double gpTurn) throws IOException { + double t = (now - startTime) / 1000.0; + double x = robot.follower.getPose().getX(); + double y = robot.follower.getPose().getY(); + double heading = robot.follower.getHeading(); + double voltage = hardwareMap.voltageSensor.iterator().next().getVoltage(); + + dataRecorder.write(String.format(Locale.US, + "%.3f,%.4f,%.4f,%.4f,%.2f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n", + t, x, y, heading, voltage, + gpFwd, gpStr, gpTurn, + robot.intakeMotor.getPower(), + robot.loaderMotor.getPower(), + robot.leftOuttake.getPower(), + robot.rightOuttake.getPower(), + robot.turretServo.getPosition(), + robot.angleServo.getPosition(), + robot.flapsServo.getPosition() + )); + } + + public void _telemetry(double gpFwd, double gpStr, double gpTurn) { + telemetry.addData("LPS", "%.1f", 1 / lpsCounter.delta); + telemetry.addData("Recording", "V7 ACTIVE (input feedforward)"); + telemetry.addData("Inputs", "fwd=%.2f str=%.2f turn=%.2f", gpFwd, gpStr, gpTurn); + telemetry.addData("x", robot.follower.getPose().getX()); + telemetry.addData("y", robot.follower.getPose().getY()); + telemetry.addData("heading", Math.toDegrees(robot.follower.getPose().getHeading())); + telemetry.addData("shooter vel", robot.leftOuttake.getVelocity()); + telemetry.addData("turret pos", robot.turretServo.getPosition()); + robot.intake.telemetry(telemetry); + robot.loader.telemetry(telemetry); + robot.outtake.telemetry(telemetry); + drivingGP.telemetry(telemetry); + telemetry.update(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp8.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp8.java new file mode 100644 index 0000000..2375529 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp8.java @@ -0,0 +1,302 @@ +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; + +/** + * Data Recorder V8 — Exact Match to MainDrivingOp + * + * Controls and mechanisms behave IDENTICALLY to MainDrivingOp. + * Records driver inputs + pose + all mechanism states. + * + * CSV Format: + * Time,X,Y,Heading,Voltage, + * GamepadFwd,GamepadStr,GamepadTurn, + * IntakePwr,LoaderPwr,LShooterPwr,RShooterPwr, + * TurretPos,AnglePos,FlapPos, + * AutoAimEnabled,BlueTarget + */ +@TeleOp(name = "Data Recorder V8", group = "Replay") +public class DataRecordingOp8 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 + private FileWriter dataRecorder; + private static final long RECORD_INTERVAL_MS = 20; + private long startTime; + private long lastRecordTime = 0; + + // Wheel velocity recording (optional debug) + 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); + } + + 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); + } + + // Get motors and LOG THEIR DIRECTIONS for replay verification + leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); + rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); + rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); + + telemetry.addLine("=== COPY THESE DIRECTIONS TO REPLAY CODE ==="); + telemetry.addData("LF", leftFront.getDirection().toString()); + telemetry.addData("RF", rightFront.getDirection().toString()); + telemetry.addData("LR", leftRear.getDirection().toString()); + telemetry.addData("RR", rightRear.getDirection().toString()); + telemetry.addLine("============================================"); + telemetry.update(); + + String filePath = Environment.getExternalStorageDirectory().getPath() + "/robot_data_v8.csv"; + try { + dataRecorder = new FileWriter(filePath); + dataRecorder.write("Time,X,Y,Heading,Voltage,GamepadFwd,GamepadStr,GamepadTurn,IntakePwr,LoaderPwr,LShooterPwr,RShooterPwr,TurretPos,AnglePos,FlapPos,AutoAim,BlueTarget\n"); + } catch (IOException e) { + telemetry.addData("Error initializing recorder", e.getMessage()); + } + } + + @Override + public void init_loop() { + lpsCounter.getLoopTime(); + telemetry.addLine("Initialization Ready (Recording V8)"); + telemetry.addLine("Verify motor directions above match replay code!"); + telemetry.update(); + } + + @Override + public void start() { + robot.follower.startTeleopDrive(); + startTime = System.currentTimeMillis(); + } + + @Override + public void loop() { + long now = System.currentTimeMillis(); + lpsCounter.getLoopTime(); + + drivingGP.update(); + utilityGP.update(); + robot.follower.update(); + + // ===== CAPTURE DRIVER INPUTS (the feedforward signal) ===== + // EXACTLY as passed to setTeleOpDrive in MainDrivingOp + double gamepadFwd = -drivingGP.leftStick.y; + double gamepadStr = -drivingGP.leftStick.x; + double gamepadTurn = -drivingGP.rightStick.x; + + // ===== MECHANISM CONTROL — EXACT COPY OF 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/Angle aiming — EXACT copy from MainDrivingOp + 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(); + } + + // Auto-aim toggles — EXACT copy from MainDrivingOp + if (drivingGP.dpadDown.justPressed()) + robot.turret.autoAimEnabled = !robot.turret.autoAimEnabled; + + if (drivingGP.dpadUp.justPressed()) + autoAimEnabled = !autoAimEnabled; + + if (autoAimEnabled) + robot.shoot.activateRange(0); + + // Shoot presets — EXACT copy from MainDrivingOp + if (drivingGP.triangle.justPressed()) { + robot.shoot.activateRange(1); + } + if (drivingGP.square.justPressed()) { + robot.shoot.activateRange(2); + } + if (drivingGP.cross.justPressed()) { + robot.shoot.activateRange(3); + } + if (drivingGP.circle.justPressed()) { + robot.shoot.activateRange(4); + } + + // Shooter ready rumble — EXACT copy from MainDrivingOp + 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; + } + + // Left bumper — EXACT copy from MainDrivingOp + if (!autoAimEnabled && drivingGP.leftBumper.justPressed()) { + robot.turret.autoAimEnabled = true; + if (robot.outtake.on) { + robot.shoot.deactivate(); + gamepad1.rumble(1, 1, 100); + rumbled = false; + } + } + + // Blue target toggle — EXACT copy from MainDrivingOp + if (drivingGP.rightStick.button.justPressed()) + robot.Blue_Target = !robot.Blue_Target; + + // Update robot systems — EXACT copy from MainDrivingOp + robot.follower.setTeleOpDrive(gamepadFwd, gamepadStr, gamepadTurn, true); + robot.updateAllSystems(); + + // ===== RECORD DATA ===== + if (now - lastRecordTime >= RECORD_INTERVAL_MS) { + try { + recordData(now, gamepadFwd, gamepadStr, gamepadTurn); + } catch (IOException e) { + telemetry.addData("Recording Error", e.getMessage()); + } + lastRecordTime = now; + } + + _telemetry(gamepadFwd, gamepadStr, gamepadTurn); + } + + @Override + public void stop() { + robot.webcam.stop(); + if (dataRecorder != null) { + try { + dataRecorder.flush(); + dataRecorder.close(); + } catch (IOException ignored) {} + } + } + + private void recordData(long now, double gpFwd, double gpStr, double gpTurn) throws IOException { + double t = (now - startTime) / 1000.0; + double x = robot.follower.getPose().getX(); + double y = robot.follower.getPose().getY(); + double heading = robot.follower.getHeading(); + double voltage = hardwareMap.voltageSensor.iterator().next().getVoltage(); + + dataRecorder.write(String.format(Locale.US, + "%.3f,%.4f,%.4f,%.4f,%.2f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%d,%d\n", + t, x, y, heading, voltage, + gpFwd, gpStr, gpTurn, + robot.intakeMotor.getPower(), + robot.loaderMotor.getPower(), + robot.leftOuttake.getPower(), + robot.rightOuttake.getPower(), + robot.turretServo.getPosition(), + robot.angleServo.getPosition(), + robot.flapsServo.getPosition(), + autoAimEnabled ? 1 : 0, + robot.Blue_Target ? 1 : 0 + )); + } + + public void _telemetry(double gpFwd, double gpStr, double gpTurn) { + telemetry.addData("LPS", "%.1f", 1 / lpsCounter.delta); + telemetry.addData("Recording", "V8 ACTIVE (input feedforward)"); + telemetry.addData("Inputs", "fwd=%.2f str=%.2f turn=%.2f", gpFwd, gpStr, gpTurn); + telemetry.addData("x", robot.follower.getPose().getX()); + telemetry.addData("y", robot.follower.getPose().getY()); + telemetry.addData("heading", Math.toDegrees(robot.follower.getPose().getHeading())); + telemetry.addData("shooter vel", robot.leftOuttake.getVelocity()); + telemetry.addData("turret pos", robot.turretServo.getPosition()); + telemetry.addData("autoAim", autoAimEnabled); + telemetry.addData("BlueTarget", robot.Blue_Target); + robot.intake.telemetry(telemetry); + robot.loader.telemetry(telemetry); + robot.outtake.telemetry(telemetry); + drivingGP.telemetry(telemetry); + telemetry.update(); + } +} From d35e4144ded617fc3809f0bc35c2291a11dab645 Mon Sep 17 00:00:00 2001 From: Robi2903 <113847997+Robi2903@users.noreply.github.com> Date: Tue, 21 Jul 2026 15:05:55 +0300 Subject: [PATCH 2/6] VLAAAAAAAAD, GET TOOOO WOOOORK --- .../kronbot/autonomous/FinalReplayOp.java | 567 ++++++++++++++++++ .../kronbot/autonomous/ReplayAuto3_3.java | 326 ++++++++++ .../manual/DataRecordingOpV3FIXED.java | 375 ++++++++++++ .../kronbot/manual/FinalRecorderOp.java | 360 +++++++++++ 4 files changed, 1628 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/FinalReplayOp.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAuto3_3.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOpV3FIXED.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/FinalRecorderOp.java 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..fca49d5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/FinalReplayOp.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 = "Final Replay", group = "Autonomous") +public class FinalReplayOp extends LinearOpMode { + + private static final String CSV_PATH = "/sdcard/robot_data.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/autonomous/ReplayAuto3_3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAuto3_3.java new file mode 100644 index 0000000..954de1a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAuto3_3.java @@ -0,0 +1,326 @@ +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 Auto V4 — simplified, accurate point‑following. + * + * Removed: + * - D‑term (and all filtering / smoothing) + * - Lookahead (follows the exact interpolated pose) + * - Velocity ramp (start/stop transitions are smooth enough with P only) + * + * Kept: + * - Voltage‑based time scaling (compensates for battery sag without exceeding 1.0 power) + * - Frame interpolation (lerp between adjacent recorded frames) + * - Mechanism replay (intake/loader/shooter via velocity → power) + * + * Use this if V3 was overshooting, oscillating, or lagging behind. + */ +@Autonomous(name = "FINAL REPLAY LETS GO", group = "Autonomous") +public class ReplayAuto3_3 extends LinearOpMode { + + private static final String CSV_PATH = "/sdcard/robot_data.csv"; + + // P gains (tune these on your robot) + private static final double MAX_POWER = 0.8; + private static final double kP_translation = 0.10; // cm⁻¹ + private static final double kP_rotation = 1.8; // rad⁻¹ + + // Time scaling: when battery is lower than recorded, we run the replay faster + private static final double TIME_SCALING_FACTOR = 0.7; + private static final double NOMINAL_VOLTAGE = 12.0; + private static final double VOLTAGE_REFRESH_SEC = 0.1; + + // Mechanism velocity → power scaling (same as before) + private static final double INTAKE_VEL_SCALE = 500.0; + private static final double LOADER_VEL_SCALE = 500.0; + private static final double SHOOTER_VEL_THRESHOLD = 50.0; + private static final double SHOOTER_MAX_VEL = 2000.0; + + private final Robot robot = Robot.getInstance(); + private final ElapsedTime runtime = new ElapsedTime(); + private final List recordedFrames = new ArrayList<>(); + + // Voltage caching + private double cachedVoltage = NOMINAL_VOLTAGE; + private double lastVoltageReadTime = -999; + private double recordedVoltage = NOMINAL_VOLTAGE; + private double timeScalingFactor = 1.0; + + // ------------------------------------------------------------------------- + // Data model (matches V3 CSV) + // ------------------------------------------------------------------------- + private static class RobotFrame { + double timestamp; + double lrPow, rrPow, lfPow, rfPow; // not used for driving, only stored + double x, y, heading; + double voltage; + double intakeVel, loaderVel, leftShtrVel, rightShtrVel; + 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]); + } + } + + @Override + public void runOpMode() { + telemetry.addLine("Initializing Replay Auto V4..."); + 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 playback 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; + + robot.follower.startTeleopDrive(); + + while (opModeIsActive() && idx < recordedFrames.size() - 1) { + double now = runtime.seconds(); + + // Refresh voltage (cached) + refreshVoltage(now); + updateTimeScaling(); + + // Current playback time (scaled) + double recordingTime = now / timeScalingFactor; + double targetTs = startTs + recordingTime; + + // Advance index to 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); + } + + double targetX = lerp(fA.x, fB.x, t); + double targetY = lerp(fA.y, fB.y, t); + double targetH = lerpAngle(fA.heading, fB.heading, t); + + // Get current pose + robot.follower.update(); + Pose cur = robot.follower.getPose(); + + // ---- Pure P control (no D, no lookahead) ---- + double ex = targetX - cur.getX(); + double ey = targetY - cur.getY(); + + // Field‑centric correction → robot‑centric + double cosH = Math.cos(cur.getHeading()); + double sinH = Math.sin(cur.getHeading()); + double fwdCmd = cosH * ex * kP_translation + sinH * ey * kP_translation; + double strCmd = -sinH * ex * kP_translation + cosH * ey * kP_translation; + + // Clamp translation magnitude + double norm = Math.hypot(fwdCmd, strCmd); + if (norm > MAX_POWER) { + fwdCmd *= MAX_POWER / norm; + strCmd *= MAX_POWER / norm; + } + + // Rotation + double eh = normalizeAngle(targetH - cur.getHeading()); + double turnCmd = Range.clip(eh * kP_rotation, -MAX_POWER, MAX_POWER); + + // Send to PedroPathing (no direct motor writes) + 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(); + + idle(); // yield to scheduler + } + } + + // ------------------------------------------------------------------------- + // Mechanisms (unchanged from V3) + // ------------------------------------------------------------------------- + private void controlMechanisms(RobotFrame a, RobotFrame b, double t) { + 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)); + + 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 { + double braking = robot.leftOuttake.getVelocity() > 21 ? -0.1 : 0.0; + robot.leftOuttake.setPower(braking); + robot.rightOuttake.setPower(braking); + } + + 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; + } + + 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/manual/DataRecordingOpV3FIXED.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOpV3FIXED.java new file mode 100644 index 0000000..a68d9d9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOpV3FIXED.java @@ -0,0 +1,375 @@ +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.RANGE_1_VELOCITY; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_2_VELOCITY; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_3_VELOCITY; +import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_4_VELOCITY; +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.2). + * + * 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 (22 total): + * Time, LR, RR, LF, RF, X, Y, Heading, Voltage, + * IntakeVel, LoaderVel, LeftShtrVel, RightShtrVel, + * TurretPos, AnglePos, FlapPos, + * IntakeCmd, LoaderCmd, FlapOpen, ShootRange, TurretOffset, BlueTarget + * + * The first 16 columns are the V3 mechanism-state log (for direct servo + * and velocity control). The last 6 columns are the high-level mechanism + * commands the TeleOp applied — these let the replay call the same + * high-level API (intake.speed, shoot.activateRange, etc.) and let + * robot.updateAllSystems() drive the motors, exactly like the TeleOp. + * + * V3.2 changes: + * - Fixed loader scalar: was * 0.9, now * 0.8 (matches MainDrivingOp) + * - Added rightStick.button toggle for Blue_Target (matches MainDrivingOp) + * - Removed turretAligner.update() from loop (TeleOp doesn't call it) + * - Webcam init commented out (matches MainDrivingOp) + * - Added 6 high-level mechanism command columns for replay parity + * + * @version 3.2 + */ +@TeleOp(name = "RECORDERRRR", group = "Replay") +public class DataRecordingOpV3FIXED 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; + + @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"); + + // Reset IMU for a clean starting pose — done before init_loop so + // the driver doesn't notice any difference from MainDrivingOp. + try { + robot.follower.getPoseTracker().resetIMU(); + } catch (InterruptedException e) { + throw new RuntimeException(e); + } + + // Initialize Data Recorder — V3.2 header (22 columns) + String filePath = Environment.getExternalStorageDirectory().getPath() + "/robot_data.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\n"); + } catch (IOException e) { + telemetry.addData("Error initializing recorder", e.getMessage()); + } + } + + @Override + public void init_loop() { + lpsCounter.getLoopTime(); + + telemetry.addLine("Initialization Ready (Recording V3.2 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(); + + // ----- 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); + //Shoot Close/Far + if (drivingGP.triangle.justPressed()) { + robot.shoot.activateRange(1); + } + if (drivingGP.square.justPressed()) { + robot.shoot.activateRange(2); + } + if (drivingGP.cross.justPressed()) { + robot.shoot.activateRange(3); + } + if (drivingGP.circle.justPressed()) { + 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; + } + } + + 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(); + + // ----- End of MainDrivingOp-identical block ----- + + // Record Data + if (now - lastRecordTime >= RECORD_INTERVAL_SEC) { + try { + recordData(now); + } 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.2", "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) 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.2 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 = deriveShootRange(); + double turretOffset = robot.turret.driverOffset; + int blueTarget = robot.Blue_Target ? 1 : 0; + + 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\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 + )); + } + + /** + * Derives the current shoot range number (1-4) from the active config. + * + * RangeConfig doesn't store the range number itself, and Outtake doesn't + * track which range is active — that info is only known inside + * Shoot.activateRange() at the moment of the call. So we recover it by + * matching activeConfig.velocity against the known RANGE_X_VELOCITY + * constants. Returns 0 for the auto-aim interpolated range (case 0 in + * Shoot.activateRange) and -1 when the outtake is off. + * + * Brittle: if you change RANGE_X_VELOCITY, the matching may break. + * Clean fix: add {@code public int activeRange = -1;} to Robot.Outtake + * and set it in Shoot.activateRange / deactivate, then read + * {@code robot.outtake.activeRange} here directly. + */ + private int deriveShootRange() { + if (!robot.outtake.on) return -1; + double vel = robot.outtake.activeConfig.velocity; + double eps = 1.0; // velocity tolerance in ticks/sec + if (Math.abs(vel - RANGE_1_VELOCITY) < eps) return 1; + if (Math.abs(vel - RANGE_2_VELOCITY) < eps) return 2; + if (Math.abs(vel - RANGE_3_VELOCITY) < eps) return 3; + if (Math.abs(vel - RANGE_4_VELOCITY) < eps) return 4; + return 0; // auto-aim interpolated range + } +} \ 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..92c0953 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/FinalRecorderOp.java @@ -0,0 +1,360 @@ +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.3). + * + * 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 (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 + * + * 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 = "FINAL Recorder", 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.3 header (23 columns) + String filePath = Environment.getExternalStorageDirectory().getPath() + "/robot_data.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\n"); + } catch (IOException e) { + telemetry.addData("Error initializing recorder", e.getMessage()); + } + } + + @Override + public void init_loop() { + lpsCounter.getLoopTime(); + + telemetry.addLine("Initialization Ready (Recording V3.3 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(); + + // ----- 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); + //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(-drivingGP.leftStick.y, -drivingGP.leftStick.x, -drivingGP.rightStick.x, true); + robot.updateAllSystems(); + + // ----- End of MainDrivingOp-identical block ----- + + // Record Data + if (now - lastRecordTime >= RECORD_INTERVAL_SEC) { + try { + recordData(now); + } 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.3", "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) 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; + + 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\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 + )); + } +} \ No newline at end of file From 71009562326e01acf62e33616d6b6f2f229b88fc Mon Sep 17 00:00:00 2001 From: Cozma Vlad Date: Thu, 23 Jul 2026 16:44:29 +0300 Subject: [PATCH 3/6] Nu merge 100% bile, dar suntem aproape --- .../kronbot/autonomous/FinalReplayOp.java | 553 ++++++++---------- .../kronbot/manual/FinalRecorderOp.java | 48 +- 2 files changed, 271 insertions(+), 330 deletions(-) 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 index fca49d5..e40f05c 100644 --- 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 @@ -18,224 +18,106 @@ import java.util.List; /** - * Replay Autonomous — V3.3 + * Replay Autonomous — V3.4 (Feedforward + Odometry Correction) * - * 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 + * Reads the V3.4 CSV format produced by FinalRecorderOp. Older V3.3 CSVs + * still load; drive feedforward is approximated from recorded wheel powers. * - * 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. + * 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 = "Final Replay", group = "Autonomous") +@Autonomous(name = "FINAL Replay V3.4", group = "Replay") public class FinalReplayOp extends LinearOpMode { - private static final String CSV_PATH = "/sdcard/robot_data.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) + // Configuration // ------------------------------------------------------------------------- - 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; + private static final String CSV_PATH = "/sdcard/robot_data.csv"; - 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"); - } - } - } + // Playback tuning + private static final double LOOKAHEAD_TIME = 0.080; // 80ms 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 + + // PD Translation (x, y). These stay low because recorded driver input is + // the main motion command; PD only corrects odometry error. + public static double kP_translation = 0.018; + public static double kD_translation = 0.010; + + // PD Rotation (heading) + public static double kP_rotation = 0.28; + public static double kD_rotation = 0.07; + + // Constraints + private static final double BLEND_K = 0.35; + private static final double MIN_CORRECTION_CAP = 0.20; + private static final double MAX_CORRECTION_CAP = 0.60; + private static final double CORRECTION_ERROR_SCALE = 8.0; + private static final double D_FILTER_ALPHA = 0.45; + 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; // ------------------------------------------------------------------------- - // OpMode + // 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() { - telemetry.addLine("Initializing Replay Auto V3.3..."); - telemetry.update(); - - robot.initFollower(hardwareMap, true); robot.init(hardwareMap); + robot.initFollower(hardwareMap, false); - try { - robot.follower.getPoseTracker().resetIMU(); - } catch (InterruptedException e) { - telemetry.addLine("IMU Reset Interrupted"); - } + telemetry.addData("Status", "Loading CSV..."); + telemetry.update(); try { loadRecordedData(); + calculateRecordedVoltage(); if (recordedFrames.isEmpty()) { - telemetry.addLine("ERROR: No recorded data found!"); + telemetry.addData("ERROR", "CSV is empty!"); 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.addData("Status", "Loaded %d frames. Ready.", recordedFrames.size()); 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(); + // Wait for start + while (!isStarted() && !isStopRequested()) { idle(); } - robot.follower.setPose(new Pose(first.x, first.y, first.heading)); + + 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(); @@ -271,14 +153,12 @@ private void executePlayback() { filteredDx = 0; filteredDy = 0; filteredDh = 0; - prevShootRange = -2; // sentinel: no transitions fire on frame 0 + prevShootRange = -2; prevAutoAim = false; - RobotFrame firstFrame = recordedFrames.get(0); - prevTargetX = firstFrame.x; - prevTargetY = firstFrame.y; - targetWasStill = true; - motionStartTime = 0; + prevRobotFwd = 0; + prevRobotStr = 0; + prevRobotTurn = 0; robot.follower.startTeleopDrive(); @@ -288,8 +168,13 @@ private void executePlayback() { refreshVoltage(now); updateTimeScaling(); - // Constant ROBOT-time lookahead - double recordingTime = (now / timeScalingFactor) + LOOKAHEAD_TIME * timeScalingFactor; + 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 @@ -311,94 +196,96 @@ private void executePlayback() { double targetY = lerp(fA.y, fB.y, t); double targetH = lerpAngle(fA.heading, fB.heading, t); - // Update localizer + 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); - // --- PD (no feedforward) --- - double dt = now - prevTime; - if (dt <= 0) dt = 1e-6; + double exField = targetX - cur.getX(); + double eyField = targetY - cur.getY(); + double eh = normalizeAngle(targetH - curH); - double ex = targetX - cur.getX(); - double ey = targetY - cur.getY(); + double cosH = Math.cos(curH); + double sinH = Math.sin(curH); + double exRobot = cosH * exField + sinH * eyField; + double eyRobot = -sinH * exField + cosH * eyField; - double rawDx = (ex - prevErrorX) / dt; - double rawDy = (ey - prevErrorY) / dt; + double rawDx = (exRobot - prevErrorX) / dt; + double rawDy = (eyRobot - prevErrorY) / dt; + double rawDh = (eh - prevErrorHeading) / dt; filteredDx = filteredDx + D_FILTER_ALPHA * (rawDx - filteredDx); filteredDy = filteredDy + D_FILTER_ALPHA * (rawDy - filteredDy); + filteredDh = filteredDh + D_FILTER_ALPHA * (rawDh - filteredDh); - 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 corrFwd = exRobot * kP_translation + filteredDx * kD_translation; + double corrStr = eyRobot * kP_translation + filteredDy * kD_translation; + double corrTurn = eh * kP_rotation + filteredDh * kD_rotation; - 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 posError = Math.hypot(exField, eyField); + double corrScale = Math.min(posError / CORRECTION_ERROR_SCALE, 1.0); + double dynamicMaxCorr = lerp(MIN_CORRECTION_CAP, MAX_CORRECTION_CAP, corrScale); - double norm = Math.hypot(fwdCmd, strCmd); - if (norm > MAX_POWER) { - fwdCmd *= MAX_POWER / norm; - strCmd *= MAX_POWER / norm; + double corrMag = Math.hypot(corrFwd, corrStr); + if (corrMag > dynamicMaxCorr) { + corrFwd *= dynamicMaxCorr / corrMag; + corrStr *= dynamicMaxCorr / corrMag; } + corrTurn = Range.clip(corrTurn, -dynamicMaxCorr, dynamicMaxCorr); - 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); + double corrWeight = Range.clip(1.0 - Math.exp(-BLEND_K * posError), 0.0, 1.0); - fwdCmd *= ramp; - strCmd *= ramp; + double robotFwd = ffFwd + corrWeight * corrFwd; + double robotStr = ffStr + corrWeight * corrStr; + double robotTurn = ffTurn + corrWeight * corrTurn; - // Update PD state - prevErrorX = ex; - prevErrorY = ey; + 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(fwdCmd, strCmd, turnCmd, false); + robot.follower.setTeleOpDrive(robotFwd, robotStr, robotTurn, true); - // --- Apply recorded mechanism commands the same way the TeleOp does --- 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 (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("Time", "%.2f / %.2f s", now, duration); + telemetry.addData("Lookahead", "%.3f s", lookahead); + telemetry.addData("PosErr", "%.2f cm", posError); 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", + 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", - lerp(fA.intakeCmd, fB.intakeCmd, t), - lerp(fA.loaderCmd, fB.loaderCmd, t)); + lerpBool(fA.flapOpen, fB.flapOpen, t) ? "Y" : "N"); telemetry.update(); idle(); @@ -406,12 +293,9 @@ private void executePlayback() { } // ------------------------------------------------------------------------- - // Mechanism application — same code path as MainDrivingOp + // Mechanism application // ------------------------------------------------------------------------- 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; @@ -419,22 +303,21 @@ private void applyInitialMechanismState(RobotFrame f) { 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; + // Force auto-aim off for Replay; we use recorded positions. + robot.turret.autoAimEnabled = false; + prevAutoAim = false; - // 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) { + 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); - } else if (f.shootRange > 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; @@ -442,55 +325,32 @@ private void applyInitialMechanismState(RobotFrame f) { 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. + // Ensure auto-aim is off. + robot.turret.autoAimEnabled = false; + 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). + // 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) { - // positive → -1 transition: matches the TeleOp's - // leftBumper.justPressed() → robot.shoot.deactivate(). robot.shoot.deactivate(); } } @@ -500,7 +360,9 @@ private void applyMechanismCommands(RobotFrame a, RobotFrame b, double t) { private void stopMechanisms() { robot.intake.speed = 0; robot.loader.speed = 0; + robot.flap.open = false; robot.shoot.deactivate(); + robot.updateAllSystems(); } // ------------------------------------------------------------------------- @@ -512,14 +374,10 @@ private void loadRecordedData() throws IOException { 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); } } } @@ -564,4 +422,69 @@ private static double normalizeAngle(double a) { while (a < -Math.PI) a += 2 * Math.PI; return a; } -} \ No newline at end of file + + 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/manual/FinalRecorderOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/FinalRecorderOp.java index 92c0953..b0d592c 100644 --- 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 @@ -23,18 +23,18 @@ import java.util.Locale; /** - * TeleOp data recorder (V3.3). + * 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 (23 total): + * 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 + * AutoAimEnabled, DriveFwd, DriveStr, DriveTurn, OuttakeKs * * V3.3 changes over V3.2: * - Removed resetIMU() from init() to match MainDrivingOp exactly. The @@ -52,7 +52,14 @@ * with the exact same arguments the driver used, instead of trying * to reverse-engineer the range from activeConfig.velocity. * - * @version 3.3 + * 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 = "FINAL Recorder", group = "Replay") public class FinalRecorderOp extends OpMode { @@ -118,11 +125,11 @@ public void init() { // 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) + // Initialize Data Recorder — V3.4 header (27 columns) String filePath = Environment.getExternalStorageDirectory().getPath() + "/robot_data.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\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,DriveFwd,DriveStr,DriveTurn,OuttakeKs\n"); } catch (IOException e) { telemetry.addData("Error initializing recorder", e.getMessage()); } @@ -132,7 +139,7 @@ public void init() { public void init_loop() { lpsCounter.getLoopTime(); - telemetry.addLine("Initialization Ready (Recording V3.3 Enabled)"); + telemetry.addLine("Initialization Ready (Recording V3.4 Enabled)"); telemetry.update(); } @@ -153,6 +160,10 @@ public void loop() { 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 @@ -210,8 +221,10 @@ else if (robot.loader.speed < -0.2) if(drivingGP.dpadUp.justPressed()) autoAimEnabled=!autoAimEnabled; - if(autoAimEnabled) + if(autoAimEnabled) { robot.shoot.activateRange(0); + lastActivateRange = 0; + } //Shoot Close/Far if (drivingGP.triangle.justPressed()) { robot.shoot.activateRange(1); @@ -251,7 +264,7 @@ else if (robot.loader.speed < -0.2) robot.Blue_Target = !robot.Blue_Target; //Update robot systems status - robot.follower.setTeleOpDrive(-drivingGP.leftStick.y, -drivingGP.leftStick.x, -drivingGP.rightStick.x, true); + robot.follower.setTeleOpDrive(driveFwd, driveStr, driveTurn, true); robot.updateAllSystems(); // ----- End of MainDrivingOp-identical block ----- @@ -259,7 +272,7 @@ else if (robot.loader.speed < -0.2) // Record Data if (now - lastRecordTime >= RECORD_INTERVAL_SEC) { try { - recordData(now); + recordData(now, driveFwd, driveStr, driveTurn); } catch (IOException e) { telemetry.addData("Recording Error", e.getMessage()); } @@ -282,7 +295,7 @@ public void stop() { public void _telemetry() { telemetry.addData("LPS", "%.1f", 1 / lpsCounter.delta); - telemetry.addData("Recording V3.3", "ACTIVE"); + 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()); @@ -301,7 +314,7 @@ public void _telemetry() { telemetry.update(); } - private void recordData(double t) throws IOException { + 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(); @@ -329,9 +342,10 @@ private void recordData(double t) throws IOException { 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\n", + "%.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(), @@ -354,7 +368,11 @@ private void recordData(double t) throws IOException { shootRange, turretOffset, blueTarget, - autoAim + autoAim, + driveFwd, + driveStr, + driveTurn, + outtakeKs )); } -} \ No newline at end of file +} From 019a9e93f75fcab41c2f84696301e07298037a15 Mon Sep 17 00:00:00 2001 From: Cozma Vlad Date: Fri, 24 Jul 2026 13:14:53 +0300 Subject: [PATCH 4/6] Update Record and Replay --- .../src/main/AndroidManifest.xml | 4 +- .../kronbot/autonomous/FinalReplayOp.java | 4 +- .../kronbot/autonomous/OldReplayOp2.java | 341 ----------- .../kronbot/autonomous/ReplayAuto3_3.java | 326 ---------- .../kronbot/autonomous/ReplayAutoOp3.java | 451 -------------- .../kronbot/autonomous/ReplayAutoOp4.java | 528 ---------------- .../kronbot/autonomous/ReplayAutoOp7.java | 495 --------------- .../kronbot/autonomous/ReplayAutoOp8.java | 576 ------------------ .../kronbot/autonomous/ReplayOpRob.java | 567 +++++++++++++++++ .../kronbot/manual/DataRecordingOp2.java | 326 ---------- .../kronbot/manual/DataRecordingOp3.java | 322 ---------- .../kronbot/manual/DataRecordingOp4.java | 319 ---------- .../kronbot/manual/DataRecordingOp7.java | 283 --------- .../kronbot/manual/DataRecordingOp8.java | 302 --------- .../kronbot/manual/FinalRecorderOp.java | 4 +- ...rdingOpV3FIXED.java => RecorderOpRob.java} | 113 ++-- 16 files changed, 622 insertions(+), 4339 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/OldReplayOp2.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAuto3_3.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAutoOp3.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAutoOp4.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAutoOp7.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAutoOp8.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayOpRob.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp2.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp3.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp4.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp7.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp8.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/{DataRecordingOpV3FIXED.java => RecorderOpRob.java} (77%) 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/FinalReplayOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/FinalReplayOp.java index e40f05c..24adbdd 100644 --- 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 @@ -28,7 +28,7 @@ * 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 = "FINAL Replay V3.4", group = "Replay") +@Autonomous(name = "Replay Vlad", group = "Replay") public class FinalReplayOp extends LinearOpMode { private final Robot robot = Robot.getInstance(); @@ -36,7 +36,7 @@ public class FinalReplayOp extends LinearOpMode { // ------------------------------------------------------------------------- // Configuration // ------------------------------------------------------------------------- - private static final String CSV_PATH = "/sdcard/robot_data.csv"; + private static final String CSV_PATH = "/sdcard/robot_data_Vlad.csv"; // Playback tuning private static final double LOOKAHEAD_TIME = 0.080; // 80ms lookahead while moving 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/ReplayAuto3_3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAuto3_3.java deleted file mode 100644 index 954de1a..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAuto3_3.java +++ /dev/null @@ -1,326 +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 Auto V4 — simplified, accurate point‑following. - * - * Removed: - * - D‑term (and all filtering / smoothing) - * - Lookahead (follows the exact interpolated pose) - * - Velocity ramp (start/stop transitions are smooth enough with P only) - * - * Kept: - * - Voltage‑based time scaling (compensates for battery sag without exceeding 1.0 power) - * - Frame interpolation (lerp between adjacent recorded frames) - * - Mechanism replay (intake/loader/shooter via velocity → power) - * - * Use this if V3 was overshooting, oscillating, or lagging behind. - */ -@Autonomous(name = "FINAL REPLAY LETS GO", group = "Autonomous") -public class ReplayAuto3_3 extends LinearOpMode { - - private static final String CSV_PATH = "/sdcard/robot_data.csv"; - - // P gains (tune these on your robot) - private static final double MAX_POWER = 0.8; - private static final double kP_translation = 0.10; // cm⁻¹ - private static final double kP_rotation = 1.8; // rad⁻¹ - - // Time scaling: when battery is lower than recorded, we run the replay faster - private static final double TIME_SCALING_FACTOR = 0.7; - private static final double NOMINAL_VOLTAGE = 12.0; - private static final double VOLTAGE_REFRESH_SEC = 0.1; - - // Mechanism velocity → power scaling (same as before) - private static final double INTAKE_VEL_SCALE = 500.0; - private static final double LOADER_VEL_SCALE = 500.0; - private static final double SHOOTER_VEL_THRESHOLD = 50.0; - private static final double SHOOTER_MAX_VEL = 2000.0; - - private final Robot robot = Robot.getInstance(); - private final ElapsedTime runtime = new ElapsedTime(); - private final List recordedFrames = new ArrayList<>(); - - // Voltage caching - private double cachedVoltage = NOMINAL_VOLTAGE; - private double lastVoltageReadTime = -999; - private double recordedVoltage = NOMINAL_VOLTAGE; - private double timeScalingFactor = 1.0; - - // ------------------------------------------------------------------------- - // Data model (matches V3 CSV) - // ------------------------------------------------------------------------- - private static class RobotFrame { - double timestamp; - double lrPow, rrPow, lfPow, rfPow; // not used for driving, only stored - double x, y, heading; - double voltage; - double intakeVel, loaderVel, leftShtrVel, rightShtrVel; - 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]); - } - } - - @Override - public void runOpMode() { - telemetry.addLine("Initializing Replay Auto V4..."); - 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 playback 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; - - robot.follower.startTeleopDrive(); - - while (opModeIsActive() && idx < recordedFrames.size() - 1) { - double now = runtime.seconds(); - - // Refresh voltage (cached) - refreshVoltage(now); - updateTimeScaling(); - - // Current playback time (scaled) - double recordingTime = now / timeScalingFactor; - double targetTs = startTs + recordingTime; - - // Advance index to 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); - } - - double targetX = lerp(fA.x, fB.x, t); - double targetY = lerp(fA.y, fB.y, t); - double targetH = lerpAngle(fA.heading, fB.heading, t); - - // Get current pose - robot.follower.update(); - Pose cur = robot.follower.getPose(); - - // ---- Pure P control (no D, no lookahead) ---- - double ex = targetX - cur.getX(); - double ey = targetY - cur.getY(); - - // Field‑centric correction → robot‑centric - double cosH = Math.cos(cur.getHeading()); - double sinH = Math.sin(cur.getHeading()); - double fwdCmd = cosH * ex * kP_translation + sinH * ey * kP_translation; - double strCmd = -sinH * ex * kP_translation + cosH * ey * kP_translation; - - // Clamp translation magnitude - double norm = Math.hypot(fwdCmd, strCmd); - if (norm > MAX_POWER) { - fwdCmd *= MAX_POWER / norm; - strCmd *= MAX_POWER / norm; - } - - // Rotation - double eh = normalizeAngle(targetH - cur.getHeading()); - double turnCmd = Range.clip(eh * kP_rotation, -MAX_POWER, MAX_POWER); - - // Send to PedroPathing (no direct motor writes) - 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(); - - idle(); // yield to scheduler - } - } - - // ------------------------------------------------------------------------- - // Mechanisms (unchanged from V3) - // ------------------------------------------------------------------------- - private void controlMechanisms(RobotFrame a, RobotFrame b, double t) { - 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)); - - 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 { - double braking = robot.leftOuttake.getVelocity() > 21 ? -0.1 : 0.0; - robot.leftOuttake.setPower(braking); - robot.rightOuttake.setPower(braking); - } - - 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; - } - - 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/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/ReplayAutoOp4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAutoOp4.java deleted file mode 100644 index 957967b..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAutoOp4.java +++ /dev/null @@ -1,528 +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.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.Collections; -import java.util.List; - -/** - * Replay Auto V5 — Corrected Trajectory Follower - * - * FIXES from V4: - * 1. Velocities are recorded in ROBOT FRAME during TeleOp — no rotation needed in replay - * 2. Time scaling ONLY (no double velocity scaling) - * 3. Lookahead + feedforward use same time base (target frame velocity) - * 4. Slew rate limiting on motor commands (acceleration limits) - * 5. Correction is rotated into robot frame before application - * 6. Both feedforward and correction are in robot frame throughout - * - * ARCHITECTURE: - * 1. Recorded (vxRobot, vyRobot, omega) are smoothed real-time robot-frame velocities - * 2. Auto-calibrate maxLinearVel / maxAngularVel from recording - * 3. Scale velocities to [-1, 1] power units BEFORE mecanum mixing - * 4. Blend: 75% feedforward + 25% small PD correction (both in robot frame) - * 5. Normalize wheel powers ONLY if saturated (preserves ratios) - * 6. Time scaling ONLY for battery compensation (no velocity scaling) - * 7. Slew rate limiter prevents command jumps - * 8. Lookahead for smooth tracking - * - * CSV Format (from DataRecordingOp5): - * Time,X,Y,Heading,Voltage,VxRobot,VyRobot,Omega,IntakePwr,LoaderPwr,LShooterPwr,RShooterPwr,TurretPos,AnglePos,FlapPos - */ -@Autonomous(name = "Replay Auto V4", group = "Replay") -public class ReplayAutoOp4 extends LinearOpMode { - - private static final String CSV_PATH = "/sdcard/robot_data_v4.csv"; - - // ========== TUNABLE CONSTANTS ========== - - /** Feedforward weight: how much we trust recorded velocity vs correction. - * 0.75 = 75% feedforward, 25% correction. Tune 0.70–0.85. */ - private static final double FF_WEIGHT = 0.75; - - /** PD gains — LOW because feedforward does the heavy lifting. */ - private static final double kP_TRANSLATION = 0.015; - private static final double kD_TRANSLATION = 0.008; - private static final double kP_ROTATION = 0.22; - private static final double kD_ROTATION = 0.05; - - /** Max power the correction term can add. */ - private static final double MAX_CORRECTION = 0.30; - - /** Lookahead: follow a point this many seconds ahead in the recording. */ - private static final double LOOKAHEAD_TIME = 0.08; - - /** Time scaling for battery compensation. - * Lower battery → stretch time so robot has more time to execute. - * NO velocity scaling — time scaling handles everything. */ - private static final double TIME_SCALE_FACTOR = 0.55; - private static final double MAX_TIME_SCALE = 1.6; - private static final double MIN_TIME_SCALE = 0.85; - - /** D-term low-pass filter. */ - private static final double D_FILTER_ALPHA = 0.45; - - /** Error recovery: reduce FF weight when drift exceeds threshold. */ - private static final double ERROR_RECOVERY_THRESH = 6.0; - private static final double ERROR_RECOVERY_FF_WEIGHT = 0.45; - - /** Slew rate limit: max change in robot-frame command per second. - * Prevents command jumps that cause wheel slip. */ - private static final double MAX_SLEW_RATE = 4.0; // per second - - /** Voltage sensor refresh rate. */ - private static final double VOLTAGE_REFRESH_SEC = 0.1; - - /** Fallback max velocities if auto-calibration fails. */ - private static final double FALLBACK_MAX_LINEAR_VEL = 72.0; - private static final double FALLBACK_MAX_ANGULAR_VEL = 2.8; - - // ========== MOTOR DIRECTIONS ========== - // COPY THESE EXACTLY FROM THE TELEMETRY OUTPUT OF DataRecordingOp5 - private static final DcMotorSimple.Direction LF_DIR = DcMotorSimple.Direction.REVERSE; - private static final DcMotorSimple.Direction RF_DIR = DcMotorSimple.Direction.REVERSE; - private static final DcMotorSimple.Direction LR_DIR = DcMotorSimple.Direction.REVERSE; - private static final DcMotorSimple.Direction RR_DIR = DcMotorSimple.Direction.FORWARD; - - // ========== STATE ========== - private final Robot robot = Robot.getInstance(); - private final ElapsedTime runtime = new ElapsedTime(); - private final List recordedFrames = new ArrayList<>(); - - private DcMotorEx leftFront, rightFront, leftRear, rightRear; - - // Auto-calibrated max velocities - private double maxLinearVel = FALLBACK_MAX_LINEAR_VEL; - private double maxAngularVel = FALLBACK_MAX_ANGULAR_VEL; - - // PD state (all in ROBOT FRAME) - private double prevErrorX = 0, prevErrorY = 0, prevErrorHeading = 0; - private double prevTime = 0; - private double filteredDx = 0, filteredDy = 0, filteredDh = 0; - - // Slew rate limiter state - private double prevRobotFwd = 0, prevRobotStr = 0, prevRobotTurn = 0; - - // Voltage - private double cachedVoltage = 12.0; - private double lastVoltageReadTime = -999; - private double recordedVoltage = 12.0; - private double timeScale = 1.0; - - // Telemetry stats - private double maxWheelPowerSeen = 0; - private int clipCount = 0; - - // ------------------------------------------------------------------------- - // DATA MODEL - // ------------------------------------------------------------------------- - private static class RobotFrame { - double timestamp; - double x, y, heading; - double voltage; - double vxRobot, vyRobot, omega; // ROBOT-FRAME velocities (already rotated!) - double intakePwr, loaderPwr, leftShtrPwr, rightShtrPwr; - double turretPos, anglePos, flapPos; - - RobotFrame(String[] d) { - timestamp = Double.parseDouble(d[0]); - x = Double.parseDouble(d[1]); - y = Double.parseDouble(d[2]); - heading = Double.parseDouble(d[3]); - voltage = Double.parseDouble(d[4]); - vxRobot = Double.parseDouble(d[5]); - vyRobot = Double.parseDouble(d[6]); - omega = Double.parseDouble(d[7]); - intakePwr = Double.parseDouble(d[8]); - loaderPwr = Double.parseDouble(d[9]); - leftShtrPwr = Double.parseDouble(d[10]); - rightShtrPwr= Double.parseDouble(d[11]); - turretPos = Double.parseDouble(d[12]); - anglePos = Double.parseDouble(d[13]); - flapPos = Double.parseDouble(d[14]); - } - } - - // ------------------------------------------------------------------------- - // MAIN - // ------------------------------------------------------------------------- - @Override - public void runOpMode() { - telemetry.addLine("Initializing Replay Auto V4..."); - telemetry.update(); - - robot.initFollower(hardwareMap, true); - robot.init(hardwareMap); - - try { - robot.follower.getPoseTracker().resetIMU(); - } catch (InterruptedException e) { - telemetry.addLine("IMU Reset Interrupted"); - } - - // Drive motors with verified 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(LF_DIR); - rightFront.setDirection(RF_DIR); - leftRear.setDirection(LR_DIR); - rightRear.setDirection(RR_DIR); - - 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); - - try { - loadRecordedData(); - if (recordedFrames.isEmpty()) { - telemetry.addLine("ERROR: No recorded data found!"); - telemetry.update(); - return; - } - calculateRecordedVoltage(); - calibrateMaxVelocities(); - } catch (Exception e) { - telemetry.addData("ERROR", e.toString()); - telemetry.update(); - sleep(3000); - return; - } - - RobotFrame first = recordedFrames.get(0); - robot.follower.setPose(new Pose(first.x, first.y, first.heading)); - - telemetry.addLine("=== Replay Auto V5 Ready ==="); - telemetry.addData("Frames", recordedFrames.size()); - telemetry.addData("Duration", "%.2f s", - recordedFrames.get(recordedFrames.size()-1).timestamp - first.timestamp); - telemetry.addData("Rec Voltage", "%.1f V", recordedVoltage); - telemetry.addData("Max Lin Vel", "%.1f (auto)", maxLinearVel); - telemetry.addData("Max Ang Vel", "%.2f rad/s (auto)", maxAngularVel); - telemetry.addData("FF Weight", "%.0f%%", FF_WEIGHT * 100); - telemetry.addData("Lookahead", "%.2f s", LOOKAHEAD_TIME); - telemetry.update(); - - waitForStart(); - if (isStopRequested()) return; - - executePlayback(); - - stopRobot(); - stopMechanisms(); - } - - // ------------------------------------------------------------------------- - // PLAYBACK LOOP - // ------------------------------------------------------------------------- - private void executePlayback() { - runtime.reset(); - - double startTs = recordedFrames.get(0).timestamp; - double endTs = recordedFrames.get(recordedFrames.size() - 1).timestamp; - double duration = endTs - startTs; - - // Reset state - prevTime = 0; - prevErrorX = prevErrorY = prevErrorHeading = 0; - filteredDx = filteredDy = filteredDh = 0; - prevRobotFwd = prevRobotStr = prevRobotTurn = 0; - maxWheelPowerSeen = 0; - clipCount = 0; - - int idx = 0; - - while (opModeIsActive() && idx < recordedFrames.size() - 1) { - double now = runtime.seconds(); - - // Refresh voltage and compute time scaling - refreshVoltage(now); - updateTimeScaling(); - - // Current position in recording, with lookahead and time scaling - // TIME SCALING ONLY — no velocity scaling - double recordingTime = (now / timeScale) + LOOKAHEAD_TIME; - double targetTs = startTs + recordingTime; - - // Advance index to frame just before targetTs - while (idx < recordedFrames.size() - 1 && - recordedFrames.get(idx + 1).timestamp <= targetTs) { - idx++; - } - - // Interpolate between frames - RobotFrame fA = recordedFrames.get(idx); - RobotFrame fB = (idx + 1 < recordedFrames.size()) ? recordedFrames.get(idx + 1) : fA; - - double segDur = fB.timestamp - fA.timestamp; - double t = (segDur > 1e-6) - ? Range.clip((targetTs - fA.timestamp) / segDur, 0.0, 1.0) - : 0.0; - - // 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); - - // Feedforward from TARGET FRAME (same time base as lookahead) - double ffVxRobot = lerp(fA.vxRobot, fB.vxRobot, t); - double ffVyRobot = lerp(fA.vyRobot, fB.vyRobot, t); - double ffOmega = lerp(fA.omega, fB.omega, t); - - // NO velocity scaling — time scaling handles battery compensation - - // Update localization - robot.follower.getPoseTracker().update(); - Pose cur = robot.follower.getPose(); - double curH = cur.getHeading(); - - // ===== POSE ERROR (in FIELD FRAME) ===== - double errX_Field = targetX - cur.getX(); - double errY_Field = targetY - cur.getY(); - double errH = normalizeAngle(targetH - curH); - - // ===== ROTATE ERROR INTO ROBOT FRAME ===== - double cosH = Math.cos(curH); - double sinH = Math.sin(curH); - double errX_Robot = cosH * errX_Field + sinH * errY_Field; - double errY_Robot = -sinH * errX_Field + cosH * errY_Field; - - double dt = now - prevTime; - - // Filtered derivatives for D-term (in ROBOT FRAME) - double rawDx = dt > 1e-6 ? (errX_Robot - prevErrorX) / dt : 0; - double rawDy = dt > 1e-6 ? (errY_Robot - prevErrorY) / dt : 0; - double rawDh = dt > 1e-6 ? (errH - prevErrorHeading) / dt : 0; - - filteredDx = filteredDx + D_FILTER_ALPHA * (rawDx - filteredDx); - filteredDy = filteredDy + D_FILTER_ALPHA * (rawDy - filteredDy); - filteredDh = filteredDh + D_FILTER_ALPHA * (rawDh - filteredDh); - - // PD correction (in ROBOT FRAME) - double corrX_Robot = errX_Robot * kP_TRANSLATION + filteredDx * kD_TRANSLATION; - double corrY_Robot = errY_Robot * kP_TRANSLATION + filteredDy * kD_TRANSLATION; - double corrH = errH * kP_ROTATION + filteredDh * kD_ROTATION; - - // Clamp correction - double corrMag = Math.hypot(corrX_Robot, corrY_Robot); - if (corrMag > MAX_CORRECTION) { - corrX_Robot *= MAX_CORRECTION / corrMag; - corrY_Robot *= MAX_CORRECTION / corrMag; - } - corrH = Range.clip(corrH, -MAX_CORRECTION, MAX_CORRECTION); - - // Update PD state - prevErrorX = errX_Robot; - prevErrorY = errY_Robot; - prevErrorHeading = errH; - prevTime = now; - - // ===== SCALE FEEDFORWARD TO POWER UNITS ===== - double ffFwd = ffVxRobot / maxLinearVel; - double ffStr = ffVyRobot / maxLinearVel; - double ffTurn = ffOmega / maxAngularVel; - - ffFwd = Range.clip(ffFwd, -1.0, 1.0); - ffStr = Range.clip(ffStr, -1.0, 1.0); - ffTurn = Range.clip(ffTurn, -1.0, 1.0); - - // ===== ERROR RECOVERY ===== - double posError = Math.hypot(errX_Field, errY_Field); - double currentFFWeight = FF_WEIGHT; - if (posError > ERROR_RECOVERY_THRESH) { - currentFFWeight = ERROR_RECOVERY_FF_WEIGHT; - } - - // ===== COMBINE FEEDFORWARD + CORRECTION (both in ROBOT FRAME) ===== - double robotFwd = ffFwd * currentFFWeight + corrX_Robot * (1 - currentFFWeight); - double robotStr = ffStr * currentFFWeight + corrY_Robot * (1 - currentFFWeight); - double robotTurn = ffTurn * currentFFWeight + corrH * (1 - currentFFWeight); - - // ===== SLEW RATE LIMITING ===== - 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; - - // ===== MECANUM MIXING ===== - double fl = robotFwd + robotStr + robotTurn; - double fr = robotFwd - robotStr - robotTurn; - double bl = robotFwd - robotStr + robotTurn; - double br = robotFwd + robotStr - robotTurn; - - // ===== NORMALIZE ONLY IF SATURATED ===== - double maxWheel = Math.max(1.0, - Math.max(Math.abs(fl), - Math.max(Math.abs(fr), - Math.max(Math.abs(bl), Math.abs(br))))); - - if (maxWheel > 1.0) { - clipCount++; - fl /= maxWheel; - fr /= maxWheel; - bl /= maxWheel; - br /= maxWheel; - } - maxWheelPowerSeen = Math.max(maxWheelPowerSeen, maxWheel); - - leftFront.setPower(fl); - rightFront.setPower(fr); - leftRear.setPower(bl); - rightRear.setPower(br); - - // Mechanisms - controlMechanisms(fA, fB, t); - - // Telemetry - telemetry.addLine("=== Replay V5 ==="); - telemetry.addData("Time", "%.2f/%.2f s (scale %.2f)", now, duration * timeScale, timeScale); - telemetry.addData("Frame", "%d/%d (lerp %.2f)", idx, recordedFrames.size(), t); - telemetry.addData("FF%", "%.0f%%", currentFFWeight * 100); - telemetry.addData("FF cmd", "fwd=%.2f str=%.2f turn=%.2f", ffFwd, ffStr, ffTurn); - telemetry.addData("Corr", "x=%.2f y=%.2f h=%.2f", corrX_Robot, corrY_Robot, corrH); - telemetry.addData("Slew", "fwd=%.2f str=%.2f turn=%.2f", robotFwd, robotStr, robotTurn); - telemetry.addData("PosErr", "%.2f (thresh %.1f)", posError, ERROR_RECOVERY_THRESH); - telemetry.addData("Target", "(%.1f, %.1f) h=%.1f°", targetX, targetY, Math.toDegrees(targetH)); - telemetry.addData("Current", "(%.1f, %.1f) h=%.1f°", cur.getX(), cur.getY(), Math.toDegrees(curH)); - telemetry.addData("Voltage", "%.1f V (rec %.1f V)", cachedVoltage, recordedVoltage); - telemetry.addData("MaxWheel", "%.2f (clips %d)", maxWheelPowerSeen, clipCount); - telemetry.update(); - - idle(); - } - } - - // ------------------------------------------------------------------------- - // MECHANISMS - // ------------------------------------------------------------------------- - private void controlMechanisms(RobotFrame a, RobotFrame b, double t) { - 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)); - - RobotFrame 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); - } - - private void stopMechanisms() { - 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); - } - - private void stopRobot() { - leftFront.setPower(0); - rightFront.setPower(0); - leftRear.setPower(0); - rightRear.setPower(0); - } - - // ------------------------------------------------------------------------- - // DATA LOADING & CALIBRATION - // ------------------------------------------------------------------------- - 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 >= 15) recordedFrames.add(new RobotFrame(d)); - } - } - } - - private void calibrateMaxVelocities() { - List linearSpeeds = new ArrayList<>(); - List angularSpeeds = new ArrayList<>(); - - for (RobotFrame frame : recordedFrames) { - double linear = Math.hypot(frame.vxRobot, frame.vyRobot); - if (linear > 0.5) linearSpeeds.add(linear); - if (Math.abs(frame.omega) > 0.05) angularSpeeds.add(Math.abs(frame.omega)); - } - - if (!linearSpeeds.isEmpty()) { - Collections.sort(linearSpeeds); - int p95Index = (int) (linearSpeeds.size() * 0.95); - maxLinearVel = linearSpeeds.get(Math.min(p95Index, linearSpeeds.size() - 1)) * 1.15; - } - - if (!angularSpeeds.isEmpty()) { - Collections.sort(angularSpeeds); - int p95Index = (int) (angularSpeeds.size() * 0.95); - maxAngularVel = angularSpeeds.get(Math.min(p95Index, angularSpeeds.size() - 1)) * 1.15; - } - } - - private void calculateRecordedVoltage() { - double total = 0; - int count = 0; - for (RobotFrame f : recordedFrames) { - if (f.voltage > 5) { total += f.voltage; count++; } - } - if (count > 0) recordedVoltage = total / count; - } - - 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; - if (ratio < 1.0) { - timeScale = 1.0 + TIME_SCALE_FACTOR * (1.0 - ratio); - } else { - timeScale = 1.0; - } - timeScale = Range.clip(timeScale, MIN_TIME_SCALE, MAX_TIME_SCALE); - } - - // ------------------------------------------------------------------------- - // UTILITIES - // ------------------------------------------------------------------------- - 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; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAutoOp7.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAutoOp7.java deleted file mode 100644 index 71de4f1..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAutoOp7.java +++ /dev/null @@ -1,495 +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.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 Auto V7 — Driver Input Feedforward + Continuous Blending + Proper Constraints - * - * ARCHITECTURE: - * Feedforward = driver's actual gamepad stick inputs (already in [-1, 1]) - * Correction = small PD on pose error (rotated to robot frame) - * Blending = continuous exponential based on pose error magnitude - * Battery = time scaling only (no voltage compensation on power) - * Constraints = proper mecanum normalization, slew rate limiting - * - * WHY THIS IS THE BEST APPROACH: - * - Feedforward is CAUSAL: the driver's intent, not derived from effect - * - No derivation noise, no scaling factors, no max velocity calibration - * - Identical mecanum mixing path to TeleOp - * - Continuous blending is smooth and physically meaningful - * - Time scaling naturally compensates battery - * - * CSV Format (from DataRecordingOp7): - * Time,X,Y,Heading,Voltage,GamepadFwd,GamepadStr,GamepadTurn,IntakePwr,LoaderPwr,LShooterPwr,RShooterPwr,TurretPos,AnglePos,FlapPos - */ -@Autonomous(name = "Replay Auto V7", group = "Replay") -public class ReplayAutoOp7 extends LinearOpMode { - - private static final String CSV_PATH = "/sdcard/robot_data_v7.csv"; - - // ========== TUNABLE CONSTANTS ========== - - /** Continuous blending: how fast FF weight drops as error grows. - * k_blend = 0.3 means at 3.3cm error, FF weight is ~50%. - * Higher = more aggressive correction at small errors. - * Lower = more trust in feedforward. - * Start with 0.25–0.40. */ - private static final double BLEND_K = 0.30; - - /** Minimum FF weight (even at huge error). Prevents pure PD oscillation. */ - private static final double MIN_FF_WEIGHT = 0.35; - - /** PD gains — LOW because driver input does the heavy lifting. */ - private static final double kP_TRANSLATION = 0.018; - private static final double kD_TRANSLATION = 0.010; - private static final double kP_ROTATION = 0.25; - private static final double kD_ROTATION = 0.06; - - /** Max power the correction term can add. */ - private static final double MAX_CORRECTION = 0.25; - - /** Lookahead: follow a point this many seconds ahead in the recording. */ - private static final double LOOKAHEAD_TIME = 0.08; - - /** Time scaling for battery compensation. - * Lower battery → stretch time so robot has more time to execute. - * NO velocity scaling — time scaling handles everything. */ - private static final double TIME_SCALE_FACTOR = 0.55; - private static final double MAX_TIME_SCALE = 1.6; - private static final double MIN_TIME_SCALE = 0.85; - - /** D-term low-pass filter. */ - private static final double D_FILTER_ALPHA = 0.45; - - /** Slew rate limit: max change in command per second. - * Human inputs are naturally smooth, but interpolation + correction - * can create discontinuities. 5.0 = full range in 0.4s. */ - private static final double MAX_SLEW_RATE = 5.0; - - /** Voltage sensor refresh rate. */ - private static final double VOLTAGE_REFRESH_SEC = 0.1; - - // ========== MOTOR DIRECTIONS ========== - // COPY THESE EXACTLY FROM THE TELEMETRY OUTPUT OF DataRecordingOp7 - private static final DcMotorSimple.Direction LF_DIR = DcMotorSimple.Direction.REVERSE; - private static final DcMotorSimple.Direction RF_DIR = DcMotorSimple.Direction.REVERSE; - private static final DcMotorSimple.Direction LR_DIR = DcMotorSimple.Direction.REVERSE; - private static final DcMotorSimple.Direction RR_DIR = DcMotorSimple.Direction.FORWARD; - - // ========== STATE ========== - private final Robot robot = Robot.getInstance(); - private final ElapsedTime runtime = new ElapsedTime(); - private final List recordedFrames = new ArrayList<>(); - - private DcMotorEx leftFront, rightFront, leftRear, rightRear; - - // PD state (all in ROBOT FRAME) - private double prevErrorX = 0, prevErrorY = 0, prevErrorHeading = 0; - private double prevTime = 0; - private double filteredDx = 0, filteredDy = 0, filteredDh = 0; - - // Slew rate limiter state - private double prevRobotFwd = 0, prevRobotStr = 0, prevRobotTurn = 0; - - // Voltage - private double cachedVoltage = 12.0; - private double lastVoltageReadTime = -999; - private double recordedVoltage = 12.0; - private double timeScale = 1.0; - - // Telemetry stats - private double maxWheelPowerSeen = 0; - private int clipCount = 0; - private double avgFFWeight = 0; - private int loopCount = 0; - - // ------------------------------------------------------------------------- - // DATA MODEL - // ------------------------------------------------------------------------- - private static class RobotFrame { - double timestamp; - double x, y, heading; - double voltage; - double gpFwd, gpStr, gpTurn; // DRIVER INPUTS in [-1, 1] (robot-frame) - double intakePwr, loaderPwr, leftShtrPwr, rightShtrPwr; - double turretPos, anglePos, flapPos; - - RobotFrame(String[] d) { - timestamp = Double.parseDouble(d[0]); - x = Double.parseDouble(d[1]); - y = Double.parseDouble(d[2]); - heading = Double.parseDouble(d[3]); - voltage = Double.parseDouble(d[4]); - gpFwd = Double.parseDouble(d[5]); - gpStr = Double.parseDouble(d[6]); - gpTurn = Double.parseDouble(d[7]); - intakePwr = Double.parseDouble(d[8]); - loaderPwr = Double.parseDouble(d[9]); - leftShtrPwr = Double.parseDouble(d[10]); - rightShtrPwr= Double.parseDouble(d[11]); - turretPos = Double.parseDouble(d[12]); - anglePos = Double.parseDouble(d[13]); - flapPos = Double.parseDouble(d[14]); - } - } - - // ------------------------------------------------------------------------- - // MAIN - // ------------------------------------------------------------------------- - @Override - public void runOpMode() { - telemetry.addLine("Initializing Replay Auto V7..."); - telemetry.update(); - - robot.initFollower(hardwareMap, true); - robot.init(hardwareMap); - - try { - robot.follower.getPoseTracker().resetIMU(); - } catch (InterruptedException e) { - telemetry.addLine("IMU Reset Interrupted"); - } - - // Drive motors with verified 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(LF_DIR); - rightFront.setDirection(RF_DIR); - leftRear.setDirection(LR_DIR); - rightRear.setDirection(RR_DIR); - - 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); - - try { - loadRecordedData(); - if (recordedFrames.isEmpty()) { - telemetry.addLine("ERROR: No recorded data found!"); - telemetry.update(); - return; - } - calculateRecordedVoltage(); - } catch (Exception e) { - telemetry.addData("ERROR", e.toString()); - telemetry.update(); - sleep(3000); - return; - } - - RobotFrame first = recordedFrames.get(0); - robot.follower.setPose(new Pose(first.x, first.y, first.heading)); - - telemetry.addLine("=== Replay Auto V7 Ready ==="); - telemetry.addData("Frames", recordedFrames.size()); - telemetry.addData("Duration", "%.2f s", - recordedFrames.get(recordedFrames.size()-1).timestamp - first.timestamp); - telemetry.addData("Rec Voltage", "%.1f V", recordedVoltage); - telemetry.addData("Blend K", "%.2f", BLEND_K); - telemetry.addData("Lookahead", "%.2f s", LOOKAHEAD_TIME); - telemetry.update(); - - waitForStart(); - if (isStopRequested()) return; - - executePlayback(); - - stopRobot(); - stopMechanisms(); - } - - // ------------------------------------------------------------------------- - // PLAYBACK LOOP - // ------------------------------------------------------------------------- - private void executePlayback() { - runtime.reset(); - - double startTs = recordedFrames.get(0).timestamp; - double endTs = recordedFrames.get(recordedFrames.size() - 1).timestamp; - double duration = endTs - startTs; - - // Reset state - prevTime = 0; - prevErrorX = prevErrorY = prevErrorHeading = 0; - filteredDx = filteredDy = filteredDh = 0; - prevRobotFwd = prevRobotStr = prevRobotTurn = 0; - maxWheelPowerSeen = 0; - clipCount = 0; - avgFFWeight = 0; - loopCount = 0; - - int idx = 0; - - while (opModeIsActive() && idx < recordedFrames.size() - 1) { - double now = runtime.seconds(); - - // Refresh voltage and compute time scaling - refreshVoltage(now); - updateTimeScaling(); - - // Current position in recording, with lookahead and time scaling - double recordingTime = (now / timeScale) + LOOKAHEAD_TIME; - double targetTs = startTs + recordingTime; - - // Advance index to frame just before targetTs - while (idx < recordedFrames.size() - 1 && - recordedFrames.get(idx + 1).timestamp <= targetTs) { - idx++; - } - - // Interpolate between frames - RobotFrame fA = recordedFrames.get(idx); - RobotFrame fB = (idx + 1 < recordedFrames.size()) ? recordedFrames.get(idx + 1) : fA; - - double segDur = fB.timestamp - fA.timestamp; - double t = (segDur > 1e-6) - ? Range.clip((targetTs - fA.timestamp) / segDur, 0.0, 1.0) - : 0.0; - - // 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); - - // ===== FEEDFORWARD: DRIVER'S ACTUAL INPUTS ===== - // These are already in [-1, 1] robot-frame power units - // Interpolated at the SAME target frame as lookahead - double ffFwd = lerp(fA.gpFwd, fB.gpFwd, t); - double ffStr = lerp(fA.gpStr, fB.gpStr, t); - double ffTurn = lerp(fA.gpTurn, fB.gpTurn, t); - - // Clamp to [-1, 1] (should already be, but safety) - ffFwd = Range.clip(ffFwd, -1.0, 1.0); - ffStr = Range.clip(ffStr, -1.0, 1.0); - ffTurn = Range.clip(ffTurn, -1.0, 1.0); - - // Update localization - robot.follower.getPoseTracker().update(); - Pose cur = robot.follower.getPose(); - double curH = cur.getHeading(); - - // ===== POSE ERROR (in FIELD FRAME) ===== - double errX_Field = targetX - cur.getX(); - double errY_Field = targetY - cur.getY(); - double errH = normalizeAngle(targetH - curH); - - // ===== ROTATE ERROR INTO ROBOT FRAME ===== - double cosH = Math.cos(curH); - double sinH = Math.sin(curH); - double errX_Robot = cosH * errX_Field + sinH * errY_Field; - double errY_Robot = -sinH * errX_Field + cosH * errY_Field; - - double dt = now - prevTime; - - // Filtered derivatives for D-term (in ROBOT FRAME) - double rawDx = dt > 1e-6 ? (errX_Robot - prevErrorX) / dt : 0; - double rawDy = dt > 1e-6 ? (errY_Robot - prevErrorY) / dt : 0; - double rawDh = dt > 1e-6 ? (errH - prevErrorHeading) / dt : 0; - - filteredDx = filteredDx + D_FILTER_ALPHA * (rawDx - filteredDx); - filteredDy = filteredDy + D_FILTER_ALPHA * (rawDy - filteredDy); - filteredDh = filteredDh + D_FILTER_ALPHA * (rawDh - filteredDh); - - // PD correction (in ROBOT FRAME) - double corrX_Robot = errX_Robot * kP_TRANSLATION + filteredDx * kD_TRANSLATION; - double corrY_Robot = errY_Robot * kP_TRANSLATION + filteredDy * kD_TRANSLATION; - double corrH = errH * kP_ROTATION + filteredDh * kD_ROTATION; - - // Clamp correction - double corrMag = Math.hypot(corrX_Robot, corrY_Robot); - if (corrMag > MAX_CORRECTION) { - corrX_Robot *= MAX_CORRECTION / corrMag; - corrY_Robot *= MAX_CORRECTION / corrMag; - } - corrH = Range.clip(corrH, -MAX_CORRECTION, MAX_CORRECTION); - - // Update PD state - prevErrorX = errX_Robot; - prevErrorY = errY_Robot; - prevErrorHeading = errH; - prevTime = now; - - // ===== CONTINUOUS BLENDING BASED ON ERROR ===== - // w = exp(-k * error) → 1.0 at zero error, drops as error grows - double posError = Math.hypot(errX_Field, errY_Field); - double ffWeight = Math.exp(-BLEND_K * posError); - ffWeight = Range.clip(ffWeight, MIN_FF_WEIGHT, 1.0); - - // ===== COMBINE FEEDFORWARD + CORRECTION (both in ROBOT FRAME) ===== - double robotFwd = ffFwd * ffWeight + corrX_Robot * (1 - ffWeight); - double robotStr = ffStr * ffWeight + corrY_Robot * (1 - ffWeight); - double robotTurn = ffTurn * ffWeight + corrH * (1 - ffWeight); - - // ===== SLEW RATE LIMITING ===== - 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; - - // ===== MECANUM MIXING (identical to TeleOp path) ===== - double fl = robotFwd + robotStr + robotTurn; - double fr = robotFwd - robotStr - robotTurn; - double bl = robotFwd - robotStr + robotTurn; - double br = robotFwd + robotStr - robotTurn; - - // ===== PROPER MECANUM NORMALIZATION ===== - // Normalize so that |fwd| + |strafe| + |turn| <= 1 preserves ratios - double maxSum = Math.abs(robotFwd) + Math.abs(robotStr) + Math.abs(robotTurn); - if (maxSum > 1.0) { - clipCount++; - double scale = 1.0 / maxSum; - fl *= scale; - fr *= scale; - bl *= scale; - br *= scale; - } - maxWheelPowerSeen = Math.max(maxWheelPowerSeen, Math.max(Math.abs(fl), - Math.max(Math.abs(fr), Math.max(Math.abs(bl), Math.abs(br))))); - - leftFront.setPower(fl); - rightFront.setPower(fr); - leftRear.setPower(bl); - rightRear.setPower(br); - - // Mechanisms - controlMechanisms(fA, fB, t); - - // Stats - avgFFWeight += ffWeight; - loopCount++; - - // Telemetry - telemetry.addLine("=== Replay V7 ==="); - telemetry.addData("Time", "%.2f/%.2f s (scale %.2f)", now, duration * timeScale, timeScale); - telemetry.addData("Frame", "%d/%d (lerp %.2f)", idx, recordedFrames.size(), t); - telemetry.addData("FF wt", "%.0f%% (avg %.0f%%)", ffWeight * 100, (avgFFWeight / loopCount) * 100); - telemetry.addData("FF cmd", "fwd=%.2f str=%.2f turn=%.2f", ffFwd, ffStr, ffTurn); - telemetry.addData("Corr", "x=%.2f y=%.2f h=%.2f", corrX_Robot, corrY_Robot, corrH); - telemetry.addData("Slew", "fwd=%.2f str=%.2f turn=%.2f", robotFwd, robotStr, robotTurn); - telemetry.addData("PosErr", "%.2f", posError); - telemetry.addData("Target", "(%.1f, %.1f) h=%.1f°", targetX, targetY, Math.toDegrees(targetH)); - telemetry.addData("Current", "(%.1f, %.1f) h=%.1f°", cur.getX(), cur.getY(), Math.toDegrees(curH)); - telemetry.addData("Voltage", "%.1f V (rec %.1f V)", cachedVoltage, recordedVoltage); - telemetry.addData("MaxWheel", "%.2f (clips %d)", maxWheelPowerSeen, clipCount); - telemetry.update(); - - 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)); - - // Motors — snap to nearest frame (fast response, interpolation not critical) - RobotFrame 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); - } - - private void stopMechanisms() { - 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); - } - - private void stopRobot() { - leftFront.setPower(0); - rightFront.setPower(0); - leftRear.setPower(0); - rightRear.setPower(0); - } - - // ------------------------------------------------------------------------- - // DATA LOADING - // ------------------------------------------------------------------------- - 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 >= 15) recordedFrames.add(new RobotFrame(d)); - } - } - } - - private void calculateRecordedVoltage() { - double total = 0; - int count = 0; - for (RobotFrame f : recordedFrames) { - if (f.voltage > 5) { total += f.voltage; count++; } - } - if (count > 0) recordedVoltage = total / count; - } - - 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; - if (ratio < 1.0) { - timeScale = 1.0 + TIME_SCALE_FACTOR * (1.0 - ratio); - } else { - timeScale = 1.0; - } - timeScale = Range.clip(timeScale, MIN_TIME_SCALE, MAX_TIME_SCALE); - } - - // ------------------------------------------------------------------------- - // UTILITIES - // ------------------------------------------------------------------------- - 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; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAutoOp8.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAutoOp8.java deleted file mode 100644 index 04e2820..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/autonomous/ReplayAutoOp8.java +++ /dev/null @@ -1,576 +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.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 Auto V8 — Exact Match to MainDrivingOp - * - * FIXES from V7: - * 1. Bug fix: getInterpolatedFrame uses lightweight InterpolatedInputs class - * 2. Mechanisms behave IDENTICALLY to MainDrivingOp (auto-aim, toggles, etc.) - * 3. Normalization: max wheel power - * 4. Blending: additive (ff + scaled corr) - * 5. Dynamic correction cap - * 6. Disable lookahead near stop - * 7. Clamp dt - * 8. Lerp motor powers - * - * CSV Format (from DataRecordingOp8): - * Time,X,Y,Heading,Voltage,GamepadFwd,GamepadStr,GamepadTurn, - * IntakePwr,LoaderPwr,LShooterPwr,RShooterPwr,TurretPos,AnglePos,FlapPos,AutoAim,BlueTarget - */ -@Autonomous(name = "Replay Auto V8", group = "Replay") -public class ReplayAutoOp8 extends LinearOpMode { - - private static final String CSV_PATH = "/sdcard/robot_data_v8.csv"; - - // ========== TUNABLE CONSTANTS ========== - - /** Continuous blending: how fast correction weight grows with error. */ - private static final double BLEND_K = 0.35; - - /** PD gains — LOW because driver input does the heavy lifting. */ - private static final double kP_TRANSLATION = 0.018; - private static final double kD_TRANSLATION = 0.010; - private static final double kP_ROTATION = 0.28; - private static final double kD_ROTATION = 0.07; - - /** Base correction cap (small error). */ - private static final double MIN_CORRECTION_CAP = 0.20; - /** Max correction cap (large error). */ - private static final double MAX_CORRECTION_CAP = 0.60; - /** Error scale for dynamic cap transition. */ - private static final double CORRECTION_ERROR_SCALE = 8.0; - - /** Lookahead: follow a point this many seconds ahead. */ - private static final double LOOKAHEAD_TIME = 0.08; - /** Threshold to disable lookahead (near stop). */ - private static final double LOOKAHEAD_DISABLE_THRESH = 0.04; - - /** Time scaling for battery compensation. */ - private static final double TIME_SCALE_FACTOR = 0.55; - private static final double MAX_TIME_SCALE = 1.6; - private static final double MIN_TIME_SCALE = 0.85; - - /** D-term low-pass filter. */ - private static final double D_FILTER_ALPHA = 0.45; - - /** Slew rate limit: max change in command per second. */ - private static final double MAX_SLEW_RATE = 5.0; - - /** Voltage sensor refresh rate. */ - private static final double VOLTAGE_REFRESH_SEC = 0.1; - - /** dt clamp to prevent derivative spikes from timing jitter. */ - private static final double MIN_DT = 0.008; - private static final double MAX_DT = 0.050; - - // ========== MOTOR DIRECTIONS ========== - // COPY THESE EXACTLY FROM THE TELEMETRY OUTPUT OF DataRecordingOp8 - private static final DcMotorSimple.Direction LF_DIR = DcMotorSimple.Direction.REVERSE; - private static final DcMotorSimple.Direction RF_DIR = DcMotorSimple.Direction.REVERSE; - private static final DcMotorSimple.Direction LR_DIR = DcMotorSimple.Direction.REVERSE; - private static final DcMotorSimple.Direction RR_DIR = DcMotorSimple.Direction.FORWARD; - - // ========== STATE ========== - private final Robot robot = Robot.getInstance(); - private final ElapsedTime runtime = new ElapsedTime(); - private final List recordedFrames = new ArrayList<>(); - - private DcMotorEx leftFront, rightFront, leftRear, rightRear; - - // PD state (all in ROBOT FRAME) - private double prevErrorX = 0, prevErrorY = 0, prevErrorHeading = 0; - private double prevTime = 0; - private double filteredDx = 0, filteredDy = 0, filteredDh = 0; - - // Slew rate limiter state - private double prevRobotFwd = 0, prevRobotStr = 0, prevRobotTurn = 0; - - // Voltage - private double cachedVoltage = 12.0; - private double lastVoltageReadTime = -999; - private double recordedVoltage = 12.0; - private double timeScale = 1.0; - - // Telemetry stats - private double maxWheelPowerSeen = 0; - private int clipCount = 0; - private double avgCorrWeight = 0; - private int loopCount = 0; - - // ------------------------------------------------------------------------- - // DATA MODEL - // ------------------------------------------------------------------------- - private static class RobotFrame { - double timestamp; - double x, y, heading; - double voltage; - double gpFwd, gpStr, gpTurn; // DRIVER INPUTS in [-1, 1] - double intakePwr, loaderPwr, leftShtrPwr, rightShtrPwr; - double turretPos, anglePos, flapPos; - boolean autoAimEnabled; - boolean blueTarget; - - RobotFrame(String[] d) { - timestamp = Double.parseDouble(d[0]); - x = Double.parseDouble(d[1]); - y = Double.parseDouble(d[2]); - heading = Double.parseDouble(d[3]); - voltage = Double.parseDouble(d[4]); - gpFwd = Double.parseDouble(d[5]); - gpStr = Double.parseDouble(d[6]); - gpTurn = Double.parseDouble(d[7]); - intakePwr = Double.parseDouble(d[8]); - loaderPwr = Double.parseDouble(d[9]); - leftShtrPwr = Double.parseDouble(d[10]); - rightShtrPwr= Double.parseDouble(d[11]); - turretPos = Double.parseDouble(d[12]); - anglePos = Double.parseDouble(d[13]); - flapPos = Double.parseDouble(d[14]); - autoAimEnabled = Integer.parseInt(d[15]) != 0; - blueTarget = Integer.parseInt(d[16]) != 0; - } - } - - /** Lightweight class for interpolated inputs — BUG FIX from V7 */ - private static class InterpolatedInputs { - double gpFwd, gpStr, gpTurn; - double intakePwr, loaderPwr, leftShtrPwr, rightShtrPwr; - double turretPos, anglePos, flapPos; - boolean autoAimEnabled; - boolean blueTarget; - } - - // ------------------------------------------------------------------------- - // MAIN - // ------------------------------------------------------------------------- - @Override - public void runOpMode() { - telemetry.addLine("Initializing Replay Auto V8..."); - telemetry.update(); - - robot.initFollower(hardwareMap, true); - robot.init(hardwareMap); - - try { - robot.follower.getPoseTracker().resetIMU(); - } catch (InterruptedException e) { - telemetry.addLine("IMU Reset Interrupted"); - } - - // Drive motors with verified 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(LF_DIR); - rightFront.setDirection(RF_DIR); - leftRear.setDirection(LR_DIR); - rightRear.setDirection(RR_DIR); - - 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); - - try { - loadRecordedData(); - if (recordedFrames.isEmpty()) { - telemetry.addLine("ERROR: No recorded data found!"); - telemetry.update(); - return; - } - calculateRecordedVoltage(); - } catch (Exception e) { - telemetry.addData("ERROR", e.toString()); - telemetry.update(); - sleep(3000); - return; - } - - RobotFrame first = recordedFrames.get(0); - robot.follower.setPose(new Pose(first.x, first.y, first.heading)); - robot.Blue_Target = first.blueTarget; - - telemetry.addLine("=== Replay Auto V8 Ready ==="); - telemetry.addData("Frames", recordedFrames.size()); - telemetry.addData("Duration", "%.2f s", - recordedFrames.get(recordedFrames.size()-1).timestamp - first.timestamp); - telemetry.addData("Rec Voltage", "%.1f V", recordedVoltage); - telemetry.addData("Blend K", "%.2f", BLEND_K); - telemetry.addData("Lookahead", "%.2f s", LOOKAHEAD_TIME); - telemetry.update(); - - waitForStart(); - if (isStopRequested()) return; - - executePlayback(); - - stopRobot(); - stopMechanisms(); - } - - // ------------------------------------------------------------------------- - // PLAYBACK LOOP - // ------------------------------------------------------------------------- - private void executePlayback() { - runtime.reset(); - - double startTs = recordedFrames.get(0).timestamp; - double endTs = recordedFrames.get(recordedFrames.size() - 1).timestamp; - double duration = endTs - startTs; - - // Reset state - prevTime = 0; - prevErrorX = prevErrorY = prevErrorHeading = 0; - filteredDx = filteredDy = filteredDh = 0; - prevRobotFwd = prevRobotStr = prevRobotTurn = 0; - maxWheelPowerSeen = 0; - clipCount = 0; - avgCorrWeight = 0; - loopCount = 0; - - int idx = 0; - - while (opModeIsActive() && idx < recordedFrames.size() - 1) { - double now = runtime.seconds(); - - // Refresh voltage and compute time scaling - refreshVoltage(now); - updateTimeScaling(); - - // ===== LOOKAHEAD (disable near stop) ===== - double currentRecordingTime = now / timeScale; - InterpolatedInputs currentInputs = getInterpolatedInputs(currentRecordingTime, startTs); - double currentInputMag = Math.abs(currentInputs.gpFwd) - + Math.abs(currentInputs.gpStr) - + Math.abs(currentInputs.gpTurn); - - double lookahead = (currentInputMag > LOOKAHEAD_DISABLE_THRESH) ? LOOKAHEAD_TIME : 0.0; - - // Current position in recording, with lookahead and time scaling - double recordingTime = currentRecordingTime + lookahead; - double targetTs = startTs + recordingTime; - - // Advance index to frame just before targetTs - while (idx < recordedFrames.size() - 1 && - recordedFrames.get(idx + 1).timestamp <= targetTs) { - idx++; - } - - // Interpolate between frames - RobotFrame fA = recordedFrames.get(idx); - RobotFrame fB = (idx + 1 < recordedFrames.size()) ? recordedFrames.get(idx + 1) : fA; - - double segDur = fB.timestamp - fA.timestamp; - double t = (segDur > 1e-6) - ? Range.clip((targetTs - fA.timestamp) / segDur, 0.0, 1.0) - : 0.0; - - // 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); - - // ===== FEEDFORWARD: DRIVER'S ACTUAL INPUTS ===== - double ffFwd = lerp(fA.gpFwd, fB.gpFwd, t); - double ffStr = lerp(fA.gpStr, fB.gpStr, t); - double ffTurn = lerp(fA.gpTurn, fB.gpTurn, t); - - ffFwd = Range.clip(ffFwd, -1.0, 1.0); - ffStr = Range.clip(ffStr, -1.0, 1.0); - ffTurn = Range.clip(ffTurn, -1.0, 1.0); - - // Update localization - robot.follower.getPoseTracker().update(); - Pose cur = robot.follower.getPose(); - double curH = cur.getHeading(); - - // ===== POSE ERROR (in FIELD FRAME) ===== - double errX_Field = targetX - cur.getX(); - double errY_Field = targetY - cur.getY(); - double errH = normalizeAngle(targetH - curH); - - // ===== ROTATE ERROR INTO ROBOT FRAME ===== - double cosH = Math.cos(curH); - double sinH = Math.sin(curH); - double errX_Robot = cosH * errX_Field + sinH * errY_Field; - double errY_Robot = -sinH * errX_Field + cosH * errY_Field; - - // ===== CLAMPED dt ===== - double rawDt = now - prevTime; - double dt = Range.clip(rawDt, MIN_DT, MAX_DT); - - // Filtered derivatives for D-term (in ROBOT FRAME) - double rawDx = (errX_Robot - prevErrorX) / dt; - double rawDy = (errY_Robot - prevErrorY) / dt; - double rawDh = (errH - prevErrorHeading) / dt; - - filteredDx = filteredDx + D_FILTER_ALPHA * (rawDx - filteredDx); - filteredDy = filteredDy + D_FILTER_ALPHA * (rawDy - filteredDy); - filteredDh = filteredDh + D_FILTER_ALPHA * (rawDh - filteredDh); - - // PD correction (in ROBOT FRAME) - double corrX_Robot = errX_Robot * kP_TRANSLATION + filteredDx * kD_TRANSLATION; - double corrY_Robot = errY_Robot * kP_TRANSLATION + filteredDy * kD_TRANSLATION; - double corrH = errH * kP_ROTATION + filteredDh * kD_ROTATION; - - // ===== DYNAMIC CORRECTION CAP ===== - double posError = Math.hypot(errX_Field, errY_Field); - double corrScale = Math.min(posError / CORRECTION_ERROR_SCALE, 1.0); - double dynamicMaxCorr = lerp(MIN_CORRECTION_CAP, MAX_CORRECTION_CAP, corrScale); - - double corrMag = Math.hypot(corrX_Robot, corrY_Robot); - if (corrMag > dynamicMaxCorr) { - double scale = dynamicMaxCorr / corrMag; - corrX_Robot *= scale; - corrY_Robot *= scale; - } - corrH = Range.clip(corrH, -dynamicMaxCorr, dynamicMaxCorr); - - // Update PD state - prevErrorX = errX_Robot; - prevErrorY = errY_Robot; - prevErrorHeading = errH; - prevTime = now; - - // ===== CONTINUOUS BLENDING (additive) ===== - double corrWeight = 1.0 - Math.exp(-BLEND_K * posError); - corrWeight = Range.clip(corrWeight, 0.0, 1.0); - - // ===== ADDITIVE BLENDING: u = u_ff + corrWeight * u_fb ===== - double robotFwd = ffFwd + corrWeight * corrX_Robot; - double robotStr = ffStr + corrWeight * corrY_Robot; - double robotTurn = ffTurn + corrWeight * corrH; - - // Clamp combined command to [-1, 1] before mixing - double combinedMag = Math.hypot(robotFwd, robotStr); - if (combinedMag > 1.0) { - robotFwd /= combinedMag; - robotStr /= combinedMag; - } - robotTurn = Range.clip(robotTurn, -1.0, 1.0); - - // ===== SLEW RATE LIMITING ===== - 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; - - // ===== MECANUM MIXING ===== - double fl = robotFwd + robotStr + robotTurn; - double fr = robotFwd - robotStr - robotTurn; - double bl = robotFwd - robotStr + robotTurn; - double br = robotFwd + robotStr - robotTurn; - - // ===== NORMALIZE: max wheel power ===== - double maxWheel = Math.max(1.0, - Math.max(Math.abs(fl), - Math.max(Math.abs(fr), - Math.max(Math.abs(bl), Math.abs(br))))); - - if (maxWheel > 1.0) { - clipCount++; - fl /= maxWheel; - fr /= maxWheel; - bl /= maxWheel; - br /= maxWheel; - } - maxWheelPowerSeen = Math.max(maxWheelPowerSeen, maxWheel); - - leftFront.setPower(fl); - rightFront.setPower(fr); - leftRear.setPower(bl); - rightRear.setPower(br); - - // ===== MECHANISMS — EXACT MATCH TO MainDrivingOp ===== - controlMechanisms(fA, fB, t); - - // Stats - avgCorrWeight += corrWeight; - loopCount++; - - // Telemetry - telemetry.addLine("=== Replay V8 ==="); - telemetry.addData("Time", "%.2f/%.2f s (scale %.2f)", now, duration * timeScale, timeScale); - telemetry.addData("Frame", "%d/%d (lerp %.2f)", idx, recordedFrames.size(), t); - telemetry.addData("Lookahead", "%.3f s", lookahead); - telemetry.addData("Corr%", "%.0f%% (avg %.0f%%)", corrWeight * 100, (avgCorrWeight / loopCount) * 100); - telemetry.addData("FF cmd", "fwd=%.2f str=%.2f turn=%.2f", ffFwd, ffStr, ffTurn); - telemetry.addData("Corr", "x=%.2f y=%.2f h=%.2f (cap %.2f)", corrX_Robot, corrY_Robot, corrH, dynamicMaxCorr); - telemetry.addData("Final", "fwd=%.2f str=%.2f turn=%.2f", robotFwd, robotStr, robotTurn); - telemetry.addData("PosErr", "%.2f", posError); - telemetry.addData("Target", "(%.1f, %.1f) h=%.1f°", targetX, targetY, Math.toDegrees(targetH)); - telemetry.addData("Current", "(%.1f, %.1f) h=%.1f°", cur.getX(), cur.getY(), Math.toDegrees(curH)); - telemetry.addData("Voltage", "%.1f V (rec %.1f V)", cachedVoltage, recordedVoltage); - telemetry.addData("MaxWheel", "%.2f (clips %d)", maxWheelPowerSeen, clipCount); - telemetry.update(); - - idle(); - } - } - - // ------------------------------------------------------------------------- - // MECHANISMS — EXACT MATCH TO MainDrivingOp LOGIC - // ------------------------------------------------------------------------- - private void controlMechanisms(RobotFrame a, RobotFrame b, double t) { - // Interpolate all mechanism values - double intakePwr = lerp(a.intakePwr, b.intakePwr, t); - double loaderPwr = lerp(a.loaderPwr, b.loaderPwr, t); - double leftShtrPwr = lerp(a.leftShtrPwr, b.leftShtrPwr, t); - double rightShtrPwr = lerp(a.rightShtrPwr, b.rightShtrPwr, t); - double turretPos = lerp(a.turretPos, b.turretPos, t); - double anglePos = lerp(a.anglePos, b.anglePos, t); - double flapPos = lerp(a.flapPos, b.flapPos, t); - - // Auto-aim state — use nearest frame (state changes are discrete) - boolean autoAim = (t < 0.5) ? a.autoAimEnabled : b.autoAimEnabled; - boolean blueTarget = (t < 0.5) ? a.blueTarget : b.blueTarget; - - // Apply to robot — EXACT same as MainDrivingOp - if (robot.intakeMotor != null) robot.intakeMotor.setPower(intakePwr); - if (robot.loaderMotor != null) robot.loaderMotor.setPower(loaderPwr); - if (robot.leftOuttake != null) robot.leftOuttake.setPower(leftShtrPwr); - if (robot.rightOuttake != null) robot.rightOuttake.setPower(rightShtrPwr); - - robot.turretServo.setPosition(turretPos); - robot.angleServo.setPosition(anglePos); - robot.flapsServo.setPosition(flapPos); - - // Set robot state variables for turret auto-aim logic - robot.Blue_Target = blueTarget; - // Note: autoAim state is used by turret.update() if it checks this - // The recorded turretPos already includes the result of auto-aim calculations - } - - private void stopMechanisms() { - 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); - } - - private void stopRobot() { - leftFront.setPower(0); - rightFront.setPower(0); - leftRear.setPower(0); - rightRear.setPower(0); - } - - // ------------------------------------------------------------------------- - // HELPERS — BUG FIX: lightweight InterpolatedInputs class - // ------------------------------------------------------------------------- - - /** - * Get interpolated inputs at a given recording time. - * Uses lightweight InterpolatedInputs instead of RobotFrame constructor. - */ - private InterpolatedInputs getInterpolatedInputs(double recordingTime, double startTs) { - double targetTs = startTs + recordingTime; - InterpolatedInputs result = new InterpolatedInputs(); - - for (int i = 0; i < recordedFrames.size() - 1; i++) { - RobotFrame a = recordedFrames.get(i); - RobotFrame b = recordedFrames.get(i + 1); - if (b.timestamp > targetTs) { - double segDur = b.timestamp - a.timestamp; - double t = (segDur > 1e-6) - ? Range.clip((targetTs - a.timestamp) / segDur, 0.0, 1.0) - : 0.0; - result.gpFwd = lerp(a.gpFwd, b.gpFwd, t); - result.gpStr = lerp(a.gpStr, b.gpStr, t); - result.gpTurn = lerp(a.gpTurn, b.gpTurn, t); - return result; - } - } - - // Fallback: return last frame - RobotFrame last = recordedFrames.get(recordedFrames.size() - 1); - result.gpFwd = last.gpFwd; - result.gpStr = last.gpStr; - result.gpTurn = last.gpTurn; - return result; - } - - // ------------------------------------------------------------------------- - // DATA LOADING - // ------------------------------------------------------------------------- - 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 >= 17) recordedFrames.add(new RobotFrame(d)); - } - } - } - - private void calculateRecordedVoltage() { - double total = 0; - int count = 0; - for (RobotFrame f : recordedFrames) { - if (f.voltage > 5) { total += f.voltage; count++; } - } - if (count > 0) recordedVoltage = total / count; - } - - 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; - if (ratio < 1.0) { - timeScale = 1.0 + TIME_SCALE_FACTOR * (1.0 - ratio); - } else { - timeScale = 1.0; - } - timeScale = Range.clip(timeScale, MIN_TIME_SCALE, MAX_TIME_SCALE); - } - - // ------------------------------------------------------------------------- - // UTILITIES - // ------------------------------------------------------------------------- - 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; - } -} 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/DataRecordingOp3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp3.java deleted file mode 100644 index d5bee1a..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp3.java +++ /dev/null @@ -1,322 +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.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; - -/** - * Enhanced TeleOP data recorder (V3). - * Records drive motor powers (for feedforward replay) and mechanism velocities - * (for faithful shooter reproduction). - * - * CSV columns (17 total): - * Time, LR, RR, LF, RF, X, Y, Heading, Voltage, - * IntakeVel, LoaderVel, LeftShtrVel, RightShtrVel, - * TurretPos, AnglePos, FlapPos - * - * 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) - * - * @version 3.0 - */ -@TeleOp(name = "Data Recorder V3", group = "Replay") -public class DataRecordingOp3 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; - - @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); - } - - 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"; - try { - dataRecorder = new FileWriter(filePath); - dataRecorder.write("Time,LR,RR,LF,RF,X,Y,Heading,Voltage,IntakeVel,LoaderVel,LeftShtrVel,RightShtrVel,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 V3 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(); - - // Intake - robot.intake.speed = utilityGP.rightStick.y; - robot.intake.reversed = INTAKE_REVERSE; - - // Alignment - 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) * 0.9; - 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/Angle 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(); - } - - if(drivingGP.dpadDown.justPressed()) - robot.turret.autoAimEnabled = !robot.turret.autoAimEnabled; - - if(drivingGP.dpadUp.justPressed()) - autoAimEnabled=!autoAimEnabled; - - if(autoAimEnabled) - robot.shoot.activateRange(0); - // Shoot ranges - if (drivingGP.triangle.justPressed()) { - robot.shoot.activateRange(1); - } - if (drivingGP.square.justPressed()) { - robot.shoot.activateRange(2); - } - if (drivingGP.cross.justPressed()) { - robot.shoot.activateRange(3); - } - if (drivingGP.circle.justPressed()) { - 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 - 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) - if (now - lastRecordTime >= RECORD_INTERVAL_SEC) { - try { - recordData(now); - } catch (IOException e) { - telemetry.addData("Recording Error", e.getMessage()); - } - lastRecordTime = now; - } - - _telemetry(); - } - - @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 V3", "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) 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(); - - // 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(); - double rightShtrVel = robot.rightOuttake.getVelocity(); - - // 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", - t, - leftRear.getPower(), - rightRear.getPower(), - leftFront.getPower(), - rightFront.getPower(), - x, - y, - heading, - voltage, - intakeVel, - loaderVel, - leftShtrVel, - rightShtrVel, - 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/DataRecordingOp4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp4.java deleted file mode 100644 index 837f8f1..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp4.java +++ /dev/null @@ -1,319 +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.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; - -/** - * Data Recorder V4 — Correct Frame Recording - * - * Records pose + ROBOT-FRAME smoothed velocities + mechanism states at 50Hz. - * Velocities are computed in the ROBOT'S FRAME during recording, so replay - * does not need to rotate them. This eliminates the field→robot frame - * mismatch bug entirely. - * - * CSV Format: - * Time,X,Y,Heading,Voltage,VxRobot,VyRobot,Omega,IntakePwr,LoaderPwr,LShooterPwr,RShooterPwr,TurretPos,AnglePos,FlapPos - * - * @version 5.0 - */ -@TeleOp(name = "Data Recorder V4", group = "Replay") -public class DataRecordingOp4 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 - private FileWriter dataRecorder; - private static final long RECORD_INTERVAL_MS = 20; - private long startTime; - private long lastRecordTime = 0; - - // Velocity smoothing (in ROBOT FRAME) - private double smoothedVxRobot = 0, smoothedVyRobot = 0, smoothedOmega = 0; - private static final double VEL_SMOOTH_ALPHA = 0.35; - - // Previous pose for velocity computation - private double prevX = 0, prevY = 0, prevHeading = 0; - private long prevPoseTime = 0; - private boolean firstPose = true; - - // Wheel velocity recording (optional debug) - 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); - } - - 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); - } - - // Get motors and LOG THEIR DIRECTIONS for replay verification - leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); - rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); - leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); - rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); - - telemetry.addLine("=== COPY THESE DIRECTIONS TO REPLAY CODE ==="); - telemetry.addData("LF", leftFront.getDirection().toString()); - telemetry.addData("RF", rightFront.getDirection().toString()); - telemetry.addData("LR", leftRear.getDirection().toString()); - telemetry.addData("RR", rightRear.getDirection().toString()); - telemetry.addLine("============================================"); - telemetry.update(); - - String filePath = Environment.getExternalStorageDirectory().getPath() + "/robot_data_v4.csv"; - try { - dataRecorder = new FileWriter(filePath); - dataRecorder.write("Time,X,Y,Heading,Voltage,VxRobot,VyRobot,Omega,IntakePwr,LoaderPwr,LShooterPwr,RShooterPwr,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 V4)"); - telemetry.addLine("Verify motor directions above match replay code!"); - telemetry.update(); - } - - @Override - public void start() { - robot.follower.startTeleopDrive(); - startTime = System.currentTimeMillis(); - firstPose = true; - } - - @Override - public void loop() { - long now = System.currentTimeMillis(); - lpsCounter.getLoopTime(); - - drivingGP.update(); - utilityGP.update(); - robot.follower.update(); - - // ===== MECHANISM CONTROL (same as before) ===== - robot.intake.speed = utilityGP.rightStick.y; - robot.intake.reversed = INTAKE_REVERSE; - turretAligner.update(); - - 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; - } - - // Turret/Angle aiming - if (autoAimEnabled) { - // To do - } else { - if (drivingGP.dpadLeft.pressed()) { - if (turretTimer.seconds() == 0) turretTimer.reset(); - double increment = turretTimer.seconds() > 1.5 ? 0.1 : - turretTimer.seconds() > 1.0 ? 0.07 : 0.03; - robot.turret.driverOffset += increment; - } else if (drivingGP.dpadRight.pressed()) { - if (turretTimer.seconds() == 0) turretTimer.reset(); - double decrement = turretTimer.seconds() > 1.5 ? 0.1 : - turretTimer.seconds() > 1.0 ? 0.07 : 0.03; - robot.turret.driverOffset -= decrement; - } else { - turretTimer.reset(); - } - - if (drivingGP.dpadUp.pressed()) - robot.outtake.activeConfig.angle += 0.01; - else if (drivingGP.dpadDown.pressed()) - robot.outtake.activeConfig.angle -= 0.01; - } - - // Shoot presets - 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); - } - - // Rumble when shooter ready - 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; - } - } - - 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(now); - } catch (IOException e) { - telemetry.addData("Recording Error", e.getMessage()); - } - lastRecordTime = now; - } - - _telemetry(); - } - - @Override - public void stop() { - robot.webcam.stop(); - if (dataRecorder != null) { - try { - dataRecorder.flush(); - dataRecorder.close(); - } catch (IOException ignored) {} - } - } - - private void recordData(long now) throws IOException { - double t = (now - startTime) / 1000.0; - double x = robot.follower.getPose().getX(); - double y = robot.follower.getPose().getY(); - double heading = robot.follower.getHeading(); - double voltage = hardwareMap.voltageSensor.iterator().next().getVoltage(); - - // Compute raw velocity from pose delta (in FIELD FRAME) - double rawVxField = 0, rawVyField = 0, rawOmega = 0; - if (!firstPose) { - double dt = (now - prevPoseTime) / 1000.0; - if (dt > 0.001) { - rawVxField = (x - prevX) / dt; - rawVyField = (y - prevY) / dt; - rawOmega = normalizeAngle(heading - prevHeading) / dt; - } - } else { - firstPose = false; - } - - // Rotate field-frame velocity into ROBOT FRAME - double cosH = Math.cos(heading); - double sinH = Math.sin(heading); - double rawVxRobot = cosH * rawVxField + sinH * rawVyField; - double rawVyRobot = -sinH * rawVxField + cosH * rawVyField; - - // Exponential smoothing (in ROBOT FRAME) - smoothedVxRobot = smoothedVxRobot + VEL_SMOOTH_ALPHA * (rawVxRobot - smoothedVxRobot); - smoothedVyRobot = smoothedVyRobot + VEL_SMOOTH_ALPHA * (rawVyRobot - smoothedVyRobot); - smoothedOmega = smoothedOmega + VEL_SMOOTH_ALPHA * (rawOmega - smoothedOmega); - - // Update previous pose - prevX = x; - prevY = y; - prevHeading = heading; - prevPoseTime = now; - - dataRecorder.write(String.format(Locale.US, - "%.3f,%.4f,%.4f,%.4f,%.2f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n", - t, x, y, heading, voltage, - smoothedVxRobot, smoothedVyRobot, smoothedOmega, - robot.intakeMotor.getPower(), - robot.loaderMotor.getPower(), - robot.leftOuttake.getPower(), - robot.rightOuttake.getPower(), - robot.turretServo.getPosition(), - robot.angleServo.getPosition(), - robot.flapsServo.getPosition() - )); - } - - private static double normalizeAngle(double a) { - while (a > Math.PI) a -= 2 * Math.PI; - while (a < -Math.PI) a += 2 * Math.PI; - return a; - } - - public void _telemetry() { - telemetry.addData("LPS", "%.1f", 1 / lpsCounter.delta); - telemetry.addData("Recording", "V4 ACTIVE (robot-frame velocities)"); - telemetry.addData("x", robot.follower.getPose().getX()); - telemetry.addData("y", robot.follower.getPose().getY()); - telemetry.addData("heading", Math.toDegrees(robot.follower.getPose().getHeading())); - telemetry.addData("robot V", "%.1f, %.1f, %.1f°/s", smoothedVxRobot, smoothedVyRobot, Math.toDegrees(smoothedOmega)); - telemetry.addData("shooter vel", robot.leftOuttake.getVelocity()); - telemetry.addData("turret pos", robot.turretServo.getPosition()); - robot.intake.telemetry(telemetry); - robot.loader.telemetry(telemetry); - robot.outtake.telemetry(telemetry); - drivingGP.telemetry(telemetry); - telemetry.update(); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp7.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp7.java deleted file mode 100644 index d0b15c9..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp7.java +++ /dev/null @@ -1,283 +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.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; - -/** - * Data Recorder V7 — Driver Input Feedforward + Continuous Blending Replay - * - * Records the ACTUAL DRIVER INPUTS (gamepad stick values) that caused the motion, - * plus pose for correction reference. - * - * CSV Format: - * Time,X,Y,Heading,Voltage, - * GamepadFwd,GamepadStr,GamepadTurn, // [-1, 1] robot-frame stick values - * IntakePwr,LoaderPwr,LShooterPwr,RShooterPwr, - * TurretPos,AnglePos,FlapPos - * - * @version 7.0 - */ -@TeleOp(name = "Data Recorder V7", group = "Replay") -public class DataRecordingOp7 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 - private FileWriter dataRecorder; - private static final long RECORD_INTERVAL_MS = 20; - private long startTime; - private long lastRecordTime = 0; - - // Wheel velocity recording (optional debug) - 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); - } - - 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); - } - - // Get motors and LOG THEIR DIRECTIONS for replay verification - leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); - rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); - leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); - rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); - - telemetry.addLine("=== COPY THESE DIRECTIONS TO REPLAY CODE ==="); - telemetry.addData("LF", leftFront.getDirection().toString()); - telemetry.addData("RF", rightFront.getDirection().toString()); - telemetry.addData("LR", leftRear.getDirection().toString()); - telemetry.addData("RR", rightRear.getDirection().toString()); - telemetry.addLine("============================================"); - telemetry.update(); - - String filePath = Environment.getExternalStorageDirectory().getPath() + "/robot_data_v7.csv"; - try { - dataRecorder = new FileWriter(filePath); - dataRecorder.write("Time,X,Y,Heading,Voltage,GamepadFwd,GamepadStr,GamepadTurn,IntakePwr,LoaderPwr,LShooterPwr,RShooterPwr,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 V7)"); - telemetry.addLine("Verify motor directions above match replay code!"); - telemetry.update(); - } - - @Override - public void start() { - robot.follower.startTeleopDrive(); - startTime = System.currentTimeMillis(); - } - - @Override - public void loop() { - long now = System.currentTimeMillis(); - lpsCounter.getLoopTime(); - - drivingGP.update(); - utilityGP.update(); - robot.follower.update(); - - // ===== CAPTURE DRIVER INPUTS (the feedforward signal) ===== - // These are the EXACT values passed to setTeleOpDrive - // Note: setTeleOpDrive takes (forward, strafe, turn, robotCentric=true) - // We negate Y because gamepad Y is up=negative in FTC - double gamepadFwd = -drivingGP.leftStick.y; // forward/back - double gamepadStr = -drivingGP.leftStick.x; // strafe - double gamepadTurn = -drivingGP.rightStick.x; // turn - - // ===== MECHANISM CONTROL (same as before) ===== - robot.intake.speed = utilityGP.rightStick.y; - robot.intake.reversed = INTAKE_REVERSE; - turretAligner.update(); - - 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; - } - - // Turret/Angle aiming - if (autoAimEnabled) { - // To do - } else { - if (drivingGP.dpadLeft.pressed()) { - if (turretTimer.seconds() == 0) turretTimer.reset(); - double increment = turretTimer.seconds() > 1.5 ? 0.1 : - turretTimer.seconds() > 1.0 ? 0.07 : 0.03; - robot.turret.driverOffset += increment; - } else if (drivingGP.dpadRight.pressed()) { - if (turretTimer.seconds() == 0) turretTimer.reset(); - double decrement = turretTimer.seconds() > 1.5 ? 0.1 : - turretTimer.seconds() > 1.0 ? 0.07 : 0.03; - robot.turret.driverOffset -= decrement; - } else { - turretTimer.reset(); - } - - if (drivingGP.dpadUp.pressed()) - robot.outtake.activeConfig.angle += 0.01; - else if (drivingGP.dpadDown.pressed()) - robot.outtake.activeConfig.angle -= 0.01; - } - - // Shoot presets - 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); - } - - // Rumble when shooter ready - 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; - } - } - - // Pass inputs to PedroPathing (same as always) - robot.follower.setTeleOpDrive(gamepadFwd, gamepadStr, gamepadTurn, true); - robot.updateAllSystems(); - - // ===== RECORD DATA ===== - if (now - lastRecordTime >= RECORD_INTERVAL_MS) { - try { - recordData(now, gamepadFwd, gamepadStr, gamepadTurn); - } catch (IOException e) { - telemetry.addData("Recording Error", e.getMessage()); - } - lastRecordTime = now; - } - - _telemetry(gamepadFwd, gamepadStr, gamepadTurn); - } - - @Override - public void stop() { - robot.webcam.stop(); - if (dataRecorder != null) { - try { - dataRecorder.flush(); - dataRecorder.close(); - } catch (IOException ignored) {} - } - } - - private void recordData(long now, double gpFwd, double gpStr, double gpTurn) throws IOException { - double t = (now - startTime) / 1000.0; - double x = robot.follower.getPose().getX(); - double y = robot.follower.getPose().getY(); - double heading = robot.follower.getHeading(); - double voltage = hardwareMap.voltageSensor.iterator().next().getVoltage(); - - dataRecorder.write(String.format(Locale.US, - "%.3f,%.4f,%.4f,%.4f,%.2f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n", - t, x, y, heading, voltage, - gpFwd, gpStr, gpTurn, - robot.intakeMotor.getPower(), - robot.loaderMotor.getPower(), - robot.leftOuttake.getPower(), - robot.rightOuttake.getPower(), - robot.turretServo.getPosition(), - robot.angleServo.getPosition(), - robot.flapsServo.getPosition() - )); - } - - public void _telemetry(double gpFwd, double gpStr, double gpTurn) { - telemetry.addData("LPS", "%.1f", 1 / lpsCounter.delta); - telemetry.addData("Recording", "V7 ACTIVE (input feedforward)"); - telemetry.addData("Inputs", "fwd=%.2f str=%.2f turn=%.2f", gpFwd, gpStr, gpTurn); - telemetry.addData("x", robot.follower.getPose().getX()); - telemetry.addData("y", robot.follower.getPose().getY()); - telemetry.addData("heading", Math.toDegrees(robot.follower.getPose().getHeading())); - telemetry.addData("shooter vel", robot.leftOuttake.getVelocity()); - telemetry.addData("turret pos", robot.turretServo.getPosition()); - robot.intake.telemetry(telemetry); - robot.loader.telemetry(telemetry); - robot.outtake.telemetry(telemetry); - drivingGP.telemetry(telemetry); - telemetry.update(); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp8.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp8.java deleted file mode 100644 index 2375529..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOp8.java +++ /dev/null @@ -1,302 +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.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; - -/** - * Data Recorder V8 — Exact Match to MainDrivingOp - * - * Controls and mechanisms behave IDENTICALLY to MainDrivingOp. - * Records driver inputs + pose + all mechanism states. - * - * CSV Format: - * Time,X,Y,Heading,Voltage, - * GamepadFwd,GamepadStr,GamepadTurn, - * IntakePwr,LoaderPwr,LShooterPwr,RShooterPwr, - * TurretPos,AnglePos,FlapPos, - * AutoAimEnabled,BlueTarget - */ -@TeleOp(name = "Data Recorder V8", group = "Replay") -public class DataRecordingOp8 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 - private FileWriter dataRecorder; - private static final long RECORD_INTERVAL_MS = 20; - private long startTime; - private long lastRecordTime = 0; - - // Wheel velocity recording (optional debug) - 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); - } - - 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); - } - - // Get motors and LOG THEIR DIRECTIONS for replay verification - leftFront = hardwareMap.get(DcMotorEx.class, "leftFront"); - rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); - leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); - rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); - - telemetry.addLine("=== COPY THESE DIRECTIONS TO REPLAY CODE ==="); - telemetry.addData("LF", leftFront.getDirection().toString()); - telemetry.addData("RF", rightFront.getDirection().toString()); - telemetry.addData("LR", leftRear.getDirection().toString()); - telemetry.addData("RR", rightRear.getDirection().toString()); - telemetry.addLine("============================================"); - telemetry.update(); - - String filePath = Environment.getExternalStorageDirectory().getPath() + "/robot_data_v8.csv"; - try { - dataRecorder = new FileWriter(filePath); - dataRecorder.write("Time,X,Y,Heading,Voltage,GamepadFwd,GamepadStr,GamepadTurn,IntakePwr,LoaderPwr,LShooterPwr,RShooterPwr,TurretPos,AnglePos,FlapPos,AutoAim,BlueTarget\n"); - } catch (IOException e) { - telemetry.addData("Error initializing recorder", e.getMessage()); - } - } - - @Override - public void init_loop() { - lpsCounter.getLoopTime(); - telemetry.addLine("Initialization Ready (Recording V8)"); - telemetry.addLine("Verify motor directions above match replay code!"); - telemetry.update(); - } - - @Override - public void start() { - robot.follower.startTeleopDrive(); - startTime = System.currentTimeMillis(); - } - - @Override - public void loop() { - long now = System.currentTimeMillis(); - lpsCounter.getLoopTime(); - - drivingGP.update(); - utilityGP.update(); - robot.follower.update(); - - // ===== CAPTURE DRIVER INPUTS (the feedforward signal) ===== - // EXACTLY as passed to setTeleOpDrive in MainDrivingOp - double gamepadFwd = -drivingGP.leftStick.y; - double gamepadStr = -drivingGP.leftStick.x; - double gamepadTurn = -drivingGP.rightStick.x; - - // ===== MECHANISM CONTROL — EXACT COPY OF 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/Angle aiming — EXACT copy from MainDrivingOp - 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(); - } - - // Auto-aim toggles — EXACT copy from MainDrivingOp - if (drivingGP.dpadDown.justPressed()) - robot.turret.autoAimEnabled = !robot.turret.autoAimEnabled; - - if (drivingGP.dpadUp.justPressed()) - autoAimEnabled = !autoAimEnabled; - - if (autoAimEnabled) - robot.shoot.activateRange(0); - - // Shoot presets — EXACT copy from MainDrivingOp - if (drivingGP.triangle.justPressed()) { - robot.shoot.activateRange(1); - } - if (drivingGP.square.justPressed()) { - robot.shoot.activateRange(2); - } - if (drivingGP.cross.justPressed()) { - robot.shoot.activateRange(3); - } - if (drivingGP.circle.justPressed()) { - robot.shoot.activateRange(4); - } - - // Shooter ready rumble — EXACT copy from MainDrivingOp - 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; - } - - // Left bumper — EXACT copy from MainDrivingOp - if (!autoAimEnabled && drivingGP.leftBumper.justPressed()) { - robot.turret.autoAimEnabled = true; - if (robot.outtake.on) { - robot.shoot.deactivate(); - gamepad1.rumble(1, 1, 100); - rumbled = false; - } - } - - // Blue target toggle — EXACT copy from MainDrivingOp - if (drivingGP.rightStick.button.justPressed()) - robot.Blue_Target = !robot.Blue_Target; - - // Update robot systems — EXACT copy from MainDrivingOp - robot.follower.setTeleOpDrive(gamepadFwd, gamepadStr, gamepadTurn, true); - robot.updateAllSystems(); - - // ===== RECORD DATA ===== - if (now - lastRecordTime >= RECORD_INTERVAL_MS) { - try { - recordData(now, gamepadFwd, gamepadStr, gamepadTurn); - } catch (IOException e) { - telemetry.addData("Recording Error", e.getMessage()); - } - lastRecordTime = now; - } - - _telemetry(gamepadFwd, gamepadStr, gamepadTurn); - } - - @Override - public void stop() { - robot.webcam.stop(); - if (dataRecorder != null) { - try { - dataRecorder.flush(); - dataRecorder.close(); - } catch (IOException ignored) {} - } - } - - private void recordData(long now, double gpFwd, double gpStr, double gpTurn) throws IOException { - double t = (now - startTime) / 1000.0; - double x = robot.follower.getPose().getX(); - double y = robot.follower.getPose().getY(); - double heading = robot.follower.getHeading(); - double voltage = hardwareMap.voltageSensor.iterator().next().getVoltage(); - - dataRecorder.write(String.format(Locale.US, - "%.3f,%.4f,%.4f,%.4f,%.2f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f,%d,%d\n", - t, x, y, heading, voltage, - gpFwd, gpStr, gpTurn, - robot.intakeMotor.getPower(), - robot.loaderMotor.getPower(), - robot.leftOuttake.getPower(), - robot.rightOuttake.getPower(), - robot.turretServo.getPosition(), - robot.angleServo.getPosition(), - robot.flapsServo.getPosition(), - autoAimEnabled ? 1 : 0, - robot.Blue_Target ? 1 : 0 - )); - } - - public void _telemetry(double gpFwd, double gpStr, double gpTurn) { - telemetry.addData("LPS", "%.1f", 1 / lpsCounter.delta); - telemetry.addData("Recording", "V8 ACTIVE (input feedforward)"); - telemetry.addData("Inputs", "fwd=%.2f str=%.2f turn=%.2f", gpFwd, gpStr, gpTurn); - telemetry.addData("x", robot.follower.getPose().getX()); - telemetry.addData("y", robot.follower.getPose().getY()); - telemetry.addData("heading", Math.toDegrees(robot.follower.getPose().getHeading())); - telemetry.addData("shooter vel", robot.leftOuttake.getVelocity()); - telemetry.addData("turret pos", robot.turretServo.getPosition()); - telemetry.addData("autoAim", autoAimEnabled); - telemetry.addData("BlueTarget", robot.Blue_Target); - robot.intake.telemetry(telemetry); - robot.loader.telemetry(telemetry); - robot.outtake.telemetry(telemetry); - drivingGP.telemetry(telemetry); - telemetry.update(); - } -} 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 index b0d592c..b478bd4 100644 --- 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 @@ -61,7 +61,7 @@ * * @version 3.4 */ -@TeleOp(name = "FINAL Recorder", group = "Replay") +@TeleOp(name = "Recorder Vlad", group = "Replay") public class FinalRecorderOp extends OpMode { private final Robot robot = Robot.getInstance(); private Controls drivingGP; @@ -126,7 +126,7 @@ public void init() { // first recorded frame, so this is fine. // Initialize Data Recorder — V3.4 header (27 columns) - String filePath = Environment.getExternalStorageDirectory().getPath() + "/robot_data.csv"; + 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"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOpV3FIXED.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/RecorderOpRob.java similarity index 77% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOpV3FIXED.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/RecorderOpRob.java index a68d9d9..1ee9b4a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/DataRecordingOpV3FIXED.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kronbot/manual/RecorderOpRob.java @@ -3,10 +3,6 @@ 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.RANGE_1_VELOCITY; -import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_2_VELOCITY; -import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_3_VELOCITY; -import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RANGE_4_VELOCITY; import static org.firstinspires.ftc.teamcode.kronbot.utils.Constants.RedTowerCoords; import android.os.Environment; @@ -27,35 +23,39 @@ import java.util.Locale; /** - * TeleOp data recorder (V3.2). + * TeleOp data recorder (V3.3). * * 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 (22 total): + * 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 + * IntakeCmd, LoaderCmd, FlapOpen, ShootRange, TurretOffset, BlueTarget, + * AutoAimEnabled * - * The first 16 columns are the V3 mechanism-state log (for direct servo - * and velocity control). The last 6 columns are the high-level mechanism - * commands the TeleOp applied — these let the replay call the same - * high-level API (intake.speed, shoot.activateRange, etc.) and let - * robot.updateAllSystems() drive the motors, exactly like the TeleOp. + * 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.2 changes: - * - Fixed loader scalar: was * 0.9, now * 0.8 (matches MainDrivingOp) - * - Added rightStick.button toggle for Blue_Target (matches MainDrivingOp) - * - Removed turretAligner.update() from loop (TeleOp doesn't call it) - * - Webcam init commented out (matches MainDrivingOp) - * - Added 6 high-level mechanism command columns for replay parity - * - * @version 3.2 + * @version 3.3 */ -@TeleOp(name = "RECORDERRRR", group = "Replay") -public class DataRecordingOpV3FIXED 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; @@ -81,6 +81,12 @@ public class DataRecordingOpV3FIXED 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(); @@ -106,19 +112,17 @@ public void init() { leftRear = hardwareMap.get(DcMotorEx.class, "leftRear"); rightRear = hardwareMap.get(DcMotorEx.class, "rightRear"); - // Reset IMU for a clean starting pose — done before init_loop so - // the driver doesn't notice any difference from MainDrivingOp. - try { - robot.follower.getPoseTracker().resetIMU(); - } catch (InterruptedException e) { - throw new RuntimeException(e); - } + // 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.2 header (22 columns) - String filePath = Environment.getExternalStorageDirectory().getPath() + "/robot_data.csv"; + // 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,IntakeCmd,LoaderCmd,FlapOpen,ShootRange,TurretOffset,BlueTarget\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()); } @@ -128,7 +132,7 @@ public void init() { public void init_loop() { lpsCounter.getLoopTime(); - telemetry.addLine("Initialization Ready (Recording V3.2 Enabled)"); + telemetry.addLine("Initialization Ready (Recording V3.3 Enabled)"); telemetry.update(); } @@ -211,15 +215,19 @@ else if (robot.loader.speed < -0.2) //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 && @@ -233,6 +241,7 @@ 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; } @@ -273,7 +282,7 @@ public void stop() { public void _telemetry() { telemetry.addData("LPS", "%.1f", 1 / lpsCounter.delta); - telemetry.addData("Recording V3.2", "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()); @@ -309,19 +318,20 @@ private void recordData(double t) throws IOException { double anglePos = robot.angleServo.getPosition(); double flapPos = robot.flapsServo.getPosition(); - // High-level mechanism commands (V3.2 columns) — what the TeleOp + // 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 = deriveShootRange(); + 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; 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\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(), @@ -343,33 +353,8 @@ private void recordData(double t) throws IOException { flapOpen, shootRange, turretOffset, - blueTarget + blueTarget, + autoAim )); } - - /** - * Derives the current shoot range number (1-4) from the active config. - * - * RangeConfig doesn't store the range number itself, and Outtake doesn't - * track which range is active — that info is only known inside - * Shoot.activateRange() at the moment of the call. So we recover it by - * matching activeConfig.velocity against the known RANGE_X_VELOCITY - * constants. Returns 0 for the auto-aim interpolated range (case 0 in - * Shoot.activateRange) and -1 when the outtake is off. - * - * Brittle: if you change RANGE_X_VELOCITY, the matching may break. - * Clean fix: add {@code public int activeRange = -1;} to Robot.Outtake - * and set it in Shoot.activateRange / deactivate, then read - * {@code robot.outtake.activeRange} here directly. - */ - private int deriveShootRange() { - if (!robot.outtake.on) return -1; - double vel = robot.outtake.activeConfig.velocity; - double eps = 1.0; // velocity tolerance in ticks/sec - if (Math.abs(vel - RANGE_1_VELOCITY) < eps) return 1; - if (Math.abs(vel - RANGE_2_VELOCITY) < eps) return 2; - if (Math.abs(vel - RANGE_3_VELOCITY) < eps) return 3; - if (Math.abs(vel - RANGE_4_VELOCITY) < eps) return 4; - return 0; // auto-aim interpolated range - } } \ No newline at end of file From a8a4a475639b5577ee1823fd34af33fe881ab516 Mon Sep 17 00:00:00 2001 From: Cozma Vlad Date: Fri, 24 Jul 2026 14:48:38 +0300 Subject: [PATCH 5/6] Update constants for replay position error. --- .../autonomous/AutonomousConstants.java | 16 ++++++++ .../kronbot/autonomous/FinalReplayOp.java | 37 +++++++++++-------- 2 files changed, 37 insertions(+), 16 deletions(-) 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..6c22bb1 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,6 +65,22 @@ 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.45; + + + public static Pose coordinates(Coordinates coord) { return new Pose(coord.x, coord.y, coord.heading); } 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 index 24adbdd..022e4f8 100644 --- 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 @@ -1,5 +1,14 @@ 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; @@ -44,21 +53,6 @@ public class FinalReplayOp extends LinearOpMode { private static final double VOLTAGE_REFRESH_SEC = 0.10; private static final double TIME_SCALING_FACTOR = 0.5; // adjust replay speed for voltage drops - // PD Translation (x, y). These stay low because recorded driver input is - // the main motion command; PD only corrects odometry error. - public static double kP_translation = 0.018; - public static double kD_translation = 0.010; - - // PD Rotation (heading) - public static double kP_rotation = 0.28; - public static double kD_rotation = 0.07; - - // Constraints - private static final double BLEND_K = 0.35; - private static final double MIN_CORRECTION_CAP = 0.20; - private static final double MAX_CORRECTION_CAP = 0.60; - private static final double CORRECTION_ERROR_SCALE = 8.0; - private static final double D_FILTER_ALPHA = 0.45; 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; @@ -160,6 +154,10 @@ private void executePlayback() { prevRobotStr = 0; prevRobotTurn = 0; + double posErrorSum = 0; + double posErrorPeak = 0; + int posErrorSamples = 0; + robot.follower.startTeleopDrive(); while (opModeIsActive() && idx < recordedFrames.size() - 1) { @@ -227,6 +225,11 @@ private void executePlayback() { 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); @@ -277,7 +280,9 @@ private void executePlayback() { telemetry.addData("Time", "%.2f / %.2f s", now, duration); telemetry.addData("Lookahead", "%.3f s", lookahead); telemetry.addData("PosErr", "%.2f cm", posError); - telemetry.addData("HeadErr", "%.1f °", Math.toDegrees(Math.abs(eh))); + 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); From 7c0023f3f6891aea14dd671579ac0fbbce56d98b Mon Sep 17 00:00:00 2001 From: Robi2903 <113847997+Robi2903@users.noreply.github.com> Date: Mon, 27 Jul 2026 14:49:11 +0300 Subject: [PATCH 6/6] LETS GOOOOOOOOOOOOOOOOOOOOOOO --- .../kronbot/autonomous/AutonomousConstants.java | 4 ++-- .../kronbot/autonomous/FinalReplayOp.java | 16 +++++++++++----- .../ftc/teamcode/kronbot/utils/Constants.java | 4 ++-- 3 files changed, 15 insertions(+), 9 deletions(-) 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 6c22bb1..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 @@ -77,7 +77,7 @@ public Coordinates(double x, double y, double heading) { 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.45; + public static double D_FILTER_ALPHA = 0.25; @@ -85,4 +85,4 @@ 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 index 022e4f8..55d0dc3 100644 --- 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 @@ -48,7 +48,7 @@ public class FinalReplayOp extends LinearOpMode { private static final String CSV_PATH = "/sdcard/robot_data_Vlad.csv"; // Playback tuning - private static final double LOOKAHEAD_TIME = 0.080; // 80ms lookahead while moving + 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 @@ -213,9 +213,12 @@ private void executePlayback() { double exRobot = cosH * exField + sinH * eyField; double eyRobot = -sinH * exField + cosH * eyField; - double rawDx = (exRobot - prevErrorX) / dt; - double rawDy = (eyRobot - prevErrorY) / dt; - double rawDh = (eh - prevErrorHeading) / dt; + // 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); @@ -244,7 +247,10 @@ private void executePlayback() { double robotFwd = ffFwd + corrWeight * corrFwd; double robotStr = ffStr + corrWeight * corrStr; - double robotTurn = ffTurn + corrWeight * corrTurn; + // 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) { 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;