diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HWI.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HWI.java new file mode 100644 index 000000000000..05d264f3ff8c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HWI.java @@ -0,0 +1,127 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.hardware.TouchSensor; +import com.qualcomm.robotcore.util.RobotLog; + +import java.util.ArrayList; +import java.util.Arrays; + +public class HWI { //HWI = Hardware Interface + private LinearOpMode ExternalLinOp; + private OpMode ExternalOp; + private boolean isLinear; + + private DcMotor leftBack, rightBack, leftFront, rightFront, leftLauncher, rightLauncher, intakeMotor, lift; + private CRServo launchLiftRight, launchLiftLeft; + private Servo scoop, turnTableServo, backDoor, frontDoor; + private GoBildaPinpointDriver pinpoint; + private Limelight3A limelight; + @SuppressWarnings("FieldCanBeLocal") + private TouchSensor topBump, bottomBump, intakeBump1, intakeBump2; + private ArrayList leds; + private CRServo LED1; + + //HWI name = new HWI(this); + public HWI (OpMode opMode, LinearOpMode linOpMode) { + if (linOpMode == null & opMode != null) { + ExternalOp = opMode; + isLinear = false; + } else if (linOpMode != null) { + ExternalLinOp = linOpMode; + isLinear = true; + } else { + RobotLog.ii("HWI", "OpMode not provided, please check HWI's initialization statement."); + } + } + + public void init() { + if (isLinear) { + + } else { + //Drive Definitions + leftBack = ExternalOp.hardwareMap.get(DcMotor.class, "leftBack"); + rightBack = ExternalOp.hardwareMap.get(DcMotor.class, "rightBack"); + leftFront = ExternalOp.hardwareMap.get(DcMotor.class, "leftFront"); + rightFront = ExternalOp.hardwareMap.get(DcMotor.class, "rightFront"); + //Intake Definitions + intakeMotor = ExternalOp.hardwareMap.get(DcMotor.class, "intakeMotor"); + frontDoor = ExternalOp.hardwareMap.get(Servo.class, "goofyAhhhhFrontDoor"); + intakeBump1 = ExternalOp.hardwareMap.get(TouchSensor.class, "intakeBump1"); + intakeBump2 = ExternalOp.hardwareMap.get(TouchSensor.class, "intakeBump2"); + //Launcher Definitions + leftLauncher = ExternalOp.hardwareMap.get(DcMotor.class, "leftLauncher"); + rightLauncher = ExternalOp.hardwareMap.get(DcMotor.class, "rightLauncher"); + launchLiftRight = ExternalOp.hardwareMap.get(CRServo.class, "launchLiftRight"); + launchLiftLeft = ExternalOp.hardwareMap.get(CRServo.class, "launchLiftLeft"); + topBump = ExternalOp.hardwareMap.get(TouchSensor.class, "TopBump"); + bottomBump = ExternalOp.hardwareMap.get(TouchSensor.class, "BottomBump"); + backDoor = ExternalOp.hardwareMap.get(Servo.class, "backDoor"); + scoop = ExternalOp.hardwareMap.get(Servo.class, "scoop"); + //Lift/Skis Definition + lift = ExternalOp.hardwareMap.get(DcMotor.class, "lift"); + //Turntable Definition + turnTableServo = ExternalOp.hardwareMap.get(Servo.class, "turnTableServo"); + //Pinpoint Definition + pinpoint = ExternalOp.hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint"); + //LED Definition + LED1 = ExternalOp.hardwareMap.get(CRServo.class, "Led1"); + //Limelight Definition + limelight = ExternalOp.hardwareMap.get(Limelight3A.class, "limelight"); + //Drive Config + leftBack.setDirection(DcMotor.Direction.FORWARD); + rightBack.setDirection(DcMotor.Direction.FORWARD); + leftFront.setDirection(DcMotor.Direction.REVERSE); + rightFront.setDirection(DcMotor.Direction.FORWARD); + rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + //Intake Config + intakeMotor.setDirection(DcMotor.Direction.REVERSE); + //Launcher Config + leftLauncher.setDirection(DcMotor.Direction.REVERSE); + rightLauncher.setDirection(DcMotor.Direction.FORWARD); + leftLauncher.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER); + rightLauncher.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER); + leftLauncher.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightLauncher.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + launchLiftRight.setDirection(CRServo.Direction.REVERSE); + launchLiftLeft.setDirection(CRServo.Direction.FORWARD); + //Lift/Skis Config + lift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + //Odometry Config + pinpoint.initialize(); //Initializes odometry for use in code + pinpoint.update(); + //LED Config + leds = new ArrayList<>(Arrays.asList(null, LED1)); //creates a list of LEDs for ledManager to use + //Limelight Config/Setup + limelight.pipelineSwitch(0); //Sets the config the limelight should use + limelight.setPollRateHz(100); //Limelight data polling rate + limelight.start(); //Initializes limelight for use in code + } + } + public void drive(double linear, double lateral, double rotational, double speed) { + speed = Utils.clamp(speed, 0, 1); + leftFront.setPower(((linear + lateral) - rotational) * speed); + leftBack.setPower((linear - lateral - rotational) * speed); + rightFront.setPower(((linear + lateral) + rotational) * speed); + rightBack.setPower(((linear - lateral) + rotational) * speed); + } + private static class Utils{ + private static double clamp(double value, double min, double max){ + if (value < min) { + return min; + } else if (value > max) { + return max; + } else return value; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HWIExampleImlementation.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HWIExampleImlementation.java new file mode 100644 index 000000000000..ea7b0eb91197 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HWIExampleImlementation.java @@ -0,0 +1,19 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +@TeleOp +public class HWIExampleImlementation extends OpMode { + HWI robot = new HWI(this, null); + + @Override + public void init() { + robot.init(); + } + + @Override + public void loop() { + robot.drive(gamepad1.right_stick_y, gamepad1.right_stick_x, gamepad1.left_stick_x, 0.7); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MMHS26Lib.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MMHS26Lib.java new file mode 100644 index 000000000000..c942e88f3009 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MMHS26Lib.java @@ -0,0 +1,1244 @@ +package org.firstinspires.ftc.teamcode; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.SequentialAction; +import com.acmerobotics.roadrunner.TrajectoryActionBuilder; +import com.acmerobotics.roadrunner.Vector2d; +import com.acmerobotics.roadrunner.ftc.Actions; + +import com.bylazar.configurables.annotations.Configurable; +import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.hardware.TouchSensor; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.RobotLog; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; +import org.firstinspires.ftc.teamcode.roadRunner.MecanumDrive; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +@SuppressWarnings({"unused", "UnusedReturnValue"}) +public class MMHS26Lib { + //Hardware variables + private static DcMotor leftBack, rightBack, leftFront, rightFront, leftLauncher, rightLauncher, intakeMotor, lift; + private static CRServo launchLiftRight, launchLiftLeft; + private static Servo scoop, turnTableServo, backDoor, frontDoor; + private static GoBildaPinpointDriver pinpoint; + private static Limelight3A limelight; + @SuppressWarnings("FieldCanBeLocal") + private static TouchSensor topBump, bottomBump, intakeBump1, intakeBump2; + private static ArrayList leds; + + //Constants + public static final double ticPerIn = 254.7; + public static ElapsedTime runtime26Lib = new ElapsedTime(); + + + //Internal variables + private static HardwareMap hwMap; + private static Pose2d startPose; + private static Telemetry telemetry; + + //External Variables + public static int count = 0; + + public MMHS26Lib(HardwareMap hardwareMap, Pose2d initPose, Telemetry initTelemetry) { + //Drive Definitions + leftBack = hardwareMap.get(DcMotor.class, "leftBack"); + rightBack = hardwareMap.get(DcMotor.class, "rightBack"); + leftFront = hardwareMap.get(DcMotor.class, "leftFront"); + rightFront = hardwareMap.get(DcMotor.class, "rightFront"); + //Intake Definitions + intakeMotor = hardwareMap.get(DcMotor.class, "intakeMotor"); + frontDoor = hardwareMap.get(Servo.class, "goofyAhhhhFrontDoor"); + intakeBump1 = hardwareMap.get(TouchSensor.class, "intakeBump1"); + intakeBump2 = hardwareMap.get(TouchSensor.class, "intakeBump2"); + //Launcher Definitions + leftLauncher = hardwareMap.get(DcMotor.class, "leftLauncher"); + rightLauncher = hardwareMap.get(DcMotor.class, "rightLauncher"); + launchLiftRight = hardwareMap.get(CRServo.class, "launchLiftRight"); + launchLiftLeft = hardwareMap.get(CRServo.class, "launchLiftLeft"); + topBump = hardwareMap.get(TouchSensor.class, "TopBump"); + bottomBump = hardwareMap.get(TouchSensor.class, "BottomBump"); + backDoor = hardwareMap.get(Servo.class, "backDoor"); + scoop = hardwareMap.get(Servo.class, "scoop"); + //Lift/Skis Definition + lift = hardwareMap.get(DcMotor.class, "lift"); + //Turntable Definition + turnTableServo = hardwareMap.get(Servo.class, "turnTableServo"); + //Drive Config + leftBack.setDirection(DcMotor.Direction.FORWARD); + rightBack.setDirection(DcMotor.Direction.FORWARD); + leftFront.setDirection(DcMotor.Direction.REVERSE); + rightFront.setDirection(DcMotor.Direction.FORWARD); + rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + //Intake Config + intakeMotor.setDirection(DcMotor.Direction.REVERSE); + //Launcher Config + leftLauncher.setDirection(DcMotor.Direction.REVERSE); + rightLauncher.setDirection(DcMotor.Direction.FORWARD); + leftLauncher.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER); + rightLauncher.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER); + leftLauncher.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightLauncher.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + launchLiftRight.setDirection(CRServo.Direction.REVERSE); + launchLiftLeft.setDirection(CRServo.Direction.FORWARD); + //Lift/Skis Config + lift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + //Odometry Config + pinpoint = hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint"); + pinpoint.initialize(); //Initializes odometry for use in code + pinpoint.update(); + pinpoint.setPosition(new Pose2D(DistanceUnit.INCH, initPose.position.x, initPose.position.y, AngleUnit.DEGREES, Math.toDegrees(initPose.heading.toDouble()))); + pinpoint.setHeading(initPose.heading.toDouble(), AngleUnit.RADIANS); + pinpoint.update(); + //LED Config + CRServo LED1 = hardwareMap.get(CRServo.class, "Led1"); + leds = new ArrayList<>(Arrays.asList(null, LED1)); //creates a list of LEDs for ledManager to use + //Limelight Config/Setup + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + limelight.pipelineSwitch(0); //Sets the config the limelight should use + limelight.setPollRateHz(100); //Limelight data polling rate + limelight.start(); //Initializes limelight for use in code + + //Internal Data (DO NOT TOUCH) + hwMap = hardwareMap; + initTelemetry.addData("HW Map Initialized", hwMap); + initTelemetry.update(); + + startPose = initPose; + initTelemetry.addData("Start Pose Initialized", initPose); + initTelemetry.update(); + + telemetry = initTelemetry; + initTelemetry.addData("Internal Telemetry Initialized", hwMap); + initTelemetry.update(); + + telemetry.addData("Initialization Finished", true); + telemetry.update(); + } + + //Optional flag(s) to enable internal debugging tools + @Config + @Configurable + public static class debug { + //debug telemetry has to be static to appear on ftc dashboard + public static boolean debugTelemetry = false; + } + + //Sleep function taken from LinearOpMode as extending LinearOpMode doesn't work on static classes + private static void sleep(long milliseconds) {try {Thread.sleep(milliseconds);} catch (InterruptedException e) {Thread.currentThread().interrupt();}} + + //Gets the robots current position on the field + public static Pose2d currentPose() { + pinpoint.update(); + return new Pose2d(new Vector2d(pinpoint.getPosX(DistanceUnit.INCH) + startPose.position.x, pinpoint.getPosY(DistanceUnit.INCH) + startPose.position.y), pinpoint.getHeading(AngleUnit.RADIANS) + startPose.heading.toDouble()); + } + public static class conversions { + public conversions() {super();} + + public static class pose { + public pose() {super();} + + //Converts FTC Pose2D to RoadRunner Pose2d + public static Pose2d Pose2DToPose2d(Pose2D Pose2D) {return new Pose2d(new Vector2d(Pose2D.getX(DistanceUnit.INCH), Pose2D.getY(DistanceUnit.INCH)), Pose2D.getHeading(AngleUnit.DEGREES));} + + //Converts RoadRunner Pose2d to FTC Pose2D + public static Pose2D Pose2dToPose2D(Pose2d Pose2d) {return new Pose2D(DistanceUnit.INCH, Pose2d.position.x, Pose2d.position.y, AngleUnit.DEGREES, Pose2d.heading.log());} + + //Converts FTC Pose3D to FTC Pose2D + public static Pose2D Pose3DToPose2D(Pose3D pose3D) {return new Pose2D(pose3D.getPosition().unit, pose3D.getPosition().x, pose3D.getPosition().y, AngleUnit.DEGREES, pose3D.getOrientation().getYaw(AngleUnit.DEGREES));} + + //Converts FTC Pose3D to RoadRunner Pose2d + public static Pose2d Pose3DtoPose2d(Pose3D pose3D) {return Pose2DToPose2d(Pose3DToPose2D(pose3D));} + } + public static class units { + public units() {super();} + + //Converts millimeters to inches + public static double millimeterToInch(double millimeter) {return (millimeter / 25.4);} + + //Converts inches to millimeters + public static double inchToMillimeter(double inch) {return (inch * 25.4);} + } + } + + //Class for managing basic functions relating to movement + public static class motion { + public motion() {super();} + public static void mecanumDrive(double X, double Y, double R, double maxSpeed, long T){ + /// X, x movement; Y, y movement; R, turning; and maxSpeed all must be considered as percentages with T representing the time in milliseconds that the robot should move + //Ensures Y is within the range of -1 to 1 + double controlY = Y; + if (controlY > 1){ + controlY = 1; + } else if (controlY < 0){ + controlY = 0; + } + //Ensures X is within the range of -1 to 1 + double controlX = -X; + if (controlX > 1){ + controlX = 1; + } else if (controlX < 0){ + controlX = 0; + } + //Ensures R is within the range of -1 to 1 + double controlRX = -R; + if (controlRX > 1){ + controlRX = 1; + } else if (controlRX < 0){ + controlRX = 0; + } + //Ensures maxSpeed is within the range of -1 to 1 + if (maxSpeed > 1){ + maxSpeed = 1; + } else if (maxSpeed < 0){ + maxSpeed = 0; + } + //Ensures T is not less than zero + if (T < 0) { + T = 0; + } + + leftFront.setPower(((controlY - controlX) + controlRX) * maxSpeed); + leftBack.setPower((controlY + controlX + controlRX) * maxSpeed); + rightFront.setPower(((controlY - controlX) - controlRX) * maxSpeed); + rightBack.setPower(((controlY + controlX) - controlRX) * maxSpeed); + sleep(T); + halt(); + } + //DO NOT USE STRAFE UNLESS NEEDED + public static void strafeLeft(double Speed, long time) { + leftBack.setPower(Speed); + leftFront.setPower(-Speed); + rightBack.setPower(Speed); + rightFront.setPower(-Speed); + sleep(time); + halt(); + } + //DO NOT USE STRAFE UNLESS NEEDED + public static void strafeRight(double Speed, long time) { + leftBack.setPower(-Speed); + leftFront.setPower(Speed); + rightBack.setPower(-Speed); + rightFront.setPower(Speed); + sleep(time); + halt(); + } + + public static void turnRight(double Speed, long time) { + leftBack.setPower(Speed); + leftFront.setPower(Speed); + rightBack.setPower(Speed); + rightFront.setPower(Speed); + sleep(time); + halt(); + } + + public static void turnLeft(double Speed, long time) { + leftBack.setPower(-Speed); + leftFront.setPower(-Speed); + rightBack.setPower(-Speed); + rightFront.setPower(-Speed); + sleep(time); + halt(); + } + + public static void moveBackward(double Speed, long time) { + leftBack.setPower(-Speed); + leftFront.setPower(-Speed); + rightBack.setPower(Speed); + rightFront.setPower(Speed); + sleep(time); + halt(); + } + + public static void moveForward(double Speed, long time) { + leftBack.setPower(Speed); + leftFront.setPower(Speed); + rightBack.setPower(-Speed); + rightFront.setPower(-Speed); + sleep(time); + halt(); + } + + public static void halt() { + leftBack.setPower(0); + leftFront.setPower(0); + rightBack.setPower(0); + rightFront.setPower(0); + } + } + + //Class for limelight functions + public static class Limelight { + public Limelight() {super();} + + //moves to a predetermined point on the field + public static void localizer(double localPower, int sleepTime, boolean updatePose) { + //loops until escaped + while (true) { + LLResult result = limelight.getLatestResult(); + //vertical min & max of the april tag + double txMax = 14.700; + double txMin = 14.400; + /* + not currently used, ty would be used for strafing + double tyMax = 13.5; + double tyMin = 12; + */ + //min & max % of the camera that the april tag takes up + double taMax = .88; + double taMin = .91; + //current values of tx, ty, ta + double tx; + double ty; + double ta; + //ensures non-null results + if (result != null && result.isValid()) { + tx = result.getTx(); // How far left or right the target is (degrees) + ty = result.getTy(); // How far up or down the target is (degrees) + ta = result.getTa(); // How big the target looks (0%-100% of the image) + if (debug.debugTelemetry) { + telemetry.addData("Target X", tx); + telemetry.addData("Target Y", ty); + telemetry.addData("Target Area", ta); + telemetry.update(); + } + if (tx < txMin) { + // turn right + motion.turnRight(localPower, sleepTime); + } else if (tx > txMax) { + //turn left + motion.turnLeft(localPower, sleepTime); + } else if (ta > taMax) { + //move backward + motion.moveBackward(localPower, sleepTime); + } else if (ta < taMin) { + //move forward + motion.moveForward(localPower, sleepTime); + } else { + break; + } + poseLimelight(updatePose); + } else { + if (debug.debugTelemetry) { + telemetry.addData("Limelight", "No Targets"); + telemetry.update(); + } + //stop movement + motion.halt(); + } + } + } + + //Outputs a list of AprilTag IDs that the limelight can see + public static int processLimeLightMotif() { + //determines whether to run the function + boolean processTrig = true; + //array and individual ids + int id; + ArrayList IDs = new ArrayList<>(); + int Motif; + //current values of tx, ty, ta + double tx; + double ty; + double ta; + //processing trig allows loop to escape early or ends if the loop has exceeded 1000 iterations + while (count <= 1000 && processTrig) { + LLResult result = limelight.getLatestResult(); + //ensures non-null results + if (result != null && result.isValid()) { + // Get the list of all AprilTags + List fiducialList = result.getFiducialResults(); + + tx = result.getTx(); // How far left or right the target is (degrees) + ty = result.getTy(); // How far up or down the target is (degrees) + ta = result.getTa(); // How big the target looks (0%-100% of the image) + + if (debug.debugTelemetry) { + telemetry.addData("Target X", tx); + telemetry.addData("Target Y", ty); + telemetry.addData("Target Area", ta); + telemetry.update(); + } + //checks if there are april tags + if (!fiducialList.isEmpty()) { + if (debug.debugTelemetry) { + telemetry.addData("Detections Found", fiducialList.size()); + telemetry.update(); + } + // Iterate through each detected tag + for (LLResultTypes.FiducialResult fiducial : fiducialList) { + id = fiducial.getFiducialId(); + IDs.add(id); + if (debug.debugTelemetry) { + telemetry.addData("Tag ID", id); + telemetry.update(); + } + //early escape + processTrig = false; + } + } else { + if (debug.debugTelemetry) { + telemetry.addData("Detections Found", "None"); + telemetry.update(); + } + } + } else { + if (debug.debugTelemetry) { + telemetry.addData("Limelight Data", "Invalid or Stale"); + assert result != null; + telemetry.addData("Staleness", result.getStaleness()); + telemetry.update(); + } + } + if (debug.debugTelemetry) { + telemetry.update(); + } + //increments count + count++; + sleep(1); + } + //checks for motif ids + if (IDs.contains(21)) { + Motif = 1; + telemetry.addData("Motif", "GPP " + Motif); + } else if (IDs.contains(22)) { + Motif = 2; + telemetry.addData("Motif", "PGP " + Motif); + } else if (IDs.contains(23)) { + Motif = 3; + telemetry.addData("Motif", "PPG " + Motif); + } else { + Motif = 0; + telemetry.addData("Motif", "Check failed " + Motif); + } + telemetry.update(); + + return Motif; + } + public static Pose2d poseLimelight(boolean updatePose) { + LLResult result = limelight.getLatestResult(); + Pose3D pose; + //ensures non-null results + if (result != null && result.isValid()) { + pose = result.getBotpose(); + //gets robot's 3D position and orientation + telemetry.addData("X", (pose.getPosition().x * 39.37)); + telemetry.addData("Y", (pose.getPosition().y * 39.37)); + telemetry.addData("Rotation", pose.getOrientation().getYaw()); + telemetry.update(); + if (updatePose){ + pinpoint.setPosition(conversions.pose.Pose3DToPose2D(pose)); + } + //converts Pose3D to a Pose2d usable with RoadRunner + return new Pose2d(pose.getPosition().x * 39.37, pose.getPosition().y * 39.37, pose.getOrientation().getYaw() - 180); + } else { + //failsafe if pose can't be obtained + telemetry.addData("Limelight", "Failed to localize, defaulting to X:0 Y:0 θ:0"); + RobotLog.ii("Limelight", "Failed to localize, defaulting to X:0 Y:0 θ:0"); + RobotLog.addGlobalWarningMessage("Limelight", "Failed to localize, defaulting to X:0 Y:0 θ:0"); + if (updatePose) { + telemetry.addData("Pinpoint", "Pose Not Updated due to localizer failure"); + RobotLog.ii("Pinpoint", "Pose Not Updated due to localizer failure"); + RobotLog.addGlobalWarningMessage("Pinpoint", "Pose Not Updated due to localizer failure"); + } + telemetry.update(); + + return new Pose2d(0, 0, 0); + } + } + } + + //Class for functions relating to the autonomous pathing tool RoadRunner + public static class roadRunner { + public roadRunner() {super();} + + //Creates a curved path for the robot to automatically follow + public static class spline { + public spline() {super();} + //follows the arc of a circle when going to a 2d point + public static Pose2d splineTo(double x, double y, double tangent, Pose2d startingPose) { + + MecanumDrive drive = new MecanumDrive(hwMap, startingPose); + TrajectoryActionBuilder splineTo = drive.actionBuilder(startingPose) + .splineTo(new Vector2d(x, y), Math.toRadians(tangent)); + Actions.runBlocking( + new SequentialAction( + splineTo.build())); + + return (currentPose()); + } + //follows the arc of a circle when going to a 2d point while facing a single angle + public static Pose2d splineToConstantHeading(double x, double y, double tangent, Pose2d startingPose) { + MecanumDrive drive = new MecanumDrive(hwMap, startingPose); + TrajectoryActionBuilder splineToConstantHeading = drive.actionBuilder(startingPose) + .splineToConstantHeading(new Vector2d(x, y), Math.toRadians(tangent)); + Actions.runBlocking( + new SequentialAction( + splineToConstantHeading.build())); + + return (currentPose()); + } + //follows the arc of a circle when going to a 2d point while allowing for an ending angle, similar to if splineTo and turnTo are chained + public static Pose2d splineToLinearHeading(double x, double y, double angle, double tangent, Pose2d startingPose) { + MecanumDrive drive = new MecanumDrive(hwMap, startingPose); + TrajectoryActionBuilder splineToLinearHeading = drive.actionBuilder(startingPose) + .splineToLinearHeading(new Pose2d(new Vector2d(x, y), angle), tangent); + Actions.runBlocking( + new SequentialAction( + splineToLinearHeading.build())); + + return currentPose(); + } + //follows the arc of a circle when going to a 2d point while always facing a user determined angle + public static Pose2d splineToSplineHeading(double x, double y, double angle, double tangent, Pose2d startingPose) { + MecanumDrive drive = new MecanumDrive(hwMap, startingPose); + TrajectoryActionBuilder splineToSplineHeading = drive.actionBuilder(startingPose) + .splineToSplineHeading(new Pose2d(new Vector2d(x, y), angle), tangent); + Actions.runBlocking( + new SequentialAction( + splineToSplineHeading.build())); + + return (currentPose()); + } + } + + //Creates a horizontal path for the robot to move along + public static class strafe { + public strafe() {super();} + //strafes to a 2d point + public static Pose2d strafeTo(double x, double y, boolean VelCon, boolean AccCon, Pose2d startingPose) { + MecanumDrive drive = new MecanumDrive(hwMap, startingPose); + TrajectoryActionBuilder strafeTo; + if (VelCon && AccCon) { + strafeTo = drive.actionBuilder(startingPose) + .strafeTo(new Vector2d(x, y), drive.defaultVelConstraint, drive.defaultAccelConstraint); + } else if (VelCon) { + strafeTo = drive.actionBuilder(startingPose) + .strafeTo(new Vector2d(x, y), drive.defaultVelConstraint, null); + } else if (AccCon) { + strafeTo = drive.actionBuilder(startingPose) + .strafeTo(new Vector2d(x, y), null, drive.defaultAccelConstraint); + } else { + strafeTo = drive.actionBuilder(startingPose) + .strafeTo(new Vector2d(x, y)); + } + + Actions.runBlocking( + new SequentialAction( + strafeTo.build())); + + return (currentPose()); + } + //strafes to a 2d point while maintaining heading + public static Pose2d strafeToConstantHeading(double x, double y, boolean VelCon, boolean AccCon, Pose2d startingPose) { + MecanumDrive drive = new MecanumDrive(hwMap, startingPose); + TrajectoryActionBuilder strafeToConstantHeading; + if (VelCon && AccCon) { + strafeToConstantHeading = drive.actionBuilder(startingPose) + .strafeToConstantHeading(new Vector2d(x, y), drive.defaultVelConstraint, drive.defaultAccelConstraint); + } else if (VelCon) { + strafeToConstantHeading = drive.actionBuilder(startingPose) + .strafeToConstantHeading(new Vector2d(x, y), drive.defaultVelConstraint, null); + } else if (AccCon) { + strafeToConstantHeading = drive.actionBuilder(startingPose) + .strafeToConstantHeading(new Vector2d(x, y), null, drive.defaultAccelConstraint); + } else { + strafeToConstantHeading = drive.actionBuilder(startingPose) + .strafeToConstantHeading(new Vector2d(x, y)); + } + + Actions.runBlocking( + new SequentialAction( + strafeToConstantHeading.build())); + + return (currentPose()); + } + //strafes to a 2d point and ending at a user determined heading + public static Pose2d strafeToLinearHeading(double x, double y, double angle, boolean VelCon, boolean AccCon, Pose2d startingPose) { + MecanumDrive drive = new MecanumDrive(hwMap, startingPose); + TrajectoryActionBuilder strafeToLinearHeading; + if (VelCon && AccCon) { + strafeToLinearHeading = drive.actionBuilder(startingPose) + .strafeToLinearHeading(new Vector2d(x, y), Math.toRadians(angle), drive.defaultVelConstraint, drive.defaultAccelConstraint); + } else if (VelCon) { + strafeToLinearHeading = drive.actionBuilder(startingPose) + .strafeToLinearHeading(new Vector2d(x, y), Math.toRadians(angle), drive.defaultVelConstraint, null); + } else if (AccCon) { + strafeToLinearHeading = drive.actionBuilder(startingPose) + .strafeToLinearHeading(new Vector2d(x, y), Math.toRadians(angle), null, drive.defaultAccelConstraint); + } else { + strafeToLinearHeading = drive.actionBuilder(startingPose) + .strafeToLinearHeading(new Vector2d(x, y), Math.toRadians(angle)); + } + + Actions.runBlocking( + new SequentialAction( + strafeToLinearHeading.build())); + + return (currentPose()); + } + //strafes to a 2d point while facing a user determined heading + public static Pose2d strafeToSplineHeading(double x, double y, double angle, boolean VelCon, boolean AccCon, Pose2d startingPose) { + MecanumDrive drive = new MecanumDrive(hwMap, startingPose); + TrajectoryActionBuilder strafeToSplineHeading; + if (VelCon && AccCon) { + strafeToSplineHeading = drive.actionBuilder(startingPose) + .strafeToSplineHeading(new Vector2d(x, y), Math.toRadians(angle), drive.defaultVelConstraint, drive.defaultAccelConstraint); + } else if (VelCon) { + strafeToSplineHeading = drive.actionBuilder(startingPose) + .strafeToSplineHeading(new Vector2d(x, y), Math.toRadians(angle), drive.defaultVelConstraint, null); + } else if (AccCon) { + strafeToSplineHeading = drive.actionBuilder(startingPose) + .strafeToSplineHeading(new Vector2d(x, y), Math.toRadians(angle), null, drive.defaultAccelConstraint); + } else { + strafeToSplineHeading = drive.actionBuilder(startingPose) + .strafeToSplineHeading(new Vector2d(x, y), Math.toRadians(angle)); + } + + Actions.runBlocking( + new SequentialAction( + strafeToSplineHeading.build())); + + return (currentPose()); + } + } + + //takes a line to a point on a specified axis (x or y) + public static class lineTo { + public lineTo() {super();} + //takes a linear path on the x-axis to a given x value + public static Pose2d lineToX(double X, boolean VelCon, boolean AccCon, Pose2d startingPose) { + MecanumDrive drive = new MecanumDrive(hwMap, startingPose); + TrajectoryActionBuilder lineToX; + if (VelCon && AccCon) { + lineToX = drive.actionBuilder(startingPose) + .lineToX(X, drive.defaultVelConstraint, drive.defaultAccelConstraint); + } else if (VelCon) { + lineToX = drive.actionBuilder(startingPose) + .lineToX(X, drive.defaultVelConstraint, null); + } else if (AccCon) { + lineToX = drive.actionBuilder(startingPose) + .lineToX(X, null, drive.defaultAccelConstraint); + } else { + lineToX = drive.actionBuilder(startingPose) + .lineToX(X); + } + + Actions.runBlocking( + new SequentialAction( + lineToX.build())); + + return (currentPose()); + } + //takes a linear path on the x-axis to a given x value while maintaining its starting heading + public static Pose2d lineToXConstantHeading(double X, boolean VelCon, boolean AccCon, Pose2d startingPose) { + MecanumDrive drive = new MecanumDrive(hwMap, startingPose); + TrajectoryActionBuilder lineToXConstantHeading; + if (VelCon && AccCon) { + lineToXConstantHeading = drive.actionBuilder(startingPose) + .lineToXConstantHeading(X, drive.defaultVelConstraint, drive.defaultAccelConstraint); + } else if (VelCon) { + lineToXConstantHeading = drive.actionBuilder(startingPose) + .lineToXConstantHeading(X, drive.defaultVelConstraint, null); + } else if (AccCon) { + lineToXConstantHeading = drive.actionBuilder(startingPose) + .lineToXConstantHeading(X, null, drive.defaultAccelConstraint); + } else { + lineToXConstantHeading = drive.actionBuilder(startingPose) + .lineToXConstantHeading(X); + } + + Actions.runBlocking( + new SequentialAction( + lineToXConstantHeading.build())); + + return (currentPose()); + } + //takes a linear path on the x-axis to a given x value and ending at a specified heading + public static Pose2d lineToXLinearHeading(double X, double angle, boolean VelCon, boolean AccCon, Pose2d startingPose) { + MecanumDrive drive = new MecanumDrive(hwMap, startingPose); + TrajectoryActionBuilder lineToXLinearHeading; + if (VelCon && AccCon) { + lineToXLinearHeading = drive.actionBuilder(startingPose) + .lineToXLinearHeading(X, Math.toRadians(angle), drive.defaultVelConstraint, drive.defaultAccelConstraint); + } else if (VelCon) { + lineToXLinearHeading = drive.actionBuilder(startingPose) + .lineToXLinearHeading(X, Math.toRadians(angle), drive.defaultVelConstraint, null); + } else if (AccCon) { + lineToXLinearHeading = drive.actionBuilder(startingPose) + .lineToXLinearHeading(X, Math.toRadians(angle), null, drive.defaultAccelConstraint); + } else { + lineToXLinearHeading = drive.actionBuilder(startingPose) + .lineToXLinearHeading(X, Math.toRadians(angle)); + } + + Actions.runBlocking( + new SequentialAction( + lineToXLinearHeading.build())); + + return (currentPose()); + } + //takes a linear path on the x-axis to a given x value while facing a user defined direction + public static Pose2d lineToXSplineHeading(double X, double angle, boolean VelCon, boolean AccCon, Pose2d startingPose) { + MecanumDrive drive = new MecanumDrive(hwMap, startingPose); + TrajectoryActionBuilder lineToXSplineHeading; + if (VelCon && AccCon) { + lineToXSplineHeading = drive.actionBuilder(startingPose) + .lineToXSplineHeading(X, Math.toRadians(angle), drive.defaultVelConstraint, drive.defaultAccelConstraint); + } else if (VelCon) { + lineToXSplineHeading = drive.actionBuilder(startingPose) + .lineToXSplineHeading(X, Math.toRadians(angle), drive.defaultVelConstraint, null); + } else if (AccCon) { + lineToXSplineHeading = drive.actionBuilder(startingPose) + .lineToXSplineHeading(X, Math.toRadians(angle), null, drive.defaultAccelConstraint); + } else { + lineToXSplineHeading = drive.actionBuilder(startingPose) + .lineToXSplineHeading(X, Math.toRadians(angle)); + } + + Actions.runBlocking( + new SequentialAction( + lineToXSplineHeading.build())); + + return (currentPose()); + } + //takes a linear path on the y-axis to a given y value + public static Pose2d lineToY(double Y, boolean VelCon, boolean AccCon, Pose2d startingPose) { + MecanumDrive drive = new MecanumDrive(hwMap, startingPose); + TrajectoryActionBuilder lineToY; + if (VelCon && AccCon) { + lineToY = drive.actionBuilder(startingPose) + .lineToY(Y, drive.defaultVelConstraint, drive.defaultAccelConstraint); + } else if (VelCon) { + lineToY = drive.actionBuilder(startingPose) + .lineToY(Y, drive.defaultVelConstraint, null); + } else if (AccCon) { + lineToY = drive.actionBuilder(startingPose) + .lineToY(Y, null, drive.defaultAccelConstraint); + } else { + lineToY = drive.actionBuilder(startingPose) + .lineToY(Y); + } + + Actions.runBlocking( + new SequentialAction( + lineToY.build())); + + return (currentPose()); + } + //takes a linear path on the x-axis to a given x value while maintaining its starting heading + public static Pose2d lineToYConstantHeading(double Y, boolean VelCon, boolean AccCon, Pose2d startingPose) { + MecanumDrive drive = new MecanumDrive(hwMap, startingPose); + TrajectoryActionBuilder lineToYConstantHeading; + if (VelCon && AccCon) { + lineToYConstantHeading = drive.actionBuilder(startingPose) + .lineToYConstantHeading(Y, drive.defaultVelConstraint, drive.defaultAccelConstraint); + } else if (VelCon) { + lineToYConstantHeading = drive.actionBuilder(startingPose) + .lineToYConstantHeading(Y, drive.defaultVelConstraint, null); + } else if (AccCon) { + lineToYConstantHeading = drive.actionBuilder(startingPose) + .lineToYConstantHeading(Y, null, drive.defaultAccelConstraint); + } else { + lineToYConstantHeading = drive.actionBuilder(startingPose) + .lineToYConstantHeading(Y); + } + + Actions.runBlocking( + new SequentialAction( + lineToYConstantHeading.build())); + + return (currentPose()); + } + //takes a linear path on the x-axis to a given x value and ending at a specified heading + public static Pose2d lineToYLinearHeading(double Y, double angle, boolean VelCon, boolean AccCon, Pose2d startingPose) { + MecanumDrive drive = new MecanumDrive(hwMap, startingPose); + TrajectoryActionBuilder lineToYLinearHeading; + if (VelCon && AccCon) { + lineToYLinearHeading = drive.actionBuilder(startingPose) + .lineToYLinearHeading(Y, Math.toRadians(angle), drive.defaultVelConstraint, drive.defaultAccelConstraint); + } else if (VelCon) { + lineToYLinearHeading = drive.actionBuilder(startingPose) + .lineToYLinearHeading(Y, Math.toRadians(angle), drive.defaultVelConstraint, null); + } else if (AccCon) { + lineToYLinearHeading = drive.actionBuilder(startingPose) + .lineToYLinearHeading(Y, Math.toRadians(angle), null, drive.defaultAccelConstraint); + } else { + lineToYLinearHeading = drive.actionBuilder(startingPose) + .lineToYLinearHeading(Y, Math.toRadians(angle)); + } + + Actions.runBlocking( + new SequentialAction( + lineToYLinearHeading.build())); + + return (currentPose()); + } + //takes a linear path on the x-axis to a given x value while facing a user defined direction + public static Pose2d lineToYSplineHeading(double Y, double angle, boolean VelCon, boolean AccCon, Pose2d startingPose) { + MecanumDrive drive = new MecanumDrive(hwMap, startingPose); + TrajectoryActionBuilder lineToYSplineHeading; + if (VelCon && AccCon) { + lineToYSplineHeading = drive.actionBuilder(startingPose) + .lineToYSplineHeading(Y, Math.toRadians(angle), drive.defaultVelConstraint, drive.defaultAccelConstraint); + } else if (VelCon) { + lineToYSplineHeading = drive.actionBuilder(startingPose) + .lineToYSplineHeading(Y, Math.toRadians(angle), drive.defaultVelConstraint, null); + } else if (AccCon) { + lineToYSplineHeading = drive.actionBuilder(startingPose) + .lineToYSplineHeading(Y, Math.toRadians(angle), null, drive.defaultAccelConstraint); + } else { + lineToYSplineHeading = drive.actionBuilder(startingPose) + .lineToYSplineHeading(Y, Math.toRadians(angle)); + } + + Actions.runBlocking( + new SequentialAction( + lineToYSplineHeading.build())); + + return (currentPose()); + } + } + + //turns to a specified angle + public static Pose2d turnTo(double angle, Pose2d startingPose) { + MecanumDrive drive = new MecanumDrive(hwMap, startingPose); + TrajectoryActionBuilder turnTo = drive.actionBuilder(startingPose) + .turnTo(angle); + Actions.runBlocking( + new SequentialAction( + turnTo.build())); + + return (currentPose()); + + } + + //turns to a specified angle + public static Pose2d turn(double angle, Pose2d startingPose) { + MecanumDrive drive = new MecanumDrive(hwMap, startingPose); + TrajectoryActionBuilder turn = drive.actionBuilder(startingPose) + .turn(angle); + Actions.runBlocking( + new SequentialAction( + turn.build())); + + return (currentPose()); + } + } + + //Class for non-critical utility systems + public static class utils { + public utils() {super();} + //Class for managing telemetry systems + public static class telemetrySys { + telemetrySys() {super();} + //outputs motor telemetry + public static class motor { + motor() {super();} + public static void leftBackMotor() { + telemetry.addData("Left Back", leftBack); + telemetry.addData("Left Back", leftBack.getConnectionInfo()); + telemetry.addData("Left Back", leftBack.getDeviceName()); + telemetry.addData("Left Back", leftBack.getDirection()); + telemetry.addData("Left Back", leftBack.getManufacturer()); + telemetry.addData("Left Back", leftBack.getVersion()); + telemetry.addData("Left Back", leftBack.getClass()); + telemetry.addData("Left Back", leftBack.isBusy()); + telemetry.addData("Left Back", leftBack.getController()); + telemetry.addData("Left Back", leftBack.getMotorType()); + telemetry.addData("Left Back", leftBack.getCurrentPosition()); + telemetry.addData("Left Back", leftBack.getMode()); + telemetry.addData("Left Back", leftBack.getPortNumber()); + telemetry.addData("Left Back", leftBack.getPowerFloat()); + telemetry.addData("Left Back", leftBack.getTargetPosition()); + telemetry.addData("Left Back", leftBack.getZeroPowerBehavior()); + telemetry.update(); + } + public static void rightBackMotor() { + telemetry.addData("Right Back", rightBack); + telemetry.addData("Right Back", rightBack.getConnectionInfo()); + telemetry.addData("Right Back", rightBack.getDeviceName()); + telemetry.addData("Right Back", rightBack.getDirection()); + telemetry.addData("Right Back", rightBack.getManufacturer()); + telemetry.addData("Right Back", rightBack.getVersion()); + telemetry.addData("Right Back", rightBack.getClass()); + telemetry.addData("Right Back", rightBack.isBusy()); + telemetry.addData("Right Back", rightBack.getController()); + telemetry.addData("Right Back", rightBack.getMotorType()); + telemetry.addData("Right Back", rightBack.getCurrentPosition()); + telemetry.addData("Right Back", rightBack.getMode()); + telemetry.addData("Right Back", rightBack.getPortNumber()); + telemetry.addData("Right Back", rightBack.getPowerFloat()); + telemetry.addData("Right Back", rightBack.getTargetPosition()); + telemetry.addData("Right Back", rightBack.getZeroPowerBehavior()); + telemetry.update(); + } + public static void rightFrontMotor() { + telemetry.addData("Right Front", rightFront); + telemetry.addData("Right Front", rightFront.getConnectionInfo()); + telemetry.addData("Right Front", rightFront.getDeviceName()); + telemetry.addData("Right Front", rightFront.getDirection()); + telemetry.addData("Right Front", rightFront.getManufacturer()); + telemetry.addData("Right Front", rightFront.getVersion()); + telemetry.addData("Right Front", rightFront.getClass()); + telemetry.addData("Right Front", rightFront.isBusy()); + telemetry.addData("Right Front", rightFront.getController()); + telemetry.addData("Right Front", rightFront.getMotorType()); + telemetry.addData("Right Front", rightFront.getCurrentPosition()); + telemetry.addData("Right Front", rightFront.getMode()); + telemetry.addData("Right Front", rightFront.getPortNumber()); + telemetry.addData("Right Front", rightFront.getPowerFloat()); + telemetry.addData("Right Front", rightFront.getTargetPosition()); + telemetry.addData("Right Front", rightFront.getZeroPowerBehavior()); + telemetry.update(); + } + public static void leftFrontMotor() { + telemetry.addData("Left Front", leftFront); + telemetry.addData("Left Front", leftFront.getConnectionInfo()); + telemetry.addData("Left Front", leftFront.getDeviceName()); + telemetry.addData("Left Front", leftFront.getDirection()); + telemetry.addData("Left Front", leftFront.getManufacturer()); + telemetry.addData("Left Front", leftFront.getVersion()); + telemetry.addData("Left Front", leftFront.getClass()); + telemetry.addData("Left Front", leftFront.isBusy()); + telemetry.addData("Left Front", leftFront.getController()); + telemetry.addData("Left Front", leftFront.getMotorType()); + telemetry.addData("Left Front", leftFront.getCurrentPosition()); + telemetry.addData("Left Front", leftFront.getMode()); + telemetry.addData("Left Front", leftFront.getPortNumber()); + telemetry.addData("Left Front", leftFront.getPowerFloat()); + telemetry.addData("Left Front", leftFront.getTargetPosition()); + telemetry.addData("Left Front", leftFront.getZeroPowerBehavior()); + telemetry.update(); + } + public static void leftLauncherMotor() { + telemetry.addData("Left Launcher", leftLauncher); + telemetry.addData("Left Launcher", leftLauncher.getConnectionInfo()); + telemetry.addData("Left Launcher", leftLauncher.getDeviceName()); + telemetry.addData("Left Launcher", leftLauncher.getDirection()); + telemetry.addData("Left Launcher", leftLauncher.getManufacturer()); + telemetry.addData("Left Launcher", leftLauncher.getVersion()); + telemetry.addData("Left Launcher", leftLauncher.getClass()); + telemetry.addData("Left Launcher", leftLauncher.isBusy()); + telemetry.addData("Left Launcher", leftLauncher.getController()); + telemetry.addData("Left Launcher", leftLauncher.getMotorType()); + telemetry.addData("Left Launcher", leftLauncher.getCurrentPosition()); + telemetry.addData("Left Launcher", leftLauncher.getMode()); + telemetry.addData("Left Launcher", leftLauncher.getPortNumber()); + telemetry.addData("Left Launcher", leftLauncher.getPowerFloat()); + telemetry.addData("Left Launcher", leftLauncher.getTargetPosition()); + telemetry.addData("Left Launcher", leftLauncher.getZeroPowerBehavior()); + telemetry.update(); + } + public static void rightLauncherMotor() { + telemetry.addData("Right Launcher", rightLauncher); + telemetry.addData("Right Launcher", rightLauncher.getConnectionInfo()); + telemetry.addData("Right Launcher", rightLauncher.getDeviceName()); + telemetry.addData("Right Launcher", rightLauncher.getDirection()); + telemetry.addData("Right Launcher", rightLauncher.getManufacturer()); + telemetry.addData("Right Launcher", rightLauncher.getVersion()); + telemetry.addData("Right Launcher", rightLauncher.getClass()); + telemetry.addData("Right Launcher", rightLauncher.isBusy()); + telemetry.addData("Right Launcher", rightLauncher.getController()); + telemetry.addData("Right Launcher", rightLauncher.getMotorType()); + telemetry.addData("Right Launcher", rightLauncher.getCurrentPosition()); + telemetry.addData("Right Launcher", rightLauncher.getMode()); + telemetry.addData("Right Launcher", rightLauncher.getPortNumber()); + telemetry.addData("Right Launcher", rightLauncher.getPowerFloat()); + telemetry.addData("Right Launcher", rightLauncher.getTargetPosition()); + telemetry.addData("Right Launcher", rightLauncher.getZeroPowerBehavior()); + telemetry.update(); + } + public static void liftMotor() { + telemetry.addData("Lift", lift); + telemetry.addData("Lift", lift.getConnectionInfo()); + telemetry.addData("Lift", lift.getDeviceName()); + telemetry.addData("Lift", lift.getDirection()); + telemetry.addData("Lift", lift.getManufacturer()); + telemetry.addData("Lift", lift.getVersion()); + telemetry.addData("Lift", lift.getClass()); + telemetry.addData("Lift", lift.isBusy()); + telemetry.addData("Lift", lift.getController()); + telemetry.addData("Lift", lift.getMotorType()); + telemetry.addData("Lift", lift.getCurrentPosition()); + telemetry.addData("Lift", lift.getMode()); + telemetry.addData("Lift", lift.getPortNumber()); + telemetry.addData("Lift", lift.getPowerFloat()); + telemetry.addData("Lift", lift.getTargetPosition()); + telemetry.addData("Lift", lift.getZeroPowerBehavior()); + telemetry.update(); + } + public static void intakeMotor() { + telemetry.addData("Intake", intakeMotor); + telemetry.addData("Intake", intakeMotor.getConnectionInfo()); + telemetry.addData("Intake", intakeMotor.getDeviceName()); + telemetry.addData("Intake", intakeMotor.getDirection()); + telemetry.addData("Intake", intakeMotor.getManufacturer()); + telemetry.addData("Intake", intakeMotor.getVersion()); + telemetry.addData("Intake", intakeMotor.getClass()); + telemetry.addData("Intake", intakeMotor.isBusy()); + telemetry.addData("Intake", intakeMotor.getController()); + telemetry.addData("Intake", intakeMotor.getMotorType()); + telemetry.addData("Intake", intakeMotor.getCurrentPosition()); + telemetry.addData("Intake", intakeMotor.getMode()); + telemetry.addData("Intake", intakeMotor.getPortNumber()); + telemetry.addData("Intake", intakeMotor.getPowerFloat()); + telemetry.addData("Intake", intakeMotor.getTargetPosition()); + telemetry.addData("Intake", intakeMotor.getZeroPowerBehavior()); + telemetry.update(); + } + } + //outputs servo telemetry + public static class servos { + servos() {super();} + public static void turnTable() { + telemetry.addData("Turn Table", turnTableServo); + telemetry.addData("Turn Table", turnTableServo.getController()); + telemetry.addData("Turn Table", turnTableServo.getDirection()); + telemetry.addData("Turn Table", turnTableServo.getPortNumber()); + telemetry.addData("Turn Table", turnTableServo.getPosition()); + telemetry.addData("Turn Table", turnTableServo.getConnectionInfo()); + telemetry.addData("Turn Table", turnTableServo.getDeviceName()); + telemetry.addData("Turn Table", turnTableServo.getManufacturer()); + telemetry.addData("Turn Table", turnTableServo.getVersion()); + telemetry.addData("Turn Table", turnTableServo.getClass()); + telemetry.update(); + } + public static void intakeDoor() { + telemetry.addData("Front Door", frontDoor); + telemetry.addData("Front Door", frontDoor.getController()); + telemetry.addData("Front Door", frontDoor.getDirection()); + telemetry.addData("Front Door", frontDoor.getPortNumber()); + telemetry.addData("Front Door", frontDoor.getPosition()); + telemetry.addData("Front Door", frontDoor.getConnectionInfo()); + telemetry.addData("Front Door", frontDoor.getDeviceName()); + telemetry.addData("Front Door", frontDoor.getManufacturer()); + telemetry.addData("Front Door", frontDoor.getVersion()); + telemetry.addData("Front Door", frontDoor.getClass()); + telemetry.update(); + } + public static void launchDoor() { + telemetry.addData("Back Door", backDoor); + telemetry.addData("Back Door", backDoor.getController()); + telemetry.addData("Back Door", backDoor.getDirection()); + telemetry.addData("Back Door", backDoor.getPortNumber()); + telemetry.addData("Back Door", backDoor.getPosition()); + telemetry.addData("Back Door", backDoor.getConnectionInfo()); + telemetry.addData("Back Door", backDoor.getDeviceName()); + telemetry.addData("Back Door", backDoor.getManufacturer()); + telemetry.addData("Back Door", backDoor.getVersion()); + telemetry.addData("Back Door", backDoor.getClass()); + telemetry.update(); + } + public static void launchScoop() { + telemetry.addData("Scoop", scoop); + telemetry.addData("Scoop", scoop.getController()); + telemetry.addData("Scoop", scoop.getDirection()); + telemetry.addData("Scoop", scoop.getPortNumber()); + telemetry.addData("Scoop", scoop.getPosition()); + telemetry.addData("Scoop", scoop.getConnectionInfo()); + telemetry.addData("Scoop", scoop.getDeviceName()); + telemetry.addData("Scoop", scoop.getManufacturer()); + telemetry.addData("Scoop", scoop.getVersion()); + telemetry.addData("Scoop", scoop.getClass()); + telemetry.update(); + } + public static void leftLaunchLift() { + telemetry.addData("Left Launch Lift", launchLiftLeft); + telemetry.addData("Left Launch Lift", launchLiftLeft.getController()); + telemetry.addData("Left Launch Lift", launchLiftLeft.getDirection()); + telemetry.addData("Left Launch Lift", launchLiftLeft.getPortNumber()); + telemetry.addData("Left Launch Lift", launchLiftLeft.getPower()); + telemetry.addData("Left Launch Lift", launchLiftLeft.getConnectionInfo()); + telemetry.addData("Left Launch Lift", launchLiftLeft.getDeviceName()); + telemetry.addData("Left Launch Lift", launchLiftLeft.getManufacturer()); + telemetry.addData("Left Launch Lift", launchLiftLeft.getVersion()); + telemetry.addData("Left Launch Lift", launchLiftLeft.getClass()); + telemetry.update(); + } + public static void rightLaunchLift() { + telemetry.addData("Right Launch Lift", launchLiftRight); + telemetry.addData("Right Launch Lift", launchLiftRight.getController()); + telemetry.addData("Right Launch Lift", launchLiftRight.getDirection()); + telemetry.addData("Right Launch Lift", launchLiftRight.getPortNumber()); + telemetry.addData("Right Launch Lift", launchLiftRight.getPower()); + telemetry.addData("Right Launch Lift", launchLiftRight.getConnectionInfo()); + telemetry.addData("Right Launch Lift", launchLiftRight.getDeviceName()); + telemetry.addData("Right Launch Lift", launchLiftRight.getManufacturer()); + telemetry.addData("Right Launch Lift", launchLiftRight.getVersion()); + telemetry.addData("Right Launch Lift", launchLiftRight.getClass()); + telemetry.update(); + } + } + } + //Function to manage the color of LED(s) on the robot + public static void ledManager(String type, int ledNumber) { + CRServo led = leds.get(ledNumber); + if (led != null) { + switch (type) { + case "Clear": + led.setPower(.5); //White + + break; + case "Good": + led.setPower(0); //Green + + break; + case "Warn": + led.setPower(-.25); //Yellow + + break; + case "Alert": + led.setPower(-.35); //Orange + + break; + case "Error": + led.setPower(-0.44); //red + + break; + case "Null": + led.setPower(-.6); //Blank + + break; + case "Match Alert": + led.setPower(0); //Purple + + break; + case "Blue": + led.setPower(0.216); //Blue + + break; + case "Purple": + led.setPower(0.415); //Purple + + break; + case "Pink": + led.setPower(0.275); //Pink + + break; + default: + if (debug.debugTelemetry) { + telemetry.addData("Led Manager Error", "Wrong or Invalid Input"); + RobotLog.ii("Led Manager Error", "Wrong or Invalid Input"); + } + break; + } + } + } + + public static class auto { + public auto() {super();} + //automatically intakes based on if the launcher is up or down + public void intake3Balls(double searchSpeed, double returnSpeed, double returnDistance, int kickTime) { + int safeTrig; + turnTableServo.setPosition(0); + frontDoor.setPosition(1); + intakeMotor.setPower(0.8); + safeTrig = BackwardsTillBump(searchSpeed, 0); + if (safeTrig == 1) { + moveForwardTics(returnSpeed, returnDistance * ticPerIn); + halfKick(kickTime); + sleep(250); + turnTableServo.setPosition(0.5); + frontDoor.setPosition(1); + + safeTrig = BackwardsTillBump(searchSpeed, 0); + if (safeTrig == 1) { + moveForwardTics(returnSpeed, returnDistance * ticPerIn); + halfKick(kickTime); + sleep(250); + turnTableServo.setPosition(1); + frontDoor.setPosition(1); + safeTrig = BackwardsTillBump(searchSpeed, 0); + if (safeTrig == 1) { + halfKick(kickTime); + intakeMotor.setPower(0); + + } else { + + frontDoor.setPosition(.5); + intakeMotor.setPower(0); + } + + } else { + frontDoor.setPosition(.5); + intakeMotor.setPower(0); + } + } else { + frontDoor.setPosition(.5); + intakeMotor.setPower(0); + } + } + //moves backwards for a certain of tics + private void moveForwardTics(double Speed, double tic) { + pinpoint.update(); + double xvalue = pinpoint.getEncoderX(); + while (xvalue - pinpoint.getEncoderX() <= tic) { + leftBack.setPower(-Speed); + leftFront.setPower(-Speed); + rightBack.setPower(-Speed); + rightFront.setPower(-Speed); + } + motion.halt(); + } + //moves backwards until a sensor is triggered + public int BackwardsTillBump(double Speed, int delay) { + int returnSave; + + ElapsedTime BackwardsTillBumpClock = new ElapsedTime(); + + BackwardsTillBumpClock.reset(); + while (BackwardsTillBumpClock.seconds() <= 2 && !(!intakeBump1.isPressed() || intakeBump2.isPressed())) { + leftBack.setPower(Speed); + leftFront.setPower(Speed); + rightBack.setPower(Speed); + rightFront.setPower(Speed); + sleep(1); + count++; + } + count = 0; + + if (!intakeBump1.isPressed() || intakeBump2.isPressed()) { + returnSave = 1; + } else if (BackwardsTillBumpClock.seconds() >= 2) { + returnSave = 0; + } else { + returnSave = 1; + } + + leftBack.setPower(0); + leftFront.setPower(0); + rightBack.setPower(0); + rightFront.setPower(0); + return returnSave; + } + //moves kicker inside before returning it to door position + public void halfKick(int time) { + frontDoor.setPosition(0); + sleep(time); + frontDoor.setPosition(.5); + } + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MainTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MainTeleOp.java new file mode 100644 index 000000000000..eadd88b73123 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MainTeleOp.java @@ -0,0 +1,1072 @@ + +package org.firstinspires.ftc.teamcode; + +import com.acmerobotics.roadrunner.Pose2d; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.ColorSensor; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DistanceSensor; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.hardware.TouchSensor; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +@TeleOp(name = "Main TeleOp") +public class MainTeleOp extends LinearOpMode { + Limelight3A limelight; + + ArrayList IDs = new ArrayList<>(); + //.5=green | 1=p | 0=p + ArrayList motifArray = new ArrayList<>(Arrays.asList(.5, 0.0, 1.0, 1.0, .5, 0.0, 1.0, 0.0, .5)); + + int id; + + DcMotor intakeMotor; + Servo goofyAhhhhFrontDoor; + DcMotor leftBack; + DcMotor rightBack; + DcMotor leftFront; + DcMotor rightFront; + DcMotor leftLauncher; + DcMotor rightLauncher; + DcMotor lift; + CRServo launchLiftRight; + CRServo launchLiftLeft; + Servo backDoor; + Servo scoop; + Servo turnTableServo; + TouchSensor TopBump; + TouchSensor BottomBump; + CRServo LED1; + GoBildaPinpointDriver pinpoint; + private TouchSensor intakeBump1; + private TouchSensor intakeBump2; + + int highLauncherSpeed = 2350;//2400; + int lowLauncherSpeed = 1750; + int triangleFuncRunning = 0; + double turnTablePos2 = 0; + int launcherSpeed = 0; + int LauncherON = 0; + int intakeCount = 0; + int ballCount = 0; + int ballTrig = 0; + + int motiff = 1; + int manualMotif = 1; + double speed = 0; + + + + + + ElapsedTime triangleClock = new ElapsedTime(); + ElapsedTime ReKickClock = new ElapsedTime(); + ElapsedTime ScoopClock = new ElapsedTime(); + ElapsedTime LaunchClock = new ElapsedTime(); + ElapsedTime LaunchMotiffClock = new ElapsedTime(); + ElapsedTime intake3BallsClock = new ElapsedTime(); + + int LaunchMotiffTrig = 0; + int LaunchTrig = 0; + int RekickTrig = 0; + int scoopTrig = 0; + int triTrig = 0; + int LocalTrig = 0; + int ledTrig = 0; + int bumpTrig = 0; + int motifTrig = 1; + boolean processTrig = true; + boolean launchAbort = false; + boolean safeTrig = true; + boolean resetTrig = false; + boolean reverseLaunchTrig = true; + + double txMax = 14.700; + double txMin = 14.400; + double tyMax = 13.5; + double tyMin = 12; + double taMax = .88; + double taMin = .91; + + Pose2d redParkPose = new Pose2d(60, -60, Math.toRadians(0)); + Pose2d blueParkPose = new Pose2d(60, 60, Math.toRadians(0)); + + + @Override + public void runOpMode() { + + intakeMotor = hardwareMap.get(DcMotor.class, "intakeMotor"); + goofyAhhhhFrontDoor = hardwareMap.get(Servo.class, "goofyAhhhhFrontDoor"); + leftBack = hardwareMap.get(DcMotor.class, "leftBack"); + rightBack = hardwareMap.get(DcMotor.class, "rightBack"); + leftFront = hardwareMap.get(DcMotor.class, "leftFront"); + rightFront = hardwareMap.get(DcMotor.class, "rightFront"); + leftLauncher = hardwareMap.get(DcMotor.class, "leftLauncher"); + rightLauncher = hardwareMap.get(DcMotor.class, "rightLauncher"); + lift = hardwareMap.get(DcMotor.class, "lift"); + launchLiftRight = hardwareMap.get(CRServo.class, "launchLiftRight"); + launchLiftLeft = hardwareMap.get(CRServo.class, "launchLiftLeft"); + backDoor = hardwareMap.get(Servo.class, "backDoor"); + scoop = hardwareMap.get(Servo.class, "scoop"); + turnTableServo = hardwareMap.get(Servo.class, "turnTableServo"); + TopBump = hardwareMap.get(TouchSensor.class, "TopBump"); + BottomBump = hardwareMap.get(TouchSensor.class, "BottomBump"); + DistanceSensor distance = hardwareMap.get(DistanceSensor.class, "distance"); + ColorSensor color_DistanceSensor = hardwareMap.get(ColorSensor.class, "color"); + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + LED1 = hardwareMap.get(CRServo.class, "Led1"); + pinpoint = hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint"); + intakeBump1 = hardwareMap.get(TouchSensor.class, "intakeBump1"); + intakeBump2 = hardwareMap.get(TouchSensor.class, "intakeBump2"); + + + // Put initialization blocks here. + triangleFuncRunning = 0; + leftBack.setDirection(DcMotor.Direction.FORWARD); + rightBack.setDirection(DcMotor.Direction.FORWARD); + leftFront.setDirection(DcMotor.Direction.REVERSE); + rightFront.setDirection(DcMotor.Direction.FORWARD); + intakeMotor.setDirection(DcMotor.Direction.REVERSE); + leftLauncher.setDirection(DcMotor.Direction.REVERSE); + rightLauncher.setDirection(DcMotor.Direction.FORWARD); + launchLiftRight.setDirection(CRServo.Direction.REVERSE); + launchLiftLeft.setDirection(CRServo.Direction.FORWARD); + leftLauncher.setPower(0); + rightLauncher.setPower(0); + leftLauncher.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER); + rightLauncher.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER); + leftLauncher.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightLauncher.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + leftBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + lift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + limelight.pipelineSwitch(0); + limelight.setPollRateHz(100); // This sets how often we ask Limelight for data (100 times per second) + limelight.start(); + ledManager("Null"); + + + waitForStart(); + speed = 0.75; + backDoor.setPosition(1); + goofyAhhhhFrontDoor.setPosition(0.5); + scoop.setPosition(1); + turnTableServo.setPosition(0.5); + turnTablePos2 = 0; + launcherSpeed = (1700 * 28) / 60; + triangleFuncRunning = 1; + + pinpoint.initialize(); + + new MMHS26Lib(hardwareMap, new Pose2d(0, 0, 0), telemetry); + + if (opModeIsActive()) { + while (opModeIsActive()) { + telemetry.update(); + pinpoint.update(); + telemetry.addData("x", pinpoint.getEncoderX()); + telemetry.addData("y", pinpoint.getEncoderY()); + telemetry.addData("posX", pinpoint.getPosX(DistanceUnit.MM)); + telemetry.addData("posY", pinpoint.getPosY(DistanceUnit.MM)); + telemetry.addData("headingDeg", pinpoint.getHeading(AngleUnit.DEGREES)); + telemetry.addData("intakeCount",intakeCount); + telemetry.addData("intakeBump1",intakeBump1.isPressed()); + + intakeControl(); + drive(); + turnTablePos(); + timeTriangleFunction(); + backDoorControl(); + timeReKick(); + timeScoop(); + controlLauncher(); + goofyAhhhhFrontDoorControl(); + launcherTiltControl(); + //distanceSensorControl(); + killSwitch(); + localize(0.3, 10); + lift(); + motifControl(); + timeLaunchMotif(manualMotif, launcherSpeed); + toClose(); + intake3Balls(); + reverseLaunch(); + park(); + + /* + + if (manualMotif != 3) { + timeLaunchMotif(manualMotif, launcherSpeed); + } else { + timeLaunchMotif(motiff, launcherSpeed); + } + + if (motifTrig == 1 && manualMotif == 3) { + processLimeLightResults(); + checkID(); + } + + */ + + + } + } + } + private void checkID() { + for (int idd : IDs) { + if (idd == 21){ + motiff = 0; + gamepad1.rumbleBlips(2); + motifTrig = 0; + } else if (idd == 22) { + motiff = 1; + gamepad1.rumbleBlips(2); + motifTrig = 0; + } else if (idd == 23) { + motiff = 2; + gamepad1.rumbleBlips(2); + motifTrig = 0; + } + } + } + + + private void intakeControl() { + if (gamepad1.left_trigger == 1) { + intakeMotor.setPower(0.8); + goofyAhhhhFrontDoor.setPosition(1); + } else if (gamepad1.right_trigger == 1) { + intakeMotor.setPower(0); + } + } + + private void backDoorControl() { + if (gamepad2.squareWasPressed()) { + backDoor.setPosition(1); + } + if (gamepad2.circleWasPressed()) { + backDoor.setPosition(0); + } + } + + private void launcherTiltControl() { + + if (-0.1 >= gamepad2.right_stick_y && !BottomBump.isPressed()) { + launchLiftRight.setPower(gamepad2.right_stick_y * 0.35); + launchLiftLeft.setPower(gamepad2.right_stick_y * 0.35); + } else if (0.1 <= gamepad2.right_stick_y && !TopBump.isPressed()) { + launchLiftRight.setPower(gamepad2.right_stick_y * 0.35); + launchLiftLeft.setPower(gamepad2.right_stick_y * 0.35); + } else if (0.1 <= gamepad2.right_stick_y && TopBump.isPressed()) { + launchLiftRight.setPower(0); + launchLiftRight.setPower(0); + launchLiftLeft.setPower(0); + gamepad2.rumbleBlips(1); + } else if (-0.1 >= gamepad2.right_stick_y && BottomBump.isPressed()) { + launchLiftRight.setPower(0); + launchLiftLeft.setPower(0); + gamepad2.rumbleBlips(1); + } else { + launchLiftRight.setPower(0); + launchLiftLeft.setPower(0); + } + if (TopBump.isPressed() && gamepad2.right_stick_y != 0) { + ledManager("Blue"); + ledTrig = 1; + bumpTrig = 1; + } else if (BottomBump.isPressed() && gamepad2.right_stick_y != 0) { + ledManager("Blue"); + ledTrig = 1; + bumpTrig = 1; + }else if (bumpTrig == 1 && !(BottomBump.isPressed() || TopBump.isPressed())){ + ledManager("Null"); + ledTrig = 0; + bumpTrig = 0; + } + } + + private void goofyAhhhhFrontDoorControl() { + if (gamepad1.squareWasReleased()) { + goofyAhhhhFrontDoor.setPosition(1); + } else if (gamepad1.circleWasReleased()) { + goofyAhhhhFrontDoor.setPosition(0); + } else if (gamepad1.crossWasReleased()) { + goofyAhhhhFrontDoor.setPosition(0.5); + } + } + + private void killSwitch() { + if (gamepad1.touchpadWasPressed()) { + leftLauncher.setPower(0); + rightLauncher.setPower(0); + intakeMotor.setPower(0); + launchAbort = true; + } + } + + private void turnTablePos() { + if (gamepad2.leftBumperWasPressed() && 0 != goofyAhhhhFrontDoor.getPosition()) { + turnTablePos2 += 0.5; + if (1.5 <= turnTablePos2) { + turnTablePos2 = 0; + } + turnTableServo.setPosition(turnTablePos2); + } + if (gamepad2.rightBumperWasPressed() && 0 != goofyAhhhhFrontDoor.getPosition()) { + turnTablePos2 -= 0.5; + if (-0.5 >= turnTablePos2) { + turnTablePos2 = 1; + } + turnTableServo.setPosition(turnTablePos2); + } + if (gamepad2.psWasPressed() && 0 != goofyAhhhhFrontDoor.getPosition()) { + turnTablePos2 = 0; + turnTableServo.setPosition(turnTablePos2); + } + } + + private void drive() { + @SuppressWarnings("SuspiciousNameCombination") + float ControlY = gamepad1.left_stick_x; + float ControlX = -gamepad1.right_stick_x; + float ControlRX = -gamepad1.right_stick_y; + leftFront.setPower(((ControlY - ControlX) + ControlRX) * speed); + leftBack.setPower((ControlY + ControlX + ControlRX) * speed); + rightFront.setPower(((ControlY - ControlX) - ControlRX) * speed); + rightBack.setPower(((ControlY + ControlX) - ControlRX) * speed); + if (gamepad1.dpad_up) { + speed = 1; + } else if (gamepad1.dpad_right) { + speed = 0.75; + } else if (gamepad1.dpad_down) { + speed = 0.35; + } + } + + private void timeReKick() { + if (gamepad2.touchpadWasReleased()) { + ReKickClock.reset(); + telemetry.addData("Elapsed Time", ReKickClock.seconds()); + RekickTrig = 1; + } + if (RekickTrig == 1) { + if (ReKickClock.seconds() >= 0 && ReKickClock.seconds() <= 0.75) { + goofyAhhhhFrontDoor.setPosition(0); + telemetry.update(); + } + if (ReKickClock.seconds() >= 0.75 && ReKickClock.seconds() <= 1) { + goofyAhhhhFrontDoor.setPosition(0.5); + telemetry.update(); + RekickTrig = 0; + } + } + } + + private void timeTriangleFunction() { + + if (gamepad2.triangleWasReleased()) { + triangleFuncRunning = 1; + + triangleClock.reset(); + telemetry.addData("Elapsed Time", triangleClock.seconds()); + launchAbort = false; + triTrig = 1; + } + if (triTrig == 1 && !launchAbort) { + if (triangleClock.seconds() >= 0 && triangleClock.seconds() <= 0.5) { + triangleFuncRunning = 0; + launchMotorOnTriangle(); + backDoor.setPosition(0); + telemetry.update(); + ledManager("Alert"); + ledTrig = 1; + } + if (triangleClock.seconds() >= 0.5 && triangleClock.seconds() <= 1.5) { + launchMotorOnTriangle(); + goofyAhhhhFrontDoor.setPosition(0); + telemetry.update(); + } + if (triangleClock.seconds() >= 1.5 && triangleClock.seconds() <= 2) { + launchMotorOnTriangle(); + goofyAhhhhFrontDoor.setPosition(0.5); + scoop.setPosition(0.5); + telemetry.update(); + } + if (triangleClock.seconds() >= 2.5 && triangleClock.seconds() <= 3) { + launchMotorOnTriangle(); + scoop.setPosition(1); + telemetry.update(); + triangleFuncRunning = 0; + triTrig = 0; + ledManager("Null"); + ledTrig = 0; + } + + } + } + private void timeScoop() { + if (gamepad2.dpad_up) { + ScoopClock.reset(); + telemetry.addData("Elapsed Time", ScoopClock.seconds()); + scoopTrig = 1; + } + if (scoopTrig == 1) { + if (ScoopClock.seconds() >= 0 && ScoopClock.seconds() <= 0.5) { + scoop.setPosition(0.5); + telemetry.update(); + } + if (ScoopClock.seconds() >= 0.5 && ScoopClock.seconds() <= 1) { + scoop.setPosition(1); + telemetry.update(); + scoopTrig = 0; + } + } + } + + private void controlLauncher() { + if (gamepad2.left_trigger == 1) { + + ((DcMotorEx) leftLauncher).setVelocity(launcherSpeed); + ((DcMotorEx) rightLauncher).setVelocity(launcherSpeed); + + LauncherON = 1; + } else if (gamepad2.right_trigger == 1) { + ((DcMotorEx) leftLauncher).setVelocity(0); + ((DcMotorEx) rightLauncher).setVelocity(0); + LauncherON = 0; + } + if (gamepad2.dpadLeftWasPressed()) { + launcherSpeed = (lowLauncherSpeed * 28) / 60; + gamepad2.rumbleBlips(1); + if (1 == LauncherON) { + + ((DcMotorEx) leftLauncher).setVelocity(launcherSpeed); + ((DcMotorEx) rightLauncher).setVelocity(launcherSpeed); + } + } else if (gamepad2.dpadRightWasReleased()) { + launcherSpeed = (highLauncherSpeed * 28) / 60; + gamepad2.rumbleBlips(2); + if (1 == LauncherON) { + + ((DcMotorEx) leftLauncher).setVelocity(launcherSpeed); + ((DcMotorEx) rightLauncher).setVelocity(launcherSpeed); + } + } + } + + private void launchMotorOnTriangle() { + ((DcMotorEx) leftLauncher).setVelocity(launcherSpeed * Math.abs(triangleFuncRunning - 1)); + ((DcMotorEx) rightLauncher).setVelocity(launcherSpeed * Math.abs(triangleFuncRunning - 1)); + + } + public void localize(double localizerMotorPower, int sleepTimeMilli) { + if (gamepad1.ps) { + LocalTrig = 1; + //boolean localizing = true; + + LLResult result = limelight.getLatestResult(); + double tx; + double ty; + double ta; + if (result != null && result.isValid()) { + tx = result.getTx(); // How far left or right the target is (degrees) + ty = result.getTy(); // How far up or down the target is (degrees) + ta = result.getTa(); // How big the target looks (0%-100% of the image) + + telemetry.addData("Target X", tx); + telemetry.addData("Target Y", ty); + telemetry.addData("Target Area", ta); + telemetry.update(); + + if (tx < txMin) { + // turn right + turnRight(localizerMotorPower, sleepTimeMilli); + ledManager("Clear"); + LocalTrig = 1; + } else if (tx > txMax) { + //turn left + turnLeft(localizerMotorPower, sleepTimeMilli); + ledManager("Clear"); + LocalTrig = 1; + } else if (ty < tyMin) { + //strafe in a direction (i think ) + strafeRight(localizerMotorPower, sleepTimeMilli); + ledManager("Clear"); + LocalTrig = 1; + } else if (ty > tyMax) { + //strafe in a direction (i think left) + strafeLeft(localizerMotorPower, sleepTimeMilli); + ledManager("Clear"); + LocalTrig = 1; + } else if (ta > taMax) { + //move backward + moveBackward(localizerMotorPower, sleepTimeMilli); + ledManager("Clear"); + LocalTrig = 1; + } else if (ta < taMin) { + //move Forward + moveForward(localizerMotorPower, sleepTimeMilli); + ledManager("Clear"); + LocalTrig = 1; + } else { + telemetry.addData("done", 0); + ledManager("Good"); + LocalTrig = 1; + } + + } else { + telemetry.addData("Limelight", "No Targets"); + ledManager("Error"); + LocalTrig = 1; + halt(); + } + } else if (LocalTrig == 1){ + ledManager("Null"); + LocalTrig = 0; + } + } + public void strafeLeft(double Speed, int time) { + leftBack.setPower(Speed); + leftFront.setPower(-Speed); + rightBack.setPower(-Speed); + rightFront.setPower(Speed); + sleep(time); + leftBack.setPower(0); + leftFront.setPower(0); + rightBack.setPower(0); + rightFront.setPower(0); + } + public void strafeRight(double Speed, int time) { + leftBack.setPower(-Speed); + leftFront.setPower(Speed); + rightBack.setPower(Speed); + rightFront.setPower(-Speed); + sleep(time); + leftBack.setPower(0); + leftFront.setPower(0); + rightBack.setPower(0); + rightFront.setPower(0); + } + public void turnRight(double Speed, int time) { + leftBack.setPower(Speed); + leftFront.setPower(Speed); + rightBack.setPower(-Speed); + rightFront.setPower(-Speed); + sleep(time); + leftBack.setPower(0); + leftFront.setPower(0); + rightBack.setPower(0); + rightFront.setPower(0); + } + public void turnLeft(double Speed, int time) { + leftBack.setPower(-Speed); + leftFront.setPower(-Speed); + rightBack.setPower(Speed); + rightFront.setPower(Speed); + sleep(time); + leftBack.setPower(0); + leftFront.setPower(0); + rightBack.setPower(0); + rightFront.setPower(0); + } + public void moveBackward(double Speed, int time) { + leftBack.setPower(Speed); + leftFront.setPower(Speed); + rightBack.setPower(Speed); + rightFront.setPower(Speed); + sleep(time); + leftBack.setPower(0); + leftFront.setPower(0); + rightBack.setPower(0); + rightFront.setPower(0); + } + public void moveForward(double Speed, int time) { + leftBack.setPower(-Speed); + leftFront.setPower(-Speed); + rightBack.setPower(-Speed); + rightFront.setPower(-Speed); + sleep(time); + leftBack.setPower(0); + leftFront.setPower(0); + rightBack.setPower(0); + rightFront.setPower(0); + } + public void halt() { + leftBack.setPower(0); + leftFront.setPower(0); + rightBack.setPower(0); + rightFront.setPower(0); + } + + private void lift(){ + lift.setPower(gamepad2.left_stick_y); + /* + if (!liftLimit.isPressed()) { + lift.setPower(gamepad2.right_stick_y); + } else { + lift.setPower(0); + ledManager("Good"); + } + + */ + + } + private void ledManager(String type){ + switch (type) { + case "Clear": + LED1.setPower(.5); //White + + break; + case "Good": + LED1.setPower(0); //Green + + break; + case "Warn": + LED1.setPower(-.25); //Yellow + + break; + case "Alert": + LED1.setPower(-.35); //Orange + + break; + case "Error": + LED1.setPower(-0.44); //red + + break; + case "Null": + LED1.setPower(-.6); //Blank + + break; + case "Match Alert": + LED1.setPower(0); //Purple + + break; + case "Blue": + LED1.setPower(0.216); //Blue + + break; + case "Purple": + LED1.setPower(0.415); //Purple + + break; + case "Pink": + LED1.setPower(0.275); //Pink + + break; + default: + telemetry.addData("Led Manager Error", "Wrong or Invalid Input"); + break; + } + } + + private void timeLaunchMotif(int motiff, double launcherSpeedd) { + + if (gamepad2.crossWasReleased()) { + LaunchMotiffClock.reset(); + telemetry.addData("Elapsed Time", LaunchMotiffClock.seconds()); + LaunchMotiffTrig = 1; + launchAbort = false; + telemetry.update(); + } + if (LaunchMotiffTrig == 1 && !launchAbort) { + if (LaunchMotiffClock.seconds() >= 0 && LaunchMotiffClock.seconds() <= 0.75) { + ledManager("Alert"); + launchMotorOn(launcherSpeedd); + backDoor.setPosition(0); + turnTableServo.setPosition(0); //motifArray.get(motiff*3) + telemetry.update(); + } + if (LaunchMotiffClock.seconds() >= 0.75 && LaunchMotiffClock.seconds() <= 1) { + + //backDoor.setPosition(0); + telemetry.update(); + } + if (LaunchMotiffClock.seconds() >= 1 && LaunchMotiffClock.seconds() <= 1.75) { + goofyAhhhhFrontDoor.setPosition(0); + telemetry.update(); + + } + if (LaunchMotiffClock.seconds() >= 1.75 && LaunchMotiffClock.seconds() <= 1.85) { + goofyAhhhhFrontDoor.setPosition(0.5); + telemetry.update(); + + } + if (LaunchMotiffClock.seconds() >= 1.85 && LaunchMotiffClock.seconds() <= 2.35) { + //backDoor.setPosition(1); + goofyAhhhhFrontDoor.setPosition(0.5); + scoop.setPosition(0.5); + + turnTableServo.setPosition(.5); //motifArray.get((motiff*3)+1) + telemetry.update(); + } + + + if (LaunchMotiffClock.seconds() >= 2.35 && LaunchMotiffClock.seconds() <= 2.6) { + scoop.setPosition(1); + backDoor.setPosition(0); + telemetry.update(); + } + if (LaunchMotiffClock.seconds() >= 2.6 && LaunchMotiffClock.seconds() <= 3.35) { + goofyAhhhhFrontDoor.setPosition(0); + telemetry.update(); + + } + if (LaunchMotiffClock.seconds() >= 3.35 && LaunchMotiffClock.seconds() <= 3.45) { + goofyAhhhhFrontDoor.setPosition(0.5); + telemetry.update(); + + } + if (LaunchMotiffClock.seconds() >= 3.45 && LaunchMotiffClock.seconds() <= 3.95) { + //backDoor.setPosition(1); + goofyAhhhhFrontDoor.setPosition(0.5); + scoop.setPosition(0.5); + + turnTableServo.setPosition(1); //motifArray.get((motiff*3)+2 + telemetry.update(); + } + + + if (LaunchMotiffClock.seconds() >= 3.95 && LaunchMotiffClock.seconds() <= 4.2) { + scoop.setPosition(1); + backDoor.setPosition(0); + telemetry.update(); + } + if (LaunchMotiffClock.seconds() >= 4.2 && LaunchMotiffClock.seconds() <= 4.95) { + goofyAhhhhFrontDoor.setPosition(0); + telemetry.update(); + + } + if (LaunchMotiffClock.seconds() >= 4.95 && LaunchMotiffClock.seconds() <= 5.05) { + goofyAhhhhFrontDoor.setPosition(0.5); + telemetry.update(); + + } + if (LaunchMotiffClock.seconds() >= 5.05 && LaunchMotiffClock.seconds() <= 5.55) { + + goofyAhhhhFrontDoor.setPosition(0.5); + scoop.setPosition(0.5); + + telemetry.update(); + } + if (LaunchMotiffClock.seconds() >= 5.55 && LaunchMotiffClock.seconds() <= 6.05) { + scoop.setPosition(1); + backDoor.setPosition(1); + turnTableServo.setPosition(0); + launchMotorOff(); + telemetry.update(); + ledManager("Null"); + LaunchMotiffTrig = 0; + } + } + + } + + + private int processLimeLightResults() { + double tx; + double ty; + double ta; + + LLResult result = limelight.getLatestResult(); + if (result != null && result.isValid()) { + // Get the list of ALL detected fiducials (AprilTags) + List fiducialList = result.getFiducialResults(); + + tx = result.getTx(); + ty = result.getTy(); // How far up or down the target is (degrees) + ta = result.getTa(); // How big the target looks (0%-100% of the image) + + telemetry.addData("Target X", tx); + telemetry.addData("Target Y", ty); + telemetry.addData("Target Area", ta); + telemetry.update(); + + if (!fiducialList.isEmpty()) { + telemetry.addData("Detections Found", fiducialList.size()); + telemetry.update(); + + // Iterate through each detected tag + for (LLResultTypes.FiducialResult fiducial : fiducialList) { + id = fiducial.getFiducialId(); + IDs.add(id); + telemetry.addData("Tag ID", id); + telemetry.update(); + } + processTrig = false; + } else { + telemetry.addData("Detections Found", "None"); + telemetry.update(); + } + } else { + telemetry.addData("Limelight Data", "Invalid or Stale"); + assert result != null; + telemetry.addData("Staleness", result.getStaleness()); + telemetry.update(); + } + telemetry.update(); + + return id; + } + public void motifControl() { + if (gamepad2.optionsWasPressed()) { + manualMotif++; + if (manualMotif > 2) { + manualMotif = 0; + } + /* Is there a reason for this? + if (manualMotif == 3) { + motifTrig = 1; + } + */ + + gamepad2.rumbleBlips(manualMotif+1); + } + } + + private void launchMotorOn(double launcherSpeedd) { + ((DcMotorEx) leftLauncher).setVelocity(launcherSpeedd); + ((DcMotorEx) rightLauncher).setVelocity(launcherSpeedd); + + + } + private void launchMotorOff() { + ((DcMotorEx) leftLauncher).setVelocity(0); + ((DcMotorEx) rightLauncher).setVelocity(0); + + } + private void toClose() { + if (gamepad1.options){ + MMHS26Lib.roadRunner.spline.splineToLinearHeading(-28, 24, Math.toRadians(25), 0, MMHS26Lib.Limelight.poseLimelight(false)); + } + } + private void reverseLaunch(){ + + if (gamepad1.shareWasPressed()){ + leftLauncher.setPower(-.3); + rightLauncher.setPower(-.3); + reverseLaunchTrig = true; + } else if (gamepad1.shareWasReleased()){ + leftLauncher.setPower(0); + rightLauncher.setPower(0); + + } + } + private void park() { + if(gamepad1.optionsWasReleased()){ + MMHS26Lib.roadRunner.spline.splineToLinearHeading(-28, 24, Math.toRadians(25), 0, redParkPose); + } + /* + if(gamepad1.shareWasReleased()){ + MMHS26Lib.roadRunner.spline.splineToLinearHeading(-28, 24, Math.toRadians(25), 0, blueParkPose); + } + + */ + } + + public void intake3Balls() { + + + if (gamepad1.triangleWasPressed()) { + + turnTableServo.setPosition(0); + //goofyAhhhhFrontDoor.setPosition(1); + intakeOn(); + intake3BallsClock.reset(); + intakeCount = 1; + ballCount = 0; + resetTrig = true; + safeTrig = false; + } + + if (intakeCount == 1 && (!intakeBump1.isPressed() || intakeBump2.isPressed())) { + ballTrig = 1; + ballCount = 1; + intake3BallsClock.reset(); + } else if (intakeCount == 2 && (!intakeBump1.isPressed() || intakeBump2.isPressed())) { + ballTrig = 1; + ballCount = 2; + intake3BallsClock.reset(); + } else if (intakeCount == 3 && (!intakeBump1.isPressed() || intakeBump2.isPressed())) { + ballTrig = 1; + ballCount = 3; + intake3BallsClock.reset(); + } + + if (ballCount == 1 && gamepad1.triangle == true && ballTrig == 1) { + + + if (intake3BallsClock.seconds() >= 0 && intake3BallsClock.seconds() <= .500 && gamepad1.triangle == true) { + goofyAhhhhFrontDoor.setPosition(0); + } + + if (intake3BallsClock.seconds() >= .500 && intake3BallsClock.seconds() <= .510 && gamepad1.triangle == true) { + goofyAhhhhFrontDoor.setPosition(.5); + } + + if (intake3BallsClock.seconds() >= .510 && intake3BallsClock.seconds() <= .550 && gamepad1.triangle == true) { + turnTableServo.setPosition(0.5); + goofyAhhhhFrontDoor.setPosition(1); + intakeCount = 2; + ballTrig = 0; + } + } else if (ballCount == 2 && gamepad1.triangle == true && ballTrig == 1) { + + if (intake3BallsClock.seconds() >= 0 && intake3BallsClock.seconds() <= .500 && gamepad1.triangle == true) { + goofyAhhhhFrontDoor.setPosition(0); + } + + if (intake3BallsClock.seconds() >= .500 && intake3BallsClock.seconds() <= .510 && gamepad1.triangle == true) { + goofyAhhhhFrontDoor.setPosition(.5); + } + + if (intake3BallsClock.seconds() >= .510 && intake3BallsClock.seconds() <= .550 && gamepad1.triangle == true) { + turnTableServo.setPosition(1); + goofyAhhhhFrontDoor.setPosition(1); + intakeCount = 3; + ballTrig = 0; + } + + + } else if (ballCount == 3 && gamepad1.triangle == true && ballTrig == 1) { + + if (intake3BallsClock.seconds() >= 0 && intake3BallsClock.seconds() <= .500 && gamepad1.triangle == true) { + goofyAhhhhFrontDoor.setPosition(0); + } + + if (intake3BallsClock.seconds() >= .500 && intake3BallsClock.seconds() <= .510 && gamepad1.triangle == true) { + goofyAhhhhFrontDoor.setPosition(.5); + } + + if (intake3BallsClock.seconds() >= .510 && intake3BallsClock.seconds() <= .550 && gamepad1.triangle == true) { + turnTableServo.setPosition(1); + goofyAhhhhFrontDoor.setPosition(.5); + intakeCount = 3; + ballTrig = 0; + intakeOff(); + } + + + + } + if (gamepad1.triangleWasReleased()) { + intakeCount = 0; + goofyAhhhhFrontDoor.setPosition(.5); + intakeOff(); + safeTrig = true; + resetTrig = false; + } + /* + + + if (intakeCount == 1 && (!intakeBump1.isPressed() || intakeBump2.isPressed()) && gamepad1.triangle == true) { + intake3BallsClock.reset(); + if (intake3BallsClock.seconds() >= 0 && intake3BallsClock.seconds() <= .500 && gamepad1.triangle == true) { + goofyAhhhhFrontDoor.setPosition(0); + } + + if (intake3BallsClock.seconds() >= .500 && intake3BallsClock.seconds() <= .510 && gamepad1.triangle == true) { + goofyAhhhhFrontDoor.setPosition(.5); + } + + if (intake3BallsClock.seconds() >= .510 && intake3BallsClock.seconds() <= .550 && gamepad1.triangle == true) { + turnTableServo.setPosition(0.5); + goofyAhhhhFrontDoor.setPosition(1); + intakeCount = 2; + } + + } else if (intakeCount == 2 && (!intakeBump1.isPressed() || intakeBump2.isPressed()) && gamepad1.triangle == true) { + intake3BallsClock.reset(); + if (intake3BallsClock.seconds() >= 0 && intake3BallsClock.seconds() <= .500 && gamepad1.triangle == true) { + goofyAhhhhFrontDoor.setPosition(0); + } + + if (intake3BallsClock.seconds() >= .500 && intake3BallsClock.seconds() <= .510 && gamepad1.triangle == true) { + goofyAhhhhFrontDoor.setPosition(.5); + } + + if (intake3BallsClock.seconds() >= .510 && intake3BallsClock.seconds() <= .550 && gamepad1.triangle == true) { + turnTableServo.setPosition(1); + goofyAhhhhFrontDoor.setPosition(1); + intakeCount = 3; + } + } else if (intakeCount == 3 && (!intakeBump1.isPressed() || intakeBump2.isPressed()) && gamepad1.triangle == true) { + intake3BallsClock.reset(); + if (intake3BallsClock.seconds() >= 0 && intake3BallsClock.seconds() <= .500 && gamepad1.triangle == true) { + goofyAhhhhFrontDoor.setPosition(0); + } + + if (intake3BallsClock.seconds() >= .500 && intake3BallsClock.seconds() <= .510 && gamepad1.triangle == true) { + goofyAhhhhFrontDoor.setPosition(.5); + } + + if (intake3BallsClock.seconds() >= .510 && intake3BallsClock.seconds() <= .550 && gamepad1.triangle == true) { + turnTableServo.setPosition(1); + goofyAhhhhFrontDoor.setPosition(.5); + intakeCount = 0; + intakeOff(); + } + + + } + + if (gamepad1.triangle == false && resetTrig == true) { + intakeCount = 0; + goofyAhhhhFrontDoor.setPosition(.5); + intakeOff(); + safeTrig = true; + resetTrig = false; + } + + */ + + + } + + + + public void intakeOn () { + intakeMotor.setPower(0.8); + } + + public void intakeOff () { + intakeMotor.setPower(0); + + } + + public void moveForwardTics ( double Speed, double tic){ + pinpoint.update(); + double xvalue = pinpoint.getEncoderX(); + while (xvalue - pinpoint.getEncoderX() <= tic) { + pinpoint.update(); + leftBack.setPower(-Speed); + leftFront.setPower(-Speed); + rightBack.setPower(-Speed); + rightFront.setPower(-Speed); + } + leftBack.setPower(0); + leftFront.setPower(0); + rightBack.setPower(0); + rightFront.setPower(0); + } + + } + + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test.java new file mode 100644 index 000000000000..d0045ee9ddbf --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Test.java @@ -0,0 +1,20 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import java.util.ArrayList; + +@Autonomous(name = "Test", group = "Testing") +public class Test extends OpMode { + + @Override + public void init() { + + } + + @Override + public void loop() { + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java index fa30420ac7cd..cae3b23f0324 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java @@ -1,19 +1,88 @@ package org.firstinspires.ftc.teamcode.pedroPathing; +import com.pedropathing.control.FilteredPIDFCoefficients; +import com.pedropathing.control.PIDFCoefficients; import com.pedropathing.follower.Follower; import com.pedropathing.follower.FollowerConstants; import com.pedropathing.ftc.FollowerBuilder; +import com.pedropathing.ftc.drivetrains.MecanumConstants; +import com.pedropathing.ftc.localization.constants.PinpointConstants; import com.pedropathing.paths.PathConstraints; + +import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + public class Constants { - public static FollowerConstants followerConstants = new FollowerConstants(); + public static FollowerConstants followerConstants = new FollowerConstants() + .mass(10.8862) //mass in Kilos + + .forwardZeroPowerAcceleration(-44.792129192214446) //Deceleration on the forward axis + .lateralZeroPowerAcceleration(-93.72450685019226) //Deceleration on the lateral axis + + .translationalPIDFCoefficients( //Adjusts how the robot moves side to side to correct it y-error + new PIDFCoefficients( + 0.1, + 0, + 0.00, + 0.025 + ) + ) + .headingPIDFCoefficients( //Adjusts how the robot turns to correct it's heading error + new PIDFCoefficients( + 1.5, + 0, + 0.00, + 0.01 + ) + ) + // TODO: 3/3/2026 everything below this point in follower constants in untuned and at default values + .drivePIDFCoefficients( //Adjusts how the robot moves forwards and backwards to correct it's x-error? more testing needed to confirm + new FilteredPIDFCoefficients( + 0.1, + 0.0, + 0.005, + 0.6, + 0.0 + ) + ) + .centripetalScaling(0.005) //Adjusts how the robot follows curves + ; + + public static MecanumConstants driveConstants = new MecanumConstants() + //Sets robot's max power + .maxPower(1) + //Sets up motors in Pedro Pathing hardware map + .rightFrontMotorName("rightFront") + .rightRearMotorName("rightBack") + .leftRearMotorName("leftBack") + .leftFrontMotorName("leftFront") + //Sets up motor directions + .leftFrontMotorDirection(DcMotorSimple.Direction.REVERSE) + .leftRearMotorDirection(DcMotorSimple.Direction.FORWARD) + .rightFrontMotorDirection(DcMotorSimple.Direction.REVERSE) + .rightRearMotorDirection(DcMotorSimple.Direction.REVERSE) + //Sets the robot's max velocity on the x and y axis + .xVelocity(79.59548157218873) //Velocity on the forward axis + .yVelocity(52.49261234313485); //Velocity on the lateral axis + public static PinpointConstants localizerConstants = new PinpointConstants() + .forwardPodY(0.625000528 + 3) //Inches from center of rotation on it's respective axis + .strafePodX(5.1349999548) //^^^ + .distanceUnit(DistanceUnit.INCH) //Unit for measuring distance + .hardwareMapName("pinpoint") //Hardware map setup for Pedro Pathing localizer + .encoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD) //Sets the encoder resolution to a preset for GoBilda Pinpoint + .forwardEncoderDirection(GoBildaPinpointDriver.EncoderDirection.FORWARD) //Direction that the encoder records data in + .strafeEncoderDirection(GoBildaPinpointDriver.EncoderDirection.REVERSED); //^^^ public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1, 1); public static Follower createFollower(HardwareMap hardwareMap) { return new FollowerBuilder(followerConstants, hardwareMap) + .pinpointLocalizer(localizerConstants) .pathConstraints(pathConstraints) + .mecanumDrivetrain(driveConstants) .build(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java index b31f6f596965..cdeffdc195d6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java @@ -144,7 +144,7 @@ class LocalizationTest extends OpMode { @Override public void init() { - follower.setStartingPose(new Pose(72,72)); + follower.setStartingPose(new Pose(0,0)); } /** This initializes the PoseUpdater, the drive motors, and the Panels telemetry. */ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadRunner/Drawing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadRunner/Drawing.java new file mode 100644 index 000000000000..e43b6cd33ba7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadRunner/Drawing.java @@ -0,0 +1,23 @@ +package org.firstinspires.ftc.teamcode.roadRunner; + +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.Vector2d; + +@SuppressWarnings("All") +public final class Drawing { + private Drawing() {} + + + public static void drawRobot(Canvas c, Pose2d t) { + final double ROBOT_RADIUS = 9; + + c.setStrokeWidth(1); + c.strokeCircle(t.position.x, t.position.y, ROBOT_RADIUS); + + Vector2d halfv = t.heading.vec().times(0.5 * ROBOT_RADIUS); + Vector2d p1 = t.position.plus(halfv); + Vector2d p2 = p1.plus(halfv); + c.strokeLine(p1.x, p1.y, p2.x, p2.y); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadRunner/Localizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadRunner/Localizer.java new file mode 100644 index 000000000000..9ed8d17940d7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadRunner/Localizer.java @@ -0,0 +1,27 @@ +package org.firstinspires.ftc.teamcode.roadRunner; + +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.PoseVelocity2d; + +@SuppressWarnings("ALL") + +/** + * Interface for localization methods. + */ +public interface Localizer { + void setPose(Pose2d pose); + + /** + * Returns the current pose estimate. + * NOTE: Does not update the pose estimate; + * you must call update() to update the pose estimate. + * @return the Localizer's current pose + */ + Pose2d getPose(); + + /** + * Updates the Localizer's pose estimate. + * @return the Localizer's current velocity estimate + */ + PoseVelocity2d update(); +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadRunner/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadRunner/MecanumDrive.java new file mode 100644 index 000000000000..14b795e2a5d7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadRunner/MecanumDrive.java @@ -0,0 +1,502 @@ +package org.firstinspires.ftc.teamcode.roadRunner; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.canvas.Canvas; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.*; +import com.acmerobotics.roadrunner.AngularVelConstraint; +import com.acmerobotics.roadrunner.DualNum; +import com.acmerobotics.roadrunner.HolonomicController; +import com.acmerobotics.roadrunner.MecanumKinematics; +import com.acmerobotics.roadrunner.MinVelConstraint; +import com.acmerobotics.roadrunner.MotorFeedforward; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.Pose2dDual; +import com.acmerobotics.roadrunner.ProfileAccelConstraint; +import com.acmerobotics.roadrunner.Time; +import com.acmerobotics.roadrunner.TimeTrajectory; +import com.acmerobotics.roadrunner.TimeTurn; +import com.acmerobotics.roadrunner.TrajectoryActionBuilder; +import com.acmerobotics.roadrunner.TurnConstraints; +import com.acmerobotics.roadrunner.VelConstraint; +import com.acmerobotics.roadrunner.ftc.DownsampledWriter; +import com.acmerobotics.roadrunner.ftc.Encoder; +import com.acmerobotics.roadrunner.ftc.FlightRecorder; +import com.acmerobotics.roadrunner.ftc.LazyHardwareMapImu; +import com.acmerobotics.roadrunner.ftc.LazyImu; +import com.acmerobotics.roadrunner.ftc.OverflowEncoder; +import com.acmerobotics.roadrunner.ftc.PositionVelocityPair; +import com.acmerobotics.roadrunner.ftc.RawEncoder; +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.VoltageSensor; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import org.firstinspires.ftc.teamcode.roadRunner.messages.DriveCommandMessage; +import org.firstinspires.ftc.teamcode.roadRunner.messages.MecanumCommandMessage; +import org.firstinspires.ftc.teamcode.roadRunner.messages.MecanumLocalizerInputsMessage; +import org.firstinspires.ftc.teamcode.roadRunner.messages.PoseMessage; + +import java.lang.Math; +import java.util.Arrays; +import java.util.LinkedList; +import java.util.List; + +@SuppressWarnings("ALL") +@Config +public final class MecanumDrive { + public static class Params { + // IMU orientation + // TODO: fill in these values based on + // see https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html?highlight=imu#physical-hub-mounting + public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection = + RevHubOrientationOnRobot.LogoFacingDirection.FORWARD; + public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection = + RevHubOrientationOnRobot.UsbFacingDirection.LEFT; + + // drive model parameters + public double inPerTick = 0.001978; + public double lateralInPerTick = inPerTick; + public double trackWidthTicks = 6262.2113440294725; + + // feedforward parameters (in tick units) + public double kS = 1.9879744500408736; + public double kV = 0.00017877067614429902; + public double kA = 0.0000825; + + // path profile parameters (in inches) + public double maxWheelVel = 50; + public double minProfileAccel = -30; + public double maxProfileAccel = 50; + + // turn profile parameters (in radians) + public double maxAngVel = Math.PI; // shared with path + public double maxAngAccel = Math.PI; + + // path controller gains + public double axialGain = 2.0; + public double lateralGain = 0.01; + public double headingGain = 5.7; // shared with turn + + public double axialVelGain = 3.5; + public double lateralVelGain = 0.5; + public double headingVelGain = 0.3; // shared with turn + } + + + public static Params PARAMS = new Params(); + + public final MecanumKinematics kinematics = new MecanumKinematics( + PARAMS.inPerTick * PARAMS.trackWidthTicks, PARAMS.inPerTick / PARAMS.lateralInPerTick); + + public final TurnConstraints defaultTurnConstraints = new TurnConstraints( + PARAMS.maxAngVel, -PARAMS.maxAngAccel, PARAMS.maxAngAccel); + public final VelConstraint defaultVelConstraint = + new MinVelConstraint(Arrays.asList( + kinematics.new WheelVelConstraint(PARAMS.maxWheelVel), + new AngularVelConstraint(PARAMS.maxAngVel) + )); + public final AccelConstraint defaultAccelConstraint = + new ProfileAccelConstraint(PARAMS.minProfileAccel, PARAMS.maxProfileAccel); + + public final DcMotorEx leftFront, leftBack, rightBack, rightFront; + + public final VoltageSensor voltageSensor; + + public final LazyImu lazyImu; + + public final Localizer localizer; + private final LinkedList poseHistory = new LinkedList<>(); + + private final DownsampledWriter estimatedPoseWriter = new DownsampledWriter("ESTIMATED_POSE", 50_000_000); + private final DownsampledWriter targetPoseWriter = new DownsampledWriter("TARGET_POSE", 50_000_000); + private final DownsampledWriter driveCommandWriter = new DownsampledWriter("DRIVE_COMMAND", 50_000_000); + private final DownsampledWriter mecanumCommandWriter = new DownsampledWriter("MECANUM_COMMAND", 50_000_000); + + public class DriveLocalizer implements Localizer { + public final Encoder leftFront, leftBack, rightBack, rightFront; + public final IMU imu; + + private int lastLeftFrontPos, lastLeftBackPos, lastRightBackPos, lastRightFrontPos; + private Rotation2d lastHeading; + private boolean initialized; + private Pose2d pose; + + public DriveLocalizer(Pose2d pose) { + leftFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftFront)); + leftBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftBack)); + rightBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightBack)); + rightFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightFront)); + + imu = lazyImu.get(); + + // TODO: reverse encoders if needed + // leftFront.setDirection(DcMotorSimple.Direction.REVERSE); + + this.pose = pose; + } + + @Override + public void setPose(Pose2d pose) { + this.pose = pose; + } + + @Override + public Pose2d getPose() { + return pose; + } + + @Override + public PoseVelocity2d update() { + PositionVelocityPair leftFrontPosVel = leftFront.getPositionAndVelocity(); + PositionVelocityPair leftBackPosVel = leftBack.getPositionAndVelocity(); + PositionVelocityPair rightBackPosVel = rightBack.getPositionAndVelocity(); + PositionVelocityPair rightFrontPosVel = rightFront.getPositionAndVelocity(); + + YawPitchRollAngles angles = imu.getRobotYawPitchRollAngles(); + + FlightRecorder.write("MECANUM_LOCALIZER_INPUTS", new MecanumLocalizerInputsMessage( + leftFrontPosVel, leftBackPosVel, rightBackPosVel, rightFrontPosVel, angles)); + + Rotation2d heading = Rotation2d.exp(angles.getYaw(AngleUnit.RADIANS)); + + if (!initialized) { + initialized = true; + + lastLeftFrontPos = leftFrontPosVel.position; + lastLeftBackPos = leftBackPosVel.position; + lastRightBackPos = rightBackPosVel.position; + lastRightFrontPos = rightFrontPosVel.position; + + lastHeading = heading; + + return new PoseVelocity2d(new Vector2d(0.0, 0.0), 0.0); + } + + double headingDelta = heading.minus(lastHeading); + Twist2dDual