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:53:59 -06:00
parent e3c259587e
commit 75b3dc7fd4
1 changed files with 76 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -28,7 +28,7 @@ import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam;
import org.firstinspires.ftc.teamcode.utils.Robot;
@Config
@Autonomous
@Autonomous(preselectTeleOp = "TeleopV2")
public class Red_V2 extends LinearOpMode {
Robot robot;
@@ -43,9 +43,9 @@ public class Red_V2 extends LinearOpMode {
double velo = 0.0;
double targetVelocity = 0.0;
public static double intake1Time = 6.5;
public static double intake1Time = 2.9;
public static double intake2Time = 6.5;
public static double intake2Time = 2.9;
public static double colorDetect = 3.0;
@@ -131,8 +131,12 @@ public class Red_V2 extends LinearOpMode {
TELE.addData("Velocity", velo);
TELE.update();
if (last && !steady){
stamp = getRuntime();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
return false;
} else if (steady) {
stamp = getRuntime();
@@ -230,6 +234,11 @@ public class Red_V2 extends LinearOpMode {
TELE.addData("Velocity", velo);
TELE.addLine("spindex");
TELE.update();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
return !spindexPosEqual(spindexer);
}
};
@@ -284,6 +293,10 @@ public class Red_V2 extends LinearOpMode {
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
if (ticker == 1) {
transferStamp = getRuntime();
@@ -339,9 +352,12 @@ public class Red_V2 extends LinearOpMode {
TELE.addLine("Intaking");
TELE.update();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
robot.intake.setPower(1);
if ((s1D < 40.0 && s2D < 40.0 && s3D < 40.0) || getRuntime() - stamp > intakeTime) {
robot.intake.setPower(0);
return false;
} else {
return true;
@@ -350,6 +366,27 @@ public class Red_V2 extends LinearOpMode {
};
}
public Action intakeReject() {
return new Action() {
double stamp = 0.0;
int ticker = 0;
@Override
public boolean run(@NonNull TelemetryPacket telemetryPacket) {
if (ticker == 0) {
stamp = getRuntime();
}
ticker++;
if (getRuntime() - stamp < 0.3){
return true;
}else {
robot.intake.setPower(0);
return false;
}
}
};
}
public Action ColorDetect() {
return new Action() {
double stamp = 0.0;
@@ -374,6 +411,10 @@ public class Red_V2 extends LinearOpMode {
double s2D = robot.color2.getDistance(DistanceUnit.MM);
double s3D = robot.color3.getDistance(DistanceUnit.MM);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
if (s1D < 40) {
double green = robot.color1.getNormalizedColors().green;
@@ -477,14 +518,14 @@ public class Red_V2 extends LinearOpMode {
while (opModeInInit()) {
if (gamepad2.dpadUpWasPressed()) {
hoodDefault -= 0.01;
hoodAuto-= 0.01;
}
if (gamepad2.dpadDownWasPressed()) {
hoodDefault += 0.01;
hoodAuto += 0.01;
}
robot.hood.setPosition(hoodDefault);
robot.hood.setPosition(hoodAuto);
robot.turr1.setPosition(turret_detectRed);
robot.turr2.setPosition(1 - turret_detectRed);
@@ -499,13 +540,14 @@ public class Red_V2 extends LinearOpMode {
TELE.update();
}
waitForStart();
if (isStopRequested()) return;
if (opModeIsActive()) {
robot.hood.setPosition(hoodStart);
robot.hood.setPosition(hoodAuto);
Actions.runBlocking(
new ParallelAction(
@@ -514,6 +556,9 @@ public class Red_V2 extends LinearOpMode {
Obelisk()
)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
@@ -522,7 +567,11 @@ public class Red_V2 extends LinearOpMode {
shootingSequence();
robot.hood.setPosition(hoodDefault);
robot.hood.setPosition(hoodAuto);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
Actions.runBlocking(
new ParallelAction(
@@ -530,21 +579,32 @@ public class Red_V2 extends LinearOpMode {
intake(intake1Time)
)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
Actions.runBlocking(
new ParallelAction(
shoot1.build(),
ColorDetect(),
steadyShooter(AUTO_CLOSE_VEL, true)
steadyShooter(AUTO_CLOSE_VEL, true),
intakeReject()
)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
velo = flywheel.getVelo();
robot.shooter1.setPower(powPID);
robot.shooter2.setPower(powPID);
shootingSequence();
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
Actions.runBlocking(
new ParallelAction(
@@ -552,12 +612,17 @@ public class Red_V2 extends LinearOpMode {
intake(intake2Time)
)
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
Actions.runBlocking(
new ParallelAction(
shoot2.build(),
ColorDetect(),
steadyShooter(AUTO_CLOSE_VEL, true)
steadyShooter(AUTO_CLOSE_VEL, true),
intakeReject()
)
);