Initial Code Commit

Author: jjophoven

TeamCode/src/main/java/org/firstinspires/ftc/blackice/DEVELOPER_NOTES.md

@@ -0,0 +1,79 @@
+package org.firstinspires.ftc.blackice;
+
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+
+import org.firstinspires.ftc.blackice.core.control.VelocityController;
+import org.firstinspires.ftc.blackice.core.hardware.localization.localizers.GoBildaPinpointDriver;
+import org.firstinspires.ftc.blackice.core.hardware.localization.localizers.LocalizerConfig;
+import org.firstinspires.ftc.blackice.core.hardware.localization.localizers.PinpointConfig;
+import org.firstinspires.ftc.blackice.core.hardware.drivetrain.DrivetrainConfig;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.blackice.core.control.QuadraticDampedPIDController;
+import org.firstinspires.ftc.blackice.core.follower.FollowerConfig;
+import org.firstinspires.ftc.blackice.core.control.PIDFController;
+import org.firstinspires.ftc.blackice.core.paths.PathBehavior;
+import org.firstinspires.ftc.blackice.core.hardware.drivetrain.MecanumConfig;
+
+/**
+ * Set default configuration for all OpModes using the Follower.
+ */
+public final class FollowerConstants {
+    public static LocalizerConfig localizerConfig = new PinpointConfig()
+        .distanceUnit(DistanceUnit.INCH)
+        .podDirection(
+            GoBildaPinpointDriver.EncoderDirection.REVERSED,
+            GoBildaPinpointDriver.EncoderDirection.REVERSED)
+        .podOffset(-36, 0);
+    
+    public static DrivetrainConfig drivetrainConfig = new MecanumConfig()
+        .frontLeft("frontLeft", DcMotorSimple.Direction.REVERSE)
+        .backLeft("backLeft", DcMotorSimple.Direction.REVERSE)
+        .frontRight("frontRight", DcMotorSimple.Direction.REVERSE)
+        .backRight("backRight", DcMotorSimple.Direction.FORWARD)
+        .maxForwardSpeed(60)
+        .maxStrafeSpeed(45);
+    
+    public static PathBehavior defaultPathBehavior = new PathBehavior()
+        .continueMomentumAtEnd()
+        .setStuckTimeoutSeconds(3.0)
+        .setPauseTimeoutSeconds(5.0)
+        .setTimeoutSeconds(8.0);
+    
+    public static FollowerConfig defaultFollowerConfig = new FollowerConfig()
+        .localizerConfig(localizerConfig)
+        .drivetrainConfig(drivetrainConfig)
+        .defaultPathBehavior(defaultPathBehavior)
+        .headingPID(new PIDFController(2, 0, 0.1, 0))
+        .positionalPID(new QuadraticDampedPIDController(0.5, 0.07, 0.001))
+        .driveVelocityController(new VelocityController(0.01, 0.0159, 0.04, 40)) // 34
+        // only needed for following velocity profiles.
+        .maxReversalBrakingPower(0.3)
+        .centripetalFeedforward(0.005) // only needed for following curves
+        .stopIfVoltageBelow(7);
+}
+
+// ! https://visualizer.pedropathing.com
+
+//public static VelocityToStoppingDistanceVectorModel BRAKING_DISPLACEMENT =
+//    new VelocityToStoppingDistanceVectorModel(
+//        //Drift, braking force
+//        new QuadraticLinearBrakingModel(0.00112, 0.07316),
+//        new QuadraticLinearBrakingModel(0.00165, 0.05054)
+//    );
+
+//
+//Heading 0 is the front side of the robot facing away from the starting wall
+//positive X-axis is forward for the robot with 0 heading (facing the center of the field)
+//positive Y-axis is strafing the robot to the left from 0 heading
+//
+//This is the axis with the robot's HEADING AT 0 DEGREES: The robot is touching the wall
+//    ```
+//    ^ +Y Axis (Robot Left Strafing)
+//│
+//    │ ┌──────────┐
+//    │ │          │
+//    │ │          │--> FRONT OF ROBOT (FORWARD)
+//│ │          │
+//    │ └──────────┘
+//    └──────────────> +X Axis (Robot Forward)
+//```

TeamCode/src/main/java/org/firstinspires/ftc/blackice/FollowerConstants.java

@@ -0,0 +1,48 @@
+package org.firstinspires.ftc.blackice.core.control;
+
+/**
+ * PIDController implements a basic Proportional-Integral-Derivative control algorithm.
+ * It calculates an output value based on the error between a target and current value,
+ * with optional integral and derivative terms.
+ */
+public class PIDController {
+    double kP, kI, kD;
+    double previousError, integralSum;
+
+    public PIDController(double kP, double kI, double kD) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+    }
+
+    public double run(double target, double current, double deltaTime) {
+        double error = target - current;
+        double errorRate = kD != 0 ? (error - previousError) / deltaTime : 0;
+        return runFromError(error, errorRate);
+    }
+
+    public double runFromError(double error, double derivative) {
+        double output = kP * error;
+
+        if (kI != 0) {
+            integralSum += error;
+            output += kI * integralSum;
+        }
+        if (kD != 0) output += kD * derivative;
+
+        previousError = error;
+        return output;
+    }
+
+    public void reset() {
+        previousError = 0;
+        integralSum = 0;
+    }
+    
+    public PIDController setCoefficients(double kP, double kI, double kD) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+        return this;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/blackice/README.md

