Merge pull request #94 from flamingchickens1540/drive-framework-for-m…

…erge

Basic Drive Framework

README.md

@@ -18,11 +18,17 @@ A common library of useful classes and systems intended to be used for all Team
 
 Advanced closed-loop drive code for a tank drive.
 
+#### Drive Pipeline System
+
+`org.team1540.rooster.drive.pipeline`
+
+A flexible system for controlling a robot drive. More docs [here](docs/Drive%20Pipelines.md).
+
 #### Motion Profiling
 
 `org.team1540.rooster.motionprofiling`
 
-A system for executing Pathfinder motion profiles on a tank drive.
+A system for executing motion profiles on a tank drive.
 
 #### Power Management
 
@@ -68,13 +74,12 @@ Classes (mostly WPILib `Commands`) to make life easier.
 
 ### Installation
 
-#### Using Gradle
 Add the library by adding these lines in your `build.gradle` file:
 
 ```Gradle
 repositories {
 	// other repositories
-     mavenCentral() // needed for JetBrains Annotations
+     mavenCentral()
      maven { url 'https://jitpack.io' }
 }
 
@@ -92,19 +97,6 @@ If needed, you can build off of specific commits or branches. See the [JitPack p
 
 _Note: If you need to use changes that have just been pushed to master, you may need to force Gradle to check for a new version instead of using an already-cached older version.  Open a terminal in your project and run `./gradlew build --refresh-dependencies`._
 
-#### Manually
-
-Download the latest version from the [releases page](https://github.com/flamingchickens1540/ROOSTER/releases) and attach it to your project. 
-
-You'll also need ROOSTER's dependencies:
-
-- WPILibJ version 2018.3.2 or higher
-- CTRE Phoenix version 5.3.1.0 or higher
-- [Kauai's NavX library](https://www.pdocs.kauailabs.com/navx-mxp/software/roborio-libraries/java/) 3.0.346 or higher
-- [Jaci's Pathfinder](https://github.com/JacisNonsense/Pathfinder) version 1.8 or higher
-- [MatchData](https://github.com/Open-RIO/2018-MatchData) version 2018.01.07 or higher
-- [Jetbrains Annotations](https://mvnrepository.com/artifact/org.jetbrains/annotations/15.0) version 15.0 or higher
-
 ## Developing ROOSTER
 
 ### Project Structure

build.gradle

@@ -1,16 +1,36 @@
+buildscript {
+    ext.kotlin_version = '1.3.0'
+
+    repositories {
+        mavenCentral()
+    }
+
+    dependencies {
+        classpath "org.jetbrains.kotlin:kotlin-gradle-plugin:$kotlin_version"
+    }
+}
+
 task wrapper(type: Wrapper) {
     gradleVersion = '4.4'
     distributionUrl = "https://services.gradle.org/distributions/gradle-4.4-all.zip"
 }
 
 subprojects {
     apply plugin: "java"
+    apply plugin: "kotlin"
+
+    compileKotlin {
+        kotlinOptions {
+            jvmTarget = "1.8"
+        }
+    }
 
     repositories {
         mavenCentral()
     }
 
     dependencies {
         compile 'org.jetbrains:annotations:16.0.3'
+        compile "org.jetbrains.kotlin:kotlin-stdlib-jdk8:$kotlin_version"
     }
 }

docs/Drive Pipelines.md

@@ -0,0 +1,109 @@
+# Drive Pipelines
+
+Drive pipelines are an extensible method to create multiple different ways of driving a robot. Since pipelines are just lambdas, they are highly customizable.
+
+Some "stock" classes (inputs, processors, and outputs) are available in the `org.team1540.rooster.drive.pipeline` package.
+
+## What's a Drive Pipeline?
+
+A pipeline can be used for almost any concievable method of drivetrain control, including standard open-loop teleop drive, closed-loop motion profiling, and anywhere in between.
+
+Pipelines consist of three different kinds of stages: *inputs*, *processors*, and *outputs*. Since inputs, processors, and outputs are just `Suppliers`, `Functions`, and `Consumers` respectively, they can be extended easily and flexibly.
+
+- An input produces target values for processing; for example, an input could get current values from the driver's joysticks, or the currently executing point in a motion profile.
+
+- A processor, well, processes values: for example, a closed-loop processor might take a desired position, velocity, and/or acceleration and convert them into setpoints, feed-forwards, etc. to send to motors. Note that processors can receive data from things that are not the currently configured input; for example, a gyro.
+
+- An output turns values from a processor into commands for motors or other things. An output for Talon SRX motors might just pass a setpoint to the Talons' native closed-loop functionality, while an output for PWM motors might perform additional PID logic.
+
+## Examples
+
+### Hello World
+
+Control two talons in open-loop tank drive:
+
+```java
+Executable pipeline = new SimpleJoystickInput(new Joystick(0), 1, 5, false, false)
+    .then(new CTREOutput(leftTalon, rightTalon));
+```
+
+Breakdown:
+
+- ` SimpleJoystickInput(new Joystick(0), 1, 5, false, false)`: Takes values from a joystick on port 0, with axis 1 as the left and axis 5 on the right, inverting neither
+- `.then(new TalonSRXOutput(leftTalon, rightTalon))`: Sends the output of the previous `SimpleJoystickInput` to your `leftTalon` and `rightTalon`. Since the output of `SimpleJoystickInput` only sets the feed-forward (i.e. raw throttle) term, it'll automatically use `PercentOutput` output mode.
+
+### Execute a Motion Profile
+
+```java
+Executable pipeline = new ProfileInput(leftProfile, rightProfile)
+    .then(new OpenLoopFeedForward(kV, vIntercept, kA))
+    .then(new CTREOutput(leftTalon, rightTalon));
+```
+
+Breakdown:
+
+- `ProfileInput` takes values from two provided `MotionProfile` instances and returns the setpoint for the current time. The timer starts when the pipeline is first executed.
+- `OpenLoopFeedForward` takes the velocity and acceleration setpoints from the `ProfileInput` and calculates a suitable feed-forward for them using coefficients you provide, Oblarg-style. It then passes those velocities down.
+- `TalonSRXOutput`, since it's receiving position setpoints from the `ProfileInput`, tells the Talon closed-loop to PID to those setpoints while providing the feed-forward from the `OpenLoopFeedForward` as an additional bump term.
+
+### Use in a Command
+
+A properly composed pipeline (i.e. with an input on one end and an output on the other) implements `Executable`, so it can be used as the argument to a `SimpleCommand` or `SimpleLoopCommand`:
+
+```java
+// drivePipeline contains your pipeline
+Command command = new SimpleLoopCommand("Drive", drivePipeline, Robot.driveTrain);
+```
+
+### Custom Logic
+
+Most "stock" pipeline elements pass around `TankDriveData` instances to encapsulate position, velocity, etc. It's possible to define your own data classes but there's not too much reason to.
+
+#### Custom Input
+
+An input that returns the same `TankDriveData` every time:
+
+```java
+TankDriveData tankDriveData = new TankDriveData(
+    new DriveData(0), 
+    new DriveData(0), 
+    OptionalDouble.empty(), 
+    OptionalDouble.empty());
+
+Executable pipeline = ((Input) () -> tankDriveData)
+    .then(new CTREOutput(leftTalon, rightTalon))
+```
+
+#### Custom Processor
+
+A processor that multiplies the received position by two:
+
+```java
+Executable pipeline = new SimpleJoystickInput(new Joystick(0), 1, 5, false, false)
+    .then(data -> return new TankDriveData(
+        new DriveData(
+            d.left.position.isPresent() ? OptionalDouble.of(d.left.position.getAsDouble() * 2) : d.left.position,
+            d.left.velocity,
+            d.left.acceleration,
+            d.left.additionalFeedForward
+        ),
+        new DriveData(
+            d.right.position.isPresent() ? OptionalDouble.of(d.right.position.getAsDouble() * 2) : d.right.position,
+            d.right.velocity,
+            d.right.acceleration,
+            d.right.additionalFeedForward
+        ),
+        d.heading, d.turningRate);
+    ))
+    .then(new CTREOutput(leftTalon, rightTalon))
+```
+
+#### Custom Output
+
+Here's an output that just prints the data it receives instead of sending it to a motor:
+
+```java
+Executable pipeline = new SimpleJoystickInput(new Joystick(0), 1, 5, false, false)
+    .then(data -> System.out.println(data))
+```
+

lib/build.gradle

@@ -30,19 +30,21 @@ javadoc {
     options.addStringOption("doctitle", "ROOSTER API")
     options.addStringOption("windowtitle", "ROOSTER API")
     options.addBooleanOption("-allow-script-in-comments", true)
+    options.addBooleanOption('Xdoclint:all,-missing', true)
 }
 
 
 dependencies {
-    compile "edu.wpi.first.wpilibj:wpilibj-java:2018.3.2"
-    compile "edu.wpi.first.ntcore:ntcore-java:4.0.0"
-    compile "edu.wpi.first.wpiutil:wpiutil-java:3.1.0"
+    compile "edu.wpi.first.wpilibj:wpilibj-java:2018.4.1"
+    compile "edu.wpi.first.ntcore:ntcore-java:4.1.0"
+    compile "edu.wpi.first.wpiutil:wpiutil-java:3.2.0"
     compile "org.opencv:opencv-java:3.2.0"
-    compile "edu.wpi.first.cscore:cscore-java:1.2.0"
+    compile "edu.wpi.first.cscore:cscore-java:1.3.0"
     compile "openrio.mirror.third.ctre:CTRE-phoenix-java:5.3.1.0"
     compile "openrio.mirror.third.kauailabs:navx-java:3.0.346"
     compile "jaci.pathfinder:Pathfinder-Java:1.8"
     compile "openrio.powerup:MatchData:2018.01.07"
     compile "com.google.guava:guava:27.0-jre"
-    compile "org.apache.commons:commons-math3:3.6.1"
+    // https://mvnrepository.com/artifact/org.apache.commons/commons-math3
+    compile 'org.apache.commons:commons-math3:3.6.1'
 }

lib/src/main/java/org/team1540/rooster/Utilities.java

@@ -101,6 +101,18 @@ public static double constrain(double input, double lowerCap, double upperCap) {
     }
   }
 
+  /**
+   * Raises the input to the provided power while preserving the sign. Useful for joystick scaling.
+   *
+   * @param input The input to be raised.
+   * @param pow The power.
+   * @return The input raised to the provided power, with the sign of the input.
+   */
+  @Contract(pure = true)
+  public static double scale(double input, double pow) {
+    return Math.copySign(Math.pow(Math.abs(input), pow), input);
+  }
+
   // should never be instantiated
   private Utilities() {
   }

lib/src/main/java/org/team1540/rooster/drive/JoystickScaling.java

@@ -1,7 +1,13 @@
 package org.team1540.rooster.drive;
 
+import org.team1540.rooster.drive.pipeline.Processor;
+
 @FunctionalInterface
-public interface JoystickScaling {
+public interface JoystickScaling extends Processor<Double, Double> {
 
   public double scale(double input);
+
+  public default Double apply(Double input) {
+    return scale(input);
+  }
 }