diff --git a/.idea/compiler.xml b/.idea/compiler.xml index aac03b89..f84dc7d8 100644 --- a/.idea/compiler.xml +++ b/.idea/compiler.xml @@ -4,6 +4,10 @@ + + + + diff --git a/.idea/encodings.xml b/.idea/encodings.xml new file mode 100644 index 00000000..15a15b21 --- /dev/null +++ b/.idea/encodings.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/.idea/kotlinc.xml b/.idea/kotlinc.xml new file mode 100644 index 00000000..8b7f4afd --- /dev/null +++ b/.idea/kotlinc.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/.idea/runConfigurations/Deploy_AdvancedJoystickInputPipelineTestRobot.xml b/.idea/runConfigurations/Deploy_AdvancedJoystickInputPipelineTestRobot.xml new file mode 100644 index 00000000..b148e52b --- /dev/null +++ b/.idea/runConfigurations/Deploy_AdvancedJoystickInputPipelineTestRobot.xml @@ -0,0 +1,21 @@ + + + + + + + true + + + diff --git a/.idea/runConfigurations/Deploy_HelloWorldTestRobot.xml b/.idea/runConfigurations/Deploy_HelloWorldTestRobot.xml index b6ef5116..e9f24c4d 100644 --- a/.idea/runConfigurations/Deploy_HelloWorldTestRobot.xml +++ b/.idea/runConfigurations/Deploy_HelloWorldTestRobot.xml @@ -4,7 +4,7 @@ diff --git a/.idea/runConfigurations/Deploy_SimpleDrivePipelineTestRobot.xml b/.idea/runConfigurations/Deploy_SimpleDrivePipelineTestRobot.xml new file mode 100644 index 00000000..d9c7e833 --- /dev/null +++ b/.idea/runConfigurations/Deploy_SimpleDrivePipelineTestRobot.xml @@ -0,0 +1,21 @@ + + + + + + + true + + + diff --git a/README.md b/README.md index fa647864..5a1cd765 100644 --- a/README.md +++ b/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 diff --git a/build.gradle b/build.gradle index e60e5708..b4c9a5f1 100644 --- a/build.gradle +++ b/build.gradle @@ -1,3 +1,15 @@ +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" @@ -5,6 +17,13 @@ task wrapper(type: Wrapper) { subprojects { apply plugin: "java" + apply plugin: "kotlin" + + compileKotlin { + kotlinOptions { + jvmTarget = "1.8" + } + } repositories { mavenCentral() @@ -12,5 +31,6 @@ subprojects { dependencies { compile 'org.jetbrains:annotations:16.0.3' + compile "org.jetbrains.kotlin:kotlin-stdlib-jdk8:$kotlin_version" } } diff --git a/docs/Drive Pipelines.md b/docs/Drive Pipelines.md new file mode 100644 index 00000000..df7d6068 --- /dev/null +++ b/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)) +``` + diff --git a/lib/build.gradle b/lib/build.gradle index ba5da841..8b81dcb5 100644 --- a/lib/build.gradle +++ b/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' } diff --git a/lib/src/main/java/org/team1540/rooster/Utilities.java b/lib/src/main/java/org/team1540/rooster/Utilities.java index f47b01e3..bfc50aa1 100644 --- a/lib/src/main/java/org/team1540/rooster/Utilities.java +++ b/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() { } diff --git a/lib/src/main/java/org/team1540/rooster/drive/JoystickScaling.java b/lib/src/main/java/org/team1540/rooster/drive/JoystickScaling.java index d7f386aa..345d1cac 100644 --- a/lib/src/main/java/org/team1540/rooster/drive/JoystickScaling.java +++ b/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 { public double scale(double input); + + public default Double apply(Double input) { + return scale(input); + } } diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/AdvancedArcadeJoystickInput.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/AdvancedArcadeJoystickInput.java new file mode 100644 index 00000000..2db11286 --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/AdvancedArcadeJoystickInput.java @@ -0,0 +1,133 @@ +package org.team1540.rooster.drive.pipeline; + +import java.util.OptionalDouble; +import java.util.function.DoubleSupplier; +import org.jetbrains.annotations.NotNull; +import org.team1540.rooster.Utilities; + +/** + * Modified arcade drive joystick input. + * + * This arcade drive uses one throttle input and two wheel inputs (soft and hard). The soft wheel + * input is multiplied by the absolute value of the throttle input before being used, while the hard + * is not—this allows for smoother control of the robot's path at a variety of speeds. The + * core algorithm is adapted from Team 2471's drive code, which can be found + * here. + *

