Essentially Photon, which caches power and pos for Servos and Motors, Custom PID, PD, P, and PIDF Objects

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/BaseOpMode.java

@@ -11,6 +11,8 @@ import com.qualcomm.robotcore.hardware.VoltageSensor;
 import com.bylazar.telemetry.JoinedTelemetry;
 
 import org.firstinspires.ftc.teamcode.lib.actions.RoutineManager;
+import org.firstinspires.ftc.teamcode.lib.pid.BaseController;
+import org.firstinspires.ftc.teamcode.lib.util.EnhancedGamepad;
 import org.firstinspires.ftc.teamcode.teleOp.Constants;
 import org.firstinspires.ftc.teamcode.util.FPSCounter;
 
@@ -39,10 +41,17 @@ public abstract class BaseOpMode extends OpMode {
 
     @Override
     public final void init() {
+
+        if (manager.isRobotHealthy()) {
+            unifiedTelemetry.addData("SYSTEM STATUS", "READY / HEALTHY");
+        } else {
+            unifiedTelemetry.addData("SYSTEM STATUS", "!!! HARDWARE FAILURE !!!");
+            manager.health(unifiedTelemetry);
+        }
         unifiedTelemetry = new JoinedTelemetry(telemetry);
 
         batterySensor = hardwareMap.voltageSensor.iterator().next();
-        PIDController.currentSystemVoltage = batterySensor.getVoltage();
+        BaseController.currentSystemVoltage = batterySensor.getVoltage();
 
         allHubs = hardwareMap.getAll(LynxModule.class);
         for (LynxModule hub : allHubs) {
@@ -78,7 +87,7 @@ public abstract class BaseOpMode extends OpMode {
         g2.update();
 
         if (voltageTimer.milliseconds() >= Constants.PIDController.VOLTAGE_CHECK_INTERVAL_MS) {
-            PIDController.currentSystemVoltage = batterySensor.getVoltage();
+            BaseController.currentSystemVoltage = batterySensor.getVoltage();
             voltageTimer.reset();
         }
         stateMachineUpdate();
@@ -89,7 +98,7 @@ public abstract class BaseOpMode extends OpMode {
             manager.publishTelemetryAll(unifiedTelemetry);
 
             if (Constants.GLOBAL.DEBUG_MODE) {
-                unifiedTelemetry.addData("System Voltage", PIDController.currentSystemVoltage);
+                unifiedTelemetry.addData("System Voltage", BaseController.currentSystemVoltage);
                 unifiedTelemetry.addData("Loop Time (ms)", fps.getCurrentLoopTime());
                 unifiedTelemetry.addData("FPS", fps.getAverageFps());
             }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/PIDController.java

@@ -1,57 +0,0 @@
-package org.firstinspires.ftc.teamcode.lib;
-
-import com.qualcomm.robotcore.util.ElapsedTime;
-import org.firstinspires.ftc.teamcode.teleOp.Constants;
-
-public class PIDController {
-    public static double currentSystemVoltage = 12.0;
-
-    private double kP, kI, kD;
-    private double lastError = 0;
-    private double integralSum = 0;
-    private final ElapsedTime timer = new ElapsedTime();
-    private boolean hasRunOnce = false;
-
-    public PIDController(double kP, double kI, double kD) {
-        setCoefficients(kP, kI, kD);
-    }
-
-    public void setCoefficients(double kP, double kI, double kD) {
-        this.kP = kP;
-        this.kI = kI;
-        this.kD = kD;
-    }
-
-    public double calculate(double target, double current) {
-        double error = target - current;
-
-        if (!hasRunOnce) {
-            timer.reset();
-            lastError = error;
-            hasRunOnce = true;
-            return kP * error;
-        }
-
-        double dt = timer.seconds();
-        timer.reset();
-
-        if (dt == 0.0) return 0.0;
-
-        double derivative = (error - lastError) / dt;
-        integralSum += (error * dt);
-        lastError = error;
-
-        double rawOutput = (kP * error) + (kI * integralSum) + (kD * derivative);
-
-        double compensationMultiplier = Constants.PIDController.VOLTAGE_REFERENCE / currentSystemVoltage;
-
-        return rawOutput * compensationMultiplier;
-    }
-
-    public void reset() {
-        integralSum = 0;
-        lastError = 0;
-        hasRunOnce = false;
-        timer.reset();
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/SubsysManager.java

@@ -31,4 +31,18 @@ public class SubsysManager {
             sub.publishTelemetry(telemetry);
         }
     }
+
+    public boolean isRobotHealthy() {
+        for (Subsystem sub : subsystems) {
+            if (!sub.isHealthy()) return false;
+        }
+        return true;
+    }
+
+    public void health(Telemetry telemetry) {
+        for (Subsystem sub : subsystems) {
+            String status = sub.isHealthy() ? "[ OK ]" : "[ ERROR ]";
+            telemetry.addData(sub.getClass().getSimpleName(), status);
+        }
+    }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/Subsystem.java

@@ -3,10 +3,12 @@ package org.firstinspires.ftc.teamcode.lib;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 
+import java.util.List;
+
 public abstract class Subsystem {
     public abstract void init(HardwareMap hwMap);
-
     public abstract void update();
-
     public void publishTelemetry(Telemetry telemetry) {}
+
+    public boolean isHealthy() { return true; }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/hardware/CMotor.java

@@ -0,0 +1,62 @@
+package org.firstinspires.ftc.teamcode.lib.hardware;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+public class CMotor {
+    protected DcMotor motor;
+
+    // Cached States
+    protected double lastPower = 0.0;
+    protected DcMotor.RunMode lastMode = null;
+    protected DcMotor.Direction lastDirection = null;
+    protected DcMotor.ZeroPowerBehavior lastZeroBehavior = null;
+    protected int lastTargetPos = 0;
+
+    protected final double CHANGE_TRES = 0.005;
+
+    public CMotor(DcMotor motor) {
+        this.motor = motor;
+    }
+
+    public void setPower(double power) {
+        // Optimization: Only write if power changed significantly
+        // Always write if power is exactly 0 or 1 (boundary cases)
+        if (Math.abs(power - lastPower) > CHANGE_TRES || (power == 0 && lastPower != 0) || (power == 1 && lastPower != 1)) {
+            motor.setPower(power);
+            lastPower = power;
+        }
+    }
+
+    public void setMode(DcMotor.RunMode mode) {
+        if (mode != lastMode) {
+            motor.setMode(mode);
+            lastMode = mode;
+        }
+    }
+
+    public void setDirection(DcMotor.Direction direction) {
+        if (direction != lastDirection) {
+            motor.setDirection(direction);
+            lastDirection = direction;
+        }
+    }
+
+    public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior behavior) {
+        if (behavior != lastZeroBehavior) {
+            motor.setZeroPowerBehavior(behavior);
+            lastZeroBehavior = behavior;
+        }
+    }
+
+    public void setTargetPosition(int pos) {
+        if (pos != lastTargetPos) {
+            motor.setTargetPosition(pos);
+            lastTargetPos = pos;
+        }
+    }
+
+    // Passthrough for reads (Bulk caching handles the speed here)
+    public int getCurrentPosition() { return motor.getCurrentPosition(); }
+    public boolean isBusy() { return motor.isBusy(); }
+    public DcMotor getInternalMotor() { return motor; }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/hardware/CMotorEx.java

@@ -0,0 +1,36 @@
+package org.firstinspires.ftc.teamcode.lib.hardware;
+
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+
+public class CMotorEx extends CMotor {
+    private DcMotorEx motorEx;
+
+    private double lastVelocity = 0;
+
+    public CMotorEx(DcMotorEx motor) {
+        super(motor);
+        this.motorEx = motor;
+    }
+
+    public void setVelocity(double angularRate) {
+        // Velocity often fluctuates slightly, so we use a small epsilon
+        if (Math.abs(angularRate - lastVelocity) > 0.1) {
+            motorEx.setVelocity(angularRate);
+            lastVelocity = angularRate;
+        }
+    }
+
+    public void setPIDFCoefficients(DcMotorEx.RunMode mode, PIDFCoefficients coefficients) {
+        // We always set PIDF as it's usually done in init, and caching it
+        // adds more complexity than it saves time.
+        motorEx.setPIDFCoefficients(mode, coefficients);
+    }
+
+    // Extended Read Methods
+    public double getVelocity() { return motorEx.getVelocity(); }
+    public double getCurrentAmps() { return motorEx.getCurrent(org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit.AMPS); }
+
+    @Override
+    public DcMotorEx getInternalMotor() { return motorEx; }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/hardware/CServo.java

@@ -0,0 +1,32 @@
+package org.firstinspires.ftc.teamcode.lib.hardware;
+
+import com.qualcomm.robotcore.hardware.Servo;
+
+public class CServo {
+    protected final Servo servo;
+    protected double lastPos = -1.0;
+    protected Servo.Direction lastDir = null;
+
+    protected final double EPSILON = 0.001;
+
+    public CServo(Servo servo) {
+        this.servo = servo;
+    }
+
+    public void setPosition(double pos) {
+        if (Math.abs(pos - lastPos) > EPSILON) {
+            servo.setPosition(pos);
+            lastPos = pos;
+        }
+    }
+
+    public void setDirection(Servo.Direction direction) {
+        if (direction != lastDir) {
+            servo.setDirection(direction);
+            lastDir = direction;
+        }
+    }
+
+    public double getPosition() { return servo.getPosition(); }
+    public Servo getInternalServo() { return servo; }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/hardware/CServoEx.java

@@ -0,0 +1,26 @@
+package org.firstinspires.ftc.teamcode.lib.hardware;
+
+import com.qualcomm.robotcore.hardware.PwmControl;
+import com.qualcomm.robotcore.hardware.ServoImplEx;
+
+public class CServoEx extends CServo {
+    private final ServoImplEx servoEx;
+    private boolean lastPwmEnabled = true;
+
+    public CServoEx(ServoImplEx servo) {
+        super(servo);
+        this.servoEx = servo;
+    }
+
+    public void setPwmEnable(boolean enable) {
+        if (enable != lastPwmEnabled) {
+            if (enable) servoEx.setPwmEnable();
+            else servoEx.setPwmDisable();
+            lastPwmEnabled = enable;
+        }
+    }
+
+    public void setPwmRange(PwmControl.PwmRange range) {
+        servoEx.setPwmRange(range);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/motion/smooth.java

@@ -0,0 +1,78 @@
+package org.firstinspires.ftc.teamcode.lib.motion;
+
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+public class smooth {
+    private double maxV, maxA;
+    private double dAccel, dCruise, dDecel;
+    private double tAccel, tCruise, tDecel, tTotal;
+    private double startPos, targetPos;
+    private final ElapsedTime timer = new ElapsedTime();
+
+    public smooth(double maxV, double maxA) {
+        this.maxV = maxV;
+        this.maxA = maxA;
+    }
+
+    public void generate(double currentPos, double targetPos) {
+        this.startPos = currentPos;
+        this.targetPos = targetPos;
+        double distance = Math.abs(targetPos - currentPos);
+
+        // Calculate time to reach max velocity
+        tAccel = maxV / maxA;
+        dAccel = 0.5 * maxA * Math.pow(tAccel, 2);
+
+        // If distance is too short to reach maxV, it becomes a triangular profile
+        if (dAccel * 2 > distance) {
+            tAccel = Math.sqrt(distance / maxA);
+            dAccel = 0.5 * maxA * Math.pow(tAccel, 2);
+            tCruise = 0;
+            dCruise = 0;
+        } else {
+            dCruise = distance - (2 * dAccel);
+            tCruise = dCruise / maxV;
+        }
+
+        tDecel = tAccel;
+        dDecel = dAccel;
+        tTotal = tAccel + tCruise + tDecel;
+        timer.reset();
+    }
+
+    public State calculate() {
+        double t = timer.seconds();
+        double dir = Math.signum(targetPos - startPos);
+        double p, v;
+
+        if (t < tAccel) {
+            // Acceleration Phase
+            v = maxA * t;
+            p = 0.5 * maxA * Math.pow(t, 2);
+        } else if (t < tAccel + tCruise) {
+            // Cruise Phase
+            v = maxV;
+            p = dAccel + maxV * (t - tAccel);
+        } else if (t < tTotal) {
+            // Deceleration Phase
+            double tDecelLocal = t - tAccel - tCruise;
+            v = maxV - (maxA * tDecelLocal);
+            p = dAccel + dCruise + (maxV * tDecelLocal) - (0.5 * maxA * Math.pow(tDecelLocal, 2));
+        } else {
+            // Finished
+            v = 0;
+            p = Math.abs(targetPos - startPos);
+        }
+
+        return new State(startPos + (p * dir), v * dir);
+    }
+
+    public boolean isFinished() {
+        return timer.seconds() >= tTotal;
+    }
+
+    public static class State {
+        public double pos, vel;
+        public State(double p, double v) { pos = p; vel = v; }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/pid/BaseController.java

@@ -0,0 +1,47 @@
+package org.firstinspires.ftc.teamcode.lib.pid;
+
+import com.qualcomm.robotcore.util.ElapsedTime;
+import org.firstinspires.ftc.teamcode.teleOp.Constants;
+
+public abstract class BaseController {
+    // Voltage is updated globally by BaseOpMode
+    public static double currentSystemVoltage = 12.0;
+
+    protected double target = 0;
+    protected double tolerance = 0;
+    protected double settleTimeMs = 0;
+    protected final ElapsedTime settleTimer = new ElapsedTime();
+    protected final ElapsedTime dtTimer = new ElapsedTime();
+    
+    protected boolean hasRun = false;
+
+    public void setTarget(double target) {
+        this.target = target;
+    }
+
+    public void setTolerance(double tolerance, double settleTimeMs) {
+        this.tolerance = tolerance;
+        this.settleTimeMs = settleTimeMs;
+    }
+
+    /**
+     * Returns true if the error is within tolerance for the required settle time.
+     */
+    public boolean atSetpoint(double current) {
+        if (Math.abs(target - current) <= tolerance) {
+            if (settleTimer.milliseconds() >= settleTimeMs) {
+                return true;
+            }
+        } else {
+            settleTimer.reset();
+        }
+        return false;
+    }
+
+    protected double getVoltageMultiplier() {
+        return Constants.PIDController.VOLTAGE_REFERENCE / currentSystemVoltage;
+    }
+
+    public abstract double calculate(double current);
+    public abstract void reset();
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/pid/PController.java

@@ -0,0 +1,20 @@
+package org.firstinspires.ftc.teamcode.lib.pid;
+
+public class PController extends BaseController {
+    public double kP;
+
+    public PController(double kP) {
+        this.kP = kP;
+    }
+
+    @Override
+    public double calculate(double current) {
+        double error = target - current;
+        return (error * kP) * getVoltageMultiplier();
+    }
+
+    @Override
+    public void reset() {
+        hasRun = false;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/pid/PDController.java

@@ -0,0 +1,38 @@
+package org.firstinspires.ftc.teamcode.lib.pid;
+
+public class PDController extends PController {
+    public double kD;
+    protected double lastError = 0;
+    protected double lastDerivative = 0;
+    
+    public double lowPassCoeff = 0.0; 
+
+    public PDController(double kP, double kD) {
+        super(kP);
+        this.kD = kD;
+    }
+
+    @Override
+    public double calculate(double current) {
+        double error = target - current;
+        double dt = dtTimer.seconds();
+        dtTimer.reset();
+
+        if (!hasRun || dt == 0) {
+            hasRun = true;
+            lastError = error;
+            return super.calculate(current);
+        }
+
+        double rawDerivative = (error - lastError) / dt;
+        
+        // Low-Pass Filter: smooths out noisy encoder data
+        double filteredDerivative = (lowPassCoeff * lastDerivative) + ((1 - lowPassCoeff) * rawDerivative);
+        
+        lastError = error;
+        lastDerivative = filteredDerivative;
+
+        double out = (error * kP) + (filteredDerivative * kD);
+        return out * getVoltageMultiplier();
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/pid/PIDController.java

@@ -0,0 +1,36 @@
+package org.firstinspires.ftc.teamcode.lib.pid;
+
+import com.qualcomm.robotcore.util.Range;
+
+public class PIDController extends PDController {
+    public double kI;
+    protected double integralSum = 0;
+    public double maxIntegral = 0.25; // Clamps the "I" term to max 25% power
+
+    public PIDController(double kP, double kI, double kD) {
+        super(kP, kD);
+        this.kI = kI;
+    }
+
+    @Override
+    public double calculate(double current) {
+        double error = target - current;
+        double dt = dtTimer.seconds();
+        // dtTimer.reset() is handled in super.calculate
+
+        if (hasRun && dt > 0) {
+            integralSum += error * dt;
+            // Anti-Windup: Clamp the sum so it doesn't grow to infinity
+            integralSum = Range.clip(integralSum, -maxIntegral/kI, maxIntegral/kI);
+        }
+
+        double pidOut = super.calculate(current) + (integralSum * kI);
+        return pidOut; // Multiplier already applied in PD super.calculate
+    }
+
+    @Override
+    public void reset() {
+        super.reset();
+        integralSum = 0;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/pid/PIDFController.java

@@ -0,0 +1,41 @@
+package org.firstinspires.ftc.teamcode.lib.pid;
+
+public class PIDFController extends PIDController {
+    public double kG = 0;   // Gravity (Linear)
+    public double kCos = 0; // Gravity (Angular/Arm)
+    public double kS = 0;   // Static Friction (Stiction)
+    public double kV = 0;   // Velocity (Target-based)
+
+    public PIDFController(double kP, double kI, double kD) {
+        super(kP, kI, kD);
+    }
+
+    /**
+     * @param current The current position (ticks/degrees)
+     * @param armAngle Only required if using kCos (Radians)
+     */
+    public double calculate(double current, double armAngle) {
+        double pidOut = super.calculate(current);
+        
+        // --- FEEDFORWARD MODELS ---
+        
+        // 1. Static Friction: Jumps the motor past mechanical resistance
+        double ffStatic = Math.signum(target - current) * kS;
+        
+        // 2. Linear Gravity: Holds a vertical lift up
+        double ffGravity = kG;
+        
+        // 3. Angular Gravity: kCos * Cosine of arm angle
+        double ffArm = Math.cos(armAngle) * kCos;
+        
+        // 4. Velocity FF: Based on the target (useful for flywheels)
+        double ffVelocity = target * kV;
+
+        return pidOut + ffStatic + ffGravity + ffArm + ffVelocity;
+    }
+
+    @Override
+    public double calculate(double current) {
+        return calculate(current, 0);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/util/EnhancedGamepad.java

@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.lib;
+package org.firstinspires.ftc.teamcode.lib.util;
 
 import com.qualcomm.robotcore.hardware.Gamepad;
 import org.firstinspires.ftc.teamcode.teleOp.Constants;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/util/LLUtil.java

@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.lib;
+package org.firstinspires.ftc.teamcode.lib.util;
 
 import com.qualcomm.hardware.limelightvision.LLResult;
 import com.qualcomm.hardware.limelightvision.Limelight3A;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/util/health.java

@@ -0,0 +1,22 @@
+package org.firstinspires.ftc.teamcode.lib.util;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class health {
+    private static final List<String> faults = new ArrayList<>();
+
+    public static void reportFault(String deviceName) {
+        if (!faults.contains(deviceName)) {
+            faults.add(deviceName);
+        }
+    }
+
+    public static List<String> getFaults() {
+        return faults;
+    }
+
+    public static void clear() {
+        faults.clear();
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsys/Drivetrain.java

@@ -6,19 +6,22 @@ import org.firstinspires.ftc.robotcore.external.Telemetry;
 
 import org.firstinspires.ftc.teamcode.teleOp.Constants;
 import org.firstinspires.ftc.teamcode.lib.Subsystem;
+import org.firstinspires.ftc.teamcode.lib.hardware.CMotor; // Your caching wrapper
 
 public class Drivetrain extends Subsystem {
 
-    // Subsystem Micro-States
     public enum DriveState {
         NORMAL,
         PRECISION,
-        LOCKED // Safety state to halt motors instantly
+        LOCKED
     }
 
     private DriveState currentState = DriveState.NORMAL;
 
-    private DcMotor frontLeft, frontRight, backLeft, backRight;
+    // Use the Cached Wrapper instead of raw DcMotor
+    private CMotor frontLeft, frontRight, backLeft, backRight;
+
+    private boolean hardwareExists = false;
 
     private double driveY = 0.0;
     private double driveX = 0.0;
@@ -26,10 +29,15 @@ public class Drivetrain extends Subsystem {
 
     @Override
     public void init(HardwareMap hwMap) {
-        frontLeft = hwMap.get(DcMotor.class, Constants.DRIVE.FL_NAME);
+        try {
-        frontRight = hwMap.get(DcMotor.class, Constants.DRIVE.FR_NAME);
+            frontLeft = new CMotor(hwMap.get(DcMotor.class, Constants.DRIVE.FL_NAME));
-        backLeft = hwMap.get(DcMotor.class, Constants.DRIVE.BL_NAME);
+            frontRight = new CMotor(hwMap.get(DcMotor.class, Constants.DRIVE.FR_NAME));
-        backRight = hwMap.get(DcMotor.class, Constants.DRIVE.BR_NAME);
+            backLeft = new CMotor(hwMap.get(DcMotor.class, Constants.DRIVE.BL_NAME));
+            backRight = new CMotor(hwMap.get(DcMotor.class, Constants.DRIVE.BR_NAME));
+            hardwareExists = true;
+        } catch (Exception e) {
+            hardwareExists = false;
+        }
 
         frontRight.setDirection(DcMotor.Direction.REVERSE);
         backRight.setDirection(DcMotor.Direction.REVERSE);
@@ -40,7 +48,12 @@ public class Drivetrain extends Subsystem {
         backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
     }
 
-    // Command Hooks for the OpMode to control this subsystem
+    @Override
+    public boolean isHealthy() {
+        return hardwareExists && frontLeft != null && frontRight != null
+                && backLeft != null && backRight != null;
+    }
+
     public void setTargetState(DriveState newState) {
         this.currentState = newState;
     }
@@ -54,8 +67,7 @@ public class Drivetrain extends Subsystem {
     @Override
     public void update() {
         if (currentState == DriveState.LOCKED) {
-            frontLeft.setPower(0); frontRight.setPower(0);
+            stopMotors();
-            backLeft.setPower(0); backRight.setPower(0);
             return;
         }
 
@@ -68,7 +80,6 @@ public class Drivetrain extends Subsystem {
         double blPower = (driveY - driveX + driveTurn) * speedMultiplier;
         double brPower = (driveY + driveX - driveTurn) * speedMultiplier;
 
-        // Normalization (prevents math outputting numbers > 1.0)
         double max = Math.max(Math.abs(flPower), Math.abs(frPower));
         max = Math.max(max, Math.abs(blPower));
         max = Math.max(max, Math.abs(brPower));
@@ -84,11 +95,18 @@ public class Drivetrain extends Subsystem {
         backRight.setPower(brPower);
     }
 
+    private void stopMotors() {
+        frontLeft.setPower(0);
+        frontRight.setPower(0);
+        backLeft.setPower(0);
+        backRight.setPower(0);
+    }
+
     @Override
     public void publishTelemetry(Telemetry telemetry) {
         if (Constants.GLOBAL.DEBUG_MODE) {
             telemetry.addData("Drive State", currentState.toString());
-            telemetry.addData("Inputs (Y/X/T)", String.format("%.2f / %.2f / %.2f", driveY, driveX, driveTurn));
+            telemetry.addData("Drive Vectors", String.format("Y:%.2f X:%.2f T:%.2f", driveY, driveX, driveTurn));
         }
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/Constants.java

@@ -10,6 +10,8 @@ public class Constants {
         public static double TELEMETRY_DELAY_MS = 250.0;
         public static boolean DEBUG_MODE = true;
         public static int TARGET_FPS = 0; // No target FPS (UNCAPPED)
+
+
     }
 
     public static class Gamepad {
@@ -19,6 +21,9 @@ public class Constants {
     public static class PIDController {
         public static double VOLTAGE_REFERENCE = 12.0; // Voltage PID Controller was tuned at
         public static double VOLTAGE_CHECK_INTERVAL_MS = 500.0; // Every half second
+
+        public static double MAX_VELOCITY = 2000;
+        public static double MAX_ACCELERATION = 1500;
     }
 
     @Configurable

build.dependencies.gradle

@@ -17,6 +17,5 @@ dependencies {
 
     implementation 'com.pedropathing:ftc:2.1.0'
     implementation 'com.pedropathing:telemetry:1.0.0'
-    implementation 'com.bylazar:fullpanels:1.0.12'
 }
 

gradle.properties

@@ -6,7 +6,13 @@ android.useAndroidX=true
 # We no longer need to auto-convert third-party libraries to use AndroidX, which slowed down the build
 android.enableJetifier=false
 
-# Allow Gradle to use up to 1 GB of RAM
+# Allow Gradle to use up to  2GB of RAM
-org.gradle.jvmargs=-Xmx1024M
+org.gradle.jvmargs=-Xmx2048M
 
 android.nonTransitiveRClass=false
+org.gradle.configuration-cache=true
+
+org.gradle.parallel=true
+org.gradle.caching=true
+
+org.gradle.daemon=true