superquals

Fission310 · Feb 13, 2023 · 1559fc2 · 1559fc2
1 parent 7890b5e
commit 1559fc2
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 9 deletions.
diff --git 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
@@ -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
@@ -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) {