qualifier FIRST-Tech-Challenge#3 tweaks

MeepMeepTesting/src/main/java/com/example/meepmeeptesting/Trajectories.java

@@ -147,10 +147,10 @@ public enum Trajectories implements Function<DriveShim, TrajectorySequence> {
             .forward(1)
             .build()),
     REMOTE(drive -> drive.trajectorySequenceBuilder(pose(-35, -62, 90))
-            .lineTo(pos(calculatePoint(-35, -62, -7, -39, false, -58), -58))
-            .lineToSplineHeading(pose(-7, -39, 270))
+            .lineTo(pos(calculatePoint(-35, -62, -7, -40, false, -58), -58))
+            .lineToSplineHeading(pose(-7, -40, 270))
             .lineToLinearHeading(pose(-61,-51, 245))
-            .lineToSplineHeading(pose(-20, -53, 0))
+            .lineToSplineHeading(pose(-55, -48, 0))
             .splineToConstantHeading(pos(-15, -64), rad(270))
 //            .strafeRight(5)
             .forward(60)

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmHandler.java

@@ -18,7 +18,7 @@ public ArmHandler(HardwareMap map, Gamepad controller) {
     }
 
     public void onStart() {
-        vertical.max();
+        vertical.setAndUpdate(.8);
         startMillis = System.currentTimeMillis();
 //        horizontal.setAndUpdate(0.4);
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BucketHandler.java

@@ -48,9 +48,9 @@ public void halfway() {
     public void wiggleUntil(Supplier<Boolean> test) {
          for (int i = 0; i < 15; i++) {
              halfway();
-             waitFor(30);
+             waitFor(25);
              forwards();
-             waitFor(30);
+             waitFor(25);
              if (test.get()) return;
          }
      }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Cycle.java

@@ -58,7 +58,7 @@ public void start() {
             stage = Stage.IN_START;
             holdValues(true);
             double distanceCm = distanceSensor.getDistance(DistanceUnit.CM);
-            boolean preFilled = distanceCm < 5;
+            boolean preFilled = distanceCm < 7;
             if (!preFilled) {
                 sweeper.forwards(1);
 
@@ -72,7 +72,7 @@ public void start() {
                     distanceCm = distanceSensor.getDistance(DistanceUnit.CM);
                     runtime = System.currentTimeMillis() - startTime;
 
-                    if (distanceCm < 5) { // if item in bucket
+                    if (distanceCm < 7) { // if item in bucket
 
                         // keep track of how long an item is in the bucket to prevent
                         // stuff bouncing out but still triggering loop exit
@@ -89,7 +89,7 @@ public void start() {
                 }
             }
 
-            boolean objectPickedUp = distanceCm < 5;
+            boolean objectPickedUp = distanceCm < 7;
             if (objectPickedUp) {
                 bucket.halfway();
                 waitFor(300); // give bucket time to rotate
@@ -131,7 +131,7 @@ public void finish() {
             bucket.backwards();
 
             lift.pursueTarget(Position.LOW);
-            waitFor(targetPosition.pos * 10L);
+            waitFor(targetPosition.pos * 5L);
 
             holdValues(false);
             stage = Stage.COMPLETE;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CycleHandler.java

@@ -23,8 +23,9 @@ public class CycleHandler {
     private final LightHandler light;
     private final Gamepad controller;
     private final DistanceSensor distanceSensor;
-    private Position targetPosition = Position.LOW;
+    private Position targetPosition = Position.HIGH;
     private Cycle currentCycle = null;
+    private String cycleData = "";
 
     public CycleHandler(SweeperHandler sweeper, BucketHandler bucket, LiftHandler lift,
                         Gamepad controller, DistanceSensor distanceSensor,
@@ -60,13 +61,16 @@ private void handleTelemetry() {
         }
         telemetry.addData("Target", targetPosition);
         telemetry.addData("Current detected distance (cm)", distanceSensor.getDistance(DistanceUnit.CM));
+        if (!cycleData.isEmpty()) {
+            telemetry.addData("Cycle data", cycleData);
+        }
     }
 
     private void handleCurrentCycleFinishStage() {
         boolean failed = !currentCycle.errorMessage.isEmpty();
         if (failed) {
             controller.rumble(300);
-            telemetry.addData("Cycle error", currentCycle.errorMessage);
+            cycleData = "Cycle error: " + currentCycle.errorMessage;
             light.setColor(LightHandler.Color.RED);
         }
 
@@ -75,7 +79,9 @@ private void handleCurrentCycleFinishStage() {
             light.setColor(LightHandler.Color.OFF);
         } else if (!failed) {
             light.setColor(LightHandler.Color.YELLOW);
+            cycleData = "";
         }
+        if (cycleData.isEmpty()) cycleData += " | last result success: " + !failed;
         telemetry.addData("Last cycle result successful", !failed);
     }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DuckHandler.java

@@ -18,7 +18,7 @@ public class DuckHandler {
     private static final double MOTOR_TICKS_PER_REV = 28;
     private static final double MOTOR_GEAR_RATIO = 16;
     public static double MILLIS_FOR_PLATE_REV = 825;
-    public static double RPMSPEED = 300;
+    public static double RPMSPEED = 275;
     private double speed = rpmToTicksPerSecond(RPMSPEED) * (reversed ? -1 : 1);
     public boolean fast = false;
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LiftHandler.java

@@ -19,10 +19,7 @@ public class LiftHandler {
     private final Gamepad gamepad;
     public boolean initialized = true;
     public volatile boolean shouldHoldPos = false;
-    public static final int INTAKING = 10;
-    public static final int LOW = 20;
-    public static final int MIDDLE = 190;
-    public static final int HIGH = 310;
+    private Position lastPos = null;
 
     public LiftHandler(HardwareMap map, Gamepad gamepad, Telemetry telemetry) {
         this.telemetry = telemetry;
@@ -38,6 +35,8 @@ public void reset() {
     }
 
     public void tick() {
+        if (lastPos != null) telemetry.addData("last lift pos", lastPos);
+        telemetry.addData("should hold", shouldHoldPos);
         if (gamepad != null && !shouldHoldPos) {
             motor.setAndUpdate(gamepad.left_stick_y);
             if (gamepad.left_stick_button && gamepad.right_stick_button &&
@@ -60,16 +59,19 @@ public void tick() {
                 if (target != null) pursueTarget(target);
             }
         }
+        if (lastPos != null) {
+            pursueTarget(lastPos);
+        }
     }
 
-    public void pursueTarget(int pos) {
+    private void pursueTarget(int pos) {
         // goToPosition will make RunMode RUN_TO_POSITION
         motor.goToPosition(pos, 1);
-        motor.update();
     }
 
     public void pursueTarget(Position position) {
         pursueTarget(position.pos);
+        lastPos = position;
     }
 
     public enum Position {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MotorWrapper.java

@@ -40,9 +40,13 @@ public static MotorWrapper get(String name, HardwareMap map) {
 
     public void goToPosition(int pos, double power) {
         power = clampPower(power);
+        setAndUpdate(power);
         motor.setTargetPosition(pos);
-        setPower(power);
         motor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        // conflicting javadocs - just in case
+        setAndUpdate(power);
+        motor.setTargetPosition(pos);
+        motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
     }
 
     public static double clampPower(double original) {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/ControlledOpMode.java

@@ -117,7 +117,6 @@ public void init() {
         intakeServo = new IntakeServoHandler(hardwareMap);
         intakeServo.hook();
         telemetry.addData("Status", "Initialized");
-        telemetry.setAutoClear(false);
     }
 
     /**
@@ -150,7 +149,6 @@ public void loop() {
         arm.tick();
         cycles.tick();
         telemetry.addData("Status", "Run Time: " + runtime);
-        telemetry.update();
     }
 
     /**

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/autonomous/solo/Remote.java

@@ -61,7 +61,7 @@
 public class Remote extends LinearOpMode {
     // Declare OpMode members.
     private float xCenter;
-    private int target = LiftHandler.HIGH;
+    private LiftHandler.Position target = LiftHandler.Position.HIGH;
     private CameraHandler camera;
     private SampleMecanumDrive drive;
     private DuckHandler duck;
@@ -81,6 +81,7 @@ private enum State {
         DELIVER_DUCKS, // Deliver ducks + lower lift
         ALIGN, // Align the robot with the wall
         TO_WAREHOUSE_INITIAL, // Go to warehouse; TODO: starts intake cycle
+        PARK, //Strafes left a little
         IDLE            // Our bot will enter the IDLE state when done
     }
 
@@ -113,23 +114,28 @@ public void runOpMode() throws InterruptedException {
         drive.setPoseEstimate(startPose);
 
         Trajectory toHubInitial = drive.trajectoryBuilder(startPose)
-                .lineTo(pos(calculatePoint(-35, -62, -7, -39, false, -58), -58))
-                .lineToSplineHeading(pose(-7, -39, 270))
+                .lineTo(pos(calculatePoint(-35, -62, -7, -40, false, -58), -58))
+                .lineToSplineHeading(pose(-7, -40, 270))
                 .build();
 
         Trajectory toDuckSpinner = drive.trajectoryBuilder(toHubInitial.end())
                 .lineToLinearHeading(pose(-61,-51, 245))
                 .build();
 
         Trajectory align = drive.trajectoryBuilder(toDuckSpinner.end())
-                .lineToSplineHeading(pose(-20, -53, 0))
+                .lineToSplineHeading(pose(-55, -48, 0))
                 .splineToConstantHeading(pos(-15, -64), rad(270))
+                .strafeRight(7)
                 .build();
 
         Trajectory toWarehouseInitial = drive.trajectoryBuilder(pose(align.end().getX(), -65.25, 0))
                 .forward(60)
                 .build();
 
+        Trajectory park = drive.trajectoryBuilder(toWarehouseInitial.end())
+                .strafeLeft(15)
+                .build();
+
         while (!opModeIsActive() && !isStopRequested()) {
             camera.tick();
             // Get x-coordinate of center of box
@@ -150,53 +156,25 @@ public void runOpMode() throws InterruptedException {
         light
                 .pause()
                 //R
-                .dot()
-                .dash()
-                .dot()
-                .pause()
+                .dot().dash().dot().pause()
                 //O
-                .dash()
-                .dash()
-                .dash()
-                .pause()
+                .dash().dash().dash().pause()
                 //B
-                .dash()
-                .dot()
-                .dot()
-                .dot()
-                .pause()
+                .dash().dot().dot().dot().pause()
                 //O
-                .dash()
-                .dash()
-                .dash()
-                .pause()
+                .dash().dash().dash().pause()
                 //P
-                .dot()
-                .dash()
-                .dash()
-                .dot()
-                .pause()
+                .dot().dash().dash().dot().pause()
                 //A
-                .dot()
-                .dash()
-                .pause()
+                .dot().dash().pause()
                 //N
-                .dash()
-                .dot()
-                .pause()
+                .dash().dot().pause()
                 //D
-                .dash()
-                .dot()
-                .dot()
-                .pause()
+                .dash().dot().dot().pause()
                 //A
-                .dot()
-                .dash()
-                .pause()
+                .dot().dash().pause()
                 //S
-                .dot()
-                .dot()
-                .dot();
+                .dot().dot().dot();
 
         // Set the current state to TO_HUB_INITIAL, our first step
         // Then have it follow that trajectory
@@ -236,8 +214,13 @@ public void runOpMode() throws InterruptedException {
                         // Drop item
                         bucket.forwards();
                         double startTime = getRuntime();
-                        while (getRuntime() - startTime < 0.350); // Wait 350 ms
-                        bucket.wiggleUntil(() -> getRuntime() - startTime < 1); // wiggle for 1 sec at most
+                        while (getRuntime() - startTime < 0.350) { // Wait 350 ms
+                            light.run();
+                        }
+                        bucket.wiggleUntil(() -> {
+                            light.run();
+                            return getRuntime() - startTime > 1; // wiggle for 1 sec at most
+                        });
                         // Retract bucket
                         bucket.backwards();
                     }
@@ -255,19 +238,21 @@ public void runOpMode() throws InterruptedException {
                         currentState = State.DELIVER_DUCKS;
 
                         // Lower lift
-                        lift.pursueTarget(LiftHandler.INTAKING);
+                        lift.pursueTarget(LiftHandler.Position.LOW);
                         // Run duck spinner for 2.5 seconds
                         double startTime = getRuntime();
                         while (getRuntime() - startTime < 1.5) {
                             duck.tick();
                             duck.start(); // red does not need reversing
+                            light.run();
                         }
                         // Stop duck motor
                         duck.stop();
                         duck.tick();
                     }
                     break;
                 case DELIVER_DUCKS:
+
                     if (!drive.isBusy()) {
                         currentState = State.ALIGN;
 
@@ -286,6 +271,13 @@ public void runOpMode() throws InterruptedException {
                     }
                     break;
                 case TO_WAREHOUSE_INITIAL:
+                    if (!drive.isBusy()) {
+                        currentState = State.PARK;
+
+                        drive.followTrajectoryAsync(park);
+                    }
+                    break;
+                case PARK:
                     if (!drive.isBusy()) {
                         currentState = State.IDLE;
                     }
@@ -314,11 +306,11 @@ private void determineTarget() {
         if (camera.mostConfident != null) {
             if (xCenter < 300) {
                 //Set the target to low
-                target = LiftHandler.LOW;
+                target = LiftHandler.Position.LOW;
             }
             else if (xCenter < 600) {
                 //Set the target to middle
-                target = LiftHandler.MIDDLE;
+                target = LiftHandler.Position.MIDDLE;
             }
         }
     }