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 8fbd664e..74fe7fc8 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 @@ -2,6 +2,7 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.control.PIDCoefficients; +import com.acmerobotics.roadrunner.geometry.Pose2d; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; @@ -65,9 +66,10 @@ public static double getMotorVelocityF(double ticksPerSecond) { public static double FORWARD_OFFSET = -7.5; private Encoder leftEncoder, rightEncoder, frontEncoder; - public static double X_MULTIPLIER = 96.0/94.8; // Multiplier in the X direction + public static double X_MULTIPLIER = 1; // Multiplier in the X direction // BAD: 92.8 93.3 92.9 - public static double Y_MULTIPLIER = 96.0/89.2; // Multiplier in the Y direction + public static double Y_MULTIPLIER = 1; // Multiplier in the Y direction + // BAD: 87.8 87.2 87.8 public final Servo purpleServo; @@ -79,9 +81,9 @@ public static double getMotorVelocityF(double ticksPerSecond) { public PsiBot(HardwareMap hardwareMap) { super(hardwareMap); cameras = new AprilTagCamera[3]; - cameras[0] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Left"), 5.5, -6.5); + cameras[0] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Left"), 5.5, -5); cameras[1] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Center"), 0, 0); - cameras[2] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Right"), 5.5, 6.5); + cameras[2] = new AprilTagCamera(hardwareMap.get(WebcamName.class, "Right"), 5.5, 5); purpleServo = hardwareMap.get(Servo.class, "purple"); planeServo = hardwareMap.get(Servo.class, "plane"); @@ -125,6 +127,7 @@ public PsiBot(HardwareMap hardwareMap) { DW_GEAR_RATIO, DW_WHEEL_RADIUS, DW_TICKS_PER_REV + ); drive.leftFront.setDirection(DcMotorSimple.Direction.REVERSE); drive.leftRear.setDirection(DcMotorSimple.Direction.REVERSE); @@ -148,12 +151,12 @@ public AprilTagCamera getPrimaryCamera() { public void dropPurplePixel(boolean state) { if (state) { //purpleServo.setPosition(1); - for (double i = 0; i < 0.7; i += 0.1) { + for (double i = .7; i > 0; i -= 0.1) { purpleServo.setPosition(i); //GlobalOpMode.opMode.sleep(10); } } else { - purpleServo.setPosition(0); + purpleServo.setPosition(.7); } } 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 2a4ebbfb..931623bc 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 @@ -34,13 +34,15 @@ public void drive() { if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_AUDIENCE) { trajB = trajB.lineTo(new Vector2d(startPose.getX(), -12)) - .lineTo(new Vector2d(40, -12)); + .lineTo(new Vector2d(40, -12)) + .turn(Math.toRadians(-8)); + if (propPos == TensorFlowDetection.PropPosition.LEFT) { - trajB = trajB.lineTo(new Vector2d(40, -28.25)); + trajB = trajB.lineTo(new Vector2d(56, -28.25)); } else if (propPos == TensorFlowDetection.PropPosition.RIGHT) { - trajB = trajB.lineTo(new Vector2d(40, -42.5)); + trajB = trajB.lineTo(new Vector2d(56, -42.5)); } else { - trajB = trajB.lineTo(new Vector2d(40, -35)); + trajB = trajB.lineTo(new Vector2d(56, -35)); } trajB = trajB.addTemporalMarker(() -> { @@ -50,13 +52,15 @@ public void drive() { } else if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_BOARD) { trajB = trajB.lineTo(new Vector2d(startPose.getX(), -12)) - .lineTo(new Vector2d(40, -12)); + .lineTo(new Vector2d(40, -12)) + .turn(Math.toRadians(-8)); + if (propPos == TensorFlowDetection.PropPosition.LEFT) { - trajB = trajB.lineTo(new Vector2d(40, -28.25)); + trajB = trajB.lineTo(new Vector2d(56, -28.25)); } else if (propPos == TensorFlowDetection.PropPosition.RIGHT) { - trajB = trajB.lineTo(new Vector2d(40, -42.5)); + trajB = trajB.lineTo(new Vector2d(56, -42.5)); } else { - trajB = trajB.lineTo(new Vector2d(40, -35)); + trajB = trajB.lineTo(new Vector2d(56, -35)); } trajB = trajB @@ -68,18 +72,20 @@ public void drive() { } else if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_BOARD) { trajB = trajB.lineTo(new Vector2d(startPose.getX(), 12)) - .lineTo(new Vector2d(40, 12)); + .lineTo(new Vector2d(40, 12)) + .turn(Math.toRadians(-8)); + if (propPos == TensorFlowDetection.PropPosition.LEFT) { - trajB = trajB.lineTo(new Vector2d(40, 28.25)); + trajB = trajB.lineTo(new Vector2d(56, 33)); } else if (propPos == TensorFlowDetection.PropPosition.RIGHT) { - trajB = trajB.lineTo(new Vector2d(40, 42.5)); + trajB = trajB.lineTo(new Vector2d(56, 22)); } else { - trajB = trajB.lineTo(new Vector2d(40, 35)); + trajB = trajB.lineTo(new Vector2d(56, 28.5)); } trajB = trajB - .turn(Math.toRadians(90 - startPose.getHeading())) + //TODO:fix error with temporal marker because ryan is a dum dum .addTemporalMarker(() -> { android.util.Log.i("PLACE PIXEL", "Placed pixel at Blue Board"); @@ -88,21 +94,23 @@ public void drive() { } else { - trajB = trajB.lineTo(new Vector2d(startPose.getX(), 12)) - .lineTo(new Vector2d(40, 12)); + trajB = trajB.lineTo(new Vector2d(startPose.getX(), 8)) + .lineTo(new Vector2d(40, 8)) + .turn(Math.toRadians(-8)); + if (propPos == TensorFlowDetection.PropPosition.LEFT) { - trajB = trajB.lineTo(new Vector2d(40, 28.25)); + trajB = trajB.lineTo(new Vector2d(56, 28.25)); } else if (propPos == TensorFlowDetection.PropPosition.RIGHT) { - trajB = trajB.lineTo(new Vector2d(40, 42.5)); + trajB = trajB.lineTo(new Vector2d(56, 42.5)); } else { - trajB = trajB.lineTo(new Vector2d(40, 38.5)); + trajB = trajB.lineTo(new Vector2d(56, 38.5)); } trajB = trajB .addTemporalMarker(() -> { android.util.Log.i("PLACE PIXEL", "Placed pixel at Blue Board"); }) - .turnTo(0); + .turnTo(Math.toRadians(0)); } 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 1b1ff8da..bbe8ff7d 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 @@ -4,6 +4,7 @@ import com.acmerobotics.roadrunner.geometry.Vector2d; +import org.firstinspires.ftc.teamcode.drive.PsiBot; import org.firstinspires.ftc.teamcode.drive.Robot; import org.firstinspires.ftc.teamcode.drive.RoboticaBot; import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; @@ -41,6 +42,8 @@ private Vector2d currentTapeMarks() { @Override public void drive() { + double PURPLE_X_OFFSET=0; + double PURPLE_Y_OFFSET=0; TrajectorySequenceBuilder trajB = getRobot().getDrive().trajectorySequenceBuilder(getRobot().getDrive().getPoseEstimate()); trajB.lineTo(currentTapeMarks()); @@ -49,26 +52,41 @@ public void drive() { if (getRobot().getClass() == RoboticaBot.class) { //trajB = trajB.turn(Math.toRadians(180)); driveBackwards = -1; + } else { + PURPLE_X_OFFSET=7.5; + PURPLE_Y_OFFSET=11.5; } if (propPosition == TensorFlowDetection.PropPosition.LEFT) { trajB.turn(Math.toRadians(90)) .forward(8 * driveBackwards); // orig 13 + trajB.waitSeconds(1) + .addTemporalMarker(() -> getRobot().dropPurplePixel(true)) + .waitSeconds(2); } else if (propPosition == TensorFlowDetection.PropPosition.RIGHT) { trajB.turn(Math.toRadians(-90)) .forward(12 * driveBackwards); + trajB.waitSeconds(1) + .addTemporalMarker(() -> getRobot().dropPurplePixel(true)) + .waitSeconds(2); } else { if (moveTowardsCenter) { trajB.turn(Math.toRadians(180)) - .forward(-16 * driveBackwards); + .forward(-20 * driveBackwards) + .forward(8 * driveBackwards + PURPLE_Y_OFFSET); + trajB.waitSeconds(1) + .addTemporalMarker(() -> getRobot().dropPurplePixel(true)) + .waitSeconds(2); + if (getRobot().getClass() == PsiBot.class) { + trajB.forward(4 * driveBackwards) + .strafeLeft(16); + } } else { trajB.forward(10 * driveBackwards); } } - trajB.waitSeconds(1) - .addTemporalMarker(() -> getRobot().dropPurplePixel(true)) - .waitSeconds(2); + //if (propPosition == TensorFlowDetection.PropPosition.CENTER && (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_AUDIENCE || getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_AUDIENCE )) { if (moveTowardsCenter && propPosition == TensorFlowDetection.PropPosition.CENTER) {