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

Before LM1

Browse Source
This commit is contained in:
KeshavAnandCode
2025-11-08 00:09:12 -06:00
parent 526bd62224
commit e89a659136
5 changed files with 311 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

277
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red.java
View File

@@ -22,6 +22,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.constants.ServoPositions;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.subsystems.AprilTag;
import org.firstinspires.ftc.teamcode.subsystems.Intake;
import org.firstinspires.ftc.teamcode.subsystems.Shooter;
import org.firstinspires.ftc.teamcode.subsystems.Spindexer;
import org.firstinspires.ftc.teamcode.subsystems.Transfer;
@@ -77,12 +78,14 @@ public class Red extends LinearOpMode {
shooter.setShooterMode("MANUAL");
shooter.sethoodPosition(0.54);
shooter.sethoodPosition(0.53);
transfer = new Transfer(robot);
transfer.setTransferPosition(transferServo_out);
Intake intake = new Intake(robot);
@@ -94,11 +97,36 @@ public class Red extends LinearOpMode {
.strafeToLinearHeading(new Vector2d(x1, y1), h1 );
TrajectoryActionBuilder traj2 = drive.actionBuilder(new Pose2d(x1, y1, h1))
.turnTo(Math.toRadians(135))
.strafeToLinearHeading(new Vector2d(x2, y2), h2 );
TrajectoryActionBuilder traj3 = drive.actionBuilder(new Pose2d(x2, y2, h2))
.strafeToLinearHeading(new Vector2d(x1, y1), h1 );
TrajectoryActionBuilder traj4 = drive.actionBuilder(new Pose2d(x1, y1, h1))
.strafeToLinearHeading(new Vector2d(x2_b, y2_b), h2_b )
.strafeToLinearHeading(new Vector2d(x3, y3), h3 );
TrajectoryActionBuilder traj5 = drive.actionBuilder(new Pose2d(x3, y3, h3))
.strafeToLinearHeading(new Vector2d(x1, y1), h1 );
TrajectoryActionBuilder traj6 = drive.actionBuilder(new Pose2d(x1, y1, h1))
.strafeToLinearHeading(new Vector2d(x1, y1-10), h1 );
while(opModeInInit()) {
shooter.setTurretPosition(0.33);
aprilTag.initTelemetry();
aprilTag.update();
shooter.update();
TELE.update();
}
@@ -123,7 +151,6 @@ public class Red extends LinearOpMode {
)
);
shooter.moveTurret(angle);
transfer.setTransferPower(1);
@@ -133,83 +160,251 @@ public class Red extends LinearOpMode {
double stamp = getRuntime();
boolean gpp =false;
boolean pgp = false;
boolean ppg = false;
stamp = getRuntime();
aprilTag.turnTelemetryOn(true);
while(getRuntime()-stamp <4.5) {
aprilTag.update();
while (getRuntime()-stamp<4.5) {
gpp = aprilTag.isDetected(21);
pgp = aprilTag.isDetected(22);
shooter.moveTurret(0.31);
ppg = aprilTag.isDetected(23);
double time = getRuntime()-stamp;
TELE.update();
}
robot.turr1.setPosition(0.9);
robot.turr2.setPosition(0.1);
int ticker = 0;
if (gpp){
if (time < 0.3) {
ticker = 0;
transfer.transferOut();
transfer.setTransferPower(1);
} else if (time < 2) {
} else if (time < 1) {
spindexer.outtake3();
} else if (time < 1.4) {
transfer.transferIn();
} else if (time < 2.6) {
transfer.transferOut();
spindexer.outtake2();
} else if (time < 2.5) {
transfer.transferIn();
} else if (time < 4) {
transfer.transferOut();
spindexer.outtake1();
ticker++;
} else if (time < 4.5) {
} else if (time < 3.0) {
transfer.transferIn();
} else if (time < 6) {
} else if (time < 4.0) {
transfer.transferOut();
spindexer.outtake1();
} else if (time < 6.5) {
} else if (time < 4.4) {
transfer.transferIn();
} else {
transfer.transferOut();
spindexer.outtake3();
shooter.setManualPower(0);
}
transfer.update();
shooter.update();
spindexer.update();
}
spindexer.outtake3();
robot.intake.setPower(1);
Actions.runBlocking(
traj2.build()
);
Actions.runBlocking(
traj3.build()
);
shooter.setManualPower(1);
robot.intake.setPower(0);
spindexer.outtake3();
stamp = getRuntime();
shooter.setManualPower(1);
while (getRuntime()-stamp<4.5) {
shooter.moveTurret(0.31);
double time = getRuntime()-stamp;
if (time < 0.3) {
transfer.transferOut();
transfer.setTransferPower(1);
} else if (time < 1) {
spindexer.outtake3();
} else if (time < 1.4) {
transfer.transferIn();
} else if (time < 2.6) {
transfer.transferOut();
spindexer.outtake2();
} else if (time < 3.0) {
transfer.transferIn();
} else if (time < 4.0) {
transfer.transferOut();
spindexer.outtake1();
} else if (time < 4.4) {
transfer.transferIn();
} else {
transfer.transferOut();
spindexer.outtake3();
shooter.setManualPower(0);
}
transfer.update();
shooter.update();
spindexer.update();
}
spindexer.outtake3();
robot.intake.setPower(1);
Actions.runBlocking(
traj4.build()
);
Actions.runBlocking(
traj5.build()
);
shooter.setManualPower(1);
robot.intake.setPower(0);
stamp = getRuntime();
shooter.setManualPower(1);
while (getRuntime()-stamp<4.5) {
shooter.moveTurret(0.31);
double time = getRuntime()-stamp;
if (time < 0.3) {
transfer.transferOut();
transfer.setTransferPower(1);
} else if (time < 1) {
spindexer.outtake3();
} else if (time < 1.4) {
transfer.transferIn();
} else if (time < 2.6) {
transfer.transferOut();
spindexer.outtake2();
} else if (time < 3.0) {
transfer.transferIn();
} else if (time < 4.0) {
transfer.transferOut();
spindexer.outtake1();
} else if (time < 4.4) {
transfer.transferIn();
} else {
transfer.transferOut();
spindexer.outtake3();
shooter.setManualPower(0);
}
transfer.update();
shooter.update();
spindexer.update();
}
spindexer.outtake3();
Actions.runBlocking(
traj6.build()
);
drive.updatePoseEstimate();
teleStart = drive.localizer.getPose();
TELE.addLine("finished");
TELE.update();
sleep (2000);
}

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java
View File

@@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.constants;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.Pose2d;
@Config
public class Poses {
@@ -14,6 +15,15 @@ public class Poses {
public static double x1 = 50, y1 = 0, h1 = 0;
public static double x2 = 31, y2 = 32, h2 = Math.toRadians(135);
public static double x2_b = 75, y2_b = 25, h2_b = Math.toRadians(135);
public static double x3 = 45, y3 = 53, h3 = Math.toRadians(135);
public static Pose2d teleStart = new Pose2d(x1,-10,0);
}

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java
View File

@@ -73,9 +73,9 @@ public final class MecanumDrive {
public double kA = 0.000046;
// path profile parameters (in inches)
public double maxWheelVel = 80;
public double maxWheelVel = 120;
public double minProfileAccel = -30;
public double maxProfileAccel = 80;
public double maxProfileAccel = 120;
// turn profile parameters (in radians)
public double maxAngVel = 2* Math.PI; // shared with path

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java
View File

@@ -1,5 +1,7 @@
package org.firstinspires.ftc.teamcode.teleop;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
@@ -118,7 +120,7 @@ public class TeleopV1 extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
drive = new MecanumDrive(hardwareMap, new Pose2d(0,0,0));
drive = new MecanumDrive(hardwareMap, teleStart);
@@ -216,7 +218,7 @@ public class TeleopV1 extends LinearOpMode {
if (isStopRequested()) return;
drive = new MecanumDrive(hardwareMap, new Pose2d(0,0,0));
drive = new MecanumDrive(hardwareMap, teleStart);
while(opModeIsActive()){

59
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/WebcamTest.java Normal file
View File

@@ -0,0 +1,59 @@
package org.firstinspires.ftc.teamcode.teleop;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.subsystems.AprilTag;
@TeleOp
@Config
public class WebcamTest extends LinearOpMode {
AprilTag webcam;
MultipleTelemetry TELE;
Robot robot;
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
webcam = new AprilTag(robot, TELE);
webcam.turnTelemetryOn(true);
while(opModeInInit()){
webcam.initTelemetry();
TELE.update();
};
if(isStopRequested()) return;
while (opModeIsActive()){
webcam.update();
TELE.update();
}
}
}