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

stash

Browse Source
This commit is contained in:
DanTheMan-byte
2025-12-31 16:36:06 -06:00
parent a58371d3d7
commit 61f314d71d
1 changed files with 273 additions and 146 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -7,6 +7,7 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.turrDefault;
import static org.firstinspires.ftc.teamcode.constants.ShooterVars.waitTransfer; import static org.firstinspires.ftc.teamcode.constants.ShooterVars.waitTransfer;
import static org.firstinspires.ftc.teamcode.constants.ShooterVars.waitTransferOut; import static org.firstinspires.ftc.teamcode.constants.ShooterVars.waitTransferOut;
import static org.firstinspires.ftc.teamcode.tests.ShooterTest.kP; import static org.firstinspires.ftc.teamcode.tests.ShooterTest.kP;
@@ -14,26 +15,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;
@@ -46,11 +41,20 @@ public class TeleopV2 extends LinearOpMode {
public static double hoodDefaultPos = 0.5; public static double hoodDefaultPos = 0.5;
public static double desiredTurretAngle = 180; public static double desiredTurretAngle = 180;
public static double velMultiplier = 20; public static double velMultiplier = 20;
public static double shootStamp2 = 0.0;
public double vel = 3000; public double vel = 3000;
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;
double turrCamAdjust = 0.0; 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;
double autoHoodOffset = 0.0;
Robot robot; Robot robot;
MultipleTelemetry TELE; MultipleTelemetry TELE;
boolean intake = false; boolean intake = false;
@@ -58,7 +62,6 @@ public class TeleopV2 extends LinearOpMode {
double xOffset = 0.0; double xOffset = 0.0;
double yOffset = 0.0; double yOffset = 0.0;
double headingOffset = 0.0; double headingOffset = 0.0;
boolean camDetected = false;
int ticker = 0; int ticker = 0;
List<Double> s1G = new ArrayList<>(); List<Double> s1G = new ArrayList<>();
List<Double> s2G = new ArrayList<>(); List<Double> s2G = new ArrayList<>();
@@ -69,12 +72,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;
@@ -84,11 +83,13 @@ 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;
boolean overrideTurr = 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;
@@ -97,13 +98,22 @@ 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;
double turretPos() { public static double velPrediction(double distance) {
return (scalar * ((robot.turr1Pos.getVoltage() - restPos) / 3.3));
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
@@ -129,16 +139,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;
@@ -149,28 +155,25 @@ 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;
overrideTurr = false;
} }
if (gamepad1.leftBumperWasPressed()) { if (gamepad1.leftBumperWasPressed()) {
intake = false; intake = false;
reject = true; emergency = !emergency;
shootAll = false;
} }
@@ -264,7 +267,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);
} }
@@ -276,7 +278,6 @@ public class TeleopV2 extends LinearOpMode {
green3 = checkGreen(s3, s3T); green3 = checkGreen(s3, s3T);
} }
//SHOOTER: //SHOOTER:
double penguin = 0; double penguin = 0;
@@ -325,13 +326,15 @@ public class TeleopV2 extends LinearOpMode {
robot.transfer.setPower(1); robot.transfer.setPower(1);
//TURRET: //TURRET:
double offset; double offset;
double robotX = drive.localizer.getPose().position.x - xOffset; double robX = drive.localizer.getPose().position.x;
double robotY = drive.localizer.getPose().position.y - xOffset; double robY = drive.localizer.getPose().position.y;
double robotX = robX - xOffset;
double robotY = robY - yOffset;
double robotHeading = drive.localizer.getPose().heading.toDouble(); double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -10; double goalX = -10;
@@ -352,7 +355,7 @@ public class TeleopV2 extends LinearOpMode {
offset -= 360; offset -= 360;
} }
double pos = 0.4; double pos = turrDefault;
TELE.addData("offset", offset); TELE.addData("offset", offset);
@@ -365,7 +368,13 @@ public class TeleopV2 extends LinearOpMode {
} }
if (manualTurret) { if (manualTurret) {
pos = 0.4 + (manualOffset/100); pos = turrDefault + (manualOffset / 100);
}
if (!overrideTurr) {
robot.turr1.setPosition(pos);
robot.turr2.setPosition(1 - pos);
} }
if (gamepad2.dpad_right) { if (gamepad2.dpad_right) {
@@ -374,8 +383,6 @@ public class TeleopV2 extends LinearOpMode {
manualOffset += 2; manualOffset += 2;
} }
//VELOCITY AUTOMATIC //VELOCITY AUTOMATIC
if (autoVel) { if (autoVel) {
@@ -392,12 +399,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;
} }
@@ -405,26 +410,29 @@ public class TeleopV2 extends LinearOpMode {
//HOOD: //HOOD:
if (autoHood) { if (autoHood) {
robot.hood.setPosition(hoodAnglePrediction(distanceToGoal)); robot.hood.setPosition(hoodAnglePrediction(distanceToGoal) + autoHoodOffset);
} else { } else {
robot.hood.setPosition(hoodDefaultPos + hoodOffset); robot.hood.setPosition(hoodDefaultPos + hoodOffset);
} }
if (gamepad2.dpadUpWasPressed()) { if (gamepad2.dpadUpWasPressed()) {
hoodOffset -= 0.03; hoodOffset -= 0.03;
autoHoodOffset -= 0.02;
} else if (gamepad2.dpadDownWasPressed()) { } else if (gamepad2.dpadDownWasPressed()) {
hoodOffset += 0.03; hoodOffset += 0.03;
autoHoodOffset += 0.02;
} }
if (gamepad2.left_stick_x > 0.5) { if (gamepad2.left_stick_x > 0.5) {
manualTurret = false; manualTurret = false;
} else if (gamepad2.left_stick_x < -0.5) { } else if (gamepad2.left_stick_x < -0.5) {
manualTurret = true;
manualOffset = 0; manualOffset = 0;
manualTurret = false;
if (gamepad2.left_bumper) { if (gamepad2.left_bumper) {
xOffset = robotX; drive = new MecanumDrive(hardwareMap, new Pose2d(2, 0, 0));
yOffset = robotY; sleep(1200);
headingOffset = robotHeading;
} }
} }
@@ -440,70 +448,95 @@ public class TeleopV2 extends LinearOpMode {
} }
} }
//SHOOT ALL: //SHOOT ALL:]
if (emergency) {
intake = false;
reject = true;
if (shootAll) { if (getRuntime() % 3 > 1.5) {
robot.spin1.setPosition(0);
robot.spin2.setPosition(1);
} else {
robot.spin1.setPosition(1);
robot.spin2.setPosition(0);
}
robot.transferServo.setPosition(transferServo_out);
robot.transfer.setPower(1);
} else if (shootAll) {
TELE.addData("100% works", shootOrder); TELE.addData("100% works", shootOrder);
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) {
turrCamAdjust = turretPos() + d20.ftcPose.bearing; overrideTurr = true;
TELE.addData("Turret Pos Target by Camera", turrCamAdjust); double bearing = d20.ftcPose.bearing;
camDetected = true;
} else { double finalPos = robot.turr1.getPosition() - (bearing / 1300);
camDetected = false; robot.turr1.setPosition(finalPos);
robot.turr2.setPosition(1 - finalPos);
TELE.addData("Bear", bearing);
} }
if (d24 != null) { if (d24 != null) {
turrCamAdjust = turretPos() + d24.ftcPose.bearing; overrideTurr = true;
TELE.addData("Turret Pos Target by Camera", turrCamAdjust);
camDetected = true; double bearing = d24.ftcPose.bearing;
} else { double finalPos = robot.turr1.getPosition() - (bearing / 1300);
camDetected = false; robot.turr1.setPosition(finalPos);
robot.turr2.setPosition(1 - finalPos);
}
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, shootStamp2);
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, shootStamp2);
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, shootStamp2);
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;
@@ -511,10 +544,11 @@ 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);
shootStamp2 = getRuntime();
} }
} else { } else {
@@ -527,31 +561,47 @@ public class TeleopV2 extends LinearOpMode {
reject = false; reject = false;
intake = true; intake = true;
shootAll = false; shootAll = false;
} outtake1 = false;
outtake2 = false;
outtake3 = false;
overrideTurr = false;
}
} }
if (gamepad2.squareWasPressed()) { if (gamepad2.squareWasPressed()) {
square = true; square = true;
shootStamp = getRuntime(); shootStamp = getRuntime();
shootStamp2 = getRuntime();
outtake1 = false;
outtake2 = false;
outtake3 = false;
} }
if (gamepad2.circleWasPressed()) { if (gamepad2.circleWasPressed()) {
circle = true; circle = true;
shootStamp = getRuntime(); shootStamp = getRuntime();
shootStamp2 = getRuntime();
outtake1 = false;
outtake2 = false;
outtake3 = false;
} }
if (gamepad2.triangleWasPressed()) { if (gamepad2.triangleWasPressed()) {
triangle = true; triangle = true;
shootStamp = getRuntime(); shootStamp = getRuntime();
shootStamp2 = getRuntime();
outtake1 = false;
outtake2 = false;
outtake3 = false;
} }
if (square || circle || triangle) { if (square || circle || triangle) {
// Count green balls // Count green balls
@@ -583,15 +633,6 @@ public class TeleopV2 extends LinearOpMode {
square = false; square = false;
triangle = false; triangle = false;
}
if (camDetected){
robot.turr1.setPosition(turrCamAdjust);
robot.turr2.setPosition(1 - turrCamAdjust);
} else {
robot.turr1.setPosition(pos);
robot.turr2.setPosition(1 - pos);
} }
// Right bumper shoots all balls fastest, ignoring colors // Right bumper shoots all balls fastest, ignoring colors
@@ -599,22 +640,82 @@ 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)
if (ballIn(3)) {
shootOrder.add(3); shootOrder.add(3);
shootOrder.add(2);
shootOrder.add(1);
shootAll = true;
shootPos = drive.localizer.getPose();
} }
if (gamepad2.x){ if (ballIn(2)) {
shootAll = false; shootOrder.add(2);
} }
if (ballIn(1)) {
shootOrder.add(1);
}
if (!shootOrder.contains(3)) {
shootOrder.add(3);
}
if (!shootOrder.contains(2)) {
shootOrder.add(2);
}
if (!shootOrder.contains(1)) {
shootOrder.add(1);
}
shootAll = true;
shootPos = drive.localizer.getPose();
}
// // Right bumper shoots all balls fastest, ignoring colors
// if (gamepad2.leftBumperWasPressed()) {
// shootOrder.clear();
// shootStamp = getRuntime();
//
// outtake1 = false;
// outtake2 = false;
// outtake3 = false;
//
// // Fastest order (example: slot 3 → 2 → 1)
//
// if (ballIn(3)) {
// shootOrder.add(3);
// }
//
// if (ballIn(2)) {
// shootOrder.add(2);
// }
// if (ballIn(1)) {
// shootOrder.add(1);
// }
// shootAll = true;
// shootPos = drive.localizer.getPose();
//
// }
//
if (gamepad2.crossWasPressed()) {
emergency = true;
}
if (gamepad2.leftBumperWasPressed()) {
emergency = false;
}
//MISC: //MISC:
drive.updatePoseEstimate(); drive.updatePoseEstimate();
@@ -623,9 +724,9 @@ public class TeleopV2 extends LinearOpMode {
hub.clearBulkCache(); hub.clearBulkCache();
} }
TELE.addData("Spin1Green", green1); TELE.addData("Spin1Green", green1 + ": " + ballIn(1));
TELE.addData("Spin2Green", green2); TELE.addData("Spin2Green", green2 + ": " + ballIn(2));
TELE.addData("Spin3Green", green3); TELE.addData("Spin3Green", green3 + ": " + ballIn(3));
TELE.addData("pose", drive.localizer.getPose()); TELE.addData("pose", drive.localizer.getPose());
TELE.addData("heading", drive.localizer.getPose().heading.toDouble()); TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
@@ -638,7 +739,6 @@ public class TeleopV2 extends LinearOpMode {
aprilTagWebcam.update(); aprilTagWebcam.update();
TELE.update(); TELE.update();
ticker++; ticker++;
@@ -672,13 +772,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, double stamp) {
// 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 || getRuntime() - stamp > 1.5) {
if (tickerA == 1) { if (tickerA == 1) {
transferStamp = getRuntime(); transferStamp = getRuntime();
tickerA++; tickerA++;
@@ -714,9 +814,22 @@ 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) {
@@ -728,8 +841,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) {
@@ -768,6 +879,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);
@@ -777,35 +889,50 @@ 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) { boolean ballIn(int slot) {
switch (slot) {
case 1:
if (distance < 30) { if (!s1T.isEmpty()) {
return 2750;
} else if (distance > 100) { return !(s1T.get(s1T.size() - 1) < (getRuntime()) - 3);
if (distance > 160) {
return 4200;
} }
return 3700;
} else { case 2:
// linear interpolation between 40->2650 and 120->3600
double slope = (3700.0 - 2750.0) / (100.0 - 30); if (!s2T.isEmpty()) {
return (int) Math.round(2750 + slope * (distance - 30));
return !(s2T.get(s2T.size() - 1) < (getRuntime()) - 3);
}
case 3:
if (!s3T.isEmpty()) {
return !(s3T.get(s3T.size() - 1) < (getRuntime()) - 3);
}
}
return true; // default
}
public double turretPos() {
return (scalar * ((robot.turr1Pos.getVoltage() - restPos) / 3.3));
} }
} }
}