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

added webcam to Red_V2.java and changed localization in TeleopV2.java

Browse Source
This commit is contained in:
DanTheMan-byte
2026-01-03 18:15:12 -06:00
parent 4b998766a1
commit 4588321b44
3 changed files with 94 additions and 121 deletions
Download Patch File Download Diff File Expand all files Collapse all files

50
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java
View File

@@ -3,7 +3,6 @@ package org.firstinspires.ftc.teamcode.autonomous;
import static org.firstinspires.ftc.teamcode.constants.Poses.*;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*;
import static org.firstinspires.ftc.teamcode.tests.PIDServoTest.*;
import androidx.annotation.NonNull;
@@ -26,6 +25,8 @@ import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utils.Servos;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
@Config
@Autonomous(preselectTeleOp = "TeleopV2")
@@ -41,6 +42,8 @@ public class Red_V2 extends LinearOpMode {
Flywheel flywheel;
Servos servo;
double velo = 0.0;
public static double intake1Time = 2.9;
@@ -56,19 +59,14 @@ public class Red_V2 extends LinearOpMode {
double powPID = 0.0;
double bearing = 0.0;
int b1 = 0; // 0 = no ball, 1 = green, 2 = purple
int b2 = 0;// 0 = no ball, 1 = green, 2 = purple
int b3 = 0;// 0 = no ball, 1 = green, 2 = purple
boolean spindexPosEqual(double spindexer) {
TELE.addLine("spindex equal");
TELE.update();
return (scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) > spindexer - 0.01 &&
scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) < spindexer + 0.01);
}
public Action initShooter(int vel) {
return new Action() {
double stamp = 0.0;
@@ -125,7 +123,7 @@ public class Red_V2 extends LinearOpMode {
TELE.addData("Velocity", velo);
TELE.update();
detectTag();
if (last && !steady){
stamp = getRuntime();
@@ -171,9 +169,10 @@ public class Red_V2 extends LinearOpMode {
TELE.update();
if (gpp || pgp || ppg){
robot.turr1.setPower(turret_red);
robot.turr2.setPower(1 - turret_red);
return false;
double turretPID = servo.setTurrPos(turret_red);
robot.turr1.setPower(turretPID);
robot.turr2.setPower(-turretPID);
return !servo.turretEqual(turret_red);
} else {
return true;
}
@@ -200,7 +199,7 @@ public class Red_V2 extends LinearOpMode {
teleStart = drive.localizer.getPose();
return !spindexPosEqual(spindexer);
return !servo.spinEqual(spindexer);
}
};
}
@@ -222,6 +221,7 @@ public class Red_V2 extends LinearOpMode {
robot.shooter2.setPower(powPID);
drive.updatePoseEstimate();
detectTag();
teleStart = drive.localizer.getPose();
@@ -454,8 +454,9 @@ public class Red_V2 extends LinearOpMode {
robot.hood.setPosition(hoodAuto);
robot.turr1.setPower(turret_detectRed);
robot.turr2.setPower(1 - turret_detectRed);
double turretPID = servo.setTurrPos(turret_detectRed);
robot.turr1.setPower(turretPID);
robot.turr2.setPower(-turretPID);
robot.transferServo.setPosition(transferServo_out);
@@ -574,6 +575,25 @@ public class Red_V2 extends LinearOpMode {
}
public void detectTag(){
AprilTagDetection d20 = aprilTag.getTagById(20);
AprilTagDetection d24 = aprilTag.getTagById(24);
if (d20 != null) {
bearing = d20.ftcPose.bearing;
TELE.addData("Bear", bearing);
}
if (d24 != null) {
bearing = d24.ftcPose.bearing;
TELE.addData("Bear", bearing);
}
double turretPos = servo.getTurrPos() - (bearing / 1300);
double turretPID = servo.setTurrPos(turretPos);
robot.turr1.setPower(turretPID);
robot.turr2.setPower(-turretPID);
}
public void shootingSequence() {
TELE.addData("Velocity", velo);
if (gpp) {

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

@@ -16,7 +16,9 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam;
import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utils.Servos;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
import java.util.ArrayList;
@@ -25,7 +27,8 @@ import java.util.List;
@TeleOp
@Config
public class TeleopV2 extends LinearOpMode {
Servos servo;
Flywheel flywheel;
public static double manualVel = 3000;
public static double hood = 0.5;
public static double hoodDefaultPos = 0.5;
@@ -88,6 +91,11 @@ public class TeleopV2 extends LinearOpMode {
private double transferStamp = 0.0;
private int tickerA = 1;
private boolean transferIn = false;
double turretPID = 0.0;
double turretPos = 0.5;
double spindexPID = 0.0;
double spindexPos = spindexer_intakePos1;
double error = 0.0;
public static double velPrediction(double distance) {
@@ -119,6 +127,8 @@ public class TeleopV2 extends LinearOpMode {
TELE = new MultipleTelemetry(
telemetry, FtcDashboard.getInstance().getTelemetry()
);
servo = new Servos(hardwareMap);
flywheel = new Flywheel();
drive = new MecanumDrive(hardwareMap, teleStart);
@@ -126,9 +136,6 @@ public class TeleopV2 extends LinearOpMode {
aprilTagWebcam.init(new Robot(hardwareMap), TELE);
robot.turr1.setPower(0.4);
robot.turr2.setPower(1 - 0.4);
waitForStart();
if (isStopRequested()) return;
while (opModeIsActive()) {
@@ -150,6 +157,19 @@ public class TeleopV2 extends LinearOpMode {
robot.frontRight.setPower(frontRightPower);
robot.backRight.setPower(backRightPower);
// PID SERVOS
turretPID = servo.setTurrPos(turretPos);
robot.turr1.setPower(turretPID);
robot.turr2.setPower(-turretPID);
if (!servo.spinEqual(spindexPos)){
robot.spin1.setPower(spindexPID);
robot.spin2.setPower(-spindexPID);
} else{
robot.spin1.setPower(0);
robot.spin2.setPower(0);
}
//INTAKE:
if (gamepad1.rightBumperWasPressed()) {
@@ -173,22 +193,15 @@ public class TeleopV2 extends LinearOpMode {
robot.intake.setPower(1);
double position;
if ((getRuntime() % 0.3) > 0.15) {
position = spindexer_intakePos1 + 0.015;
spindexPos = spindexer_intakePos1 + 0.015;
} else {
position = spindexer_intakePos1 - 0.015;
spindexPos = spindexer_intakePos1 - 0.015;
}
robot.spin1.setPower(position);
robot.spin2.setPower(1 - position);
} else if (reject) {
robot.intake.setPower(-1);
double position = spindexer_intakePos1;
robot.spin1.setPower(position);
robot.spin2.setPower(1 - position);
spindexPos = spindexer_intakePos1;
} else {
robot.intake.setPower(0);
}
@@ -270,46 +283,7 @@ public class TeleopV2 extends LinearOpMode {
//SHOOTER:
double penguin = 0;
if (ticker % 8 == 0) {
penguin = (double) robot.shooterEncoder.getCurrentPosition() / 2048;
double stamp = getRuntime();
velo1 = -60 * ((penguin - initPos) / (stamp - stamp1));
initPos = penguin;
stamp1 = stamp;
}
velo = velo1;
double feed = vel / 4500;
if (vel > 500) {
feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18;
}
// --- PROPORTIONAL CORRECTION ---
double error = vel - velo1;
double correction = kP * error;
// limit how fast power changes (prevents oscillation)
correction = Math.max(-maxStep, Math.min(maxStep, correction));
// --- FINAL MOTOR POWER ---
double powPID = feed + correction;
// clamp to allowed range
powPID = Math.max(0, Math.min(1, powPID));
if (vel - velo > 1000) {
powPID = 1;
} else if (velo - vel > 1000) {
powPID = 0;
}
TELE.addData("PIDPower", powPID);
TELE.addData("vel", velo1);
double powPID = flywheel.manageFlywheel((int) vel, robot.shooter1.getCurrentPosition());
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
@@ -345,7 +319,7 @@ public class TeleopV2 extends LinearOpMode {
offset -= 360;
}
double pos = turrDefault;
double pos = turrDefault + error;
TELE.addData("offset", offset);
@@ -357,14 +331,33 @@ public class TeleopV2 extends LinearOpMode {
pos = 0.97;
}
if (y < 0.1 && y > -0.1 && x < 0.1 && x > -0.1 && rx < 0.1 && rx > -0.1){
AprilTagDetection d20 = aprilTagWebcam.getTagById(20);
AprilTagDetection d24 = aprilTagWebcam.getTagById(24);
if (d20 != null) {
overrideTurr = true;
double bearing = d20.ftcPose.bearing;
turretPos = servo.getTurrPos() - (bearing/1300);
TELE.addData("Bear", bearing);
}
if (d24 != null) {
overrideTurr = true;
double bearing = d24.ftcPose.bearing;
turretPos = servo.getTurrPos() - (bearing/1300);
TELE.addData("Bear", bearing);
}
}
if (manualTurret) {
pos = turrDefault + (manualOffset / 100);
}
if (!overrideTurr) {
robot.turr1.setPower(pos);
robot.turr2.setPower(1 - pos);
turretPos = pos;
} else{
error = servo.getSpinPos() - pos;
}
if (gamepad2.dpad_right) {
@@ -445,12 +438,9 @@ public class TeleopV2 extends LinearOpMode {
reject = true;
if (getRuntime() % 3 > 1.5) {
robot.spin1.setPower(0);
robot.spin2.setPower(1);
spindexPos = 1;
} else {
robot.spin1.setPower(1);
robot.spin2.setPower(0);
spindexPos = 0;
}
robot.transferServo.setPosition(transferServo_out);
@@ -468,39 +458,14 @@ public class TeleopV2 extends LinearOpMode {
int currentSlot = shootOrder.get(0); // Peek, do NOT remove yet
boolean shootingDone = false;
AprilTagDetection d20 = aprilTagWebcam.getTagById(20);
AprilTagDetection d24 = aprilTagWebcam.getTagById(24);
if (d20 != null) {
overrideTurr = true;
double bearing = d20.ftcPose.bearing;
double finalPos = robot.turr1.getPower() - (bearing / 1300);
robot.turr1.setPower(finalPos);
robot.turr2.setPower(1 - finalPos);
TELE.addData("Bear", bearing);
}
if (d24 != null) {
overrideTurr = true;
double bearing = d24.ftcPose.bearing;
double finalPos = robot.turr1.getPower() - (bearing / 1300);
robot.turr1.setPower(finalPos);
robot.turr2.setPower(1 - finalPos);
}
if (!outtake1) {
outtake1 = (spindexPosEqual(spindexer_outtakeBall1));
outtake1 = (servo.spinEqual(spindexer_outtakeBall1));
}
if (!outtake2) {
outtake2 = (spindexPosEqual(spindexer_outtakeBall2));
outtake2 = (servo.spinEqual(spindexer_outtakeBall2));
}
if (!outtake3) {
outtake3 = (spindexPosEqual(spindexer_outtakeBall3));
outtake3 = (servo.spinEqual(spindexer_outtakeBall3));
}
switch (currentSlot) {
@@ -543,8 +508,7 @@ public class TeleopV2 extends LinearOpMode {
} else {
// Finished shooting all balls
robot.spin1.setPower(spindexer_intakePos1);
robot.spin2.setPower(1 - spindexer_intakePos1);
spindexPos = spindexer_intakePos1;
shootA = true;
shootB = true;
shootC = true;
@@ -757,15 +721,9 @@ public class TeleopV2 extends LinearOpMode {
return countTrue > countWindow / 2.0; // more than 50% true
}
boolean spindexPosEqual(double spindexer) {
return (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, boolean spinOk, double stamp) {
// Set spin positions
robot.spin1.setPower(spindexer);
robot.spin2.setPower(1 - spindexer);
spindexPos = spindexer;
// Check if spindexer has reached the target position
if (spinOk || getRuntime() - stamp > 1.5) {
@@ -920,9 +878,4 @@ public class TeleopV2 extends LinearOpMode {
}
return true; // default
}
public double turretPos() {
return (scalar * ((robot.turr1Pos.getVoltage() - restPos) / 3.3));
}
}