m) {
+ mX = x;
+ mY = y;
+ mM = m;
+ }
+
+ public InterpLUT() {
+ }
+
+ /**
+ * Adds a control point to the LUT
+ */
+ public void add(double input, double output) {
+ mX.add(input);
+ mY.add(output);
+ }
+
+ /**
+ * Creates a monotone cubic spline from a given set of control points.
+ *
+ *
+ * The spline is guaranteed to pass through each control point exactly. Moreover, assuming the control points are
+ * monotonic (Y is non-decreasing or non-increasing) then the interpolated values will also be monotonic.
+ *
+ * @throws IllegalArgumentException if the X or Y arrays are null, have different lengths or have fewer than 2 values.
+ */
+ //public static LUTWithInterpolator createLUT(List x, List y) {
+ public void createLUT() {
+ List x = this.mX;
+ List y = this.mY;
+
+ if (x == null || y == null || x.size() != y.size() || x.size() < 2) {
+ throw new IllegalArgumentException("There must be at least two control "
+ + "points and the arrays must be of equal length.");
+ }
+
+ final int n = x.size();
+ Double[] d = new Double[n - 1]; // could optimize this out
+ Double[] m = new Double[n];
+
+ // Compute slopes of secant lines between successive points.
+ for (int i = 0; i < n - 1; i++) {
+ Double h = x.get(i + 1) - x.get(i);
+ if (h <= 0f) {
+ throw new IllegalArgumentException("The control points must all "
+ + "have strictly increasing X values.");
+ }
+ d[i] = (y.get(i + 1) - y.get(i)) / h;
+ }
+
+ // Initialize the tangents as the average of the secants.
+ m[0] = d[0];
+ for (int i = 1; i < n - 1; i++) {
+ m[i] = (d[i - 1] + d[i]) * 0.5f;
+ }
+ m[n - 1] = d[n - 2];
+
+ // Update the tangents to preserve monotonicity.
+ for (int i = 0; i < n - 1; i++) {
+ if (d[i] == 0f) { // successive Y values are equal
+ m[i] = Double.valueOf(0f);
+ m[i + 1] = Double.valueOf(0f);
+ } else {
+ double a = m[i] / d[i];
+ double b = m[i + 1] / d[i];
+ double h = Math.hypot(a, b);
+ if (h > 9f) {
+ double t = 3f / h;
+ m[i] = t * a * d[i];
+ m[i + 1] = t * b * d[i];
+ }
+ }
+ }
+ mX = x;
+ mY = y;
+ mM = Arrays.asList(m);
+ }
+
+ /**
+ * Interpolates the value of Y = f(X) for given X. Clamps X to the domain of the spline.
+ *
+ * @param input The X value.
+ * @return The interpolated Y = f(X) value.
+ */
+ public double get(double input) {
+ // Handle the boundary cases.
+ final int n = mX.size();
+ if (Double.isNaN(input)) {
+ return input;
+ }
+ if (input <= mX.get(0)) {
+ throw new IllegalArgumentException("User requested value outside of bounds of LUT. Bounds are: " + mX.get(0).toString() + " to " + mX.get(n - 1).toString() + ". Value provided was: " + input);
+ }
+ if (input >= mX.get(n - 1)) {
+ throw new IllegalArgumentException("User requested value outside of bounds of LUT. Bounds are: " + mX.get(0).toString() + " to " + mX.get(n - 1).toString() + ". Value provided was: " + input);
+ }
+
+ // Find the index 'i' of the last point with smaller X.
+ // We know this will be within the spline due to the boundary tests.
+ int i = 0;
+ while (input >= mX.get(i + 1)) {
+ i += 1;
+ if (input == mX.get(i)) {
+ return mY.get(i);
+ }
+ }
+
+ // Perform cubic Hermite spline interpolation.
+ double h = mX.get(i + 1) - mX.get(i);
+ double t = (input - mX.get(i)) / h;
+ return (mY.get(i) * (1 + 2 * t) + h * mM.get(i) * t) * (1 - t) * (1 - t)
+ + (mY.get(i + 1) * (3 - 2 * t) + h * mM.get(i + 1) * (t - 1)) * t * t;
+ }
+
+ // For debugging.
+ @Override
+ public String toString() {
+ StringBuilder str = new StringBuilder();
+ final int n = mX.size();
+ str.append("[");
+ for (int i = 0; i < n; i++) {
+ if (i != 0) {
+ str.append(", ");
+ }
+ str.append("(").append(mX.get(i));
+ str.append(", ").append(mY.get(i));
+ str.append(": ").append(mM.get(i)).append(")");
+ }
+ str.append("]");
+ return str.toString();
+ }
+
+ }
+
+ public class InterpLUTPoint{
+ public InterpLUTPoint(
+ double X,
+ double Y,
+ String Name
+ ){
+
+ }
+ }
+
+ public enum telemetrySortCategory {
+ DRIVETRAIN,
+ INTAKE,
+ TRANSFER,
+ TURRET,
+ TESTINGVALUES,
+ OTHER,
+ }
+
+ /**
+ * This class is for the runtime per-loop sorting and displaying of every single TelemetryObject.
+ * There should only be one of these per program.
+ */
+ public class TelemetryManager{
+ List telemtryList = new ArrayList<>();
+ List sortedTelemetryList = new ArrayList<>();
+
+ public void addLine(TelemetryObject telemetryline){
+ telemtryList.add(telemetryline);
+ }
+
+ public void updateTelemetry(){
+ //sortedTelemetryList = telemtryList.sort(Comparator.comparing(TelemetryObject::getName));
+ //for(TelemetryObject telemObj : telemtryList){
+
+ //}
+ }
+ }
+
+ /**
+ * This class represents a single line of telemetry, which will be sorted and managed by a TelemetryManager.
+ * It will contain all the relevant data for lookup and sorting, as well as functionality for live runtime editing of keys and values
+ */
+ public class TelemetryObject{
+
+ Object name;
+ Object key;
+ Object value;
+ telemetrySortCategory category;
+
+ public TelemetryObject(Object initialName,
+ Object initialKey,
+ Object initialValue,
+ telemetrySortCategory telemCategory,
+ TelemetryManager telemetryManagerParent){
+ name = initialName;
+ key = initialKey;
+ value = initialValue;
+ category = telemCategory;
+ }
+
+ public Object getName(){
+ return name;
+ }
+ public Object getKey(){
+ return key;
+ }
+ public Object getValue(){
+ return value;
+ }
+ public telemetrySortCategory getTelemCategory(){
+ return category;
+ }
+
+ }
+
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Notes and TODOs b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Notes and TODOs
new file mode 100644
index 000000000000..d5b4ec13edc1
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Notes and TODOs
@@ -0,0 +1,35 @@
+//TODO, implement all our external libraries and functionality.
+
+//DONE, Implement controls graph to TeleOp
+
+//DONE, Consolidate the generic actuator functionalities folder into one file
+
+//TODO, Organize telemetry before adding proper telem system
+
+//TODO, Add a more advanced telemetry handler for better organization, readability, and debugging
+
+//DONE, Add proper functionality for manual turret control to Turret.java
+
+//DONE, Shoot mode state machine Off->On->LastShot->Delay->ResetActuators->Off etc. cycled with B button on Gamepad2
+
+//TODO, Reset the belt, intake, & transfer kicker back to private after December 6th
+
+//TODO, add the new code structure graph, control graph, and gamepad HTML to the development branch Readme
+
+//DONE, Finish translating Dylan's December 6th controls into the program.
+
+//DONE, Figure out how to use NextFTC's Pedropathing extension for auto.
+
+
+
+//1. TODO, test the auto paths used in the generic autos - ARI WORKING ON IT!
+//2. TODO, create two generic autos for testing and competition
+//3. TODO, test the turret rotation autoaim with shooting
+//4. TODO, establish static (non-changing) flywheel speeds for near/far zones
+//5. TODO, make hood autoaim using InterpLUT
+//6. TODO, test turret hood and rotation autoaim together
+//7. TODO, test turret autoaim with the generic autos - ADD THE TURRET CONTROL FUNCTIONALITY TO COMMANDS
+
+// TODO: create paths to pick up human player zone Artifacts and to open the gate
+
+//TODO Link the PID update to the flywheel state so it can float instead of break at zero power
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Testing_/PedroDriveToAprilTag.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Testing_/PedroDriveToAprilTag.java
new file mode 100644
index 000000000000..e61e6cbb6d4d
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Testing_/PedroDriveToAprilTag.java
@@ -0,0 +1,291 @@
+/* Copyright (c) 2023 FIRST. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted (subject to the limitations in the disclaimer below) provided that
+ * the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list
+ * of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice, this
+ * list of conditions and the following disclaimer in the documentation and/or
+ * other materials provided with the distribution.
+ *
+ * Neither the name of FIRST nor the names of its contributors may be used to endorse or
+ * promote products derived from this software without specific prior written permission.
+ *
+ * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND NEAR ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+package org.firstinspires.ftc.teamcode.LOADCode.Testing_;
+
+import com.bylazar.configurables.annotations.IgnoreConfigurable;
+import com.bylazar.telemetry.TelemetryManager;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.Pose;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.util.Range;
+
+import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.ExposureControl;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.GainControl;
+import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
+import org.firstinspires.ftc.vision.VisionPortal;
+import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
+import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
+
+import java.util.List;
+import java.util.concurrent.TimeUnit;
+
+/*
+ * This OpMode illustrates using a camera to locate and drive towards a specific AprilTag.
+ * The code assumes a Holonomic (Mecanum or X Drive) Robot.
+ *
+ * For an introduction to AprilTags, see the ftc-docs link below:
+ * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html
+ *
+ * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera.
+ * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below.
+ * https://ftc-docs.firstinspires.org/apriltag-detection-values
+ *
+ * The drive goal is to rotate to keep the Tag centered in the camera, while strafing to be directly in front of the tag, and
+ * driving towards the tag to achieve the desired distance.
+ * To reduce any motion blur (which will interrupt the detection process) the Camera exposure is reduced to a very low value (5mS)
+ * You can determine the best Exposure and Gain values by using the ConceptAprilTagOptimizeExposure OpMode in this Samples folder.
+ *
+ * The code assumes a Robot Configuration with motors named: front_left_drive and front_right_drive, back_left_drive and back_right_drive.
+ * The motor directions must be set so a positive power goes forward on all wheels.
+ * This sample assumes that the current game AprilTag Library (usually for the current season) is being loaded by default,
+ * so you should choose to approach a valid tag ID.
+ *
+ * Under manual control, the left stick will move forward/back & left/right. The right stick will rotate the robot.
+ * Manually drive the robot until it displays Target data on the Driver Station.
+ *
+ * Press and hold the *Left Bumper* to enable the automatic "Drive to target" mode.
+ * Release the Left Bumper to return to manual driving mode.
+ *
+ * Under "Drive To Target" mode, the robot has three goals:
+ * 1) Turn the robot to always keep the Tag centered on the camera frame. (Use the Target Bearing to turn the robot.)
+ * 2) Strafe the robot towards the centerline of the Tag, so it approaches directly in front of the tag. (Use the Target Yaw to strafe the robot)
+ * 3) Drive towards the Tag to get to the desired distance. (Use Tag Range to drive the robot forward/backward)
+ *
+ * Use DESIRED_DISTANCE to set how close you want the robot to get to the target.
+ * Speed and Turn sensitivity can be adjusted using the SPEED_GAIN, STRAFE_GAIN and TURN_GAIN constants.
+ *
+ * Use Android Studio to Copy this Class, and Paste it into the TeamCode/src/main/java/org/firstinspires/ftc/teamcode folder.
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
+ *
+ */
+@TeleOp(name="Follow AprilTag", group = "TestTeleOp")
+public class PedroDriveToAprilTag extends LinearOpMode
+{
+ // Adjust these numbers to suit your robot.
+ final double DESIRED_DISTANCE = 60.0; // this is how close the camera should get to the target (inches)
+
+ // Set the GAIN constants to control the relationship between the measured position error, and how much power is
+ // applied to the drive motors to correct the error.
+ // Drive = Error * Gain Make these values smaller for smoother control, or larger for a more aggressive response.
+ final double SPEED_GAIN = 0.04 ; // Forward Speed Control "Gain". e.g. Ramp up to 50% power at a 25 inch error. (0.50 / 25.0)
+ final double STRAFE_GAIN = 0.04 ; // Strafe Speed Control "Gain". e.g. Ramp up to 37% power at a 25 degree Yaw error. (0.375 / 25.0)
+ final double TURN_GAIN = 0.04 ; // Turn Control "Gain". e.g. Ramp up to 25% power at a 25 degree error. (0.25 / 25.0)
+
+ final double MAX_AUTO_SPEED = 1; // Clip the approach speed to this max value (adjust for your robot)
+ final double MAX_AUTO_STRAFE= 1; // Clip the strafing speed to this max value (adjust for your robot)
+ final double MAX_AUTO_TURN = 1; // Clip the turn speed to this max value (adjust for your robot)
+
+ private static final boolean USE_WEBCAM = true; // Set true to use a webcam, or false for a phone camera
+ private static final int DESIRED_TAG_ID = -1; // Choose the tag you want to approach or set to -1 for ANY tag.
+ private VisionPortal visionPortal; // Used to manage the video source.
+ private AprilTagProcessor aprilTag; // Used for managing the AprilTag detection process.
+ private AprilTagDetection desiredTag = null; // Used to hold the data for a detected AprilTag
+
+ private boolean toggle = false;
+ private boolean oldToggleVal = false;
+
+ public static Follower follower;
+ @IgnoreConfigurable
+ static TelemetryManager telemetryM;
+ /** Start Pose of our robot. This can be changed or saved from the autonomous period. */
+ private final Pose startPose = new Pose(0,0, Math.toRadians(0));
+
+ @Override public void runOpMode()
+ {
+ boolean targetFound; // Set to true when an AprilTag target is detected
+ double drive; // Desired forward power/speed (-1 to +1)
+ double strafe; // Desired strafe power/speed (-1 to +1)
+ double turn; // Desired turning power/speed (-1 to +1)
+
+ // Initialize the Apriltag Detection process
+ initAprilTag();
+
+ if (USE_WEBCAM)
+ setManualExposure(6, 250); // Use low exposure time to reduce motion blur
+
+ follower = Constants.createFollower(hardwareMap);
+ follower.setStartingPose(startPose);
+ follower.update();
+
+ // Wait for driver to press start
+ telemetry.addData("Camera preview on/off", "3 dots, Camera Stream");
+ telemetry.addData(">", "Touch START to start OpMode");
+ telemetry.update();
+ waitForStart();
+
+ follower.startTeleopDrive();
+ follower.update();
+
+ while (opModeIsActive())
+ {
+ targetFound = false;
+ desiredTag = null;
+
+ // Step through the list of detected tags and look for a matching tag
+ List currentDetections = aprilTag.getDetections();
+ for (AprilTagDetection detection : currentDetections) {
+ // Look to see if we have size info on this tag.
+ if (detection.metadata != null) {
+ // Check to see if we want to track towards this tag.
+ if ((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID)) {
+ // Yes, we want to use this tag.
+ targetFound = true;
+ desiredTag = detection;
+ break; // don't look any further.
+ } else {
+ // This tag is in the library, but we do not want to track it right now.
+ telemetry.addData("Skipping", "Tag ID %d is not desired", detection.id);
+ }
+ } else {
+ // This tag is NOT in the library, so we don't have enough information to track to it.
+ telemetry.addData("Unknown", "Tag ID %d is not in TagLibrary", detection.id);
+ }
+ }
+
+ // Tell the driver what we see, and what to do.
+ if (targetFound) {
+ telemetry.addData("\n>","HOLD Left-Bumper to Drive to Target\n");
+ telemetry.addData("Found", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name);
+ telemetry.addData("Range", "%5.1f inches", desiredTag.ftcPose.range);
+ telemetry.addData("Bearing","%3.0f degrees", desiredTag.ftcPose.bearing);
+ telemetry.addData("Yaw","%3.0f degrees", desiredTag.ftcPose.yaw);
+ } else {
+ telemetry.addData("\n>","Drive using joysticks to find valid target\n");
+ }
+
+ if (gamepad1.b && !oldToggleVal){
+ toggle = !toggle;
+ oldToggleVal = true;
+ }else if (!gamepad1.b && oldToggleVal){
+ oldToggleVal = false;
+ }
+
+ // If Left Bumper is being pressed, AND we have found the desired target, Drive to target Automatically .
+ if (toggle && targetFound) {
+
+ // Determine heading, range and Yaw (tag image rotation) error so we can use them to control the robot automatically.
+ double rangeError = (desiredTag.ftcPose.range - DESIRED_DISTANCE);
+ double headingError = desiredTag.ftcPose.bearing;
+ double yawError = desiredTag.ftcPose.yaw;
+
+ // Use the speed and turn "gains" to calculate how we want the robot to move.
+ drive = Range.clip(rangeError * SPEED_GAIN, -MAX_AUTO_SPEED, MAX_AUTO_SPEED);
+ turn = Range.clip(headingError * TURN_GAIN, -MAX_AUTO_TURN, MAX_AUTO_TURN) ;
+ strafe = Range.clip(-yawError * STRAFE_GAIN, -MAX_AUTO_STRAFE, MAX_AUTO_STRAFE);
+
+ telemetry.addData("Auto","Drive %5.2f, Strafe %5.2f, Turn %5.2f ", drive, strafe, turn);
+ } else {
+
+ // drive using manual POV Joystick mode. Slow things down to make the robot more controlable.
+ drive = -gamepad1.left_stick_y / 2.0; // Reduce drive rate to 50%.
+ strafe = -gamepad1.left_stick_x / 2.0; // Reduce strafe rate to 50%.
+ turn = -gamepad1.right_stick_x / 3.0; // Reduce turn rate to 33%.
+ telemetry.addData("Manual","Drive %5.2f, Strafe %5.2f, Turn %5.2f ", drive, strafe, turn);
+ }
+ telemetry.update();
+
+ // Apply desired axes motions to the drivetrain.
+ follower.setTeleOpDrive(drive, strafe, turn, true);
+ follower.update();
+ sleep(10);
+ }
+ }
+
+ /**
+ * Initialize the AprilTag processor.
+ */
+ private void initAprilTag() {
+ // Create the AprilTag processor by using a builder.
+ aprilTag = new AprilTagProcessor.Builder().build();
+
+ // Adjust Image Decimation to trade-off detection-range for detection-rate.
+ // e.g. Some typical detection data using a Logitech C920 WebCam
+ // Decimation = 1 .. Detect 2" Tag from 10 feet away at 10 Frames per second
+ // Decimation = 2 .. Detect 2" Tag from 6 feet away at 22 Frames per second
+ // Decimation = 3 .. Detect 2" Tag from 4 feet away at 30 Frames Per Second
+ // Decimation = 3 .. Detect 5" Tag from 10 feet away at 30 Frames Per Second
+ // Note: Decimation can be changed on-the-fly to adapt during a match.
+ aprilTag.setDecimation(2);
+
+ // Create the vision portal by using a builder.
+ if (USE_WEBCAM) {
+ visionPortal = new VisionPortal.Builder()
+ .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"))
+ .addProcessor(aprilTag)
+ .build();
+ } else {
+ visionPortal = new VisionPortal.Builder()
+ .setCamera(BuiltinCameraDirection.BACK)
+ .addProcessor(aprilTag)
+ .build();
+ }
+ }
+
+ /*
+ Manually set the camera gain and exposure.
+ This can only be called AFTER calling initAprilTag(), and only works for Webcams;
+ */
+ private void setManualExposure(int exposureMS, int gain) {
+ // Wait for the camera to be open, then use the controls
+
+ if (visionPortal == null) {
+ return;
+ }
+
+ // Make sure camera is streaming before we try to set the exposure controls
+ if (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING) {
+ telemetry.addData("Camera", "Waiting");
+ telemetry.update();
+ while (!isStopRequested() && (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING)) {
+ sleep(20);
+ }
+ telemetry.addData("Camera", "Ready");
+ telemetry.update();
+ }
+
+ // Set camera controls unless we are stopping.
+ if (!isStopRequested())
+ {
+ ExposureControl exposureControl = visionPortal.getCameraControl(ExposureControl.class);
+ if (exposureControl.getMode() != ExposureControl.Mode.Manual) {
+ exposureControl.setMode(ExposureControl.Mode.Manual);
+ sleep(50);
+ }
+ exposureControl.setExposure((long)exposureMS, TimeUnit.MILLISECONDS);
+ sleep(20);
+ GainControl gainControl = visionPortal.getCameraControl(GainControl.class);
+ gainControl.setGain(gain);
+ sleep(20);
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Testing_/TurretAprilTagTracking.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Testing_/TurretAprilTagTracking.java
new file mode 100644
index 000000000000..4bf67c5e0d6e
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Testing_/TurretAprilTagTracking.java
@@ -0,0 +1,277 @@
+/* Copyright (c) 2023 FIRST. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted (subject to the limitations in the disclaimer below) provided that
+ * the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list
+ * of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice, this
+ * list of conditions and the following disclaimer in the documentation and/or
+ * other materials provided with the distribution.
+ *
+ * Neither the name of FIRST nor the names of its contributors may be used to endorse or
+ * promote products derived from this software without specific prior written permission.
+ *
+ * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND NEAR ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+package org.firstinspires.ftc.teamcode.LOADCode.Testing_;
+
+import com.bylazar.configurables.annotations.IgnoreConfigurable;
+import com.bylazar.telemetry.TelemetryManager;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.Pose;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.AnalogInput;
+import com.qualcomm.robotcore.hardware.CRServo;
+
+import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.ExposureControl;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.GainControl;
+import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
+import org.firstinspires.ftc.vision.VisionPortal;
+import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
+import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
+
+import java.util.List;
+import java.util.concurrent.TimeUnit;
+
+/*
+ * This OpMode illustrates using a camera to locate and drive towards a specific AprilTag.
+ * The code assumes a Holonomic (Mecanum or X Drive) Robot.
+ *
+ * For an introduction to AprilTags, see the ftc-docs link below:
+ * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html
+ *
+ * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera.
+ * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below.
+ * https://ftc-docs.firstinspires.org/apriltag-detection-values
+ *
+ * The drive goal is to rotate to keep the Tag centered in the camera, while strafing to be directly in front of the tag, and
+ * driving towards the tag to achieve the desired distance.
+ * To reduce any motion blur (which will interrupt the detection process) the Camera exposure is reduced to a very low value (5mS)
+ * You can determine the best Exposure and Gain values by using the ConceptAprilTagOptimizeExposure OpMode in this Samples folder.
+ *
+ * The code assumes a Robot Configuration with motors named: front_left_drive and front_right_drive, back_left_drive and back_right_drive.
+ * The motor directions must be set so a positive power goes forward on all wheels.
+ * This sample assumes that the current game AprilTag Library (usually for the current season) is being loaded by default,
+ * so you should choose to approach a valid tag ID.
+ *
+ * Under manual control, the left stick will move forward/back & left/right. The right stick will rotate the robot.
+ * Manually drive the robot until it displays Target data on the Driver Station.
+ *
+ * Press and hold the *Left Bumper* to enable the automatic "Drive to target" mode.
+ * Release the Left Bumper to return to manual driving mode.
+ *
+ * Under "Drive To Target" mode, the robot has three goals:
+ * 1) Turn the robot to always keep the Tag centered on the camera frame. (Use the Target Bearing to turn the robot.)
+ * 2) Strafe the robot towards the centerline of the Tag, so it approaches directly in front of the tag. (Use the Target Yaw to strafe the robot)
+ * 3) Drive towards the Tag to get to the desired distance. (Use Tag Range to drive the robot forward/backward)
+ *
+ * Use DESIRED_DISTANCE to set how close you want the robot to get to the target.
+ * Speed and Turn sensitivity can be adjusted using the SPEED_GAIN, STRAFE_GAIN and TURN_GAIN constants.
+ *
+ */
+@Disabled
+@TeleOp(name="Turret April-Tag Tracking", group = "TestTeleOp")
+public class TurretAprilTagTracking extends LinearOpMode
+{
+ private static final boolean USE_WEBCAM = true; // Set true to use a webcam, or false for a phone camera
+ private static final int DESIRED_TAG_ID = 24; // Choose the tag you want to approach or set to -1 for ANY tag.
+ private VisionPortal visionPortal; // Used to manage the video source.
+ private AprilTagProcessor aprilTag; // Used for managing the AprilTag detection process.
+ public static Follower follower; // Used for managing the PedroPathing path follower
+ @IgnoreConfigurable
+ public TelemetryManager telemetryM; // Used for putting telemetry data on Panels
+
+ // Contains the start Pose of our robot. This can be changed or saved from the autonomous period.
+ private final Pose startPose = new Pose(60,96, Math.toRadians(0));
+
+ @Override public void runOpMode()
+ {
+ boolean targetFound; // Set to true when an AprilTag target is detected
+ double headingError = 0; // The error in degrees in the turret's angle
+
+ // Used to store the hardware objects for the robot
+ CRServo turret;
+ AnalogInput turretEncoder;
+
+ double turretPos; // Used to store the current angle of the turret
+ double turretPower; // Used to store the calculated power to output to the turret servo
+
+ initAprilTag(); // Initialize the Apriltag Detection process
+
+ if (USE_WEBCAM)
+ setManualExposure(6, 250); // Use low exposure time to reduce motion blur
+
+ follower = Constants.createFollower(hardwareMap); // Initializes the PedroPathing path follower
+ follower.setStartingPose(startPose); // Sets the initial position of the robot on the field
+ follower.update(); // Applies the initialization
+
+ telemetry.addData("Camera preview on/off", "3 dots, Camera Stream");
+ telemetry.addData(">", "Touch START to start OpMode");
+ telemetry.update();
+
+ waitForStart(); // Wait for driver to press start
+
+ follower.startTeleopDrive(); // Activate the manual drive function for the drivetrain
+ follower.update(); // Apply the activation
+
+ // Fetch the actual hardware objects and store them in their respective variables
+ turret = hardwareMap.get(CRServo.class, "turret");
+ turretEncoder = hardwareMap.get(AnalogInput.class,"turretEncoder");
+
+ while (opModeIsActive())
+ {
+ // Used to indicate whether or not a valid AprilTag has been detected
+ targetFound = false;
+ // Used to hold the data for a detected AprilTag
+ AprilTagDetection desiredTag = null;
+
+ // Read the Axon servo encoder position and convert it from a voltage to an angle in degrees
+ turretPos = (turretEncoder.getVoltage() / 3.3) * 360;
+
+ // Step through the list of detected tags and look for a matching tag
+ List currentDetections = aprilTag.getDetections();
+ for (AprilTagDetection detection : currentDetections) {
+ // Look to see if we have size info on this tag.
+ if (detection.metadata != null) {
+ // Check to see if we want to track towards this tag.
+ if ((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID)) {
+ // Yes, we want to use this tag.
+ targetFound = true;
+ desiredTag = detection;
+ // Determine heading error so we can use them to control the robot automatically.
+ headingError = -desiredTag.ftcPose.bearing;
+ break; // don't look any further.
+ } else {
+ // This tag is in the library, but we do not want to track it right now.
+ telemetry.addData("Skipping", "Tag ID %d is not desired", detection.id);
+ }
+ } else {
+ // This tag is NOT in the library, so we don't have enough information to track to it.
+ telemetry.addData("Unknown", "Tag ID %d is not in TagLibrary", detection.id);
+ }
+ }
+
+ // Tell the driver what we see, and what to do.
+ if (targetFound) {
+ telemetry.addData("Found", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name);
+ telemetry.addData("Range", "%5.1f inches", desiredTag.ftcPose.range);
+ telemetry.addData("Bearing","%3.0f degrees", desiredTag.ftcPose.bearing);
+ telemetry.addData("Yaw","%3.0f degrees", desiredTag.ftcPose.yaw);
+ } else {
+ telemetry.addData("\n>","Drive using joysticks to find a valid target\n");
+ }
+
+ // If B on Gamepad1 is being pressed, AND we have found the desired target, automatically aim the turret at the AprilTag.
+ // Otherwise, stop moving
+ if (gamepad1.b) {
+ if (Math.abs(headingError) >= 0.5) {
+ turretPower = Math.pow(Math.min(Math.abs(headingError/7.5),1), 2) * Math.signum(headingError) * 0.2;
+ } else {
+ turretPower = 0;
+ }
+ } else {
+ turretPower = 0;
+ }
+ if(gamepad1.b){
+ turret.setPower(turretPower);
+ }
+
+ // Apply desired axes motions to the drivetrain.
+ follower.setTeleOpDrive(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x/2, true);
+ follower.update();
+
+ telemetry.addData("Turret Power:", turretPower);
+ telemetry.addData("Heading Error:", headingError);
+ telemetry.addData("Absolute Angle:", turretPos);
+
+ telemetry.update();
+ sleep(1);
+ }
+ }
+
+ /**
+ * Initialize the AprilTag processor.
+ */
+ private void initAprilTag() {
+ // Create the AprilTag processor by using a builder.
+ aprilTag = new AprilTagProcessor.Builder().build();
+
+ // Adjust Image Decimation to trade-off detection-range for detection-rate.
+ // e.g. Some typical detection data using a Logitech C920 WebCam
+ // Decimation = 1 .. Detect 2" Tag from 10 feet away at 10 Frames per second
+ // Decimation = 2 .. Detect 2" Tag from 6 feet away at 22 Frames per second
+ // Decimation = 3 .. Detect 2" Tag from 4 feet away at 30 Frames Per Second
+ // Decimation = 3 .. Detect 5" Tag from 10 feet away at 30 Frames Per Second
+ // Note: Decimation can be changed on-the-fly to adapt during a match.
+ aprilTag.setDecimation(3);
+
+ // Create the vision portal by using a builder.
+ if (USE_WEBCAM) {
+ visionPortal = new VisionPortal.Builder()
+ .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"))
+ .addProcessor(aprilTag)
+ .build();
+ } else {
+ visionPortal = new VisionPortal.Builder()
+ .setCamera(BuiltinCameraDirection.BACK)
+ .addProcessor(aprilTag)
+ .build();
+ }
+ }
+
+ /*
+ Manually set the camera gain and exposure.
+ This can only be called AFTER calling initAprilTag(), and only works for Webcams;
+ */
+ private void setManualExposure(int exposureMS, int gain) {
+ // Wait for the camera to be open, then use the controls
+
+ if (visionPortal == null) {
+ return;
+ }
+
+ // Make sure camera is streaming before we try to set the exposure controls
+ if (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING) {
+ telemetry.addData("Camera", "Waiting");
+ telemetry.update();
+ while (!isStopRequested() && (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING)) {
+ sleep(20);
+ }
+ telemetry.addData("Camera", "Ready");
+ telemetry.update();
+ }
+
+ // Set camera controls unless we are stopping.
+ if (!isStopRequested())
+ {
+ ExposureControl exposureControl = visionPortal.getCameraControl(ExposureControl.class);
+ if (exposureControl.getMode() != ExposureControl.Mode.Manual) {
+ exposureControl.setMode(ExposureControl.Mode.Manual);
+ sleep(50);
+ }
+ exposureControl.setExposure((long)exposureMS, TimeUnit.MILLISECONDS);
+ sleep(20);
+ GainControl gainControl = visionPortal.getCameraControl(GainControl.class);
+ gainControl.setGain(gain);
+ sleep(20);
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/Color.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/Color.java
new file mode 100644
index 000000000000..afcc91b81cc0
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/Color.java
@@ -0,0 +1,56 @@
+/* MIT License
+ * Copyright (c) [2025] [Base 10 Assets, LLC]
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.firstinspires.ftc.teamcode.Prism;
+
+public class Color {
+ public int red;
+ public int green;
+ public int blue;
+
+ public Color(int red, int green, int blue)
+ {
+ this.red = Math.min(red, 255);
+ this.green = Math.min(green, 255);
+ this.blue = Math.min(blue, 255);
+ }
+
+ @Override
+ public String toString()
+ {
+ return String.format("%d, %d, %d", red, green, blue);
+ }
+
+ public static final Color RED = new Color(255, 0, 0);
+ public static final Color ORANGE = new Color(255, 165, 0);
+ public static final Color YELLOW = new Color(255, 255, 0);
+ public static final Color OLIVE = new Color(128, 128, 0);
+ public static final Color GREEN = new Color(0, 255, 0);
+ public static final Color CYAN = new Color(0, 255, 255);
+ public static final Color BLUE = new Color(0, 0, 255);
+ public static final Color TEAL = new Color(0, 128, 128);
+ public static final Color MAGENTA = new Color(255, 0, 255);
+ public static final Color PURPLE = new Color(128, 0, 128);
+ public static final Color PINK = new Color(255, 20, 128);
+ public static final Color WHITE = new Color(255, 255, 255);
+ public static final Color TRANSPARENT = new Color(0, 0, 0);
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/Direction.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/Direction.java
new file mode 100644
index 000000000000..44205f3150df
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/Direction.java
@@ -0,0 +1,28 @@
+/* MIT License
+ * Copyright (c) [2025] [Base 10 Assets, LLC]
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.firstinspires.ftc.teamcode.Prism;
+
+public enum Direction {
+ Forward,
+ Backward
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/GoBildaPrismConfigurator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/GoBildaPrismConfigurator.java
new file mode 100644
index 000000000000..c2d1b25c5da7
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/GoBildaPrismConfigurator.java
@@ -0,0 +1,1200 @@
+/* MIT License
+ * Copyright (c) [2025] [Base 10 Assets, LLC]
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.firstinspires.ftc.teamcode;
+
+import static org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.LayerHeight;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.Prism.Color;
+import org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver;
+import org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.Artboard;
+import org.firstinspires.ftc.teamcode.Prism.PrismAnimations;
+import org.firstinspires.ftc.teamcode.Prism.PrismAnimations.AnimationType;
+import org.firstinspires.ftc.teamcode.Prism.PrismAnimations.PoliceLights;
+
+import java.util.concurrent.TimeUnit;
+
+/*
+ * This code creates a "Configurator" UI which exposes a somewhat limited amount of the functionality
+ * available to create animations and artboards to users without needing to work with very much code.
+ *
+ * This file is not meant to serve as example code - unless you're trying to create a telemetry-based
+ * UI for the drivers station.
+ * For example code that your team can leverage in your autonomous/teleop code to recall artboards
+ * check out the GoBildaPrismArtboardExample. Run this code on your robot and use it to create
+ * the artboards you'd like to be able to switch between. This means that switching between animations
+ * can be very fast and easy, and the work of creating animations (and sending this information over
+ * I²C) only has to happen once!
+ *
+ */
+
+@TeleOp(name="Prism Configurator", group="Linear OpMode")
+//@Disabled
+
+public class GoBildaPrismConfigurator extends LinearOpMode {
+
+ GoBildaPrismDriver prism;
+
+ // an enum which is used in a mini state machine to allow the user to set different colors.
+ public enum AnimationColor{
+ PRIMARY_COLOR,
+ SECONDARY_COLOR,
+ TERTIARY_COLOR,
+ }
+ AnimationColor animationColor = AnimationColor.PRIMARY_COLOR;
+
+ // Set a default style for the Police Lights Animation.
+ PoliceLights.PoliceLightsStyle policeLightsStyle = PoliceLights.PoliceLightsStyle.Style1;
+
+ int startPoint = 0; // the start LED for any configured animation
+ int endPoint = 12; // the end LED for a configured animation
+ int brightness = 50; // the brightness of configured animation
+ int period = 1000; // the period of a configured animation
+ float speed = 0.5F; // the speed of a configured animation
+
+ int layerSelector = 0; // an integer used to create a cursor to select a layer
+ int animationSelector = 1; // animation cursor
+ int artboardSelector = 0; // artboard cursor
+
+ String hsbTelemetry; // string meant to send over telemetry that gets updated by hsbViaJoystick()
+ String hueTelemetry; // updated by hueViaJoystick()
+
+ /*
+ * An array of colors passed to the SingleFill animation.
+ */
+ Color[] singleFillColors = {
+ Color.RED, Color.WHITE, Color.BLUE
+ };
+
+ AnimationType selectedAnimation = AnimationType.SOLID; // store the animation that is being selected.
+
+ /*
+ * the enum powering the main state machine that this code moves through, each state represents
+ * a page the user can see displayed via telemetry on the driver's station.
+ */
+ public enum ConfigState{
+ WELCOME_SCREEN,
+ SELECT_LAYER,
+ SET_ENDPOINTS,
+ SELECT_ANIMATION,
+ CONFIGURE_ANIMATION,
+ SET_BRIGHTNESS,
+ SET_SPEED,
+ FORK_IN_THE_ROAD,
+ SAVE_TO_ARTBOARD,
+ COMPLETE;
+ }
+
+ ConfigState configState = ConfigState.WELCOME_SCREEN;
+
+ /*
+ * This is actually a bit of a duplicate of the LayerHeight found in PrismAnimations. This adds
+ * an animation slot which can be stored at each position in the enum. This is not required for
+ * the Prism side, but I want to be able to show a user what animation is stored at what layer
+ * after they've created their first animation.
+ */
+ public enum Layers{
+ LAYER_0 (AnimationType.NONE,0,LayerHeight.LAYER_0),
+ LAYER_1 (AnimationType.NONE,1,LayerHeight.LAYER_1),
+ LAYER_2 (AnimationType.NONE,2,LayerHeight.LAYER_2),
+ LAYER_3 (AnimationType.NONE,3,LayerHeight.LAYER_3),
+ LAYER_4 (AnimationType.NONE,4,LayerHeight.LAYER_4),
+ LAYER_5 (AnimationType.NONE,5,LayerHeight.LAYER_5),
+ LAYER_6 (AnimationType.NONE,6,LayerHeight.LAYER_6),
+ LAYER_7 (AnimationType.NONE,7,LayerHeight.LAYER_7),
+ LAYER_8 (AnimationType.NONE,8,LayerHeight.LAYER_8),
+ LAYER_9 (AnimationType.NONE,9,LayerHeight.LAYER_9);
+
+ private AnimationType animationType;
+ private final int index;
+ private final LayerHeight layerHeight;
+
+ Layers(AnimationType animationType, int index, LayerHeight layerHeight){
+ this.animationType = animationType;
+ this.index = index;
+ this.layerHeight = layerHeight;
+ }
+ }
+
+ /*
+ * This enum captures the kind of speed we can control on the animation.
+ */
+ public enum SpeedType{
+ NO_SPEED,
+ PERIOD_ONLY,
+ SPEED_ONLY,
+ PERIOD_AND_SPEED,
+ }
+
+ Layers selectedLayer = Layers.LAYER_0;
+
+ Artboard selectedArtboard = Artboard.ARTBOARD_0;
+
+ /*
+ * Create each Prism Animation which can be customized by the user.
+ */
+ PrismAnimations.Solid solid = new PrismAnimations.Solid();
+ PrismAnimations.Solid endpointsAnimation = new PrismAnimations.Solid();
+ PrismAnimations.Blink blink = new PrismAnimations.Blink();
+ PrismAnimations.Pulse pulse = new PrismAnimations.Pulse();
+ PrismAnimations.SineWave sineWave = new PrismAnimations.SineWave();
+ PrismAnimations.DroidScan droidScan = new PrismAnimations.DroidScan();
+ PrismAnimations.Rainbow rainbow = new PrismAnimations.Rainbow();
+ PrismAnimations.Snakes snakes = new PrismAnimations.Snakes();
+ PrismAnimations.Random random = new PrismAnimations.Random();
+ PrismAnimations.Sparkle sparkle = new PrismAnimations.Sparkle();
+ PrismAnimations.SingleFill singleFill = new PrismAnimations.SingleFill();
+ PrismAnimations.RainbowSnakes rainbowSnakes = new PrismAnimations.RainbowSnakes();
+ PoliceLights policeLights = new PoliceLights();
+
+
+ @Override
+ public void runOpMode() {
+ prism = hardwareMap.get(GoBildaPrismDriver.class,"prism");
+
+ telemetry.addLine("Welcome to the Prism Configurator, enjoy these fun stats " +
+ "and click the 'Play' button to continue");
+ telemetry.addLine("");
+
+ telemetry.addData("Device ID", prism.getDeviceID());
+ telemetry.addLine(prism.getFirmwareVersionString());
+ telemetry.addLine(prism.getHardwareVersionString());
+ telemetry.addData("Power Cycle Count", prism.getPowerCycleCount());
+ telemetry.addData("Run Time (Minutes)",prism.getRunTime(TimeUnit.MINUTES));
+ telemetry.addData("Run Time (Hours)",prism.getRunTime(TimeUnit.HOURS));
+ telemetry.update();
+
+ // Wait for the game to start (driver presses START)
+ waitForStart();
+ resetRuntime();
+
+ // run until the end of the match (driver presses STOP)
+ while (opModeIsActive()) {
+
+ switch (configState){
+ case WELCOME_SCREEN:
+ welcomeScreen();
+ if(gamepad1.aWasPressed()){
+ //if the user presses "a" we move onto the next state.
+ prism.clearAllAnimations();
+ configState = ConfigState.SELECT_LAYER;
+ }
+ break;
+ case SELECT_LAYER:
+ /*
+ * You can't convince me that this is the correct way to implement a cursor,
+ * but you also can't tell me this one doesn't work.
+ * We allow the user to increment the layerSelecter variable, before constraining
+ * it to within a valid range. We then pass that into our "selectLayer()"
+ * function which displays the UI elements via telemetry and saves the selected
+ * layer every loop.
+ */
+ if(gamepad1.dpadUpWasPressed()){
+ layerSelector -=1;
+ }
+ if(gamepad1.dpadDownWasPressed()){
+ layerSelector +=1;
+ }
+ layerSelector = Math.min(9, Math.max(0,layerSelector));
+ selectLayer(layerSelector);
+ if(gamepad1.aWasPressed()){
+ configureEndpointsAnimation(true);
+ configState = ConfigState.SET_ENDPOINTS;
+ }
+ break;
+ case SET_ENDPOINTS:
+ configureEndPoints();
+ if(gamepad1.aWasPressed()){
+ telemetry.clear();
+ selectAnimation(animationSelector);
+ configState = ConfigState.SELECT_ANIMATION;
+ }
+ if(gamepad1.bWasPressed()){
+ configState = ConfigState.SELECT_LAYER;
+ }
+ break;
+ case SELECT_ANIMATION:
+ if(gamepad1.dpadUpWasPressed()){
+ animationSelector -=1;
+ animationSelector = Math.min(12, Math.max(1,animationSelector));
+ selectAnimation(animationSelector);
+ }
+ if(gamepad1.dpadDownWasPressed()){
+ animationSelector +=1;
+ animationSelector = Math.min(12, Math.max(1,animationSelector));
+ selectAnimation(animationSelector);
+ }
+ if(gamepad1.aWasPressed()){
+ selectedLayer.animationType = selectedAnimation;
+ configState = ConfigState.CONFIGURE_ANIMATION;
+ }
+ if(gamepad1.bWasPressed()){
+ configureEndpointsAnimation(true);
+ configState = ConfigState.SET_ENDPOINTS;
+ }
+ break;
+ case CONFIGURE_ANIMATION:
+ configureAnimation(true,false);
+ if(gamepad1.aWasPressed()){
+ configState = ConfigState.SET_BRIGHTNESS;
+ }
+ if(gamepad1.bWasPressed()){
+ telemetry.clear();
+ selectAnimation(animationSelector);
+ configState = ConfigState.SELECT_ANIMATION;
+ }
+ break;
+ case SET_BRIGHTNESS:
+ configureBrightness();
+ if(gamepad1.aWasPressed()){
+ configState = ConfigState.SET_SPEED;
+ }
+ if(gamepad1.bWasPressed()){
+ configState = ConfigState.CONFIGURE_ANIMATION;
+ }
+ break;
+ case SET_SPEED:
+ if(configureSpeed()){
+ configState = ConfigState.FORK_IN_THE_ROAD;
+ }
+ if(gamepad1.aWasPressed()){
+ configState = ConfigState.FORK_IN_THE_ROAD;
+ }
+ if(gamepad1.bWasPressed()){
+ configState = ConfigState.SET_BRIGHTNESS;
+ }
+ break;
+ case FORK_IN_THE_ROAD:
+ forkInTheRoad();
+ if(gamepad1.aWasPressed()){
+ telemetry.clear();
+ configState = ConfigState.SAVE_TO_ARTBOARD;
+ selectArtboard(artboardSelector);
+ }
+ if(gamepad1.bWasPressed()){
+ if(configureSpeed()){
+ configState = ConfigState.SET_BRIGHTNESS;
+ } else configState = ConfigState.SET_SPEED;
+ }
+ if(gamepad1.yWasPressed()){
+ configState = ConfigState.SELECT_LAYER;
+ }
+ if(gamepad1.xWasPressed()){
+ configState = ConfigState.WELCOME_SCREEN;
+ }
+ break;
+ case SAVE_TO_ARTBOARD:
+ if(gamepad1.dpadUpWasPressed()){
+ artboardSelector -=1;
+ artboardSelector = Math.min(7, Math.max(0,artboardSelector));
+ selectArtboard(artboardSelector);
+ }
+ if(gamepad1.dpadDownWasPressed()){
+ artboardSelector +=1;
+ artboardSelector = Math.min(7, Math.max(0,artboardSelector));
+ selectArtboard(artboardSelector);
+ }
+ if(gamepad1.aWasPressed()){
+ prism.saveCurrentAnimationsToArtboard(selectedArtboard);
+ prism.enableDefaultBootArtboard(true);
+ configState = ConfigState.COMPLETE;
+ }
+ if(gamepad1.bWasPressed()){
+ configState = ConfigState.FORK_IN_THE_ROAD;
+ }
+ break;
+ case COMPLETE:
+ telemetry.addLine("Artboard saved!");
+ telemetry.addLine("");
+ telemetry.addLine("Press X to return to the beginning.");
+ if(gamepad1.xWasPressed()){
+ configState = ConfigState.WELCOME_SCREEN;
+ }
+ break;
+ }
+ telemetry.update();
+ sleep(20);
+ }
+ }
+
+ /*
+ * through this code I try and keep as much code as possible inside of functions, with how
+ * complex our main state machine is, stuff like telemetry is is nice to keep only where we need it.
+ */
+ public void welcomeScreen(){
+ telemetry.addLine("Welcome to the goBILDA Prism Configurator!");
+ telemetry.addLine("Hold on tight - we were cooking when we made this.");
+ telemetry.addLine("");
+ telemetry.addLine("Core to understanding how to use this product is knowing these three terms:");
+ telemetry.addLine("Animations: (Like RAINBOW or BLINK) - These have properties you can configure, " +
+ "like their color. And they can have unique start and end points.");
+ telemetry.addLine("");
+ telemetry.addLine("Layers: There are 10 layers, each of which can store an animation. " +
+ "These are hierarchical. So an Animation on layer 5 will cover an animation on layer 2 " +
+ "if they overlap. You can use start and end points to have layers overlap to create new patterns!" +
+ "Or show multiple animations at once on different LEDs.");
+ telemetry.addLine("");
+ telemetry.addLine("Artboards: An Artboard is a set of 10 layers which is stored on the Prism. " +
+ "you can have up to 8 unique Artboards. Artboards are easy and computationally fast to switch between. " +
+ "We recommend that you configure your Artboards for your robot once and then switch between them.");
+ telemetry.addLine("");
+ telemetry.addLine("Press A to continue");
+ resetStoredAnimations();
+ }
+
+ public void selectLayer(int selector){
+ telemetry.addLine("Select the Layer that you wish to save an Animation to.");
+ telemetry.addLine("Use D-Pad up and D-Pad down to navigate through the layers.");
+ telemetry.addLine("");
+ telemetry.addData("LAYER_0 Animation", Layers.LAYER_0.animationType+layerCursor(Layers.LAYER_0,selector));
+ telemetry.addData("LAYER_1 Animation", Layers.LAYER_1.animationType+layerCursor(Layers.LAYER_1,selector));
+ telemetry.addData("LAYER_2 Animation", Layers.LAYER_2.animationType+layerCursor(Layers.LAYER_2,selector));
+ telemetry.addData("LAYER_3 Animation", Layers.LAYER_3.animationType+layerCursor(Layers.LAYER_3,selector));
+ telemetry.addData("LAYER_4 Animation", Layers.LAYER_4.animationType+layerCursor(Layers.LAYER_4,selector));
+ telemetry.addData("LAYER_5 Animation", Layers.LAYER_5.animationType+layerCursor(Layers.LAYER_5,selector));
+ telemetry.addData("LAYER_6 Animation", Layers.LAYER_6.animationType+layerCursor(Layers.LAYER_6,selector));
+ telemetry.addData("LAYER_7 Animation", Layers.LAYER_7.animationType+layerCursor(Layers.LAYER_7,selector));
+ telemetry.addData("LAYER_8 Animation", Layers.LAYER_8.animationType+layerCursor(Layers.LAYER_8,selector));
+ telemetry.addData("LAYER_9 Animation", Layers.LAYER_9.animationType+layerCursor(Layers.LAYER_9,selector));
+ telemetry.addLine("");
+ telemetry.addLine("Press A to continue");
+ }
+
+ public String layerCursor(Layers layer, int selector){
+ if(layer.index == selector){
+ selectedLayer = layer;
+ return "<--";
+ }else return "";
+ }
+
+ public void selectAnimation(int animationSelector){
+ telemetry.addLine("Select the Animation that you wish to use");
+ telemetry.addLine("Use D-Pad up and D-Pad down to navigate through the Animations.");
+ telemetry.addLine("");
+ telemetry.addData("Solid",animationCursor(AnimationType.SOLID,animationSelector));
+ telemetry.addData("Blink",animationCursor(AnimationType.BLINK,animationSelector));
+ telemetry.addData("Pulse",animationCursor(AnimationType.PULSE,animationSelector));
+ telemetry.addData("Sine Wave",animationCursor(AnimationType.SINE_WAVE,animationSelector));
+ telemetry.addData("Droid Scan",animationCursor(AnimationType.DROID_SCAN,animationSelector));
+ telemetry.addData("Rainbow",animationCursor(AnimationType.RAINBOW,animationSelector));
+ telemetry.addData("Snakes",animationCursor(AnimationType.SNAKES,animationSelector));
+ telemetry.addData("Random",animationCursor(AnimationType.RANDOM,animationSelector));
+ telemetry.addData("Sparkle",animationCursor(AnimationType.SPARKLE,animationSelector));
+ telemetry.addData("Single Fill",animationCursor(AnimationType.SINGLE_FILL,animationSelector));
+ telemetry.addData("Rainbow Snakes",animationCursor(AnimationType.RAINBOW_SNAKES,animationSelector));
+ telemetry.addData("Police Lights",animationCursor(AnimationType.POLICE_LIGHTS,animationSelector));
+ telemetry.addLine("");
+ telemetry.addLine("Press A to continue");
+ telemetry.addLine("Press B to go back");
+ }
+
+ public String animationCursor(AnimationType animationType, int selector){
+ if(animationType.AnimationTypeIndex == selector){
+ selectedAnimation = animationType;
+ configureAnimation(false,true);
+ return "<--";
+ }else return "";
+ }
+
+ public void configureEndPoints(){
+ if(gamepad1.dpadLeftWasPressed()){
+ startPoint -= 1;
+ startPoint = Math.min(Math.min(255,Math.max(0,startPoint)),endPoint-1);
+ configureEndpointsAnimation(false);
+ }
+ if(gamepad1.dpadRightWasPressed()){
+ startPoint += 1;
+ startPoint = Math.min(Math.min(255,Math.max(0,startPoint)),endPoint-1);
+ configureEndpointsAnimation(false);
+ }
+ if(gamepad1.leftBumperWasPressed()){
+ endPoint -= 1;
+ endPoint = Math.max(Math.min(255,Math.max(0,endPoint)),startPoint+1);
+ configureEndpointsAnimation(false);
+ }
+ if(gamepad1.rightBumperWasPressed()){
+ endPoint += 1;
+ endPoint = Math.max(Math.min(255,Math.max(0,endPoint)),startPoint+1);
+ configureEndpointsAnimation(false);
+ }
+
+ telemetry.addLine("Set the start and stop point for each LED");
+ telemetry.addLine("");
+ telemetry.addLine("Use the d-pad to set the start point, d-pad right moves it further from the Prism. " +
+ "d-pad left moves it closer.");
+ telemetry.addLine("Bumpers move the endpoint, The left bumper moves it closer to the Prism, right moves it further.");
+ telemetry.addData("Start Point", startPoint);
+ telemetry.addData("End Point", endPoint);
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ }
+
+ public void configureBrightness(){
+ if(gamepad1.dpadDownWasPressed()){
+ brightness -= 10;
+ brightness = Math.min(100,Math.max(0,brightness));
+ configureAnimation(false,false);
+ }
+ if(gamepad1.dpadUpWasPressed()){
+ brightness += 10;
+ brightness = Math.min(100,Math.max(0,brightness));
+ configureAnimation(false,false);
+ }
+
+ telemetry.addLine("Set the brightness for this animation");
+ telemetry.addLine("");
+ telemetry.addLine("Use the d-pad to adjust the brightness, up increases, down decreases.");
+ telemetry.addLine("");
+ telemetry.addData("Brightness", brightness);
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ }
+
+ /**
+ * Returns: True if we should skip this animation type.
+ */
+ public boolean configureSpeed(){
+ switch (speedFromAnimation(selectedAnimation)){
+ case NO_SPEED: return true;
+ case SPEED_ONLY:
+ if(gamepad1.dpadUpWasPressed()){
+ speed += 0.1f;
+ speed = Math.min(1.0f, Math.max(0,speed));
+ configureAnimation(false,false);
+ }
+ if(gamepad1.dpadDownWasPressed()){
+ speed -= 0.1f;
+ speed = Math.min(1.0f, Math.max(0,speed));
+ configureAnimation(false,false);
+ }
+ telemetry.addLine("Set the speed for this animation from 0 to 1");
+ telemetry.addLine("");
+ telemetry.addLine("Use the d-pad to adjust the speed, up increases, down decreases.");
+ telemetry.addLine("");
+ telemetry.addData("Speed", speed);
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ break;
+ case PERIOD_ONLY:
+ if(gamepad1.dpadUpWasPressed()){
+ if(period < 401){
+ period += 100;
+ } else period += 500;
+ period = Math.min(300000, Math.max(0,period));
+ configureAnimation(false,false);
+ }
+ if(gamepad1.dpadDownWasPressed()){
+ if(period < 501){
+ period -= 100;
+ } else period -= 500;
+ period = Math.min(300000, Math.max(0,period));
+ configureAnimation(false,false);
+ }
+ telemetry.addLine("Set the period for this animation in Milliseconds");
+ telemetry.addLine("");
+ telemetry.addLine("Use the d-pad to adjust the period, up increases, down decreases.");
+ telemetry.addLine("");
+ telemetry.addData("Period", period);
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ break;
+ case PERIOD_AND_SPEED:
+ if(gamepad1.dpadUpWasPressed()){
+ if(period < 401){
+ period += 100;
+ } else period += 500;
+ period = Math.min(300000, Math.max(0,period));
+ configureAnimation(false,false);
+ }
+ if(gamepad1.dpadDownWasPressed()){
+ if(period < 501){
+ period -= 100;
+ } else period -= 500;
+ period = Math.min(300000, Math.max(0,period));
+ configureAnimation(false,false);
+ }
+ if(gamepad1.dpadRightWasPressed()){
+ speed += 0.1f;
+ speed = Math.min(1.0f, Math.max(0,speed));
+ configureAnimation(false,false);
+ }
+ if(gamepad1.dpadLeftWasPressed()){
+ speed -= 0.1f;
+ speed = Math.min(1.0f, Math.max(0,speed));
+ configureAnimation(false,false);
+ }
+ telemetry.addLine("Set the period for this animation in Milliseconds");
+ telemetry.addLine("");
+ telemetry.addLine("Set the speed for this animation from 0-1");
+ telemetry.addLine("");
+ telemetry.addLine("Use the d-pad to adjust the period, up increases, down decreases.");
+ telemetry.addLine("");
+ telemetry.addData("Period", period);
+ telemetry.addLine("");
+ telemetry.addLine("Use the d-pad to adjust the speed, right increases, left decreases.");
+ telemetry.addLine("");
+ telemetry.addData("Speed", speed);
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ break;
+ }
+ return false;
+ }
+
+ public SpeedType speedFromAnimation(AnimationType animationType){
+ if(animationType == AnimationType.BLINK || animationType == AnimationType.PULSE ||
+ animationType == AnimationType.SPARKLE || animationType == AnimationType.POLICE_LIGHTS){
+ return SpeedType.PERIOD_ONLY;
+ }
+ if (animationType == AnimationType.DROID_SCAN || animationType == AnimationType.RAINBOW ||
+ animationType == AnimationType.SNAKES || animationType == AnimationType.RANDOM ||
+ animationType == AnimationType.RAINBOW_SNAKES){
+ return SpeedType.SPEED_ONLY;
+ }
+ if(animationType == AnimationType.SINE_WAVE || animationType == AnimationType.SINGLE_FILL){
+ return SpeedType.PERIOD_AND_SPEED;
+ }
+ else return SpeedType.NO_SPEED;
+ }
+
+ public void forkInTheRoad(){
+ telemetry.addLine("If you are done with your creation, press A to save it to an Artboard.");
+ telemetry.addLine("");
+ telemetry.addLine("To go back, press B.");
+ telemetry.addLine("");
+ telemetry.addLine("If you'd instead like to layer another animation on top of this " +
+ "one, press Y.");
+ telemetry.addLine("");
+ telemetry.addLine("Press X to return to the start and clear currently set animations.");
+ }
+
+ public void selectArtboard(int artboardSelector){
+ telemetry.addLine("Select the Artboard that you wish to save to");
+ telemetry.addLine("Use D-Pad up and D-Pad down to navigate through the Artboards.");
+ telemetry.addLine("");
+ telemetry.addData("Artboard 0",artboardCursor(Artboard.ARTBOARD_0,artboardSelector));
+ telemetry.addData("Artboard 1",artboardCursor(Artboard.ARTBOARD_1,artboardSelector));
+ telemetry.addData("Artboard 2",artboardCursor(Artboard.ARTBOARD_2,artboardSelector));
+ telemetry.addData("Artboard 3",artboardCursor(Artboard.ARTBOARD_3,artboardSelector));
+ telemetry.addData("Artboard 4",artboardCursor(Artboard.ARTBOARD_4,artboardSelector));
+ telemetry.addData("Artboard 5",artboardCursor(Artboard.ARTBOARD_5,artboardSelector));
+ telemetry.addData("Artboard 6",artboardCursor(Artboard.ARTBOARD_6,artboardSelector));
+ telemetry.addData("Artboard 7",artboardCursor(Artboard.ARTBOARD_7,artboardSelector));
+ telemetry.addLine("");
+ telemetry.addLine("Press A to save");
+ telemetry.addLine("Press B to go back");
+ }
+
+ public String artboardCursor(Artboard artboard, int selector){
+ if(artboard.index == selector){
+ selectedArtboard = artboard;
+ return "<--";
+ }else return "";
+ }
+
+ public void configureAnimation(boolean showTelemetry, boolean isBeingInserted){
+ switch(selectedAnimation){
+ case SOLID:
+ configureSolid(showTelemetry, isBeingInserted);
+ break;
+ case BLINK:
+ configureBlink(showTelemetry, isBeingInserted);
+ break;
+ case PULSE:
+ configurePulse(showTelemetry, isBeingInserted);
+ break;
+ case SINE_WAVE:
+ configureSineWave(showTelemetry, isBeingInserted);
+ break;
+ case DROID_SCAN:
+ configureDroidScan(showTelemetry, isBeingInserted);
+ break;
+ case RAINBOW:
+ configureRainbow(showTelemetry, isBeingInserted);
+ break;
+ case SNAKES:
+ configureSnakes(showTelemetry, isBeingInserted);
+ break;
+ case RANDOM:
+ configureRandom(showTelemetry, isBeingInserted);
+ break;
+ case SPARKLE:
+ configureSparkle(showTelemetry, isBeingInserted);
+ break;
+ case SINGLE_FILL:
+ configureSingleFill(showTelemetry, isBeingInserted);
+ break;
+ case RAINBOW_SNAKES:
+ configureRainbowSnakes(showTelemetry, isBeingInserted);
+ break;
+ case POLICE_LIGHTS:
+ configurePoliceLights(showTelemetry, isBeingInserted);
+ break;
+ }
+ }
+
+ public void configureEndpointsAnimation(boolean isBeingInserted){
+ endpointsAnimation.setStartIndex(startPoint);
+ endpointsAnimation.setStopIndex(endPoint);
+ if(isBeingInserted){
+ prism.insertAndUpdateAnimation(selectedLayer.layerHeight, endpointsAnimation);
+ } else {
+ prism.updateAnimationFromIndex(selectedLayer.layerHeight);
+ }
+ }
+
+ /*
+ * Past this point, we have a function to create each type of animation and configure the
+ * specifics to it. As often as I could, you'll find that actions that multiple animations
+ * share (like setPrimaryColor) get abstracted into their own functions (here we have
+ * "hsbViaJoystick) to do the work. This avoids copy and pasting code.
+ */
+
+ public void configureSolid(boolean showTelemetry, boolean isBeingInserted){
+ solid.setPrimaryColor(hsbViaJoystick(solid.getPrimaryColor()));
+ solid.setStartIndex(startPoint);
+ solid.setStopIndex(endPoint);
+ solid.setBrightness(brightness);
+
+ if(isBeingInserted){
+ prism.insertAndUpdateAnimation(selectedLayer.layerHeight, solid);
+ } else {
+ prism.updateAnimationFromIndex(selectedLayer.layerHeight);
+ }
+
+ if(showTelemetry){
+ telemetry.addLine("Selected Animation: Solid");
+ showHsbTelemetry();
+ telemetry.addLine(hsbTelemetry);
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ }
+ }
+
+ public void configureBlink(boolean showTelemetry, boolean isBeingInserted){
+ switch (animationColor){
+ case PRIMARY_COLOR:
+ blink.setPrimaryColor(hsbViaJoystick(blink.getPrimaryColor()));
+ break;
+ case SECONDARY_COLOR:
+ blink.setSecondaryColor(hsbViaJoystick(blink.getSecondaryColor()));
+ break;
+ }
+ toggleThroughColors(gamepad1.yWasPressed(),false);
+
+ blink.setStartIndex(startPoint);
+ blink.setStopIndex(endPoint);
+ blink.setBrightness(brightness);
+ blink.setPeriod(period);
+ blink.setPrimaryColorPeriod(period/2);
+
+ if(isBeingInserted){
+ prism.insertAndUpdateAnimation(selectedLayer.layerHeight, blink);
+ } else {
+ prism.updateAnimationFromIndex(selectedLayer.layerHeight);
+ }
+
+ if(showTelemetry){
+ telemetry.addLine("Selected Animation: Blink");
+ showHsbTelemetry();
+ telemetry.addLine(hsbTelemetry);
+ telemetry.addLine("");
+ telemetry.addLine("Click the Y button to switch between setting the primary and secondary color");
+ telemetry.addLine(animationColor.toString());
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ }
+ }
+
+ public void configurePulse(boolean showTelemetry, boolean isBeingInserted){
+ switch (animationColor){
+ case PRIMARY_COLOR:
+ pulse.setPrimaryColor(hsbViaJoystick(pulse.getPrimaryColor()));
+ break;
+ case SECONDARY_COLOR:
+ pulse.setSecondaryColor(hsbViaJoystick(pulse.getSecondaryColor()));
+ break;
+ }
+
+ toggleThroughColors(gamepad1.yWasPressed(),false);
+
+ pulse.setStartIndex(startPoint);
+ pulse.setStopIndex(endPoint);
+ pulse.setBrightness(brightness);
+ pulse.setPeriod(period);
+
+ if(isBeingInserted){
+ prism.insertAndUpdateAnimation(selectedLayer.layerHeight, pulse);
+ } else {
+ prism.updateAnimationFromIndex(selectedLayer.layerHeight);
+ }
+
+ if(showTelemetry){
+ telemetry.addLine("Selected Animation: Pulse");
+ showHsbTelemetry();
+ telemetry.addLine(hsbTelemetry);
+ telemetry.addLine("");
+ telemetry.addLine("Click the Y button to switch between setting the primary and secondary color");
+ telemetry.addLine(animationColor.toString());
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ }
+ }
+
+ public void configureSineWave(boolean showTelemetry, boolean isBeingInserted){
+ switch (animationColor){
+ case PRIMARY_COLOR:
+ sineWave.setPrimaryColor(hsbViaJoystick(sineWave.getPrimaryColor()));
+ break;
+ case SECONDARY_COLOR:
+ sineWave.setSecondaryColor(hsbViaJoystick(sineWave.getSecondaryColor()));
+ break;
+ }
+
+ toggleThroughColors(gamepad1.yWasPressed(),false);
+
+ sineWave.setStartIndex(startPoint);
+ sineWave.setStopIndex(endPoint);
+ sineWave.setBrightness(brightness);
+ sineWave.setPeriod(period);
+ sineWave.setSpeed(speed);
+
+ if(isBeingInserted){
+ prism.insertAndUpdateAnimation(selectedLayer.layerHeight, sineWave);
+ } else {
+ prism.updateAnimationFromIndex(selectedLayer.layerHeight);
+ }
+
+ if(showTelemetry){
+ telemetry.addLine("Selected Animation: Sine Wave");
+ showHsbTelemetry();
+ telemetry.addLine(hsbTelemetry);
+ telemetry.addLine("");
+ telemetry.addLine("Click the Y button to switch between setting the primary and secondary color");
+ telemetry.addLine(animationColor.toString());
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ }
+ }
+
+ public void configureDroidScan(boolean showTelemetry, boolean isBeingInserted){
+ switch (animationColor){
+ case PRIMARY_COLOR:
+ droidScan.setPrimaryColor(hsbViaJoystick(droidScan.getPrimaryColor()));
+ break;
+ case SECONDARY_COLOR:
+ droidScan.setSecondaryColor(hsbViaJoystick(droidScan.getSecondaryColor()));
+ break;
+ }
+
+ toggleThroughColors(gamepad1.yWasPressed(),false);
+
+ droidScan.setStartIndex(startPoint);
+ droidScan.setStopIndex(endPoint);
+ droidScan.setBrightness(brightness);
+ droidScan.setSpeed(speed);
+
+ if(isBeingInserted){
+ prism.insertAndUpdateAnimation(selectedLayer.layerHeight, droidScan);
+ } else {
+ prism.updateAnimationFromIndex(selectedLayer.layerHeight);
+ }
+
+ if(showTelemetry){
+ telemetry.addLine("Selected Animation: Droid Scan");
+ showHsbTelemetry();
+ telemetry.addLine(hsbTelemetry);
+ telemetry.addLine("");
+ telemetry.addLine("Click the Y button to switch between setting the primary and secondary color");
+ telemetry.addLine(animationColor.toString());
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ }
+ }
+
+ public void configureRainbow(boolean showTelemetry, boolean isBeingInserted){
+ float[] hues = hueViaJoystick(rainbow.getStartHue(),rainbow.getStopHue());
+ rainbow.setStartHue(hues[0]);
+ rainbow.setStopHue(hues[1]);
+
+ rainbow.setStartIndex(startPoint);
+ rainbow.setStopIndex(endPoint);
+ rainbow.setBrightness(brightness);
+ rainbow.setSpeed(speed);
+
+ if(isBeingInserted){
+ prism.insertAndUpdateAnimation(selectedLayer.layerHeight, rainbow);
+ } else {
+ prism.updateAnimationFromIndex(selectedLayer.layerHeight);
+ }
+ if(showTelemetry){
+ telemetry.addLine("Selected Animation: Rainbow");
+ telemetry.addLine("Move the joysticks to configure the range of the rainbow");
+ telemetry.addLine("Left Joystick X (side-to-side) changes the start hue");
+ telemetry.addLine("Right Joystick x (side-to-side) changes the end hue");
+ telemetry.addLine("");
+ telemetry.addLine(hueTelemetry);
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ }
+ }
+
+ public void configureSnakes(boolean showTelemetry, boolean isBeingInserted){
+ snakes.setColors(hsbViaJoystick(snakes.getColors()[0]));
+
+ snakes.setStartIndex(startPoint);
+ snakes.setStopIndex(endPoint);
+ snakes.setBrightness(brightness);
+ snakes.setSpeed(speed);
+
+ if(isBeingInserted){
+ prism.insertAndUpdateAnimation(selectedLayer.layerHeight, snakes);
+ } else {
+ prism.updateAnimationFromIndex(selectedLayer.layerHeight);
+ }
+
+ if(showTelemetry) {
+ telemetry.addLine("Selected Animation: Snakes");
+ showHsbTelemetry();
+ telemetry.addLine(hsbTelemetry);
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ }
+ }
+
+ public void configureRandom(boolean showTelemetry, boolean isBeingInserted){
+ float[] hues = hueViaJoystick(random.getStartHue(), random.getStopHue());
+ random.setStartHue(hues[0]);
+ random.setStopHue(hues[1]);
+
+ random.setStartIndex(startPoint);
+ random.setStopIndex(endPoint);
+ random.setBrightness(brightness);
+ random.setSpeed(speed);
+
+ if(isBeingInserted){
+ prism.insertAndUpdateAnimation(selectedLayer.layerHeight, random);
+ } else {
+ prism.updateAnimationFromIndex(selectedLayer.layerHeight);
+ }
+
+ if(showTelemetry){
+ telemetry.addLine("Selected Animation: Random");
+ telemetry.addLine("Move the joysticks to configure the range of the random colors");
+ telemetry.addLine("Left Joystick X (side-to-side) changes the start hue");
+ telemetry.addLine("Right Joystick x (side-to-side) changes the end hue");
+ telemetry.addLine("");
+ telemetry.addLine(hueTelemetry);
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ }
+ }
+
+ public void configureSparkle(boolean showTelemetry, boolean isBeingInserted){
+ switch (animationColor){
+ case PRIMARY_COLOR:
+ sparkle.setPrimaryColor(hsbViaJoystick(sparkle.getPrimaryColor()));
+ break;
+ case SECONDARY_COLOR:
+ sparkle.setSecondaryColor(hsbViaJoystick(sparkle.getSecondaryColor()));
+ break;
+ }
+
+ toggleThroughColors(gamepad1.yWasPressed(),false);
+
+ sparkle.setStartIndex(startPoint);
+ sparkle.setStopIndex(endPoint);
+ sparkle.setBrightness(brightness);
+ sparkle.setPeriod(period);
+
+ if(isBeingInserted){
+ prism.insertAndUpdateAnimation(selectedLayer.layerHeight, sparkle);
+ } else {
+ prism.updateAnimationFromIndex(selectedLayer.layerHeight);
+ }
+
+ if(showTelemetry){
+ telemetry.addLine("Selected Animation: Sparkle");
+ showHsbTelemetry();
+ telemetry.addLine(hsbTelemetry);
+ telemetry.addLine("");
+ telemetry.addLine("Click the Y button to switch between setting the primary and secondary color");
+ telemetry.addLine(animationColor.toString());
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ }
+ }
+
+ public void configureSingleFill(boolean showTelemetry, boolean isBeingInserted){
+ switch (animationColor){
+ case PRIMARY_COLOR:
+ singleFillColors[0] = hsbViaJoystick(singleFill.getColors()[0]);
+ break;
+ case SECONDARY_COLOR:
+ singleFillColors[1] = hsbViaJoystick(singleFill.getColors()[1]);
+ break;
+ case TERTIARY_COLOR:
+ singleFillColors[2] = hsbViaJoystick(singleFill.getColors()[2]);
+ break;
+ }
+
+ toggleThroughColors(gamepad1.yWasPressed(),true);
+
+ singleFill.setColors(singleFillColors);
+
+ singleFill.setStartIndex(startPoint);
+ singleFill.setStopIndex(endPoint);
+ singleFill.setBrightness(brightness);
+ singleFill.setSpeed(speed);
+ singleFill.setPeriod(period);
+
+ if(isBeingInserted){
+ prism.insertAndUpdateAnimation(selectedLayer.layerHeight, singleFill);
+ } else {
+ prism.updateAnimationFromIndex(selectedLayer.layerHeight);
+ }
+
+ if(showTelemetry){
+ telemetry.addLine("Selected Animation: Single Fill");
+ telemetry.addLine("");
+ showHsbTelemetry();
+ telemetry.addLine(hsbTelemetry);
+ telemetry.addLine("");
+ telemetry.addLine("Click the Y button to switch between setting the primary, secondary, and tertiary color");
+ telemetry.addLine(animationColor.toString());
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ }
+ }
+
+ public void configureRainbowSnakes(boolean showTelemetry, boolean isBeingInserted){
+ float[] hues = hueViaJoystick(rainbowSnakes.getStartHue(),rainbowSnakes.getStopHue());
+ rainbowSnakes.setStartHue(hues[0]);
+ rainbowSnakes.setStopHue(hues[1]);
+
+ rainbowSnakes.setStartIndex(startPoint);
+ rainbowSnakes.setStopIndex(endPoint);
+ rainbowSnakes.setBrightness(brightness);
+ rainbowSnakes.setSpeed(speed);
+
+ if(isBeingInserted){
+ prism.insertAndUpdateAnimation(selectedLayer.layerHeight, rainbowSnakes);
+ } else {
+ prism.updateAnimationFromIndex(selectedLayer.layerHeight);
+ }
+
+ if(showTelemetry){
+ telemetry.addLine("Selected Animation: Rainbow Snakes");
+ telemetry.addLine("Move the joysticks to configure the range of the rainbow");
+ telemetry.addLine("Left Joystick X (side-to-side) changes the start hue");
+ telemetry.addLine("Right Joystick x (side-to-side) changes the end hue");
+ telemetry.addLine("");
+ telemetry.addLine(hueTelemetry);
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ }
+ }
+
+ public void configurePoliceLights(boolean showTelemetry, boolean isBeingInserted){
+ switch (animationColor){
+ case PRIMARY_COLOR:
+ policeLights.setPrimaryColor(hsbViaJoystick(policeLights.getPrimaryColor()));
+ break;
+ case SECONDARY_COLOR:
+ policeLights.setSecondaryColor(hsbViaJoystick(policeLights.getSecondaryColor()));
+ break;
+ case TERTIARY_COLOR:
+ policeLights.setTertiaryColor(hsbViaJoystick(policeLights.getTertiaryColor()));
+ break;
+ }
+
+ toggleThroughColors(gamepad1.yWasPressed(),true);
+
+ switch (policeLightsStyle){
+ case Style1:
+ if(gamepad1.rightStickButtonWasPressed()){
+ policeLightsStyle = PoliceLights.PoliceLightsStyle.Style2;
+ }
+ break;
+ case Style2:
+ if(gamepad1.rightStickButtonWasPressed()){
+ policeLightsStyle = PoliceLights.PoliceLightsStyle.Style3;
+ }
+ break;
+ case Style3:
+ if(gamepad1.rightStickButtonWasPressed()){
+ policeLightsStyle = PoliceLights.PoliceLightsStyle.Style1;
+ }
+ break;
+ }
+
+ policeLights.setStartIndex(startPoint);
+ policeLights.setStopIndex(endPoint);
+ policeLights.setBrightness(brightness);
+ policeLights.setPoliceLightsStyle(policeLightsStyle);
+ policeLights.setPeriod(period);
+
+ if(isBeingInserted){
+ prism.insertAndUpdateAnimation(selectedLayer.layerHeight, policeLights);
+ } else {
+ prism.updateAnimationFromIndex(selectedLayer.layerHeight);
+ }
+
+ if(showTelemetry){
+ telemetry.addLine("Selected Animation: Police Lights");
+ telemetry.addLine("");
+ telemetry.addLine("Toggle between styles by clicking the right joystick");
+ telemetry.addData("Police Lights Style: ",policeLightsStyle);
+ telemetry.addLine("");
+ showHsbTelemetry();
+ telemetry.addLine(hsbTelemetry);
+ telemetry.addLine("");
+ telemetry.addLine("Click the Y button to switch between setting the primary, secondary, and tertiary color");
+ telemetry.addLine(animationColor.toString());
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ }
+ }
+
+ public void resetStoredAnimations(){
+ Layers.LAYER_0.animationType = AnimationType.NONE;
+ Layers.LAYER_1.animationType = AnimationType.NONE;
+ Layers.LAYER_2.animationType = AnimationType.NONE;
+ Layers.LAYER_3.animationType = AnimationType.NONE;
+ Layers.LAYER_4.animationType = AnimationType.NONE;
+ Layers.LAYER_5.animationType = AnimationType.NONE;
+ Layers.LAYER_6.animationType = AnimationType.NONE;
+ Layers.LAYER_7.animationType = AnimationType.NONE;
+ Layers.LAYER_8.animationType = AnimationType.NONE;
+ Layers.LAYER_9.animationType = AnimationType.NONE;
+ }
+
+ /*
+ * Reasonably, you might be wondering why I'm configuring the colors in HSB/HSL instead
+ * of RGB - The sane choice. And it's all to create a more intuitive user experience.
+ * Having the user create a color by combining sliders of red, green, and blue is very true
+ * to the colorspace we are actually working in, but folks I tried this on found it very
+ * difficult. Hue/Saturation/Brightness allows us to create one slider for each three intuitive
+ * variables. Hue changes the color, saturation changes the intensity of the color, and brightness
+ * changes, well the brightness.
+ * Actually implementing this isn't very clean, but the result for the user is a better experience.
+ */
+ public Color hsbViaJoystick(Color previousColor){
+ final float HUE_JOYSTICK_SCALAR = 5;
+ final float SATURATION_JOYSTICK_SCALAR = 0.05F;
+ final float BRIGHTNESS_JOYSTICK_SCALAR = 0.05f;
+
+ float[] hsb = new float[3]; // Android graphics library wants an array containing RGB values.
+ android.graphics.Color.RGBToHSV(previousColor.red,previousColor.green,previousColor.blue,hsb);
+
+ /*
+ * Here we let the user increase or decrease H, S, or B with the joystick.
+ * Pushing the stick more moves the value more.
+ */
+ hsb[0] = Math.max(Math.min(hsb[0] +(gamepad1.left_stick_x*HUE_JOYSTICK_SCALAR), 360),0);
+ hsb[1] = Math.max(Math.min(hsb[1] +(gamepad1.right_stick_x*SATURATION_JOYSTICK_SCALAR), 1),0);
+ hsb[2] = Math.max(Math.min(hsb[2] +(-gamepad1.right_stick_y*BRIGHTNESS_JOYSTICK_SCALAR), 0.98f),0);
+
+ /*
+ * Here we create an integer where the Android graphics library will store each component of
+ * our RGB color one-after-another. I hope we can agree that this is cursed.
+ */
+ int colorInt = android.graphics.Color.HSVToColor(hsb);
+ Color color = new Color(0,0,0); // Create a new color to return.
+
+ /*
+ * One of the big downsides in this multi-color model is that some behavior isn't very
+ * intuitive. In this case as you approach a saturation of 1, the number of hues you can
+ * display are limited to very pure versions of red, green, and blue. To avoid this we
+ * limit the maximum saturation to 0.98 (this limit happens above) and to reduce confusion,
+ * we display this to the user over telemetry as 1.0.
+ */
+ if(hsb[2] > 0.97){
+ hsb[2] = 1.0f;
+ }
+ hsbTelemetry = String.format("Hue/Saturation/Brightness: %4.2f %4.2f %4.2f", hsb[0], hsb[1], hsb[2]);
+
+ color.red = android.graphics.Color.red(colorInt);
+ color.green = android.graphics.Color.green(colorInt);
+ color.blue = android.graphics.Color.blue(colorInt);
+ return color;
+ }
+
+ public float[] hueViaJoystick(float previousStartHue, float previousEndHue){
+ final float HUE_JOYSTICK_SCALAR = 5;
+ float[] hues = new float[2];
+
+ hues[0] = Math.max(Math.min(previousStartHue +(gamepad1.left_stick_x*HUE_JOYSTICK_SCALAR), 360),0);
+ hues[1] = Math.max(Math.min(previousEndHue +(gamepad1.right_stick_x*HUE_JOYSTICK_SCALAR),360),0);
+
+ hueTelemetry = String.format("Start hue/end hue: %4.2f %4.2f", hues[0], hues[1]);
+
+ return hues;
+ }
+
+ public void showHsbTelemetry(){
+ telemetry.addLine("Move the joysticks to configure the color of the LEDs");
+ telemetry.addLine("Left Joystick X (side-to-side) Changes the Hue of a color");
+ telemetry.addLine("Right Joystick X (side-to-side) changes the Saturation");
+ telemetry.addLine("Right Joystick Y (up-and-down) changes the Brightness");
+ telemetry.addLine("");
+ }
+
+ public void toggleThroughColors(boolean button, boolean thirdColor){
+ if(button){
+ switch (animationColor){
+ case PRIMARY_COLOR:
+ animationColor = AnimationColor.SECONDARY_COLOR;
+ break;
+ case SECONDARY_COLOR:
+ if(thirdColor){
+ animationColor = AnimationColor.TERTIARY_COLOR;
+ } else{
+ animationColor = AnimationColor.PRIMARY_COLOR;
+ }
+ break;
+ case TERTIARY_COLOR:
+ animationColor = AnimationColor.PRIMARY_COLOR;
+ break;
+ }
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/GoBildaPrismDriver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/GoBildaPrismDriver.java
new file mode 100644
index 000000000000..974a208502fa
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/GoBildaPrismDriver.java
@@ -0,0 +1,440 @@
+/* MIT License
+ * Copyright (c) [2025] [Base 10 Assets, LLC]
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.firstinspires.ftc.teamcode.Prism;
+
+import static com.qualcomm.robotcore.util.TypeConversion.byteArrayToInt;
+import static com.qualcomm.robotcore.util.TypeConversion.unsignedByteToInt;
+
+import com.qualcomm.hardware.lynx.LynxI2cDeviceSynch;
+import com.qualcomm.robotcore.hardware.I2cAddr;
+import com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice;
+import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple;
+import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties;
+import com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType;
+import com.qualcomm.robotcore.util.TypeConversion;
+
+import java.nio.ByteOrder;
+import java.util.Arrays;
+import java.util.concurrent.TimeUnit;
+
+@I2cDeviceType
+@DeviceProperties(
+ name = "goBILDA® Prism RGB LED Driver",
+ xmlTag = "goBILDAPrism",
+ description ="Prism RGB LED Driver (6-30V Input, I²C / PWM Control)"
+)
+public class GoBildaPrismDriver extends I2cDeviceSynchDevice {
+ private static final byte DEFAULT_ADDRESS = 0x38;
+ private final int MAXIMUM_NUMBER_OF_ANIMATIONS = 10;
+ private final int MAXIMUM_NUMBER_OF_ANIMATION_GROUPS = 8;
+ private PrismAnimations.AnimationBase[] animations = new PrismAnimations.AnimationBase[MAXIMUM_NUMBER_OF_ANIMATIONS];
+
+ //#region Public Types
+ public enum LayerHeight
+ {
+ LAYER_0 (Register.ANIMATION_SLOT_0),
+ LAYER_1 (Register.ANIMATION_SLOT_1),
+ LAYER_2 (Register.ANIMATION_SLOT_2),
+ LAYER_3 (Register.ANIMATION_SLOT_3),
+ LAYER_4 (Register.ANIMATION_SLOT_4),
+ LAYER_5 (Register.ANIMATION_SLOT_5),
+ LAYER_6 (Register.ANIMATION_SLOT_6),
+ LAYER_7 (Register.ANIMATION_SLOT_7),
+ LAYER_8 (Register.ANIMATION_SLOT_8),
+ LAYER_9 (Register.ANIMATION_SLOT_9),
+ DISABLED(Register.NULL);
+
+ /* Package Private */ final Register register;
+ /* Package Private */ final int index;
+
+ LayerHeight(Register register){
+ this.register = register;
+ if(register == Register.NULL)
+ this.index = -1;
+ else
+ this.index = register.address - Register.ANIMATION_SLOT_0.address;
+ }
+ }
+
+ public enum Artboard
+ {
+ ARTBOARD_0 (0,0),
+ ARTBOARD_1 (1,1),
+ ARTBOARD_2 (2,2),
+ ARTBOARD_3 (3,3),
+ ARTBOARD_4 (4,4),
+ ARTBOARD_5 (5,5),
+ ARTBOARD_6 (6,6),
+ ARTBOARD_7 (7,7);
+
+ /* Package Private */ final byte bitmask;
+ public final int index;
+
+ Artboard(int val,int index){
+ this.bitmask = (byte)(1 << val);
+ this.index = index;
+ }
+ }
+ //#endregion
+
+ //#region Package-Private types
+ /**
+ * Captures the length of each type of register used on the device.
+ */
+ /* Package Private */ enum RegisterType
+ {
+ INT8(1,255),
+ INT16(2, 65535),
+ INT24(3, 16777215),
+ INT32(4, 2147483647);
+
+ /* Package Private */ final int lengthBytes;
+ /* Package Private */ final int maxValue;
+
+ RegisterType(int lengthBytes, int maxValue){
+ this.lengthBytes = lengthBytes;
+ this.maxValue = maxValue;
+ }
+ }
+
+ /* Package Private */ enum RegisterAccess
+ {
+ READ_ONLY,
+ WRITE_ONLY,
+ READ_AND_WRITE;
+ }
+
+ /**
+ * Register map, including register address and register type
+ */
+ /* Package Private */ enum Register
+ {
+ DEVICE_ID (0 , RegisterType.INT8 , RegisterAccess.READ_ONLY),
+ FIRMWARE_VERSION (1 , RegisterType.INT24, RegisterAccess.READ_ONLY),
+ HARDWARE_VERSION (2 , RegisterType.INT16, RegisterAccess.READ_ONLY),
+ POWER_CYCLE_COUNT (3 , RegisterType.INT32, RegisterAccess.READ_ONLY),
+ RUNTIME_IN_MINUTES(4 , RegisterType.INT32, RegisterAccess.READ_ONLY),
+ STATUS (5 , RegisterType.INT32, RegisterAccess.READ_ONLY),
+ CONTROL (6 , RegisterType.INT32, RegisterAccess.WRITE_ONLY),
+ ARTBOARD_CONTROL (7 , RegisterType.INT32, RegisterAccess.WRITE_ONLY),
+ ANIMATION_SLOT_0 (8 , RegisterType.INT32, RegisterAccess.READ_AND_WRITE),
+ ANIMATION_SLOT_1 (9 , RegisterType.INT32, RegisterAccess.READ_AND_WRITE),
+ ANIMATION_SLOT_2 (10, RegisterType.INT32, RegisterAccess.READ_AND_WRITE),
+ ANIMATION_SLOT_3 (11, RegisterType.INT32, RegisterAccess.READ_AND_WRITE),
+ ANIMATION_SLOT_4 (12, RegisterType.INT32, RegisterAccess.READ_AND_WRITE),
+ ANIMATION_SLOT_5 (13, RegisterType.INT32, RegisterAccess.READ_AND_WRITE),
+ ANIMATION_SLOT_6 (14, RegisterType.INT32, RegisterAccess.READ_AND_WRITE),
+ ANIMATION_SLOT_7 (15, RegisterType.INT32, RegisterAccess.READ_AND_WRITE),
+ ANIMATION_SLOT_8 (16, RegisterType.INT32, RegisterAccess.READ_AND_WRITE),
+ ANIMATION_SLOT_9 (17, RegisterType.INT32, RegisterAccess.READ_AND_WRITE),
+ NULL (18, RegisterType.INT8 , RegisterAccess.READ_ONLY);
+
+ /* Package Private */ final int address;
+ /* Package Private */ final RegisterType registerType;
+ /* Package Private */ final RegisterAccess registerAccess;
+
+ Register(int address, RegisterType registerType, RegisterAccess registerAccess){
+ this.address = address;
+ this.registerType = registerType;
+ this.registerAccess = registerAccess;
+ }
+ }
+
+ private int readInt(Register register){
+ return byteArrayToInt(deviceClient.read(register.address,register.registerType.lengthBytes),ByteOrder.LITTLE_ENDIAN);
+ }
+
+ //#endregion
+
+ public GoBildaPrismDriver(I2cDeviceSynchSimple deviceClient, boolean deviceClientIsOwned)
+ {
+ super(deviceClient, deviceClientIsOwned);
+
+ this.deviceClient.setI2cAddress(I2cAddr.create7bit(DEFAULT_ADDRESS));
+ super.registerArmingStateCallback(false);
+ }
+
+ @Override
+ public Manufacturer getManufacturer()
+ {
+ return Manufacturer.GoBilda;
+ }
+
+ @Override
+ protected synchronized boolean doInitialize()
+ {
+ ((LynxI2cDeviceSynch)(deviceClient)).setBusSpeed(LynxI2cDeviceSynch.BusSpeed.FAST_400K);
+ return true;
+ }
+
+ @Override
+ public String getDeviceName()
+ {
+ return "goBILDA® Prism RGB LED Driver";
+ }
+
+ /**
+ * @return 3 if device is functional.
+ */
+ public int getDeviceID(){
+ //return readInt(Register.DEVICE_ID);
+ byte[] packet = deviceClient.read(Register.DEVICE_ID.address, Register.DEVICE_ID.registerType.lengthBytes);
+ return packet[0];
+ }
+
+ /**
+ * @return Firmware Version of device. Array[0] is the major version, Array[1] is the minor version
+ * Array[2] is the patch version.
+ */
+ public int[] getFirmwareVersion(){
+ byte[] packet = deviceClient.read(Register.FIRMWARE_VERSION.address,Register.FIRMWARE_VERSION.registerType.lengthBytes);
+ int[] output = new int[3];
+ output[0] = packet[2];
+ output[1] = packet[1];
+ output[2] = packet[0];
+ return output;
+ }
+
+ /**
+ * @return Hardware version of the device as a string.
+ */
+ public String getFirmwareVersionString(){
+ byte[] packet = deviceClient.read(Register.FIRMWARE_VERSION.address,Register.FIRMWARE_VERSION.registerType.lengthBytes);
+ int[] output = new int[3];
+ output[0] = packet[2];
+ output[1] = packet[1];
+ output[2] = packet[0];
+ return String.format("%d.%d.%d", output[0], output[1], output[2]);
+ }
+
+ /**
+ * @return Hardware version of device. Array[0] is the major version, Array[1] is the minor version.
+ */
+ public int[] getHardwareVersion(){
+ byte[] packet = deviceClient.read(Register.HARDWARE_VERSION.address,Register.HARDWARE_VERSION.registerType.lengthBytes);
+ int[] output = new int[2];
+ output[0] = packet[1];
+ output[1] = packet[0];
+ return output;
+ }
+ public String getHardwareVersionString(){
+ byte[] packet = deviceClient.read(Register.HARDWARE_VERSION.address,Register.HARDWARE_VERSION.registerType.lengthBytes);
+ int[] output = new int[2];
+ output[0] = packet[1];
+ output[1] = packet[0];
+ return String.format("%d.%d", output[0], output[1]);
+ }
+
+ /**
+ * @return The number of times the device has power cycled in its lifetime.
+ */
+ public int getPowerCycleCount(){
+ return readInt(Register.POWER_CYCLE_COUNT);
+ }
+
+ /**
+ * @return Total device runtime in minutes
+ */
+ public long getRunTime(TimeUnit timeUnit){
+ return timeUnit.convert(readInt(Register.RUNTIME_IN_MINUTES),TimeUnit.MINUTES);
+ }
+
+ /**
+ * @return total LEDs in strip
+ */
+ public int getNumberOfLEDs(){
+ byte[] packet = deviceClient.read(Register.STATUS.address,Register.STATUS.registerType.lengthBytes);
+ return unsignedByteToInt(packet[0]);
+ }
+
+ /**
+ * @return Current Animation frames per second
+ */
+ public int getCurrentFPS(){
+ byte[] inputPacket = deviceClient.read(Register.STATUS.address,Register.STATUS.registerType.lengthBytes);
+ byte[] outputPacket = new byte[4];
+ outputPacket[0] = inputPacket[1];
+ outputPacket[1] = inputPacket[2];
+
+ return byteArrayToInt(outputPacket,ByteOrder.LITTLE_ENDIAN);
+ }
+
+ /**
+ * @return
+ */
+ public boolean fpsLimited(){
+ byte[] packet = deviceClient.read(Register.STATUS.address, Register.STATUS.registerType.lengthBytes);
+ //return packet[3] >> 7;
+ return false;
+ }
+
+ /**
+ * return What artboard is set as default boot animation?
+ */
+ public int getBootAnimationArtboard(){
+ byte[] packet = deviceClient.read(Register.STATUS.address, Register.STATUS.registerType.lengthBytes);
+ return 0;
+ }
+
+
+ /**
+ * Inserts an animation into the specified slot in the animations array.
+ *
+ * @param height the height where the animation should be inserted (0-9)
+ * @param animation the AnimationBase object to insert into the array
+ * @return true if the animation was successfully inserted, false if the index
+ * is out of bounds or the animation is null
+ */
+ public boolean insertAnimation(LayerHeight height, PrismAnimations.AnimationBase animation)
+ {
+ if(height == LayerHeight.DISABLED || animation == null)
+ return false;
+
+ animations[height.index] = animation;
+ animations[height.index].layerHeight = height;
+ return true;
+ }
+
+ /**
+ * Inserts an animation at the specified index and immediately updates it over I2C.
+ *
+ * @param height the height where the animation should be inserted and updated (0-9)
+ * @param animation the AnimationBase object to insert and update
+ * @return true if both insertion and update operations were successful,
+ * false if either operation failed
+ */
+ public boolean insertAndUpdateAnimation(LayerHeight height, PrismAnimations.AnimationBase animation)
+ {
+ if(insertAnimation(height, animation))
+ return updateAnimationFromIndex(height, true);
+ return false;
+ }
+
+ /**
+ * Updates all animations in the manager by sending their data over I2C.
+ * Iterates through all animation slots and updates any non-null animations.
+ *
+ * @return true if the update process completed (currently always returns true)
+ */
+ public boolean updateAllAnimations()
+ {
+ for(int i = 0; i < MAXIMUM_NUMBER_OF_ANIMATIONS; i++){
+ if(animations[i] != null && animations[i].layerHeight != LayerHeight.DISABLED)
+ animations[i].updateAnimationOverI2C(this.deviceClient, false);
+ }
+ return true;
+ }
+
+ /**
+ * Updates a specific animation at the given Layer Height by sending its data over I2C.
+ *
+ * @param height the height of the animation to update (0-9)
+ * @return true if the animation was successfully updated, false if the index
+ * is out of bounds or if no animation exists at the specified index
+ */
+ public boolean updateAnimationFromIndex(LayerHeight height)
+ {
+ return updateAnimationFromIndex(height, false);
+ }
+
+ public void clearAllAnimations()
+ {
+ byte[] packet = TypeConversion.intToByteArray(1 << 25, ByteOrder.LITTLE_ENDIAN);
+
+ deviceClient.write(Register.CONTROL.address, packet);
+ Arrays.fill(animations, null);
+ }
+
+ public void setTargetFPS(int targetFPS)
+ {
+ final int boundedTargetFps = Math.max(0, Math.min(targetFPS, 0x7FFF));
+ final int ChangeTargetFpsBit = 1 << 15;
+ final int ChangeTargetFpsCommand = ChangeTargetFpsBit | boundedTargetFps;
+ byte[] packet = TypeConversion.intToByteArray(ChangeTargetFpsCommand, ByteOrder.LITTLE_ENDIAN);
+ deviceClient.write(Register.CONTROL.address, packet);
+ }
+
+ public void setStripLength(int stripLength)
+ {
+ final int boundedStripLength = Math.max(0, Math.min(stripLength, 0xFF));
+ final int ChangeStripLengthBit = 1 << 24;
+ final int ChangeStripLengthCommand = ChangeStripLengthBit | (boundedStripLength << 16);
+ byte[] packet = TypeConversion.intToByteArray(ChangeStripLengthCommand, ByteOrder.LITTLE_ENDIAN);
+ deviceClient.write(Register.CONTROL.address, packet);
+ }
+
+ public void saveCurrentAnimationsToArtboard(Artboard artboard)
+ {
+ byte[] data = {
+ artboard.bitmask,
+ 0,
+ 0,
+ 0
+ };
+ deviceClient.write(Register.ARTBOARD_CONTROL.address, data);
+ }
+
+ public void loadAnimationsFromArtboard(Artboard artboard)
+ {
+ byte[] data = {
+ 0,
+ artboard.bitmask,
+ 0,
+ 0
+ };
+ deviceClient.write(Register.ARTBOARD_CONTROL.address, data);
+ }
+
+ public void setDefaultBootArtboard(Artboard artboard)
+ {
+ byte[] data = {
+ 0,
+ 0,
+ artboard.bitmask,
+ 0
+ };
+ deviceClient.write(Register.ARTBOARD_CONTROL.address, data);
+ }
+
+ public void enableDefaultBootArtboard(boolean enable)
+ {
+ byte[] data = {
+ 0,
+ 0,
+ 0,
+ enable ? (byte)0b00000001 : (byte)0b00000010
+ };
+ deviceClient.write(Register.ARTBOARD_CONTROL.address, data);
+ }
+
+ private boolean updateAnimationFromIndex(LayerHeight height, boolean isBeingInserted)
+ {
+ if(height == LayerHeight.DISABLED || animations[height.index] == null)
+ return false;
+
+ boolean animationEnabled = animations[height.index].layerHeight != LayerHeight.DISABLED;
+ if(animationEnabled)
+ animations[height.index].updateAnimationOverI2C(this.deviceClient, isBeingInserted);
+ return animationEnabled;
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/GoBildaPrismExample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/GoBildaPrismExample.java
new file mode 100644
index 000000000000..c844fe416215
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/GoBildaPrismExample.java
@@ -0,0 +1,154 @@
+/* MIT License
+ * Copyright (c) [2025] [Base 10 Assets, LLC]
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.firstinspires.ftc.teamcode;
+
+
+import static org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.LayerHeight;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.Prism.Color;
+import org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver;
+import org.firstinspires.ftc.teamcode.Prism.PrismAnimations;
+
+import java.util.concurrent.TimeUnit;
+
+/*
+ * This example file shows how to create a couple of different Animations on the Prism, and save
+ * them to an Artboard on the device.
+ * The example file called "GoBildaPrismConfigurator" is designed to create a user-interface
+ * through your driver's station. This is a great way to create Artboards from Animations, but
+ * doesn't let you control quite everything about the available Animations. If you'd like to
+ * do something more complex, or would just prefer to create your Artboard in Java, this
+ * program shows you how.
+ * The example file called "GoBildaPrismArtboardExample" shows how to recall different Artboards
+ * that you have already saved to the device. That file shows the code you should consider adding
+ * to your OpMode if you would like to dynamically change the Artboard shown by your LEDs during
+ * the match.
+ *
+ * Core to understanding how to use this product is knowing these three terms:
+ * Animations: (Like RAINBOW or BLINK) - These have properties you can configure, like their color.
+ * They can have unique start and end points!
+ * Layers: There are 10 layers, each of which can store an animation. These are hierarchical.
+ * So an Animation on layer 5 will cover an animation on layer 2 if they overlap.
+ * You can use start and end points to have layers overlap to create new patterns! Or show multiple
+ * animations at once on different LEDs.
+ * Artboards: An Artboard is a set of 10 layers which is stored on the Prism.
+ * you can have up to 8 unique Artboards. Artboards are easy and computationally fast to switch between.
+ */
+
+@TeleOp(name="Prism Animations Example", group="Linear OpMode")
+//@Disabled
+
+public class GoBildaPrismExample extends LinearOpMode {
+
+ GoBildaPrismDriver prism;
+
+ PrismAnimations.Solid solid = new PrismAnimations.Solid(Color.BLUE);
+ PrismAnimations.RainbowSnakes rainbowSnakes = new PrismAnimations.RainbowSnakes();
+
+ @Override
+ public void runOpMode() {
+ /*
+ * Initialize the hardware variables. Note that the strings used here must correspond
+ * to the names assigned during the robot configuration step on the driver's station.
+ */
+ prism = hardwareMap.get(GoBildaPrismDriver.class,"prism");
+
+ /*
+ * Set the number of LEDs (starting at 0) that are in your strip. This can be longer
+ * than the actual length of the strip, but some animations won't look quite right.
+ */
+ prism.setStripLength(32);
+
+ /*
+ * Here you can customize the specifics of different animations. Each animation has it's
+ * own set of parameters that you can customize to create something unique! Each animation
+ * has carefully selected default parameters. So you do not need to set each parameter
+ * for every animation!
+ */
+ solid.setBrightness(50);
+ solid.setStartIndex(0);
+ solid.setStopIndex(12);
+
+ rainbowSnakes.setNumberOfSnakes(2);
+ rainbowSnakes.setSnakeLength(3);
+ rainbowSnakes.setSpacingBetween(6);
+ rainbowSnakes.setSpeed(0.5f);
+
+ telemetry.addData("Device ID: ", prism.getDeviceID());
+ telemetry.addData("Firmware Version: ", prism.getFirmwareVersionString());
+ telemetry.addData("Hardware Version: ", prism.getHardwareVersionString());
+ telemetry.addData("Power Cycle Count: ", prism.getPowerCycleCount());
+ telemetry.update();
+
+ // Wait for the game to start (driver presses START)
+ waitForStart();
+ resetRuntime();
+
+ // run until the end of the match (driver presses STOP)
+ while (opModeIsActive()) {
+
+
+ if(gamepad1.aWasPressed()){
+ /*
+ * Here we insert and update the animation to the Prism, this by default does not
+ * save it to an Artboard, it just starts the Animation playing. If you have
+ * already inserted an animation at a layer height, you can instead call
+ * .updateAnimationFromIndex(LayerHeight.LAYER_0) to update an animation at a
+ * specific layer height without overwriting it completely.
+ */
+ prism.insertAndUpdateAnimation(LayerHeight.LAYER_0, solid);
+ prism.insertAndUpdateAnimation(LayerHeight.LAYER_1,rainbowSnakes);
+ }
+
+ if(gamepad1.xWasPressed()){
+ /*
+ * Clearing the animation doesn't erase any saved Artboards, but it removes all the
+ * currently displayed animations.
+ */
+ prism.clearAllAnimations();
+ }
+
+ if(gamepad1.dpadDownWasPressed()){
+ /*
+ * Here we save the animation we are currently displaying to Artboard 0.
+ */
+ prism.saveCurrentAnimationsToArtboard(GoBildaPrismDriver.Artboard.ARTBOARD_0);
+ }
+
+ telemetry.addLine("Press A to insert and update the created animations.");
+ telemetry.addLine("Press X to clear current animations.");
+ telemetry.addLine("Press D-Pad Down to save current animations to Artboard #0");
+ telemetry.addLine();
+ telemetry.addData("Run Time (Hours): ",prism.getRunTime(TimeUnit.HOURS));
+ telemetry.addData("Run Time (Minutes): ",prism.getRunTime(TimeUnit.MINUTES));
+ telemetry.addData("Number of LEDS: ", prism.getNumberOfLEDs());
+ telemetry.addData("Current FPS: ", prism.getCurrentFPS());
+ telemetry.update();
+ sleep(50);
+ }
+ }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/PrismAnimations.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/PrismAnimations.java
new file mode 100644
index 000000000000..6474bc42f2fe
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/PrismAnimations.java
@@ -0,0 +1,1069 @@
+/* MIT License
+ * Copyright (c) [2025] [Base 10 Assets, LLC]
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.firstinspires.ftc.teamcode.Prism;
+
+import static org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.LayerHeight;
+
+import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple;
+import com.qualcomm.robotcore.util.TypeConversion;
+
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import java.util.concurrent.TimeUnit;
+
+public class PrismAnimations {
+ public enum AnimationType{
+ NONE(0),
+ SOLID(1),
+ BLINK(2),
+ PULSE(3),
+ SINE_WAVE(4),
+ DROID_SCAN(5),
+ RAINBOW(6),
+ SNAKES(7),
+ RANDOM(8),
+ SPARKLE(9),
+ SINGLE_FILL(10),
+ RAINBOW_SNAKES(11),
+ POLICE_LIGHTS(12);
+
+ public final int AnimationTypeIndex;
+ AnimationType(int animationType){
+ this.AnimationTypeIndex = animationType;
+ }
+ }
+
+ public static abstract class AnimationBase {
+ protected final AnimationType animationType;
+ protected int brightness = 100;
+ protected int startIndex = 0;
+ protected int stopIndex = 255;
+ protected LayerHeight layerHeight;
+
+ //region Constructors
+ protected AnimationBase(AnimationType type){
+ this.animationType = type;
+ }
+
+ /**
+ * @param brightness from 0 to 100.
+ */
+ protected AnimationBase(AnimationType type, int brightness) {
+ this(type);
+ this.brightness = Math.min(brightness, 100);
+ }
+
+ /**
+ * Set both the first, and last LED in a string that you'd like this animation to show on.
+ * @param startIndex from 0 to 255.
+ * @param stopIndex from 0 to 255.
+ */
+ protected AnimationBase(AnimationType type, int startIndex, int stopIndex) {
+ this(type);
+ this.startIndex = Math.min(startIndex, 255);
+ this.stopIndex = Math.min(stopIndex, 255);
+ }
+
+ /**
+ * @param brightness from 0 to 100.
+ * @param startIndex from 0 to 255.
+ * @param stopIndex from 0 to 255.
+ */
+ protected AnimationBase(AnimationType type, int brightness, int startIndex, int stopIndex){
+ this(type, brightness);
+ this.startIndex = Math.min(startIndex, 255);
+ this.stopIndex = Math.min(stopIndex, 255);
+ }
+
+ /**
+ * @param brightness from 0 to 100.
+ * @param startIndex from 0 to 255.
+ * @param stopIndex from 0 to 255.
+ * @param layerHeight the height that this animation should sit at from 0 to 9.
+ */
+ protected AnimationBase(AnimationType type, int brightness, int startIndex, int stopIndex, LayerHeight layerHeight){
+ this(type, brightness, startIndex, stopIndex);
+ this.layerHeight = layerHeight;
+ }
+ //endregion
+
+ //region Setters / Getters
+ public int getBrightness() { return brightness; }
+ public int getStartIndex() { return startIndex; }
+ public int getStopIndex() { return stopIndex; }
+
+ /**
+ * Brightness of selected animation.
+ * @param brightness From 0 to 100.
+ */
+ public void setBrightness(int brightness) { this.brightness = Math.min(brightness, 100); }
+
+ /**
+ * The first LED in the string that you'd like this animation to display on.
+ * @param startIndex from 0 to 255.
+ */
+ public void setStartIndex(int startIndex) { this.startIndex = Math.min(startIndex, 255); }
+
+ /**
+ * The last LED in the string that you'd like this animation to display on.
+ * @param stopIndex from 0 to 255.
+ */
+ public void setStopIndex(int stopIndex) { this.stopIndex = Math.min(stopIndex, 255); }
+
+ /**
+ * Set both the first, and last LED in a string that you'd like this animation to show on.
+ * @param startIndex from 0 to 255.
+ * @param stopIndex from 0 to 255.
+ */
+ public void setIndexes(int startIndex, int stopIndex){
+ this.startIndex = (byte)startIndex;
+ this.stopIndex = (byte)stopIndex;
+ }
+ //endregion
+
+ protected abstract void updateAnimationSpecificValuesOverI2C(I2cDeviceSynchSimple deviceClient);
+
+ protected void updateAnimationOverI2C(I2cDeviceSynchSimple deviceClient, boolean isInsertingAnimation)
+ {
+ if(isInsertingAnimation)
+ deviceClient.write(layerHeight.register.address, GetByteArray(0, animationType.AnimationTypeIndex));
+ deviceClient.write(layerHeight.register.address, GetByteArray(1, brightness));
+ deviceClient.write(layerHeight.register.address, GetByteArray(2, startIndex));
+ deviceClient.write(layerHeight.register.address, GetByteArray(3, stopIndex));
+ updateAnimationSpecificValuesOverI2C(deviceClient);
+ }
+
+ protected byte[] floatToByteArray (float value, ByteOrder byteOrder)
+ {
+ return ByteBuffer.allocate(4).order(byteOrder).putFloat(value).array();
+ }
+
+ private byte unsignedIntToByte(int data)
+ {
+ int boundedData = Math.min(Math.max(data, 0), 255);
+ ByteBuffer buffer = ByteBuffer.allocate(4).putInt(boundedData);
+
+ return buffer.array()[3];
+ }
+
+ protected byte[] GetByteArray(int subRegister, Color color)
+ {
+ return new byte[] {
+ (byte)Math.max(0, Math.min(subRegister, 12)),
+ (byte)color.red,
+ (byte)color.green,
+ (byte)color.blue
+ };
+ }
+
+ protected byte[] GetByteArray(int subRegister, int data)
+ {
+ byte[] packet = TypeConversion.intToByteArray(data, ByteOrder.LITTLE_ENDIAN);
+ return new byte[] {
+ (byte)Math.max(0, Math.min(subRegister, 12)),
+ packet[0],
+ packet[1],
+ packet[2],
+ packet[3]
+ };
+ }
+
+ protected byte[] GetByteArray(int subRegister, float data)
+ {
+ byte[] packet = floatToByteArray(data, ByteOrder.LITTLE_ENDIAN);
+ return new byte[] {
+ (byte)Math.max(0, Math.min(subRegister, 12)),
+ packet[0],
+ packet[1],
+ packet[2],
+ packet[3]
+ };
+ }
+
+ protected byte[] GetByteArray(int subRegister, byte data)
+ {
+ return new byte[]{
+ (byte)subRegister,
+ data
+ };
+ }
+
+ protected byte[] GetByteArray(int subRegister, Direction direction)
+ {
+ boolean bool = direction == Direction.Forward;
+ return new byte[]{
+ (byte)Math.max(0, Math.min(subRegister, 12)),
+ (byte) (bool ? 1 : 0)
+ };
+ }
+
+ protected byte[] GetByteArray(int subRegister, Color... colors)
+ {
+ byte[] array = new byte[1+(colors.length*3)];
+
+ array[0] = (byte)Math.max(0, Math.min(subRegister, 12));
+
+ for(int i = 0; i < colors.length; i++){
+ array[1+(i*3)] = unsignedIntToByte(colors[i].red);
+ array[1+(i*3+1)] = unsignedIntToByte(colors[i].green);
+ array[1+(i*3+2)] = unsignedIntToByte(colors[i].blue);
+ }
+
+ return array;
+ }
+ }
+
+ public static class Blink extends AnimationBase{
+ private Color primaryColor = Color.BLUE;
+ private Color secondaryColor = Color.RED;
+ private int period = 2000;
+ private int primaryColorPeriod = 1000;
+
+ public Blink(){ super(AnimationType.BLINK); }
+ public Blink(Color primaryColor) {
+ this();
+ this.primaryColor = primaryColor;
+ }
+ public Blink(Color primaryColor, Color secondaryColor) {
+ this(primaryColor);
+ this.secondaryColor = secondaryColor;
+ }
+ public Blink(Color primaryColor, Color secondaryColor, int period) {
+ this(primaryColor, secondaryColor);
+ this.period = period;
+ }
+ public Blink(Color primaryColor, Color secondaryColor, int period, int primaryColorPeriod) {
+ this(primaryColor, secondaryColor, period);
+ this.primaryColorPeriod = primaryColorPeriod;
+ }
+
+ /**
+ * set the length of one loop of an animation in milliseconds.
+ * @param period from 0 - 4,294,697,295. Larger is longer.
+ */
+ public void setPeriod(int period) { this.period = period; }
+
+ /**
+ * set the length of one loop of an animation in specified unit.
+ * @param duration duration of loop.
+ * @param timeUnit unit to use.
+ */
+ public void setPeriod(int duration, TimeUnit timeUnit){
+ this.period = Math.toIntExact(timeUnit.toMillis(duration));
+ }
+ public void setPrimaryColor(Color color) { primaryColor = color; }
+ public void setSecondaryColor(Color color) { secondaryColor = color; }
+ public void setPrimaryColor(int red, int green, int blue) { primaryColor = new Color(red, green, blue); }
+ public void setSecondaryColor(int red, int green, int blue) { secondaryColor = new Color(red, green, blue); }
+ /**
+ * set the duration of the primary color in a blink.
+ * @param primaryColorPeriod from 0 - 4,294,697,295. Larger is longer.
+ */
+ public void setPrimaryColorPeriod(int primaryColorPeriod) { this.primaryColorPeriod = primaryColorPeriod; }
+
+ /**
+ * set the duration of the primary color in a blink.
+ * @param duration duration of loop.
+ * @param timeUnit unit to use.
+ */
+ public void setPrimaryColorPeriod(int duration, TimeUnit timeUnit){
+ this.primaryColorPeriod = Math.toIntExact(timeUnit.toMillis(duration));
+ }
+
+ public int getPeriod() { return period; }
+ public Color getPrimaryColor() { return primaryColor; }
+ public Color getSecondaryColor() { return secondaryColor; }
+ public int getPrimaryColorPeriod() { return primaryColorPeriod; }
+
+ @Override
+ protected void updateAnimationSpecificValuesOverI2C(I2cDeviceSynchSimple deviceClient)
+ {
+ deviceClient.write(layerHeight.register.address, GetByteArray(4, primaryColor));
+ deviceClient.write(layerHeight.register.address, GetByteArray(5, secondaryColor));
+ deviceClient.write(layerHeight.register.address, GetByteArray(6, period));
+ deviceClient.write(layerHeight.register.address, GetByteArray(7, primaryColorPeriod));
+ }
+ }
+
+ public static class DroidScan extends AnimationBase{
+ private Color primaryColor = Color.RED;
+ private Color secondaryColor = Color.TRANSPARENT;
+ private DroidScanStyle style = DroidScanStyle.BOTH_TAIL;
+ private float speed = .4f;
+ private int eyeWidth = 3;
+ private int trailWidth = 3;
+
+ public enum DroidScanStyle{
+ NO_TAIL(0),
+ FRONT_TAIL(1),
+ BACK_TAIL(2),
+ BOTH_TAIL(3);
+
+ /* Package Private */ final byte styleValue;
+ DroidScanStyle(int value){
+ styleValue = (byte)value;
+ }
+ }
+
+ public DroidScan(){ super(AnimationType.DROID_SCAN); }
+ public DroidScan(Color primaryColor) {
+ this();
+ this.primaryColor = primaryColor;
+ }
+ public DroidScan(Color primaryColor, Color secondaryColor) {
+ this(primaryColor);
+ this.secondaryColor = secondaryColor;
+ }
+
+ public void setSecondaryColor(Color color) { this.secondaryColor = color; }
+ public void setPrimaryColor(Color color) { this.primaryColor = color; }
+
+ /**
+ * Sets the speed of the animation
+ * @param speed from 0 to 1.
+ */
+ public void setSpeed(float speed) { this.speed = speed; }
+
+ /**
+ * Sets the width of the "eye" in the animation in pixels
+ * @param eyeWidth from 0 to 255.
+ */
+ public void setEyeWidth(int eyeWidth) { this.eyeWidth = eyeWidth; }
+
+ /**
+ * Sets the width of the trail of the eye in the animation in pixels
+ * @param trailWidth from 0 to 255.
+ */
+ public void setTrailWidth(int trailWidth) { this.trailWidth = trailWidth; }
+ public void setDroidScanStyle(DroidScanStyle style) { this.style = style; }
+
+ public Color getSecondaryColor() { return secondaryColor; }
+ public Color getPrimaryColor() { return primaryColor; }
+ public float getSpeed() { return speed; }
+ public int getEyeWidth() { return eyeWidth; }
+ public int getTrailWidth() { return trailWidth; }
+ public DroidScanStyle getDroidScanStyle() { return style; }
+
+ @Override
+ protected void updateAnimationSpecificValuesOverI2C(I2cDeviceSynchSimple deviceClient)
+ {
+ deviceClient.write(layerHeight.register.address, GetByteArray(4 , primaryColor));
+ deviceClient.write(layerHeight.register.address, GetByteArray(5 , secondaryColor));
+ deviceClient.write(layerHeight.register.address, GetByteArray(6 , speed));
+ deviceClient.write(layerHeight.register.address, GetByteArray(7 , (byte)eyeWidth));
+ deviceClient.write(layerHeight.register.address, GetByteArray(8 , (byte)trailWidth));
+ deviceClient.write(layerHeight.register.address, GetByteArray(11, style.styleValue));
+ }
+ }
+
+ public static class PoliceLights extends AnimationBase {
+ private Color red = Color.RED;
+ private Color white = Color.WHITE;
+ private Color blue = Color.BLUE;
+ private int period = 1000;
+ private PoliceLightsStyle style = PoliceLightsStyle.Style1;
+
+ public enum PoliceLightsStyle{
+ Style1(0),
+ Style2(1),
+ Style3(2);
+
+ public byte styleValue;
+ PoliceLightsStyle(int value){
+ styleValue = (byte)value;
+ }
+ }
+
+ public PoliceLights(){ super(AnimationType.POLICE_LIGHTS); }
+
+ /**
+ * set the length of one loop of an animation in milliseconds.
+ * @param period from 0 - 4,294,697,295. Larger is longer.
+ */
+ public void setPeriod(int period) { this.period = period; }
+
+ /**
+ * set the length of one loop of an animation in specified unit.
+ * @param duration duration of loop.
+ * @param timeUnit unit to use.
+ */
+ public void setPeriod(int duration, TimeUnit timeUnit){
+ this.period = Math.toIntExact(timeUnit.toMillis(duration));
+ }
+ public void setPoliceLightsStyle(PoliceLightsStyle style){this.style = style;}
+ public void setPrimaryColor(Color color){this.red = color;}
+ public void setSecondaryColor(Color color){this.white = color;}
+ public void setTertiaryColor(Color color){this.blue = color;}
+
+ public PoliceLightsStyle getPoliceLightsStyle(){return style;}
+ public Color getPrimaryColor(){return this.red;}
+ public Color getSecondaryColor(){return this.white;}
+ public Color getTertiaryColor(){return this.blue;}
+
+ @Override
+ protected void updateAnimationSpecificValuesOverI2C(I2cDeviceSynchSimple deviceClient)
+ {
+ deviceClient.write(layerHeight.register.address, GetByteArray(6 , period));
+ deviceClient.write(layerHeight.register.address, GetByteArray(7 , red));
+ deviceClient.write(layerHeight.register.address, GetByteArray(8 , white));
+ deviceClient.write(layerHeight.register.address, GetByteArray(9 , blue));
+ deviceClient.write(layerHeight.register.address, GetByteArray(11, style.styleValue));
+ }
+ }
+
+ public static class Pulse extends AnimationBase {
+ private Color primaryColor = Color.GREEN;
+ private Color secondaryColor = Color.RED;
+ private int period = 1000;
+
+ public Pulse(){ super(AnimationType.PULSE); }
+ public Pulse(Color primaryColor) {
+ this();
+ this.primaryColor = primaryColor;
+ }
+ public Pulse(Color primaryColor, Color secondaryColor) {
+ this(primaryColor);
+ this.secondaryColor = secondaryColor;
+ }
+ public Pulse(Color primaryColor, Color secondaryColor, int period) {
+ this(primaryColor, secondaryColor);
+ this.period = period;
+ }
+
+ /**
+ * set the period of the pulse in milliseconds.
+ * @param period from 0 - 4,294,697,295. Larger is longer.
+ */
+ public void setPeriod(int period) { this.period = period; }
+
+ /**
+ * set the period of the pulse in specified unit.
+ * @param duration duration of loop.
+ * @param timeUnit unit to use.
+ */
+ public void setPeriod(int duration, TimeUnit timeUnit){
+ this.period = Math.toIntExact(timeUnit.toMillis(duration));
+ }
+ public void setPrimaryColor(Color color) { primaryColor = color; }
+ public void setSecondaryColor(Color color) { secondaryColor = color; }
+ public void setPrimaryColor(int red, int green, int blue) { primaryColor = new Color((byte)red, (byte)green, (byte)blue); }
+ public void setSecondaryColor(int red, int green, int blue) { secondaryColor = new Color((byte)red, (byte)green, (byte)blue); }
+
+ public int getPeriod() { return period; }
+ public Color getPrimaryColor() { return primaryColor; }
+ public Color getSecondaryColor() { return secondaryColor; }
+
+ @Override
+ protected void updateAnimationSpecificValuesOverI2C(I2cDeviceSynchSimple deviceClient)
+ {
+ deviceClient.write(layerHeight.register.address, GetByteArray(4, primaryColor));
+ deviceClient.write(layerHeight.register.address, GetByteArray(5, secondaryColor));
+ deviceClient.write(layerHeight.register.address, GetByteArray(6, period));
+ }
+ }
+
+ public static class Rainbow extends AnimationBase {
+ private float startHue = 0.0f;
+ private float stopHue = 360.0f;
+ private float speed = 0.50f; // 0.00 - 1.00
+ private int repeatAfter = 25;
+ private Direction direction = Direction.Forward;
+
+ public Rainbow(){ super(AnimationType.RAINBOW); }
+ public Rainbow(float startHue) {
+ this();
+ this.startHue = startHue;
+ }
+ public Rainbow(float startHue, float stopHue){
+ this(startHue);
+ this.stopHue = stopHue;
+ }
+ public Rainbow(float startHue, float stopHue, float speed){
+ this(startHue, stopHue);
+ this.speed = speed;
+ }
+ public Rainbow(float startHue, float stopHue, float speed, Direction direction){
+ this(startHue, stopHue, speed);
+ this.direction = direction;
+ }
+ public Rainbow(float startHue, float stopHue, float speed, Direction direction, int repeatAfter){
+ this(startHue, stopHue, speed, direction);
+ this.repeatAfter = repeatAfter;
+ }
+
+ //region Getters/Setters
+ public float getSpeed() { return speed;}
+ public float getStopHue() { return stopHue; }
+ public float getStartHue() { return startHue; }
+ public Direction getDirection() { return direction; }
+ public int getRepeatAfter() { return repeatAfter; }
+
+ /**
+ * Sets the speed of the animation
+ * @param speed from 0 to 1.
+ */
+ public void setSpeed(float speed) { this.speed = speed; }
+
+ /**
+ * Can be used to limit the colors shown in the rainbow. This is a hue so it reflects
+ * the position as an angle on a color wheel. Red is 0, Green is 120, Blue is 240, etc.
+ * @param startHue from 0 to 360.
+ */
+ public void setStartHue(float startHue){ this.startHue = startHue; }
+
+ /**
+ * Can be used to limit the colors shown in the rainbow. This is a hue so it reflects
+ * the position as an angle on a color wheel. Red is 0, Green is 120, Blue is 240, etc.
+ * @param stopHue from 0 to 360.
+ */
+ public void setStopHue(float stopHue) { this.stopHue = stopHue; }
+ public void setDirection(Direction direction) { this.direction = direction; }
+
+ /**
+ * The number of pixels before the rainbow repeats. Default is 20.
+ * @param repeatAfter From 0 to 255.
+ */
+ public void setRepeatAfter(int repeatAfter) { this.repeatAfter = repeatAfter; }
+ public void setHues(float startHue, float stopHue) {
+ this.startHue = startHue;
+ this.stopHue = stopHue;
+ }
+ //endregion
+ @Override
+ protected void updateAnimationSpecificValuesOverI2C(I2cDeviceSynchSimple deviceClient)
+ {
+ deviceClient.write(layerHeight.register.address, GetByteArray(4, startHue));
+ deviceClient.write(layerHeight.register.address, GetByteArray(5, stopHue));
+ deviceClient.write(layerHeight.register.address, GetByteArray(6, speed));
+ deviceClient.write(layerHeight.register.address, GetByteArray(7, direction));
+ deviceClient.write(layerHeight.register.address, GetByteArray(9, (byte)repeatAfter));
+ }
+ }
+
+ public static class RainbowSnakes extends AnimationBase {
+ private int numberOfSnakes = 3;
+ private int snakeLength = 5;
+ private int spacingBetween = 2;
+ private int repeatAfter = 15;
+ private float startHue = 0.0f;
+ private float stopHue = 360.0f;
+ private Color backgroundColor = Color.TRANSPARENT;
+ private float speed = 0.50f;
+ private Direction direction = Direction.Backward;
+
+ //region Constructors
+ public RainbowSnakes(){ super(AnimationType.RAINBOW_SNAKES); }
+ public RainbowSnakes(float startHue, float stopHue) {
+ this();
+ this.startHue = startHue;
+ this.stopHue = stopHue;
+ }
+ public RainbowSnakes(float startHue, float stopHue, int numberOfSnakes){
+ this(startHue, stopHue);
+ this.numberOfSnakes = (byte)numberOfSnakes;
+ }
+ public RainbowSnakes(float startHue, float stopHue, int numberOfSnakes, int snakeLength){
+ this(startHue, stopHue, numberOfSnakes);
+ this.snakeLength = (byte)snakeLength;
+ }
+ public RainbowSnakes(float startHue, float stopHue, int numberOfSnakes, int snakeLength, int spacingBetween){
+ this(startHue, stopHue, numberOfSnakes, snakeLength);
+ this.spacingBetween = (byte)spacingBetween;
+ }
+ public RainbowSnakes(float startHue, float stopHue, int numberOfSnakes, int snakeLength, int spacingBetween, int repeatAfter){
+ this(startHue, stopHue, numberOfSnakes, snakeLength, spacingBetween);
+ this.repeatAfter = (byte)repeatAfter;
+ }
+ public RainbowSnakes(float startHue, float stopHue, int numberOfSnakes, int snakeLength, int spacingBetween, int repeatAfter, Color backgroundColor){
+ this(startHue, stopHue, numberOfSnakes, snakeLength, spacingBetween, repeatAfter);
+ this.backgroundColor = backgroundColor;
+ }
+ public RainbowSnakes(float startHue, float stopHue, int numberOfSnakes, int snakeLength, int spacingBetween, int repeatAfter, Color backgroundColor, float speed){
+ this(startHue, stopHue, numberOfSnakes, snakeLength, spacingBetween, repeatAfter, backgroundColor);
+ this.speed = speed;
+ }
+ public RainbowSnakes(float startHue, float stopHue, int numberOfSnakes, int snakeLength, int spacingBetween, int repeatAfter, Color backgroundColor, float speed, Direction direction){
+ this(startHue, stopHue, numberOfSnakes, snakeLength, spacingBetween, repeatAfter, backgroundColor, speed);
+ this.direction = direction;
+ }
+ //endregion
+
+ //region Getters/Setters
+ public float getSpeed() { return speed; }
+ public float getStartHue() { return startHue;}
+ public float getStopHue() { return stopHue; }
+ public int getSnakeLength() { return snakeLength; }
+ public int getRepeatAfter() { return repeatAfter; }
+ public Direction getDirection() { return direction; }
+ public int getSpacingBetween() { return spacingBetween; }
+ public Color getBackgroundColor() { return backgroundColor; }
+ public int getNumberOfSnakes() { return numberOfSnakes; }
+
+ /**
+ * Sets the speed of the animation
+ * @param speed from 0 to 1.
+ */
+ public void setSpeed(float speed) { this.speed = speed; }
+
+ /**
+ * Can be used to limit the colors shown in the rainbow. This is a hue so it reflects
+ * the position as an angle on a color wheel. Red is 0, Green is 120, Blue is 240, etc.
+ * @param startHue from 0 to 360.
+ */
+ public void setStartHue(float startHue) { this.startHue = startHue; }
+
+ /**
+ * Can be used to limit the colors shown in the rainbow. This is a hue so it reflects
+ * the position as an angle on a color wheel. Red is 0, Green is 120, Blue is 240, etc.
+ * @param stopHue from 0 to 360.
+ */
+ public void setStopHue(float stopHue) { this.stopHue = stopHue; }
+ public void setHues(float startHue, float stopHue) {
+ this.startHue = startHue;
+ this.stopHue = stopHue;
+ }
+ public void setDirection(Direction direction) { this.direction = direction; }
+
+ /**
+ * Length in pixels of each snake.
+ * @param snakeLength from 0 to 255.
+ */
+ public void setSnakeLength(int snakeLength) { this.snakeLength = snakeLength; }
+
+ /**
+ * The number of pixels between the last snake and the first snake of a repeating animation.
+ * @param repeatAfter from 0 to 255.
+ */
+ public void setRepeatAfter(int repeatAfter) { this.repeatAfter = repeatAfter; }
+
+ /**
+ * The number of pixels between consecutive snakes.
+ * @param spacingBetween from 0 to 255.
+ */
+ public void setSpacingBetween(int spacingBetween) { this.spacingBetween = spacingBetween; }
+ public void setBackgroundColor(Color backgroundColor) { this.backgroundColor = backgroundColor; }
+ public void setNumberOfSnakes(int numberOfSnakes) { this.numberOfSnakes = numberOfSnakes; }
+ //endregion
+
+ @Override
+ protected void updateAnimationSpecificValuesOverI2C(I2cDeviceSynchSimple deviceClient)
+ {
+ deviceClient.write(layerHeight.register.address, GetByteArray(4 , startHue));
+ deviceClient.write(layerHeight.register.address, GetByteArray(5 , stopHue));
+ deviceClient.write(layerHeight.register.address, GetByteArray(6 , speed));
+ deviceClient.write(layerHeight.register.address, GetByteArray(7 , direction));
+ deviceClient.write(layerHeight.register.address, GetByteArray(8 , (byte)spacingBetween));
+ deviceClient.write(layerHeight.register.address, GetByteArray(9 , (byte)repeatAfter));
+ deviceClient.write(layerHeight.register.address, GetByteArray(10, backgroundColor));
+ deviceClient.write(layerHeight.register.address, GetByteArray(11, (byte)snakeLength));
+ deviceClient.write(layerHeight.register.address, GetByteArray(12, (byte)numberOfSnakes));
+ }
+ }
+
+ public static class Random extends AnimationBase {
+ private float startHue = 0f;
+ private float stopHue = 360f;
+ private float speed = 0.1f;
+
+ //region Constructors
+ public Random(){ super(AnimationType.RANDOM); }
+ public Random(float startHue, float stopHue){
+ this();
+ setHues(startHue, stopHue);
+ }
+ public Random(float startHue, float stopHue, float speed){
+ this(startHue, stopHue);
+ this.speed = speed;
+ }
+ //endregion
+
+ //region Getters/Setters
+ public float getSpeed() { return speed; }
+ public float getStopHue() { return stopHue; }
+ public float getStartHue() { return startHue; }
+
+ /**
+ * Sets the speed of the animation
+ * @param speed from 0 to 1.
+ */
+ public void setSpeed(float speed) { this.speed = speed; }
+
+ /**
+ * Can be used to limit the colors shown in the rainbow. This is a hue so it reflects
+ * the position as an angle on a color wheel. Red is 0, Green is 120, Blue is 240, etc.
+ * @param startHue from 0 to 360.
+ */
+ public void setStartHue(float startHue) { this.startHue = startHue; }
+
+ /**
+ * Can be used to limit the colors shown in the rainbow. This is a hue so it reflects
+ * the position as an angle on a color wheel. Red is 0, Green is 120, Blue is 240, etc.
+ * @param stopHue from 0 to 360.
+ */
+ public void setStopHue(float stopHue) { this.stopHue = stopHue; }
+ public void setHues(float startHue, float stopHue) {
+ this.startHue = startHue;
+ this.stopHue = stopHue;
+ }
+ //endregion
+ @Override
+ protected void updateAnimationSpecificValuesOverI2C(I2cDeviceSynchSimple deviceClient)
+ {
+ deviceClient.write(layerHeight.register.address, GetByteArray(4, startHue));
+ deviceClient.write(layerHeight.register.address, GetByteArray(5, stopHue));
+ deviceClient.write(layerHeight.register.address, GetByteArray(6, speed));
+ }
+ }
+
+ public static class SineWave extends AnimationBase {
+ private Color secondaryColor = Color.BLUE;
+ private Color primaryColor = Color.RED;
+ private Direction direction = Direction.Forward;
+ private float offset = 0.5f;
+ private float speed = 0.5f;
+ private int period = 1000;
+
+ public SineWave(){ super(AnimationType.SINE_WAVE); }
+ public SineWave(Color primaryColor) {
+ this();
+ this.primaryColor = primaryColor;
+ }
+ public SineWave(Color primaryColor, Color secondaryColor) {
+ this(primaryColor);
+ this.secondaryColor = secondaryColor;
+ }
+ public SineWave(Color primaryColor, Color secondaryColor, int period) {
+ this(primaryColor, secondaryColor);
+ this.period = period;
+ }
+ public SineWave(Color primaryColor, Color secondaryColor, int period, float speed) {
+ this(primaryColor, secondaryColor, period);
+ this.speed = speed;
+ }
+ public SineWave(Color primaryColor, Color secondaryColor, int period, float speed, float offset) {
+ this(primaryColor, secondaryColor, period, speed);
+ this.offset = offset;
+ }
+ public SineWave(Color primaryColor, Color secondaryColor, int period, float speed, float offset, Direction direction) {
+ this(primaryColor, secondaryColor, period, speed, offset);
+ this.direction = direction;
+ }
+
+ /**
+ * set the period of the sine wave in milliseconds.
+ * @param period from 0 - 4,294,697,295. Larger is longer.
+ */
+ public void setPeriod(int period) { this.period = period; }
+
+ /**
+ * set the period of the sine wave in specified unit.
+ * @param duration duration of loop.
+ * @param timeUnit unit to use.
+ */
+ public void setPeriod(int duration, TimeUnit timeUnit){
+ this.period = Math.toIntExact(timeUnit.toMillis(duration));
+ }
+
+ /**
+ * Sets the speed of the animation
+ * @param speed from 0 to 1.
+ */
+ public void setSpeed(float speed) { this.speed = speed; }
+
+ /**
+ * Sets the vertical height of the sine wave, this will increase the contrast between the
+ * two shown colors.
+ * @param offset from 0 to 1.
+ */
+ public void setOffset(float offset) { this.offset = offset; }
+ public void setPrimaryColor(Color color) { primaryColor = color; }
+ public void setSecondaryColor(Color color) { secondaryColor = color; }
+ public void setDirection(Direction direction) { this.direction = direction; }
+ public void setPrimaryColor(int red, int green, int blue) { primaryColor = new Color((byte)red, (byte)green, (byte)blue); }
+ public void setSecondaryColor(int red, int green, int blue) { secondaryColor = new Color((byte)red, (byte)green, (byte)blue); }
+
+ public int getPeriod() { return period; }
+ public float getSpeed() { return speed; }
+ public float getOffset(){ return offset; }
+ public Direction getDirection() { return direction; }
+ public Color getPrimaryColor() { return primaryColor; }
+ public Color getSecondaryColor() { return secondaryColor; }
+
+ @Override
+ protected void updateAnimationSpecificValuesOverI2C(I2cDeviceSynchSimple deviceClient)
+ {
+ deviceClient.write(layerHeight.register.address, GetByteArray(4, primaryColor));
+ deviceClient.write(layerHeight.register.address, GetByteArray(5, secondaryColor));
+ deviceClient.write(layerHeight.register.address, GetByteArray(6, period));
+ deviceClient.write(layerHeight.register.address, GetByteArray(7, direction));
+ deviceClient.write(layerHeight.register.address, GetByteArray(8, offset));
+ deviceClient.write(layerHeight.register.address, GetByteArray(9, speed));
+ }
+ }
+
+ public static class SingleFill extends AnimationBase {
+ private Color[] colors = {Color.WHITE, Color.GREEN, Color.BLUE};
+ private int period = 750;
+ private float speed = 0.5f;
+ private int pixelLength = 3;
+ private Direction direction = Direction.Backward;
+ private SingleFillStyle style = SingleFillStyle.FillIn;
+
+ public enum SingleFillStyle{
+ FillIn(0),
+ FillOut(1);
+
+ final byte styleValue;
+ SingleFillStyle(int styleValue){ this.styleValue = (byte)styleValue; }
+ }
+
+ public SingleFill(){ super(AnimationType.SINGLE_FILL); }
+ public SingleFill(Color... colors) {
+ this();
+ this.colors = colors;
+ }
+
+ public int getPeriod() { return period; }
+ public float getSpeed() { return speed; }
+ public Color[] getColors() { return colors; }
+ public int getPixelLength() { return pixelLength; }
+ public Direction getDirection() { return direction; }
+ public SingleFillStyle getStyle() { return style; }
+
+ /**
+ * set the period of the single fill in milliseconds.
+ * @param period from 0 - 4,294,697,295. Larger is longer.
+ */
+ public void setPeriod(int period) { this.period = period; }
+
+ /**
+ * set the period of the single fill in specified unit.
+ * @param duration duration of loop.
+ * @param timeUnit unit to use.
+ */
+ public void setPeriod(int duration, TimeUnit timeUnit){
+ this.period = Math.toIntExact(timeUnit.toMillis(duration));
+ }
+
+ /**
+ * Sets the speed of the animation
+ * @param speed from 0 to 1.
+ */
+ public void setSpeed(float speed) { this.speed = speed; }
+ public void setColors(Color... colors) {
+ for(int i = 0; i < Math.min(colors.length,10); i++){
+ this.colors[i] = colors[i];
+ }
+ }
+ public void setPixelLength(int pixelLength) { this.pixelLength = pixelLength; }
+ public void setStyle(SingleFillStyle style) { this.style = style; }
+ public void setDirection(Direction direction) { this.direction = direction; }
+
+ @Override
+ protected void updateAnimationSpecificValuesOverI2C(I2cDeviceSynchSimple deviceClient)
+ {
+ deviceClient.write(layerHeight.register.address, GetByteArray(4, (byte)colors.length));
+ deviceClient.write(layerHeight.register.address, GetByteArray(5, colors));
+ deviceClient.write(layerHeight.register.address, GetByteArray(6, period));
+ deviceClient.write(layerHeight.register.address, GetByteArray(7, direction));
+ deviceClient.write(layerHeight.register.address, GetByteArray(8, (byte)pixelLength));
+ deviceClient.write(layerHeight.register.address, GetByteArray(9, speed));
+ deviceClient.write(layerHeight.register.address, GetByteArray(11, style.styleValue));
+ }
+ }
+
+ public static class Snakes extends AnimationBase {
+ private Color[] colors = {Color.RED, Color.WHITE, Color.BLUE};
+ private int snakeLength = 5;
+ private int spacingBetween = 2;
+ private int repeatAfter = 15;
+ private Color backgroundColor = Color.TRANSPARENT;
+ private float speed = 0.50f;
+ private Direction direction = Direction.Backward;
+
+ //region Constructors
+ public Snakes(){ super(AnimationType.SNAKES); }
+ public Snakes(Color... colors) {
+ this();
+ this.colors = colors;
+ }
+ public Snakes(int snakeLength, Color... colors){
+ this(colors);
+ this.snakeLength = (byte)snakeLength;
+ }
+ public Snakes(int snakeLength, int spacingBetween, Color... colors){
+ this(snakeLength, colors);
+ this.spacingBetween = (byte)spacingBetween;
+ }
+ public Snakes(int snakeLength, int spacingBetween, int repeatAfter, Color... colors){
+ this(snakeLength, spacingBetween, colors);
+ this.repeatAfter = (byte)repeatAfter;
+ }
+ public Snakes(int snakeLength, int spacingBetween, int repeatAfter, Color backgroundColor, Color... colors){
+ this(snakeLength, spacingBetween, repeatAfter, colors);
+ this.backgroundColor = backgroundColor;
+ }
+ public Snakes(int snakeLength, int spacingBetween, int repeatAfter, Color backgroundColor, float speed, Color... colors){
+ this(snakeLength, spacingBetween, repeatAfter, backgroundColor, colors);
+ this.speed = speed;
+ }
+ public Snakes(int snakeLength, int spacingBetween, int repeatAfter, Color backgroundColor, float speed, Direction direction, Color... colors){
+ this(snakeLength, spacingBetween, repeatAfter, backgroundColor, speed, colors);
+ this.direction = direction;
+ }
+ //endregion
+
+ //region Getters/Setters
+ public float getSpeed() { return speed; }
+ public Color[] getColors() { return colors; }
+ public int getSnakeLength() { return snakeLength; }
+ public int getRepeatAfter() { return repeatAfter; }
+ public Direction getDirection() { return direction; }
+ public int getSpacingBetween() { return spacingBetween; }
+ public Color getBackgroundColor() { return backgroundColor; }
+
+ /**
+ * Sets the speed of the animation.
+ * @param speed from 0 to 1.
+ */
+ public void setSpeed(float speed) { this.speed = speed; }
+ public void setColors(Color... colors) { this.colors = colors;}
+ public void setDirection(Direction direction) { this.direction = direction; }
+
+ /**
+ * Length in pixels of each snake.
+ * @param snakeLength from 0 to 255.
+ */
+ public void setSnakeLength(int snakeLength) { this.snakeLength = snakeLength; }
+
+ /**
+ * The number of pixels between the last snake and the first snake of a repeating animation.
+ * @param repeatAfter from 0 to 255.
+ */
+ public void setRepeatAfter(int repeatAfter) { this.repeatAfter = repeatAfter; }
+
+ /**
+ * The number of pixels between consecutive snakes.
+ * @param spacingBetween from 0 to 255.
+ */
+ public void setSpacingBetween(int spacingBetween) { this.spacingBetween = spacingBetween; }
+ public void setBackgroundColor(Color backgroundColor) { this.backgroundColor = backgroundColor; }
+ //endregion
+ @Override
+ protected void updateAnimationSpecificValuesOverI2C(I2cDeviceSynchSimple deviceClient)
+ {
+ deviceClient.write(layerHeight.register.address, GetByteArray(4, (byte)colors.length));
+ deviceClient.write(layerHeight.register.address, GetByteArray(5, colors));
+ deviceClient.write(layerHeight.register.address, GetByteArray(6, speed));
+ deviceClient.write(layerHeight.register.address, GetByteArray(7, direction));
+ deviceClient.write(layerHeight.register.address, GetByteArray(8, (byte)spacingBetween));
+ deviceClient.write(layerHeight.register.address, GetByteArray(9, (byte)repeatAfter));
+ deviceClient.write(layerHeight.register.address, GetByteArray(10, backgroundColor));
+ deviceClient.write(layerHeight.register.address, GetByteArray(11, (byte)snakeLength));
+ }
+ }
+
+ public static class Solid extends AnimationBase {
+ private Color primaryColor = Color.RED;
+
+ public Solid(){super(AnimationType.SOLID);}
+ public Solid(Color primaryColor) {
+ super(AnimationType.SOLID);
+ this.primaryColor = primaryColor;
+ }
+ public Solid(Color primaryColor, int brightness) {
+ super(AnimationType.SOLID, brightness);
+ this.primaryColor = primaryColor;
+ }
+ public Solid(Color primaryColor, int startIndex, int stopIndex){
+ super(AnimationType.SOLID, startIndex, stopIndex);
+ this.primaryColor = primaryColor;
+ }
+ public Solid(Color primaryColor, int brightness, int startIndex, int stopIndex){
+ super(AnimationType.SOLID, brightness, startIndex, stopIndex);
+ this.primaryColor = primaryColor;
+ }
+
+ public void setPrimaryColor(int red, int green, int blue) { primaryColor = new Color((byte)red, (byte)green, (byte)blue); }
+ public void setPrimaryColor(Color color) { primaryColor = color; }
+ public Color getPrimaryColor() { return primaryColor; }
+
+ @Override
+ protected void updateAnimationSpecificValuesOverI2C(I2cDeviceSynchSimple deviceClient)
+ {
+ deviceClient.write(layerHeight.register.address, GetByteArray(4, primaryColor));
+ }
+ }
+
+ public static class Sparkle extends AnimationBase {
+ private Color primaryColor = Color.WHITE;
+ private Color secondaryColor = Color.TRANSPARENT;
+ private int sparkleProbability = 16;
+ private int period = 100;
+
+ //region Constructors
+ public Sparkle() { super(AnimationType.SPARKLE); }
+ public Sparkle(Color primaryColor){
+ this();
+ this.primaryColor = primaryColor;
+ }
+ public Sparkle(Color primaryColor, Color secondaryColor){
+ this(primaryColor);
+ this.secondaryColor = secondaryColor;
+ }
+ public Sparkle(Color primaryColor, Color secondaryColor, int period){
+ this(primaryColor, secondaryColor);
+ this.period = period;
+ }
+ //endregion
+
+ //region Getters/Setters
+ public int getPeriod() { return period; }
+ public Color getPrimaryColor() { return primaryColor; }
+ public Color getSecondaryColor() { return secondaryColor; }
+ public int getSparkleProbability() { return sparkleProbability; }
+
+ /**
+ * sets the time between updating the sparkles.
+ * @param period from 0 - 4,294,697,295. Larger is longer.
+ */
+ public void setPeriod(int period) { this.period = period; }
+ public void setPrimaryColor(Color primaryColor) { this.primaryColor = primaryColor; }
+ public void setSecondaryColor(Color secondaryColor) { this.secondaryColor = secondaryColor; }
+
+ /**
+ * Sets the probability/density of sparkles. Lower is denser. Default is 16.
+ * @param sparkleProbability from 0 to 255.
+ */
+ public void setSparkleProbability(int sparkleProbability) { this.sparkleProbability = sparkleProbability; };
+ //endregion
+ @Override
+ protected void updateAnimationSpecificValuesOverI2C(I2cDeviceSynchSimple deviceClient)
+ {
+ deviceClient.write(layerHeight.register.address, GetByteArray(4, primaryColor));
+ deviceClient.write(layerHeight.register.address, GetByteArray(5, secondaryColor));
+ deviceClient.write(layerHeight.register.address, GetByteArray(6, period));
+ deviceClient.write(layerHeight.register.address, GetByteArray(7, (byte)sparkleProbability));
+ }
+ }
+}
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..d4338b8b7e58 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,71 @@
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(13.6) // TODO: Change this to the actual weight of the robot
+ .forwardZeroPowerAcceleration(-42.5878)
+ .lateralZeroPowerAcceleration(-68.2022)
+ // Set following parameters to true to enable dual PID
+ .useSecondaryTranslationalPIDF(false)
+ .useSecondaryHeadingPIDF(false)
+ .useSecondaryDrivePIDF(false)
+ .centripetalScaling(0.0002)
+ .translationalPIDFCoefficients(new PIDFCoefficients(0.15, 0, 0.01, 0.05))
+ .headingPIDFCoefficients(new PIDFCoefficients(2, 0, 0.1, 0.026))
+ .drivePIDFCoefficients(new FilteredPIDFCoefficients(0.02, 0, 0.001, 0.6, 0.02));
+
+ public static PathConstraints pathConstraints = new PathConstraints(
+ 0.99,
+ 0.1,
+ 0.1,
+ 0.009,
+ 50,
+ 1.25,
+ 10,
+ 1
+ );
+
+ public static MecanumConstants driveConstants = new MecanumConstants()
+ .maxPower(1)
+ .rightFrontMotorName("FR")
+ .rightRearMotorName("BR")
+ .leftRearMotorName("BL")
+ .leftFrontMotorName("FL")
+ .leftFrontMotorDirection(DcMotorSimple.Direction.REVERSE)
+ .leftRearMotorDirection(DcMotorSimple.Direction.REVERSE)
+ .rightFrontMotorDirection(DcMotorSimple.Direction.FORWARD)
+ .rightRearMotorDirection(DcMotorSimple.Direction.FORWARD)
+ .xVelocity(65.1356)
+ .yVelocity(39.2622);
- public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1, 1);
+ public static PinpointConstants localizerConstants = new PinpointConstants()
+ .forwardPodY(-3.25)
+ .strafePodX(-6.25)
+ .distanceUnit(DistanceUnit.INCH)
+ .hardwareMapName("pinpoint")
+ .encoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD)
+ .forwardEncoderDirection(GoBildaPinpointDriver.EncoderDirection.FORWARD)
+ .strafeEncoderDirection(GoBildaPinpointDriver.EncoderDirection.FORWARD);
public static Follower createFollower(HardwareMap hardwareMap) {
return new FollowerBuilder(followerConstants, hardwareMap)
.pathConstraints(pathConstraints)
+ .mecanumDrivetrain(driveConstants)
+ .pinpointLocalizer(localizerConstants)
.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 5fb6b3280092..01693c124f90 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
@@ -1,8 +1,8 @@
package org.firstinspires.ftc.teamcode.pedroPathing;
import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.changes;
-import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.drawOnlyCurrent;
import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.draw;
+import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.drawOnlyCurrent;
import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.follower;
import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.stopRobot;
import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.telemetryM;
@@ -15,13 +15,16 @@
import com.bylazar.field.Style;
import com.bylazar.telemetry.PanelsTelemetry;
import com.bylazar.telemetry.TelemetryManager;
-import com.pedropathing.ErrorCalculator;
import com.pedropathing.follower.Follower;
-import com.pedropathing.geometry.*;
-import com.pedropathing.math.*;
-import com.pedropathing.paths.*;
+import com.pedropathing.geometry.BezierCurve;
+import com.pedropathing.geometry.BezierLine;
+import com.pedropathing.geometry.Pose;
+import com.pedropathing.math.Vector;
+import com.pedropathing.paths.HeadingInterpolator;
+import com.pedropathing.paths.Path;
+import com.pedropathing.paths.PathChain;
import com.pedropathing.telemetry.SelectableOpMode;
-import com.pedropathing.util.*;
+import com.pedropathing.util.PoseHistory;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
@@ -330,7 +333,7 @@ public void loop() {
*/
class ForwardVelocityTuner extends OpMode {
private final ArrayList velocities = new ArrayList<>();
- public static double DISTANCE = 48;
+ public static double DISTANCE = 96;
public static double RECORD_NUMBER = 10;
private boolean end;
@@ -437,7 +440,7 @@ public void loop() {
class LateralVelocityTuner extends OpMode {
private final ArrayList velocities = new ArrayList<>();
- public static double DISTANCE = 48;
+ public static double DISTANCE = 96;
public static double RECORD_NUMBER = 10;
private boolean end;
@@ -537,7 +540,7 @@ public void loop() {
*/
class ForwardZeroPowerAccelerationTuner extends OpMode {
private final ArrayList accelerations = new ArrayList<>();
- public static double VELOCITY = 30;
+ public static double VELOCITY = 60;
private double previousVelocity;
private long previousTimeNano;
@@ -643,7 +646,7 @@ public void loop() {
*/
class LateralZeroPowerAccelerationTuner extends OpMode {
private final ArrayList accelerations = new ArrayList<>();
- public static double VELOCITY = 30;
+ public static double VELOCITY = 35;
private double previousVelocity;
private long previousTimeNano;
private boolean stopping;
diff --git a/build.dependencies.gradle b/build.dependencies.gradle
index a61e71307f59..52bde17c270f 100644
--- a/build.dependencies.gradle
+++ b/build.dependencies.gradle
@@ -18,5 +18,7 @@ dependencies {
implementation 'com.pedropathing:ftc:2.0.5'
implementation 'com.pedropathing:telemetry:1.0.0'
implementation 'com.bylazar:fullpanels:1.0.9'
+
+ implementation 'dev.nextftc:control:1.0.0'
}