diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/PsiBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/PsiBot.java index 18cd9d8b..013ceb85 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/PsiBot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/PsiBot.java @@ -168,11 +168,11 @@ public void launchPlane() { //pull the pixel pusher servo to take out the pin //wait a bit here //turn the drone servo forward - planeServo.setPosition(.09); + planeServo.setPosition(.5); GlobalOpMode.opMode.sleep(10); - mosaicServo.setPosition(1); - GlobalOpMode.opMode.sleep(10); - planeServo.setPosition(1); + mosaicServo.setPosition(0); + GlobalOpMode.opMode.sleep(1000); + planeServo.setPosition(0); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestAuto.java index 956568bb..d766aaf8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestAuto.java @@ -45,7 +45,7 @@ private TensorFlowDetection.PropPosition detectProp(Robot robot, int timeout) { position = TensorFlowDetection.PropPosition.CENTER; } if (robot.getClass() == PsiBot.class) { - ((PsiBot) robot).armMotor.setTargetPosition(-109); + ((PsiBot) robot).armMotor.setTargetPosition(-200); ((PsiBot) robot).armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); ((PsiBot) robot).armMotor.setPower(.1); } @@ -132,6 +132,13 @@ public void runOpMode() throws InterruptedException { for (Component component : componentList) { component.drive(); } + if (robot.getClass() == PsiBot.class) { + while (((PsiBot) robot).armMotor.getCurrentPosition() > 10 || ((PsiBot) robot).armMotor.getCurrentPosition() < -10) { + ((PsiBot) robot).armMotor.setTargetPosition(-0); + ((PsiBot) robot).armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + ((PsiBot) robot).armMotor.setPower(.1); + } + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestTeleOp.java index d1228367..8fc20dc0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/TestTeleOp.java @@ -78,14 +78,14 @@ public void runOpMode() throws InterruptedException { if (robot.getClass() == PsiBot.class) { - if (gamepad1.left_bumper) { + if (gamepad2.left_bumper) { purplePose = purplePose + 0.1; - } else if (gamepad1.right_bumper) { + } else if (gamepad2.right_bumper) { purplePose = purplePose - 0.1; } - if (gamepad2.left_bumper) { + if (gamepad1.left_bumper) { wristPose += 0.005; - } else if (gamepad2.right_bumper) { + } else if (gamepad1.right_bumper) { wristPose -= 0.005; } if (wristPose < 0) wristPose = 0; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/GoToBoard.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/GoToBoard.java index d8bea1a4..b0da92ae 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/GoToBoard.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/GoToBoard.java @@ -38,17 +38,18 @@ public void drive() { .turn(Math.toRadians(-8)); if (propPos == TensorFlowDetection.PropPosition.LEFT) { - trajB = trajB.lineTo(new Vector2d(52, -33)); + trajB = trajB.lineTo(new Vector2d(48, -37)); } else if (propPos == TensorFlowDetection.PropPosition.RIGHT) { - trajB = trajB.lineTo(new Vector2d(52, -26)); + trajB = trajB.lineTo(new Vector2d(48, -31)); } else { - trajB = trajB.lineTo(new Vector2d(52, -30)); + trajB = trajB.lineTo(new Vector2d(48, -34)); } trajB = trajB.addTemporalMarker(() -> { android.util.Log.i("PLACE PIXEL", "Placed pixel at Red Board"); }) - .turnTo(0); + .turnTo(Math.toRadians(180)) .forward(-.5); + if (getRobot().getClass() == PsiBot.class) { trajB.turn(Math.toRadians(-8)); } @@ -59,11 +60,11 @@ public void drive() { .turn(Math.toRadians(-8)); if (propPos == TensorFlowDetection.PropPosition.LEFT) { - trajB = trajB.lineTo(new Vector2d(52, -33)); + trajB = trajB.lineTo(new Vector2d(50, -40)); } else if (propPos == TensorFlowDetection.PropPosition.RIGHT) { - trajB = trajB.lineTo(new Vector2d(52, -26)); + trajB = trajB.lineTo(new Vector2d(50, -34)); } else { - trajB = trajB.lineTo(new Vector2d(52, -30)); + trajB = trajB.lineTo(new Vector2d(50, -37)); } trajB = trajB @@ -71,7 +72,8 @@ public void drive() { .addTemporalMarker(() -> { android.util.Log.i("PLACE PIXEL", "Placed pixel at Red Board"); }) - .turnTo(0); + .turnTo(Math.toRadians(180)) .forward(-.5); + if (getRobot().getClass() == PsiBot.class) { trajB.turn(Math.toRadians(-8)); } @@ -82,11 +84,11 @@ public void drive() { .turn(Math.toRadians(-8)); if (propPos == TensorFlowDetection.PropPosition.LEFT) { - trajB = trajB.lineTo(new Vector2d(52, 33)); + trajB = trajB.lineTo(new Vector2d(50, 40)); } else if (propPos == TensorFlowDetection.PropPosition.RIGHT) { - trajB = trajB.lineTo(new Vector2d(52, 26)); + trajB = trajB.lineTo(new Vector2d(50, 34)); } else { - trajB = trajB.lineTo(new Vector2d(52, 30)); + trajB = trajB.lineTo(new Vector2d(50, 37)); } trajB = trajB @@ -96,7 +98,9 @@ public void drive() { .addTemporalMarker(() -> { android.util.Log.i("PLACE PIXEL", "Placed pixel at Blue Board"); }) - .turnTo(0); + .turnTo(Math.toRadians(180)) + .forward(-.5); + if (getRobot().getClass() == PsiBot.class) { trajB.turn(Math.toRadians(-8)); } @@ -108,27 +112,28 @@ public void drive() { .turn(Math.toRadians(-8)); if (propPos == TensorFlowDetection.PropPosition.LEFT) { - trajB = trajB.lineTo(new Vector2d(52, 33)); + trajB = trajB.lineTo(new Vector2d(48, 37)); } else if (propPos == TensorFlowDetection.PropPosition.RIGHT) { - trajB = trajB.lineTo(new Vector2d(52, 26)); + trajB = trajB.lineTo(new Vector2d(48, 31)); } else { - trajB = trajB.lineTo(new Vector2d(52, 30)); + trajB = trajB.lineTo(new Vector2d(48, 34)); } trajB = trajB .addTemporalMarker(() -> { android.util.Log.i("PLACE PIXEL", "Placed pixel at Blue Board"); }) - .turnTo(Math.toRadians(0)); + .turnTo(Math.toRadians(180)) + .forward(-.5); if (getRobot().getClass() == PsiBot.class) { trajB.turn(Math.toRadians(-8)); } } - if (getRobot().getClass() == PsiBot.class) { - trajB.turn(Math.toRadians(180)); - } + // if (getRobot().getClass() == PsiBot.class) { + // trajB.turn(Math.toRadians(180)); + // } TrajectorySequence traj = trajB.build(); Log.i("PROGRESS", "1"); @@ -137,11 +142,23 @@ public void drive() { Log.i("PROGRESS", "2"); GlobalOpMode.opMode.telemetry.log().add("2"); while ((((PsiBot) getRobot()).armMotor.getCurrentPosition() >= -1000 || ((PsiBot) getRobot()).armMotor.getCurrentPosition() <= -1100)) + if (getRobot().getClass() == PsiBot.class) { - ((PsiBot) getRobot()).armMotor.setTargetPosition(-1056); - ((PsiBot) getRobot()).armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); - ((PsiBot) getRobot()).armMotor.setPower(.1); + + while (((PsiBot) getRobot()).armMotor.getCurrentPosition() > -1046 || ((PsiBot) getRobot()).armMotor.getCurrentPosition() < -1066) { + ((PsiBot) getRobot()).wristServo.setPosition(.65); + + // ((PsiBot) getRobot()).armMotor.setTargetPosition(-1056); + ((PsiBot) getRobot()).armMotor.setTargetPosition(-1100); + ((PsiBot) getRobot()).armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + ((PsiBot) getRobot()).armMotor.setPower(.1); + } +// GlobalOpMode.opMode.sleep(400); + while (((PsiBot) getRobot()).wristServo.getPosition() > .25 || ((PsiBot) getRobot()).wristServo.getPosition() < .15 ) { + + ((PsiBot) getRobot()).wristServo.setPosition(.2); + } Log.i("PROGRESS", "4"); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/PurplePixelComponent.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/PurplePixelComponent.java index df392d7d..7bce291b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/PurplePixelComponent.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/PurplePixelComponent.java @@ -15,8 +15,8 @@ public class PurplePixelComponent extends Component { Vector2d RED_AUDIENCE_TAPE_MARKS = new Vector2d(fi(-3,0), fi(-3,0)); Vector2d BLUE_AUDIENCE_TAPE_MARKS = new Vector2d(-3 * 12, 2 * 12 + 12); - Vector2d RED_BOARD_TAPE_MARKS = new Vector2d(fi(1,0), fi(-3,0)); - Vector2d BLUE_BOARD_TAPE_MARKS = new Vector2d(12, 2 * 12 + 12); + Vector2d RED_BOARD_TAPE_MARKS = new Vector2d(13.5, (-2*12)-14); + Vector2d BLUE_BOARD_TAPE_MARKS = new Vector2d(13.5, 2 * 12 + 14); private final TensorFlowDetection.PropPosition propPosition; private final boolean moveTowardsCenter; @@ -60,7 +60,7 @@ public void drive() { if (propPosition == TensorFlowDetection.PropPosition.LEFT) { trajB.turn(Math.toRadians(90)) - .forward(8 * driveBackwards) + .forward(10 * driveBackwards) .forward(-3.5); // orig 13 trajB .addTemporalMarker(() -> getRobot().dropPurplePixel(true))