From bfcecd42d3bf3c55a5d55567a68d1538c85c6b2e Mon Sep 17 00:00:00 2001 From: KeshavAnandCode Date: Thu, 15 Jan 2026 21:04:56 -0600 Subject: [PATCH] @Abhiram pls fix this --- .../ftc/teamcode/teleop/TeleopV3.java | 239 +++++++++--------- .../ftc/teamcode/tests/PIDServoTest.java | 3 +- .../ftc/teamcode/utils/FlywheelV2.java | 14 +- .../ftc/teamcode/utils/Servos.java | 2 +- 4 files changed, 128 insertions(+), 130 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index d404815..9e29141 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -3,6 +3,8 @@ package org.firstinspires.ftc.teamcode.teleop; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; +import static org.firstinspires.ftc.teamcode.utils.Servos.*; +import static org.firstinspires.ftc.teamcode.utils.Servos.spin_scalar; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; @@ -13,10 +15,12 @@ import com.acmerobotics.roadrunner.TrajectoryActionBuilder; import com.acmerobotics.roadrunner.TranslationalVelConstraint; import com.acmerobotics.roadrunner.Vector2d; import com.acmerobotics.roadrunner.ftc.Actions; +import com.arcrobotics.ftclib.controller.PIDFController; import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.PIDFCoefficients; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; @@ -47,6 +51,9 @@ public class TeleopV3 extends LinearOpMode { public boolean circle = false; public boolean square = false; public boolean triangle = false; + + PIDFController spinPID = new PIDFController(spinP, spinI, spinD, spinF); + public TranslationalVelConstraint VEL_CONSTRAINT = new TranslationalVelConstraint(200); public ProfileAccelConstraint ACCEL_CONSTRAINT = new ProfileAccelConstraint(-Math.abs(60), 200); Robot robot; @@ -85,7 +92,7 @@ public class TeleopV3 extends LinearOpMode { double turretPID = 0.0; double turretPos = 0.5; double spindexPID = 0.0; - double spindexPos = spindexer_intakePos1; + public static double spindexPos = spindexer_intakePos1; double error = 0.0; double spinCurrentPos = 0.0, spinInitPos = 0.0, intakeStamp = 0.0; boolean reverse = false; @@ -97,6 +104,21 @@ public class TeleopV3 extends LinearOpMode { private int tickerA = 1; private boolean transferIn = false; + public static double velPrediction(double distance) { + if (distance < 30) { + return 2750; + } else if (distance > 100) { + if (distance > 160) { + return 4200; + } + return 3700; + } else { + // linear interpolation between 40->2650 and 120->3600 + double slope = (3700.0 - 2750.0) / (100.0 - 30); + return (int) Math.round(2750 + slope * (distance - 30)); + } + } + @Override public void runOpMode() throws InterruptedException { List allHubs = hardwareMap.getAll(LynxModule.class); @@ -180,11 +202,16 @@ public class TeleopV3 extends LinearOpMode { // robot.turr1.setPower(turretPID); // robot.turr2.setPower(-turretPID); -// if (!servo.spinEqual(spindexPos) && !gamepad1.right_bumper) { -// spindexPID = servo.setSpinPos(spindexPos); -// robot.spin1.setPower(spindexPID); -// robot.spin2.setPower(-spindexPID); -// } + if (!servo.spinEqual(spindexPos) && !gamepad1.right_bumper) { + spinCurrentPos = servo.getSpinPos(); + + double spindexPID = spinPID.calculate(spinCurrentPos, spindexPos); + + robot.spin1.setPower(spindexPID); + robot.spin2.setPower(-spindexPID); + + } + //TODO: Use color sensors to switch between positions...switch after ball detected in @@ -199,7 +226,7 @@ public class TeleopV3 extends LinearOpMode { } spinInitPos = spinCurrentPos; } - if (reverse && intakeTicker % intakeJamSwap < (intakeJamSwap/2)) { + if (reverse && intakeTicker % intakeJamSwap < (intakeJamSwap / 2)) { robot.spin1.setPower(1); robot.spin2.setPower(-1); } else if (reverse) { @@ -214,8 +241,19 @@ public class TeleopV3 extends LinearOpMode { TELE.addData("Reverse?", reverse); TELE.update(); } else { - robot.spin1.setPower(0); - robot.spin2.setPower(0); + if (!servo.spinEqual(spindexPos)) { + spinCurrentPos = servo.getSpinPos(); + + double spindexPID = spinPID.calculate(spinCurrentPos, spindexPos); + + robot.spin1.setPower(spindexPID); + robot.spin2.setPower(-spindexPID); + + } else { + + robot.spin1.setPower(0); + robot.spin2.setPower(0); + } if (getRuntime() - intakeStamp < 1) { robot.intake.setPower(-(getRuntime() - intakeStamp) * 2); @@ -226,41 +264,6 @@ public class TeleopV3 extends LinearOpMode { intakeTicker = 0; } -// // INTAKE -// if (gamepad1.right_bumper) { -// robot.transferServo.setPosition(transferServo_out); -//// intakeTicker++; -//// if (intakeTicker % 16 == 0) { -//// spinCurrentPos = servo.getSpinPos(); -//// if (Math.abs(spinCurrentPos - spinInitPos) == 0.0) { -//// reverse = !reverse; -//// } -//// spinInitPos = spinCurrentPos; -//// } -// if (reverse) { -// robot.spin1.setPower(spinningPow); -// robot.spin2.setPower(-spinningPow); -// } else { -// robot.spin1.setPower(-spinningPow); -// robot.spin2.setPower(spinningPow); -// } -// robot.intake.setPower(1); -// intakeStamp = getRuntime(); -// TELE.addData("Reverse?", reverse); -// -// } else if (gamepad1.left_bumper) { -// robot.intake.setPower(-(getRuntime() - intakeStamp) * 2); -// spindexPos = spindexer_intakePos2; -// intakeTicker = 0; -// } else { -// if (getRuntime() - intakeStamp < 1) { -// robot.intake.setPower(-(getRuntime() - intakeStamp) * 2); -// } else { -// robot.intake.setPower(0); -// } -// intakeTicker = 0; -// } - //COLOR: double s1D = robot.color1.getDistance(DistanceUnit.MM); @@ -276,8 +279,9 @@ public class TeleopV3 extends LinearOpMode { double gP = green / (green + red + blue); s1G.add(gP); + TELE.addData("gp1", gP); - if (gP >= 0.43) { + if (gP >= 0.36) { s1.add(true); } else { s1.add(false); @@ -296,6 +300,7 @@ public class TeleopV3 extends LinearOpMode { double gP = green / (green + red + blue); s2G.add(gP); + TELE.addData("gp2", gP); if (gP >= 0.43) { s2.add(true); @@ -314,6 +319,8 @@ public class TeleopV3 extends LinearOpMode { double gP = green / (green + red + blue); + TELE.addData("gp3", gP); + s3G.add(gP); if (gP >= 0.43) { @@ -336,42 +343,66 @@ public class TeleopV3 extends LinearOpMode { green3 = checkGreen(s3, s3T); } -// //SHOOTER: -// -// double powPID = flywheel.manageFlywheel((int) vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); -// -// robot.shooter1.setPower(powPID); -// robot.shooter2.setPower(powPID); -// -// robot.transfer.setPower(1); -// -// double offset; -// -// double robX = drive.localizer.getPose().position.x; -// double robY = drive.localizer.getPose().position.y; -// -// double robotX = robX - xOffset; -// double robotY = robY - yOffset; -// double robotHeading = drive.localizer.getPose().heading.toDouble(); -// -// double goalX = -10; -// double goalY = 0; -// -// double dx = goalX - robotX; // delta x from robot to goal -// double dy = goalY - robotY; // delta y from robot to goal -// -// double distanceToGoal = Math.sqrt(dx * dx + dy * dy); -// -// desiredTurretAngle = (Math.toDegrees(Math.atan2(dy, dx)) + 360) % 360; -// -// desiredTurretAngle += manualOffset; -// -// offset = desiredTurretAngle - 180 - (Math.toDegrees(robotHeading - headingOffset)); -// -// if (offset > 135) { -// offset -= 360; -// } -// + robot.transfer.setPower(1); + + double offset; + + double robX = drive.localizer.getPose().position.x; + double robY = drive.localizer.getPose().position.y; + + double robotX = robX - xOffset; + double robotY = robY - yOffset; + double robotHeading = drive.localizer.getPose().heading.toDouble(); + + double goalX = -10; + double goalY = 0; + + double dx = goalX - robotX; // delta x from robot to goal + double dy = goalY - robotY; // delta y from robot to goal + + double distanceToGoal = Math.sqrt(dx * dx + dy * dy); + + desiredTurretAngle = (Math.toDegrees(Math.atan2(dy, dx)) + 360) % 360; + + desiredTurretAngle += manualOffset; + + offset = desiredTurretAngle - 180 - (Math.toDegrees(robotHeading - headingOffset)); + + if (offset > 135) { + offset -= 360; + } + + //SHOOTER: + + double powPID = flywheel.manageFlywheel((int) vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition()); + + robot.shooter1.setPower(powPID); + robot.shooter2.setPower(powPID); + + //VELOCITY AUTOMATIC + + if (autoVel) { + vel = velPrediction(distanceToGoal); + } else { + vel = manualVel; + } + + if (gamepad2.right_stick_button) { + autoVel = true; + } else if (gamepad2.right_stick_y < -0.5) { + autoVel = false; + manualVel = 4600; + } else if (gamepad2.right_stick_y > 0.5) { + autoVel = false; + manualVel = 2700; + } else if (gamepad2.right_stick_x > 0.5) { + autoVel = false; + manualVel = 3600; + } else if (gamepad2.right_stick_x < -0.5) { + autoVel = false; + manualVel = 3100; + } + // //TODO: test the camera teleop code // double pos = turrDefault + (error / 8); // adds the overall error to the default // @@ -431,30 +462,7 @@ public class TeleopV3 extends LinearOpMode { // manualOffset += 2; // } // -// //VELOCITY AUTOMATIC -// -// if (autoVel) { -// vel = velPrediction(distanceToGoal); -// } else { -// vel = manualVel; -// } -// -// if (gamepad2.right_stick_button) { -// autoVel = true; -// } else if (gamepad2.right_stick_y < -0.5) { -// autoVel = false; -// manualVel = 4100; -// } else if (gamepad2.right_stick_y > 0.5) { -// autoVel = false; -// manualVel = 2700; -// } else if (gamepad2.right_stick_x > 0.5) { -// autoVel = false; -// manualVel = 3600; -// } else if (gamepad2.right_stick_x < -0.5) { -// autoVel = false; -// manualVel = 3100; -// } -// + // //HOOD: // // if (autoHood) { @@ -682,9 +690,9 @@ public class TeleopV3 extends LinearOpMode { hub.clearBulkCache(); } // -// TELE.addData("Spin1Green", green1 + ": " + ballIn(1)); -// TELE.addData("Spin2Green", green2 + ": " + ballIn(2)); -// TELE.addData("Spin3Green", green3 + ": " + ballIn(3)); + TELE.addData("Spin1Green", green1 + ": " + ballIn(1)); + TELE.addData("Spin2Green", green2 + ": " + ballIn(2)); + TELE.addData("Spin3Green", green3 + ": " + ballIn(3)); TELE.addData("pose", drive.localizer.getPose()); TELE.addData("heading", drive.localizer.getPose().heading.toDouble()); @@ -864,19 +872,4 @@ public class TeleopV3 extends LinearOpMode { return times.get(times.size() - 1) > getRuntime() - 2; } - -// public static double velPrediction(double distance) { -// if (distance < 30) { -// return 2750; -// } else if (distance > 100) { -// if (distance > 160) { -// return 4200; -// } -// return 3700; -// } else { -// // linear interpolation between 40->2650 and 120->3600 -// double slope = (3700.0 - 2750.0) / (100.0 - 30); -// return (int) Math.round(2750 + slope * (distance - 30)); -// } - } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/PIDServoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/PIDServoTest.java index 3286912..9df375c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/PIDServoTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/PIDServoTest.java @@ -6,8 +6,6 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.arcrobotics.ftclib.controller.PIDFController; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.CRServo; -import com.qualcomm.robotcore.hardware.DcMotorSimple; import org.firstinspires.ftc.teamcode.utils.Robot; @@ -71,4 +69,5 @@ public class PIDServoTest extends LinearOpMode { } } + } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/FlywheelV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/FlywheelV2.java index 64a8c72..c414672 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/FlywheelV2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/FlywheelV2.java @@ -73,12 +73,18 @@ public class FlywheelV2 { targetVelocity = commandedVelocity; velo = getVelo(shooter1CurPos, shooter2CurPos); // Flywheel PID code here - if (targetVelocity - velo > 500) { + if (targetVelocity - velo > 4500) { powPID = 1.0; - } else if (velo - targetVelocity > 500) { + } else if (velo - targetVelocity > 4500) { powPID = 0.0; } else { - double feed = Math.log((668.39 / (targetVelocity + 591.96)) - 0.116) / -4.18; + + double a = 2539.07863; + double c = 1967.6498; + double d = -0.289647; + double h = -1.1569; + + double feed = Math.log((a / (targetVelocity + c)) + d) / h; // --- PROPORTIONAL CORRECTION --- double error = targetVelocity - velo; @@ -88,7 +94,7 @@ public class FlywheelV2 { correction = Math.max(-maxStep, Math.min(maxStep, correction)); // --- FINAL MOTOR POWER --- - powPID = feed + correction; + powPID = feed; // clamp to allowed range powPID = Math.max(0, Math.min(1, powPID)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java index 967fa78..76202c7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Servos.java @@ -14,7 +14,7 @@ public class Servos { //PID constants // TODO: get PIDF constants - public static double spinP = 3.4, spinI = 0, spinD = 0.075, spinF = 0.02; + public static double spinP = 3.3, spinI = 0, spinD = 0.1, spinF = 0.02; public static double turrP = 4.0, turrI = 0.0, turrD = 0.0, turrF = 0.0; public static double spin_scalar = 1.0086;