+ * This class is an {@link Input} that provides a {@link TankDriveData}. The resulting {@link + * TankDriveData} will have the left and right velocities set with units corresponding to the max + * velocity set on construction, as well as the turning rate in radians per second. All other values + * are empty {@link OptionalDouble OptionalDoubles}. + * + * @see Input + * @see FeedForwardProcessor + */ +public class AdvancedArcadeJoystickInput implements Input { + + private double maxVelocity; + private double trackWidth; + private boolean reverseBackwards; + @NotNull + private DoubleSupplier throttleInput; + @NotNull + private DoubleSupplier softTurnInput; + @NotNull + private DoubleSupplier hardTurnInput; + + /** + * Creates a new {@code AdvancedArcadeJoystickInput} that does not reverse while going backwards. + * + * @param maxVelocity The maximum velocity of the robot; joystick values will be scaled to this + * amount. This should be in position units per second to keep with the specification of {@link + * TankDriveData}. + * @param trackWidth The track width of the robot (distance between the wheels); this should be in + * the same position units as maxVelocity. + * @param throttleInput A {@link DoubleSupplier} that supplies the input for the throttle, from -1 + * to 1 inclusive. + * @param softTurnInput A {@link DoubleSupplier} that supplies the input for the soft-turn, from + * -1 (full left) to 1 (full right) inclusive. + * @param hardTurnInput A {@link DoubleSupplier} that supplies the input for the soft-turn, from + */ + public AdvancedArcadeJoystickInput(double maxVelocity, double trackWidth, + @NotNull DoubleSupplier throttleInput, + @NotNull DoubleSupplier softTurnInput, + @NotNull DoubleSupplier hardTurnInput) { + this(maxVelocity, trackWidth, true, throttleInput, softTurnInput, hardTurnInput); + } + + /** + * Creates a new {@code AdvancedArcadeJoystickInput}. + * + * @param maxVelocity The maximum velocity of the robot; joystick values will be scaled to this + * amount. This should be in position units per second to keep with the specification of {@link + * TankDriveData}. + * @param trackWidth The track width of the robot (distance between the wheels); this should be in + * the same position units as maxVelocity. + * @param reverseBackwards If {@code true}, reverses the direction of the soft turn when the + * throttle is negative. + * @param throttleInput A {@link DoubleSupplier} that supplies the input for the throttle, from -1 + * to 1 inclusive. + * @param softTurnInput A {@link DoubleSupplier} that supplies the input for the soft-turn, from + * -1 (full left) to 1 (full right) inclusive. + * @param hardTurnInput A {@link DoubleSupplier} that supplies the input for the soft-turn, from + */ + public AdvancedArcadeJoystickInput(double maxVelocity, double trackWidth, + boolean reverseBackwards, @NotNull DoubleSupplier throttleInput, + @NotNull DoubleSupplier softTurnInput, + @NotNull DoubleSupplier hardTurnInput) { + this.maxVelocity = maxVelocity; + this.trackWidth = trackWidth; + this.reverseBackwards = reverseBackwards; + this.throttleInput = throttleInput; + this.softTurnInput = softTurnInput; + this.hardTurnInput = hardTurnInput; + } + + /** + * Gets the desired output. + * + * @return A {@link TankDriveData} with only the left and right velocity fields specified. + */ + @Override + public TankDriveData get() { + double throttle = throttleInput.getAsDouble(); + double soft = softTurnInput.getAsDouble(); + double hard = hardTurnInput.getAsDouble(); + + // scale the soft turn by the throttle, but don't scale the hard turn + // add turn value to left and subtract from right + double leftPowerRaw = throttle + + (soft * Utilities.invertIf(reverseBackwards && throttle < 0, Math.abs(throttle))) + + hard; + double rightPowerRaw = throttle + - (soft * Utilities.invertIf(reverseBackwards && throttle < 0, Math.abs(throttle))) + - hard; + + // scale the powers, so if the total power for one side is greater than 1 we start reducing the + // other side to compensate + double maxPower = Math.max(Math.abs(leftPowerRaw), Math.abs(rightPowerRaw)); + + double leftPower, rightPower; + if (maxPower > 1) { + leftPower = leftPowerRaw / maxPower; + rightPower = rightPowerRaw / maxPower; + } else { + leftPower = leftPowerRaw; + rightPower = rightPowerRaw; + } + + // omega (dtheta / dt or yaw rate) is just the difference in velocities (powers) divided by the + // track width + + double leftVelocity = leftPower * maxVelocity; + double rightVelocity = rightPower * maxVelocity; + + double omega = (rightVelocity - leftVelocity) / trackWidth; + + return new TankDriveData( + new DriveData(OptionalDouble.of(leftVelocity)), + new DriveData(OptionalDouble.of(rightVelocity)), + OptionalDouble.empty(), + OptionalDouble.of(omega)); + } +} diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/CTREOutput.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/CTREOutput.java new file mode 100644 index 00000000..49825fcd --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/CTREOutput.java @@ -0,0 +1,118 @@ +package org.team1540.rooster.drive.pipeline; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.DemandType; +import com.ctre.phoenix.motorcontrol.IMotorController; +import java.util.Objects; +import org.jetbrains.annotations.Contract; +import org.jetbrains.annotations.NotNull; + +/** + * {@link Output} to pass drive commands to Talon SRX and Victor SPX motor controllers. For output + * details, see the method documentation for {@link #accept(TankDriveData) accept()}. + */ +public class CTREOutput implements Output { + + private IMotorController left; + private IMotorController right; + private boolean closedLoop; + + /** + * Command previously set motors according to the provided {@link TankDriveData}. + *

If closed-loop is enabled ({@link #isClosedLoop()} returns {@code true}), when the provided + * {@link TankDriveData} has non-empty position or velocity fields, that setpoint will be sent to + * the motor controllers as a closed-loop setpoint, with any additional feed-forward sent via + * throttle bump. (Position has priority over velocity when deciding which setpoint to send.) If + * it is {@code false}, or if the provided {@link TankDriveData} has empty position and velocity + * fields, {@link DriveData#additionalFeedForward} (or 0 if it is not present) will be passed in + * as the motor throttle from -1 to 1 inclusive. + * + * @param tankDriveData The data to accept. + */ + @Override + @Contract(pure = true) + public void accept(@NotNull TankDriveData tankDriveData) { + processSide(tankDriveData.left, left); + processSide(tankDriveData.right, right); + } + + private void processSide(DriveData data, IMotorController controller) { + if (data.position.isPresent() && closedLoop) { + if (data.additionalFeedForward.isPresent()) { + controller.set(ControlMode.Position, data.position.getAsDouble(), + DemandType.ArbitraryFeedForward, data.additionalFeedForward.getAsDouble()); + } else { + controller.set(ControlMode.Position, data.position.getAsDouble()); + } + } else if (data.velocity.isPresent() && closedLoop) { + if (data.additionalFeedForward.isPresent()) { + controller.set(ControlMode.Velocity, data.velocity.getAsDouble(), + DemandType.ArbitraryFeedForward, data.additionalFeedForward.getAsDouble()); + } else { + controller.set(ControlMode.Velocity, data.velocity.getAsDouble()); + } + } else { + controller.set(ControlMode.PercentOutput, data.additionalFeedForward.orElse(0)); + } + } + + /** + * Returns whether this {@code CTREOutput} commands its controllers in closed-loop mode if + * possible. If {@code true}, when {@link #accept(TankDriveData) accept()} is called and the + * provided {@link TankDriveData} has non-empty position or velocity fields, that setpoint will be + * sent to the motor controllers as a closed-loop setpoint, with any additional feed-forward sent + * via throttle bump (position has priority over velocity when deciding which setpoint to send.) + * If {@code false}, or if the provided {@link TankDriveData} has empty position and velocity + * fields, {@link DriveData#additionalFeedForward} (or 0 if it is not present) will be passed in + * as the motor throttle from -1 to 1 inclusive. + * + * @return {@code true} if closed-loop control will be used, {@code false} otherwise. + */ + @Contract(pure = true) + public boolean isClosedLoop() { + return closedLoop; + } + + /** + * Sets whether this {@code CTREOutput} commands its controllers in closed-loop mode if possible. + * If {@code closedLoop} is {@code true}, when {@link #accept(TankDriveData) accept()} is called + * and the provided {@link TankDriveData} has non-empty position or velocity fields, that setpoint + * will be sent to the motor controllers as a closed-loop setpoint, with any additional + * feed-forward sent via throttle bump. (Position has priority over velocity when deciding which + * setpoint to send.) If it is {@code false}, or if the provided {@link TankDriveData} has empty + * position and velocity fields, {@link DriveData#additionalFeedForward} (or 0 if it is not + * present) will be passed in as the motor throttle from -1 to 1 inclusive. + * + * @param closedLoop Whether to use closed-loop, if possible. + */ + public void setClosedLoop(boolean closedLoop) { + this.closedLoop = closedLoop; + } + + /** + * Creates a new {@code CTREOutput}. This is equivalent to calling {@link + * #CTREOutput(IMotorController, IMotorController, boolean)} with {@code closedLoop} equal to + * {@code true}. + * + * @param left The left-side motor controller. + * @param right The right-side motor controller. + */ + public CTREOutput(@NotNull IMotorController left, @NotNull IMotorController right) { + this(left, right, true); + } + + /** + * Creates a new {@code CTREOutput}. + * + * @param left The left-side motor controller. + * @param right The right-side motor controller. + * @param closedLoop Whether to command the controllers in closed-loop mode if possible. (See + * {@link #isClosedLoop()}/{@link #setClosedLoop(boolean) setClosedLoop()}). + */ + public CTREOutput(@NotNull IMotorController left, @NotNull IMotorController right, + boolean closedLoop) { + this.left = Objects.requireNonNull(left); + this.right = Objects.requireNonNull(right); + this.closedLoop = closedLoop; + } +} diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/DriveData.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/DriveData.java new file mode 100644 index 00000000..68367ebc --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/DriveData.java @@ -0,0 +1,85 @@ +package org.team1540.rooster.drive.pipeline; + +import java.util.Objects; +import java.util.OptionalDouble; +import org.jetbrains.annotations.NotNull; + +/** + * Encapsulates data for one side of a tank drivetrain. + * + * {@code DriveData} instances are typically passed around as members of {@link TankDriveData} + * instances in drive pipelines. Each {@code DriveData} instance contains values (or empty {@link + * OptionalDouble OptionalDoubles}) for {@linkplain #position}, {@linkplain #velocity}, {@linkplain + * #acceleration}, and {@linkplain #additionalFeedForward feed-forward}. + * + * @see TankDriveData + */ +@SuppressWarnings("OptionalUsedAsFieldOrParameterType") +public class DriveData { + + /** + * The desired position in position-units, or an empty optional if velocity should not be + * controlled. + */ + @NotNull + public final OptionalDouble position; + /** + * The desired velocity in position-units per second, or an empty optional if velocity should not + * be controlled. + */ + @NotNull + public final OptionalDouble velocity; + /** + * The desired acceleration in position-units per second squared, or an empty optional if + * acceleration should not be controlled. + */ + @NotNull + public final OptionalDouble acceleration; + + /** + * An additional raw amount (from -1 to 1 inclusive) that should be added to motor throttle after + * any closed-loop logic, or an empty optional if no feed-forward should be added. + */ + @NotNull + public final OptionalDouble additionalFeedForward; + + /** + * Create a new {@code DriveData} with all fields empty except for the provided velocity. + * + * @param velocity The desired velocity in position-units per second, or an empty optional if + * velocity should not be controlled. + */ + public DriveData(@NotNull OptionalDouble velocity) { + this(OptionalDouble.empty(), velocity, OptionalDouble.empty(), OptionalDouble.empty()); + } + + /** + * Create a new {@code DriveData} with the supplied values. + * + * @param position The desired position in position-units, or an empty optional if velocity should + * not be controlled. + * @param velocity The desired velocity in position-units per second, or an empty optional if + * velocity should not be controlled. + * @param acceleration The desired acceleration in position-units per second squared, or an empty + * optional if acceleration should not be controlled. + * @param additionalFeedForward An additional raw amount (from -1 to 1 inclusive) that should be + * added to motor throttle after any closed-loop logic, or an empty optional if no feed-forward + * should be added. + */ + public DriveData(@NotNull OptionalDouble position, @NotNull OptionalDouble velocity, + @NotNull OptionalDouble acceleration, @NotNull OptionalDouble additionalFeedForward) { + this.position = Objects.requireNonNull(position); + this.velocity = Objects.requireNonNull(velocity); + this.acceleration = Objects.requireNonNull(acceleration); + this.additionalFeedForward = Objects.requireNonNull(additionalFeedForward); + } + + @Override + public String toString() { + return (position.isPresent() ? "position " + position.getAsDouble() : "") + + (velocity.isPresent() ? ", velocity " + velocity.getAsDouble() : "") + + (acceleration.isPresent() ? ", acceleration " + acceleration.getAsDouble() : "") + + (additionalFeedForward.isPresent() ? ", feedforward " + additionalFeedForward + .getAsDouble() : ""); + } +} diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/FeedForwardProcessor.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/FeedForwardProcessor.java new file mode 100644 index 00000000..e2032e89 --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/FeedForwardProcessor.java @@ -0,0 +1,74 @@ +package org.team1540.rooster.drive.pipeline; + +import java.util.OptionalDouble; +import org.jetbrains.annotations.Contract; +import org.jetbrains.annotations.NotNull; + +/** + * {@link Processor} to apply an Oblarg-style + * feed-forward. This processor allows for velocity, acceleration, and static friction (vIntercept) + * feed-forwards. For further details, see {@link #apply(TankDriveData) apply()}. The output will + * mirror the input with the exception of a change in the feed-forward value. + */ +public class FeedForwardProcessor implements Processor { + + private final double velocityFeedFwd; + private final double throtBump; + private final double accelFeedFwd; + + /** + * Creates a {@code FeedForwardProcessor} with the provided \(k_v\) and \(v_{Intercept}\) + * + * @param velocityFeedFwd The velocity constant feed-forward \(k_v\), in output units per speed unit. + * @param throtBump The velocity intercept \(V_{intercept}\), in output units. + * @param accelFeedFwd The acceleration constant feed-forward \(k_a\), in output units per acceleration unit. + */ + public FeedForwardProcessor(double velocityFeedFwd, double throtBump, double accelFeedFwd) { + this.velocityFeedFwd = velocityFeedFwd; + this.throtBump = throtBump; + this.accelFeedFwd = accelFeedFwd; + } + + @Contract(pure = true) + private double getThrottle(double wantedSpeed, double wantedAccel) { + return (velocityFeedFwd * wantedSpeed) + + (accelFeedFwd * wantedAccel) + + (wantedSpeed != 0 ? Math.copySign(throtBump, wantedSpeed) : 0); + } + + /** + * Applies feed-forwards to the provided {@link TankDriveData}. The method for calculating the + * feed-forward is as follows: + *

    + *
  1. The feed-forward starts at 0.
    2. + *
  2. The product of the velocity (if present) and the velocity feed-forward is added.
    3. + *
  3. The product of the acceleration (if present) and the acceleration feed-forward is added.
    4. + *
  4. If the velocity is present and nonzero, the throttle bump (with the sign of the + * velocity) is added.
    5. + *
+ * The calculated feed-forward is then added to any feed-forward already present in the + * DriveData. + * + * @param command The data to use. + * @return A new {@link TankDriveData} as described above. + */ + @Override + @NotNull + public TankDriveData apply(@NotNull TankDriveData command) { + return new TankDriveData( + new DriveData(command.left.position, + command.left.velocity, + command.left.acceleration, + OptionalDouble.of(command.left.additionalFeedForward.orElse(0) + + getThrottle(command.left.velocity.orElse(0), + command.left.acceleration.orElse(0)))), + new DriveData(command.right.position, + command.right.velocity, + command.right.acceleration, + OptionalDouble.of(command.right.additionalFeedForward.orElse(0) + + getThrottle(command.right.velocity.orElse(0), + command.right.acceleration.orElse(0)))), + command.heading, + command.turningRate); + } +} diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/Input.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/Input.java new file mode 100644 index 00000000..4b4547a7 --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/Input.java @@ -0,0 +1,44 @@ +package org.team1540.rooster.drive.pipeline; + +import java.util.function.Consumer; +import java.util.function.Function; +import java.util.function.Supplier; +import org.team1540.rooster.util.Executable; + +/** + * Extension of a {@link Supplier} adding additional composition methods. + * + * {@code Input} can be used exactly like {@link Supplier} (and library functions should not take + * {@code Outputs} as method parameters, instead using {@link Supplier}). However, library functions + * pertaining to drive pipelines should return an {@code Input} where they would normally return a + * {@link Supplier}. + * + * @param The type of the input. + */ +@FunctionalInterface +public interface Input extends Supplier { + + /** + * Creates a new {@code Input} that applies the provided {@link Function} to this {@code Input}'s + * output. + * + * @param f The {@link Function} (or {@link Processor}) to apply. + * @param The return type of the provided {@link Function}, and thus the return type of the + * returned {@code Input}. + * @return A new {@code Input} as described above. + */ + public default Input then(Function f) { + return () -> f.apply(get()); + } + + /** + * Creates a new {@link Executable} that, when run, applies the provided {@link Consumer} (or + * {@link Output}) to this {@code Input}'s output. + * + * @param c The {@link Consumer} (or {@link Output} to use. + * @return A new {@link Executable} as described above. + */ + public default Executable then(Consumer c) { + return () -> c.accept(get()); + } +} diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/Output.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/Output.java new file mode 100644 index 00000000..2f9f67d3 --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/Output.java @@ -0,0 +1,44 @@ +package org.team1540.rooster.drive.pipeline; + +import java.util.function.Consumer; +import java.util.function.Function; +import java.util.function.Supplier; +import org.team1540.rooster.util.Executable; + +/** + * Extension of a {@link Consumer} adding an additional composition method. + * + * {@code Output} can be used exactly like {@link Consumer} (and library functions should not take + * {@code Outputs} as method parameters, instead using {@link Consumer}). However, library functions + * pertaining to drive pipelines should return an {@code Output} where they would normally return a + * {@link Consumer}. + * + * @param The type of the output. + */ +@FunctionalInterface +public interface Output extends Consumer { + + /** + * Creates a new {@code Output} that applies the provided {@link Function} to the input before + * passing it to this {@code Output}. + * + * @param f The {@link Function} (or {@link Processor}) to apply. + * @param The input type of the {@link Function} (and thus the {@link Input} type of the + * resulting {@code Output}). + * @return A new {@code Output} as described above. + */ + public default Output after(Function f) { + return i -> accept(f.apply(i)); + } + + /** + * Creates a new {@link Executable} that, when run, applies this {@code Output} to the provided + * {@link Supplier}'s (or {@link Input}'s) output. + * + * @param i The {@link Supplier} (or {@link Input} to use. + * @return A new {@link Executable} as described above. + */ + public default Executable after(Supplier i) { + return () -> accept(i.get()); + } +} diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/Processor.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/Processor.java new file mode 100644 index 00000000..adc0e0a8 --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/Processor.java @@ -0,0 +1,41 @@ +package org.team1540.rooster.drive.pipeline; + +import java.util.function.Consumer; +import java.util.function.Function; +import java.util.function.Supplier; + +/** + * Extension of a {@link Function} adding additional composition methods. + * + * {@code Processor} can be used exactly like {@link Function} (and library functions should not + * take {@code Processors} as method parameters, instead using {@link Function}). However, library + * functions pertaining to drive pipelines should return a {@code Processor} where they would + * normally return a {@link Function}. + * + * @param The type of the output. + */ +@FunctionalInterface +public interface Processor extends Function { + + /** + * Creates a new {@link Input} that applies this {@code Processor} to the output of the supplied + * {@link Supplier}. + * + * @param i The {@link Supplier} (or {@link Input}) to process in the returned {@link Input}. + * @return An {@link Input} as described above. + */ + public default Input process(Supplier i) { + return () -> apply(i.get()); + } + + /** + * Creates a new {@link Output} that applies this {@code Processor} to the input before passing it + * to the provided {@link Consumer}. + * + * @param o The {@link Consumer} (or {@link Output}) to pass the processed results to. + * @return A new {@link Output} as described above. + */ + public default Output followedBy(Consumer o) { + return t -> o.accept(apply(t)); + } +} diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/SimpleJoystickInput.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/SimpleJoystickInput.java new file mode 100644 index 00000000..49c7558f --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/SimpleJoystickInput.java @@ -0,0 +1,130 @@ +package org.team1540.rooster.drive.pipeline; + +import edu.wpi.first.wpilibj.Joystick; +import java.util.OptionalDouble; +import org.team1540.rooster.Utilities; + +/** + * Simple tank-style input from a WPILib {@link Joystick}. The left and right joysticks are used to + * control the left and right sides of the robot respectively. Additionally, there is an optional + * "forward axis" and "back axis"; if the forward axis is bound to one trigger on an Xbox controller + * and the back to the other, the forward trigger will cause the robot to drive straight forward and + * the back trigger will cause it to drive straight backwards. This {@link Input} creates a {@link + * TankDriveData} with only the feed-forward fields set on the left and right sides. + */ +public class SimpleJoystickInput implements Input { + + private Joystick joystick; + private int leftAxis; + private int rightAxis; + private int fwdAxis; + private int backAxis; // or "baxis" if you don't have much time + private boolean invertLeft; + private boolean invertRight; + + private double deadzone; + + @Override + public TankDriveData get() { + double triggerValue; + if (fwdAxis != -1 && backAxis != -1) { + triggerValue = Utilities.processDeadzone(joystick.getRawAxis(fwdAxis), deadzone) + - Utilities.processDeadzone(joystick.getRawAxis(backAxis), deadzone); + } else { + triggerValue = 0; + } + double leftThrottle = Utilities.constrain( + Utilities.processDeadzone( + Utilities.invertIf(invertLeft, joystick.getRawAxis(leftAxis)), deadzone + ) + triggerValue, 1); + double rightThrottle = Utilities.constrain( + Utilities.processDeadzone( + Utilities.invertIf(invertRight, joystick.getRawAxis(rightAxis)), deadzone + ) + triggerValue, 1); + + return new TankDriveData( + new DriveData( + OptionalDouble.empty(), + OptionalDouble.empty(), + OptionalDouble.empty(), + OptionalDouble.of(leftThrottle) + ), + new DriveData( + OptionalDouble.empty(), + OptionalDouble.empty(), + OptionalDouble.empty(), + OptionalDouble.of(rightThrottle) + ), + OptionalDouble.empty(), + OptionalDouble.empty() + ); + } + + /** + * Creates a new {@code SimpleJoystickInput} with a deadzone of 0.1 and no forward/back axis + * control. + * + * @param joystick The {@link Joystick} to use. + * @param leftAxis The axis number (as provided to {@link Joystick#getRawAxis(int)} for the left + * side. + * @param rightAxis The axis number (as provided to {@link Joystick#getRawAxis(int)} for the right + * side. + * @param invertLeft Whether to invert the axis value of the left axis. + * @param invertRight Whether to invert the axis value of the right axis. + */ + public SimpleJoystickInput(Joystick joystick, int leftAxis, int rightAxis, + boolean invertLeft, boolean invertRight) { + this(joystick, leftAxis, rightAxis, -1, -1, invertLeft, + invertRight); + } + + /** + * Creates a new {@code SimpleJoystickInput} with a deadzone of 0.1. + * + * @param joystick The {@link Joystick} to use. + * @param leftAxis The axis number (as provided to {@link Joystick#getRawAxis(int)} for the left + * side. + * @param rightAxis The axis number (as provided to {@link Joystick#getRawAxis(int)} for the right + * side. + * @param fwdAxis The axis number (as provided to {@link Joystick#getRawAxis(int)} for the forward + * axis, or -1 for none. + * @param backAxis The axis number (as provided to {@link Joystick#getRawAxis(int)} for the back + * axis, or -1 for none. + * @param invertLeft Whether to invert the axis value of the left axis. + * @param invertRight Whether to invert the axis value of the right axis. + */ + public SimpleJoystickInput(Joystick joystick, int leftAxis, int rightAxis, int fwdAxis, + int backAxis, boolean invertLeft, boolean invertRight) { + this(joystick, leftAxis, rightAxis, fwdAxis, backAxis, invertLeft, invertRight, + 0.1); + } + + /** + * Creates a new {@code SimpleJoystickInput}. + * + * @param joystick The {@link Joystick} to use. + * @param leftAxis The axis number (as provided to {@link Joystick#getRawAxis(int)} for the left + * side. + * @param rightAxis The axis number (as provided to {@link Joystick#getRawAxis(int)} for the right + * side. + * @param fwdAxis The axis number (as provided to {@link Joystick#getRawAxis(int)} for the forward + * axis. + * @param backAxis The axis number (as provided to {@link Joystick#getRawAxis(int)} for the back + * axis. + * @param invertLeft Whether to invert the axis value of the left axis. + * @param invertRight Whether to invert the axis value of the right axis. + * @param deadzone The deadzone for the axes (see {@link Utilities#processDeadzone(double, + * double)}). + */ + public SimpleJoystickInput(Joystick joystick, int leftAxis, int rightAxis, int fwdAxis, + int backAxis, boolean invertLeft, boolean invertRight, double deadzone) { + this.joystick = joystick; + this.leftAxis = leftAxis; + this.rightAxis = rightAxis; + this.fwdAxis = fwdAxis; + this.backAxis = backAxis; + this.invertLeft = invertLeft; + this.invertRight = invertRight; + this.deadzone = deadzone; + } +} diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/TankDriveData.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/TankDriveData.java new file mode 100644 index 00000000..ee169a1c --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/TankDriveData.java @@ -0,0 +1,68 @@ +package org.team1540.rooster.drive.pipeline; + +import java.util.Objects; +import java.util.OptionalDouble; +import org.jetbrains.annotations.NotNull; + +/** + * Encapsulates drive commands for a tank drive. + * + * {@code TankDriveData} instances are usually passed around in drive pipelines. Each {@code + * TankDriveData} instance contains {@link DriveData} instances for the left and right side, as well + * as commands for {@linkplain #heading} and {@linkplain #turningRate turning rate}. + * + * @see DriveData + */ +@SuppressWarnings("OptionalUsedAsFieldOrParameterType") +public class TankDriveData { + + /** + * The drive data for the left side. + */ + @NotNull + public final DriveData left; + /** + * The drive data for the right side. + */ + @NotNull + public final DriveData right; + /** + * The desired heading in radians from 0 (straight forward) to 2π, increasing clockwise, or an + * empty optional if heading should not be controlled. + */ + @NotNull + public final OptionalDouble heading; + /** + * The desired turning rate in radians/sec, or an empty optional if turning rate should not be + * controlled. + */ + @NotNull + public final OptionalDouble turningRate; + + /** + * Creates a new {@code TankDriveData} with the supplied values. + * + * @param left The {@link DriveData} for the left side. + * @param right The {@link DriveData} for the right side. + * @param heading The desired heading in radians from 0 (straight forward) to 2π, increasing + * clockwise, or an empty optional if heading should not be controlled. + * @param turningRate The desired turning rate in radians/sec, or an empty optional if turning + * rate should not be controlled. + */ + public TankDriveData(@NotNull DriveData left, @NotNull DriveData right, + @NotNull OptionalDouble heading, + @NotNull OptionalDouble turningRate) { + this.left = Objects.requireNonNull(left); + this.right = Objects.requireNonNull(right); + this.heading = Objects.requireNonNull(heading); + this.turningRate = Objects.requireNonNull(turningRate); + } + + @Override + public String toString() { + return "TankDriveData: " + "left: " + left + + ", right:" + right + + (heading.isPresent() ? ", heading " + heading.getAsDouble() : "") + + (turningRate.isPresent() ? ", turning rate " + turningRate.getAsDouble() : ""); + } +} diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/UnitScaler.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/UnitScaler.java new file mode 100644 index 00000000..f9b10675 --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/UnitScaler.java @@ -0,0 +1,68 @@ +package org.team1540.rooster.drive.pipeline; + +import java.util.OptionalDouble; + +/** + * Scales units by a desired factor. For details of the scaling, see {@link #apply(TankDriveData) + * apply()}. + */ +public class UnitScaler implements Processor { + + private double distanceFactor; + private double timeFactor; + + /** + * Scales the units in the provided {@link TankDriveData} instance. During scaling, for both sides + * ({@link TankDriveData#left left} and {@link TankDriveData#right right}), {@link + * DriveData#position position} is multiplied by the distance factor, and {@link + * DriveData#velocity velocity} and {@link DriveData#acceleration acceleration} are multiplied by + * the distance factor then divided by the time factor. If any of these parameters are not + * present, they will also simply not be present in the output. + * + * Feed-forward, heading, and turning rate setpoints are passed through unaffected. + * + * @param d The data to scale. + * @return A new {@link TankDriveData}, scaled as above. + */ + @Override + public TankDriveData apply(TankDriveData d) { + return new TankDriveData( + new DriveData( + d.left.position.isPresent() ? + OptionalDouble.of(d.left.position.getAsDouble() * distanceFactor) + : d.left.position, + d.left.velocity.isPresent() ? + OptionalDouble.of(d.left.velocity.getAsDouble() * distanceFactor / timeFactor) + : d.left.velocity, + d.left.acceleration.isPresent() ? + OptionalDouble.of(d.left.acceleration.getAsDouble() * distanceFactor / timeFactor) + : d.left.acceleration, + d.left.additionalFeedForward + ), + new DriveData( + d.right.position.isPresent() ? + OptionalDouble.of(d.right.position.getAsDouble() * distanceFactor) + : d.right.position, + d.right.velocity.isPresent() ? + OptionalDouble.of(d.right.velocity.getAsDouble() * distanceFactor / timeFactor) + : d.right.velocity, + d.right.acceleration.isPresent() ? + OptionalDouble.of(d.right.acceleration.getAsDouble() * distanceFactor / timeFactor) + : d.right.acceleration, + d.right.additionalFeedForward + ), + d.heading, d.turningRate); + } + + /** + * Creates a new {@code UnitScaler}. + * + * @param distanceFactor The scale factor from input distance units (e.g. feet, meters) to output + * distance units (e.g. ticks). + * @param timeFactor The scale factor from input time units (e.g. seconds) to output time units. + */ + public UnitScaler(double distanceFactor, double timeFactor) { + this.distanceFactor = distanceFactor; + this.timeFactor = timeFactor; + } +} diff --git a/lib/src/main/java/org/team1540/rooster/drive/pipeline/package-info.java b/lib/src/main/java/org/team1540/rooster/drive/pipeline/package-info.java new file mode 100644 index 00000000..22280472 --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/drive/pipeline/package-info.java @@ -0,0 +1,11 @@ +/** + * Collection of useful inputs, processors, and outputs. Contains "stock" or "in-the-box" classes + * that implement the {@link org.team1540.rooster.drive.pipeline.Input Input}, {@link + * org.team1540.rooster.drive.pipeline.Processor Processor}, and {@link + * org.team1540.rooster.drive.pipeline.Output Output} interfaces (as well as those interfaces + * themselves). + * + *

Unless otherwise noted, all methods, constructors, etc. in this package throw a {@link + * java.lang.NullPointerException} if any parameters are {@code null}. + */ +package org.team1540.rooster.drive.pipeline; diff --git a/lib/src/main/java/org/team1540/rooster/util/SimpleAsyncCommand.java b/lib/src/main/java/org/team1540/rooster/util/SimpleAsyncCommand.java new file mode 100644 index 00000000..93fef841 --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/util/SimpleAsyncCommand.java @@ -0,0 +1,52 @@ +package org.team1540.rooster.util; + +import edu.wpi.first.wpilibj.Sendable; +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.util.Objects; +import org.jetbrains.annotations.NotNull; + +/** + * A simple way to construct an {@link AsyncCommand}.

To create a {@code SimpleCommand} easily, + * simply pass it a no-argument lambda containing the code you would like to execute. For example, + * to create a {@link AsyncCommand} that loops every 30 milliseconds and requires the {@code + * Robot.shifter} {@link Subsystem}, simply write: + * 

+ *   Command shift = new SimpleAsyncCommand("Shift", 30, () -> Robot.shifter.shift(), Robot.shifter);
+ *
+ * Multiple {@code Subsystems} can be added onto the end of the constructor to add multiple + * requirements.

This can be used to quickly and easily put a button on the + * SmartDashboard/Shuffleboard to run some code. Use {@link SmartDashboard#putData(Sendable)} to + * pass it a newly created instance of this class. + * + * @see SimpleAsyncCommand + * @see SimpleLoopCommand + * @see Command + */ +public class SimpleAsyncCommand extends AsyncCommand { + + private final Executable executable; + + /** + * Constructs a new {@code SimpleAsyncCommand} with a preset periodic interval that runs the + * provided {@link Executable}. + * + * @param interval The interval between calls to the {@link Executable}, in milliseconds. + */ + public SimpleAsyncCommand(@NotNull String name, long interval, @NotNull Executable executable, + @NotNull Subsystem... requirements) { + super(interval); + this.setName(Objects.requireNonNull(name)); + + this.executable = Objects.requireNonNull(executable); + for (Subsystem requirement : requirements) { + requires(Objects.requireNonNull(requirement)); + } + } + + @Override + protected void runPeriodic() { + executable.execute(); + } +} diff --git a/lib/src/main/java/org/team1540/rooster/util/robots/package-info.java b/lib/src/main/java/org/team1540/rooster/util/robots/package-info.java new file mode 100644 index 00000000..95592ede --- /dev/null +++ b/lib/src/main/java/org/team1540/rooster/util/robots/package-info.java @@ -0,0 +1,5 @@ +/** + * Self-contained robots to perform various tasks. To load one of these robots, set your + * robot class (in build.gradle) to org.team1540.rooster.util.robots.<robot name>. + */ +package org.team1540.rooster.util.robots; diff --git a/lib/src/main/kotlin/org/team1540/rooster/drive/pipeline/PipelineExtensions.kt b/lib/src/main/kotlin/org/team1540/rooster/drive/pipeline/PipelineExtensions.kt new file mode 100644 index 00000000..4c27ac01 --- /dev/null +++ b/lib/src/main/kotlin/org/team1540/rooster/drive/pipeline/PipelineExtensions.kt @@ -0,0 +1,17 @@ +@file:JvmName("PipelineExtensions") + +package org.team1540.rooster.drive.pipeline + +import org.team1540.rooster.util.Executable +import java.util.function.Consumer +import java.util.function.Supplier + +// somewhat unfortunate names +operator fun (() -> T).plus(f: (T) -> Unit): () -> Unit = { f(this()) } + +operator fun Supplier.plus(f: Consumer): Executable = Executable { f.accept(this.get()) } + +@JvmName("plusProcessor") +operator fun (() -> T).plus(f: (T) -> R): () -> R = { f(this()) } + +operator fun Supplier.plus(f: java.util.function.Function): Supplier = Supplier { f.apply(this.get()) } diff --git a/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt b/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt new file mode 100644 index 00000000..0321f531 --- /dev/null +++ b/test/src/main/kotlin/org/team1540/rooster/testing/DrivePipelineTesting.kt @@ -0,0 +1,191 @@ +package org.team1540.rooster.testing + +import com.ctre.phoenix.motorcontrol.ControlMode +import com.kauailabs.navx.frc.AHRS +import edu.wpi.first.wpilibj.* +import edu.wpi.first.wpilibj.command.Command +import edu.wpi.first.wpilibj.command.Scheduler +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard +import org.team1540.rooster.Utilities +import org.team1540.rooster.drive.pipeline.* +import org.team1540.rooster.preferencemanager.Preference +import org.team1540.rooster.preferencemanager.PreferenceManager +import org.team1540.rooster.util.Executable +import org.team1540.rooster.util.SimpleAsyncCommand +import org.team1540.rooster.util.SimpleCommand +import org.team1540.rooster.util.SimpleLoopCommand +import org.team1540.rooster.wrappers.ChickenTalon +import java.util.function.DoubleSupplier + +/** + * Base class that all other testing classes inherit from; just has a command that gets started when + * teleop starts. + */ +abstract class DrivePipelineTestRobot : IterativeRobot() { + protected abstract val command: Command + + override fun teleopInit() { + command.start() + } + + override fun robotPeriodic() { + Scheduler.getInstance().run() + } +} + +/** + * just to test that everything's sane; joystick tank drive + * */ +class SimpleDrivePipelineTestRobot : DrivePipelineTestRobot() { + override val command = SimpleLoopCommand("Drive", + SimpleJoystickInput(Joystick(0), 1, 5, 3, 2, false, false) + + CTREOutput(PipelineDriveTrain.left1, PipelineDriveTrain.right1) + ) +} + +/** + * Testing class for [AdvancedArcadeJoystickInput]. + */ +class AdvancedJoystickInputPipelineTestRobot : DrivePipelineTestRobot() { + @JvmField + @Preference(persistent = false) + var maxVelocity = 1.0 + @JvmField + @Preference(persistent = false) + var trackWidth = 1.0 + @JvmField + @Preference(persistent = false) + var tpu = 1.0 + @JvmField + @Preference(persistent = false) + var power = 0.0 + + @JvmField + @Preference(persistent = false) + var p = 0.0 + @JvmField + @Preference(persistent = false) + var i = 0.0 + @JvmField + @Preference(persistent = false) + var d = 0.0 + @JvmField + @Preference(persistent = false) + var ramp = 0.0 + @JvmField + @Preference(persistent = false) + var revBack = false + + + private val joystick = XboxController(0) + + override fun robotInit() { + PreferenceManager.getInstance().add(this) + val reset = SimpleCommand("reset", Executable { + _command = SimpleAsyncCommand("Drive", 20, AdvancedArcadeJoystickInput( + maxVelocity, trackWidth, revBack, + DoubleSupplier { Utilities.scale(-Utilities.processDeadzone(joystick.getY(GenericHID.Hand.kLeft), 0.1), power) }, + DoubleSupplier { Utilities.scale(Utilities.processDeadzone(joystick.getX(GenericHID.Hand.kRight), 0.1), power) }, + DoubleSupplier { + Utilities.scale((Utilities.processDeadzone(joystick.getTriggerAxis(GenericHID.Hand.kRight), 0.1) + - Utilities.processDeadzone(joystick.getTriggerAxis(GenericHID.Hand.kLeft), 0.1)), power) + }) + + (FeedForwardProcessor(1 / maxVelocity, 0.0, 0.0)) + + UnitScaler(tpu, 0.1) + + (CTREOutput(PipelineDriveTrain.left1, PipelineDriveTrain.right1))) + + listOf(PipelineDriveTrain.left1, PipelineDriveTrain.right1).forEach { + it.configClosedloopRamp(ramp) + it.config_kP(0, p) + it.config_kI(0, i) + it.config_kD(0, d) + it.config_kF(0, 0.0) + } + + }).apply { + setRunWhenDisabled(true) + start() + } + + SmartDashboard.putData(reset) + } + + private lateinit var _command: Command + override val command get() = _command +} + +/** + * Common drive train object to be used by all pipeline test robots. + */ +@Suppress("unused") +private object PipelineDriveTrain { + val left1 = ChickenTalon(1).apply { + setBrake(true) + configClosedloopRamp(0.0) + configOpenloopRamp(0.0) + configPeakOutputForward(1.0) + configPeakOutputReverse(-1.0) + enableCurrentLimit(false) + inverted = false + setSensorPhase(true) + } + + private val left2 = ChickenTalon(2).apply { + setBrake(true) + configClosedloopRamp(0.0) + configOpenloopRamp(0.0) + configPeakOutputForward(1.0) + configPeakOutputReverse(-1.0) + enableCurrentLimit(false) + inverted = false + set(ControlMode.Follower, left1.deviceID.toDouble()) + } + private val left3 = ChickenTalon(3).apply { + setBrake(true) + configClosedloopRamp(0.0) + configOpenloopRamp(0.0) + configPeakOutputForward(1.0) + configPeakOutputReverse(-1.0) + enableCurrentLimit(false) + inverted = false + set(ControlMode.Follower, left1.deviceID.toDouble()) + } + + val right1 = ChickenTalon(4).apply { + setBrake(true) + configClosedloopRamp(0.0) + configOpenloopRamp(0.0) + configPeakOutputForward(1.0) + configPeakOutputReverse(-1.0) + enableCurrentLimit(false) + inverted = true + setSensorPhase(true) + } + private val right2 = ChickenTalon(5).apply { + setBrake(true) + configClosedloopRamp(0.0) + configOpenloopRamp(0.0) + configPeakOutputForward(1.0) + configPeakOutputReverse(-1.0) + enableCurrentLimit(false) + inverted = true + set(ControlMode.Follower, right1.deviceID.toDouble()) + } + private val right3 = ChickenTalon(6).apply { + setBrake(true) + configClosedloopRamp(0.0) + configOpenloopRamp(0.0) + configPeakOutputForward(1.0) + configPeakOutputReverse(-1.0) + enableCurrentLimit(false) + inverted = true + set(ControlMode.Follower, right1.deviceID.toDouble()) + } +} + +/** + * Just an object to hold a NavX for pipeline testers. + */ +private object PipelineNavx { + val navx = AHRS(SPI.Port.kMXP) +}