diff --git a/hardware/ScoringFSM.java b/hardware/ScoringFSM.java index 8847b2b..f985ac5 100644 --- a/hardware/ScoringFSM.java +++ b/hardware/ScoringFSM.java @@ -28,7 +28,7 @@ public class ScoringFSM extends Mechanism { private SlidesMotors slidesMotors = new SlidesMotors(opMode); private Arm arm = new Arm(opMode); private Clamp clamp = new Clamp(opMode); - private ConeSensor coneSensor = new ConeSensor(opMode); +// private ConeSensor coneSensor = new ConeSensor(opMode); private ElapsedTime time; private boolean clampOverride; @@ -42,7 +42,7 @@ public void init(HardwareMap hwMap) { slidesMotors.init(hwMap); arm.init(hwMap); clamp.init(hwMap); - coneSensor.init(hwMap); +// coneSensor.init(hwMap); time = new ElapsedTime(); clampOverride = false; diff --git a/opmode/auton/AutoConstants.java b/opmode/auton/AutoConstants.java index c049549..ac45cb1 100644 --- a/opmode/auton/AutoConstants.java +++ b/opmode/auton/AutoConstants.java @@ -10,7 +10,7 @@ public class AutoConstants { /** ==== CONSTANTS FOR CONE AUTOS ==== **/ public static final double DELAY_PRELOAD_PICKUP = 4.15; // 4.15 - public static final double DELAY_PICKUP = 2.35; // 2.52 + public static final double DELAY_PICKUP = 2.52; // 2.52 public static final double DELAY_OPEN_CLAMP = 1.25; // 1.35 public static final double DELAY_SCORING = 0.05; // 0.15 @@ -35,14 +35,14 @@ public class AutoConstants { public static final double RR_CENTER_X = 35; public static final Pose2d RR_START_POSE = new Pose2d(RR_CENTER_X, RR_WALL_POS, RR_HEADING); - public static final double RR_CONE_STACK_X = 59.5; // 60 for 5 cone! + public static final double RR_CONE_STACK_X = 59.8; // 60 for 5 cone! public static final double RR_CONE_STACK_Y = -17.2; // -17.2 public static final double RR_PRELOAD_CONE_STACK_Y = -17; public static final double RR_HIGH_GOAL_X = 25.4; - public static final double RR_HIGH_GOAL_Y = -7.08; // -7.5 - public static final double RR_HIGH_GOAL_Y_PRELOAD_OFFSET = 5.3 + RR_HIGH_GOAL_Y; // 5.8 + + public static final double RR_HIGH_GOAL_Y = -7.08; // -7.08 + public static final double RR_HIGH_GOAL_Y_PRELOAD_OFFSET = 5.3 + RR_HIGH_GOAL_Y; // 5.3 + public static final double RR_PRELOAD_X_OFFSET = 1.5; // -0.3 @@ -53,7 +53,7 @@ public class AutoConstants { public static final double RR_CONE_STACK_END_ANGLE = 0; public static final double RR_CONE_STACK_ANGLE_OFFSET = 25; - public static final double RR_PARK_LEFT_X = 11; // 9.3 + public static final double RR_PARK_LEFT_X = 10; // 9.3 // public static final Pose2d RR_PARK_LEFT = new Pose2d(RR_PARK_LEFT_X, RR_CONE_STACK_Y, RR_HEADING); // public static final Pose2d RR_PARK_MIDDLE = new Pose2d(RR_CENTER_X, RR_CONE_STACK_Y, RR_HEADING); diff --git a/opmode/auton/right/FiveConeAuto.java b/opmode/auton/right/FiveConeAuto.java index a41eb66..f271915 100644 --- a/opmode/auton/right/FiveConeAuto.java +++ b/opmode/auton/right/FiveConeAuto.java @@ -40,6 +40,13 @@ public class FiveConeAuto extends LinearOpMode { private boolean canContinue = false; private boolean canSlidesExtend = false; + private static double RR_CONE_STACK_X = 60.5; + private static double RR_CONE_STACK_Y = -17.2; + public static double RR_PRELOAD_CONE_STACK_Y = -17; + public static Vector2d RR_PRELOAD_CONE_STACK_VECTOR = new Vector2d(RR_CONE_STACK_X, RR_PRELOAD_CONE_STACK_Y); + + public static Vector2d RR_CONE_STACK_VECTOR = new Vector2d(RR_CONE_STACK_X, RR_CONE_STACK_Y); + public Runnable scoreReady = () -> { try { // extend slides lvl 3 @@ -130,7 +137,7 @@ public void runOpMode() throws InterruptedException { .setReversed(true) .setTangent(Math.toRadians(AutoConstants.RR_CONE_STACK_ANGLE + AutoConstants.RR_CONE_STACK_ANGLE_OFFSET)) - .splineToConstantHeading(AutoConstants.RR_PRELOAD_CONE_STACK_VECTOR, Math.toRadians(AutoConstants.RR_CONE_STACK_END_ANGLE)) + .splineToConstantHeading(RR_PRELOAD_CONE_STACK_VECTOR, Math.toRadians(AutoConstants.RR_CONE_STACK_END_ANGLE)) .build(); coneStackToHighGoal = drive.trajectorySequenceBuilder(preloadToConeStack.end()) @@ -144,7 +151,7 @@ public void runOpMode() throws InterruptedException { .setConstraints(VELO, ACCEL) .setReversed(true) .setTangent(Math.toRadians(AutoConstants.RR_CONE_STACK_ANGLE)) - .splineTo(AutoConstants.RR_CONE_STACK_VECTOR, Math.toRadians(AutoConstants.RR_CONE_STACK_END_ANGLE)) + .splineTo(RR_CONE_STACK_VECTOR, Math.toRadians(AutoConstants.RR_CONE_STACK_END_ANGLE)) .build(); TrajectorySequence toLeftPark = drive.trajectorySequenceBuilder(highGoalToConeStack.end()) @@ -179,6 +186,7 @@ public void runOpMode() throws InterruptedException { switch (trajectoryState) { case PRELOAD: if (!drive.isBusy()) { + slides.extendToPosition(slides.getPosition() - 3); clamp.open(); runThread(scoreThread); if (canContinue) {