@@ -0,0 +1,36 @@
+package org.firstinspires.ftc.blackice.core.control;
+
+
+/**
+ * Represents a PIDF (Proportional-Integral-Derivative-Feedforward) controller.
+ */
+public class PIDFController extends PIDController {
+    @FunctionalInterface
+    public interface Feedforward {
+        double compute(double target);
+    }
+    
+    public Feedforward feedforward;
+
+    public PIDFController(double kP, double kI, double kD, double feedforwardGain) {
+        this(kP, kI, kD, target -> target * feedforwardGain);
+    }
+
+    public PIDFController(double kP, double kI, double kD, Feedforward feedforward) {
+        super(kP, kI, kD);
+        this.feedforward = feedforward;
+    }
+    
+    @Override
+    public double run(double target, double current, double deltaTime) {
+        return super.run(target, current, deltaTime) + feedforward.compute(target);
+    }
+    
+    public PIDFController setCoefficients(double kP, double kI, double kD, Feedforward feedforward) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+        this.feedforward = feedforward;
+        return this;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/blackice/core/control/PIDController.java

@@ -0,0 +1,39 @@
+package org.firstinspires.ftc.blackice.core.control;
+
+/**
+ * A PD controller with quadratic damping **quadratic-damped PIDs** to model the non-linear effects of friction and momentum.but more empirical. Also has basically a
+ * Derivative of
+ * Derivative term or velocity^2
+ * term to help stop more smoothly because EMF braking is not perfectly quadratic or linear.
+ * Also is predictive because D term compensates for predictedPosition = position + velocity * deltaTime.
+ */
+public class QuadraticDampedPIDController extends PIDController {
+    private double kBraking;
+    private double kFriction;
+    
+    public QuadraticDampedPIDController(double kP, double kLinearBraking,
+                                        double kQuadraticFriction) {
+        super(kP, 0, 0);
+        this.kBraking = kLinearBraking;
+        this.kFriction = kQuadraticFriction;
+    }
+    
+    @Override
+    public double runFromError(double error, double derivative) {
+        double velocity = -derivative;
+        double predictedBrakingDisplacement = velocity * Math.abs(velocity) * kFriction + velocity *
+            kBraking;
+        return super.runFromError(
+            error - predictedBrakingDisplacement,
+            derivative
+        );
+    }
+    
+    public QuadraticDampedPIDController setCoefficients(double kP, double kLinearBraking,
+                                          double kQuadraticFriction) {
+        this.kP = kP;
+        this.kBraking = kLinearBraking;
+        this.kFriction = kQuadraticFriction;
+        return this;
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/blackice/core/control/PIDFController.java

@@ -0,0 +1,56 @@
+package org.firstinspires.ftc.blackice.core.control;
+
+
+import org.firstinspires.ftc.blackice.util.Validator;
+
+/**
+ * A PIDF (Proportional-Integral-Derivative-Feedforward) controller that controls
+ * velocity.
+ * <p>
+ * Does not use Integral or Derivative terms as they are usually unnecessary
+ * for this controller and can often add instability unless tuned appropriately.
+ * <p>
+ * Uses feedforward based on {@code kA} and {@code kS} (velocity and static gain).
+ * Uses momentum damping based off the robot's natural deceleration instead of {@code
+ * kA} (acceleration gain).
+ */
+public class VelocityController extends PIDFController {
+    public double naturalDeceleration;
+
+    /**
+     * Creates a simple velocity controller of a proportional and feedforward based
+     * on kV and kS (velocity and static gain).
+     * <p>
+     * Does not use Integral or Derivative terms as they are usually unnecessary
+     * for this controller and can often add instability unless tuned appropriately.
+     * Uses momentum damping based off the robot's natural deceleration instead of {@code
+     * kA} (acceleration gain).
+     *
+     * @param kP the proportional gain, how much power the robot reacts to error.
+     *          Usually around 0.01.
+     * @param kV the velocity gain, how much power the robot requires per
+     *                  velocity. Should be around 1/maxVelocity or 1/65 ~= 0.015
+     * @param kS the static gain, the minimum power required to start moving.
+     *                Should be determined experimentally. Should be around 0.05.
+     * @param naturalDeceleration the natural deceleration of your robot (in/s^2).
+     *                            Should be positive. Used for momentum damping.
+     */
+    public VelocityController(double kP, double kV, double kS,
+                              double naturalDeceleration) {
+        super(kP, 0, 0, target -> target * kV + kS);
+        this.naturalDeceleration = Validator.ensurePositiveSign(naturalDeceleration);
+    }
+
+    public double run(double targetVelocity, double currentVelocity,
+                      double feedforwardVelocity) {
+        double error = targetVelocity - currentVelocity;
+        return runFromError(error, 0) + feedforward.compute(feedforwardVelocity);
+    }
+    
+    public VelocityController setCoefficients(double kP, double kV, double kS, double naturalDeceleration) {
+        this.kP = kP;
+        this.feedforward = target -> target * kV + kS;
+        this.naturalDeceleration = naturalDeceleration;
+        return this;
+    }
+}