super()
+// * @param startingPose Indicates the starting pose of the robot
+// * @param runTurret Indicates whether to run the turret auto aim functions
+// */
+// Auto(Pose startingPose, Boolean runTurret){
+// turretOn = runTurret;
+// startPose = startingPose;
+// }
+// Auto(Pose startingPose){
+// turretOn = true;
+// startPose = startingPose;
+// }
+
+ abstract Pose getStartPose();
+ abstract boolean getTurretEnabled();
+
+ /** Override this to schedule the auto command*/
+ abstract void runAuto();
+ /** Override this to rename the auto*/
+ @NonNull
+ @Override
+ public abstract String toString();
+ }
+
+ private class Leave_Far_Generic extends Auto{
+ public Pose startPose = paths.farStart;
+ public boolean turretEnabled = false;
+
+ @Override
+ public Pose getStartPose(){
+ return startPose;
+ }
+ @Override
+ public boolean getTurretEnabled(){
+ return turretEnabled;
+ }
+
+ @Override
+ public void runAuto(){
+ new SequentialGroup(
+ Commands.runPath(paths.farStart_to_NoTurret_FarShoot, true, 1),
+ Commands.shootBalls(),
+ Commands.setIntakeMode(Intake.intakeMode.INTAKING),
+ Commands.runPath(paths.farShoot_noTurret_to_farPreload, true, 1),
+ Commands.setIntakeMode(Intake.intakeMode.OFF)
+ ).schedule();
+ }
+
+ @NonNull
+ @Override
+ public String toString(){return "Far Zone No Turret Generic";}
+ }
+
+ private class Near_9Ball extends Auto{
+ public Pose startPose = paths.nearStart;
+ public boolean turretEnabled = true;
+
+ @Override
+ public Pose getStartPose(){
+ return startPose;
+ }
+ @Override
+ public boolean getTurretEnabled(){
+ return turretEnabled;
+ }
+
+ @Override
+ public void runAuto(){
+ new SequentialGroup(
+ Commands.setIntakeMode(Intake.intakeMode.INTAKING),
+ new InstantCommand(Commands.setFlywheelState(Turret.flywheelState.ON)),
+ Commands.runPath(paths.nearStart_to_midShoot, true, 1),
+ Commands.shootBalls(),
+ Commands.setFlywheelState(Turret.flywheelState.ON),
+ Commands.setIntakeMode(Intake.intakeMode.INTAKING),
+ Commands.runPath(paths.midShoot_to_nearPreload, true, 1),
+ Commands.runPath(paths.nearPreload_to_midShoot, true, 1),
+ Commands.shootBalls(),
+ Commands.setFlywheelState(Turret.flywheelState.ON),
+ Commands.setIntakeMode(Intake.intakeMode.INTAKING),
+ Commands.runPath(paths.midShoot_to_midPreload, true, 1),
+ Commands.runPath(paths.midPreload_to_midShoot, true, 1),
+ Commands.shootBalls(),
+ Commands.runPath(paths.midShoot_to_nearLeave, true, 1)
+ ).schedule();
+ }
+
+ @NonNull
+ @Override
+ public String toString(){return "Near Zone 9 Ball Auto";}
+ }
+
+ private class Near_12Ball extends Auto{
+ public Pose startPose = paths.nearStart;
+ public boolean turretEnabled = true;
+
+ @Override
+ public Pose getStartPose(){
+ return startPose;
+ }
+ @Override
+ public boolean getTurretEnabled(){
+ return turretEnabled;
+ }
+
+ @Override
+ public void runAuto(){
+ new SequentialGroup(
+ Commands.setIntakeMode(Intake.intakeMode.INTAKING),
+ new InstantCommand(Commands.setFlywheelState(Turret.flywheelState.ON)),
+ Commands.runPath(paths.nearStart_to_midShoot, true, 1),
+ Commands.shootBalls(),
+ Commands.setFlywheelState(Turret.flywheelState.ON),
+ Commands.setIntakeMode(Intake.intakeMode.INTAKING),
+ Commands.runPath(paths.midShoot_to_nearPreload, true, 1),
+ Commands.runPath(paths.nearPreload_to_midShoot, true, 1),
+ Commands.shootBalls(),
+ Commands.setFlywheelState(Turret.flywheelState.ON),
+ Commands.setIntakeMode(Intake.intakeMode.INTAKING),
+ Commands.runPath(paths.midShoot_to_midPreload, true, 1),
+ Commands.runPath(paths.midPreload_to_midShoot, true, 1),
+ Commands.shootBalls(),
+ Commands.setFlywheelState(Turret.flywheelState.ON),
+ Commands.setIntakeMode(Intake.intakeMode.INTAKING),
+ Commands.runPath(paths.midShoot_to_farPreload, true, 1),
+ Commands.runPath(paths.farPreload_to_midShoot, true, 1),
+ Commands.shootBalls(),
+ Commands.runPath(paths.midShoot_to_nearLeave, true, 1)
+ ).schedule();
+ }
+
+ @NonNull
+ @Override
+ public String toString(){return "Near Zone 12 Ball Auto";}
}
/**
- * This auto starts at the near zone,
- * shoots it's preloads after 5 seconds,
- * and goes to the leave pose that is in the launch zone.
+ * This auto starts at the far zone
*/
- private Command Leave_Near_Launch() {
- turretOn = true;
- return new SequentialGroup(
- Commands.setFlywheelState(Robot, Turret.flywheelstate.ON),
- new ParallelGroup(
- new WaitUntil(() -> Robot.turret.getFlywheelRPM() > 5900),
- new Delay(5)
- ),
- Commands.setIntakeMode(Robot, Intake.Mode.SHOOTING),
- new Delay(2),
- Commands.setIntakeMode(Robot, Intake.Mode.INTAKING),
- new Delay(1),
- Commands.setIntakeMode(Robot, Intake.Mode.SHOOTING),
- new Delay(2),
- Commands.setTransferState(Robot, Intake.transferState.UP),
- new Delay(1),
- Commands.setTransferState(Robot, Intake.transferState.DOWN),
- Commands.setIntakeMode(Robot, Intake.Mode.OFF),
- Commands.setFlywheelState(Robot, Turret.flywheelstate.OFF),
- Commands.runPath(Robot.drivetrain.paths.start1_to_leave1, true, 0.6)
- );
+ private class Complex_Test_Auto extends Auto{
+ public Pose startPose = paths.farStart;
+ public boolean turretEnabled = true;
+
+ @Override
+ public Pose getStartPose(){
+ return startPose;
+ }
+ @Override
+ public boolean getTurretEnabled(){
+ return turretEnabled;
+ }
+
+ @Override
+ void runAuto() {
+ new SequentialGroup(
+ new Delay(1),
+ //Commands.shootBalls(),
+ Commands.setIntakeMode(Intake.intakeMode.INTAKING),
+ Commands.runPath(paths.farStart_to_farPreload, true, 1),
+ Commands.setIntakeMode(Intake.intakeMode.OFF),
+ Commands.runPath(paths.farPreload_to_farShoot, true, 1),
+ //Commands.shootBalls(),
+ Commands.setIntakeMode(Intake.intakeMode.INTAKING),
+ Commands.runPath(paths.farStart_to_midPreload, true, 1),
+ Commands.setIntakeMode(Intake.intakeMode.OFF),
+ Commands.runPath(paths.midPreload_to_farShoot, true, 1),
+ //Commands.shootBalls(),
+ Commands.runPath(paths.farShoot_to_farLeave, true, 1)
+ ).schedule();
+ }
+
+ @NonNull
+ @Override
+ public String toString() {
+ return "Complex Test Auto";
+ }
+ }
+
+ private class test_Auto extends Auto{
+ public Pose startPose = paths.farStart;
+ public boolean turretEnabled = false;
+
+ @Override
+ public Pose getStartPose(){
+ return startPose;
+ }
+ @Override
+ public boolean getTurretEnabled(){
+ return turretEnabled;
+ }
+
+ @Override
+ public void runAuto(){
+ double tempSpeed = 1;
+ new SequentialGroup(
+ Commands.runPath(paths.farStart_to_farPreload,true,tempSpeed),
+ Commands.runPath(paths.farPreload_to_farShoot,true,tempSpeed),
+ Commands.runPath(paths.farShoot_to_midPreload, true, tempSpeed),
+ Commands.runPath(paths.midPreload_to_midShoot, true, tempSpeed),
+ Commands.runPath(paths.midShoot_to_nearPreload, true, tempSpeed),
+ Commands.runPath(paths.nearPreload_to_nearShoot, true, tempSpeed)
+ ).schedule();
+ }
+
+ @NonNull
+ @Override
+ public String toString(){return "Test Auto";}
}
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/NextFTC_Pedro_Example_Auto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/NextFTC_Pedro_Example_Auto.java
new file mode 100644
index 000000000000..bcbe78d74df4
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/NextFTC_Pedro_Example_Auto.java
@@ -0,0 +1,54 @@
+//package org.firstinspires.ftc.teamcode.LOADCode.Main_.Auto_;
+//
+//import com.acmerobotics.roadrunner.path.BezierLine;
+//import com.pedropathing.geometry.Pose;
+//import com.pedropathing.paths.PathChain;
+//import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+//
+//import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
+//
+//import dev.nextftc.core.commands.groups.SequentialGroup;
+//import dev.nextftc.core.components.SubsystemComponent;
+//import dev.nextftc.extensions.pedro.FollowPath;
+//import dev.nextftc.extensions.pedro.PedroComponent;
+//import dev.nextftc.ftc.NextFTCOpMode;
+//
+//@Autonomous(name = "blueBottom")
+//public class NextFTC_Pedro_Example_Auto extends NextFTCOpMode {
+// private final Pose startPose = new Pose(85.5, 8.3, Math.toRadians(90.0));
+// private final Pose depositPose = new Pose(84.3, 61.9, Math.toRadians(0.0));
+// private final Pose curvePoint = new Pose(138.2, 48.1);
+//
+// private PathChain skib;
+//
+// public NextFTC_Pedro_Example_Auto() {
+// addComponents(
+// new PedroComponent(Constants::createFollower)
+// );
+// }
+//
+// private void buildPaths() {
+// skib = follower.pathBuilder()
+// .addPath(new BezierLine(startPose, depositPose))
+// .setLinearHeadingInterpolation(startPose.heading, depositPose.heading)
+// .build();
+// }
+//
+// public Command getSecondRoutine() {
+// return new SequentialGroup(
+// new FollowPath(skib)
+// );
+// }
+//
+// @Override
+// public void onInit() {
+// follower.setMaxPower(0.7);
+// follower.setStartingPose(startPose);
+// buildPaths();
+// }
+//
+// @Override
+// public void onStartButtonPressed() {
+// getSecondRoutine().run();
+// }
+//}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Devices.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Devices.java
index f664d27fd09d..1464fc9d6324 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Devices.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Devices.java
@@ -7,8 +7,14 @@
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.DigitalChannel;
+import com.qualcomm.robotcore.hardware.DistanceSensor;
+import com.qualcomm.robotcore.hardware.NormalizedColorSensor;
+import com.qualcomm.robotcore.hardware.NormalizedRGBA;
import com.qualcomm.robotcore.hardware.Servo;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+
import dev.nextftc.control.ControlSystem;
import dev.nextftc.control.KineticState;
import dev.nextftc.control.feedback.PIDCoefficients;
@@ -46,14 +52,25 @@ public double getPower() {
}
public static class DcMotorExClass {
- // PID pidCoefficients
+
+ // Old PID Coefficients
+ PIDCoefficients old_pidCoefficients = new PIDCoefficients(0, 0, 0);
+ BasicFeedforwardParameters old_ffCoefficients = new BasicFeedforwardParameters(0,0,0);
+
+ // PID Coefficients
PIDCoefficients pidCoefficients = new PIDCoefficients(0, 0, 0);
BasicFeedforwardParameters ffCoefficients = new BasicFeedforwardParameters(0,0,0);
- // Encoder ticks/rotation
- // 1620rpm Gobilda - 103.8 ticks at the motor shaft
+ /**
+ * Mode.INTAKINGMode.SHOOTINGMode.REVERSINGMode.OFFintakeMode.INTAKINGintakeMode.SHOOTINGintakeMode.REVERSINGintakeMode.OFFMode.INTAKINGMode.SHOOTINGMode.REVERSINGMode.OFFintakeMode.INTAKINGintakeMode.SHOOTINGintakeMode.REVERSINGintakeMode.OFFgatestate.OPENgatestate.CLOSEDupdateFlywheel() must be called every loop for this to be effective.
* @param state The state to set the flywheel to (ON/OFF)
*/
- public void setFlywheel(flywheelstate state){
- flywheelState = state;
+ public void setFlywheelState(flywheelState state){
+ flywheelMode = state;
+ }
+
+ public double getFlywheelCurrentMaxSpeed(){
+ return targetRPM;
+ }
+
+
+ public boolean zeroTurret(){
+ if (!zeroed){
+ rotation.setPower(1);
+ if (hall.getTriggered()){
+ rotation.setPower(0);
+ rotation.resetEncoder();
+ zeroed = true;
+ }
+ }
+ opMode.telemetry.addLine("ZEROING TURRET");
+ opMode.telemetry.addData("Turret Power", rotation.getPower());
+ opMode.telemetry.update();
+ return !zeroed;
}
/**
* Updates the flywheel PID. Must be called every loop.
*/
public void updateFlywheel(){
- if (flywheelState == flywheelstate.ON){
- setFlywheelRPM(5485.714285714286);
- } else if (flywheelState == flywheelstate.OFF){
+ robotZone.setPosition(Robot.drivetrain.follower.getPose().getX(), Robot.drivetrain.follower.getPose().getY());
+ robotZone.setRotation(Robot.drivetrain.follower.getPose().getHeading());
+
+ opMode.telemetry.addData("In Far Zone", robotZone.isInside(LoadHardwareClass.FarLaunchZone));
+ opMode.telemetry.addData("In Near Zone", robotZone.isInside(LoadHardwareClass.ReallyNearLaunchZoneRed));
+
+ if (robotZone.isInside(LoadHardwareClass.FarLaunchZone)){
+ targetRPM = flywheelFarSpeed;
+ actualFlywheelCoefficients = flywheelCoefficients4200;
+ actualFlywheelFFCoefficients = flywheelFFCoefficients4200;
+ }else{
+ targetRPM = flywheelNearSpeed;
+ actualFlywheelCoefficients = flywheelCoefficients3500;
+ actualFlywheelFFCoefficients = flywheelFFCoefficients3500;
+ }
+
+ if (flywheelMode == flywheelState.ON){
+ setFlywheelRPM(targetRPM);
+ }else if (flywheelMode == flywheelState.OFF){
setFlywheelRPM(0);
}
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java
index 49131376c7c5..248a3e8939e8 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java
@@ -1,51 +1,95 @@
package org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_;
+import androidx.annotation.NonNull;
+
import com.pedropathing.paths.PathChain;
+import com.skeletonarmy.marrow.TimerEx;
import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Actuators_.Intake;
import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Actuators_.Turret;
import dev.nextftc.core.commands.Command;
+import dev.nextftc.core.commands.delays.WaitUntil;
+import dev.nextftc.core.commands.groups.SequentialGroup;
import dev.nextftc.core.commands.utility.InstantCommand;
import dev.nextftc.core.commands.utility.LambdaCommand;
import dev.nextftc.extensions.pedro.FollowPath;
public class Commands {
- public static Object driveSystem = null;
- public static Object turretSystem = null;
- public static Object intakeSystem = null;
+ // Robot Object for command access
+ public LoadHardwareClass Robot;
+ public Commands(@NonNull LoadHardwareClass robot){
+ Robot = robot;
+ }
- public static Command runPath(PathChain path, boolean holdEnd) {
- return new FollowPath(path, holdEnd).requires(driveSystem);
+ // Delay timer for shooting sequence
+ private static final TimerEx shootingTimer = new TimerEx(1);
+ private static Command resetShootingTimer() {
+ return new LambdaCommand("resetShootingTimer").setStart(shootingTimer::restart);
}
- public static Command runPath(PathChain path, boolean holdEnd, double maxPower) {
- return new FollowPath(path, holdEnd, maxPower).requires(driveSystem);
+
+ public Command runPath(PathChain path, boolean holdEnd, double maxPower) {
+ return new FollowPath(path, holdEnd, maxPower);
}
- public static Command setFlywheelState(LoadHardwareClass Robot, Turret.flywheelstate state) {
+ public Command setFlywheelState(Turret.flywheelState state) {
return new LambdaCommand("setFlywheelState()")
.setInterruptible(false)
- .setStart(() -> Robot.turret.setFlywheel(state))
+ .setStart(() -> Robot.turret.setFlywheelState(state))
.setIsDone(() -> {
- if (state == Turret.flywheelstate.ON){
- return Robot.turret.getFlywheelRPM() > 5900;
+ if (state == Turret.flywheelState.ON){
+ return Robot.turret.getFlywheelRPM() > Robot.turret.getFlywheelCurrentMaxSpeed() - 100;
}else{
- return Robot.turret.getFlywheelRPM() < 100;
+ return true;
}
})
- .requires(turretSystem);
+ ;
}
- public static Command setIntakeMode(LoadHardwareClass Robot, Intake.Mode state) {
+ private Command setGateState(Turret.gatestate state){
+ return new InstantCommand(new LambdaCommand("setGateState")
+ .setStart(() -> Robot.turret.setGateState(state))
+ );
+ }
+
+ public Command setIntakeMode(Intake.intakeMode state) {
return new InstantCommand(new LambdaCommand("setIntakeMode()")
.setStart(() -> Robot.intake.setMode(state))
- .requires(intakeSystem));
+ .setIsDone(() -> true)
+ );
}
- public static Command setTransferState(LoadHardwareClass Robot, Intake.transferState state) {
+ public Command setTransferState(Intake.transferState state) {
return new InstantCommand(new LambdaCommand("setIntakeMode()")
.setStart(() -> Robot.intake.setTransfer(state))
- .requires(intakeSystem));
+ .setIsDone(() -> true)
+ );
}
+
+ public Command shootBalls(){
+ return new SequentialGroup(
+ // Ensure the flywheel is up to speed, if not, spin up first
+ setFlywheelState(Turret.flywheelState.ON),
+
+ // Shoot the first two balls
+ setIntakeMode(Intake.intakeMode.INTAKING),
+ setGateState(Turret.gatestate.OPEN),
+ resetShootingTimer(),
+ new WaitUntil(() -> Robot.intake.getTopSensorState() && !Robot.intake.getBottomSensorState() && shootingTimer.isDone()),
+
+ // Shoot the last ball
+ setIntakeMode(Intake.intakeMode.SHOOTING),
+ setTransferState(Intake.transferState.UP),
+ resetShootingTimer(),
+ new WaitUntil(shootingTimer::isDone),
+
+ // Reset the systems
+ setIntakeMode(Intake.intakeMode.OFF),
+ setGateState(Turret.gatestate.CLOSED),
+ setTransferState(Intake.transferState.DOWN),
+ setFlywheelState(Turret.flywheelState.OFF)
+ );
+ }
+
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/MecanumDrivetrainClass.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/MecanumDrivetrainClass.java
index 89044cb1c545..6f40ac67922b 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/MecanumDrivetrainClass.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/MecanumDrivetrainClass.java
@@ -7,6 +7,7 @@
import com.pedropathing.paths.PathChain;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.LoadHardwareClass;
import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
public class MecanumDrivetrainClass {
@@ -15,7 +16,8 @@ public class MecanumDrivetrainClass {
// Misc Constants
public Follower follower = null;
- public Pedro_Paths paths = null;
+
+ public static Pose robotPose = null;
/**
* Initializes the PedroPathing follower.
@@ -26,12 +28,8 @@ public class MecanumDrivetrainClass {
public void init (@NonNull OpMode myOpMode, Pose initialPose){
// PedroPathing initialization
follower = Constants.createFollower(myOpMode.hardwareMap); // Initializes the PedroPathing path follower
- paths = new Pedro_Paths(follower);
follower.setStartingPose(initialPose); // Sets the initial position of the robot on the field
follower.update(); // Applies the initialization
-
- follower.startTeleopDrive();
- follower.update();
}
/**
@@ -39,15 +37,16 @@ public void init (@NonNull OpMode myOpMode, Pose initialPose){
* Needs to be run once after all hardware is initialized.
* @param myOpMode Allows the follower access to the robot hardware.
* @param initialPose The starting pose of the robot.
- * @param follow The follower object.
+ * @param follow The PedroPathing follower object
*/
public void init (@NonNull OpMode myOpMode, Pose initialPose, Follower follow){
// PedroPathing initialization
- follower = follow;
- paths = new Pedro_Paths(follower);
- follower.setStartingPose(initialPose); // Sets the initial position of the robot on the field
+ follower = follow; // Initializes the PedroPathing path follower
+ follower.setPose(initialPose); // Sets the initial position of the robot on the field
follower.update(); // Applies the initialization
+ }
+ public void startTeleOpDrive(){
follower.startTeleopDrive();
follower.update();
}
@@ -62,9 +61,9 @@ public void init (@NonNull OpMode myOpMode, Pose initialPose, Follower follow){
*/
public void pedroMecanumDrive(double forward, double strafe, double rotate, boolean robotCentric){
follower.setTeleOpDrive(
- -forward,
- -strafe,
- -rotate,
+ -forward * speedMultiplier,
+ -strafe * speedMultiplier,
+ -rotate * speedMultiplier,
robotCentric);
follower.update();
}
@@ -74,6 +73,12 @@ public void runPath(PathChain path, boolean holdEndpoint){
follower.update();
}
+ public double distanceFromGoal(){
+ Pose goalPose = new Pose(0,144,0);
+ if (LoadHardwareClass.selectedAlliance == LoadHardwareClass.Alliance.RED) {goalPose = new Pose(144, 144, 0);}
+ return follower.getPose().distanceFrom(goalPose);
+ }
+
public boolean pathComplete(){
return !follower.isBusy();
}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/Pedro_Paths.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/Pedro_Paths.java
index d45fa0c3d763..5a18905eb4d1 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/Pedro_Paths.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Drivetrain_/Pedro_Paths.java
@@ -10,432 +10,532 @@
public class Pedro_Paths {
// The variable to store PedroPathing's follower object for path building
- public Follower follower;
+ private Follower follower;
/**
* Define primary poses to be used in paths
*/
- // Dummy Stand-In Poses
- public Pose dummyStartPose = new Pose(0,0,0);
- public Pose dummyMoveRPPose = new Pose(0,28,0);
// Start Poses
- public Pose startPose1 = new Pose(112, 136.6, Math.toRadians(270));
- public Pose startPose2 = new Pose(88, 7.4, Math.toRadians(90));
+ public Pose nearStart = new Pose(118, 132, Math.toRadians(306));
+ public Pose farStart = new Pose(88, 7.4, Math.toRadians(90));
// Preload Poses
- public Pose preloadPose1 = new Pose(130.000, 83.500, Math.toRadians(90));
- public Pose preloadPose2 = new Pose(132.000, 59.500, Math.toRadians(90));
- public Pose preloadPose3 = new Pose(132.000, 35.500, Math.toRadians(90));
+ public Pose nearPreload = new Pose(127.000, 83.500, Math.toRadians(0));
+ public Pose midPreload = new Pose(132.000, 59.500, Math.toRadians(0));
+ public Pose farPreload = new Pose(132.000, 35.500, Math.toRadians(0));
// Shooting Poses
- public Pose shootingPose1 = new Pose(115, 120, Math.toRadians(-35));
- public Pose shootingPose2 = new Pose(85, 85, Math.toRadians(-15));
- public Pose shootingPose3 = new Pose(85, 15, Math.toRadians(60));
+ public Pose nearShoot = new Pose(115, 120, Math.toRadians(-35));
+ public Pose midShoot = new Pose(85, 85, Math.toRadians(-15));
+ public Pose farShoot = new Pose(85, 15, Math.toRadians(60));
+ public Pose noTurretMidShoot = new Pose(85, 85, Math.toRadians(45));
+ public Pose noTurretFarShoot = new Pose(85, 15, Math.toRadians(67.3));
// Leave Poses
- public Pose leavePose1 = new Pose(90,120);
- public Pose leavePose2 = new Pose(95,55);
- public Pose leavePose3 = new Pose(115,20);
+ public Pose nearLeave = new Pose(90,120, Math.toRadians(90));
+ public Pose midLeave = new Pose(95,55, Math.toRadians(90));
+ public Pose farLeave = new Pose(115,20, Math.toRadians(90));
- // Define all path variables
- // Dummy Paths
- public PathChain basicMoveRPPath;
+ /**
+ * N/AIntake Direction/PowerIntake INTurret Angle OverrideBelt INN/AN/AN/AToggle Turret Autoaim (Default on, locks to forward when offShootN/AFlywheel ToggleHood Up OverrideHood Down OverrideN/AHood Up (Does not work currently)Hood Down (Does not work currently)Sets hood to ideal angle for far zone shootingN/AN/AN/AIntake ReverseN/AIntake Direction/PowerN/ATransfer Belt IN ONLYN/AN/AN/AKicker/Flap/FeederN/AFlywheel ToggleN/AN/AN/AN/AN/AN/AN/AN/A
+ * 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(ListTelemetryObject.
+ * There should only be one of these per program.
+ */
+ public class TelemetryManager{
+ ListTelemetryManager.
+ * 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
index 8dae2b096875..d5b4ec13edc1 100644
--- 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
@@ -1,7 +1,5 @@
//TODO, implement all our external libraries and functionality.
-//TODO, add 2 generic autos for preloads and shooting and stuff
-
//DONE, Implement controls graph to TeleOp
//DONE, Consolidate the generic actuator functionalities folder into one file
@@ -10,7 +8,7 @@
//TODO, Add a more advanced telemetry handler for better organization, readability, and debugging
-//TODO, Add proper functionality for manual turret control to Turret.java
+//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
@@ -20,4 +18,18 @@
//DONE, Finish translating Dylan's December 6th controls into the program.
-//TODO, Figure out how to use NextFTC's Pedropathing extension for auto.
\ No newline at end of file
+//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
index 8f3c7cb0f91c..e61e6cbb6d4d 100644
--- 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
@@ -22,7 +22,7 @@
* 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 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * 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.
*/
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
index 0b39aaea7655..4bf67c5e0d6e 100644
--- 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
@@ -22,7 +22,7 @@
* 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 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * 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.
*/
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