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

This commit is contained in:
2026-03-23 18:57:24 -05:00
parent 545c0cbda0
commit 57bf6e3ad3
21 changed files with 514 additions and 80 deletions

View File

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

View File

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

View File

@@ -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);
}
}
}

View File

@@ -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; }
}

View File

@@ -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; }
}

View File

@@ -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; }
}

View File

@@ -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; }
}

View File

@@ -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);
}
}

View File

@@ -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; }
}
}

View File

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

View File

@@ -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;
}
}

View File

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

View File

@@ -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;
}
}

View File

@@ -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);
}
}

View File

@@ -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;

View File

@@ -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;

View File

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

View File

@@ -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);
frontRight = hwMap.get(DcMotor.class, Constants.DRIVE.FR_NAME);
backLeft = hwMap.get(DcMotor.class, Constants.DRIVE.BL_NAME);
backRight = hwMap.get(DcMotor.class, Constants.DRIVE.BR_NAME);
try {
frontLeft = new CMotor(hwMap.get(DcMotor.class, Constants.DRIVE.FL_NAME));
frontRight = new CMotor(hwMap.get(DcMotor.class, Constants.DRIVE.FR_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);
backLeft.setPower(0); backRight.setPower(0);
stopMotors();
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));
}
}
}

View File

@@ -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

View File

@@ -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'
}

View File

@@ -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
org.gradle.jvmargs=-Xmx1024M
# Allow Gradle to use up to 2GB of RAM
org.gradle.jvmargs=-Xmx2048M
android.nonTransitiveRClass=false
org.gradle.configuration-cache=true
org.gradle.parallel=true
org.gradle.caching=true
org.gradle.daemon=true