diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/RoboticaBot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/RoboticaBot.java index ab60af12..853fcfdc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/RoboticaBot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/RoboticaBot.java @@ -56,7 +56,7 @@ public static double getMotorVelocityF(double ticksPerSecond) { * Maximum Angular Velocity is calculated as: maximum velocity / trackWidth * (180 / Math.PI) but capped at 360°/s. * You are free to raise this on your own if you would like. It is best determined through experimentation. */ - public static double MAX_VEL = 30; + public static double MAX_VEL = 70; public static double MAX_ACCEL = MAX_VEL; public static double MAX_ANG_ACCEL = Math.toRadians(360); public static double MAX_ANG_VEL = Math.toRadians(360); @@ -222,7 +222,7 @@ public void dropPurplePixel(boolean state) { public static double PLANE_START_ANGLE = 0.6; - public static double PLANE_LAUNCH_ANGLE = 0.80; + public static double PLANE_LAUNCH_ANGLE = 0.74; public static double PLANE_FOR_INCR = 0.001; public static int PLANE_DELAY = 10; @Override @@ -244,7 +244,7 @@ public TrajectoryDrive getDrive() { public void recalibrateShoulder() { // Raise the arm up - shoulderMotor.setTargetPosition(500); + shoulderMotor.setTargetPosition(700); shoulderMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); shoulderMotor.setPower(1); while (shoulderMotor.isBusy() && !GlobalOpMode.opMode.isStopRequested()) {} 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 1e8b7735..bdf7a8fc 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 @@ -113,7 +113,7 @@ public void runOpMode() throws InterruptedException { List componentList = new ArrayList<>(); if (config.placePixel) { - componentList.add(new PurplePixelComponent(robot, tensorPos, Objects.equals(config.park, "inner") || Objects.equals(config.park, "board"))); + componentList.add(new PurplePixelComponent(robot, tensorPos, Objects.equals(config.park, "inner"))); } switch (config.park) { 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 6eada118..fb289562 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 @@ -154,6 +154,7 @@ public void runOpMode() throws InterruptedException { // HANG if (gamepad2.y) { rrobot.hangMotor.setPower(1.0); + rrobot.planeAngleServo.setPosition(0.3); } else if (gamepad2.x) { rrobot.hangMotor.setPower(-1.0); } else { @@ -204,4 +205,4 @@ public void runOpMode() throws InterruptedException { public static int PICKUP_ARM = -116 ; public static double PICKUP_WRIST = 0.472; -} + public s 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 574a8b84..187fa5f1 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 @@ -1,14 +1,20 @@ package org.firstinspires.ftc.teamcode.opmode.components; +import static org.firstinspires.ftc.teamcode.util.Units.fi; + import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; import org.firstinspires.ftc.teamcode.drive.Robot; +import org.firstinspires.ftc.teamcode.drive.RoboticaBot; +import org.firstinspires.ftc.teamcode.drive.TrajectoryDrive; +import org.firstinspires.ftc.teamcode.opmode.TestTeleOp; +import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence; import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceBuilder; +import org.firstinspires.ftc.teamcode.util.GlobalOpMode; import org.firstinspires.ftc.teamcode.vision.TensorFlowDetection; public class GoToBoard extends Component { - - TensorFlowDetection.PropPosition propPos; public GoToBoard(Robot robot, TensorFlowDetection.PropPosition prop) { @@ -21,6 +27,56 @@ public void drive() { Pose2d startPose = getRobot().getDrive().getPoseEstimate(); TrajectorySequenceBuilder trajB = getRobot().getDrive().trajectorySequenceBuilder(startPose); + // If we're on the blue side, then set this to 1 foot, -1 foot on red side + int y = (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_BOARD) || (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_AUDIENCE) ? fi(4,0) : fi(-4,0); + + trajB.lineTo(new Vector2d(startPose.getX(), y)) + .lineTo(new Vector2d(fi(3, 10), y)) + .turnTo(Math.toRadians(0)) + //.turn(Math.toRadians(90)) + .lineTo(new Vector2d(fi(3,8), fi(-2,11))); + + TrajectorySequence traj = trajB.build(); + + getRobot().getDrive().followTrajectorySequence(traj); + + if (getRobot().getClass() == RoboticaBot.class) { + ((RoboticaBot) getRobot()).recalibrateShoulder(); + ((RoboticaBot) getRobot()).setShoulderTargetPosition(TestTeleOp.RAISED_ARM); + } + + //GlobalOpMode.opMode.sleep(2000); + + TrajectorySequenceBuilder trajC = getRobot().getDrive().trajectorySequenceBuilder(getRobot().getDrive().getPoseEstimate()); + trajC.back(5); + getRobot().getDrive().followTrajectorySequence(trajC.build()); + + ((RoboticaBot) getRobot()).elbowServo.innerServo.setPower(-1.0); + GlobalOpMode.opMode.sleep(400); + ((RoboticaBot) getRobot()).elbowServo.innerServo.setPower(0.0); + ((RoboticaBot) getRobot()).wristTwistServo.setPosition(0.5); + ((RoboticaBot) getRobot()).wristLiftServo.setPosition(TestTeleOp.RAISED_WRIST); + + TrajectorySequenceBuilder trajD = getRobot().getDrive().trajectorySequenceBuilder(getRobot().getDrive().getPoseEstimate()); + trajD.forward(8); + getRobot().getDrive().followTrajectorySequence(trajD.build()); + + ((RoboticaBot) getRobot()).pinchServo.innerServo.setPower(1.0); // OPEN + + GlobalOpMode.opMode.sleep(900); + + ((RoboticaBot) getRobot()).pinchServo.innerServo.setPower(0.0); + + TrajectorySequenceBuilder trajE = getRobot().getDrive().trajectorySequenceBuilder(getRobot().getDrive().getPoseEstimate()); + trajE.back(8); + getRobot().getDrive().followTrajectorySequence(trajE.build()); + + + GlobalOpMode.opMode.sleep(30000); + + + + // //getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_BOARD 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 4fb2e50f..7c8971cd 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 @@ -77,9 +77,10 @@ public void drive() { } } - trajB.waitSeconds(1) + //trajB.waitSeconds(0.5) + trajB .addTemporalMarker(() -> getRobot().dropPurplePixel(true)) - .waitSeconds(2); + .waitSeconds(0.5); //if (propPosition == TensorFlowDetection.PropPosition.CENTER && (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_AUDIENCE || getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_AUDIENCE )) { if (propPosition == TensorFlowDetection.PropPosition.LEFT && getRobot().getClass() == RoboticaBot.class && getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_BOARD) { @@ -95,5 +96,7 @@ public void drive() { TrajectorySequence traj = trajB.build(); getRobot().getDrive().followTrajectorySequence(traj); getRobot().dropPurplePixel(false); + + //GlobalOpMode.opMode.sleep(1000); } }