* 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}.
+ * TankDriveData} will have the left and right feed-forwards set to throttles between -1 and 1. All
+ * other values are empty {@link OptionalDouble OptionalDoubles}.
*
* @see Input
* @see FeedForwardProcessor
*/
public class AdvancedArcadeJoystickInput implements Input Unless otherwise noted, all methods, constructors, etc. in this package throw a {@link
* java.lang.NullPointerException} if any parameters are {@code null}.
diff --git a/src/main/java/org/team1540/rooster/functional/Executable.java b/src/main/java/org/team1540/rooster/functional/Executable.java
new file mode 100644
index 00000000..01a94db8
--- /dev/null
+++ b/src/main/java/org/team1540/rooster/functional/Executable.java
@@ -0,0 +1,13 @@
+package org.team1540.rooster.functional;
+
+/**
+ * Functional interface for a function that takes no arguments and returns no output.
+ */
+@FunctionalInterface
+public interface Executable {
+
+ /**
+ * Perform the required action.
+ */
+ void execute();
+}
diff --git a/src/main/java/org/team1540/rooster/drive/pipeline/Input.java b/src/main/java/org/team1540/rooster/functional/Input.java
similarity index 86%
rename from src/main/java/org/team1540/rooster/drive/pipeline/Input.java
rename to src/main/java/org/team1540/rooster/functional/Input.java
index 24cf0beb..72f9e72b 100644
--- a/src/main/java/org/team1540/rooster/drive/pipeline/Input.java
+++ b/src/main/java/org/team1540/rooster/functional/Input.java
@@ -1,17 +1,15 @@
-package org.team1540.rooster.drive.pipeline;
+package org.team1540.rooster.functional;
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}.
+ * should return an {@code Input} where they would normally return a {@link Supplier}.
*
* @param The folder provided should contain profile CSV files (formatted so as to be read by {@link
+ * Pathfinder#readFromCSV(File)}) where each profile named {@code name} is stored in two files,
+ * {@code name}_left.csv for the left-side profile and {@code name}_right.csv for the right-side
+ * profile. An example folder structure would be as follows (where the profiles/ directory is
+ * provided to the constructor):
+ *
+ * This class is immutable after instantiation.
+ */
+public class ProfileContainer {
+
+ @NotNull
+ private Map To create a {@code SimpleCommand} easily,
diff --git a/src/main/java/org/team1540/rooster/util/SimpleCommand.java b/src/main/java/org/team1540/rooster/util/SimpleCommand.java
index b9ce154a..af6981ac 100644
--- a/src/main/java/org/team1540/rooster/util/SimpleCommand.java
+++ b/src/main/java/org/team1540/rooster/util/SimpleCommand.java
@@ -6,6 +6,7 @@
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.Objects;
import org.jetbrains.annotations.NotNull;
+import org.team1540.rooster.functional.Executable;
/**
* A simple way to construct an {@link InstantCommand}. To create a {@code SimpleCommand}
diff --git a/src/main/java/org/team1540/rooster/util/SimpleLoopCommand.java b/src/main/java/org/team1540/rooster/util/SimpleLoopCommand.java
index 5f3f4e81..a236015e 100644
--- a/src/main/java/org/team1540/rooster/util/SimpleLoopCommand.java
+++ b/src/main/java/org/team1540/rooster/util/SimpleLoopCommand.java
@@ -6,6 +6,7 @@
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.Objects;
import org.jetbrains.annotations.NotNull;
+import org.team1540.rooster.functional.Executable;
/**
* A simple way to construct a {@link Command} which executes every tick. To create a {@code
diff --git a/src/main/java/org/team1540/rooster/util/robots/AccelerationCharacterizationRobot.java b/src/main/java/org/team1540/rooster/util/robots/AccelerationCharacterizationRobot.java
new file mode 100644
index 00000000..7e032da0
--- /dev/null
+++ b/src/main/java/org/team1540/rooster/util/robots/AccelerationCharacterizationRobot.java
@@ -0,0 +1,233 @@
+package org.team1540.rooster.util.robots;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.Notifier;
+import edu.wpi.first.wpilibj.command.Scheduler;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import java.io.File;
+import java.io.FileNotFoundException;
+import java.io.PrintWriter;
+import java.util.LinkedList;
+import java.util.List;
+import org.apache.commons.math3.stat.regression.SimpleRegression;
+import org.team1540.rooster.preferencemanager.Preference;
+import org.team1540.rooster.preferencemanager.PreferenceManager;
+import org.team1540.rooster.wrappers.ChickenTalon;
+
+/**
+ * Self-contained robot class to characterize a drivetrain's acceleration term.
+ *
+ * When deployed to a three-motor-per-side robot with left motors on motors 1, 2, and 3 and right
+ * motors on motors 4, 5, and 6, whenever the A button is pressed and held during teleop the robot
+ * will carry out an acceleration characterization as described in Eli Barnett's paper "FRC
+ * Drivetrain Characterization" until the button is released. (Encoders should be on motors 1 and
+ * 4.) The results will be saved in a file in the /home/lvuser/dtmeasure directory named
+ * "measureaccel-" followed by the Unix timestamp of the test start and ".csv".
+ */
+public class AccelerationCharacterizationRobot extends IterativeRobot {
+
+ @Preference("kV")
+ public double kV;
+ @Preference("VIntercept")
+ public double vIntercept;
+
+ @Preference("Invert Left Motor")
+ public boolean invertLeft = false;
+ @Preference("Invert Right Motor")
+ public boolean invertRight = true;
+ @Preference(persistent = false)
+ public int accelMeasurementWindow = 6;
+
+ @Preference(persistent = false)
+ public double setpoint = 0.6;
+
+ private ChickenTalon driveLeftMotorA = new ChickenTalon(1);
+ private ChickenTalon driveLeftMotorB = new ChickenTalon(2);
+ private ChickenTalon driveLeftMotorC = new ChickenTalon(3);
+ private ChickenTalon[] driveLeftMotors = new ChickenTalon[]{driveLeftMotorA, driveLeftMotorB,
+ driveLeftMotorC};
+ private ChickenTalon driveRightMotorA = new ChickenTalon(4);
+ private ChickenTalon driveRightMotorB = new ChickenTalon(5);
+ private ChickenTalon driveRightMotorC = new ChickenTalon(6);
+ private ChickenTalon[] driveRightMotors = new ChickenTalon[]{driveRightMotorA, driveRightMotorB,
+ driveRightMotorC};
+ private ChickenTalon[] driveMotorAll = new ChickenTalon[]{driveLeftMotorA, driveLeftMotorB,
+ driveLeftMotorC, driveRightMotorA, driveRightMotorB, driveRightMotorC};
+ private ChickenTalon[] driveMotorMasters = new ChickenTalon[]{driveLeftMotorA, driveRightMotorA};
+
+ private PrintWriter csvWriter = null;
+
+ private Notifier notifier = new Notifier(this::run);
+
+ @SuppressWarnings("SuspiciousNameCombination")
+ private void run() {
+ if (!isOperatorControl() && csvWriter != null) {
+ csvWriter.close();
+ csvWriter = null;
+ }
+ if (leftVelocities.size() == accelMeasurementWindow) {
+ leftVelocities.remove(0);
+ rightVelocities.remove(0);
+ leftVoltages.remove(0);
+ rightVoltages.remove(0);
+ times.remove(0);
+ }
+ double leftVelocity = driveLeftMotorA.getSelectedSensorVelocity();
+ double rightVelocity = driveRightMotorA.getSelectedSensorVelocity();
+
+ leftVelocities.add(leftVelocity);
+ rightVelocities.add(rightVelocity);
+
+ double accelCausingVoltageLeft =
+ driveLeftMotorA.getMotorOutputVoltage() - (kV * leftVelocity
+ + vIntercept);
+ double accelCausingVoltageRight =
+ driveRightMotorA.getMotorOutputVoltage() - (kV * rightVelocity
+ + vIntercept);
+ leftVoltages.add(accelCausingVoltageLeft);
+ rightVoltages.add(accelCausingVoltageRight);
+ times.add((double) System.currentTimeMillis() / 1000.0);
+
+ if (leftVelocities.size() == accelMeasurementWindow) {
+ double lAccel = bestFitSlope(times, leftVelocities);
+ double rAccel = bestFitSlope(times, rightVelocities);
+ SmartDashboard.putNumber("Left Accel", lAccel);
+ SmartDashboard.putNumber("Right Accel", rAccel);
+ }
+
+ if (joystick.getRawButton(1)) { // if button A is pressed
+ if (csvWriter == null) {
+ // create a new CSV writer, reset everything
+ reset();
+ try {
+ File dir = new File("/home/lvuser/dtmeasure/");
+ if (!dir.exists()) {
+ dir.mkdirs();
+ }
+ csvWriter = new PrintWriter(new File(
+ "/home/lvuser/dtmeasure/measureaccel-" + System.currentTimeMillis() + ".csv"));
+ csvWriter.println("lvoltage,laccel,rvoltage,raccel");
+ } catch (FileNotFoundException e) {
+ throw new RuntimeException(e);
+ }
+ driveLeftMotorA.set(ControlMode.PercentOutput, 0);
+ driveRightMotorA.set(ControlMode.PercentOutput, 0);
+ } else {
+ SmartDashboard.putNumber("Left Output", driveLeftMotorA.getMotorOutputPercent());
+ SmartDashboard.putNumber("Left Velocity", leftVelocity);
+ SmartDashboard.putNumber("Right Output", driveRightMotorA.getMotorOutputPercent());
+ SmartDashboard.putNumber("Right Velocity", rightVelocity);
+
+ if (leftVelocities.size() == accelMeasurementWindow) {
+ double lAccel = bestFitSlope(times, leftVelocities);
+ double rAccel = bestFitSlope(times, rightVelocities);
+ csvWriter.println(
+ leftVoltages.get(1) + "," + lAccel + "," + rightVoltages.get(1) + "," + rAccel);
+ System.out.println(leftVelocities.toString());
+ System.out.println(times.toString());
+ System.out.println(lAccel);
+ }
+ driveLeftMotorA.set(ControlMode.PercentOutput, setpoint);
+ driveRightMotorA.set(ControlMode.PercentOutput, setpoint);
+ }
+ } else {
+ if (csvWriter != null) {
+ csvWriter.close();
+ csvWriter = null;
+ }
+ driveLeftMotorA.set(ControlMode.PercentOutput, 0);
+ driveRightMotorA.set(ControlMode.PercentOutput, 0);
+ }
+ }
+
+ private Joystick joystick = new Joystick(0);
+
+ private List
+ * profiles/
+ * foo_left.csv
+ * foo_right.csv
+ * bar_left.csv
+ * bar_right.csv
+ *
+ *
+ * This would cause the {@code ProfileContainer} to load two profiles named {@code foo} and {@code
+ * bar}.
+ *
+ *