Essentially Photon, which caches power and pos for Servos and Motors, Custom PID, PD, P, and PIDF Objects
This commit is contained in:
@@ -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());
|
||||
}
|
||||
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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; }
|
||||
}
|
||||
@@ -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; }
|
||||
}
|
||||
@@ -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; }
|
||||
}
|
||||
@@ -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; }
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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; }
|
||||
}
|
||||
}
|
||||
@@ -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();
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
|
||||
@@ -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'
|
||||
}
|
||||
|
||||
|
||||
@@ -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
|
||||
android.nonTransitiveRClass=false
|
||||
org.gradle.configuration-cache=true
|
||||
|
||||
org.gradle.parallel=true
|
||||
org.gradle.caching=true
|
||||
|
||||
org.gradle.daemon=true
|
||||
Reference in New Issue
Block a user