KeshavAnandCode/FTCDecodeMain
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Working hood angle regression

Browse Source
This commit is contained in:
KeshavAnandCode
2025-12-05 20:46:52 -06:00
parent 0838fc35f9
commit 46a565c2c8
1 changed files with 144 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files

204
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java
View File

@@ -14,26 +14,20 @@ import static org.firstinspires.ftc.teamcode.tests.ShooterTest.maxStep;
import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.restPos; import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.restPos;
import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.scalar; import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.scalar;
import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam; import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
import java.util.ArrayList; import java.util.ArrayList;
import java.util.List; import java.util.List;
@@ -50,6 +44,13 @@ public class TeleopV2 extends LinearOpMode {
public boolean autoVel = true; public boolean autoVel = true;
public double manualOffset = 0.0; public double manualOffset = 0.0;
public boolean autoHood = true; public boolean autoHood = true;
public boolean green1 = false;
public boolean green2 = false;
public boolean green3 = false;
public double shootStamp = 0.0;
public boolean circle = false;
public boolean square = false;
public boolean triangle = false;
Robot robot; Robot robot;
MultipleTelemetry TELE; MultipleTelemetry TELE;
boolean intake = false; boolean intake = false;
@@ -67,12 +68,8 @@ public class TeleopV2 extends LinearOpMode {
List<Boolean> s1 = new ArrayList<>(); List<Boolean> s1 = new ArrayList<>();
List<Boolean> s2 = new ArrayList<>(); List<Boolean> s2 = new ArrayList<>();
List<Boolean> s3 = new ArrayList<>(); List<Boolean> s3 = new ArrayList<>();
boolean oddBallColor = false; boolean oddBallColor = false;
AprilTagWebcam aprilTagWebcam = new AprilTagWebcam(); AprilTagWebcam aprilTagWebcam = new AprilTagWebcam();
MecanumDrive drive; MecanumDrive drive;
double hoodOffset = 0.0; double hoodOffset = 0.0;
boolean shoot1 = false; boolean shoot1 = false;
@@ -82,11 +79,12 @@ public class TeleopV2 extends LinearOpMode {
boolean shootC = true; boolean shootC = true;
boolean manualTurret = false; boolean manualTurret = false;
public boolean green1 = false; boolean outtake1 = false;
public boolean green2 = false; boolean outtake2 = false;
public boolean green3 = false; boolean outtake3 = false;
List<Integer> shootOrder = new ArrayList<>(); List<Integer> shootOrder = new ArrayList<>();
boolean emergency = false;
private double lastEncoderRevolutions = 0.0; private double lastEncoderRevolutions = 0.0;
private double lastTimeStamp = 0.0; private double lastTimeStamp = 0.0;
private double velo1, velo; private double velo1, velo;
@@ -95,11 +93,23 @@ public class TeleopV2 extends LinearOpMode {
private double transferStamp = 0.0; private double transferStamp = 0.0;
private int tickerA = 1; private int tickerA = 1;
private boolean transferIn = false; private boolean transferIn = false;
public double shootStamp = 0.0;
public boolean circle = false;
public boolean square = false;
public boolean triangle = 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 @Override
public void runOpMode() throws InterruptedException { public void runOpMode() throws InterruptedException {
@@ -124,16 +134,12 @@ public class TeleopV2 extends LinearOpMode {
robot.turr1.setPosition(0.4); robot.turr1.setPosition(0.4);
robot.turr2.setPosition(1 - 0.4); robot.turr2.setPosition(1 - 0.4);
waitForStart(); waitForStart();
if (isStopRequested()) return; if (isStopRequested()) return;
while (opModeIsActive()) { while (opModeIsActive()) {
//DRIVETRAIN: //DRIVETRAIN:
double y = -gamepad1.right_stick_y; // Remember, Y stick value is reversed double y = -gamepad1.right_stick_y; // Remember, Y stick value is reversed
double x = gamepad1.right_stick_x * 1.1; // Counteract imperfect strafing double x = gamepad1.right_stick_x * 1.1; // Counteract imperfect strafing
double rx = gamepad1.left_stick_x; double rx = gamepad1.left_stick_x;
@@ -144,21 +150,18 @@ public class TeleopV2 extends LinearOpMode {
double frontRightPower = (y - x - rx) / denominator; double frontRightPower = (y - x - rx) / denominator;
double backRightPower = (y + x - rx) / denominator; double backRightPower = (y + x - rx) / denominator;
robot.frontLeft.setPower(frontLeftPower); robot.frontLeft.setPower(frontLeftPower);
robot.backLeft.setPower(backLeftPower); robot.backLeft.setPower(backLeftPower);
robot.frontRight.setPower(frontRightPower); robot.frontRight.setPower(frontRightPower);
robot.backRight.setPower(backRightPower); robot.backRight.setPower(backRightPower);
//INTAKE: //INTAKE:
if (gamepad1.rightBumperWasPressed()) { if (gamepad1.rightBumperWasPressed()) {
intake = !intake; intake = !intake;
reject = false; reject = false;
shootAll = false; shootAll = false;
emergency = false;
} }
@@ -259,7 +262,6 @@ public class TeleopV2 extends LinearOpMode {
s3T.add(getRuntime()); s3T.add(getRuntime());
} }
if (!s1.isEmpty()) { if (!s1.isEmpty()) {
green1 = checkGreen(s1, s1T); green1 = checkGreen(s1, s1T);
} }
@@ -271,7 +273,6 @@ public class TeleopV2 extends LinearOpMode {
green3 = checkGreen(s3, s3T); green3 = checkGreen(s3, s3T);
} }
//SHOOTER: //SHOOTER:
double penguin = 0; double penguin = 0;
@@ -320,7 +321,6 @@ public class TeleopV2 extends LinearOpMode {
robot.transfer.setPower(1); robot.transfer.setPower(1);
//TURRET: //TURRET:
double offset; double offset;
@@ -372,8 +372,6 @@ public class TeleopV2 extends LinearOpMode {
manualOffset += 2; manualOffset += 2;
} }
//VELOCITY AUTOMATIC //VELOCITY AUTOMATIC
if (autoVel) { if (autoVel) {
@@ -390,12 +388,10 @@ public class TeleopV2 extends LinearOpMode {
} else if (gamepad2.right_stick_y > 0.5) { } else if (gamepad2.right_stick_y > 0.5) {
autoVel = false; autoVel = false;
manualVel = 2700; manualVel = 2700;
} } else if (gamepad2.right_stick_x > 0.5) {
else if (gamepad2.right_stick_x > 0.5) {
autoVel = false; autoVel = false;
manualVel = 3600; manualVel = 3600;
} } else if (gamepad2.right_stick_x < -0.5) {
else if (gamepad2.right_stick_x < -0.5) {
autoVel = false; autoVel = false;
manualVel = 3100; manualVel = 3100;
} }
@@ -438,30 +434,45 @@ public class TeleopV2 extends LinearOpMode {
} }
} }
//SHOOT ALL: //SHOOT ALL:]
if (emergency) {
intake = false;
reject = false;
if (shootAll) { if (getRuntime() % 3 > 1.5) {
robot.spin1.setPosition(0);
robot.spin2.setPosition(1);
} else {
robot.spin1.setPosition(1);
robot.spin2.setPosition(0);
}
if (getRuntime() % 1 < 0.5) {
robot.transferServo.setPosition(transferServo_in);
} else {
robot.transferServo.setPosition(transferServo_out);
}
robot.transfer.setPower(1);
} else if (shootAll) {
TELE.addData("100% works", shootOrder); TELE.addData("100% works", shootOrder);
TELE.update(); TELE.update();
intake = false; intake = false;
reject = false; reject = false;
if (!shootOrder.isEmpty() && (getRuntime()-shootStamp < 10)) { if (!shootOrder.isEmpty() && (getRuntime() - shootStamp < 12)) {
int currentSlot = shootOrder.get(0); // Peek, do NOT remove yet int currentSlot = shootOrder.get(0); // Peek, do NOT remove yet
boolean shootingDone = false; boolean shootingDone = false;
AprilTagDetection d20 = aprilTagWebcam.getTagById(20); AprilTagDetection d20 = aprilTagWebcam.getTagById(20);
AprilTagDetection d24 = aprilTagWebcam.getTagById(24); AprilTagDetection d24 = aprilTagWebcam.getTagById(24);
if (d20 != null) { if (d20 != null) {
//TODO: Add logic here and below for webcam if using //TODO: Add logic here and below for webcam if using
} }
@@ -470,30 +481,40 @@ public class TeleopV2 extends LinearOpMode {
} }
if (!outtake1) {
outtake1 = (spindexPosEqual(spindexer_outtakeBall1));
}
if (!outtake2) {
outtake2 = (spindexPosEqual(spindexer_outtakeBall2));
}
if (!outtake3) {
outtake3 = (spindexPosEqual(spindexer_outtakeBall3));
}
switch (currentSlot) { switch (currentSlot) {
case 1: case 1:
shootA = shootTeleop(spindexer_outtakeBall1); shootA = shootTeleop(spindexer_outtakeBall1, outtake1);
TELE.addData("shootA", shootA); TELE.addData("shootA", shootA);
if ((getRuntime()- shootStamp) < 3* (4-shootOrder.size())){ if ((getRuntime() - shootStamp) < 4 * (4 - shootOrder.size())) {
shootingDone = !shootA; shootingDone = !shootA;
} else { } else {
shootingDone = true; shootingDone = true;
} }
break; break;
case 2: case 2:
shootB = shootTeleop(spindexer_outtakeBall2); shootB = shootTeleop(spindexer_outtakeBall2, outtake2);
TELE.addData("shootB", shootB); TELE.addData("shootB", shootB);
if ((getRuntime()- shootStamp) < 3* (4-shootOrder.size())){ if ((getRuntime() - shootStamp) < 4 * (4 - shootOrder.size())) {
shootingDone = !shootB; shootingDone = !shootB;
} else { } else {
shootingDone = true; shootingDone = true;
} }
break; break;
case 3: case 3:
shootC = shootTeleop(spindexer_outtakeBall3); shootC = shootTeleop(spindexer_outtakeBall3, outtake3);
TELE.addData("shootC", shootC); TELE.addData("shootC", shootC);
if ((getRuntime()- shootStamp) < 3* (4-shootOrder.size())){ if ((getRuntime() - shootStamp) < 4 * (4 - shootOrder.size())) {
shootingDone = !shootC; shootingDone = !shootC;
} else { } else {
shootingDone = true; shootingDone = true;
@@ -501,7 +522,6 @@ public class TeleopV2 extends LinearOpMode {
break; break;
} }
// Remove from the list only if shooting is complete // Remove from the list only if shooting is complete
if (shootingDone) { if (shootingDone) {
shootOrder.remove(0); shootOrder.remove(0);
@@ -517,31 +537,40 @@ public class TeleopV2 extends LinearOpMode {
reject = false; reject = false;
intake = true; intake = true;
shootAll = false; shootAll = false;
} outtake1 = false;
outtake2 = false;
outtake3 = false;
}
} }
if (gamepad2.squareWasPressed()) { if (gamepad2.squareWasPressed()) {
square = true; square = true;
shootStamp = getRuntime(); shootStamp = getRuntime();
outtake1 = false;
outtake2 = false;
outtake3 = false;
} }
if (gamepad2.circleWasPressed()) { if (gamepad2.circleWasPressed()) {
circle = true; circle = true;
shootStamp = getRuntime(); shootStamp = getRuntime();
outtake1 = false;
outtake2 = false;
outtake3 = false;
} }
if (gamepad2.triangleWasPressed()) { if (gamepad2.triangleWasPressed()) {
triangle = true; triangle = true;
shootStamp = getRuntime(); shootStamp = getRuntime();
outtake1 = false;
outtake2 = false;
outtake3 = false;
} }
if (square || circle || triangle) { if (square || circle || triangle) {
// Count green balls // Count green balls
@@ -573,7 +602,6 @@ public class TeleopV2 extends LinearOpMode {
square = false; square = false;
triangle = false; triangle = false;
} }
// Right bumper shoots all balls fastest, ignoring colors // Right bumper shoots all balls fastest, ignoring colors
@@ -581,6 +609,10 @@ public class TeleopV2 extends LinearOpMode {
shootOrder.clear(); shootOrder.clear();
shootStamp = getRuntime(); shootStamp = getRuntime();
outtake1 = false;
outtake2 = false;
outtake3 = false;
// Fastest order (example: slot 3 → 2 → 1) // Fastest order (example: slot 3 → 2 → 1)
shootOrder.add(3); shootOrder.add(3);
shootOrder.add(2); shootOrder.add(2);
@@ -588,12 +620,10 @@ public class TeleopV2 extends LinearOpMode {
shootAll = true; shootAll = true;
shootPos = drive.localizer.getPose(); shootPos = drive.localizer.getPose();
} }
if (gamepad2.x){ if (gamepad2.crossWasPressed()) {
shootAll = false; emergency = !emergency;
} }
@@ -620,7 +650,6 @@ public class TeleopV2 extends LinearOpMode {
aprilTagWebcam.update(); aprilTagWebcam.update();
TELE.update(); TELE.update();
ticker++; ticker++;
@@ -654,13 +683,13 @@ public class TeleopV2 extends LinearOpMode {
scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) < spindexer + 0.01); scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) < spindexer + 0.01);
} }
public boolean shootTeleop(double spindexer) { public boolean shootTeleop(double spindexer, boolean spinOk) {
// Set spin positions // Set spin positions
robot.spin1.setPosition(spindexer); robot.spin1.setPosition(spindexer);
robot.spin2.setPosition(1 - spindexer); robot.spin2.setPosition(1 - spindexer);
// Check if spindexer has reached the target position // Check if spindexer has reached the target position
if (spindexPosEqual(spindexer)) { if (spinOk) {
if (tickerA == 1) { if (tickerA == 1) {
transferStamp = getRuntime(); transferStamp = getRuntime();
tickerA++; tickerA++;
@@ -696,10 +725,24 @@ public class TeleopV2 extends LinearOpMode {
} }
} }
public double hoodAnglePrediction(double distance) { public double hoodAnglePrediction(double x) {
if (x < 34) {
double L = 1.04471;
double U = 0.711929;
double Q = 120.02263;
double B = 0.780982;
double M = 20.61191;
double v = 10.40506;
return 0.4; double inner = 1 + Q * Math.exp(-B * (x - M));
return L + (U - L) / Math.pow(inner, 1.0 / v);
} else {
// x >= 34
return 1.94372 * Math.exp(-0.0528731 * x) + 0.39;
} }
}
void addOddThenRest(List<Integer> order, boolean oddColor) { void addOddThenRest(List<Integer> order, boolean oddColor) {
// Odd ball first // Odd ball first
@@ -710,8 +753,6 @@ public class TeleopV2 extends LinearOpMode {
TELE.addData("oddBall", oddColor); TELE.addData("oddBall", oddColor);
shootAll = true; shootAll = true;
} }
void addOddInMiddle(List<Integer> order, boolean oddColor) { void addOddInMiddle(List<Integer> order, boolean oddColor) {
@@ -750,6 +791,7 @@ public class TeleopV2 extends LinearOpMode {
shootAll = true; shootAll = true;
} }
void addOddLast(List<Integer> order, boolean oddColor) { void addOddLast(List<Integer> order, boolean oddColor) {
// Odd ball last // Odd ball last
for (int i = 1; i <= 3; i++) if (getBallColor(i) != oddColor) order.add(i); for (int i = 1; i <= 3; i++) if (getBallColor(i) != oddColor) order.add(i);
@@ -759,35 +801,19 @@ public class TeleopV2 extends LinearOpMode {
TELE.addData("oddBall", oddColor); TELE.addData("oddBall", oddColor);
shootAll = true; shootAll = true;
} }
// Returns color of ball in slot i (1-based) // Returns color of ball in slot i (1-based)
boolean getBallColor(int slot) { boolean getBallColor(int slot) {
switch (slot) { switch (slot) {
case 1: return green1; case 1:
case 2: return green2; return green1;
case 3: return green3; case 2:
return green2;
case 3:
return green3;
} }
return false; // default return false; // default
} }
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));
}
}
} }