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 74fe7fc8..414c74b3 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 @@ -39,7 +39,7 @@ public static double getMotorVelocityF(double ticksPerSecond) { public static double GEAR_RATIO = 1; public static double WHEEL_RADIUS = 1.8898; // public static double MAX_VEL = ((MAX_RPM / 60) * GEAR_RATIO * WHEEL_RADIUS * 2 * Math.PI) * 0.85; - public static double MAX_VEL = 30; + public static double MAX_VEL = 50; public static double MAX_ACCEL = MAX_VEL; // public static double MAX_ANG_ACCEL = Math.toRadians(80.66924999999999); @@ -151,12 +151,12 @@ public AprilTagCamera getPrimaryCamera() { public void dropPurplePixel(boolean state) { if (state) { //purpleServo.setPosition(1); - for (double i = .7; i > 0; i -= 0.1) { + for (double i = 0; i < .7; i += 0.1) { purpleServo.setPosition(i); //GlobalOpMode.opMode.sleep(10); } } else { - purpleServo.setPosition(.7); + purpleServo.setPosition(0); } } 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 293f25f0..6225864f 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,11 +78,19 @@ public void runOpMode() throws InterruptedException { if (robot.getClass() == PsiBot.class) { - if (gamepad1.left_bumper || gamepad2.left_bumper) { + if (gamepad1.left_bumper) { purplePose = purplePose + 0.1; - } else if (gamepad1.right_bumper|| gamepad2.right_bumper) { + } else if (gamepad1.right_bumper) { purplePose = purplePose - 0.1; } + if (gamepad2.left_bumper) { + wristPose += 0.005; + } else if (gamepad2.right_bumper) { + wristPose -= 0.005; + } + if (wristPose < 0) wristPose = 0; + if (wristPose > 1) wristPose = 1; + ((PsiBot) robot).wristServo.setPosition(wristPose); if (purplePose > 1) purplePose = 1; if (purplePose < 0) purplePose = 0; ((PsiBot) robot).planeServo.setPosition(purplePose); @@ -131,22 +139,22 @@ public void runOpMode() throws InterruptedException { - if (gamepad1.left_bumper) { - wristPose += 0.05; - } else if (gamepad1.right_bumper) { - wristPose -= 0.05; - } - if (wristPose < 0) wristPose = 0; - if (wristPose > 1) wristPose = 1; + if (gamepad1.left_bumper) { + wristPose += 0.05; + } else if (gamepad1.right_bumper) { + wristPose -= 0.05; + } + if (wristPose < 0) wristPose = 0; + if (wristPose > 1) wristPose = 1; + + + //rrobot.planeAngleServo.setPosition(wristPose); //rrobot..setPosition(wristPose); //rrobot.wristServo.setPosition(wristPose); - if (robot.getClass() == PsiBot.class) { - - } double p; if (gamepad1.dpad_left) { 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 931623bc..e72cc217 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,20 @@ public void drive() { .turn(Math.toRadians(-8)); if (propPos == TensorFlowDetection.PropPosition.LEFT) { - trajB = trajB.lineTo(new Vector2d(56, -28.25)); + trajB = trajB.lineTo(new Vector2d(52, -28.25)); } else if (propPos == TensorFlowDetection.PropPosition.RIGHT) { - trajB = trajB.lineTo(new Vector2d(56, -42.5)); + trajB = trajB.lineTo(new Vector2d(52, -42.5)); } else { - trajB = trajB.lineTo(new Vector2d(56, -35)); + trajB = trajB.lineTo(new Vector2d(52, -30)); } trajB = trajB.addTemporalMarker(() -> { android.util.Log.i("PLACE PIXEL", "Placed pixel at Red Board"); }) .turnTo(0); + if (getRobot().getClass() == PsiBot.class) { + trajB.turn(Math.toRadians(-8)); + } } else if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_BOARD) { trajB = trajB.lineTo(new Vector2d(startPose.getX(), -12)) @@ -56,11 +59,11 @@ public void drive() { .turn(Math.toRadians(-8)); if (propPos == TensorFlowDetection.PropPosition.LEFT) { - trajB = trajB.lineTo(new Vector2d(56, -28.25)); + trajB = trajB.lineTo(new Vector2d(52, -28.25)); } else if (propPos == TensorFlowDetection.PropPosition.RIGHT) { - trajB = trajB.lineTo(new Vector2d(56, -42.5)); + trajB = trajB.lineTo(new Vector2d(52, -42.5)); } else { - trajB = trajB.lineTo(new Vector2d(56, -35)); + trajB = trajB.lineTo(new Vector2d(52, -30)); } trajB = trajB @@ -69,6 +72,9 @@ public void drive() { android.util.Log.i("PLACE PIXEL", "Placed pixel at Red Board"); }) .turnTo(0); + if (getRobot().getClass() == PsiBot.class) { + trajB.turn(Math.toRadians(-8)); + } } else if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_BOARD) { trajB = trajB.lineTo(new Vector2d(startPose.getX(), 12)) @@ -76,11 +82,11 @@ public void drive() { .turn(Math.toRadians(-8)); if (propPos == TensorFlowDetection.PropPosition.LEFT) { - trajB = trajB.lineTo(new Vector2d(56, 33)); + trajB = trajB.lineTo(new Vector2d(52, 33)); } else if (propPos == TensorFlowDetection.PropPosition.RIGHT) { - trajB = trajB.lineTo(new Vector2d(56, 22)); + trajB = trajB.lineTo(new Vector2d(52, 22)); } else { - trajB = trajB.lineTo(new Vector2d(56, 28.5)); + trajB = trajB.lineTo(new Vector2d(52, 30)); } trajB = trajB @@ -91,6 +97,9 @@ public void drive() { android.util.Log.i("PLACE PIXEL", "Placed pixel at Blue Board"); }) .turnTo(0); + if (getRobot().getClass() == PsiBot.class) { + trajB.turn(Math.toRadians(-8)); + } } else { @@ -99,11 +108,11 @@ public void drive() { .turn(Math.toRadians(-8)); if (propPos == TensorFlowDetection.PropPosition.LEFT) { - trajB = trajB.lineTo(new Vector2d(56, 28.25)); + trajB = trajB.lineTo(new Vector2d(52, 28.25)); } else if (propPos == TensorFlowDetection.PropPosition.RIGHT) { - trajB = trajB.lineTo(new Vector2d(56, 42.5)); + trajB = trajB.lineTo(new Vector2d(52, 42.5)); } else { - trajB = trajB.lineTo(new Vector2d(56, 38.5)); + trajB = trajB.lineTo(new Vector2d(52, 30)); } trajB = trajB @@ -111,6 +120,9 @@ public void drive() { android.util.Log.i("PLACE PIXEL", "Placed pixel at Blue Board"); }) .turnTo(Math.toRadians(0)); + if (getRobot().getClass() == PsiBot.class) { + trajB.turn(Math.toRadians(-8)); + } } @@ -127,7 +139,7 @@ public void drive() { while ((((PsiBot) getRobot()).armMotor.getCurrentPosition() >= -1000 || ((PsiBot) getRobot()).armMotor.getCurrentPosition() <= -1100)) if (getRobot().getClass() == PsiBot.class) { - ((PsiBot) getRobot()).armMotor.setTargetPosition(-1052); + ((PsiBot) getRobot()).armMotor.setTargetPosition(-1056); ((PsiBot) getRobot()).armMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); ((PsiBot) getRobot()).armMotor.setPower(.1); 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 bbe8ff7d..b1130e87 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 @@ -53,8 +53,8 @@ public void drive() { //trajB = trajB.turn(Math.toRadians(180)); driveBackwards = -1; } else { - PURPLE_X_OFFSET=7.5; - PURPLE_Y_OFFSET=11.5; + PURPLE_X_OFFSET=0; + PURPLE_Y_OFFSET=0; } if (propPosition == TensorFlowDetection.PropPosition.LEFT) { @@ -66,21 +66,20 @@ public void drive() { } else if (propPosition == TensorFlowDetection.PropPosition.RIGHT) { trajB.turn(Math.toRadians(-90)) .forward(12 * driveBackwards); - trajB.waitSeconds(1) + trajB .addTemporalMarker(() -> getRobot().dropPurplePixel(true)) .waitSeconds(2); } else { if (moveTowardsCenter) { - trajB.turn(Math.toRadians(180)) + + trajB.forward(5) + .turn(Math.toRadians(180)) .forward(-20 * driveBackwards) .forward(8 * driveBackwards + PURPLE_Y_OFFSET); - trajB.waitSeconds(1) + trajB .addTemporalMarker(() -> getRobot().dropPurplePixel(true)) - .waitSeconds(2); - if (getRobot().getClass() == PsiBot.class) { - trajB.forward(4 * driveBackwards) - .strafeLeft(16); - } + .waitSeconds(1); + } else { trajB.forward(10 * driveBackwards); }