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

auto coded

Browse Source
This commit is contained in:
DanTheMan-byte
2026-06-03 00:21:51 -05:00
parent 7665957c7a
commit 28451ce26d
1 changed files with 366 additions and 341 deletions
Download Patch File Download Diff File Expand all files Collapse all files

707
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java
View File

@@ -1,341 +1,366 @@
//package org.firstinspires.ftc.teamcode.autonomous; package org.firstinspires.ftc.teamcode.autonomous;
//
//import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
//import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH;
//import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY;
//import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX;
//import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH;
//import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY; import com.acmerobotics.dashboard.FtcDashboard;
//import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX; import com.acmerobotics.dashboard.config.Config;
// import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
//import com.acmerobotics.dashboard.FtcDashboard; import com.pedropathing.follower.Follower;
//import com.acmerobotics.dashboard.config.Config; import com.pedropathing.geometry.BezierCurve;
//import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.pedropathing.geometry.BezierLine;
//import com.pedropathing.follower.Follower; import com.pedropathing.geometry.Pose;
//import com.pedropathing.geometry.BezierCurve; import com.pedropathing.paths.PathChain;
//import com.pedropathing.geometry.BezierLine; import com.qualcomm.hardware.lynx.LynxModule;
//import com.pedropathing.geometry.Pose; import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
//import com.pedropathing.paths.PathChain; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
//import com.qualcomm.hardware.lynx.LynxModule;
//import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import org.firstinspires.ftc.teamcode.constants.Color;
//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
// import org.firstinspires.ftc.teamcode.utilsv2.Flywheel;
//import org.firstinspires.ftc.teamcode.constants.Color; import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
//import org.firstinspires.ftc.teamcode.pedroPathing.Constants; import org.firstinspires.ftc.teamcode.utilsv2.Robot;
//import org.firstinspires.ftc.teamcode.utilsv2.Flywheel; import org.firstinspires.ftc.teamcode.utilsv2.Turret;
//import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
//import org.firstinspires.ftc.teamcode.utilsv2.Robot; import java.util.List;
//import org.firstinspires.ftc.teamcode.utils.Servos;
//import org.firstinspires.ftc.teamcode.utils.Spindexer; @Config
//import org.firstinspires.ftc.teamcode.utils.Targeting; @Autonomous (preselectTeleOp = "TeleopV4")
//import org.firstinspires.ftc.teamcode.utilsv2.Turret; public class Auto12Ball_Back_Sorted extends LinearOpMode {
// Robot robot;
//import java.util.List; MultipleTelemetry TELE;
// Follower follower;
//@Config MeasuringLoopTimes loopTimes;
//@Autonomous (preselectTeleOp = "TeleopV4")
//public class Auto12Ball_Back_Sorted extends LinearOpMode { // Wait Times
// Robot robot; public static double shootTime = 2;
// MultipleTelemetry TELE; public static double openGateTime = 1.5;
// // Flywheel flywheel;
//// Targeting targeting; // Extra Variables
//// Targeting.Settings targetingSettings; public static double intakePower = 0.3;
// Follower follower; double shootX, shootY, shootH;
// // Turret turret;
//// Spindexer spindexer; // Initialize path state machine
//// Servos servos; private enum PathState {
// MeasuringLoopTimes loopTimes; PUSHBOT, DRIVE_SHOOT0, WAIT_SHOOT0,
// PICKUP1, DRIVE_OPENGATE, OPENGATE, DRIVE_SHOOT1, WAIT_SHOOT1,
// // Wait Times DRIVE_PICKUP2, PICKUP2, DRIVE_SHOOT2, WAIT_SHOOT2,
// public static double shootTime = 2; DRIVE_PICKUP3, PICKUP3, DRIVE_SHOOT3, WAIT_SHOOT3
// }
// // Extra Variables PathState pathState = PathState.PUSHBOT;
// public static double intakePower = 0.3;
// double shootX, shootY, shootH; // Poses
// public static double startPoseX = 84, startPoseY = 7, startPoseH = 90;
// // Initialize path state machine public static double pushBotX = 94, pushBotY = 9, pushBotH = 100;
// private enum PathState { public static double shoot0ControlX = 88.29667812142038, shoot0ControlY = 52.03493699885454;
// PUSHBOT, DRIVE_SHOOT0, WAIT_SHOOT0, public static double shoot0X = 91, shoot0Y = 80, shoot0H = 0;
// PICKUP1, OPENGATE, DRIVE_SHOOT1, WAIT_SHOOT1, public static double pickup1ControlX = 109.29381443298968, pickup1ControlY = 82.70618556701031;
// DRIVE_PICKUP2, PICKUP2, DRIVE_SHOOT2, WAIT_SHOOT2, public static double pickup1X = 126, pickup1Y = 82, pickup1H = 0;
// DRIVE_PICKUP3, PICKUP3, DRIVE_SHOOT3, WAIT_SHOOT3 public static double openGateControlX = 109.184421534937, openGateControlY = 74.24455899198165;
// } public static double openGateX = 129, openGateY = 74, openGateH = 0;
// PathState pathState = PathState.DRIVE_SHOOT0; public static double shoot1ControlX = 112, shoot1ControlY = 75;
// public static double shoot1X = 91, shoot1Y = 80, shoot1H = -12;
// // Poses public static double drivePickup2X = 102, drivePickup2Y = 58.5, drivePickup2H = 0;
// public static double startPoseX = 84, startPoseY = 7, startPoseH = 90; public static double pickup2X = 133, pickup2Y = 57, pickup2H = 0;
// public static double pushBotX = 94, pushBotY = 9, pushBotH = 100; public static double shoot2ControlX = 102, shoot2ControlY = 63;
// public static double shoot0X = 91, shoot0Y = 80, shoot0H = 0; public static double shoot2X = 91, shoot2Y = 80, shoot2H = -50;
// public static double pickup1X = 126, pickup1Y = 82, pickup1H = 0; public static double drivePickup3X = 102, drivePickup3Y = 34.5, drivePickup3H = 0;
// public static double openGateControlX = 109.184421534937, openGateControlY = 74.24455899198165; public static double pickup3X = 133, pickup3Y = 34.5, pickup3H = 0;
// public static double openGateX = 129, openGateY = 74, openGateH = 0; public static double shoot3ControlX = 97.62371134020621, shoot3ControlY = 34.813287514318446;
// public static double shoot1ControlX = 112, shoot1ControlY = 75; public static double shoot3X = 84, shoot3Y = 105, shoot3H = -80;
// public static double shoot1X = 91, shoot1Y = 80, shoot1H = -12; Pose startPose, pushBot, shoot0Control, shoot0,
// public static double drivePickup2X = 102, drivePickup2Y = 58.5, drivePickup2H = 0; pickup1Control, pickup1, openGateControl, openGate, shoot1Control, shoot1,
// public static double pickup2X = 133, pickup2Y = 57, pickup2H = 0; drivePickup2, pickup2, shoot2Control, shoot2,
// public static double shoot2ControlX = 102, shoot2ControlY = 63; drivePickup3, pickup3, shoot3Control, shoot3;
// public static double shoot2X = 91, shoot2Y = 80, shoot2H = -50; private void initializePoses(){
// public static double drivePickup3X = 102, drivePickup3Y = 34.5, drivePickup3H = 0; startPose = new Pose(startPoseX, startPoseY, Math.toRadians(startPoseH));
// public static double pickup3X = 133, pickup3Y = 34.5, pickup3H = 0; pushBot = new Pose(pushBotX, pushBotY, Math.toRadians(pushBotH));
// public static double shoot3ControlX = 97.62371134020621, shoot3ControlY = 34.813287514318446; shoot0Control = new Pose(shoot0ControlX, shoot0ControlY);
// public static double shoot3X = 84, shoot3Y = 105, shoot3H = -80; shoot0 = new Pose(shoot0X, shoot0Y, Math.toRadians(shoot0H));
// Pose startPose, pushBot, shoot0, pickup1Control = new Pose(pickup1ControlX, pickup1ControlY);
// pickup1, openGateControl, openGate, shoot1Control, shoot1, pickup1 = new Pose(pickup1X, pickup1Y, Math.toRadians(pickup1H));
// drivePickup2, pickup2, shoot2Control, shoot2, openGateControl = new Pose(openGateControlX, openGateControlY);
// drivePickup3, pickup3, shoot3Control, shoot3; openGate = new Pose(openGateX, openGateY, Math.toRadians(openGateH));
// private void initializePoses(){ shoot1Control = new Pose(shoot1ControlX, shoot1ControlY);
// startPose = new Pose(startPoseX, startPoseY, Math.toRadians(startPoseH)); shoot1 = new Pose(shoot1X, shoot1Y, Math.toRadians(shoot1H));
// pushBot = new Pose(pushBotX, pushBotY, Math.toRadians(pushBotH)); drivePickup2 = new Pose(drivePickup2X, drivePickup2Y, Math.toRadians(drivePickup2H));
// shoot0 = new Pose(shoot0X, shoot0Y, Math.toRadians(shoot0H)); pickup2 = new Pose(pickup2X, pickup2Y, Math.toRadians(pickup2H));
// pickup1 = new Pose(pickup1X, pickup1Y, Math.toRadians(pickup1H)); shoot2Control = new Pose(shoot2ControlX, shoot2ControlY);
// openGateControl = new Pose(openGateControlX, openGateControlY); shoot2 = new Pose(shoot2X, shoot2Y, Math.toRadians(shoot2H));
// openGate = new Pose(openGateX, openGateY, Math.toRadians(openGateH)); drivePickup3 = new Pose(drivePickup3X, drivePickup3Y, Math.toRadians(drivePickup3H));
// shoot1Control = new Pose(shoot1ControlX, shoot1ControlY); pickup3 = new Pose(pickup3X, pickup3Y, Math.toRadians(pickup3H));
// shoot1 = new Pose(shoot1X, shoot1Y, Math.toRadians(shoot1H)); shoot3Control = new Pose(shoot3ControlX, shoot3ControlY);
// drivePickup2 = new Pose(drivePickup2X, drivePickup2Y, Math.toRadians(drivePickup2H)); shoot3 = new Pose(shoot3X, shoot3Y, Math.toRadians(shoot3H));
// pickup2 = new Pose(pickup2X, pickup2Y, Math.toRadians(pickup2H)); }
// shoot2Control = new Pose(shoot2ControlX, shoot2ControlY);
// shoot2 = new Pose(shoot2X, shoot2Y, Math.toRadians(shoot2H)); //Building Paths
// drivePickup3 = new Pose(drivePickup3X, drivePickup3Y, Math.toRadians(drivePickup3H)); PathChain startPose_pushBot, pushBot_shoot0,
// pickup3 = new Pose(pickup3X, pickup3Y, Math.toRadians(pickup3H)); shoot0_pickup1, pickup1_openGate, openGate_shoot1,
// shoot3Control = new Pose(shoot3ControlX, shoot3ControlY); shoot1_drivePickup2, drivePickup2_pickup2, pickup2_shoot2,
// shoot3 = new Pose(shoot3X, shoot3Y, Math.toRadians(shoot3H)); shoot2_drivePickup3, drivePickup3_pickup3, pickup3_shoot3;
// } private void buildPaths(){
// startPose_pushBot = follower.pathBuilder()
// //Building Paths .addPath(new BezierLine(startPose, pushBot))
// PathChain startPose_pushBot, pushBot_shoot0, .setLinearHeadingInterpolation(startPose.getHeading(), pushBot.getHeading())
// shoot0_pickup1, pickup1_openGate, openGate_shoot1, .build();
// shoot1_drivePickup2, drivePickup2_pickup2, pickup2_shoot2,
// shoot2_drivePickup3, drivePickup3_pickup3, pickup3_shoot3; pushBot_shoot0 = follower.pathBuilder()
// private void buildPaths(){ .addPath(new BezierCurve(pushBot, shoot0Control, shoot0))
// startPose_pushBot = follower.pathBuilder() .setLinearHeadingInterpolation(pushBot.getHeading(), shoot0.getHeading())
// .addPath(new BezierLine(startPose, pushBot)) .build();
// .setLinearHeadingInterpolation(startPose.getHeading(), pushBot.getHeading())
// .build(); shoot0_pickup1 = follower.pathBuilder()
// } .addPath(new BezierCurve(shoot0, pickup1Control, pickup1))
// .setLinearHeadingInterpolation(shoot0.getHeading(), pickup1.getHeading())
// //Path State Machine .build();
// private boolean startAuto = true;
// private double timeStamp = 0; pickup1_openGate = follower.pathBuilder()
// private void pathStateMachine(){ .addPath(new BezierCurve(pickup1, openGateControl, openGate))
// double currentTime = (double) System.currentTimeMillis() / 1000; .setLinearHeadingInterpolation(pickup1.getHeading(), openGate.getHeading())
// switch(pathState){ .build();
// case DRIVE_SHOOT0:
// if (startAuto){ openGate_shoot1 = follower.pathBuilder()
// follower.followPath(startPose_shoot0, true); .addPath(new BezierCurve(openGate, shoot1Control, shoot1))
// startAuto = false; .setLinearHeadingInterpolation(openGate.getHeading(), shoot1.getHeading())
// shootX = shoot0X; .build();
// shootY = shoot0Y;
// shootH = shoot0H; shoot1_drivePickup2 = follower.pathBuilder()
// } .addPath(new BezierLine(shoot1, drivePickup2))
// driveShoot(PathState.WAIT_SHOOT0, currentTime); .setLinearHeadingInterpolation(shoot1.getHeading(), drivePickup2.getHeading())
// break; .build();
// case WAIT_SHOOT0:
// waitShoot(PathState.DRIVE_PICKUP1, shoot0_drivePickup1, currentTime); drivePickup2_pickup2 = follower.pathBuilder()
// break; .addPath(new BezierLine(drivePickup2, pickup2))
// case DRIVE_PICKUP1: .setLinearHeadingInterpolation(drivePickup2.getHeading(), pickup2.getHeading())
// drivePickup(PathState.PICKUP1, drivePickup1_pickup1); .build();
// break;
// case PICKUP1: pickup2_shoot2 = follower.pathBuilder()
// pickup(PathState.DRIVE_SHOOT1, pickup1_shoot1); .addPath(new BezierCurve(pickup2, shoot2Control, shoot2))
// shootX = shoot1X; .setLinearHeadingInterpolation(pickup2.getHeading(), shoot2.getHeading())
// shootY = shoot1Y; .build();
// shootH = shoot1H;
// break; shoot2_drivePickup3 = follower.pathBuilder()
// case DRIVE_SHOOT1: .addPath(new BezierLine(shoot2, drivePickup3))
// intakePowerDown(timeStamp, currentTime); .setLinearHeadingInterpolation(shoot2.getHeading(), drivePickup3.getHeading())
// driveShoot(PathState.WAIT_SHOOT1, currentTime); .build();
// break;
// case WAIT_SHOOT1: drivePickup3_pickup3 = follower.pathBuilder()
// waitShoot(PathState.DRIVE_PICKUP2, shoot1_drivePickup2, currentTime); .addPath(new BezierLine(drivePickup3, pickup3))
// break; .setLinearHeadingInterpolation(drivePickup3.getHeading(), pickup3.getHeading())
// case DRIVE_PICKUP2: .build();
// drivePickup(PathState.PICKUP2, drivePickup2_pickup2);
// break; pickup3_shoot3 = follower.pathBuilder()
// case PICKUP2: .addPath(new BezierCurve(pickup3, shoot3Control, shoot3))
// pickup(PathState.DRIVE_SHOOT2, pickup2_shoot2); .setLinearHeadingInterpolation(pickup3.getHeading(), shoot3.getHeading())
// shootX = shoot2X; .build();
// shootY = shoot2Y; }
// shootH = shoot2H;
// break; //Path State Machine
// case DRIVE_SHOOT2: private boolean startAuto = true;
// intakePowerDown(timeStamp, currentTime); private double timeStamp = 0;
// driveShoot(PathState.WAIT_SHOOT2, currentTime); private void pathStateMachine(){
// break; double currentTime = (double) System.currentTimeMillis() / 1000;
// case WAIT_SHOOT2: switch(pathState){
// waitShoot(PathState.DRIVE_PICKUP3, shoot2_drivePickup3, currentTime); case PUSHBOT:
// break; if (startAuto){
// case DRIVE_PICKUP3: follower.followPath(startPose_pushBot, true);
// drivePickup(PathState.PICKUP3, drivePickup3_pickup3); startAuto = false;
// break; shootX = shoot0X;
// case PICKUP3: shootY = shoot0Y;
// pickup(PathState.DRIVE_SHOOT3, pickup3_shoot3); shootH = shoot0H;
// shootX = shoot3X; }
// shootY = shoot3Y; if (!follower.isBusy()){
// shootH = shoot3H; follower.followPath(pushBot_shoot0, true);
// break; pathState = PathState.DRIVE_SHOOT0;
// case DRIVE_SHOOT3: }
// intakePowerDown(timeStamp, currentTime); break;
// driveShoot(PathState.WAIT_SHOOT3, currentTime); case DRIVE_SHOOT0:
// break; if (!follower.isBusy()){
// case WAIT_SHOOT3: timeStamp = currentTime;
//// if (spindexer.shootAllComplete()){ pathState = PathState.WAIT_SHOOT0;
//// spindexer.resetSpindexer(); }
//// TELE.addLine("Done Auto"); break;
//// } case WAIT_SHOOT0:
// break; if (currentTime - timeStamp > shootTime){
// default: follower.followPath(shoot0_pickup1, intakePower, false);
// break; pathState = PathState.PICKUP1;
// } }
// TELE.update(); // use for debugging break;
// } case PICKUP1:
// if (!follower.isBusy()){
// // Voids for State Machine follower.followPath(pickup1_openGate, true);
// private void intakePowerDown(double stamp, double currentTime) { pathState = PathState.OPENGATE;
// double pow = (stamp - currentTime) / 2; // adjust denominator to see how much time to adjust shootX = shoot1X;
// if (pow < -1) {pow = 0;} shootY = shoot1Y;
//// spindexer.setIntakePower(pow); shootH = shoot1H;
// } }
// private void driveShoot(PathState nextState, double currentTime){ break;
// if (!follower.isBusy()){ case DRIVE_OPENGATE:
// pathState = nextState; if (!follower.isBusy()){
// timeStamp = currentTime; pathState = PathState.OPENGATE;
//// spindexer.prepareShootAllContinous(); timeStamp = currentTime;
// } }
// } break;
// private void waitShoot(PathState nextState, PathChain nextPath, double currentTime) { case OPENGATE:
// if (currentTime - timeStamp > shootTime) { // spindexer.shootAllComplete() || if (currentTime - timeStamp > openGateTime){
//// spindexer.resetSpindexer(); follower.followPath(openGate_shoot1, true);
// pathState = nextState; pathState = PathState.DRIVE_SHOOT1;
// follower.followPath(nextPath, true); }
//// spindexer.setIntakePower(1); break;
// } case DRIVE_SHOOT1:
// } if (!follower.isBusy()){
// private void drivePickup(PathState nextState, PathChain nextPath) { pathState = PathState.WAIT_SHOOT1;
// if (!follower.isBusy()) { timeStamp = currentTime;
// pathState = nextState; }
// follower.followPath(nextPath, intakePower, false); break;
// } case WAIT_SHOOT1:
// } if (currentTime - timeStamp > shootTime){
// private void pickup(PathState nextState, PathChain nextPath) { follower.followPath(shoot1_drivePickup2, true);
// if (!follower.isBusy()) { pathState = PathState.DRIVE_PICKUP2;
// pathState = nextState; }
// follower.followPath(nextPath, true); break;
// } case DRIVE_PICKUP2:
// } if (!follower.isBusy()) {
// follower.followPath(drivePickup2_pickup2, intakePower, false);
// // Used for changing alliance pathState = PathState.PICKUP2;
// private double adjustXPoseBasedOnAlliance(double pose) {return (144-pose);} }
// private double adjustHeadingBasedOnAlliance(double heading){ break;
// heading = 180 - heading; case PICKUP2:
// while (heading > 180) {heading-=360;} if (!follower.isBusy()){
// while (heading <= -180) {heading+=360;} follower.followPath(pickup2_shoot2, true);
// return heading; pathState = PathState.DRIVE_SHOOT2;
// } }
// shootX = shoot2X;
// @Override shootY = shoot2Y;
// public void runOpMode() throws InterruptedException { shootH = shoot2H;
// Robot.resetInstance(); break;
// robot = Robot.getInstance(hardwareMap); case DRIVE_SHOOT2:
// List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class); if (!follower.isBusy()){
// for (LynxModule hub : allHubs) { pathState = PathState.WAIT_SHOOT2;
// hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); timeStamp = currentTime;
// } }
// TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); break;
//// flywheel = new Flywheel(hardwareMap); case WAIT_SHOOT2:
//// targeting = new Targeting(); if (currentTime - timeStamp > shootTime){
//// targetingSettings = new Targeting.Settings(0,0); follower.followPath(shoot2_drivePickup3, true);
// follower = Constants.createFollower(hardwareMap); pathState = PathState.DRIVE_PICKUP3;
// follower.setStartingPose(new Pose(72,72,0)); }
//// turret = new Turret(robot, TELE, robot.limelight); break;
//// spindexer = new Spindexer(hardwareMap); case DRIVE_PICKUP3:
//// servos = new Servos(hardwareMap); if (!follower.isBusy()){
// loopTimes = new MeasuringLoopTimes(); follower.followPath(drivePickup3_pickup3, intakePower, false);
// loopTimes.init(); pathState = PathState.PICKUP3;
// }
//// robot.light.setPosition(Color.LightRed); break;
// case PICKUP3:
// boolean initializeRobot = false; if (!follower.isBusy()){
// while (opModeInInit()){ follower.followPath(pickup3_shoot3, true);
// follower.update(); pathState = PathState.DRIVE_SHOOT3;
// }
// if (gamepad1.crossWasPressed() && !initializeRobot){ shootX = shoot3X;
// Color.redAlliance = !Color.redAlliance; shootY = shoot3Y;
//// if (Color.redAlliance){ shootH = shoot3H;
//// robot.light.setPosition(Color.LightRed); break;
//// } else { case DRIVE_SHOOT3:
//// robot.light.setPosition(Color.LightBlue); if (!follower.isBusy()){
//// } pathState = PathState.WAIT_SHOOT3;
// }
//// double[] xPoses = {startPoseX, shoot0X, break;
//// drivePickup1X, pickup1X, shoot1X, case WAIT_SHOOT3:
//// drivePickup2ControlX, drivePickup2X, pickup2X, shoot2ControlX, shoot2X, // add line here to say "done auto'
//// drivePickup3ControlX, drivePickup3X, pickup3X, shoot3ControlX, shoot3X}; break;
//// default:
//// double[] headings = {startPoseH, shoot0H, break;
//// drivePickup1H, pickup1H, shoot1H, }
//// drivePickup2H, pickup2H, shoot2H, TELE.update(); // use for debugging
//// drivePickup3H, pickup3H, shoot3H}; }
//
//// for (int i = 0; i < xPoses.length; i++) {xPoses[i] = adjustXPoseBasedOnAlliance(xPoses[i]);} // Used for changing alliance
//// for (int i = 0; i < headings.length; i++) {headings[i] = adjustHeadingBasedOnAlliance(headings[i]);} private double adjustXPoseBasedOnAlliance(double pose) {return (144-pose);}
// } private double adjustHeadingBasedOnAlliance(double heading){
// heading = 180 - heading;
// if (gamepad1.triangleWasPressed()){ while (heading > 180) {heading-=360;}
// initializeRobot = true; while (heading <= -180) {heading+=360;}
// initializePoses(); return heading;
// follower.setPose(startPose); }
// buildPaths();
// sleep(2000); @Override
// public void runOpMode() throws InterruptedException {
//// turret.setTurret(turrDefault); Robot.resetInstance();
//// servos.setSpinPos(spinStartPos); robot = Robot.getInstance(hardwareMap);
// } List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class);
// for (LynxModule hub : allHubs) {
// TELE.addData("Red Alliance?", Color.redAlliance); hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
// TELE.addData("Initialized Robot? (Don't change this until alliance is selected)", initializeRobot); }
// TELE.addData("Start Pose", follower.getPose()); TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
// TELE.update(); follower = Constants.createFollower(hardwareMap);
// } follower.setStartingPose(new Pose(72,72,0));
// loopTimes = new MeasuringLoopTimes();
// waitForStart(); loopTimes.init();
//
// if (isStopRequested()) return; boolean initializeRobot = false;
// while (opModeInInit()){
//// robot.transfer.setPower(1); follower.update();
// limelightUsed = false;
// if (gamepad1.crossWasPressed() && !initializeRobot){
// while (opModeIsActive()){ Color.redAlliance = !Color.redAlliance;
// follower.update();
// pathStateMachine(); double[] xPoses = {startPoseX, pushBotX, shoot0ControlX, shoot0X,
// Pose currentPose = follower.getPose(); pickup1ControlX, pickup1X, openGateControlX, openGateX, shoot1ControlX, shoot1X,
//// teleStartPoseX = currentPose.getX(); drivePickup2X, pickup2X, shoot2ControlX, shoot2X,
//// teleStartPoseY = currentPose.getY(); drivePickup3X, pickup3X, shoot3ControlX, shoot3X};
//// teleStartPoseH = Math.toDegrees(currentPose.getHeading());
//// double[] headings = {startPoseH, pushBotH, shoot0H,
//// turret.trackGoal(new Pose(shootX, shootY, Math.toRadians(shootH))); pickup1H, openGateH, shoot1H,
//// targetingSettings = targeting.calculateSettings(shootX, shootY, Math.toRadians(shootH), 0, turretInterpolate); drivePickup2H, pickup2H, shoot2H,
//// drivePickup3H, pickup3H, shoot3H};
//// double voltage = robot.voltage.getVoltage();
//// flywheel.setPIDF(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F / voltage); for (int i = 0; i < xPoses.length; i++) {xPoses[i] = adjustXPoseBasedOnAlliance(xPoses[i]);}
//// flywheel.manageFlywheel(targetingSettings.flywheelRPM); for (int i = 0; i < headings.length; i++) {headings[i] = adjustHeadingBasedOnAlliance(headings[i]);}
//// servos.setHoodPos(targetingSettings.hoodAngle); }
////
//// if (driveToShoot()){servos.setSpinPos(spinStartPos);} if (gamepad1.triangleWasPressed()){
//// else {spindexer.processIntake();} initializeRobot = true;
// initializePoses();
// for (LynxModule hub : allHubs) { follower.setPose(startPose);
// hub.clearBulkCache(); buildPaths();
// } sleep(2000);
// loopTimes.loop(); }
//
// TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime()); TELE.addData("Red Alliance?", Color.redAlliance);
// TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin()); TELE.addData("Initialized Robot? (Don't change this until alliance is selected)", initializeRobot);
// TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin()); TELE.addData("Start Pose", follower.getPose());
// TELE.addData("X:", currentPose.getX()); TELE.update();
// TELE.addData("Y:", currentPose.getY()); }
// TELE.addData("H:", currentPose.getHeading());
// TELE.update(); waitForStart();
// }
// } if (isStopRequested()) return;
//}
limelightUsed = false;
while (opModeIsActive()){
follower.update();
pathStateMachine();
Pose currentPose = follower.getPose();
teleStartPoseX = currentPose.getX();
teleStartPoseY = currentPose.getY();
teleStartPoseH = Math.toDegrees(currentPose.getHeading());
for (LynxModule hub : allHubs) {
hub.clearBulkCache();
}
loopTimes.loop();
TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime());
TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin());
TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin());
TELE.addData("X:", currentPose.getX());
TELE.addData("Y:", currentPose.getY());
TELE.addData("H:", currentPose.getHeading());
TELE.update();
}
}
}