Merge pull request #16 from frc5024/controllers

.github/workflows/build.yml

@@ -4,7 +4,7 @@ jobs:
   gradle:
     strategy:
       matrix:
-        os: [ubuntu-latest, macos-latest, windows-latest]
+        os: [ubuntu-latest,  windows-latest]
     runs-on: ${{ matrix.os }}
     steps:
     - uses: actions/checkout@v2

.github/workflows/bundle.yml

@@ -4,7 +4,7 @@ jobs:
   gradle:
     strategy:
       matrix:
-        os: [ubuntu-latest, macos-latest, windows-latest]
+        os: [ubuntu-latest, windows-latest]
     runs-on: ${{ matrix.os }}
     steps:
     - uses: actions/checkout@v2

build.gradle

@@ -8,10 +8,14 @@
 
 
 buildscript {
-    repositories { jcenter() }
+    repositories { 
+      jcenter() 
+      maven { url "https://plugins.gradle.org/m2/" }
+    }
 
     dependencies {
         classpath 'com.netflix.nebula:gradle-aggregate-javadocs-plugin:2.2.+'
+        classpath "edu.wpi.first:GradleRIO:2020.+"
     }
 }
 
@@ -26,7 +30,7 @@ apply plugin: "java-library"
 project.version = "1.1.0"
 
 allprojects{
-    apply plugin: "java"
+  apply plugin: "java"
 } 
 
 gradleEnterprise {

control_loops/src/main/java/io/github/frc5024/lib5k/control_loops/TBHController.java

@@ -0,0 +1,91 @@
+package io.github.frc5024.lib5k.control_loops;
+
+import io.github.frc5024.lib5k.control_loops.base.SettlingController;
+import io.github.frc5024.lib5k.hardware.ni.roborio.fpga.FPGAClock;
+
+/**
+ * A Take-Back-Half controller designed for controlling flywheels.
+ * 
+ * The controller increases speed until the measurement goes over the target,
+ * then it "takes back half", and continues this process until the velocity is
+ * stable.
+ * 
+ * The gain determines how quickly the speed is changed.
+ * 
+ * This is based off of this paper:
+ * https://www.chiefdelphi.com/t/paper-take-back-half-shooter-wheel-speed-control/121640
+ */
+public class TBHController extends SettlingController {
+
+    // Controller gain
+    private double gain;
+
+    // Controller state
+    private double tbh;
+    private double previousError;
+    private double output;
+
+    // Timekeeping
+    private double lastTime;
+
+    /**
+     * Create a TBHController
+     * 
+     * @param gain Controller gain (should be between 0.0 and 1.0)
+     */
+    public TBHController(double gain) {
+
+        // Set up controller
+        setGain(gain);
+        reset();
+
+        // Track last time
+        lastTime = FPGAClock.getFPGAMilliseconds();
+    }
+
+    /**
+     * Set the controller gain
+     * 
+     * @param gainController gain (should be between 0.0 and 1.0)
+     */
+    public void setGain(double gain) {
+        this.gain = gain;
+    }
+
+    /**
+     * Reset the controller
+     */
+    @Override
+    public void resetInternals() {
+        tbh = 0.0;
+        previousError = 0.0;
+        output = 0.0;
+
+        // Track last time
+        lastTime = FPGAClock.getFPGAMilliseconds();
+    }
+
+    @Override
+    protected double calculate(double error) {
+
+        // Calculate dt
+        double time = FPGAClock.getFPGAMilliseconds();
+        double dt = time - lastTime;
+        lastTime = time;
+
+        // Calculate base output
+        output += gain * error * dt;
+
+        // Calculate TBH
+        if ((error < 0) != (previousError < 0)) {
+            output += tbh;
+            output *= 0.5;
+
+            tbh = output;
+            previousError = error;
+        }
+
+        return output;
+    }
+
+}

control_loops/src/main/java/io/github/frc5024/lib5k/control_loops/base/ControllerBase.java

@@ -0,0 +1,114 @@
+package io.github.frc5024.lib5k.control_loops.base;
+
+/**
+ * Base class for all non-statespace controllers.
+ */
+public abstract class ControllerBase {
+
+    // Controller epsilon
+    private double epsilon = 0.0;
+
+    // Controller goal
+    private double goal;
+    private double measurement;
+
+    /**
+     * Update the system measurement, and get new output
+     * 
+     * @param measurement Current sensor measurement
+     * @return Calculated output
+     */
+    public double update(double measurement) {
+        return update(measurement, getGoal());
+    }
+
+    /**
+     * Update the system measurement and goal, and get new output
+     * 
+     * @param measurement Current sensor measurement
+     * @param goal        System goal
+     * @return Calculated output
+     */
+    public double update(double measurement, double goal) {
+        // Update internal trackers
+        setGoal(goal);
+        this.measurement = measurement;
+
+        // Call calculator
+        return calculate(measurement - goal);
+    }
+
+    /**
+     * Internal controller calculation
+     * 
+     * @param error Error in system
+     * @return Output
+     */
+    protected abstract double calculate(double error);
+
+    /**
+     * Check if the system is at it's goal
+     * 
+     * @return Is at goal?
+     */
+    public abstract boolean isAtGoal();
+
+    /**
+     * Set acceptable error around goal
+     * 
+     * @param eps Acceptable error
+     */
+    public void setEpsilon(double eps) {
+        this.epsilon = eps;
+    }
+
+    /**
+     * Get acceptable error around goal
+     * 
+     * @return Acceptable error
+     */
+    public double getEpsilon() {
+        return epsilon;
+    }
+
+    /**
+     * Set the system goal
+     * 
+     * @param goal Goal
+     */
+    public void setGoal(double goal) {
+        this.goal = goal;
+    }
+
+    /**
+     * Get the system goal
+     * 
+     * @return Goal
+     */
+    public double getGoal() {
+        return goal;
+    }
+
+    /**
+     * Get the last known measurement
+     * 
+     * @return Last measurement
+     */
+    public double getLastMeasurement() {
+        return measurement;
+    }
+
+    /**
+     * Reset the controller
+     */
+    public void reset() {
+        measurement = 0.0;
+        resetInternals();
+    }
+
+    /**
+     * Override this for controller-specific resets
+     */
+    protected abstract void resetInternals();
+
+}

control_loops/src/main/java/io/github/frc5024/lib5k/control_loops/base/SettlingController.java

@@ -0,0 +1,83 @@
+package io.github.frc5024.lib5k.control_loops.base;
+
+import ca.retrylife.ewmath.MathUtils;
+import edu.wpi.first.wpilibj.Timer;
+
+/**
+ * An extension of the {@link ControllerBase} that will wait a set amount of
+ * time before finishing.
+ */
+public abstract class SettlingController extends ControllerBase {
+
+    // Rest time
+    private double restMS = 0.0;
+    private Timer timer;
+
+    public SettlingController() {
+
+        // Set up timer
+        timer = new Timer();
+        timer.reset();
+    }
+
+    @Override
+    public double update(double measurement, double goal) {
+        // Get internal calculation
+        double calculation = super.update(measurement, goal);
+
+        // Handle timer
+        if (rawIsAtGoal()) {
+            timer.start();
+        } else {
+            timer.stop();
+            timer.reset();
+        }
+
+        return calculation;
+    }
+
+    /**
+     * Set the amount of time the system must be stable for in order for "is at
+     * goal" to be true
+     * 
+     * @param ms Amount of time to wait for system to settle
+     */
+    public void setMinRestTime(double ms) {
+        this.restMS = ms;
+    }
+
+    /**
+     * Get the amount of time the system must be stable for in order for "is at
+     * goal" to be true
+     * 
+     * @return Amount of time to wait for system to settle
+     */
+    public double getRestMS() {
+        return restMS;
+    }
+
+    /**
+     * Check if the system is at it's goal without waiting for the timer
+     * 
+     * @return Raw getter for "is at goal"
+     */
+    public boolean rawIsAtGoal() {
+        return MathUtils.epsilonEquals(getGoal(), getLastMeasurement(), getEpsilon());
+    }
+
+    @Override
+    public boolean isAtGoal() {
+        // Check if the timer has been reached
+        return timer.hasElapsed(this.restMS / 1000);
+    }
+
+    @Override
+    public void reset() {
+        super.reset();
+
+        // Add a timer reset
+        timer.stop();
+        timer.reset();
+    }
+
+}

control_loops/src/test/java/io/github/frc5024/lib5k/control_loops/TestTBH.java

@@ -0,0 +1,20 @@
+package io.github.frc5024.lib5k.control_loops;
+
+import org.junit.Test;
+
+public class TestTBH {
+
+    /**
+     * This test simply checks if the TBH controller actually puts out some output
+     */
+    @Test
+    public void ensureTBHProvidesOutput() {
+
+        // Create controller
+        TBHController controller = new TBHController(0.3);
+
+        // Calculate & check
+        assert controller.calculate(100) > 0.0;
+    }
+
+}

examples/build.gradle

@@ -2,7 +2,6 @@
 
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2020.3.2" 
 }
 
 apply from: "../gradle_utils/libversions.gradle"