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(); + + + } +}