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

stash

Browse Source
This commit is contained in:
KeshavAnandCode
2025-12-03 19:24:06 -06:00
parent ef08883014
commit 705eee180f
1 changed files with 54 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -271,42 +271,6 @@ public class TeleopV2 extends LinearOpMode {
robot.hood.setPosition(hood);
//TODO: ADD CODE TO CHANGE VARIABLE VEL BASED ON POSITION
//TODO: ADD CODE TO CHANGE VARIABLE HOOD ANGLE BASED ON POSITION
//SHOOT ALL:
if (gamepad2.rightBumperWasPressed()) {
shootAll = true;
}
if (shootAll) {
intake = false;
reject = false;
if (shootA) {
shootA = shootTeleop(spindexer_outtakeBall3);
TELE.addData("shootA", shootA);
} else if (shootB) {
shootB = shootTeleop(spindexer_outtakeBall2);
TELE.addData("shootB", shootB);
} else if (shootC) {
shootC = shootTeleop(spindexer_outtakeBall1);
TELE.addData("shootC", shootC);
} else {
robot.spin1.setPosition(spindexer_intakePos1);
robot.spin2.setPosition(1 - spindexer_intakePos1);
shootAll = false;
}
}
//TURRET:
double offset;
@@ -320,6 +284,8 @@ public class TeleopV2 extends LinearOpMode {
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;
@@ -350,6 +316,55 @@ public class TeleopV2 extends LinearOpMode {
robot.turr1.setPosition(pos);
robot.turr2.setPosition(1 - pos);
if (autoVel){
vel = 2000 + distanceToGoal * 12;
} else {
}
//TODO: ADD CODE TO CHANGE VARIABLE HOOD ANGLE BASED ON POSITION
//SHOOT ALL:
if (gamepad2.rightBumperWasPressed()) {
shootAll = true;
}
if (shootAll) {
intake = false;
reject = false;
if (shootA) {
shootA = shootTeleop(spindexer_outtakeBall3);
TELE.addData("shootA", shootA);
} else if (shootB) {
shootB = shootTeleop(spindexer_outtakeBall2);
TELE.addData("shootB", shootB);
} else if (shootC) {
shootC = shootTeleop(spindexer_outtakeBall1);
TELE.addData("shootC", shootC);
} else {
robot.spin1.setPosition(spindexer_intakePos1);
robot.spin2.setPosition(1 - spindexer_intakePos1);
shootA = true;
shootB = true;
shootC = true;
shootAll = false;
}
}
//SPINDEXER:
if (gamepad2.squareWasPressed()){
@@ -375,6 +390,8 @@ public class TeleopV2 extends LinearOpMode {
TELE.addData("pose", drive.localizer.getPose());
TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
TELE.addData("distanceToGoal", distanceToGoal);
TELE.update();
@@ -460,4 +477,5 @@ public class TeleopV2 extends LinearOpMode {
}