Before LM1
This commit is contained in:
@@ -0,0 +1,425 @@
|
||||
package org.firstinspires.ftc.teamcode.autonomous;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.h1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.h2;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.h2_b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.h3;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.x1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.x2;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.x2_b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.x3;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.y1;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.y2;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.y2_b;
|
||||
import static org.firstinspires.ftc.teamcode.constants.Poses.y3;
|
||||
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.config.Config;
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.acmerobotics.roadrunner.ParallelAction;
|
||||
import com.acmerobotics.roadrunner.Pose2d;
|
||||
import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
|
||||
import com.acmerobotics.roadrunner.Vector2d;
|
||||
import com.acmerobotics.roadrunner.ftc.Actions;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
|
||||
|
||||
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;
|
||||
import org.firstinspires.ftc.teamcode.utils.Robot;
|
||||
|
||||
|
||||
@Config
|
||||
@Autonomous
|
||||
public class Blue extends LinearOpMode {
|
||||
|
||||
Robot robot;
|
||||
|
||||
MultipleTelemetry TELE;
|
||||
|
||||
MecanumDrive drive;
|
||||
|
||||
AprilTag aprilTag;
|
||||
|
||||
Shooter shooter;
|
||||
|
||||
|
||||
|
||||
public static double angle = 0.1;
|
||||
|
||||
Spindexer spindexer;
|
||||
|
||||
Transfer transfer;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@Override
|
||||
public void runOpMode() throws InterruptedException {
|
||||
|
||||
|
||||
|
||||
|
||||
robot = new Robot(hardwareMap);
|
||||
TELE = new MultipleTelemetry(
|
||||
telemetry, FtcDashboard.getInstance().getTelemetry()
|
||||
);
|
||||
|
||||
drive = new MecanumDrive(hardwareMap, new Pose2d(
|
||||
0,0,0
|
||||
));
|
||||
|
||||
aprilTag = new AprilTag(robot, TELE);
|
||||
|
||||
shooter = new Shooter(robot, TELE);
|
||||
|
||||
spindexer = new Spindexer(robot, TELE);
|
||||
|
||||
spindexer.outtake3();
|
||||
|
||||
shooter.setShooterMode("MANUAL");
|
||||
|
||||
shooter.sethoodPosition(0.53);
|
||||
|
||||
transfer = new Transfer(robot);
|
||||
|
||||
transfer.setTransferPosition(transferServo_out);
|
||||
|
||||
Intake intake = new Intake(robot);
|
||||
|
||||
robot.hood.setPosition(0.54);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
TrajectoryActionBuilder traj1 = drive.actionBuilder(new Pose2d(0, 0, 0))
|
||||
.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-30), h1 );
|
||||
|
||||
while(opModeInInit()) {
|
||||
|
||||
shooter.setTurretPosition(0.33);
|
||||
|
||||
aprilTag.initTelemetry();
|
||||
|
||||
aprilTag.update();
|
||||
shooter.update();
|
||||
|
||||
TELE.update();
|
||||
}
|
||||
|
||||
|
||||
waitForStart();
|
||||
|
||||
if (isStopRequested()) return;
|
||||
|
||||
if (opModeIsActive()){
|
||||
|
||||
robot.hood.setPosition(0.54);
|
||||
|
||||
transfer.setTransferPower(1);
|
||||
|
||||
transfer.update();
|
||||
|
||||
|
||||
|
||||
|
||||
Actions.runBlocking(
|
||||
new ParallelAction(
|
||||
traj1.build()
|
||||
)
|
||||
);
|
||||
|
||||
|
||||
transfer.setTransferPower(1);
|
||||
|
||||
transfer.update();
|
||||
|
||||
shooter.setManualPower(1);
|
||||
|
||||
double stamp = getRuntime();
|
||||
|
||||
|
||||
stamp = getRuntime();
|
||||
|
||||
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(
|
||||
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);
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -59,6 +59,8 @@ public class Red extends LinearOpMode {
|
||||
public void runOpMode() throws InterruptedException {
|
||||
|
||||
|
||||
|
||||
|
||||
robot = new Robot(hardwareMap);
|
||||
TELE = new MultipleTelemetry(
|
||||
telemetry, FtcDashboard.getInstance().getTelemetry()
|
||||
@@ -86,6 +88,8 @@ public class Red extends LinearOpMode {
|
||||
|
||||
Intake intake = new Intake(robot);
|
||||
|
||||
robot.hood.setPosition(0.54);
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -117,7 +121,7 @@ public class Red extends LinearOpMode {
|
||||
|
||||
|
||||
TrajectoryActionBuilder traj6 = drive.actionBuilder(new Pose2d(x1, y1, h1))
|
||||
.strafeToLinearHeading(new Vector2d(x1, y1-10), h1 );
|
||||
.strafeToLinearHeading(new Vector2d(x1, y1+30), h1 );
|
||||
|
||||
while(opModeInInit()) {
|
||||
|
||||
@@ -138,6 +142,8 @@ public class Red extends LinearOpMode {
|
||||
|
||||
if (opModeIsActive()){
|
||||
|
||||
robot.hood.setPosition(0.54);
|
||||
|
||||
transfer.setTransferPower(1);
|
||||
|
||||
transfer.update();
|
||||
|
||||
@@ -17,7 +17,7 @@ public class Poses {
|
||||
|
||||
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 x2_b = 65, y2_b = 35, h2_b = Math.toRadians(135);
|
||||
|
||||
|
||||
public static double x3 = 45, y3 = 53, h3 = Math.toRadians(135);
|
||||
|
||||
@@ -295,12 +295,7 @@ public class TeleopV1 extends LinearOpMode {
|
||||
offset +=0.02;
|
||||
}
|
||||
|
||||
if (gamepad2.right_stick_y < 0.7){
|
||||
pos = shooter.getAngleByDist(
|
||||
shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0), offset)
|
||||
);
|
||||
|
||||
}
|
||||
|
||||
TELE.addData("hood", pos);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user