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
2026-01-05 14:57:42 -06:00
parent 9c2a86c3e6
commit 475fc4fe1c
3 changed files with 31 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -320,7 +320,7 @@ public class TeleopV2 extends LinearOpMode {
offset -= 360; offset -= 360;
} }
double pos = turrDefault + (error/8); double pos = turrDefault + (error/8); // adds the overall error to the default
TELE.addData("offset", offset); TELE.addData("offset", offset);
@@ -332,29 +332,28 @@ public class TeleopV2 extends LinearOpMode {
pos = 0.97; pos = 0.97;
} }
if (y < 0.1 && y > -0.1 && x < 0.1 && x > -0.1 && rx < 0.1 && rx > -0.1){ if (y < 0.1 && y > -0.1 && x < 0.1 && x > -0.1 && rx < 0.1 && rx > -0.1){ //not moving
AprilTagDetection d20 = aprilTagWebcam.getTagById(20); AprilTagDetection d20 = aprilTagWebcam.getTagById(20);
AprilTagDetection d24 = aprilTagWebcam.getTagById(24); AprilTagDetection d24 = aprilTagWebcam.getTagById(24);
double bearing = 0.0; double bearing = 0.0;
if (d20 != null || d24 != null){
if (d20 != null) { if (d20 != null) {
overrideTurr = true;
bearing = d20.ftcPose.bearing; bearing = d20.ftcPose.bearing;
turretPos = servo.getTurrPos() - (bearing/1300);
TELE.addData("Bear", bearing);
} }
if (d24 != null) { if (d24 != null) {
overrideTurr = true;
bearing = d24.ftcPose.bearing; bearing = d24.ftcPose.bearing;
}
overrideTurr = true;
turretPos = servo.getTurrPos() - (bearing/1300); turretPos = servo.getTurrPos() - (bearing/1300);
TELE.addData("Bear", bearing); TELE.addData("Bear", bearing);
}
if (camTicker < 8){ if (camTicker < 8){
error += bearing/1300; error += bearing/1300; //adds error in first 8 frames of seeing the tag to see how much to adjust the default pos
} }
camTicker++; camTicker++;
}
} else { } else {
camTicker = 0; camTicker = 0;
} }

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java
View File

@@ -89,8 +89,6 @@ public class IntakeTest extends LinearOpMode {
} else if (servo.spinEqual(spindexer_intakePos3)){ } else if (servo.spinEqual(spindexer_intakePos3)){
spindexerPos = spindexer_intakePos2; spindexerPos = spindexer_intakePos2;
} }
} else {
spindexerPos = spindexer_outtakeBall1;
} }
} }

23
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java
View File

@@ -16,9 +16,14 @@ public class PositionalServoProgrammer extends LinearOpMode {
Servos servo; Servos servo;
public static double spindexPos = 0.501; public static double spindexPos = 0.501;
public static double spindexPow = 0.0;
public static double spinHoldPow = 0.0;
public static double turretPos = 0.501; public static double turretPos = 0.501;
public static double turretPow = 0.0;
public static double turrHoldPow = 0.0;
public static double transferPos = 0.501; public static double transferPos = 0.501;
public static double hoodPos = 0.501; public static double hoodPos = 0.501;
public static int mode = 0; //0 for positional, 1 for power
@Override @Override
public void runOpMode() throws InterruptedException { public void runOpMode() throws InterruptedException {
@@ -28,21 +33,27 @@ public class PositionalServoProgrammer extends LinearOpMode {
waitForStart(); waitForStart();
if (isStopRequested()) return; if (isStopRequested()) return;
while (opModeIsActive()){ while (opModeIsActive()){
if (spindexPos != 0.501 && !servo.spinEqual(spindexPos)){ if (spindexPos != 0.501 && !servo.spinEqual(spindexPos) && mode == 0){
double pos = servo.setSpinPos(spindexPos); double pos = servo.setSpinPos(spindexPos);
robot.spin1.setPower(pos); robot.spin1.setPower(pos);
robot.spin2.setPower(-pos); robot.spin2.setPower(-pos);
} else if (mode == 0){
robot.spin1.setPower(spinHoldPow);
robot.spin2.setPower(spinHoldPow);
} else { } else {
robot.spin1.setPower(0); robot.spin1.setPower(spindexPow);
robot.spin2.setPower(0); robot.spin2.setPower(-spindexPow);
} }
if (turretPos != 0.501 && !servo.turretEqual(turretPos)){ if (turretPos != 0.501 && !servo.turretEqual(turretPos)){
double pos = servo.setTurrPos(turretPos); double pos = servo.setTurrPos(turretPos);
robot.turr1.setPower(pos); robot.turr1.setPower(pos);
robot.turr2.setPower(-pos); robot.turr2.setPower(-pos);
} else if (mode == 0){
robot.turr1.setPower(turrHoldPow);
robot.turr2.setPower(turrHoldPow);
} else { } else {
robot.turr1.setPower(0); robot.spin1.setPower(turretPow);
robot.turr2.setPower(0); robot.spin2.setPower(-turretPow);
} }
if (transferPos != 0.501){ if (transferPos != 0.501){
robot.transferServo.setPosition(transferPos); robot.transferServo.setPosition(transferPos);
@@ -51,8 +62,6 @@ public class PositionalServoProgrammer extends LinearOpMode {
robot.hood.setPosition(hoodPos); robot.hood.setPosition(hoodPos);
} }
TELE.addData("spindexer", servo.getSpinPos()); TELE.addData("spindexer", servo.getSpinPos());
TELE.addData("hood", 1-scalar*((robot.hoodPos.getVoltage() - restPos) / 3.3));
TELE.addData("transferServo", scalar*((robot.transferServoPos.getVoltage() - restPos) / 3.3));
TELE.addData("turret", servo.getTurrPos()); TELE.addData("turret", servo.getTurrPos());
TELE.addData("spindexer voltage", robot.spin1Pos.getVoltage()); TELE.addData("spindexer voltage", robot.spin1Pos.getVoltage());
TELE.addData("hood voltage", robot.hoodPos.getVoltage()); TELE.addData("hood voltage", robot.hoodPos.getVoltage());