From 202e6702d2ef7d93d5ec2a75df6311951d5a59af Mon Sep 17 00:00:00 2001 From: JJTech0130 Date: Tue, 20 Feb 2024 15:31:58 -0500 Subject: [PATCH 1/2] competition changes --- .../ftc/teamcode/drive/RoboticaBot.java | 2 +- .../ftc/teamcode/opmode/TestTeleOp.java | 17 +++++++++-------- .../opmode/components/PurplePixelComponent.java | 3 +++ 3 files changed, 13 insertions(+), 9 deletions(-) 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 2163e42b..3b05e9ae 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 @@ -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 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 8f90e324..fd7aca43 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 @@ -148,6 +148,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 { @@ -187,15 +188,15 @@ public void runOpMode() throws InterruptedException { public static int ARM_OFFSET = 94; - public static int RAISED_ARM = 756 + ARM_OFFSET; - public static double RAISED_WRIST = 0.53; + public static int RAISED_ARM = 403; + public static double RAISED_WRIST = 0.28; - public static int NEUTRAL_ARM = 521 + ARM_OFFSET; - public static double NEUTRAL_WRIST = 0.58; + public static int NEUTRAL_ARM = 31; + public static double NEUTRAL_WRIST = 0.16; - public static int PREP_ARM = 262 + ARM_OFFSET; - public static double PREP_WRIST = 0.67; + public static int PREP_ARM = -208; + public static double PREP_WRIST = 0.38; - public static int PICKUP_ARM = 204 + ARM_OFFSET; - public static double PICKUP_WRIST = 0.69; + public static int PICKUP_ARM = -208; + public static double PICKUP_WRIST = 0.48; } 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..3b99612e 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 @@ -9,6 +9,7 @@ import org.firstinspires.ftc.teamcode.drive.TrajectoryDrive; 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 PurplePixelComponent extends Component { @@ -95,5 +96,7 @@ public void drive() { TrajectorySequence traj = trajB.build(); getRobot().getDrive().followTrajectorySequence(traj); getRobot().dropPurplePixel(false); + + GlobalOpMode.opMode.sleep(1000); } } From 6cdfe8513a06aae873b4fe454990cf0bbc13eb0a Mon Sep 17 00:00:00 2001 From: JJTech0130 Date: Mon, 26 Feb 2024 15:09:52 -0500 Subject: [PATCH 2/2] stuff --- .../ftc/teamcode/drive/RoboticaBot.java | 4 +- .../ftc/teamcode/opmode/TestAuto.java | 2 +- .../teamcode/opmode/components/GoToBoard.java | 60 ++++++++++++++++++- .../components/PurplePixelComponent.java | 8 +-- 4 files changed, 65 insertions(+), 9 deletions(-) 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 3b05e9ae..79dd449d 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); @@ -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/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 3b99612e..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 @@ -9,7 +9,6 @@ import org.firstinspires.ftc.teamcode.drive.TrajectoryDrive; 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 PurplePixelComponent extends Component { @@ -78,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) { @@ -97,6 +97,6 @@ public void drive() { getRobot().getDrive().followTrajectorySequence(traj); getRobot().dropPurplePixel(false); - GlobalOpMode.opMode.sleep(1000); + //GlobalOpMode.opMode.sleep(1000); } }