From e38dad3e8f0d01847a7376b81ab19d94c6e257d8 Mon Sep 17 00:00:00 2001 From: ImposterVarunoverlord Date: Sun, 10 Mar 2024 15:00:45 -0500 Subject: [PATCH 1/9] FOr senor bot --- build.dependencies.gradle | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/build.dependencies.gradle b/build.dependencies.gradle index bc6dc2a..1f1477a 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -5,20 +5,20 @@ repositories { } dependencies { - implementation 'org.firstinspires.ftc:Inspection:9.0.1' - implementation 'org.firstinspires.ftc:Blocks:9.0.1' - implementation 'org.firstinspires.ftc:Tfod:9.0.1' - implementation 'org.firstinspires.ftc:RobotCore:9.0.1' - implementation 'org.firstinspires.ftc:RobotServer:9.0.1' - implementation 'org.firstinspires.ftc:OnBotJava:9.0.1' - implementation 'org.firstinspires.ftc:Hardware:9.0.1' - implementation 'org.firstinspires.ftc:FtcCommon:9.0.1' - implementation 'org.firstinspires.ftc:Vision:9.0.1' + implementation 'org.firstinspires.ftc:Inspection:9.1.0' + implementation 'org.firstinspires.ftc:Blocks:9.1.0' + implementation 'org.firstinspires.ftc:Tfod:9.1.0' + implementation 'org.firstinspires.ftc:RobotCore:9.1.0' + implementation 'org.firstinspires.ftc:RobotServer:9.1.0' + implementation 'org.firstinspires.ftc:OnBotJava:9.1.0' + implementation 'org.firstinspires.ftc:Hardware:9.1.0' + implementation 'org.firstinspires.ftc:FtcCommon:9.1.0' + implementation 'org.firstinspires.ftc:Vision:9.1.0' implementation 'org.firstinspires.ftc:gameAssets-CenterStage:1.0.0' implementation 'org.tensorflow:tensorflow-lite-task-vision:0.4.3' runtimeOnly 'org.tensorflow:tensorflow-lite:2.12.0' implementation 'androidx.appcompat:appcompat:1.2.0' - implementation 'com.acmerobotics.dashboard:dashboard:0.4.12' + implementation 'com.acmerobotics.dashboard:dashboard:0.4.15' implementation project(':ielib-core') implementation project(':ielib') } From f07e298100db9c3b7ee9babf352d70d192858ad9 Mon Sep 17 00:00:00 2001 From: ImposterVarunoverlord Date: Sun, 10 Mar 2024 15:00:54 -0500 Subject: [PATCH 2/9] FOr senor bot --- .../java/org/firstinspires/ftc/teamcode/hardware/Camera.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java index fd4b118..7a84ba3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java @@ -19,7 +19,7 @@ import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; public class Camera { private static final String REAR_WEBCAM_NAME = "rearWebcam"; public static final String FRONT_WEBCAM_NAME = "frontWebcam"; - private static final Pose2d REAR_CAMERA_OFFSETS = new Pose2d(8.9, 1.5, Math.PI); + private static final Pose2d REAR_CAMERA_OFFSETS = new Pose2d(9.75, 0, Math.PI); public static float PROP_REJECTION_VERTICAL_UPPER = 480f * 0.33f; public static float PROP_REJECTION_VERTICAL_LOWER = 440; private PropDetectionPipeline prop; From 28e6aa783679fb9e887267660c84e0df5c7fcbd9 Mon Sep 17 00:00:00 2001 From: ImposterVarunoverlord Date: Tue, 19 Mar 2024 19:58:03 -0500 Subject: [PATCH 3/9] 2+1(blue works not red) --- .../meepmeeptesting/MeepMeepTesting.java | 4 +- .../ftc/teamcode/hardware/Arm.java | 24 ++ .../ftc/teamcode/hardware/Camera.java | 2 +- .../ftc/teamcode/hardware/LEDs.java | 59 +++++ .../ftc/teamcode/hardware/Robot.java | 57 +++- .../ftc/teamcode/hardware/Slides.java | 32 ++- .../opmode/autonomous/BlueBackStageAuto.java | 72 ++--- .../opmode/autonomous/BlueFrontPreload.java | 47 ++-- .../opmode/autonomous/BlueFrontStageAuto.java | 248 ++++++------------ .../opmode/autonomous/RedFrontPreload.java | 55 ++-- .../teamcode/opmode/teleop/BlueTeleop.java | 239 +++++++++++++++++ .../teamcode/opmode/teleop/MainTeleOp.java | 4 +- .../ftc/teamcode/opmode/teleop/RedTeleop.java | 240 +++++++++++++++++ 13 files changed, 824 insertions(+), 259 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/LEDs.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/BlueTeleop.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java diff --git a/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java b/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java index f106437..1c06d82 100644 --- a/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java +++ b/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java @@ -95,8 +95,8 @@ public class MeepMeepTesting { .setBackgroundAlpha(0.95f) //.addEntity(myBot) //.addEntity(BlueFrontStage) - .addEntity(BlueFrontStage3) - //.addEntity(BlueFrontStage1) + //.addEntity(BlueFrontStage3) + .addEntity(BlueFrontStage1) .start(); } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java index ee1c947..09e852e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java @@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.hardware; import com.acmerobotics.dashboard.config.Config; import com.arcrobotics.ftclib.controller.PController; +import com.qualcomm.hardware.rev.RevColorSensorV3; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; @@ -12,6 +13,11 @@ public class Arm { private Servo lAServo; private Servo doorServo; private Servo wristServo; + + private Servo elbow; + + + private Slides.Position pos = Slides.Position.DOWN; private PController armController; private double armControllerTarget; @@ -22,6 +28,8 @@ public class Arm { private double armTempPos; private double doorPos; private double wristPos; + + private double elbowPos; public static double ARM_KP = 0.18; public static double doorOpenPos = 0.36; @@ -34,6 +42,10 @@ public class Arm { public static double wristStart = 0.29; public static double wristScore = 0.5; + public static double elbow_mid = 0.5; + public static double elbow_right = 0.29; + public static double elbow_left = 0.95; + public static double wristScore_highMacro = 0.3; public enum Position { @@ -49,6 +61,7 @@ public class Arm { doorServo = hardwareMap.get(Servo.class, "Door"); lAServo = hardwareMap.get(Servo.class, "LeftArm"); rAServo = hardwareMap.get(Servo.class, "RightArm"); + // elbow = hardwareMap.get(Servo.class, "Elbow"); // lAServo.setDirection(Servo.Direction.REVERSE); rAServo.setDirection(Servo.Direction.REVERSE); doorServo.setDirection(Servo.Direction.REVERSE); @@ -91,6 +104,17 @@ public class Arm { } } + public void setElbowPos (Position pos){ + if (pos == Position.INTAKE) { + wristPos = elbow_mid; + } else if (pos == Position.SCORE) { + wristPos = elbow_right; + } else if (pos == Position.SCOREHI) { + wristPos = elbow_left; + + } + } + public void setDoor(DoorPosition pos) { if (pos == DoorPosition.OPEN) { doorPos = doorOpenPos; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java index 7a84ba3..514d015 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java @@ -19,7 +19,7 @@ import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; public class Camera { private static final String REAR_WEBCAM_NAME = "rearWebcam"; public static final String FRONT_WEBCAM_NAME = "frontWebcam"; - private static final Pose2d REAR_CAMERA_OFFSETS = new Pose2d(9.75, 0, Math.PI); + private static final Pose2d REAR_CAMERA_OFFSETS = new Pose2d(6.5, 0, Math.PI); public static float PROP_REJECTION_VERTICAL_UPPER = 480f * 0.33f; public static float PROP_REJECTION_VERTICAL_LOWER = 440; private PropDetectionPipeline prop; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/LEDs.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/LEDs.java new file mode 100644 index 0000000..ad87491 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/LEDs.java @@ -0,0 +1,59 @@ +package org.firstinspires.ftc.teamcode.hardware; + + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.hardware.rev.RevBlinkinLedDriver; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import java.util.Locale; + +@Config +public class LEDs { + public static int NUMBER = 1;//80 solid + public static int RED = 80; + public static int REDFULL = 3; + public static int REDSCORING = 5;//5 + public static int REDINIT = 79; + public static int BLUE = 93; + public static int BLUEFULL = 2; + public static int BLUESCORING = 5;//5 + public static int BLUEINIT = 99; + + public static int yellow = 81; + public static int lime = 87; + public static int white = 97; + public static int purple = 78; + + //yellow:81, green: 87, white:97 , purple:78 , + private RevBlinkinLedDriver blinkinLedDriver; + private RevBlinkinLedDriver.BlinkinPattern pattern; + + public LEDs(HardwareMap hardwareMap) { + blinkinLedDriver = hardwareMap.get(RevBlinkinLedDriver.class, "LEDs"); + setPattern(); + } + + public void setPattern() { + pattern = RevBlinkinLedDriver.BlinkinPattern.fromNumber(NUMBER); + blinkinLedDriver.setPattern(pattern); + } + + public void setPattern(int patternNumber) { + pattern = RevBlinkinLedDriver.BlinkinPattern.fromNumber(patternNumber); + blinkinLedDriver.setPattern(pattern); + } + + public void nextPattern() { + pattern = pattern.next(); + blinkinLedDriver.setPattern(pattern); + } + + public void previousPattern() { + pattern = pattern.previous(); + blinkinLedDriver.setPattern(pattern); + } + + public String getTelemetry() { + return String.format(Locale.US, "Pattern: %s", pattern.toString()); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java index caefe04..5b14c99 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java @@ -6,6 +6,7 @@ import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.hardware.rev.RevBlinkinLedDriver; import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive; @@ -20,6 +21,7 @@ public class Robot { public Intake intake; public Slides slides; public Arm arm; + public LEDs leds; public endGame_Mechs endGameMechs; public double macroStartTime = 0; @@ -32,6 +34,8 @@ public class Robot { public static double scoreWait = 0.65; public static double armWait = 2.0; + public int alliance = 0; + //Name the objects public Robot(HardwareMap hardwareMap) { drive = new SampleMecanumDrive(hardwareMap); @@ -41,9 +45,16 @@ public class Robot { arm = new Arm(hardwareMap); endGameMechs = new endGame_Mechs(hardwareMap); camEnabled = true; + leds = new LEDs(hardwareMap); } public void extendMacro(int slidePos, double runTime) { + if (alliance == 1){ + leds.setPattern(2); + }else{ + leds.setPattern(3); + } + switch(macroState) { case(0): macroStartTime = runTime; @@ -66,12 +77,23 @@ public class Robot { macroState = 0; lastMacro = runningMacro; runningMacro = 0; + if (alliance == 1){ + leds.setPattern(93); + }else{ + leds.setPattern(80); + } } break; } } public void resetMacro(int slidePos, double runTime) { + if (alliance == 1){ + leds.setPattern(2); + }else{ + leds.setPattern(3); + } + switch(macroState) { case(0): macroStartTime = runTime; @@ -103,6 +125,11 @@ public class Robot { macroState = 0; lastMacro = runningMacro; runningMacro = 0; + if (alliance == 1){ + leds.setPattern(93); + }else{ + leds.setPattern(80); + } } break; } @@ -180,15 +207,35 @@ public class Robot { } public void update(double runTime) { - Pose2d estimatedPose = null; - if (camera != null) { - estimatedPose = this.camera.estimatePoseFromAprilTag(); - } - drive.update(estimatedPose); +// drive.getPoseEstimate().getX(); +// = estimatedPose.getX(); + Pose2d estimatedPose = null; +// if (camera != null) { +// estimatedPose = this.camera.estimatePoseFromAprilTag(); +// } + //drive.update(estimatedPose); + +// Pose2d p1 = drive.getPoseEstimate(); +// +// Pose2d p2 = this.camera.estimatePoseFromAprilTag(); +// +// double D = Math.sqrt(Math.pow((p2.getX()-p1.getX()),2) + Math.pow((p2.getY()-p1.getY()),2)); +// +// if (D >= 6 || D <= 1){ +// estimatedPose = null; +// }else{ +// estimatedPose = this.camera.estimatePoseFromAprilTag(); +// +// } + + drive.update(/*estimatedPose*/); slides.update(runTime); arm.update(); + //leds.setPattern(); } + + public String getTelemetry() { return String.format("Arm:\n------------\n%s\nSlides:\n------------\n%s\nIMU:\n------------\n%s" , arm.getTelemetry(), slides.getTelemetry(), drive.getExternalHeadingVelocity()); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java index c70140a..a346e8c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java @@ -9,8 +9,8 @@ import com.qualcomm.robotcore.hardware.PIDFCoefficients; @Config public class Slides { - private DcMotor slide; - private DcMotor slide2; + public DcMotor slide; + public DcMotor slide2; // public static double p = 0.0014; // public static double i = 0.02; @@ -100,28 +100,26 @@ public class Slides { // pickupPos = 20 + heightOffset; // downPos = heightOffset;// TODO add these back in -// if (target == 0) { +// if (slide.getCurrentPosition() <= pTolerance && target <= pTolerance) { // slide.setPower(0); // slide2.setPower(0); // } else { -// if (target < 5) { -// slide.setPower(0); -// slide2.setPower(0); -// } else { - double pid, ff; - controller.setPID(coefficients.p, coefficients.i, coefficients.d); - controller.setTolerance(pTolerance); - pid = controller.calculate(slide.getCurrentPosition(), target); - ff = coefficients.f; - slide.setPower(pid + ff); + double pid, ff; + controller.setPID(coefficients.p, coefficients.i, coefficients.d); + controller.setTolerance(pTolerance); - pid = controller.calculate(slide2.getCurrentPosition(), target); - ff = coefficients.f; - slide2.setPower(pid + ff); + pid = controller.calculate(slide.getCurrentPosition(), target); + ff = coefficients.f; + slide.setPower(pid + ff); + + pid = controller.calculate(slide2.getCurrentPosition(), target); + ff = coefficients.f; + slide2.setPower(pid + ff); // } // } - } + } + public String getTelemetry() { return String.format("Position: %s %s\nTarget: %s %s\nPower: %s %s", slide.getCurrentPosition(), slide2.getCurrentPosition(), target, target, slide.getPower(), slide2.getPower()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java index 89a0eb7..3d8c114 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java @@ -29,24 +29,24 @@ public class BlueBackStageAuto extends AutoBase { public static final Pose2d DROP_1 = new Pose2d(24.5, 45, Math.toRadians(-90)); public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(53.4, 28.7, Math.toRadians(-180)); - public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(51.7, 34.5, Math.toRadians(-180)); + public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(51.5, 34.5, Math.toRadians(-180)); public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(51.5, 39.3, Math.toRadians(-180)); - public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.2, 35.6, Math.toRadians(-187)); + public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.2, 35.6, Math.toRadians(-180)); - public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52.7, 32.6, Math.toRadians(-187)); + public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(51, 32.6, Math.toRadians(-180)); - public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53, 33.5, Math.toRadians(-187)); + public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53, 33.5, Math.toRadians(-180)); //public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(24, -8.5);//-36 - public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(26, 12.1 , Math.toRadians(-187));//-36 + public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(26, 11.1 , Math.toRadians(-180));//-36 - public static final Pose2d STACK_LOCATION1 = new Pose2d(-56.2, 12.1, Math.toRadians(-187)); + public static final Pose2d STACK_LOCATION1 = new Pose2d(-56.2, 11.1, Math.toRadians(-180)); - public static final Pose2d STACK_LOCATION2 = new Pose2d(-55, 12.1, Math.toRadians(-187)); + public static final Pose2d STACK_LOCATION2 = new Pose2d(-58, 11.1, Math.toRadians(-180)); - public static final Pose2d STACK_LOCATION3 = new Pose2d(-55.6, 12.1, Math.toRadians(-187)); + public static final Pose2d STACK_LOCATION3 = new Pose2d(-55.6, 11.1, Math.toRadians(-180)); @Override @@ -89,30 +89,30 @@ public class BlueBackStageAuto extends AutoBase { // RUN INTAKE case 2: // intake - if (getRuntime() < macroTime + 0.25) { - robot.intake.setDcMotor(-0.24); - } - // if intake is done move on - else { - robot.intake.setDcMotor(0); - robot.arm.setDoor(Arm.DoorPosition.CLOSE); - robot.extendMacro_auto(Slides.mini_tier1, getRuntime(), Slides.micro_tier1); - builder = this.robot.getTrajectorySequenceBuilder(); - //Scores yellow pixel - switch (teamPropLocation) { - case 1: - builder.lineToLinearHeading(DEPOSIT_PRELOAD_1); - break; - case 2: - builder.lineToLinearHeading(DEPOSIT_PRELOAD_2); - break; - case 3: - builder.lineToLinearHeading(DEPOSIT_PRELOAD_3); - break; - } - this.robot.drive.followTrajectorySequenceAsync(builder.build()); - macroState++; +// if (getRuntime() < macroTime + 0.05) { +// +// } +// // if intake is done move on +// else { + robot.intake.setDcMotor(0); + robot.arm.setDoor(Arm.DoorPosition.CLOSE); + robot.extendMacro_auto(Slides.mini_tier1, getRuntime(), Slides.micro_tier1); + builder = this.robot.getTrajectorySequenceBuilder(); + //Scores yellow pixel + switch (teamPropLocation) { + case 1: + builder.lineToLinearHeading(DEPOSIT_PRELOAD_1); + break; + case 2: + builder.lineToLinearHeading(DEPOSIT_PRELOAD_2); + break; + case 3: + builder.lineToLinearHeading(DEPOSIT_PRELOAD_3); + break; } + this.robot.drive.followTrajectorySequenceAsync(builder.build()); + macroState++; + // } break; // EXTEND AND MOVE TO BACKBOARD case 3: @@ -139,7 +139,7 @@ public class BlueBackStageAuto extends AutoBase { builder.lineToConstantHeading(STACK_LOCATION1.vec().plus(new Vector2d(0))); break; case 2: - builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-2.65))); + builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(0))); break; case 3: builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d())); @@ -154,7 +154,7 @@ public class BlueBackStageAuto extends AutoBase { robot.resetMacro(0, getRuntime()); robot.intake.setDcMotor(0.58); robot.intake.setpos(STACK6); - if (!robot.drive.isBusy() && getRuntime() > macroTime + 0.2) { + if (!robot.drive.isBusy() && getRuntime() > macroTime + 0.4) { //robot.intake.setDcMotor(0.5); macroState++; } @@ -168,7 +168,7 @@ public class BlueBackStageAuto extends AutoBase { break; //goes back to the easel case 7: - if (getRuntime() > macroTime + 0.6) { + if (getRuntime() > macroTime + 0.7) { //robot.intake.setDcMotor(-0.0); robot.arm.setDoor(Arm.DoorPosition.CLOSE); robot.intake.setDcMotor(-0.35); @@ -224,7 +224,7 @@ public class BlueBackStageAuto extends AutoBase { builder.lineToConstantHeading(STACK_LOCATION1.vec().plus(new Vector2d(-2))); break; case 2: - builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-2.75))); + builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(0))); break; case 3: builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d())); @@ -239,7 +239,7 @@ public class BlueBackStageAuto extends AutoBase { robot.resetMacro(0, getRuntime()); robot.intake.setDcMotor(0.58); robot.intake.setpos(STACK3); - if (!robot.drive.isBusy() && getRuntime() > macroTime + 0.2) { + if (!robot.drive.isBusy() && getRuntime() > macroTime + 0.4) { //robot.intake.setDcMotor(0.68); macroState++; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontPreload.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontPreload.java index 1c8bcb0..bc8a98b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontPreload.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontPreload.java @@ -21,33 +21,37 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon; @Config @Autonomous(name = "Blue FrontPreload (2+1)", group = "Competition", preselectTeleOp = "Main TeleOp") public class BlueFrontPreload extends AutoBase { - public static final Pose2d DROP_1_PART_2 = new Pose2d(-36, 33.5, Math.toRadians(0)); + public static final Pose2d DROP_1_PART_2 = new Pose2d(-33, 33.5, Math.toRadians(0)); - public static final Pose2d DROP_1 = new Pose2d(-32,33.5,Math.toRadians(0)); + public static final Pose2d DROP_1 = new Pose2d(-33,33.5,Math.toRadians(0)); public static final Pose2d DROP_2 = new Pose2d(-39.7, 33.5, Math.toRadians(-90)); public static final Pose2d DROP_2_PART_2 = new Pose2d(-39.7,36.5, Math.toRadians(-90)); - public static final Pose2d DROP_3 = new Pose2d(-46.7, 50.5, Math.toRadians(-90)); + public static final Pose2d DROP_3 = new Pose2d(-51, 50.5, Math.toRadians(-90)); public static final Pose2d DROP_1M = new Pose2d(-36, 45, Math.toRadians(-90)); public static final Pose2d DROP_2M = new Pose2d(-48.5, 30, Math.toRadians(-90)); - public static final Pose2d DROP_3M = new Pose2d(-48.7, 35.9, Math.toRadians(-90)); + public static final Pose2d DROP_3M = new Pose2d(-53.7, 35.9, Math.toRadians(-90)); - public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, 38.3, Math.toRadians(8));//187 + public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, 38.3, Math.toRadians(0));//187 - public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52, 34, Math.toRadians(8));//187 + public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(53, 34.5, Math.toRadians(0));//187 - public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.6, 23.5, Math.toRadians(6));//817 + public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(55, 25, Math.toRadians(0));//817 - public static final Pose2d STACK_LOCATION = new Pose2d(-52, 29.6, Math.toRadians(180)); + public static final Pose2d STACK_LOCATION_1 = new Pose2d(-54.5, 32.6, Math.toRadians(180)); + + public static final Pose2d STACK_LOCATION_2 = new Pose2d(-52, 29.6, Math.toRadians(180)); + + public static final Pose2d STACK_LOCATION_3 = new Pose2d(-52, 29.6, Math.toRadians(180)); public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, 12.4, Math.toRadians(10));//-36 public static final Pose2d POST_DROP_POS = new Pose2d(-45, 59.5, Math.toRadians(180)); - public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, 59.5, Math.toRadians(180)); + public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, 58.5, Math.toRadians(180)); - public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, 59.5, Math.toRadians(180)); + public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, 58.5, Math.toRadians(180)); public static final Pose2d PARK_1 = new Pose2d(45,58,Math.toRadians(-180)); @@ -113,10 +117,23 @@ public class BlueFrontPreload extends AutoBase { break; } robot.arm.setDoor(OPEN); - builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-5.4,2.5))).waitSeconds(.01); + switch (teamPropLocation) { + case (1): + //team prop location 1 + builder.lineToLinearHeading(STACK_LOCATION_1.plus(new Pose2d(0))).waitSeconds(.01); + break; + case (2): + //team prop location 2 + builder.lineToLinearHeading(STACK_LOCATION_2.plus(new Pose2d(-5.8,2))).waitSeconds(.01); + + case (3): + //team prop location 3 + builder.lineToLinearHeading(STACK_LOCATION_3.plus(new Pose2d(-4.8,2))).waitSeconds(.01); + + } - robot.intake.setDcMotor(0.74); + robot.intake.setDcMotor(0.44); robot.intake.setpos(STACK6); macroTime = getRuntime(); this.robot.drive.followTrajectorySequenceAsync(builder.build()); @@ -164,12 +181,12 @@ public class BlueFrontPreload extends AutoBase { macroTime = getRuntime(); } break; -case 5: + case 5: // if drive is done move on - if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) { + if (getRuntime() > macroTime + 2.4 || !robot.drive.isBusy()) { macroTime = getRuntime(); robot.macroState = 0; - robot.extendMacro(Slides.mini_tier1-70, getRuntime()); + robot.extendMacro(Slides.mini_tier1-30, getRuntime()); macroState++; } break; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontStageAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontStageAuto.java index 8c568c8..ce90547 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontStageAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontStageAuto.java @@ -19,35 +19,39 @@ import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySe import org.firstinspires.ftc.teamcode.util.CenterStageCommon; @Config -@Autonomous(name = "Blue FrontStage Auto (2+3)", group = "Competition", preselectTeleOp = "Main TeleOp") +@Autonomous(name = "Blue FrontPreload (2+3)", group = "Competition", preselectTeleOp = "Main TeleOp") public class BlueFrontStageAuto extends AutoBase { - public static final Pose2d DROP_1_PART_2 = new Pose2d(-36, 33.5, Math.toRadians(0)); + public static final Pose2d DROP_1_PART_2 = new Pose2d(-33, 33.5, Math.toRadians(0)); - public static final Pose2d DROP_1 = new Pose2d(-32,33.5,Math.toRadians(0)); + public static final Pose2d DROP_1 = new Pose2d(-33,33.5,Math.toRadians(0)); public static final Pose2d DROP_2 = new Pose2d(-39.7, 33.5, Math.toRadians(-90)); public static final Pose2d DROP_2_PART_2 = new Pose2d(-39.7,36.5, Math.toRadians(-90)); - public static final Pose2d DROP_3 = new Pose2d(-46.7, 50.5, Math.toRadians(-90)); + public static final Pose2d DROP_3 = new Pose2d(-49, 50.5, Math.toRadians(-90)); public static final Pose2d DROP_1M = new Pose2d(-36, 45, Math.toRadians(-90)); public static final Pose2d DROP_2M = new Pose2d(-48.5, 30, Math.toRadians(-90)); - public static final Pose2d DROP_3M = new Pose2d(-48.7, 35.9, Math.toRadians(-90)); + public static final Pose2d DROP_3M = new Pose2d(-53.7, 35.9, Math.toRadians(-90)); - public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, 38.3, Math.toRadians(8));//187 + public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, 38.3, Math.toRadians(0));//187 - public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(53, 34, Math.toRadians(8));//187 + public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(53, 34.5, Math.toRadians(0));//187 - public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.6, 23.5, Math.toRadians(6));//817 + public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(55, 25, Math.toRadians(0));//817 - public static final Pose2d STACK_LOCATION = new Pose2d(-52, 29.6, Math.toRadians(180)); + public static final Pose2d STACK_LOCATION_1 = new Pose2d(-54.5, 32.6, Math.toRadians(180)); + + public static final Pose2d STACK_LOCATION_2 = new Pose2d(-52, 29.6, Math.toRadians(180)); + + public static final Pose2d STACK_LOCATION_3 = new Pose2d(-52, 29.6, Math.toRadians(180)); public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, 12.4, Math.toRadians(10));//-36 public static final Pose2d POST_DROP_POS = new Pose2d(-45, 59.5, Math.toRadians(180)); - public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, 59.5, Math.toRadians(180)); + public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, 58.5, Math.toRadians(180)); - public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, 59.5, Math.toRadians(180)); + public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, 58.5, Math.toRadians(180)); public static final Pose2d PARK_1 = new Pose2d(45,58,Math.toRadians(-180)); @@ -113,10 +117,23 @@ public class BlueFrontStageAuto extends AutoBase { break; } robot.arm.setDoor(OPEN); - builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-5.4,2.5))).waitSeconds(.01); + switch (teamPropLocation) { + case (1): + //team prop location 1 + builder.lineToLinearHeading(STACK_LOCATION_1.plus(new Pose2d(0))).waitSeconds(.01); + break; + case (2): + //team prop location 2 + builder.lineToLinearHeading(STACK_LOCATION_2.plus(new Pose2d(-5.8,2))).waitSeconds(.01); + + case (3): + //team prop location 3 + builder.lineToLinearHeading(STACK_LOCATION_3.plus(new Pose2d(-4.8,2))).waitSeconds(.01); + + } - robot.intake.setDcMotor(0.74); + robot.intake.setDcMotor(0.44); robot.intake.setpos(STACK6); macroTime = getRuntime(); this.robot.drive.followTrajectorySequenceAsync(builder.build()); @@ -166,10 +183,10 @@ public class BlueFrontStageAuto extends AutoBase { break; case 5: // if drive is done move on - if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) { + if (getRuntime() > macroTime + 2.4 || !robot.drive.isBusy()) { macroTime = getRuntime(); robot.macroState = 0; - robot.extendMacro(Slides.mini_tier1-70, getRuntime()); + robot.extendMacro(Slides.mini_tier1-30, getRuntime()); macroState++; } break; @@ -184,183 +201,92 @@ public class BlueFrontStageAuto extends AutoBase { } break; - //Stack run 2 case 7: - robot.resetMacro(0, getRuntime()); + robot.resetMacro(0,getRuntime()); if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) { builder = this.robot.getTrajectorySequenceBuilder(); - builder.splineToConstantHeading(PRE_DEPOSIT_POS.plus(new Pose2d(0,-2)).vec(), Math.toRadians(180)); - builder.lineToConstantHeading(POST_DROP_POS.plus(new Pose2d(0,-2)).vec()); - builder.splineToConstantHeading(STACK_LOCATION.plus(new Pose2d(-3.9, -3.7)).vec(), Math.toRadians(180)); + builder.splineToConstantHeading(PRE_DEPOSIT_POS.vec(),Math.toRadians(0)); + builder.lineToConstantHeading(POST_DROP_POS_PART2.vec()); + builder.splineToConstantHeading(STACK_LOCATION_2.vec(),Math.toRadians(0)); this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; } break; - //waits for the robot to fin the trajectory + case 8: - robot.resetMacro(0, getRuntime()); robot.arm.setDoor(OPEN); - robot.intake.setDcMotor(0.54); + robot.intake.setDcMotor(0.44); robot.intake.setpos(STACK4); - if (!robot.drive.isBusy()) { - macroState++; - } - break; - //3rd and 4th pixels off the stack are in-taken by this - case 9: - robot.intake.setDcMotor(0.54); - robot.arm.setDoor(OPEN); - robot.intake.setpos(STACK3); macroTime = getRuntime(); macroState++; break; + case 9: + if (getRuntime() > macroTime + 0.06) { + robot.arm.setDoor(CLOSE); + robot.intake.setDcMotor(-0.5); + builder = this.robot.getTrajectorySequenceBuilder(); + + switch (teamPropLocation) { + case 1: + builder.setTangent(Math.toRadians(90)); + builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); + builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0)); + break; + case 2: + builder.setTangent(Math.toRadians(90)); + builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); + builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0)); + break; + case 3: + builder.setTangent(Math.toRadians(90)); + builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); + builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0)); + break; + } + this.robot.drive.followTrajectorySequenceAsync(builder.build()); + macroState++; + macroTime = getRuntime(); + } + break; + case 10: - if (getRuntime() > macroTime + 0.6) { - robot.arm.setDoor(CLOSE); - robot.intake.setDcMotor(-0.45); - builder = this.robot.getTrajectorySequenceBuilder(); - //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); - //builder.splineToLinearHeading(POST_DROP_POS, Math.toRadians(0)); - //builder.lineToLinearHeading(POST_DROP_POS_PART2.plus(new Pose2d(0,2))); - - - switch (teamPropLocation) { - case 1: - builder.setTangent(Math.toRadians(90)); - builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); - builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); - builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0)); - break; - case 2: - builder.setTangent(Math.toRadians(90)); - builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); - builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); - builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0)); - break; - case 3: - builder.setTangent(Math.toRadians(90)); - builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); - builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); - builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0)); - break; - } - this.robot.drive.followTrajectorySequenceAsync(builder.build()); - macroState++; + // if drive is done move on + if (getRuntime() > macroTime + 2.4 || !robot.drive.isBusy()) { macroTime = getRuntime(); + robot.macroState = 0; + robot.extendMacro(Slides.mini_tier1-30, getRuntime()); + macroState++; } break; + case 11: - // if drive is done move on - if (getRuntime() > macroTime + 6.4 || !robot.drive.isBusy()) { - macroTime = getRuntime(); - robot.macroState = 0; - robot.extendMacro(Slides.mini_tier1 + 20, getRuntime()); + if (robot.macroState != 0) { + robot.extendMacro(Slides.mini_tier1, getRuntime()); + } + if (robot.macroState == 0 && !robot.drive.isBusy()) { + robot.resetMacro(0, getRuntime()); macroState++; } break; - //extending the macro and about to score + + //Park + case 12: - if (robot.macroState != 0) { - robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); - } - if (robot.macroState == 0 && !robot.drive.isBusy()) { - robot.resetMacro(0, getRuntime()); - macroState++; - } - break; - - //stack run 3 - case 13: robot.resetMacro(0, getRuntime()); - if (getRuntime() > macroTime + 2.4 || robot.macroState >= 4) { + if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) { builder = this.robot.getTrajectorySequenceBuilder(); - builder.splineToConstantHeading(PRE_DEPOSIT_POS.vec(), Math.toRadians(180)); - builder.lineToLinearHeading(POST_DROP_POS); - builder.splineToConstantHeading(STACK_LOCATION.plus(new Pose2d(-1.5)).vec(), Math.toRadians(180)); + builder.lineToLinearHeading(PARK_1); this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; } break; - //waits for the robot to fin the trajectory - case 14: - robot.resetMacro(0, getRuntime()); - robot.arm.setDoor(OPEN); - robot.intake.setDcMotor(0.54); - robot.intake.setpos(STACK2); - if (!robot.drive.isBusy()) { - macroState++; - } - break; - //3rd and 4th pixels off the stack are intaken by this - case 15: - robot.intake.setDcMotor(0.54); - robot.arm.setDoor(OPEN); - robot.intake.setpos(STACK1); - macroTime = getRuntime(); - macroState++; - break; - case 16: - if (getRuntime() > macroTime + 0.6) { - robot.arm.setDoor(CLOSE); - robot.intake.setDcMotor(-0.45); - builder = this.robot.getTrajectorySequenceBuilder(); - //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); - //builder.splineToLinearHeading(POST_DROP_POS, Math.toRadians(0)); - //builder.lineToLinearHeading(POST_DROP_POS_PART2.plus(new Pose2d(0,2))); - switch (teamPropLocation) { - case 1: - builder.setTangent(Math.toRadians(90)); - builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); - builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); - builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0)); - break; - case 2: - builder.setTangent(Math.toRadians(90)); - builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); - builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); - builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0)); - break; - case 3: - builder.setTangent(Math.toRadians(90)); - builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); - builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); - builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0)); - break; - } - this.robot.drive.followTrajectorySequenceAsync(builder.build()); - macroState++; - macroTime = getRuntime(); - } - break; - case 17: - // if drive is done move on - if (getRuntime() > macroTime + 6.4 || !robot.drive.isBusy()) { - macroTime = getRuntime(); - robot.macroState = 0; - robot.extendMacro(Slides.mini_tier1 + 20, getRuntime()); - macroState++; - } - break; - //extending the macro and about to score - case 18: - if (robot.macroState != 0) { - robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); - } - if (robot.macroState == 0 && !robot.drive.isBusy()) { - robot.resetMacro(0, getRuntime()); - macroState++; - } - break; - - case 19: - robot.resetMacro(0, getRuntime()); - if (robot.macroState == 0) { - macroState = -1; - } } } -} +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontPreload.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontPreload.java index 5df7df8..58610e0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontPreload.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontPreload.java @@ -21,33 +21,37 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon; @Config @Autonomous(name = "Red FrontPreload (2+1)", group = "Competition", preselectTeleOp = "Main TeleOp") public class RedFrontPreload extends AutoBase { - public static final Pose2d DROP_1_PART_2 = new Pose2d(-36, -33.5, Math.toRadians(180)); + public static final Pose2d DROP_1_PART_2 = new Pose2d(-33, -33.5, Math.toRadians(180)); - public static final Pose2d DROP_1 = new Pose2d(-32,-33.5,Math.toRadians(180)); - public static final Pose2d DROP_2 = new Pose2d(-39.7, -33.5, Math.toRadians(90)); - public static final Pose2d DROP_2_PART_2 = new Pose2d(-39.7,-36.5, Math.toRadians(90)); - public static final Pose2d DROP_3 = new Pose2d(-46.7, -50.5, Math.toRadians(90)); + public static final Pose2d DROP_1 = new Pose2d(-33,-33.5,Math.toRadians(180)); + public static final Pose2d DROP_2 = new Pose2d(-35.7, -33.5, Math.toRadians(90)); + public static final Pose2d DROP_2_PART_2 = new Pose2d(-35.7,-41, Math.toRadians(90)); + public static final Pose2d DROP_3 = new Pose2d(-51, -50.5, Math.toRadians(90)); public static final Pose2d DROP_1M = new Pose2d(-36, -45, Math.toRadians(90)); public static final Pose2d DROP_2M = new Pose2d(-48.5, -30, Math.toRadians(90)); - public static final Pose2d DROP_3M = new Pose2d(-48.7, -35.9, Math.toRadians(90)); + public static final Pose2d DROP_3M = new Pose2d(-53.7, -35.9, Math.toRadians(90)); - public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, -38.3, Math.toRadians(188));//187 + public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, -38.3, Math.toRadians(180));//187 - public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52, -34, Math.toRadians(188));//187 + public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(50, -32, Math.toRadians(187));//187 - public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.6, -23.5, Math.toRadians(186));//817 + public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(55, -25, Math.toRadians(180));//817 - public static final Pose2d STACK_LOCATION = new Pose2d(-52, -29.6, Math.toRadians(0)); + public static final Pose2d STACK_LOCATION_1 = new Pose2d(-54.5, -32.6, Math.toRadians(180)); + + public static final Pose2d STACK_LOCATION_2 = new Pose2d(-55, -40, Math.toRadians(180)); + + public static final Pose2d STACK_LOCATION_3 = new Pose2d(-52, -29.6, Math.toRadians(180)); public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, -12.4, Math.toRadians(190));//-36 public static final Pose2d POST_DROP_POS = new Pose2d(-45, -59.5, Math.toRadians(0)); - public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, -59.5, Math.toRadians(0)); + public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-40, -58.5, Math.toRadians(0)); - public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, -59.5, Math.toRadians(0)); + public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(12, -58.5, Math.toRadians(0)); public static final Pose2d PARK_1 = new Pose2d(45,-58,Math.toRadians(180)); @@ -95,8 +99,7 @@ public class RedFrontPreload extends AutoBase { // RUN INTAKE case 2: // intake - if (getRuntime() < macroTime + 0.18) { - robot.intake.setDcMotor(-0.23); + if (getRuntime() < macroTime + 0.1) { } else{ builder = this.robot.getTrajectorySequenceBuilder(); @@ -113,10 +116,23 @@ public class RedFrontPreload extends AutoBase { break; } robot.arm.setDoor(OPEN); - builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-5.4,-2.5))).waitSeconds(.01); + switch (teamPropLocation) { + case (1): + //team prop location 1 + builder.lineToLinearHeading(STACK_LOCATION_1.plus(new Pose2d(0))).waitSeconds(.01); + break; + case (2): + //team prop location 2 + builder.lineToLinearHeading(STACK_LOCATION_2).waitSeconds(.01); + + case (3): + //team prop location 3 + builder.lineToLinearHeading(STACK_LOCATION_3.plus(new Pose2d(-4.8,-2))).waitSeconds(.01); + + } - robot.intake.setDcMotor(0.74); + robot.intake.setDcMotor(0.44); robot.intake.setpos(STACK6); macroTime = getRuntime(); this.robot.drive.followTrajectorySequenceAsync(builder.build()); @@ -132,11 +148,10 @@ public class RedFrontPreload extends AutoBase { } break; - case 4: - if (getRuntime() > macroTime + 0.06) { + if (getRuntime() > macroTime + 0.1) { robot.arm.setDoor(CLOSE); - robot.intake.setDcMotor(-0.5); + robot.intake.setDcMotor(-0.1); builder = this.robot.getTrajectorySequenceBuilder(); switch (teamPropLocation) { @@ -169,7 +184,7 @@ public class RedFrontPreload extends AutoBase { if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) { macroTime = getRuntime(); robot.macroState = 0; - robot.extendMacro(Slides.mini_tier1-70, getRuntime()); + robot.extendMacro(Slides.mini_tier1-30, getRuntime()); macroState++; } break; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/BlueTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/BlueTeleop.java new file mode 100644 index 0000000..2e52ab1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/BlueTeleop.java @@ -0,0 +1,239 @@ +package org.firstinspires.ftc.teamcode.opmode.teleop; + +import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE; +import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.hardware.VoltageSensor; + +import org.firstinspires.ftc.teamcode.controller.Controller; +import org.firstinspires.ftc.teamcode.hardware.Arm; +import org.firstinspires.ftc.teamcode.hardware.Robot; +import org.firstinspires.ftc.teamcode.hardware.Slides; + +@Config +@TeleOp(name = "Blue TeleOp", group = "OpModes") +public class BlueTeleop extends OpMode { + + public static double normal = 0.7; + public static double turbo = 1; + public static double slow_mode = 0.35; + public static double intakeMax = 0.36; + public static double intakeMax2 = -0.70; + + private Robot robot; + private Controller controller1; + private Controller controller2; + + public boolean hang_counter = false; + + + private DcMotor[] motors = new DcMotor[8]; + private Servo[] servos = new Servo[7]; + + @Override + public void init() { + this.motors[0] = this.hardwareMap.get(DcMotor.class, "Rightslide"); + this.motors[1] = this.hardwareMap.get(DcMotor.class, "Intakemotor"); + this.motors[2] = this.hardwareMap.get(DcMotor.class, "FrontRight"); + this.motors[3] = this.hardwareMap.get(DcMotor.class, "BackRight"); + this.motors[4] = this.hardwareMap.get(DcMotor.class, "BackLeft"); + this.motors[5] = this.hardwareMap.get(DcMotor.class, "FrontLeft"); + this.motors[6] = this.hardwareMap.get(DcMotor.class, "Leftslide"); + this.motors[7] = this.hardwareMap.get(DcMotor.class, "Hang"); + + this.servos[0] = this.hardwareMap.get(Servo.class, "LeftIntake"); + this.servos[1] = this.hardwareMap.get(Servo.class, "Wrist"); + this.servos[2] = this.hardwareMap.get(Servo.class, "Door"); + this.servos[3] = this.hardwareMap.get(Servo.class, "RightArm"); + this.servos[4] = this.hardwareMap.get(Servo.class, "LeftArm"); + this.servos[5] = this.hardwareMap.get(Servo.class, "Right Intake Servo"); + this.servos[6] = this.hardwareMap.get(Servo.class, "Drone"); + + controller1 = new Controller(gamepad1); + controller2 = new Controller(gamepad2); + + this.robot = new Robot(hardwareMap); +// robot.intake.setpos(Intake.Position.STACK1); + +// while (robot.camera.getFrameCount() < 1) { +// telemetry.addLine("Initializing..."); +// telemetry.update(); +// } + + telemetry.addLine("Initialized"); + this.telemetry = FtcDashboard.getInstance().getTelemetry(); + telemetry.update(); + + this.robot.alliance = 1; + this.robot.leds.setPattern(92); + + } + + double getBatteryVoltage() { + double result = Double.POSITIVE_INFINITY; + for (VoltageSensor sensor : hardwareMap.voltageSensor) { + double voltage = sensor.getVoltage(); + if (voltage > 0) { + result = Math.min(result, voltage); + } + } + return result; + } + + @Override + public void loop() { + for (DcMotor motor : this.motors) { + this.telemetry.addData(this.hardwareMap.getNamesOf(motor).stream().findFirst().get(), motor.getPower()); + } + for (Servo servo : this.servos) { + this.telemetry.addData(this.hardwareMap.getNamesOf(servo).stream().findFirst().get(), servo.getPosition()); + } + + this.telemetry.addData("battery", getBatteryVoltage()); + this.telemetry.update(); + + this.robot.leds.setPattern(93); + + // Driver 1 + double x = -gamepad1.left_stick_y; + double y = -gamepad1.left_stick_x; + double z = -gamepad1.right_stick_x; + + if (controller1.getRightTrigger().getValue() > 0.1) { + x *= turbo; + y *= turbo; + z *= turbo; + } + else if (controller1.getLeftTrigger().getValue() > 0.1) { + x *= slow_mode; + y *= slow_mode; + z *= slow_mode; + } else { + x *= normal; + y *= normal; + z *= normal; + } + robot.drive.setWeightedDrivePower(new Pose2d(x, y, z)); + // Drone launcher + if (controller1.getA().isPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) { + this.robot.endGameMechs.launch(); + } else { + this.robot.endGameMechs.reset(); + } + +// if (controller1.getRightBumper().isPressed()) { +// this.robot.endGameMechs.Finger_in(); +// }else { +// this.robot.endGameMechs.Finger_out(); +// } + //Hang Motor + +// if (controller1.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed() && !hang_counter){ +// this.robot.endGameMechs.hang_init_pos(); +// hang_counter = true; +// } +// else if (controller1.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed() && hang_counter){ +// this.robot.endGameMechs.hang_final_pos(); +// hang_counter = false; +// } + + + if (controller1.getB().isPressed()) { + this.robot.endGameMechs.hang_final_pos(); + } else { + this.robot.endGameMechs.hang_init_pos(); + } + + if (controller1.getX().isPressed()) { + this.robot.intake.wheel_spit(); + } else if (controller1.getY().isPressed()) { + this.robot.intake.wheel_swallow(); + } else { + this.robot.intake.wheel_stop(); + } + + + // Driver 2 + if (controller2.getRightTrigger().getValue()>=0.1){ + robot.intake.setDcMotor(controller2.getRightTrigger().getValue()*intakeMax); + } + else if(controller2.getLeftTrigger().getValue()>=0.1){ + robot.intake.setDcMotor(controller2.getLeftTrigger().getValue()*intakeMax2); + } + else { + robot.intake.setDcMotor(0); + } + + if (controller2.getRightBumper().isJustPressed()) { + robot.intake.incrementPos(); + } + if (controller2.getLeftBumper().isJustPressed()) { + robot.intake.decrementPos(); + } + + + + + + // macros + switch (robot.runningMacro) { + case (0): // manual mode + + robot.slides.increaseTarget(controller2.getLeftStick().getY()); + if (robot.intake.getPower() >= 0.01) { + robot.arm.setDoor(OPEN); + } else if (robot.intake.getPower() <= -0.01) { + robot.arm.setDoor(OPEN); + } else if (controller2.getLeftBumper().isPressed()) { + robot.arm.setDoor(Arm.DoorPosition.OPEN); + } else { + robot.arm.setDoor(CLOSE); + } + + if (controller2.getX().isJustPressed()) { + robot.runningMacro = 1; + } else if (controller2.getY().isJustPressed()) { + robot.runningMacro = 2; + } else if (controller2.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) { + robot.runningMacro = 3; + } else if (controller2.getA().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) { + robot.runningMacro = 4; + } else if (controller2.getDDown().isJustPressed() ) { + robot.runningMacro = 5; + } + + break; + case (1): + robot.extendMacro(Slides.tier1, getRuntime()); + break; + case (2): + robot.extendMacro(Slides.tier2, getRuntime()); + break; + case (3): + robot.extendMacro(Slides.tier3, getRuntime()); + break; + case (4): + robot.resetMacro(0, getRuntime()); + break; + case(5): + robot.extendMacro(Slides.mini_tier1, getRuntime()); + break; + } + + // update and telemetry + robot.update(getRuntime()); + controller1.update(); + controller2.update(); + telemetry.addLine(robot.getTelemetry()); + telemetry.update(); + + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java index be34909..e0ecc6d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java @@ -24,8 +24,8 @@ public class MainTeleOp extends OpMode { public static double normal = 0.7; public static double turbo = 1; public static double slow_mode = 0.35; - public static double intakeMax = 0.65; - public static double intakeMax2 = -0.65; + public static double intakeMax = 0.36; + public static double intakeMax2 = -0.70; private Robot robot; private Controller controller1; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java new file mode 100644 index 0000000..be45263 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java @@ -0,0 +1,240 @@ +package org.firstinspires.ftc.teamcode.opmode.teleop; + +import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE; +import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.hardware.VoltageSensor; + +import org.firstinspires.ftc.teamcode.controller.Controller; +import org.firstinspires.ftc.teamcode.hardware.Arm; +import org.firstinspires.ftc.teamcode.hardware.Robot; +import org.firstinspires.ftc.teamcode.hardware.Slides; + +@Config +@TeleOp(name = "Red TeleOp", group = "OpModes") +public class RedTeleop extends OpMode { + + public static double normal = 0.7; + public static double turbo = 1; + public static double slow_mode = 0.35; + public static double intakeMax = 0.36; + public static double intakeMax2 = -0.70; + + private Robot robot; + private Controller controller1; + private Controller controller2; + + public boolean hang_counter = false; + + + private DcMotor[] motors = new DcMotor[8]; + private Servo[] servos = new Servo[7]; + + @Override + public void init() { + this.motors[0] = this.hardwareMap.get(DcMotor.class, "Rightslide"); + this.motors[1] = this.hardwareMap.get(DcMotor.class, "Intakemotor"); + this.motors[2] = this.hardwareMap.get(DcMotor.class, "FrontRight"); + this.motors[3] = this.hardwareMap.get(DcMotor.class, "BackRight"); + this.motors[4] = this.hardwareMap.get(DcMotor.class, "BackLeft"); + this.motors[5] = this.hardwareMap.get(DcMotor.class, "FrontLeft"); + this.motors[6] = this.hardwareMap.get(DcMotor.class, "Leftslide"); + this.motors[7] = this.hardwareMap.get(DcMotor.class, "Hang"); + + this.servos[0] = this.hardwareMap.get(Servo.class, "LeftIntake"); + this.servos[1] = this.hardwareMap.get(Servo.class, "Wrist"); + this.servos[2] = this.hardwareMap.get(Servo.class, "Door"); + this.servos[3] = this.hardwareMap.get(Servo.class, "RightArm"); + this.servos[4] = this.hardwareMap.get(Servo.class, "LeftArm"); + this.servos[5] = this.hardwareMap.get(Servo.class, "Right Intake Servo"); + this.servos[6] = this.hardwareMap.get(Servo.class, "Drone"); + + controller1 = new Controller(gamepad1); + controller2 = new Controller(gamepad2); + + this.robot = new Robot(hardwareMap); +// robot.intake.setpos(Intake.Position.STACK1); + +// while (robot.camera.getFrameCount() < 1) { +// telemetry.addLine("Initializing..."); +// telemetry.update(); +// } + + telemetry.addLine("Initialized"); + this.telemetry = FtcDashboard.getInstance().getTelemetry(); + telemetry.update(); + this.robot.alliance = 2; + + this.robot.leds.setPattern(79); + + + } + + double getBatteryVoltage() { + double result = Double.POSITIVE_INFINITY; + for (VoltageSensor sensor : hardwareMap.voltageSensor) { + double voltage = sensor.getVoltage(); + if (voltage > 0) { + result = Math.min(result, voltage); + } + } + return result; + } + + @Override + public void loop() { + for (DcMotor motor : this.motors) { + this.telemetry.addData(this.hardwareMap.getNamesOf(motor).stream().findFirst().get(), motor.getPower()); + } + for (Servo servo : this.servos) { + this.telemetry.addData(this.hardwareMap.getNamesOf(servo).stream().findFirst().get(), servo.getPosition()); + } + + this.telemetry.addData("battery", getBatteryVoltage()); + this.telemetry.update(); + + this.robot.leds.setPattern(80); + + // Driver 1 + double x = -gamepad1.left_stick_y; + double y = -gamepad1.left_stick_x; + double z = -gamepad1.right_stick_x; + + if (controller1.getRightTrigger().getValue() > 0.1) { + x *= turbo; + y *= turbo; + z *= turbo; + } + else if (controller1.getLeftTrigger().getValue() > 0.1) { + x *= slow_mode; + y *= slow_mode; + z *= slow_mode; + } else { + x *= normal; + y *= normal; + z *= normal; + } + robot.drive.setWeightedDrivePower(new Pose2d(x, y, z)); + // Drone launcher + if (controller1.getA().isPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) { + this.robot.endGameMechs.launch(); + } else { + this.robot.endGameMechs.reset(); + } + +// if (controller1.getRightBumper().isPressed()) { +// this.robot.endGameMechs.Finger_in(); +// }else { +// this.robot.endGameMechs.Finger_out(); +// } + //Hang Motor + +// if (controller1.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed() && !hang_counter){ +// this.robot.endGameMechs.hang_init_pos(); +// hang_counter = true; +// } +// else if (controller1.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed() && hang_counter){ +// this.robot.endGameMechs.hang_final_pos(); +// hang_counter = false; +// } + + + if (controller1.getB().isPressed()) { + this.robot.endGameMechs.hang_final_pos(); + } else { + this.robot.endGameMechs.hang_init_pos(); + } + + if (controller1.getX().isPressed()) { + this.robot.intake.wheel_spit(); + } else if (controller1.getY().isPressed()) { + this.robot.intake.wheel_swallow(); + } else { + this.robot.intake.wheel_stop(); + } + + + // Driver 2 + if (controller2.getRightTrigger().getValue()>=0.1){ + robot.intake.setDcMotor(controller2.getRightTrigger().getValue()*intakeMax); + } + else if(controller2.getLeftTrigger().getValue()>=0.1){ + robot.intake.setDcMotor(controller2.getLeftTrigger().getValue()*intakeMax2); + } + else { + robot.intake.setDcMotor(0); + } + + if (controller2.getRightBumper().isJustPressed()) { + robot.intake.incrementPos(); + } + if (controller2.getLeftBumper().isJustPressed()) { + robot.intake.decrementPos(); + } + + + + + + // macros + switch (robot.runningMacro) { + case (0): // manual mode + + robot.slides.increaseTarget(controller2.getLeftStick().getY()); + if (robot.intake.getPower() >= 0.01) { + robot.arm.setDoor(OPEN); + } else if (robot.intake.getPower() <= -0.01) { + robot.arm.setDoor(OPEN); + } else if (controller2.getLeftBumper().isPressed()) { + robot.arm.setDoor(Arm.DoorPosition.OPEN); + } else { + robot.arm.setDoor(CLOSE); + } + + if (controller2.getX().isJustPressed()) { + robot.runningMacro = 1; + } else if (controller2.getY().isJustPressed()) { + robot.runningMacro = 2; + } else if (controller2.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) { + robot.runningMacro = 3; + } else if (controller2.getA().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) { + robot.runningMacro = 4; + } else if (controller2.getDDown().isJustPressed() ) { + robot.runningMacro = 5; + } + + break; + case (1): + robot.extendMacro(Slides.tier1, getRuntime()); + break; + case (2): + robot.extendMacro(Slides.tier2, getRuntime()); + break; + case (3): + robot.extendMacro(Slides.tier3, getRuntime()); + break; + case (4): + robot.resetMacro(0, getRuntime()); + break; + case(5): + robot.extendMacro(Slides.mini_tier1, getRuntime()); + break; + } + + // update and telemetry + robot.update(getRuntime()); + controller1.update(); + controller2.update(); + telemetry.addLine(robot.getTelemetry()); + telemetry.update(); + + + } +} From 3c4b14dc333cc7c8e98c633aa95f1477ed5dff1e Mon Sep 17 00:00:00 2001 From: ImposterVarunoverlord Date: Thu, 21 Mar 2024 08:24:29 -0500 Subject: [PATCH 4/9] 2+1(blue works not red) --- .../meepmeeptesting/MeepMeepTesting.java | 31 +- .../ftc/teamcode/hardware/Arm.java | 12 +- .../teamcode/opmode/autonomous/AutoBase.java | 2 +- .../opmode/autonomous/BlueFrontPreload.java | 4 +- .../opmode/autonomous/RedFrontPreload.java | 17 +- .../opmode/autonomous/RedFrontStageme.java | 304 ++++++++++++++++++ .../teamcode/opmode/teleop/BlueTeleop.java | 3 + 7 files changed, 356 insertions(+), 17 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontStageme.java diff --git a/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java b/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java index 1c06d82..b3b6bd1 100644 --- a/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java +++ b/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java @@ -90,13 +90,42 @@ public class MeepMeepTesting { .lineToLinearHeading(new Pose2d(45,58,Math.toRadians(-180))) .build() ); + + RoadRunnerBotEntity RedFrontStage = new DefaultBotBuilder(meepMeep) + .setConstraints(60,60,Math.toRadians(180),Math.toRadians(180),15) + .followTrajectorySequence(drive -> + drive.trajectorySequenceBuilder(new Pose2d(-36, -61.5, Math.toRadians(90))) + //Score then pick up 1 white + .lineToLinearHeading(new Pose2d(-39.7, -33.5, Math.toRadians(90))) + .lineToLinearHeading(new Pose2d(-51.5, -33.6, Math.toRadians(180)).plus(new Pose2d(-5.4,1.5))).waitSeconds(.01) + + //Spline to board + .setTangent(Math.toRadians(-90)) + .splineToConstantHeading(new Pose2d(-33, -59.5, Math.toRadians(180)).vec(),Math.toRadians(0)) + .lineToConstantHeading(new Pose2d(33, -59.5, Math.toRadians(180)).vec()) + .splineToConstantHeading(new Pose2d(52, -34, Math.toRadians(8)).vec(),Math.toRadians(0)) + + //Go back to white stack + .splineToConstantHeading(new Pose2d(33, -59.5, Math.toRadians(180)).plus(new Pose2d(0,-2)).vec(), Math.toRadians(180)) + .lineToConstantHeading(new Pose2d(-45, -59.5, Math.toRadians(180)).plus(new Pose2d(0,-2)).vec()) + .splineToConstantHeading(new Pose2d(-51.5, -33.6, Math.toRadians(180)).plus(new Pose2d(-3.9, -3.7)).vec(), Math.toRadians(180)) + + //Go back to board + .setTangent(Math.toRadians(-90)) + .splineToConstantHeading(new Pose2d(-33, -59.5, Math.toRadians(180)).vec(),Math.toRadians(0)) + .lineToConstantHeading(new Pose2d(33, -59.5, Math.toRadians(180)).vec()) + .splineToConstantHeading(new Pose2d(52, -34, Math.toRadians(8)).vec(),Math.toRadians(0)) + .build() + ); + meepMeep.setBackground(MeepMeep.Background.FIELD_CENTERSTAGE_JUICE_DARK) .setDarkMode(true) .setBackgroundAlpha(0.95f) //.addEntity(myBot) //.addEntity(BlueFrontStage) //.addEntity(BlueFrontStage3) - .addEntity(BlueFrontStage1) + //.addEntity(BlueFrontStage1) + .addEntity(RedFrontStage) .start(); } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java index 09e852e..ea3e619 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java @@ -49,7 +49,7 @@ public class Arm { public static double wristScore_highMacro = 0.3; public enum Position { - INTAKE, SCORE, SCOREHI + INTAKE, SCORE, SCOREHI, SCORELEFT, SCORERIGHT } public enum DoorPosition { @@ -61,7 +61,7 @@ public class Arm { doorServo = hardwareMap.get(Servo.class, "Door"); lAServo = hardwareMap.get(Servo.class, "LeftArm"); rAServo = hardwareMap.get(Servo.class, "RightArm"); - // elbow = hardwareMap.get(Servo.class, "Elbow"); + elbow = hardwareMap.get(Servo.class, "Elbow"); // lAServo.setDirection(Servo.Direction.REVERSE); rAServo.setDirection(Servo.Direction.REVERSE); doorServo.setDirection(Servo.Direction.REVERSE); @@ -106,11 +106,13 @@ public class Arm { public void setElbowPos (Position pos){ if (pos == Position.INTAKE) { - wristPos = elbow_mid; + elbowPos = elbow_mid; } else if (pos == Position.SCORE) { - wristPos = elbow_right; - } else if (pos == Position.SCOREHI) { + wristPos = elbow_mid; + } else if (pos == Position.SCORELEFT) { wristPos = elbow_left; + } else if (pos == Position.SCORERIGHT) { + wristPos = elbow_right; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AutoBase.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AutoBase.java index c33448e..c3b4726 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AutoBase.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AutoBase.java @@ -43,7 +43,7 @@ public abstract class AutoBase extends LinearOpMode { telemetry.addLine("Initialized"); // Detection vndafds = robot.camera.getProp() <- returns a detection // int fdsf = detection.getCenter().x <- x value on the screen between -50,50 - double PropDetection = robot.camera.getProp().getCenter().x; + double PropDetection = robot.camera.getProp().getCenter().x; if (PropDetection <= -DetectionTest ) { teamPropLocation = 1; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontPreload.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontPreload.java index bc8a98b..1388eee 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontPreload.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontPreload.java @@ -125,11 +125,11 @@ public class BlueFrontPreload extends AutoBase { case (2): //team prop location 2 builder.lineToLinearHeading(STACK_LOCATION_2.plus(new Pose2d(-5.8,2))).waitSeconds(.01); - + break; case (3): //team prop location 3 builder.lineToLinearHeading(STACK_LOCATION_3.plus(new Pose2d(-4.8,2))).waitSeconds(.01); - + break; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontPreload.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontPreload.java index 58610e0..bdeebf3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontPreload.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontPreload.java @@ -11,6 +11,7 @@ import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK6; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; @@ -19,13 +20,13 @@ import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySe import org.firstinspires.ftc.teamcode.util.CenterStageCommon; @Config -@Autonomous(name = "Red FrontPreload (2+1)", group = "Competition", preselectTeleOp = "Main TeleOp") +@Autonomous(name = "Red FrontPreload (2+1)", group = "Competition", preselectTeleOp = "Red Teleop") public class RedFrontPreload extends AutoBase { public static final Pose2d DROP_1_PART_2 = new Pose2d(-33, -33.5, Math.toRadians(180)); public static final Pose2d DROP_1 = new Pose2d(-33,-33.5,Math.toRadians(180)); public static final Pose2d DROP_2 = new Pose2d(-35.7, -33.5, Math.toRadians(90)); - public static final Pose2d DROP_2_PART_2 = new Pose2d(-35.7,-41, Math.toRadians(90)); + public static final Pose2d DROP_2_PART_2 = new Pose2d(-37.7,-42, Math.toRadians(90)); public static final Pose2d DROP_3 = new Pose2d(-51, -50.5, Math.toRadians(90)); public static final Pose2d DROP_1M = new Pose2d(-36, -45, Math.toRadians(90)); @@ -35,13 +36,13 @@ public class RedFrontPreload extends AutoBase { public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, -38.3, Math.toRadians(180));//187 - public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(50, -32, Math.toRadians(187));//187 + public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(47, -32, Math.toRadians(187));//187 public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(55, -25, Math.toRadians(180));//817 public static final Pose2d STACK_LOCATION_1 = new Pose2d(-54.5, -32.6, Math.toRadians(180)); - public static final Pose2d STACK_LOCATION_2 = new Pose2d(-55, -40, Math.toRadians(180)); + public static final Pose2d STACK_LOCATION_2 = new Pose2d(-57, -37, Math.toRadians(180)); public static final Pose2d STACK_LOCATION_3 = new Pose2d(-52, -29.6, Math.toRadians(180)); @@ -51,7 +52,7 @@ public class RedFrontPreload extends AutoBase { public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-40, -58.5, Math.toRadians(0)); - public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(12, -58.5, Math.toRadians(0)); + public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(8, -58.5, Math.toRadians(0)); public static final Pose2d PARK_1 = new Pose2d(45,-58,Math.toRadians(180)); @@ -124,11 +125,11 @@ public class RedFrontPreload extends AutoBase { case (2): //team prop location 2 builder.lineToLinearHeading(STACK_LOCATION_2).waitSeconds(.01); - + break; case (3): //team prop location 3 builder.lineToLinearHeading(STACK_LOCATION_3.plus(new Pose2d(-4.8,-2))).waitSeconds(.01); - + break; } @@ -165,7 +166,7 @@ public class RedFrontPreload extends AutoBase { builder.setTangent(Math.toRadians(-90)); builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(180)); builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); - builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(180)); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec().plus(new Vector2d((3.8))),Math.toRadians(180)); break; case 3: builder.setTangent(Math.toRadians(-90)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontStageme.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontStageme.java new file mode 100644 index 0000000..2e12a8b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontStageme.java @@ -0,0 +1,304 @@ +package org.firstinspires.ftc.teamcode.opmode.autonomous; + +import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE; +import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN; +import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK1; +import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK2; +import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK3; +import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK4; +import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK5; +import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK6; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + + +import org.firstinspires.ftc.teamcode.hardware.Slides; +import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder; +import org.firstinspires.ftc.teamcode.util.CenterStageCommon; + +@Config +@Autonomous(name = "Red FrontPreload (2+2)", group = "Competition", preselectTeleOp = "Red Teleop") +public class RedFrontStageme extends AutoBase { + public static final Pose2d DROP_1_PART_2 = new Pose2d(-33, -33.5, Math.toRadians(180)); + + public static final Pose2d DROP_1 = new Pose2d(-33,-33.5,Math.toRadians(180)); + public static final Pose2d DROP_2 = new Pose2d(-35.7, -33.5, Math.toRadians(90)); + + public static final Pose2d DROP_2_PART_2 = new Pose2d(-35.7,-41, Math.toRadians(90)); + public static final Pose2d DROP_2_PART_3 = new Pose2d(-40.7,-42, Math.toRadians(180)); + public static final Pose2d DROP_3 = new Pose2d(-51, -50.5, Math.toRadians(90)); + public static final Pose2d DROP_1M = new Pose2d(-36, -45, Math.toRadians(90)); + + public static final Pose2d DROP_2M = new Pose2d(-48.5, -30, Math.toRadians(90)); + + public static final Pose2d DROP_3M = new Pose2d(-53.7, -35.9, Math.toRadians(90)); + + public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, -38.3, Math.toRadians(180));//187 + + public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52, -32, Math.toRadians(187));//187 + + public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(55, -25, Math.toRadians(180));//817 + + public static final Pose2d STACK_LOCATION_1 = new Pose2d(-54.5, -32.6, Math.toRadians(180)); + + public static final Pose2d STACK_LOCATION_2 = new Pose2d(-57, -37, Math.toRadians(180)); + + public static final Pose2d STACK_LOCATION_3 = new Pose2d(-52, -29.6, Math.toRadians(180)); + + public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, -12.4, Math.toRadians(190));//-36 + + public static final Pose2d POST_DROP_POS = new Pose2d(-45, -59.5, Math.toRadians(0)); + + public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-40, -58.5, Math.toRadians(180)); + + public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(8, -58.5, Math.toRadians(180)); + + public static final Pose2d PARK_1 = new Pose2d(45,-58,Math.toRadians(180)); + + public static final Pose2d PARK_2 = new Pose2d(58,-58,Math.toRadians(180)); + + @Override + public void createTrajectories() { + // set start position + Pose2d start = new Pose2d(-36, -61.5, Math.toRadians(90)); + robot.drive.setPoseEstimate(start); + robot.camera.setAlliance(CenterStageCommon.Alliance.Red); + } + + @Override + public void followTrajectories() { + TrajectorySequenceBuilder builder = null; + switch (macroState) { + case 0: + builder = this.robot.getTrajectorySequenceBuilder(); + switch (teamPropLocation) { + case 1: + builder.lineToLinearHeading(DROP_1M); + builder.lineToLinearHeading(DROP_1); + break; + case 2: + builder.lineToLinearHeading(DROP_2); + break; + case 3: + builder.lineToLinearHeading(DROP_3M); +// builder.lineToLinearHeading(DROP_3); + break; + } + this.robot.drive.followTrajectorySequenceAsync(builder.build()); + macroState++; + break; + // DRIVE TO TAPE + case 1: + + // if drive is done move on + if (!robot.drive.isBusy()) { + macroTime = getRuntime(); + macroState++; + } + break; + // RUN INTAKE + case 2: + // intake + if (getRuntime() < macroTime + 0.1) { + } + else{ + builder = this.robot.getTrajectorySequenceBuilder(); + robot.intake.setDcMotor(0); + switch (teamPropLocation) { + case 1: + builder.lineToLinearHeading(DROP_1_PART_2); + break; + case 2: + builder.lineToLinearHeading(DROP_2_PART_2); + builder.lineToLinearHeading(DROP_2_PART_3); + break; + case 3: + builder.lineToLinearHeading(DROP_3); + break; + } + //robot.arm.setDoor(OPEN); + macroTime = getRuntime(); + this.robot.drive.followTrajectorySequenceAsync(builder.build()); + macroState++; + } + // if intake is done move on + break; + case 3: + // if drive is done move on + if (!robot.drive.isBusy()) { + macroTime = getRuntime(); + macroState++; + } + break; + + case 4: + if (getRuntime() > macroTime + 0.1) { + //robot.arm.setDoor(CLOSE); + robot.intake.setDcMotor(-0.1); + builder = this.robot.getTrajectorySequenceBuilder(); + + switch (teamPropLocation) { + case 1: + builder.setTangent(Math.toRadians(-90)); + builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); + builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0)); + break; + case 2: + builder.setTangent(Math.toRadians(-90)); + builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); + builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0)); + break; + case 3: + builder.setTangent(Math.toRadians(-90)); + builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); + builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0)); + break; + } + this.robot.drive.followTrajectorySequenceAsync(builder.build()); + macroState++; + macroTime = getRuntime(); + } + break; + case 5: + // if drive is done move on + if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) { + macroTime = getRuntime(); + robot.macroState = 0; + robot.extendMacro(Slides.mini_tier1-30, getRuntime()); + macroState++; + } + break; + //extending the macro and about to score + case 6: + if (robot.macroState != 0) { + robot.extendMacro(Slides.mini_tier1, getRuntime()); + } + if (robot.macroState == 0 && !robot.drive.isBusy()) { + robot.resetMacro(0, getRuntime()); + macroState++; + } + break; + + case 7: + robot.resetMacro(0,getRuntime()); + if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) { + builder = this.robot.getTrajectorySequenceBuilder(); + builder.splineToConstantHeading(PRE_DEPOSIT_POS.vec(),Math.toRadians(0)); + builder.lineToConstantHeading(POST_DROP_POS_PART2.vec()); + builder.splineToConstantHeading(STACK_LOCATION_2.vec(),Math.toRadians(0)); + this.robot.drive.followTrajectorySequenceAsync(builder.build()); + macroState++; + } + break; + + case 8: + robot.arm.setDoor(OPEN); + robot.intake.setDcMotor(0.44); + robot.intake.setpos(STACK5); + macroTime = getRuntime(); + macroState++; + break; + + case 9: + if (getRuntime() > macroTime + 0.06) { + robot.arm.setDoor(CLOSE); + robot.intake.setDcMotor(-0.5); + builder = this.robot.getTrajectorySequenceBuilder(); + + switch (teamPropLocation) { + case 1: + builder.setTangent(Math.toRadians(-90)); + builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); + builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0)); + break; + case 2: + builder.setTangent(Math.toRadians(-90)); + builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); + builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0)); + break; + case 3: + builder.setTangent(Math.toRadians(-90)); + builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); + builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0)); + break; + } + this.robot.drive.followTrajectorySequenceAsync(builder.build()); + macroState++; + macroTime = getRuntime(); + } + break; + + case 10: + // if drive is done move on + if (getRuntime() > macroTime + 2.4 || !robot.drive.isBusy()) { + macroTime = getRuntime(); + robot.macroState = 0; + robot.extendMacro(Slides.mini_tier1-30, getRuntime()); + macroState++; + } + break; + + case 11: + if (robot.macroState != 0) { + robot.extendMacro(Slides.mini_tier1, getRuntime()); + } + if (robot.macroState == 0 && !robot.drive.isBusy()) { + robot.resetMacro(0, getRuntime()); + macroState++; + } + break; + + //Park + + case 12: + robot.resetMacro(0, getRuntime()); + if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) { + builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(PARK_1); + this.robot.drive.followTrajectorySequenceAsync(builder.build()); + macroState++; + } + break; + + //Park + +// case 7: +// robot.resetMacro(0, getRuntime()); +// if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) { +// builder = this.robot.getTrajectorySequenceBuilder(); +// builder.lineToLinearHeading(PARK_1); +// this.robot.drive.followTrajectorySequenceAsync(builder.build()); +// macroState++; +// } +// break; +// +// case 8: +// // if drive is done move on +// if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) { +// macroState++; +// } +// break; +// +// case 9: +// robot.resetMacro(0, getRuntime()); +// if (robot.macroState == 0) { +// macroState = -1; +// } + + case 19: + robot.resetMacro(0, getRuntime()); + if (robot.macroState == 0) { + macroState = -1; + } + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/BlueTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/BlueTeleop.java index 2e52ab1..954c0b0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/BlueTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/BlueTeleop.java @@ -179,6 +179,9 @@ public class BlueTeleop extends OpMode { } + //Elbowpos + + From 37fd9eadf1a4d2b243c8b290f44e165ae6be934b Mon Sep 17 00:00:00 2001 From: ImposterVarunoverlord Date: Thu, 21 Mar 2024 16:19:01 -0500 Subject: [PATCH 5/9] sus 2+1 --- .../ftc/teamcode/hardware/Arm.java | 28 +++++++++------ .../teamcode/opmode/teleop/BlueTeleop.java | 35 ++++++++++++++----- .../ftc/teamcode/opmode/teleop/RedTeleop.java | 10 ++++++ 3 files changed, 53 insertions(+), 20 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java index ea3e619..f0427d1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java @@ -39,8 +39,8 @@ public class Arm { public static double armScore = 0.95; public static double armScore_highMacro = 0.95; - public static double wristStart = 0.29; - public static double wristScore = 0.5; + public static double wristStart = 0.8; + public static double wristScore = 0.31; public static double elbow_mid = 0.5; public static double elbow_right = 0.29; @@ -48,6 +48,8 @@ public class Arm { public static double wristScore_highMacro = 0.3; + public double counter; + public enum Position { INTAKE, SCORE, SCOREHI, SCORELEFT, SCORERIGHT } @@ -56,6 +58,10 @@ public class Arm { OPEN, CLOSE } + public enum Elbowpos{ + + } + public Arm(HardwareMap hardwareMap) { wristServo = hardwareMap.get(Servo.class, "Wrist"); doorServo = hardwareMap.get(Servo.class, "Door"); @@ -76,6 +82,7 @@ public class Arm { setWristPos(Position.INTAKE); setDoor(DoorPosition.CLOSE); + setElbowPos(1); } public void setArmPos(Position pos) { @@ -104,16 +111,15 @@ public class Arm { } } - public void setElbowPos (Position pos){ - if (pos == Position.INTAKE) { + public void setElbowPos (int counter){ + if (counter == 1) { elbowPos = elbow_mid; - } else if (pos == Position.SCORE) { - wristPos = elbow_mid; - } else if (pos == Position.SCORELEFT) { - wristPos = elbow_left; - } else if (pos == Position.SCORERIGHT) { - wristPos = elbow_right; - + } else if (counter == 2) { + elbowPos = elbow_mid; + } else if (counter == 3) { + elbowPos = elbow_left; + } else if (counter == 4) { + elbowPos = elbow_right; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/BlueTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/BlueTeleop.java index 954c0b0..0168741 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/BlueTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/BlueTeleop.java @@ -50,12 +50,13 @@ public class BlueTeleop extends OpMode { this.servos[0] = this.hardwareMap.get(Servo.class, "LeftIntake"); this.servos[1] = this.hardwareMap.get(Servo.class, "Wrist"); - this.servos[2] = this.hardwareMap.get(Servo.class, "Door"); + this.servos[2] = this.hardwareMap.get(Servo.class, "Elbow"); this.servos[3] = this.hardwareMap.get(Servo.class, "RightArm"); this.servos[4] = this.hardwareMap.get(Servo.class, "LeftArm"); this.servos[5] = this.hardwareMap.get(Servo.class, "Right Intake Servo"); this.servos[6] = this.hardwareMap.get(Servo.class, "Drone"); + controller1 = new Controller(gamepad1); controller2 = new Controller(gamepad2); @@ -179,10 +180,14 @@ public class BlueTeleop extends OpMode { } - //Elbowpos - - - + //ElbowPos + if (controller2.getDRight().isPressed()){ + robot.arm.setElbowPos(4); + } else if (controller2.getDLeft().isPressed()) { + robot.arm.setElbowPos(3); + }else { + robot.arm.setElbowPos(1); + } // macros @@ -190,16 +195,28 @@ public class BlueTeleop extends OpMode { case (0): // manual mode robot.slides.increaseTarget(controller2.getLeftStick().getY()); + +// if (robot.intake.getPower() >= 0.01) { +// robot.arm.setDoor(OPEN); +// } else if (robot.intake.getPower() <= -0.01) { +// robot.arm.setDoor(OPEN); +// } else if (controller2.getLeftBumper().isPressed()) { +// robot.arm.setDoor(Arm.DoorPosition.OPEN); +// } else { +// robot.arm.setDoor(CLOSE); +// } + if (robot.intake.getPower() >= 0.01) { - robot.arm.setDoor(OPEN); + robot.intake.wheel_swallow(); } else if (robot.intake.getPower() <= -0.01) { - robot.arm.setDoor(OPEN); + robot.intake.wheel_swallow(); } else if (controller2.getLeftBumper().isPressed()) { - robot.arm.setDoor(Arm.DoorPosition.OPEN); + robot.intake.wheel_swallow(); } else { - robot.arm.setDoor(CLOSE); + robot.intake.wheel_stop(); } + //Start A/B stuff if (controller2.getX().isJustPressed()) { robot.runningMacro = 1; } else if (controller2.getY().isJustPressed()) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java index be45263..e6b4620 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java @@ -188,6 +188,7 @@ public class RedTeleop extends OpMode { case (0): // manual mode robot.slides.increaseTarget(controller2.getLeftStick().getY()); + if (robot.intake.getPower() >= 0.01) { robot.arm.setDoor(OPEN); } else if (robot.intake.getPower() <= -0.01) { @@ -197,6 +198,15 @@ public class RedTeleop extends OpMode { } else { robot.arm.setDoor(CLOSE); } + //Elbowpos + if (controller2.getDRight().isPressed()){ + robot.arm.setElbowPos(4); + } else if (controller2.getDLeft().isPressed()) { + robot.arm.setElbowPos(3); + }else { + robot.arm.setElbowPos(1); + } + if (controller2.getX().isJustPressed()) { robot.runningMacro = 1; From aba3c58fd48299486f40ae54d60d0d88b31e59de Mon Sep 17 00:00:00 2001 From: ImposterVarunoverlord Date: Thu, 21 Mar 2024 17:12:54 -0500 Subject: [PATCH 6/9] sus 2+1 --- .../ftc/teamcode/hardware/Arm.java | 1 + .../ftc/teamcode/opmode/teleop/RedTeleop.java | 24 +++++++++---------- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java index f0427d1..c8e1b06 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java @@ -166,5 +166,6 @@ public class Arm { doorServo.setPosition(doorPos); wristServo.setPosition(wristPos); + elbow.setPosition(elbowPos); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java index e6b4620..00d7a23 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java @@ -189,21 +189,21 @@ public class RedTeleop extends OpMode { robot.slides.increaseTarget(controller2.getLeftStick().getY()); - if (robot.intake.getPower() >= 0.01) { - robot.arm.setDoor(OPEN); - } else if (robot.intake.getPower() <= -0.01) { - robot.arm.setDoor(OPEN); - } else if (controller2.getLeftBumper().isPressed()) { - robot.arm.setDoor(Arm.DoorPosition.OPEN); - } else { - robot.arm.setDoor(CLOSE); - } +// if (robot.intake.getPower() >= 0.01) { +// robot.arm.setDoor(OPEN); +// } else if (robot.intake.getPower() <= -0.01) { +// robot.arm.setDoor(OPEN); +// } else if (controller2.getLeftBumper().isPressed()) { +// robot.arm.setDoor(Arm.DoorPosition.OPEN); +// } else { +// robot.arm.setDoor(CLOSE); +// } //Elbowpos - if (controller2.getDRight().isPressed()){ + if (controller2.getDRight().isJustPressed()){ robot.arm.setElbowPos(4); - } else if (controller2.getDLeft().isPressed()) { + } else if (controller2.getDLeft().isJustPressed()) { robot.arm.setElbowPos(3); - }else { + }else if(controller2.getDUp().isJustPressed()) { robot.arm.setElbowPos(1); } From 96bc1442533a825cc26c9a0dcee55ee685c04a52 Mon Sep 17 00:00:00 2001 From: ImposterVarunoverlord Date: Thu, 21 Mar 2024 17:13:43 -0500 Subject: [PATCH 7/9] 9 --- .../org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java index 00d7a23..622681f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java @@ -198,7 +198,7 @@ public class RedTeleop extends OpMode { // } else { // robot.arm.setDoor(CLOSE); // } - //Elbowpos + //Elbowpo if (controller2.getDRight().isJustPressed()){ robot.arm.setElbowPos(4); } else if (controller2.getDLeft().isJustPressed()) { From d219175b4c44b2caae5553eabafaf86ce92905ab Mon Sep 17 00:00:00 2001 From: ImposterVarunoverlord Date: Thu, 21 Mar 2024 17:28:23 -0500 Subject: [PATCH 8/9] uPDATE!!!!! --- .../org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java index 622681f..00d7a23 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java @@ -198,7 +198,7 @@ public class RedTeleop extends OpMode { // } else { // robot.arm.setDoor(CLOSE); // } - //Elbowpo + //Elbowpos if (controller2.getDRight().isJustPressed()){ robot.arm.setElbowPos(4); } else if (controller2.getDLeft().isJustPressed()) { From 70e37ea5326e2bb7404d48ac97649637595c9576 Mon Sep 17 00:00:00 2001 From: ImposterVarunoverlord Date: Thu, 21 Mar 2024 17:29:17 -0500 Subject: [PATCH 9/9] uPDATE!!!!! --- .../org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java index 00d7a23..1a048f0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java @@ -198,7 +198,7 @@ public class RedTeleop extends OpMode { // } else { // robot.arm.setDoor(CLOSE); // } - //Elbowpos + //Elbowpos( if (controller2.getDRight().isJustPressed()){ robot.arm.setElbowPos(4); } else if (controller2.getDLeft().isJustPressed()) {