From e84fe97cb45d9d101c9c7416dcc31a3b7b723ab6 Mon Sep 17 00:00:00 2001 From: sihan Date: Wed, 8 Nov 2023 11:31:33 -0600 Subject: [PATCH] all 3 auto spikes work --- .../roadrunner/drive/opmode/AutomaticFeedforwardTuner.java | 2 ++ .../hardware/roadrunner/drive/opmode/BackAndForth.java | 2 ++ .../roadrunner/drive/opmode/DriveVelocityPIDTuner.java | 2 ++ .../hardware/roadrunner/drive/opmode/FollowerPIDTuner.java | 2 ++ .../hardware/roadrunner/drive/opmode/LocalizationTest.java | 2 ++ .../roadrunner/drive/opmode/ManualFeedforwardTuner.java | 2 ++ .../hardware/roadrunner/drive/opmode/MaxAngularVeloTuner.java | 2 ++ .../hardware/roadrunner/drive/opmode/MaxVelocityTuner.java | 2 ++ .../roadrunner/drive/opmode/MotorDirectionDebugger.java | 1 + .../teamcode/hardware/roadrunner/drive/opmode/SplineTest.java | 2 ++ .../teamcode/hardware/roadrunner/drive/opmode/StrafeTest.java | 2 ++ .../hardware/roadrunner/drive/opmode/StraightTest.java | 2 ++ .../hardware/roadrunner/drive/opmode/TrackWidthTuner.java | 2 ++ .../drive/opmode/TrackingWheelForwardOffsetTuner.java | 2 ++ .../drive/opmode/TrackingWheelLateralDistanceTuner.java | 2 ++ .../teamcode/hardware/roadrunner/drive/opmode/TurnTest.java | 2 ++ .../java/org/firstinspires/ftc/teamcode/opmodes/Auto.java | 4 ++-- 17 files changed, 33 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/AutomaticFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/AutomaticFeedforwardTuner.java index 57857ba..f24afb0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/AutomaticFeedforwardTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/AutomaticFeedforwardTuner.java @@ -10,6 +10,7 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.util.NanoClock; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.util.RobotLog; @@ -33,6 +34,7 @@ import java.util.List; * regression. */ @Config +@Disabled @Autonomous(group = "drive") public class AutomaticFeedforwardTuner extends LinearOpMode { public static double MAX_POWER = 0.7; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/BackAndForth.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/BackAndForth.java index dbb659d..2082bca 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/BackAndForth.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/BackAndForth.java @@ -4,6 +4,7 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.trajectory.Trajectory; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; @@ -25,6 +26,7 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; * is recommended that you use the FollowerPIDTuner opmode for further fine tuning. */ @Config +@Disabled @Autonomous(group = "drive") public class BackAndForth extends LinearOpMode { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/DriveVelocityPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/DriveVelocityPIDTuner.java index dad03d5..4db4c4b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/DriveVelocityPIDTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/DriveVelocityPIDTuner.java @@ -15,6 +15,7 @@ import com.acmerobotics.roadrunner.profile.MotionProfileGenerator; import com.acmerobotics.roadrunner.profile.MotionState; import com.acmerobotics.roadrunner.util.NanoClock; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.util.RobotLog; @@ -49,6 +50,7 @@ import java.util.List; * Pressing B/O (Xbox/PS4) will cede control back to the tuning process. */ @Config +@Disabled @Autonomous(group = "drive") public class DriveVelocityPIDTuner extends LinearOpMode { public static double DISTANCE = 72; // in diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/FollowerPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/FollowerPIDTuner.java index a527570..953730d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/FollowerPIDTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/FollowerPIDTuner.java @@ -3,6 +3,7 @@ package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.opmode; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; @@ -23,6 +24,7 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.Tra * These coefficients can be tuned live in dashboard. */ @Config +@Disabled @Autonomous(group = "drive") public class FollowerPIDTuner extends LinearOpMode { public static double DISTANCE = 48; // in diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/LocalizationTest.java index 0ab260a..00616da 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/LocalizationTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/LocalizationTest.java @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.opmode; import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; @@ -14,6 +15,7 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; * exercise is to ascertain whether the localizer has been configured properly (note: the pure * encoder localizer heading may be significantly off if the track width has not been tuned). */ +@Disabled @TeleOp(group = "drive") public class LocalizationTest extends LinearOpMode { @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/ManualFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/ManualFeedforwardTuner.java index 113ff71..5d499ae 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/ManualFeedforwardTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/ManualFeedforwardTuner.java @@ -17,6 +17,7 @@ import com.acmerobotics.roadrunner.profile.MotionProfileGenerator; import com.acmerobotics.roadrunner.profile.MotionState; import com.acmerobotics.roadrunner.util.NanoClock; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.VoltageSensor; import com.qualcomm.robotcore.util.RobotLog; @@ -42,6 +43,7 @@ import java.util.Objects; * Pressing B/O (Xbox/PS4) will cede control back to the tuning process. */ @Config +@Disabled @Autonomous(group = "drive") public class ManualFeedforwardTuner extends LinearOpMode { public static double DISTANCE = 72; // in diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/MaxAngularVeloTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/MaxAngularVeloTuner.java index 40ed050..25ee631 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/MaxAngularVeloTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/MaxAngularVeloTuner.java @@ -5,6 +5,7 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.util.ElapsedTime; @@ -23,6 +24,7 @@ import java.util.Objects; */ @Config +@Disabled @Autonomous(group = "drive") public class MaxAngularVeloTuner extends LinearOpMode { public static double RUNTIME = 4.0; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/MaxVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/MaxVelocityTuner.java index 99d13b5..f56ad80 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/MaxVelocityTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/MaxVelocityTuner.java @@ -5,6 +5,7 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.VoltageSensor; @@ -25,6 +26,7 @@ import java.util.Objects; * Further fine tuning of kF may be desired. */ @Config +@Disabled @Autonomous(group = "drive") public class MaxVelocityTuner extends LinearOpMode { public static double RUNTIME = 2.0; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/MotorDirectionDebugger.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/MotorDirectionDebugger.java index 9a4d9dc..ab5570d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/MotorDirectionDebugger.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/MotorDirectionDebugger.java @@ -38,6 +38,7 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; * * Uncomment the @Disabled tag below to use this opmode. */ + @Disabled @Config @TeleOp(group = "drive") diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/SplineTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/SplineTest.java index 945374a..4a1e6ff 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/SplineTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/SplineTest.java @@ -4,6 +4,7 @@ import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Vector2d; import com.acmerobotics.roadrunner.trajectory.Trajectory; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; @@ -11,6 +12,7 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; /* * This is an example of a more complex path to really test the tuning. */ +@Disabled @Autonomous(group = "drive") public class SplineTest extends LinearOpMode { @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/StrafeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/StrafeTest.java index 35e0550..5b3107a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/StrafeTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/StrafeTest.java @@ -6,6 +6,7 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.trajectory.Trajectory; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; @@ -15,6 +16,7 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; * This is a simple routine to test translational drive capabilities. */ @Config +@Disabled @Autonomous(group = "drive") public class StrafeTest extends LinearOpMode { public static double DISTANCE = 60; // in diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/StraightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/StraightTest.java index b02da08..d987d5e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/StraightTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/StraightTest.java @@ -6,6 +6,7 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.trajectory.Trajectory; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; @@ -15,6 +16,7 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; * This is a simple routine to test translational drive capabilities. */ @Config +@Disabled @Autonomous(group = "drive") public class StraightTest extends LinearOpMode { public static double DISTANCE = 60; // in diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/TrackWidthTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/TrackWidthTuner.java index 639377f..be46964 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/TrackWidthTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/TrackWidthTuner.java @@ -6,6 +6,7 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.util.Angle; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.util.MovingStatistics; @@ -24,6 +25,7 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; * accurate track width estimate is important or else the angular constraints will be thrown off. */ @Config +@Disabled @Autonomous(group = "drive") public class TrackWidthTuner extends LinearOpMode { public static double ANGLE = 180; // deg diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/TrackingWheelForwardOffsetTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/TrackingWheelForwardOffsetTuner.java index dd9590b..894b8e0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/TrackingWheelForwardOffsetTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/TrackingWheelForwardOffsetTuner.java @@ -6,6 +6,7 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.util.Angle; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.util.MovingStatistics; import com.qualcomm.robotcore.util.RobotLog; @@ -35,6 +36,7 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.StandardTracking * satisfactory result is produced. */ @Config +@Disabled @Autonomous(group="drive") public class TrackingWheelForwardOffsetTuner extends LinearOpMode { public static double ANGLE = 180; // deg diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/TrackingWheelLateralDistanceTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/TrackingWheelLateralDistanceTuner.java index 5d10cac..25aaa26 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/TrackingWheelLateralDistanceTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/TrackingWheelLateralDistanceTuner.java @@ -3,6 +3,7 @@ package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.opmode; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.util.Angle; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.util.RobotLog; @@ -62,6 +63,7 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.StandardTracking * precision. The heading should still line up. */ @Config +@Disabled @TeleOp(group = "drive") public class TrackingWheelLateralDistanceTuner extends LinearOpMode { public static int NUM_TURNS = 10; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/TurnTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/TurnTest.java index 396687c..56ff661 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/TurnTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/opmode/TurnTest.java @@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.opmode; import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; @@ -10,6 +11,7 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; * This is a simple routine to test turning capabilities. */ @Config +@Disabled @Autonomous(group = "drive") public class TurnTest extends LinearOpMode { public static double ANGLE = 90; // deg diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/Auto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/Auto.java index 231f99c..a4a6c80 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/Auto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/Auto.java @@ -42,12 +42,12 @@ public class Auto extends LinearOpMode { //Preload Two this.preloadTwo = this.robot.getDrive().trajectoryBuilder(initialPosition) - .lineToLinearHeading(new Pose2d(40,-25, Math.toRadians(270))) + .lineToLinearHeading(new Pose2d(35,-27, Math.toRadians(270))) .build(); //Preload Three this.preloadThree = this.robot.getDrive().trajectoryBuilder(initialPosition) - .lineToLinearHeading(new Pose2d(47.5,-37.5, Math.toRadians(270))) + .lineToLinearHeading(new Pose2d(46,-35, Math.toRadians(270))) .build(); // Do super fancy chinese shit