From 60694be2a3534a9959eab2ab832d9bc87730fa1b Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Thu, 31 May 2018 17:22:44 -0700 Subject: [PATCH 001/145] Add Kotlin to project --- .idea/modules/lib/lib_main.iml | 27 +++++++++++++++++++++++++++ .idea/modules/lib/lib_test.iml | 26 ++++++++++++++++++++++++++ .idea/modules/test/test_main.iml | 27 +++++++++++++++++++++++++++ .idea/modules/test/test_test.iml | 26 ++++++++++++++++++++++++++ build.gradle | 20 ++++++++++++++++++++ 5 files changed, 126 insertions(+) diff --git a/.idea/modules/lib/lib_main.iml b/.idea/modules/lib/lib_main.iml index 704530bf..cb94b5cd 100644 --- a/.idea/modules/lib/lib_main.iml +++ b/.idea/modules/lib/lib_main.iml @@ -1,5 +1,29 @@ + + + + + + + + + + + @@ -8,6 +32,9 @@ + + + diff --git a/.idea/modules/lib/lib_test.iml b/.idea/modules/lib/lib_test.iml index e22b654a..2adb9937 100644 --- a/.idea/modules/lib/lib_test.iml +++ b/.idea/modules/lib/lib_test.iml @@ -1,5 +1,28 @@ + + + + + + + + + + + @@ -7,6 +30,9 @@ + + + diff --git a/.idea/modules/test/test_main.iml b/.idea/modules/test/test_main.iml index 9ee23d83..4ae6e987 100644 --- a/.idea/modules/test/test_main.iml +++ b/.idea/modules/test/test_main.iml @@ -1,5 +1,29 @@ + + + + + + + + + + + @@ -9,6 +33,9 @@ + + + diff --git a/.idea/modules/test/test_test.iml b/.idea/modules/test/test_test.iml index 03bc96fb..5d07620a 100644 --- a/.idea/modules/test/test_test.iml +++ b/.idea/modules/test/test_test.iml @@ -1,5 +1,28 @@ + + + + + + + + + + + @@ -8,6 +31,9 @@ + + + diff --git a/build.gradle b/build.gradle index bce27b69..bc232216 100644 --- a/build.gradle +++ b/build.gradle @@ -1,9 +1,28 @@ +buildscript { + ext.kotlin_version = '1.2.31' + + repositories { + mavenCentral() + } + + dependencies { + classpath "org.jetbrains.kotlin:kotlin-gradle-plugin:$kotlin_version" + } +} + task wrapper(type: Wrapper) { gradleVersion = '4.4' } subprojects { apply plugin: "java" + apply plugin: "kotlin" + + compileKotlin { + kotlinOptions { + jvmTarget = "1.8" + } + } repositories { mavenCentral() @@ -11,5 +30,6 @@ subprojects { dependencies { compile 'org.jetbrains:annotations:15.0' + compile "org.jetbrains.kotlin:kotlin-stdlib-jre8:$kotlin_version" } } From 7d9a60b7a09b693a844da50e533ef846413952dd Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Thu, 31 May 2018 17:19:12 -0700 Subject: [PATCH 002/145] Create FollowProfile and FollowProfileFactory --- .../base/motionprofiling/FollowProfile.java | 170 +++++++++ .../motionprofiling/FollowProfileFactory.java | 333 ++++++++++++++++++ .../motionprofiling/SetpointConsumer.java | 30 ++ .../base/motionprofiling/package-info.java | 25 ++ 4 files changed, 558 insertions(+) create mode 100644 lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java create mode 100644 lib/src/main/java/org/team1540/base/motionprofiling/FollowProfileFactory.java create mode 100644 lib/src/main/java/org/team1540/base/motionprofiling/SetpointConsumer.java create mode 100644 lib/src/main/java/org/team1540/base/motionprofiling/package-info.java diff --git a/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java b/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java new file mode 100644 index 00000000..66048923 --- /dev/null +++ b/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java @@ -0,0 +1,170 @@ +package org.team1540.base.motionprofiling; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.command.Subsystem; +import jaci.pathfinder.Trajectory; +import jaci.pathfinder.Trajectory.Segment; +import java.util.Arrays; +import java.util.function.DoubleSupplier; +import org.jetbrains.annotations.NotNull; +import org.team1540.base.util.AsyncCommand; + +/** + * {@link edu.wpi.first.wpilibj.command.Command Command} to execute a motion profile. + *

+ * The profile-following algorithm used here is a derivative of the algorithm used by Team 2471. The + * output of a heading-based PI loop is added to the profile's position setpoint and passed to the + * motor controllers, with additional velocity, acceleration, and static friction feed-forwards (in + * line with Eli Barnett's drive + * characterization method) added via throttle bump. + *

+ * It is designed to use native Talon SRX position PID control with a throttle bump, but the + * provided {@link SetpointConsumer SetpointConsumers} could instead be used to control a RIO-side + * PID loop. + * + * @see FollowProfileFactory + */ +public class FollowProfile extends AsyncCommand { + + @NotNull + Trajectory left; + @NotNull + Trajectory right; + + @NotNull + private SetpointConsumer leftSetpointConsumer; + @NotNull + private SetpointConsumer rightSetpointConsumer; + + @NotNull + private DoubleSupplier headingSupplier; + + private double profTime; + + private double velCoeff; + private double velIntercept; + private double accelCoeff; + private double headingP; + private double headingI; + + private double gyroIAccum; + + /** + * Timer for the amount of time since profiling started (used to find current point) + */ + private Timer timer = new Timer(); + + /** + * Timer for the amount of time since the loop last ran (used for integral term) + */ + private Timer execTimer = new Timer(); + + /** + * Creates a {@code FollowProfile} command. + * + * @param subsystems The required subsystems for this command. + * @param leftSetpointConsumer The {@link SetpointConsumer} for the left-side motors. + * @param rightSetpointConsumer The {@link SetpointConsumer} for the right-side motors. + * @param headingSupplier A {@link DoubleSupplier} that returns the robot's current heading in + * radians from 0 to 2π. + * @param loopFreq The interval, in milliseconds, between profile loop execution. + * @param velCoeff The velocity coefficient (kV), in bump units per profile unit per second. + * @param velIntercept The velocity intercept (VIntercept), in bump units. + * @param accelCoeff The acceleration coefficient (kA), in bump units per profile unit per second + * squared. + * @param headingP The P coefficient for the heading controller, in profile units per radian. + * @param headingI The I coefficient for the heading controller, in profile units per + * radian-second. + */ + public FollowProfile(@NotNull Trajectory left, @NotNull Trajectory right, + @NotNull Subsystem[] subsystems, + @NotNull SetpointConsumer leftSetpointConsumer, + @NotNull SetpointConsumer rightSetpointConsumer, @NotNull DoubleSupplier headingSupplier, + long loopFreq, double velCoeff, double velIntercept, + double accelCoeff, double headingP, double headingI) { + super(loopFreq); + for (Subsystem subsystem : subsystems) { + requires(subsystem); + } + this.left = left; + this.right = right; + this.leftSetpointConsumer = leftSetpointConsumer; + this.rightSetpointConsumer = rightSetpointConsumer; + this.headingSupplier = headingSupplier; + this.velCoeff = velCoeff; + this.velIntercept = velIntercept; + this.accelCoeff = accelCoeff; + this.headingP = headingP; + this.headingI = headingI; + + profTime = Arrays.stream(left.segments).mapToDouble(s -> s.dt).sum(); + } + + private Segment getCurrentSegment(Trajectory trajectory, double currentTime) { + // Start from the current time and find the closest point. + int startIndex = Math.toIntExact(Math.round(currentTime / trajectory.segments[0].dt)); + + int length = trajectory.segments.length; + int index = startIndex; + if (startIndex >= length - 1) { + index = length - 1; + } + return trajectory.segments[index]; + } + + @Override + protected void runPeriodic() { + Segment leftSegment = getCurrentSegment(left, timer.get()); + Segment rightSegment = getCurrentSegment(right, timer.get()); + // check finish status + if (timer.get() > profTime) { + markAsFinished(); + } + + double heading = headingSupplier.getAsDouble(); + double headingTarget = leftSegment.heading; + + double error1 = heading - headingTarget; + double error2 = heading - headingTarget + 2 * Math.PI; + double error3 = heading - headingTarget - 2 * Math.PI; + + // basically magic https://stackoverflow.com/a/2007279 + double headingError = Math + .atan2(Math.sin(heading - headingTarget), Math.cos(heading - headingTarget)); + + gyroIAccum += headingError * execTimer.get(); + execTimer.reset(); + execTimer.start(); + + double gyroPOut = headingError * headingP; + double gyroIOut = gyroIAccum * headingI; + double leftVelFOut = velCoeff * leftSegment.velocity; + double rightVelFOut = velCoeff * rightSegment.velocity; + double leftVelInterceptOut = + leftSegment.velocity == 0 ? 0 : Math.copySign(velIntercept, leftSegment.velocity); + double rightVelInterceptOut = + rightSegment.velocity == 0 ? 0 : Math.copySign(velIntercept, rightSegment.velocity); + double leftAccelFOut = accelCoeff * leftSegment.acceleration; + double rightAccelFOut = accelCoeff * rightSegment.acceleration; + + leftSetpointConsumer.set(leftSegment.position - gyroPOut - gyroIOut, + leftVelFOut + leftVelInterceptOut + leftAccelFOut); + rightSetpointConsumer.set(rightSegment.position + gyroPOut + gyroIOut, + rightVelFOut + rightVelInterceptOut + rightAccelFOut); + } + + @Override + protected void runInitial() { + timer.start(); + execTimer.reset(); + gyroIAccum = 0; + } + + @Override + protected void runEnd() { + timer.stop(); + timer.reset(); + execTimer.stop(); + execTimer.reset(); + } +} diff --git a/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfileFactory.java b/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfileFactory.java new file mode 100644 index 00000000..cd90aaf1 --- /dev/null +++ b/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfileFactory.java @@ -0,0 +1,333 @@ +package org.team1540.base.motionprofiling; + +import edu.wpi.first.wpilibj.command.Subsystem; +import jaci.pathfinder.Pathfinder; +import jaci.pathfinder.Trajectory; +import java.io.File; +import java.util.function.DoubleSupplier; +import org.jetbrains.annotations.NotNull; + +/** + * Produces multiple similar {@link FollowProfile} commands for one subystem using common tuning + * values. In general, one {@code FollowProfileFactory} should be created for each subsystem to be + * motion-profiled. It can then be pre-loaded with common configuration and tuning values specific + * to a subsystem, and kept as a single instance to create any needed {@code FollowProfile} commands + * for that subsystem. + *

+ * All setters in this class follow a builder pattern; i.e they return an instance of the object + * they were called on. This allows for multiple set methods to be chained. + * + * @see FollowProfile + */ +public class FollowProfileFactory { + + @NotNull + private Subsystem[] subsystems; + @NotNull + private SetpointConsumer leftSetpointConsumer; + @NotNull + private SetpointConsumer rightSetpointConsumer; + + @NotNull + private DoubleSupplier headingSupplier = () -> 0; + + private long loopFreq = 20; + + private double velCoeff = 0; + private double velIntercept = 0; + private double accelCoeff = 0; + private double headingP = 0; + private double headingI = 0; + + /** + * Constructs a {@code FollowProfileFactory} with the provided left and right set functions and + * subsystems. + * + * @param left The {@link SetpointConsumer} for the left-side motors. + * @param right The {@link SetpointConsumer} for the right-side motors. + * @param subsystems The {@link Subsystem Subystems} that produced {@link FollowProfile} commands + * should require. + */ + public FollowProfileFactory(@NotNull SetpointConsumer left, @NotNull SetpointConsumer right, + @NotNull Subsystem... subsystems) { + this.leftSetpointConsumer = left; + this.rightSetpointConsumer = right; + this.subsystems = subsystems; + } + + /** + * Gets the subsystems used by produced {@link FollowProfile} commands. + * + * @return The required subsystems. Each of these will be declared as a requirement for each + * produced command using {@code requires()}. + */ + @NotNull + public Subsystem[] getSubsystems() { + return subsystems; + } + + /** + * Sets the subsystems required by produced {@link FollowProfile} commands. + * + * @param subsystems The required subsystems. Each of these will be declared as a requirement for + * each produced command using {@code requires()}. + * @return This {@code FollowProfileFactory} in a builder pattern. + */ + @NotNull + public FollowProfileFactory setSubsystems(@NotNull Subsystem... subsystems) { + this.subsystems = subsystems; + return this; + } + + /** + * Gets the {@link SetpointConsumer} for the left-side motors. + * + * @return The currently used {@link SetpointConsumer} for the left side. + */ + @NotNull + public SetpointConsumer getLeftSetpointConsumer() { + return leftSetpointConsumer; + } + + /** + * Sets the {@link SetpointConsumer} for the left-side motors. + * + * @param leftSetpointConsumer A {@link SetpointConsumer} that takes a setpoint in profile units + * and bump in bump units. + */ + @NotNull + public FollowProfileFactory setLeftSetpointConsumer( + @NotNull SetpointConsumer leftSetpointConsumer) { + this.leftSetpointConsumer = leftSetpointConsumer; + return this; + } + + /** + * Gets the {@link SetpointConsumer} for the left-side motors. + * + * @return The currently used {@link SetpointConsumer} for the right side. + */ + @NotNull + public SetpointConsumer getRightSetpointConsumer() { + return rightSetpointConsumer; + } + + /** + * @param rightSetpointConsumer A {@link SetpointConsumer} that takes a setpoint in profile units + * and bump in bump units and passes them to the drivetrain. + * @return This {@code FollowProfileFactory} in a builder pattern. + */ + @NotNull + public FollowProfileFactory setRightSetpointConsumer( + @NotNull SetpointConsumer rightSetpointConsumer) { + this.rightSetpointConsumer = rightSetpointConsumer; + return this; + } + + /** + * Gets the {@link DoubleSupplier} used for the input of the heading loop. + * + * @return The heading supplier. + */ + @NotNull + public DoubleSupplier getHeadingSupplier() { + return headingSupplier; + } + + /** + * Sets the {@link DoubleSupplier} used for the input of the heading loop. + * + * @param headingSupplier A {@link DoubleSupplier} that returns the robot's heading in radians + * from 0 to 2π. + * @return This {@code FollowProfileFactory} in a builder pattern. + */ + @NotNull + public FollowProfileFactory setHeadingSupplier(@NotNull DoubleSupplier headingSupplier) { + this.headingSupplier = headingSupplier; + return this; + } + + /** + * Gets the delay between profile loop execution. Defaults to 20ms. + * + * @return The time between profile loop execution, in milliseconds. + */ + public long getLoopFreq() { + return loopFreq; + } + + /** + * Sets the delay between profile loop execution. Defaults to 20ms. + * + * @param loopFreq The time between loop executions, in milliseconds. + * @return This {@code FollowProfileFactory} in a builder pattern. + */ + @NotNull + public FollowProfileFactory setLoopFreq(long loopFreq) { + this.loopFreq = loopFreq; + return this; + } + + /** + * Gets the velocity feed-forward coefficient. This is equivalent to the kV term in drive + * characterization. Defaults to 0 (no feed-forward). + * + * @return The velocity feed-forward coefficient, in bump units per profile unit per second. + */ + public double getVelCoeff() { + return velCoeff; + } + + /** + * Sets the velocity feed-forward coefficent. This is equivalent to the kV term in drive * + * characterization. Defaults to 0 (no feed-forward). + * + * @param velCoeff The velocity feed-forward coefficient, in bump units per profile unit per + * second. + * @return This {@code FollowProfileFactory} in a builder pattern. + */ + @NotNull + public FollowProfileFactory setVelCoeff(double velCoeff) { + this.velCoeff = velCoeff; + return this; + } + + /** + * Gets the velocity intercept, or VIntercept. (See Eli Barnett's drive characterization paper for + * an explanation of why this is needed.) + * + * @return The velocity intercept, in bump units. + */ + public double getVelIntercept() { + return velIntercept; + } + + /** + * Sets the velocity intercept, or VIntercept. (See Eli Barnett's drive characterization paper for + * an explanation of why this is needed.) Defaults to 0. + * + * @param velIntercept The velocity intercept, in bump units. + * @return This {@code FollowProfileFactory} in a builder pattern. + */ + @NotNull + public FollowProfileFactory setVelIntercept(double velIntercept) { + this.velIntercept = velIntercept; + return this; + } + + /** + * Gets the acceleration feed-forward. This is equivalent to the kA term in drive + * characterization. Defaults to 0 (no feed-forward). + * + * @return The currently set acceleration coefficient, in bump units per profile unit per second + * squared. + */ + public double getAccelCoeff() { + return accelCoeff; + } + + /** + * Sets the acceleration feed-forward. This is equivalent to the kA term in drive + * characterization. Defaults to 0 (no feed-forward). + * + * @param accelCoeff The acceleration coefficient, in bump units per profile unit per second + * squared. + * @return This {@code FollowProfileFactory} in a builder pattern. + */ + @NotNull + public FollowProfileFactory setAccelCoeff(double accelCoeff) { + this.accelCoeff = accelCoeff; + return this; + } + + /** + * Gets the P coefficient for the heading PI loop. Defaults to 0. + * + * @return The currently set P coefficient, in profile units per radian. + */ + public double getHeadingP() { + return headingP; + } + + /** + * Sets the P coefficient for the heading PI loop. Defaults to 0. + * + * @param headingP The P coefficient, in profile units per radian. + * @return This {@code FollowProfileFactory} in a builder pattern. + */ + @NotNull + public FollowProfileFactory setHeadingP(double headingP) { + this.headingP = headingP; + return this; + } + + + /** + * Gets the I coefficient for the heading PI loop. Defaults to 0. + * + * @return The currently set I coefficient, in profile units per radian-second. + */ + public double getHeadingI() { + return headingI; + } + + /** + * Sets the I coefficient for the heading PI loop. Defaults to 0. + * + * @param headingI The I coefficient, in profile units per radian-second. + * @return This {@code FollowProfileFactory} in a builder pattern. + */ + @NotNull + public FollowProfileFactory setHeadingI(double headingI) { + this.headingI = headingI; + return this; + } + + /** + * Creates a new {@link FollowProfile} command to follow the provided profile. This is a + * convienience method that calls {@link #create(File, File)} with two files as described below. + *

+ * With a given profile name, this method will attempt to load a profile with name "profile" from + * two files; the left side from /home/lvuser/profiles/profile_left.csv and the right side from + * /home/lvuser/profiles/profile_right.csv. + * + * @param profileName The name of the profile. + * @return A new {@link FollowProfile} command with the previously configured settings following + * the provided profile. + */ + @NotNull + public FollowProfile create(@NotNull String profileName) { + return create(new File("/home/lvuser/profiles/" + profileName + "_left.csv"), + new File("/home/lvuser/profiles/" + profileName + "_right.csv")); + } + + /** + * Creates a new {@link FollowProfile} command to follow the provided profile. This is a + * convinience method that calls {@link #create(Trajectory, Trajectory)} after loading the + * trajectories using {@link Pathfinder#readFromCSV(File)}. + * + * @param leftFile The file to load the left profile from; should be a pathfinder-formatted CSV. + * @param rightFile The file to load the right profile from; should be a pathfinder-formatted + * CSV. + * @return A new {@link FollowProfile} command with the previously configured settings following * + * the provided profile. + */ + @NotNull + public FollowProfile create(@NotNull File leftFile, @NotNull File rightFile) { + return create(Pathfinder.readFromCSV(leftFile), Pathfinder.readFromCSV(rightFile)); + } + + /** + * Creates a new {@link FollowProfile} command to follow the provided profile. + * + * @param left The trajectory for the left side. + * @param right The trajectory for the right side. + * @return A new {@link FollowProfile} command with the previously configured settings following * + * the provided profile. + */ + @NotNull + public FollowProfile create(@NotNull Trajectory left, @NotNull Trajectory right) { + return new FollowProfile(left, right, subsystems, leftSetpointConsumer, rightSetpointConsumer, + headingSupplier, loopFreq, velCoeff, velIntercept, accelCoeff, headingP, headingI); + } +} diff --git a/lib/src/main/java/org/team1540/base/motionprofiling/SetpointConsumer.java b/lib/src/main/java/org/team1540/base/motionprofiling/SetpointConsumer.java new file mode 100644 index 00000000..d43730b9 --- /dev/null +++ b/lib/src/main/java/org/team1540/base/motionprofiling/SetpointConsumer.java @@ -0,0 +1,30 @@ +package org.team1540.base.motionprofiling; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.DemandType; + +/** + * Functional interface to pass commanded values from {@link FollowProfile} to the motors. + * + * @see FollowProfile + */ +@FunctionalInterface +public interface SetpointConsumer { + + /** + * Sets the setpoint of the motors to be profiled. Usually implemented using the {@link + * com.ctre.phoenix.motorcontrol.can.TalonSRX#set(ControlMode, double, DemandType, double) + * set(ControlMode, double, DemandType, double)} method of CTRE's {@link + * com.ctre.phoenix.motorcontrol.can.TalonSRX TalonSRX}, with the {@code ControlMode} set to + * {@link ControlMode#Position} and the {@code DemandType} set to {@link + * DemandType#ArbitraryFeedForward}. + *

+ * See the {@linkplain org.team1540.base.motionprofiling package documentation} for an explanation + * of profile units, bump units, etc.. + * + * @param setpoint The position PID setpoint, in profile units. (Unit conversion to Talon SRX + * native units etc. should be performed within this method.) + * @param bump The throttle bump to apply, in bump units. + */ + void set(double setpoint, double bump); +} diff --git a/lib/src/main/java/org/team1540/base/motionprofiling/package-info.java b/lib/src/main/java/org/team1540/base/motionprofiling/package-info.java new file mode 100644 index 00000000..92d73b7b --- /dev/null +++ b/lib/src/main/java/org/team1540/base/motionprofiling/package-info.java @@ -0,0 +1,25 @@ +/** + * Utilities for executing motion profiles. + * + *

Summary

+ * + * This package's main business logic is in the {@link org.team1540.base.motionprofiling.FollowProfile} + * command, which follows a given Pathfinder motion profile. (A technical explanation of the + * profile-following algorithm can be found in the {@link org.team1540.base.motionprofiling.FollowProfile} + * documentation.) Additionally, this package contains {@link org.team1540.base.motionprofiling.FollowProfileFactory}, + * a class to create multiple {@code FollowProfile} instances using common configuration values (for + * example, multiple autonomous routines for a drivetrain). + * + *

A Note on Units

+ * Many problems relating to motion profiling stem from units not being converted properly. As such, + * to simplify things, this package is unit-agnostic (with the exception of heading which is + * required to be in radians) and does no unit conversion on its own, instead asking that provided + * coefficients incorporate necessary unit conversion. When used in package documentation, the term + * profile units means the units used in motion profiles given to the {@link + * org.team1540.base.motionprofiling.FollowProfile} command (usually inches, feet or meters); the + * term native units means the units passed to the motor controllers (usually in terms of + * raw encoder counts); and the term bump units means the units passed to the throttle-bump + * parameter of {@link org.team1540.base.motionprofiling.SetpointConsumer#set(double, double) + * SetpointConsumer.set()} (usually percentage of motor throttle). + */ +package org.team1540.base.motionprofiling; From 1b673037351e0fbf17708a937fd1c9f6ec27a541 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Thu, 31 May 2018 17:19:23 -0700 Subject: [PATCH 003/145] Deprecate old profile follower --- .../base/motionprofiling/MotionProfilingProperties.java | 6 +++++- .../team1540/base/motionprofiling/RunMotionProfiles.java | 2 ++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/lib/src/main/java/org/team1540/base/motionprofiling/MotionProfilingProperties.java b/lib/src/main/java/org/team1540/base/motionprofiling/MotionProfilingProperties.java index 6b45d381..91aa427d 100644 --- a/lib/src/main/java/org/team1540/base/motionprofiling/MotionProfilingProperties.java +++ b/lib/src/main/java/org/team1540/base/motionprofiling/MotionProfilingProperties.java @@ -4,6 +4,10 @@ import java.util.function.DoubleConsumer; import java.util.function.DoubleSupplier; +/** + * @deprecated Replaced by the {@link FollowProfile}-based system. + */ +@Deprecated public class MotionProfilingProperties { private double encoderTicksPerUnit = 1023 * 0.1 * Math.PI; @@ -113,4 +117,4 @@ public void setGetEncoderPositionFunction(DoubleSupplier getEncoderPositionFunct this.getEncoderPositionFunction = getEncoderPositionFunction; } -} \ No newline at end of file +} diff --git a/lib/src/main/java/org/team1540/base/motionprofiling/RunMotionProfiles.java b/lib/src/main/java/org/team1540/base/motionprofiling/RunMotionProfiles.java index 52bf0077..4c1b773d 100644 --- a/lib/src/main/java/org/team1540/base/motionprofiling/RunMotionProfiles.java +++ b/lib/src/main/java/org/team1540/base/motionprofiling/RunMotionProfiles.java @@ -9,7 +9,9 @@ /** * Executes a set of motion profiles (with respective properties.) + * @deprecated Replaced by the {@link FollowProfile}-based system. */ +@Deprecated public class RunMotionProfiles extends Command { private int slotId = 0; From 1105603d7c2ff6125f6eca646d0539640faf8337 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Thu, 31 May 2018 17:19:33 -0700 Subject: [PATCH 004/145] Create MotionProfileUtils --- .../motionprofiling/MotionProfileUtils.java | 37 +++++++++++++++++++ 1 file changed, 37 insertions(+) create mode 100644 lib/src/main/java/org/team1540/base/motionprofiling/MotionProfileUtils.java diff --git a/lib/src/main/java/org/team1540/base/motionprofiling/MotionProfileUtils.java b/lib/src/main/java/org/team1540/base/motionprofiling/MotionProfileUtils.java new file mode 100644 index 00000000..dc16533b --- /dev/null +++ b/lib/src/main/java/org/team1540/base/motionprofiling/MotionProfileUtils.java @@ -0,0 +1,37 @@ +package org.team1540.base.motionprofiling; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.DemandType; +import com.ctre.phoenix.motorcontrol.can.BaseMotorController; + +public class MotionProfileUtils { + + private MotionProfileUtils() { + } + + /** + * Create a {@link SetpointConsumer} that sets the setpoint of the provided CTRE motor controller + * with no adjustment to the position setpoint. + * + * @param motorController The motor controller. + * @return A {@code SetpointConsumer} to use for motion profiling. + */ + public static SetpointConsumer createSetpointConsumer(BaseMotorController motorController) { + return createSetpointConsumer(motorController, 1); + } + + /** + * Create a {@link SetpointConsumer} that sets the setpoint of the provided CTRE motor + * controller, multiplying the position setpoint by the adjustmant. + * + * @param motorController The motor controller. + * @return A {@code SetpointConsumer} to use for motion profiling. + */ + public static SetpointConsumer createSetpointConsumer(BaseMotorController motorController, + double adjustment) { + return (setpoint, bump) -> + motorController + .set(ControlMode.Position, setpoint * adjustment, DemandType.ArbitraryFeedForward, + bump); + } +} From 215cb27cddde96e42c9b4ba3c46bca002f6645a8 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Thu, 31 May 2018 17:23:52 -0700 Subject: [PATCH 005/145] Create MotionProfileTesting --- .../base/testing/MotionProfileTesting.kt | 246 ++++++++++++++++++ 1 file changed, 246 insertions(+) create mode 100644 test/src/main/java/org/team1540/base/testing/MotionProfileTesting.kt diff --git a/test/src/main/java/org/team1540/base/testing/MotionProfileTesting.kt b/test/src/main/java/org/team1540/base/testing/MotionProfileTesting.kt new file mode 100644 index 00000000..a9a2c195 --- /dev/null +++ b/test/src/main/java/org/team1540/base/testing/MotionProfileTesting.kt @@ -0,0 +1,246 @@ +package org.team1540.base.testing + +import com.ctre.phoenix.motorcontrol.ControlMode +import com.ctre.phoenix.motorcontrol.FeedbackDevice +import com.kauailabs.navx.frc.AHRS +import edu.wpi.first.wpilibj.IterativeRobot +import edu.wpi.first.wpilibj.SPI +import edu.wpi.first.wpilibj.command.Scheduler +import edu.wpi.first.wpilibj.command.Subsystem +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard +import jaci.pathfinder.Pathfinder +import jaci.pathfinder.Trajectory +import jaci.pathfinder.Waypoint +import jaci.pathfinder.modifiers.TankModifier +import org.team1540.base.motionprofiling.FollowProfileFactory +import org.team1540.base.motionprofiling.MotionProfileUtils +import org.team1540.base.preferencemanager.Preference +import org.team1540.base.preferencemanager.PreferenceManager +import org.team1540.base.wrappers.ChickenTalon +import java.util.function.DoubleSupplier + +class MotionProfileTestingRobot : IterativeRobot() { + lateinit var factory: FollowProfileFactory + + val navx = AHRS(SPI.Port.kMXP) + + @JvmField + @Preference("kV") + var kV = 0.0 + + @JvmField + @Preference("kA") + var kA = 0.0 + + @JvmField + @Preference("VIntercept") + var vIntercept = 0.0 + + @JvmField + @Preference("MP Loop Freq") + var loopFreqMs = 0L + + @JvmField + @Preference("MP Heading P") + var hdgP = 0.0 + + @JvmField + @Preference("MP Heading I") + var hdgI = 0.0 + + @JvmField + @Preference("MP Drive P") + var driveP = 0.0 + + @JvmField + @Preference("MP Drive D") + var driveD = 0.0 + + @JvmField + @Preference("MP Delta-T") + var deltaT = 0.0 + + @JvmField + @Preference("MP Max Vel") + var maxVel = 0.0 + + @JvmField + @Preference("MP Max Accel") + var maxAccel = 0.0 + + @JvmField + @Preference("MP Max Jerk") + var maxJerk = 0.0 + + @JvmField + @Preference("Wheelbase") + var wheelbase = 0.0 + + @JvmField + @Preference("Drive TPU") + var tpu = 0.0 + + override fun robotInit() { + PreferenceManager.getInstance().add(this) + DriveTrain + + DriveTrain.brake = false + SmartDashboard.setDefaultNumber("Test Profile X", 0.0) + SmartDashboard.setDefaultNumber("Test Profile Y", 0.0) + SmartDashboard.setDefaultNumber("Test Profile Angle", 0.0) + + factory = FollowProfileFactory( + MotionProfileUtils.createSetpointConsumer(DriveTrain.left1, tpu), + MotionProfileUtils.createSetpointConsumer(DriveTrain.left2, tpu), + DriveTrain + ).apply { + velIntercept = vIntercept + velCoeff = kV + accelCoeff = kA + loopFreq = loopFreqMs + headingP = hdgP + headingI = hdgI + headingSupplier = DoubleSupplier { + Math.toRadians(navx.yaw.let { if (it < 0) it + 180 else it }.toDouble()) + } + } + } + + override fun autonomousPeriodic() { + } + + override fun robotPeriodic() { + Scheduler.getInstance().run() + if (DriveTrain.p != driveP) DriveTrain.p = driveP + if (DriveTrain.d != driveP) DriveTrain.d = driveD + } + + override fun disabledInit() { + DriveTrain.brake = false + } + + override fun disabledPeriodic() { + } + + override fun teleopPeriodic() { + } + + override fun autonomousInit() { + DriveTrain.brake = true + factory.apply { + velIntercept = vIntercept + velCoeff = kV + accelCoeff = kA + loopFreq = loopFreqMs + headingP = hdgP + headingI = hdgI + leftSetpointConsumer = MotionProfileUtils.createSetpointConsumer(DriveTrain.left1, tpu) + rightSetpointConsumer = MotionProfileUtils.createSetpointConsumer(DriveTrain.right1, tpu) + } + + // generate new trajectory + val (leftTrajectory, rightTrajectory) = TankModifier(Pathfinder.generate(arrayOf( + Waypoint(0.0, 0.0, 0.0), + Waypoint( + SmartDashboard.getNumber("Test Profile X", 0.0), + SmartDashboard.getNumber("Test Profile Y", 0.0), + SmartDashboard.getNumber("Test Profile Angle", 0.0) + ) + ), Trajectory.Config( + Trajectory.FitMethod.HERMITE_CUBIC, + Trajectory.Config.SAMPLES_FAST, + deltaT, + maxVel, + maxAccel, + maxJerk + ))).modify(wheelbase).trajectories + + val command = factory.create(leftTrajectory, rightTrajectory) + + command.start() + } + + override fun teleopInit() { + DriveTrain.brake = true + } + +} + +private val TankModifier.trajectories get() = leftTrajectory to rightTrajectory + +private object DriveTrain : Subsystem() { + val left1 = ChickenTalon(1) + val left2 = ChickenTalon(2) + val left3 = ChickenTalon(3) + + val right1 = ChickenTalon(4) + val right2 = ChickenTalon(5) + val right3 = ChickenTalon(6) + + val masters = arrayOf(left1, right1) + val lefts = arrayOf(left1, left2, left3) + val rights = arrayOf(right1, right2, right3) + + val all = arrayOf(*lefts, *rights) + + var p = 0.0 + set(value) { + field = value + for (talon in masters) talon.config_kP(0, value) + } + + var d = 0.0 + set(value) { + field = value + for (talon in masters) talon.config_kD(0, value) + } + + var brake = true + set(value) { + field = value + for (talon in all) talon.setBrake(value) + } + + override fun initDefaultCommand() { + } + + fun reset() { + left1.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder) + right1.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder) + + left1.setSensorPhase(true) + + for (talon in lefts) { + talon.inverted = false + } + + right1.setSensorPhase(true) + + for (talon in rights) { + talon.inverted = true + } + + left2.set(ControlMode.Follower, left1.deviceID.toDouble()) + left3.set(ControlMode.Follower, left1.deviceID.toDouble()) + + right2.set(ControlMode.Follower, right1.deviceID.toDouble()) + right3.set(ControlMode.Follower, right1.deviceID.toDouble()) + + for (talon in all) { + talon.setBrake(brake) + } + + for (talon in masters) { + talon.config_kP(0, p) + talon.config_kI(0, 0.0) + talon.config_kD(0, d) + talon.config_kF(0, 0.0) + talon.config_IntegralZone(0, 0) + } + } + + init { + reset() + } +} + From 87fea3492c71eb21d3cdf26f72a8ceb9b452fd9e Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Thu, 31 May 2018 17:24:19 -0700 Subject: [PATCH 006/145] Create utility robots package for characterization --- .../robots/VelocityCharacterizationRobot.java | 138 ++++++++++++++++++ .../base/util/robots/package-info.java | 4 + 2 files changed, 142 insertions(+) create mode 100644 lib/src/main/java/org/team1540/base/util/robots/VelocityCharacterizationRobot.java create mode 100644 lib/src/main/java/org/team1540/base/util/robots/package-info.java diff --git a/lib/src/main/java/org/team1540/base/util/robots/VelocityCharacterizationRobot.java b/lib/src/main/java/org/team1540/base/util/robots/VelocityCharacterizationRobot.java new file mode 100644 index 00000000..08a13f7c --- /dev/null +++ b/lib/src/main/java/org/team1540/base/util/robots/VelocityCharacterizationRobot.java @@ -0,0 +1,138 @@ +package org.team1540.base.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.command.Scheduler; +import java.io.File; +import java.io.FileNotFoundException; +import java.io.PrintWriter; +import org.team1540.base.preferencemanager.Preference; +import org.team1540.base.preferencemanager.PreferenceManager; +import org.team1540.base.wrappers.ChickenTalon; + +/** + * Self-contained robot class to characterize a drivetrain's velocity 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 a quasi-static velocity characterization as described in Eli Barnett's paper "FRC + * Drivetrain Characterization" until the button is released. + */ +public class VelocityCharacterizationRobot extends IterativeRobot { + + 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 Joystick joystick = new Joystick(0); + + @Preference("Voltage Ramp Rate") + public double rampRate = 0.020833333; + + private double appliedOutput = 0; + + public long lastTime; + + @Override + public void robotInit() { + PreferenceManager.getInstance().add(this); + reset(); + } + + @Override + public void robotPeriodic() { + if (!isOperatorControl() && csvWriter != null) { + csvWriter.close(); + csvWriter = null; + } + Scheduler.getInstance().run(); + } + + @Override + public void teleopPeriodic() { + if (joystick.getRawButton(1)) { // if button A is pressed + if (csvWriter == null) { + // create a new CSV writer, reset everything + try { + csvWriter = new PrintWriter(new File( + "/home/lvuser/dtmeasure/measure-" + System.currentTimeMillis() + ".csv")); + csvWriter.println("lvoltage,lvelocity,rvoltage,rvelocity"); + } catch (FileNotFoundException e) { + throw new RuntimeException(e); + } + appliedOutput = 0; + driveLeftMotorA.set(ControlMode.PercentOutput, 0); + driveRightMotorA.set(ControlMode.PercentOutput, 0); + } else { + + csvWriter.println(driveLeftMotorA.getMotorOutputVoltage() + "," + + driveLeftMotorA.getSelectedSensorVelocity() + "," + + driveRightMotorA.getMotorOutputVoltage() + "," + + driveRightMotorA.getSelectedSensorVelocity()); + + appliedOutput += rampRate * ((System.currentTimeMillis() - lastTime) / 1000.0); + lastTime = System.currentTimeMillis(); + driveLeftMotorA.set(ControlMode.PercentOutput, appliedOutput); + driveRightMotorA.set(ControlMode.PercentOutput, appliedOutput); + } + } else { + appliedOutput = 0; + if (csvWriter != null) { + csvWriter.close(); + csvWriter = null; + } + driveLeftMotorA.set(ControlMode.PercentOutput, 0); + driveRightMotorA.set(ControlMode.PercentOutput, 0); + } + lastTime = System.currentTimeMillis(); + } + + public void reset() { + driveLeftMotorA.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); + driveRightMotorA.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); + + driveLeftMotorA.setSensorPhase(true); + + for (ChickenTalon talon : driveLeftMotors) { + talon.setInverted(false); + } + + driveRightMotorA.setSensorPhase(true); + + for (ChickenTalon talon : driveRightMotors) { + talon.setInverted(true); + } + + driveLeftMotorB.set(ControlMode.Follower, driveLeftMotorA.getDeviceID()); + driveLeftMotorC.set(ControlMode.Follower, driveLeftMotorA.getDeviceID()); + + driveRightMotorB.set(ControlMode.Follower, driveRightMotorA.getDeviceID()); + driveRightMotorC.set(ControlMode.Follower, driveRightMotorA.getDeviceID()); + + for (ChickenTalon talon : driveMotorAll) { + talon.setBrake(true); + } + + for (ChickenTalon talon : driveMotorAll) { + talon.configClosedloopRamp(0); + talon.configOpenloopRamp(0); + talon.configPeakOutputForward(1); + talon.configPeakOutputReverse(-1); + talon.enableCurrentLimit(false); + } + } +} diff --git a/lib/src/main/java/org/team1540/base/util/robots/package-info.java b/lib/src/main/java/org/team1540/base/util/robots/package-info.java new file mode 100644 index 00000000..5b1f2669 --- /dev/null +++ b/lib/src/main/java/org/team1540/base/util/robots/package-info.java @@ -0,0 +1,4 @@ +/** + * Self-contained robots to perform various checkout tasks. + */ +package org.team1540.base.util.robots; From 879af7b5cb1dbc557dfd40378c12014631ad05f5 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Fri, 1 Jun 2018 08:04:46 -0700 Subject: [PATCH 007/145] Add AccelerationCharacterizationRobot --- .../AccelerationCharacterizationRobot.java | 217 ++++++++++++++++++ 1 file changed, 217 insertions(+) create mode 100644 lib/src/main/java/org/team1540/base/util/robots/AccelerationCharacterizationRobot.java diff --git a/lib/src/main/java/org/team1540/base/util/robots/AccelerationCharacterizationRobot.java b/lib/src/main/java/org/team1540/base/util/robots/AccelerationCharacterizationRobot.java new file mode 100644 index 00000000..4bd95c8d --- /dev/null +++ b/lib/src/main/java/org/team1540/base/util/robots/AccelerationCharacterizationRobot.java @@ -0,0 +1,217 @@ +package org.team1540.base.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.team1540.base.preferencemanager.Preference; +import org.team1540.base.preferencemanager.PreferenceManager; +import org.team1540.base.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. + */ +public class AccelerationCharacterizationRobot extends IterativeRobot { + + @Preference("kV") + public double kV; + @Preference("VIntercept") + public double vIntercept; + + + 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; + } + Scheduler.getInstance().run(); + if (leftVelocities.size() == 4) { + 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() == 4) { + 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 + try { + 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() == 4) { + 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, SmartDashboard.getNumber("Setpoint", 0.6)); + driveRightMotorA.set(ControlMode.PercentOutput, SmartDashboard.getNumber("Setpoint", 0.6)); + } + } 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 leftVelocities = new LinkedList<>(); + private List leftVoltages = new LinkedList<>(); + private List rightVelocities = new LinkedList<>(); + private List rightVoltages = new LinkedList<>(); + private List times = new LinkedList<>(); + + @Override + public void robotInit() { + PreferenceManager.getInstance().add(this); + reset(); + notifier.startPeriodic(0.01); + + SmartDashboard.setDefaultNumber("Setpoint", 0.6); + } + + @Override + public void robotPeriodic() { + Scheduler.getInstance().run(); // process preferences + } + + @Override + public void teleopPeriodic() { + + } + + @Override + public void teleopInit() { + for (ChickenTalon talon : driveMotorAll) { + talon.setBrake(true); + } + } + + @Override + public void disabledInit() { + for (ChickenTalon talon : driveMotorAll) { + talon.setBrake(false); + } + } + + @SuppressWarnings("Duplicates") + public void reset() { + driveLeftMotorA.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); + driveRightMotorA.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); + + driveLeftMotorA.setSensorPhase(true); + + for (ChickenTalon talon : driveLeftMotors) { + talon.setInverted(false); + } + + driveRightMotorA.setSensorPhase(true); + + for (ChickenTalon talon : driveRightMotors) { + talon.setInverted(true); + } + + driveLeftMotorB.set(ControlMode.Follower, driveLeftMotorA.getDeviceID()); + driveLeftMotorC.set(ControlMode.Follower, driveLeftMotorA.getDeviceID()); + + driveRightMotorB.set(ControlMode.Follower, driveRightMotorA.getDeviceID()); + driveRightMotorC.set(ControlMode.Follower, driveRightMotorA.getDeviceID()); + + for (ChickenTalon talon : driveMotorAll) { + talon.setBrake(true); + } + + for (ChickenTalon talon : driveMotorAll) { + talon.configClosedloopRamp(0); + talon.configOpenloopRamp(0); + talon.configPeakOutputForward(1); + talon.configPeakOutputReverse(-1); + talon.enableCurrentLimit(false); + } + } + + private static double bestFitSlope(List xVals, List yVals) { + double avgX = xVals.stream().mapToDouble(x -> x).sum() / xVals.size(); + double avgY = yVals.stream().mapToDouble(y -> y).sum() / yVals.size(); + + double sumXY = 0; + double sumXSquared = 0; + for (int i = 0; i < xVals.size(); i++) { + sumXY += (xVals.get(i) - avgX) * (yVals.get(i) - avgY); + sumXSquared += (xVals.get(i) - avgX) * (xVals.get(i) - avgX); + } + + return sumXY / sumXSquared; + } +} From 09d570379fe10226281525b53ec57c1071c3c39d Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 4 Jun 2018 12:19:35 -0700 Subject: [PATCH 008/145] Update test robot package docs --- .../main/java/org/team1540/base/util/robots/package-info.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/lib/src/main/java/org/team1540/base/util/robots/package-info.java b/lib/src/main/java/org/team1540/base/util/robots/package-info.java index 5b1f2669..8037b312 100644 --- a/lib/src/main/java/org/team1540/base/util/robots/package-info.java +++ b/lib/src/main/java/org/team1540/base/util/robots/package-info.java @@ -1,4 +1,5 @@ /** - * Self-contained robots to perform various checkout tasks. + * Self-contained robots to perform various tasks. To load one of these robots, set your + * robot class (in build.gradle) to org.team1540.base.util.robots.<robot name>. */ package org.team1540.base.util.robots; From 130183a159bac4e83018cd765667f428d6f99ece Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 4 Jun 2018 12:21:56 -0700 Subject: [PATCH 009/145] Set motor inversions through preferences --- .../AccelerationCharacterizationRobot.java | 26 ++++++++++++++----- 1 file changed, 19 insertions(+), 7 deletions(-) diff --git a/lib/src/main/java/org/team1540/base/util/robots/AccelerationCharacterizationRobot.java b/lib/src/main/java/org/team1540/base/util/robots/AccelerationCharacterizationRobot.java index 4bd95c8d..d34e0685 100644 --- a/lib/src/main/java/org/team1540/base/util/robots/AccelerationCharacterizationRobot.java +++ b/lib/src/main/java/org/team1540/base/util/robots/AccelerationCharacterizationRobot.java @@ -22,7 +22,9 @@ * 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. + * 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 { @@ -31,16 +33,24 @@ public class AccelerationCharacterizationRobot extends IterativeRobot { @Preference("VIntercept") public double vIntercept; + @Preference("Invert Left Motor") + public boolean invertLeft = false; + @Preference("Invert Right Motor") + public boolean invertRight = true; + 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[] 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[] 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; @@ -87,6 +97,7 @@ private void run() { if (joystick.getRawButton(1)) { // if button A is pressed if (csvWriter == null) { // create a new CSV writer, reset everything + reset(); try { csvWriter = new PrintWriter(new File( "/home/lvuser/dtmeasure/measureaccel-" + System.currentTimeMillis() + ".csv")); @@ -138,7 +149,7 @@ public void robotInit() { reset(); notifier.startPeriodic(0.01); - SmartDashboard.setDefaultNumber("Setpoint", 0.6); + SmartDashboard.setDefaultNumber("Setpoint", 0.6); } @Override @@ -153,6 +164,7 @@ public void teleopPeriodic() { @Override public void teleopInit() { + reset(); for (ChickenTalon talon : driveMotorAll) { talon.setBrake(true); } @@ -173,13 +185,13 @@ public void reset() { driveLeftMotorA.setSensorPhase(true); for (ChickenTalon talon : driveLeftMotors) { - talon.setInverted(false); + talon.setInverted(invertLeft); } driveRightMotorA.setSensorPhase(true); for (ChickenTalon talon : driveRightMotors) { - talon.setInverted(true); + talon.setInverted(invertRight); } driveLeftMotorB.set(ControlMode.Follower, driveLeftMotorA.getDeviceID()); From a2ccf608344631e7100d1ade1babb0ff6d37e08f Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 4 Jun 2018 12:27:26 -0700 Subject: [PATCH 010/145] Add custom inversion to VelocityCharacterizationRobot --- .../util/robots/VelocityCharacterizationRobot.java | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/lib/src/main/java/org/team1540/base/util/robots/VelocityCharacterizationRobot.java b/lib/src/main/java/org/team1540/base/util/robots/VelocityCharacterizationRobot.java index 08a13f7c..c6628869 100644 --- a/lib/src/main/java/org/team1540/base/util/robots/VelocityCharacterizationRobot.java +++ b/lib/src/main/java/org/team1540/base/util/robots/VelocityCharacterizationRobot.java @@ -43,6 +43,11 @@ public class VelocityCharacterizationRobot extends IterativeRobot { @Preference("Voltage Ramp Rate") public double rampRate = 0.020833333; + @Preference("Invert Left Motor") + public boolean invertLeft = false; + @Preference("Invert Right Motor") + public boolean invertRight = true; + private double appliedOutput = 0; public long lastTime; @@ -67,6 +72,7 @@ public void teleopPeriodic() { if (joystick.getRawButton(1)) { // if button A is pressed if (csvWriter == null) { // create a new CSV writer, reset everything + reset(); try { csvWriter = new PrintWriter(new File( "/home/lvuser/dtmeasure/measure-" + System.currentTimeMillis() + ".csv")); @@ -101,6 +107,7 @@ public void teleopPeriodic() { lastTime = System.currentTimeMillis(); } + @SuppressWarnings("Duplicates") public void reset() { driveLeftMotorA.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); driveRightMotorA.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); @@ -108,13 +115,13 @@ public void reset() { driveLeftMotorA.setSensorPhase(true); for (ChickenTalon talon : driveLeftMotors) { - talon.setInverted(false); + talon.setInverted(invertLeft); } driveRightMotorA.setSensorPhase(true); for (ChickenTalon talon : driveRightMotors) { - talon.setInverted(true); + talon.setInverted(invertRight); } driveLeftMotorB.set(ControlMode.Follower, driveLeftMotorA.getDeviceID()); From 1d149f38cdfa9f59f6a54967368a2c84ab153696 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Tue, 5 Jun 2018 09:49:38 -0700 Subject: [PATCH 011/145] Move Kotlin files to src/main/kotlin --- .idea/modules/test/test_main.iml | 1 + .../org/team1540/base/testing/MotionProfileTesting.kt | 0 2 files changed, 1 insertion(+) rename test/src/main/{java => kotlin}/org/team1540/base/testing/MotionProfileTesting.kt (100%) diff --git a/.idea/modules/test/test_main.iml b/.idea/modules/test/test_main.iml index 4ae6e987..431c3a29 100644 --- a/.idea/modules/test/test_main.iml +++ b/.idea/modules/test/test_main.iml @@ -29,6 +29,7 @@ + diff --git a/test/src/main/java/org/team1540/base/testing/MotionProfileTesting.kt b/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt similarity index 100% rename from test/src/main/java/org/team1540/base/testing/MotionProfileTesting.kt rename to test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt From 99521b868bbd3ac3286241e2d47870d40380f0e7 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 1 Oct 2018 15:51:12 -0700 Subject: [PATCH 012/145] Create MotionProfile datastructure This is pretty much a straight clone of Pathfinder's Trajectory class; this just moves it inside the library so that we aren't affected by API changes. --- .../base/motionprofiling/MotionProfile.java | 149 ++++++++++++++++++ 1 file changed, 149 insertions(+) create mode 100644 lib/src/main/java/org/team1540/base/motionprofiling/MotionProfile.java diff --git a/lib/src/main/java/org/team1540/base/motionprofiling/MotionProfile.java b/lib/src/main/java/org/team1540/base/motionprofiling/MotionProfile.java new file mode 100644 index 00000000..ef08cbea --- /dev/null +++ b/lib/src/main/java/org/team1540/base/motionprofiling/MotionProfile.java @@ -0,0 +1,149 @@ +package org.team1540.base.motionprofiling; + +import java.util.Objects; +import org.jetbrains.annotations.Contract; +import org.jetbrains.annotations.NotNull; +import org.jetbrains.annotations.Nullable; + +/** + * A sequence of {@link Point Points} that can be executed by a {@link FollowProfile}. + */ +public class MotionProfile { + + /** + * The {@link Point Points} in the motion profile. + */ + @NotNull + public final Point[] points; + + /** + * Create a new {@link MotionProfile} from an array of points. + * + * @param points The points to use. + */ + public MotionProfile(@NotNull Point[] points) { + this.points = points; + } + + /** + * Gets the nth {@link Point} (0-indexed) in the motion profile. + * + * @param index The index of the point to get. + * @return The point at the specified index. + * @throws ArrayIndexOutOfBoundsException if {@code index} ≥ {@link #size()}. + */ + @Contract(pure = true) + public Point get(int index) { + return points[index]; + } + + /** + * Get the number of {@link Point points} in the profile. + * + * @return The number of points in the profile; specifically, {@code points.length}. + */ + @Contract(pure = true) + public int size() { + return points.length; + } + + /** + * A single instant within a {@link MotionProfile}. + */ + public static class Point { + + /** + * The time change since the previous point, in seconds. + */ + public double dt; + /** + * The x-position of the robot in {@linkplain org.team1540.base.motionprofiling profile units}, + * or 0 if not applicable. + */ + public double x; + /** + * The y-position of the robot in {@linkplain org.team1540.base.motionprofiling profile units}, + * or 0 if not applicable. + */ + public double y; + /** + * The position of the profiled mechanism, in {@linkplain org.team1540.base.motionprofiling + * profile units}. + */ + public double position; + /** + * The velocity of the profiled mechanism, in {@linkplain org.team1540.base.motionprofiling + * profile units} per second. + */ + public double velocity; + /** + * The acceleration of the profiled mechanism, in {@linkplain org.team1540.base.motionprofiling + * profile units} per second squared. + */ + public double acceleration; + /** + * The jerk of the profiled mechanism, in {@linkplain org.team1540.base.motionprofiling profile + * units} per second cubed. + */ + public double jerk; + /** + * The robot's heading in radians, or 0 if not applicable. + */ + public double heading; + + /** + * Creates a new {@code Point}. + * + * @param dt The time change since the previous point, in seconds. + * @param x The x-position of the robot in {@linkplain org.team1540.base.motionprofiling profile + * units}, or 0 if not applicable. + * @param y The y-position of the robot in {@linkplain org.team1540.base.motionprofiling profile + * units}, or 0 if not applicable. + * @param position The position of the profiled mechanism, in {@linkplain + * org.team1540.base.motionprofiling profile units}. + * @param velocity The velocity of the profiled mechanism, in {@linkplain + * org.team1540.base.motionprofiling profile units} per second. + * @param acceleration The acceleration of the profiled mechanism, in {@linkplain + * org.team1540.base.motionprofiling profile units} per second squared. + * @param jerk The jerk of the profiled mechanism, in {@linkplain org.team1540.base.motionprofiling + * profile units} per second cubed. + * @param heading The robot's heading in radians, or 0 if not applicable. + */ + public Point(double dt, double x, double y, double position, double velocity, + double acceleration, double jerk, double heading) { + this.dt = dt; + this.x = x; + this.y = y; + this.position = position; + this.velocity = velocity; + this.acceleration = acceleration; + this.jerk = jerk; + this.heading = heading; + } + + @Contract(value = "null -> false", pure = true) + @Override + public boolean equals(@Nullable Object o) { + if (this == o) { + return true; + } + if (!(o instanceof Point)) { + return false; + } + Point point = (Point) o; + return Double.compare(point.dt, dt) == 0 && + Double.compare(point.x, x) == 0 && + Double.compare(point.y, y) == 0 && + Double.compare(point.position, position) == 0 && + Double.compare(point.velocity, velocity) == 0 && + Double.compare(point.acceleration, acceleration) == 0 && + Double.compare(point.jerk, jerk) == 0 && + Double.compare(point.heading, heading) == 0; + } + + @Override + public int hashCode() { + return Objects.hash(dt, x, y, position, velocity, acceleration, jerk, heading); + } + } +} From e232ee584b7c4457dde800bc12a4e89ca44c29e1 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 1 Oct 2018 16:04:10 -0700 Subject: [PATCH 013/145] Add method to convert Pathfinder trajectories --- .../motionprofiling/MotionProfileUtils.java | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/lib/src/main/java/org/team1540/base/motionprofiling/MotionProfileUtils.java b/lib/src/main/java/org/team1540/base/motionprofiling/MotionProfileUtils.java index dc16533b..4bd7d5b4 100644 --- a/lib/src/main/java/org/team1540/base/motionprofiling/MotionProfileUtils.java +++ b/lib/src/main/java/org/team1540/base/motionprofiling/MotionProfileUtils.java @@ -3,6 +3,11 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.DemandType; import com.ctre.phoenix.motorcontrol.can.BaseMotorController; +import jaci.pathfinder.Trajectory; +import java.util.Arrays; +import org.jetbrains.annotations.Contract; +import org.jetbrains.annotations.NotNull; +import org.team1540.base.motionprofiling.MotionProfile.Point; public class MotionProfileUtils { @@ -34,4 +39,18 @@ public static SetpointConsumer createSetpointConsumer(BaseMotorController motorC .set(ControlMode.Position, setpoint * adjustment, DemandType.ArbitraryFeedForward, bump); } + + /** + * Creates a ROOSTER {@link MotionProfile} from a Pathfinder {@link Trajectory}. + * + * @param trajectory The {@link Trajectory} to convert. + * @return A {@link MotionProfile} containing the same points. Profile points are copied over, so + * subsequent changes to the {@link Trajectory} will not affect the produced {@link MotionProfile}. + */ + @Contract("_ -> new") + @NotNull + public static MotionProfile createProfile(@NotNull Trajectory trajectory) { + return new MotionProfile((Point[]) Arrays.stream(trajectory.segments).map(s -> new Point(s.dt, s.x, s.y, s.position, s.velocity, s.acceleration, s.jerk, s.heading)).toArray()); + } + } From b045a04da0118472802975a208ebced6efe73018 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 1 Oct 2018 16:05:19 -0700 Subject: [PATCH 014/145] Update execution to use new datastructures --- .../base/motionprofiling/FollowProfile.java | 23 +++++++++---------- .../motionprofiling/FollowProfileFactory.java | 14 ++++++----- 2 files changed, 19 insertions(+), 18 deletions(-) diff --git a/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java b/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java index 66048923..66142bcb 100644 --- a/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java +++ b/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java @@ -2,11 +2,10 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.command.Subsystem; -import jaci.pathfinder.Trajectory; -import jaci.pathfinder.Trajectory.Segment; import java.util.Arrays; import java.util.function.DoubleSupplier; import org.jetbrains.annotations.NotNull; +import org.team1540.base.motionprofiling.MotionProfile.Point; import org.team1540.base.util.AsyncCommand; /** @@ -27,9 +26,9 @@ public class FollowProfile extends AsyncCommand { @NotNull - Trajectory left; + MotionProfile left; @NotNull - Trajectory right; + MotionProfile right; @NotNull private SetpointConsumer leftSetpointConsumer; @@ -76,7 +75,7 @@ public class FollowProfile extends AsyncCommand { * @param headingI The I coefficient for the heading controller, in profile units per * radian-second. */ - public FollowProfile(@NotNull Trajectory left, @NotNull Trajectory right, + public FollowProfile(@NotNull MotionProfile left, @NotNull MotionProfile right, @NotNull Subsystem[] subsystems, @NotNull SetpointConsumer leftSetpointConsumer, @NotNull SetpointConsumer rightSetpointConsumer, @NotNull DoubleSupplier headingSupplier, @@ -97,25 +96,25 @@ public FollowProfile(@NotNull Trajectory left, @NotNull Trajectory right, this.headingP = headingP; this.headingI = headingI; - profTime = Arrays.stream(left.segments).mapToDouble(s -> s.dt).sum(); + profTime = Arrays.stream(left.points).mapToDouble(s -> s.dt).sum(); } - private Segment getCurrentSegment(Trajectory trajectory, double currentTime) { + private Point getCurrentSegment(MotionProfile trajectory, double currentTime) { // Start from the current time and find the closest point. - int startIndex = Math.toIntExact(Math.round(currentTime / trajectory.segments[0].dt)); + int startIndex = Math.toIntExact(Math.round(currentTime / trajectory.get(0).dt)); - int length = trajectory.segments.length; + int length = trajectory.size(); int index = startIndex; if (startIndex >= length - 1) { index = length - 1; } - return trajectory.segments[index]; + return trajectory.get(index); } @Override protected void runPeriodic() { - Segment leftSegment = getCurrentSegment(left, timer.get()); - Segment rightSegment = getCurrentSegment(right, timer.get()); + Point leftSegment = getCurrentSegment(left, timer.get()); + Point rightSegment = getCurrentSegment(right, timer.get()); // check finish status if (timer.get() > profTime) { markAsFinished(); diff --git a/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfileFactory.java b/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfileFactory.java index cd90aaf1..a4e98b27 100644 --- a/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfileFactory.java +++ b/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfileFactory.java @@ -303,8 +303,9 @@ public FollowProfile create(@NotNull String profileName) { /** * Creates a new {@link FollowProfile} command to follow the provided profile. This is a - * convinience method that calls {@link #create(Trajectory, Trajectory)} after loading the - * trajectories using {@link Pathfinder#readFromCSV(File)}. + * convinience method that calls {@link #create(MotionProfile, MotionProfile)} after loading the + * trajectories using {@link Pathfinder#readFromCSV(File)} and converting them using {@link + * MotionProfileUtils#createProfile(Trajectory)}. * * @param leftFile The file to load the left profile from; should be a pathfinder-formatted CSV. * @param rightFile The file to load the right profile from; should be a pathfinder-formatted @@ -314,19 +315,20 @@ public FollowProfile create(@NotNull String profileName) { */ @NotNull public FollowProfile create(@NotNull File leftFile, @NotNull File rightFile) { - return create(Pathfinder.readFromCSV(leftFile), Pathfinder.readFromCSV(rightFile)); + return create(MotionProfileUtils.createProfile(Pathfinder.readFromCSV(leftFile)), + MotionProfileUtils.createProfile(Pathfinder.readFromCSV(rightFile))); } /** * Creates a new {@link FollowProfile} command to follow the provided profile. * - * @param left The trajectory for the left side. - * @param right The trajectory for the right side. + * @param left The profile for the left side. + * @param right The profile for the right side. * @return A new {@link FollowProfile} command with the previously configured settings following * * the provided profile. */ @NotNull - public FollowProfile create(@NotNull Trajectory left, @NotNull Trajectory right) { + public FollowProfile create(@NotNull MotionProfile left, @NotNull MotionProfile right) { return new FollowProfile(left, right, subsystems, leftSetpointConsumer, rightSetpointConsumer, headingSupplier, loopFreq, velCoeff, velIntercept, accelCoeff, headingP, headingI); } From e25ccc85314d39e5a3f66bc16b467ea297878154 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 1 Oct 2018 16:11:04 -0700 Subject: [PATCH 015/145] Use new datastructures in MP test code That broke build 542, let's try it again --- .../kotlin/org/team1540/base/testing/MotionProfileTesting.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt b/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt index a9a2c195..4a834628 100644 --- a/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt +++ b/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt @@ -155,7 +155,7 @@ class MotionProfileTestingRobot : IterativeRobot() { maxJerk ))).modify(wheelbase).trajectories - val command = factory.create(leftTrajectory, rightTrajectory) + val command = factory.create(MotionProfileUtils.createProfile(leftTrajectory), MotionProfileUtils.createProfile(rightTrajectory)) command.start() } From 662274c8bc7837bcb4debd142683be6e32a51742 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 1 Oct 2018 16:34:30 -0700 Subject: [PATCH 016/145] Add teleop drive command for MP testing --- .../team1540/base/testing/MotionProfileTesting.kt | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt b/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt index 4a834628..1d8ec15f 100644 --- a/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt +++ b/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt @@ -4,6 +4,7 @@ import com.ctre.phoenix.motorcontrol.ControlMode import com.ctre.phoenix.motorcontrol.FeedbackDevice import com.kauailabs.navx.frc.AHRS import edu.wpi.first.wpilibj.IterativeRobot +import edu.wpi.first.wpilibj.Joystick import edu.wpi.first.wpilibj.SPI import edu.wpi.first.wpilibj.command.Scheduler import edu.wpi.first.wpilibj.command.Subsystem @@ -12,12 +13,19 @@ import jaci.pathfinder.Pathfinder import jaci.pathfinder.Trajectory import jaci.pathfinder.Waypoint import jaci.pathfinder.modifiers.TankModifier +import org.team1540.base.Utilities import org.team1540.base.motionprofiling.FollowProfileFactory import org.team1540.base.motionprofiling.MotionProfileUtils import org.team1540.base.preferencemanager.Preference import org.team1540.base.preferencemanager.PreferenceManager +import org.team1540.base.util.Executable +import org.team1540.base.util.SimpleCommand +import org.team1540.base.util.SimpleLoopCommand import org.team1540.base.wrappers.ChickenTalon import java.util.function.DoubleSupplier +import javax.rmi.CORBA.Util + +private val driver = Joystick(0) class MotionProfileTestingRobot : IterativeRobot() { lateinit var factory: FollowProfileFactory @@ -202,6 +210,10 @@ private object DriveTrain : Subsystem() { } override fun initDefaultCommand() { + defaultCommand = SimpleLoopCommand("Teleop Drive", Executable { + left1.set(ControlMode.PercentOutput, Utilities.processDeadzone(driver.getRawAxis(1), 0.1)) + right1.set(ControlMode.PercentOutput, Utilities.processDeadzone(driver.getRawAxis(5), 0.1)) + }, this) } fun reset() { From 5b837565c7bd4cd7999bfd2a9791fb798ebad744 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 1 Oct 2018 16:36:40 -0700 Subject: [PATCH 017/145] Clarify README.md --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index a72186d2..6b033b30 100644 --- a/README.md +++ b/README.md @@ -22,7 +22,7 @@ Advanced closed-loop drive code for a tank drive. `org.team1540.base.motionprofiling` -A system for executing Pathfinder motion profiles on a tank drive. +A system for executing motion profiles on a tank drive. #### Power Management @@ -117,4 +117,4 @@ We recommend using IntelliJ IDEA to develop ROOSTER. To import the project, on I ### Code Style -Team 1540 (and ROOSTER) uses [Google Java Style](https://google.github.io/styleguide/javaguide.html) for all code. Additionally, all new code should have proper nullability annotations on all public-facing parameters and return types. (`@NotNull` for parameters that must not be `null` or methods that never return `null`, `@Nullable` for the opposite.) \ No newline at end of file +Team 1540 (and ROOSTER) uses [Google Java Style](https://google.github.io/styleguide/javaguide.html) for all code. Additionally, all new code should have proper nullability annotations on all public-facing parameters and return types. (`@NotNull` for parameters that must not be `null` or methods that never return `null`, `@Nullable` for the opposite.) From a110f51e228e90403ca4aa8e333662e41a771295 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Tue, 2 Oct 2018 08:13:03 -0700 Subject: [PATCH 018/145] Privatize fields #capitalism --- .../org/team1540/base/testing/MotionProfileTesting.kt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt b/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt index 1d8ec15f..2caa785a 100644 --- a/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt +++ b/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt @@ -28,9 +28,9 @@ import javax.rmi.CORBA.Util private val driver = Joystick(0) class MotionProfileTestingRobot : IterativeRobot() { - lateinit var factory: FollowProfileFactory + private lateinit var factory: FollowProfileFactory - val navx = AHRS(SPI.Port.kMXP) + private val navx = AHRS(SPI.Port.kMXP) @JvmField @Preference("kV") @@ -216,7 +216,7 @@ private object DriveTrain : Subsystem() { }, this) } - fun reset() { + private fun reset() { left1.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder) right1.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder) From faf43f6eb58fcf782e79d9b3ccf5e8cda9de5503 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Wed, 3 Oct 2018 22:09:29 -0700 Subject: [PATCH 019/145] Use non-persistent preferences --- .../base/testing/MotionProfileTesting.kt | 28 +++++++++---------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt b/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt index 2caa785a..406c0db0 100644 --- a/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt +++ b/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt @@ -33,59 +33,59 @@ class MotionProfileTestingRobot : IterativeRobot() { private val navx = AHRS(SPI.Port.kMXP) @JvmField - @Preference("kV") + @Preference("kV", persistent = false) var kV = 0.0 @JvmField - @Preference("kA") + @Preference("kA", persistent = false) var kA = 0.0 @JvmField - @Preference("VIntercept") + @Preference("VIntercept", persistent = false) var vIntercept = 0.0 @JvmField - @Preference("MP Loop Freq") + @Preference("MP Loop Freq", persistent = false) var loopFreqMs = 0L @JvmField - @Preference("MP Heading P") + @Preference("MP Heading P", persistent = false) var hdgP = 0.0 @JvmField - @Preference("MP Heading I") + @Preference("MP Heading I", persistent = false) var hdgI = 0.0 @JvmField - @Preference("MP Drive P") + @Preference("MP Drive P", persistent = false) var driveP = 0.0 @JvmField - @Preference("MP Drive D") + @Preference("MP Drive D", persistent = false) var driveD = 0.0 @JvmField - @Preference("MP Delta-T") + @Preference("MP Delta-T", persistent = false) var deltaT = 0.0 @JvmField - @Preference("MP Max Vel") + @Preference("MP Max Vel", persistent = false) var maxVel = 0.0 @JvmField - @Preference("MP Max Accel") + @Preference("MP Max Accel", persistent = false) var maxAccel = 0.0 @JvmField - @Preference("MP Max Jerk") + @Preference("MP Max Jerk", persistent = false) var maxJerk = 0.0 @JvmField - @Preference("Wheelbase") + @Preference("Wheelbase", persistent = false) var wheelbase = 0.0 @JvmField - @Preference("Drive TPU") + @Preference("Drive TPU", persistent = false) var tpu = 0.0 override fun robotInit() { From 0e181002d8e921043f2243939e20ce953ca71cea Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 8 Oct 2018 15:44:33 -0700 Subject: [PATCH 020/145] Use voltage compensation --- .../util/robots/VelocityCharacterizationRobot.java | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/lib/src/main/java/org/team1540/base/util/robots/VelocityCharacterizationRobot.java b/lib/src/main/java/org/team1540/base/util/robots/VelocityCharacterizationRobot.java index c6628869..6f36bd11 100644 --- a/lib/src/main/java/org/team1540/base/util/robots/VelocityCharacterizationRobot.java +++ b/lib/src/main/java/org/team1540/base/util/robots/VelocityCharacterizationRobot.java @@ -40,8 +40,9 @@ public class VelocityCharacterizationRobot extends IterativeRobot { private Joystick joystick = new Joystick(0); - @Preference("Voltage Ramp Rate") - public double rampRate = 0.020833333; + private static final double SATURATION_VOLTAGE = 12; + @Preference("Voltage Ramp Rate (V/sec)") + public double rampRate = 0.25; @Preference("Invert Left Motor") public boolean invertLeft = false; @@ -90,7 +91,8 @@ public void teleopPeriodic() { + driveRightMotorA.getMotorOutputVoltage() + "," + driveRightMotorA.getSelectedSensorVelocity()); - appliedOutput += rampRate * ((System.currentTimeMillis() - lastTime) / 1000.0); + appliedOutput += + (rampRate / SATURATION_VOLTAGE) * ((System.currentTimeMillis() - lastTime) / 1000.0); lastTime = System.currentTimeMillis(); driveLeftMotorA.set(ControlMode.PercentOutput, appliedOutput); driveRightMotorA.set(ControlMode.PercentOutput, appliedOutput); @@ -140,6 +142,8 @@ public void reset() { talon.configPeakOutputForward(1); talon.configPeakOutputReverse(-1); talon.enableCurrentLimit(false); + talon.configVoltageCompSaturation(12); + talon.enableVoltageCompensation(true); } } } From e242fd47e5ce23b41d885b89342089ddb9d6a263 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 8 Oct 2018 15:47:30 -0700 Subject: [PATCH 021/145] Add commons-math dependency --- README.md | 1 + lib/build.gradle | 2 ++ 2 files changed, 3 insertions(+) diff --git a/README.md b/README.md index 6b033b30..7e63b16b 100644 --- a/README.md +++ b/README.md @@ -104,6 +104,7 @@ You'll also need ROOSTER's dependencies: - [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 +- [Apache Commons Math 3](https://mvnrepository.com/artifact/org.apache.commons/commons-math3/3.6.1) version 3.6.1 or higher ## Developing ROOSTER diff --git a/lib/build.gradle b/lib/build.gradle index 269b6e18..0953b592 100644 --- a/lib/build.gradle +++ b/lib/build.gradle @@ -43,4 +43,6 @@ dependencies { compile "openrio.mirror.third.kauailabs:navx-java:3.0.346" compile "jaci.pathfinder:Pathfinder-Java:1.8" compile "openrio.powerup:MatchData:2018.01.07" + // https://mvnrepository.com/artifact/org.apache.commons/commons-math3 + compile 'org.apache.commons:commons-math3:3.6.1' } From 21fccd69520c7e9e6a89dba706d58818f58e20ac Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 8 Oct 2018 18:32:23 -0700 Subject: [PATCH 022/145] Lock CTRE version --- test/build.gradle | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/test/build.gradle b/test/build.gradle index b4dc5f4c..3f33975e 100644 --- a/test/build.gradle +++ b/test/build.gradle @@ -6,6 +6,10 @@ plugins { id "jaci.openrio.gradle.GradleRIO" version "2018.03.06" } +wpi { + ctreVersion = "5.3.1.0" +} + dependencies { compile project(":lib") compile wpilib() From f9fa965bd9d8732dc72b1eda47f5c29ac38e6218 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 8 Oct 2018 18:35:35 -0700 Subject: [PATCH 023/145] Perform linear regression on-board This uses commons-math3 to perform linear regression; before, you would have to offload the written CSV into Excel and do regression there. --- .../robots/VelocityCharacterizationRobot.java | 63 +++++++++++-------- 1 file changed, 37 insertions(+), 26 deletions(-) diff --git a/lib/src/main/java/org/team1540/base/util/robots/VelocityCharacterizationRobot.java b/lib/src/main/java/org/team1540/base/util/robots/VelocityCharacterizationRobot.java index 6f36bd11..4293d31d 100644 --- a/lib/src/main/java/org/team1540/base/util/robots/VelocityCharacterizationRobot.java +++ b/lib/src/main/java/org/team1540/base/util/robots/VelocityCharacterizationRobot.java @@ -5,9 +5,9 @@ import edu.wpi.first.wpilibj.IterativeRobot; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.command.Scheduler; -import java.io.File; -import java.io.FileNotFoundException; -import java.io.PrintWriter; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import org.apache.commons.math3.stat.regression.SimpleRegression; +import org.jetbrains.annotations.NotNull; import org.team1540.base.preferencemanager.Preference; import org.team1540.base.preferencemanager.PreferenceManager; import org.team1540.base.wrappers.ChickenTalon; @@ -18,7 +18,8 @@ * 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 a quasi-static velocity characterization as described in Eli Barnett's paper "FRC - * Drivetrain Characterization" until the button is released. + * Drivetrain Characterization" until the button is released. The results (kV, vIntercept, and an + * R^2 value) are output to the SmartDashboard. */ public class VelocityCharacterizationRobot extends IterativeRobot { @@ -36,7 +37,9 @@ public class VelocityCharacterizationRobot extends IterativeRobot { driveLeftMotorC, driveRightMotorA, driveRightMotorB, driveRightMotorC}; private ChickenTalon[] driveMotorMasters = new ChickenTalon[]{driveLeftMotorA, driveRightMotorA}; - private PrintWriter csvWriter = null; + private SimpleRegression leftRegression = new SimpleRegression(); + private SimpleRegression rightRegression = new SimpleRegression(); + private boolean running = false; private Joystick joystick = new Joystick(0); @@ -59,37 +62,48 @@ public void robotInit() { reset(); } + private static void putRegressionData(@NotNull SimpleRegression regression, String prefix) { + // getSlope, getIntercept, and getRSquare all have the same criteria for returning NaN + if (!Double.isNaN(regression.getSlope())) { + SmartDashboard.putNumber(prefix + " Calculated kV", regression.getSlope()); + SmartDashboard.putNumber(prefix + " Calculated vIntercept", regression.getIntercept()); + SmartDashboard.putNumber(prefix + " rSquared", regression.getRSquare()); + } else { + SmartDashboard.putNumber(prefix + " Calculated kV", 0); + SmartDashboard.putNumber(prefix + " Calculated vIntercept", 0); + SmartDashboard.putNumber(prefix + " rSquared", 0); + } + } + @Override public void robotPeriodic() { - if (!isOperatorControl() && csvWriter != null) { - csvWriter.close(); - csvWriter = null; - } + putRegressionData(leftRegression, "Left"); + putRegressionData(rightRegression, "Right"); Scheduler.getInstance().run(); } @Override public void teleopPeriodic() { if (joystick.getRawButton(1)) { // if button A is pressed - if (csvWriter == null) { - // create a new CSV writer, reset everything + if (!running) { + // reset everything reset(); - try { - csvWriter = new PrintWriter(new File( - "/home/lvuser/dtmeasure/measure-" + System.currentTimeMillis() + ".csv")); - csvWriter.println("lvoltage,lvelocity,rvoltage,rvelocity"); - } catch (FileNotFoundException e) { - throw new RuntimeException(e); - } + running = true; + leftRegression.clear(); + rightRegression.clear(); appliedOutput = 0; driveLeftMotorA.set(ControlMode.PercentOutput, 0); driveRightMotorA.set(ControlMode.PercentOutput, 0); } else { + double leftVelocity = driveLeftMotorA.getSelectedSensorVelocity(); + if (leftVelocity != 0) { + leftRegression.addData(leftVelocity, driveLeftMotorA.getMotorOutputVoltage()); + } - csvWriter.println(driveLeftMotorA.getMotorOutputVoltage() + "," - + driveLeftMotorA.getSelectedSensorVelocity() + "," - + driveRightMotorA.getMotorOutputVoltage() + "," - + driveRightMotorA.getSelectedSensorVelocity()); + double rightVelocity = driveRightMotorA.getSelectedSensorVelocity(); + if (rightVelocity != 0) { + rightRegression.addData(leftVelocity, driveRightMotorA.getMotorOutputVoltage()); + } appliedOutput += (rampRate / SATURATION_VOLTAGE) * ((System.currentTimeMillis() - lastTime) / 1000.0); @@ -99,12 +113,9 @@ public void teleopPeriodic() { } } else { appliedOutput = 0; - if (csvWriter != null) { - csvWriter.close(); - csvWriter = null; - } driveLeftMotorA.set(ControlMode.PercentOutput, 0); driveRightMotorA.set(ControlMode.PercentOutput, 0); + running = false; } lastTime = System.currentTimeMillis(); } From 9e815a1d4101e78b58602db02d98c034a7ef4850 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 8 Oct 2018 18:40:01 -0700 Subject: [PATCH 024/145] Create CSV directory if it doesn't exist --- .../base/util/robots/AccelerationCharacterizationRobot.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/lib/src/main/java/org/team1540/base/util/robots/AccelerationCharacterizationRobot.java b/lib/src/main/java/org/team1540/base/util/robots/AccelerationCharacterizationRobot.java index d34e0685..9c26945d 100644 --- a/lib/src/main/java/org/team1540/base/util/robots/AccelerationCharacterizationRobot.java +++ b/lib/src/main/java/org/team1540/base/util/robots/AccelerationCharacterizationRobot.java @@ -99,6 +99,10 @@ private void run() { // 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"); From 634211debb3f4aa3fa16a5d17a1bea8805d1d1c8 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 8 Oct 2018 18:40:39 -0700 Subject: [PATCH 025/145] Enable voltage compensation --- .../base/util/robots/AccelerationCharacterizationRobot.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/lib/src/main/java/org/team1540/base/util/robots/AccelerationCharacterizationRobot.java b/lib/src/main/java/org/team1540/base/util/robots/AccelerationCharacterizationRobot.java index 9c26945d..d25e05ff 100644 --- a/lib/src/main/java/org/team1540/base/util/robots/AccelerationCharacterizationRobot.java +++ b/lib/src/main/java/org/team1540/base/util/robots/AccelerationCharacterizationRobot.java @@ -214,6 +214,8 @@ public void reset() { talon.configPeakOutputForward(1); talon.configPeakOutputReverse(-1); talon.enableCurrentLimit(false); + talon.configVoltageCompSaturation(12); + talon.enableVoltageCompensation(true); } } From 37c27f68f53cf748183b5b174b7f4e718894acb1 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 8 Oct 2018 18:41:15 -0700 Subject: [PATCH 026/145] Use commons-math for best-fit velocity slope --- .../robots/AccelerationCharacterizationRobot.java | 13 ++++--------- 1 file changed, 4 insertions(+), 9 deletions(-) diff --git a/lib/src/main/java/org/team1540/base/util/robots/AccelerationCharacterizationRobot.java b/lib/src/main/java/org/team1540/base/util/robots/AccelerationCharacterizationRobot.java index d25e05ff..e62ac8df 100644 --- a/lib/src/main/java/org/team1540/base/util/robots/AccelerationCharacterizationRobot.java +++ b/lib/src/main/java/org/team1540/base/util/robots/AccelerationCharacterizationRobot.java @@ -12,6 +12,7 @@ import java.io.PrintWriter; import java.util.LinkedList; import java.util.List; +import org.apache.commons.math3.stat.regression.SimpleRegression; import org.team1540.base.preferencemanager.Preference; import org.team1540.base.preferencemanager.PreferenceManager; import org.team1540.base.wrappers.ChickenTalon; @@ -220,16 +221,10 @@ public void reset() { } private static double bestFitSlope(List xVals, List yVals) { - double avgX = xVals.stream().mapToDouble(x -> x).sum() / xVals.size(); - double avgY = yVals.stream().mapToDouble(y -> y).sum() / yVals.size(); - - double sumXY = 0; - double sumXSquared = 0; + SimpleRegression reg = new SimpleRegression(); for (int i = 0; i < xVals.size(); i++) { - sumXY += (xVals.get(i) - avgX) * (yVals.get(i) - avgY); - sumXSquared += (xVals.get(i) - avgX) * (xVals.get(i) - avgX); + reg.addData(xVals.get(i), yVals.get(i)); } - - return sumXY / sumXSquared; + return reg.getSlope(); } } From 52adac0d6f4794373524232eedd209a7f57ee806 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Tue, 9 Oct 2018 11:15:21 -0700 Subject: [PATCH 027/145] Create run configurations --- ...ploy_AccelerationCharacterizationRobot.xml | 21 +++++++++++++++++++ .../Deploy_MotionProfileTesting.xml | 21 +++++++++++++++++++ .../Deploy_VelocityCharacterizationRobot.xml | 21 +++++++++++++++++++ 3 files changed, 63 insertions(+) create mode 100644 .idea/runConfigurations/Deploy_AccelerationCharacterizationRobot.xml create mode 100644 .idea/runConfigurations/Deploy_MotionProfileTesting.xml create mode 100644 .idea/runConfigurations/Deploy_VelocityCharacterizationRobot.xml diff --git a/.idea/runConfigurations/Deploy_AccelerationCharacterizationRobot.xml b/.idea/runConfigurations/Deploy_AccelerationCharacterizationRobot.xml new file mode 100644 index 00000000..c8e0eb8a --- /dev/null +++ b/.idea/runConfigurations/Deploy_AccelerationCharacterizationRobot.xml @@ -0,0 +1,21 @@ + + + + + + + true + + + \ No newline at end of file diff --git a/.idea/runConfigurations/Deploy_MotionProfileTesting.xml b/.idea/runConfigurations/Deploy_MotionProfileTesting.xml new file mode 100644 index 00000000..90e9f6b5 --- /dev/null +++ b/.idea/runConfigurations/Deploy_MotionProfileTesting.xml @@ -0,0 +1,21 @@ + + + + + + + true + + + \ No newline at end of file diff --git a/.idea/runConfigurations/Deploy_VelocityCharacterizationRobot.xml b/.idea/runConfigurations/Deploy_VelocityCharacterizationRobot.xml new file mode 100644 index 00000000..7943c2b4 --- /dev/null +++ b/.idea/runConfigurations/Deploy_VelocityCharacterizationRobot.xml @@ -0,0 +1,21 @@ + + + + + + + true + + + \ No newline at end of file From 0051dfe617b3bb539f6b325a5a3b931123a8d530 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Fri, 12 Oct 2018 16:10:25 -0700 Subject: [PATCH 028/145] Use x-intercept instead of y-intercept --- .../base/util/robots/VelocityCharacterizationRobot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/src/main/java/org/team1540/base/util/robots/VelocityCharacterizationRobot.java b/lib/src/main/java/org/team1540/base/util/robots/VelocityCharacterizationRobot.java index 4293d31d..77f7dea4 100644 --- a/lib/src/main/java/org/team1540/base/util/robots/VelocityCharacterizationRobot.java +++ b/lib/src/main/java/org/team1540/base/util/robots/VelocityCharacterizationRobot.java @@ -66,7 +66,7 @@ private static void putRegressionData(@NotNull SimpleRegression regression, Stri // getSlope, getIntercept, and getRSquare all have the same criteria for returning NaN if (!Double.isNaN(regression.getSlope())) { SmartDashboard.putNumber(prefix + " Calculated kV", regression.getSlope()); - SmartDashboard.putNumber(prefix + " Calculated vIntercept", regression.getIntercept()); + SmartDashboard.putNumber(prefix + " Calculated vIntercept", regression.predict(0)); SmartDashboard.putNumber(prefix + " rSquared", regression.getRSquare()); } else { SmartDashboard.putNumber(prefix + " Calculated kV", 0); From 82ba6479878e9eefdf920900773ea09edbb6b18b Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Fri, 12 Oct 2018 21:25:04 -0700 Subject: [PATCH 029/145] Use non-persistent preferences for setpoint --- .../base/util/robots/AccelerationCharacterizationRobot.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/lib/src/main/java/org/team1540/base/util/robots/AccelerationCharacterizationRobot.java b/lib/src/main/java/org/team1540/base/util/robots/AccelerationCharacterizationRobot.java index e62ac8df..02491b68 100644 --- a/lib/src/main/java/org/team1540/base/util/robots/AccelerationCharacterizationRobot.java +++ b/lib/src/main/java/org/team1540/base/util/robots/AccelerationCharacterizationRobot.java @@ -39,6 +39,8 @@ public class AccelerationCharacterizationRobot extends IterativeRobot { @Preference("Invert Right Motor") public boolean invertRight = true; + @Preference(persistent = false) + public double setpoint = 0.6; private ChickenTalon driveLeftMotorA = new ChickenTalon(1); private ChickenTalon driveLeftMotorB = new ChickenTalon(2); @@ -127,8 +129,8 @@ private void run() { System.out.println(times.toString()); System.out.println(lAccel); } - driveLeftMotorA.set(ControlMode.PercentOutput, SmartDashboard.getNumber("Setpoint", 0.6)); - driveRightMotorA.set(ControlMode.PercentOutput, SmartDashboard.getNumber("Setpoint", 0.6)); + driveLeftMotorA.set(ControlMode.PercentOutput, setpoint); + driveRightMotorA.set(ControlMode.PercentOutput, setpoint); } } else { if (csvWriter != null) { From e19910fcb3834617f541fb02e1c36f069eafd45b Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Fri, 12 Oct 2018 21:25:19 -0700 Subject: [PATCH 030/145] Make acceleration window configurable --- .../util/robots/AccelerationCharacterizationRobot.java | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/lib/src/main/java/org/team1540/base/util/robots/AccelerationCharacterizationRobot.java b/lib/src/main/java/org/team1540/base/util/robots/AccelerationCharacterizationRobot.java index 02491b68..4711c32c 100644 --- a/lib/src/main/java/org/team1540/base/util/robots/AccelerationCharacterizationRobot.java +++ b/lib/src/main/java/org/team1540/base/util/robots/AccelerationCharacterizationRobot.java @@ -38,6 +38,8 @@ public class AccelerationCharacterizationRobot extends IterativeRobot { 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; @@ -66,8 +68,7 @@ private void run() { csvWriter.close(); csvWriter = null; } - Scheduler.getInstance().run(); - if (leftVelocities.size() == 4) { + if (leftVelocities.size() == accelMeasurementWindow) { leftVelocities.remove(0); rightVelocities.remove(0); leftVoltages.remove(0); @@ -90,7 +91,7 @@ private void run() { rightVoltages.add(accelCausingVoltageRight); times.add((double) System.currentTimeMillis() / 1000.0); - if (leftVelocities.size() == 4) { + if (leftVelocities.size() == accelMeasurementWindow) { double lAccel = bestFitSlope(times, leftVelocities); double rAccel = bestFitSlope(times, rightVelocities); SmartDashboard.putNumber("Left Accel", lAccel); @@ -120,7 +121,7 @@ private void run() { SmartDashboard.putNumber("Right Output", driveRightMotorA.getMotorOutputPercent()); SmartDashboard.putNumber("Right Velocity", rightVelocity); - if (leftVelocities.size() == 4) { + if (leftVelocities.size() == accelMeasurementWindow) { double lAccel = bestFitSlope(times, leftVelocities); double rAccel = bestFitSlope(times, rightVelocities); csvWriter.println( From 10add1739be240227d120955f8e5bbf455478429 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Fri, 12 Oct 2018 23:21:03 -0700 Subject: [PATCH 031/145] Use static imports for math functions --- .../org/team1540/base/motionprofiling/FollowProfile.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java b/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java index 66142bcb..dffb6e09 100644 --- a/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java +++ b/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java @@ -1,5 +1,9 @@ package org.team1540.base.motionprofiling; +import static java.lang.Math.atan2; +import static java.lang.Math.cos; +import static java.lang.Math.sin; + import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.command.Subsystem; import java.util.Arrays; @@ -128,8 +132,7 @@ protected void runPeriodic() { double error3 = heading - headingTarget - 2 * Math.PI; // basically magic https://stackoverflow.com/a/2007279 - double headingError = Math - .atan2(Math.sin(heading - headingTarget), Math.cos(heading - headingTarget)); + double headingError = atan2(sin(heading - headingTarget), cos(heading - headingTarget)); gyroIAccum += headingError * execTimer.get(); execTimer.reset(); From f8b3b4dfb5ede593c5787f4625d668ab59814ef7 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Fri, 12 Oct 2018 23:28:50 -0700 Subject: [PATCH 032/145] Make left and right profiles private --- .../java/org/team1540/base/motionprofiling/FollowProfile.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java b/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java index dffb6e09..dcf9eb9c 100644 --- a/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java +++ b/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java @@ -30,9 +30,9 @@ public class FollowProfile extends AsyncCommand { @NotNull - MotionProfile left; + private MotionProfile left; @NotNull - MotionProfile right; + private MotionProfile right; @NotNull private SetpointConsumer leftSetpointConsumer; From 35592506ed0a8397c221553ffaa3f2774444f159 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Sat, 13 Oct 2018 00:06:35 -0700 Subject: [PATCH 033/145] Extract profile following logic from FollowProfile This splits the logic into two segments, FollowProfile and ProfileFollower. ProfileFollower has no WPILib dependencies and contains the actual execution code; FollowProfile makes it easy to hook a ProfileFollower into the command system via AsyncCommand. This also cleans up some of the code, adds line breaks, etc. --- .../base/motionprofiling/FollowProfile.java | 88 ++------- .../base/motionprofiling/ProfileFollower.java | 182 ++++++++++++++++++ 2 files changed, 197 insertions(+), 73 deletions(-) create mode 100644 lib/src/main/java/org/team1540/base/motionprofiling/ProfileFollower.java diff --git a/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java b/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java index dcf9eb9c..d10cb00e 100644 --- a/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java +++ b/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java @@ -1,31 +1,19 @@ package org.team1540.base.motionprofiling; -import static java.lang.Math.atan2; -import static java.lang.Math.cos; -import static java.lang.Math.sin; - import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.command.Subsystem; -import java.util.Arrays; import java.util.function.DoubleSupplier; import org.jetbrains.annotations.NotNull; -import org.team1540.base.motionprofiling.MotionProfile.Point; +import org.team1540.base.motionprofiling.ProfileFollower.ProfileDriveSignal; import org.team1540.base.util.AsyncCommand; /** - * {@link edu.wpi.first.wpilibj.command.Command Command} to execute a motion profile. - *

- * The profile-following algorithm used here is a derivative of the algorithm used by Team 2471. The - * output of a heading-based PI loop is added to the profile's position setpoint and passed to the - * motor controllers, with additional velocity, acceleration, and static friction feed-forwards (in - * line with Eli Barnett's drive - * characterization method) added via throttle bump. - *

- * It is designed to use native Talon SRX position PID control with a throttle bump, but the - * provided {@link SetpointConsumer SetpointConsumers} could instead be used to control a RIO-side - * PID loop. + * {@link edu.wpi.first.wpilibj.command.Command Command} to execute a motion profile. This is + * an {@link AsyncCommand}-based wrapper around {@link ProfileFollower} which handles creating + * the instance, running it in a fast loop, and sending the output to the motors. * * @see FollowProfileFactory + * @see ProfileFollower */ public class FollowProfile extends AsyncCommand { @@ -42,8 +30,6 @@ public class FollowProfile extends AsyncCommand { @NotNull private DoubleSupplier headingSupplier; - private double profTime; - private double velCoeff; private double velIntercept; private double accelCoeff; @@ -52,16 +38,13 @@ public class FollowProfile extends AsyncCommand { private double gyroIAccum; + private ProfileFollower follower; + /** * Timer for the amount of time since profiling started (used to find current point) */ private Timer timer = new Timer(); - /** - * Timer for the amount of time since the loop last ran (used for integral term) - */ - private Timer execTimer = new Timer(); - /** * Creates a {@code FollowProfile} command. * @@ -100,73 +83,32 @@ public FollowProfile(@NotNull MotionProfile left, @NotNull MotionProfile right, this.headingP = headingP; this.headingI = headingI; - profTime = Arrays.stream(left.points).mapToDouble(s -> s.dt).sum(); - } - - private Point getCurrentSegment(MotionProfile trajectory, double currentTime) { - // Start from the current time and find the closest point. - int startIndex = Math.toIntExact(Math.round(currentTime / trajectory.get(0).dt)); - - int length = trajectory.size(); - int index = startIndex; - if (startIndex >= length - 1) { - index = length - 1; - } - return trajectory.get(index); } @Override protected void runPeriodic() { - Point leftSegment = getCurrentSegment(left, timer.get()); - Point rightSegment = getCurrentSegment(right, timer.get()); // check finish status - if (timer.get() > profTime) { + if (follower.isProfileFinished(timer.get())) { markAsFinished(); } - double heading = headingSupplier.getAsDouble(); - double headingTarget = leftSegment.heading; - - double error1 = heading - headingTarget; - double error2 = heading - headingTarget + 2 * Math.PI; - double error3 = heading - headingTarget - 2 * Math.PI; - - // basically magic https://stackoverflow.com/a/2007279 - double headingError = atan2(sin(heading - headingTarget), cos(heading - headingTarget)); - - gyroIAccum += headingError * execTimer.get(); - execTimer.reset(); - execTimer.start(); - - double gyroPOut = headingError * headingP; - double gyroIOut = gyroIAccum * headingI; - double leftVelFOut = velCoeff * leftSegment.velocity; - double rightVelFOut = velCoeff * rightSegment.velocity; - double leftVelInterceptOut = - leftSegment.velocity == 0 ? 0 : Math.copySign(velIntercept, leftSegment.velocity); - double rightVelInterceptOut = - rightSegment.velocity == 0 ? 0 : Math.copySign(velIntercept, rightSegment.velocity); - double leftAccelFOut = accelCoeff * leftSegment.acceleration; - double rightAccelFOut = accelCoeff * rightSegment.acceleration; - - leftSetpointConsumer.set(leftSegment.position - gyroPOut - gyroIOut, - leftVelFOut + leftVelInterceptOut + leftAccelFOut); - rightSetpointConsumer.set(rightSegment.position + gyroPOut + gyroIOut, - rightVelFOut + rightVelInterceptOut + rightAccelFOut); + ProfileDriveSignal sig = follower.get(headingSupplier.getAsDouble(), timer.get()); + + leftSetpointConsumer.set(sig.leftSetpoint, sig.leftBump); + rightSetpointConsumer.set(sig.rightSetpoint, sig.rightBump); } @Override protected void runInitial() { + // create a new follower since ProfileFollower is very stateful/doesn't do resets + follower = new ProfileFollower(left, right, velCoeff, velIntercept, accelCoeff, headingP, + headingI); timer.start(); - execTimer.reset(); - gyroIAccum = 0; } @Override protected void runEnd() { timer.stop(); timer.reset(); - execTimer.stop(); - execTimer.reset(); } } diff --git a/lib/src/main/java/org/team1540/base/motionprofiling/ProfileFollower.java b/lib/src/main/java/org/team1540/base/motionprofiling/ProfileFollower.java new file mode 100644 index 00000000..db2fc997 --- /dev/null +++ b/lib/src/main/java/org/team1540/base/motionprofiling/ProfileFollower.java @@ -0,0 +1,182 @@ +package org.team1540.base.motionprofiling; + +import static java.lang.Math.atan2; +import static java.lang.Math.cos; +import static java.lang.Math.sin; + +import java.util.Arrays; +import org.jetbrains.annotations.NotNull; +import org.team1540.base.motionprofiling.MotionProfile.Point; + +/** + * Helper to execute motion profiles. This class has no dependency on WPILib/any external library. + * + * The profile-following algorithm used here is a derivative of the algorithm used by Team 2471. The + * output of a heading-based PI loop is added to the profile's position setpoint and passed to the + * motor controllers, with additional velocity, acceleration, and static friction feed-forwards (in + * line with Eli Barnett's drive + * characterization method) added via throttle bump. + *

+ * It is designed to use native Talon SRX position PID control with a throttle bump, but the output + * could instead be used to control a RIO-side PID loop. + */ +public class ProfileFollower { + + @NotNull + private MotionProfile left; + @NotNull + private MotionProfile right; + + private double velCoeff; + private double velIntercept; + private double accelCoeff; + private double headingP; + private double headingI; + + private double gyroIAccum; + + private double profTime; + + private double lastTime = -1; + + /** + * Creates a {@code ProfileFollower}. + * + * For an explanation of units, see the {@linkplain org.team1540.base.motionprofiling package + * docs}. + * + * @param velCoeff The velocity coefficient (kV), in bump units per profile unit per second. + * @param velIntercept The velocity intercept (VIntercept), in bump units. + * @param accelCoeff The acceleration coefficient (kA), in bump units per profile unit per second + * squared. + * @param headingP The P coefficient for the heading controller, in profile units per radian. + * @param headingI The I coefficient for the heading controller, in profile units per + * radian-second. + */ + public ProfileFollower(@NotNull MotionProfile left, @NotNull MotionProfile right, double velCoeff, + double velIntercept, double accelCoeff, double headingP, double headingI) { + this.left = left; + this.right = right; + this.velCoeff = velCoeff; + this.velIntercept = velIntercept; + this.accelCoeff = accelCoeff; + this.headingP = headingP; + this.headingI = headingI; + + profTime = Math.max(Arrays.stream(left.points).mapToDouble(s -> s.dt).sum(), + Arrays.stream(right.points).mapToDouble(s -> s.dt).sum()); + } + + + /** + * Get the output from the motion profile at a given time (usually the current time). + * + * @param heading The current gyro heading, from 0 to 2π inclusive. + * @param time The time, in seconds, since motion profile execution began. This is used to find + * the segment to execute. + * @return A {@link ProfileDriveSignal} describing the necessary drive commands. + */ + @NotNull + public ProfileDriveSignal get(double heading, double time) { + Point leftSegment = getCurrentSegment(left, time); + Point rightSegment = getCurrentSegment(right, time); + + double headingTarget = leftSegment.heading; + + // basically magic https://stackoverflow.com/a/2007279 + double headingError = atan2(sin(heading - headingTarget), cos(heading - headingTarget)); + + double timeSinceLast = lastTime == -1 ? 0 : time - lastTime; + gyroIAccum += headingError * timeSinceLast; + + lastTime = time; + + double gyroPOut = headingError * headingP; + double gyroIOut = gyroIAccum * headingI; + + double leftVelFOut = velCoeff * leftSegment.velocity; + double rightVelFOut = velCoeff * rightSegment.velocity; + + double leftVelInterceptOut = + leftSegment.velocity == 0 ? 0 : Math.copySign(velIntercept, leftSegment.velocity); + double rightVelInterceptOut = + rightSegment.velocity == 0 ? 0 : Math.copySign(velIntercept, rightSegment.velocity); + + double leftAccelFOut = accelCoeff * leftSegment.acceleration; + double rightAccelFOut = accelCoeff * rightSegment.acceleration; + + return new ProfileDriveSignal( + leftSegment.position - gyroPOut - gyroIOut, + leftVelFOut + leftVelInterceptOut + leftAccelFOut, + rightSegment.position + gyroPOut + gyroIOut, + rightVelFOut + rightVelInterceptOut + rightAccelFOut + ); + } + + /** + * Gets the total time to execute the profile. + * + * @return The time to execute the currently loaded profile, in seconds. + */ + public double getProfileTime() { + return profTime; + } + + /** + * Get whether the profile is finished (i.e. time > {@link #getProfileTime()}} + * + * @param time The current time since the profile began executing, in seconds. + * @return {@code true} if the profile is finished; {@code false} otherwise. + */ + public boolean isProfileFinished(double time) { + return time > profTime; + } + + /** + * A signal to be sent to the robot drive, consisting of left and right position setpoints and + * feed-forward bumps. + */ + public static class ProfileDriveSignal { + + /** + * The left-side position setpoint, in {@linkplain org.team1540.base.motionprofiling profile + * units}. + */ + public final double leftSetpoint; + /** + * The left-side feed-forward throttle bump, in fractions of motor throttle (i.e. 0.5 == 50% of + * max motor throttle). + */ + public final double leftBump; + /** + * The right-side position setpoint, in {@linkplain org.team1540.base.motionprofiling profile + * units}. + */ + public final double rightSetpoint; + /** + * The right-side feed-forward throttle bump, in fractions of motor throttle (i.e. 0.5 == 50% of + * max motor throttle). + */ + public final double rightBump; + + public ProfileDriveSignal(double lSetpoint, double lBump, double rSetpoint, double rBump) { + this.leftSetpoint = lSetpoint; + this.leftBump = lBump; + this.rightSetpoint = rSetpoint; + this.rightBump = rBump; + } + } + + @NotNull + private static Point getCurrentSegment(@NotNull MotionProfile trajectory, double currentTime) { + // Start from the current time and find the closest point. + int startIndex = Math.toIntExact(Math.round(currentTime / trajectory.get(0).dt)); + + int length = trajectory.size(); + int index = startIndex; + if (startIndex >= length - 1) { + index = length - 1; + } + return trajectory.get(index); + } +} From f8dd6d3d11c69ee623f8c237b9ca0e90cfd1da9c Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Sat, 13 Oct 2018 00:14:27 -0700 Subject: [PATCH 034/145] Make ProfileFollower reusable Also add a method to get the integral accumulator --- .../base/motionprofiling/FollowProfile.java | 6 ++--- .../base/motionprofiling/ProfileFollower.java | 25 +++++++++++++++++++ 2 files changed, 28 insertions(+), 3 deletions(-) diff --git a/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java b/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java index d10cb00e..bb01a16a 100644 --- a/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java +++ b/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java @@ -83,6 +83,8 @@ public FollowProfile(@NotNull MotionProfile left, @NotNull MotionProfile right, this.headingP = headingP; this.headingI = headingI; + follower = new ProfileFollower(left, right, velCoeff, velIntercept, accelCoeff, headingP, + headingI); } @Override @@ -100,9 +102,7 @@ protected void runPeriodic() { @Override protected void runInitial() { - // create a new follower since ProfileFollower is very stateful/doesn't do resets - follower = new ProfileFollower(left, right, velCoeff, velIntercept, accelCoeff, headingP, - headingI); + follower.reset(); timer.start(); } diff --git a/lib/src/main/java/org/team1540/base/motionprofiling/ProfileFollower.java b/lib/src/main/java/org/team1540/base/motionprofiling/ProfileFollower.java index db2fc997..1bb87acb 100644 --- a/lib/src/main/java/org/team1540/base/motionprofiling/ProfileFollower.java +++ b/lib/src/main/java/org/team1540/base/motionprofiling/ProfileFollower.java @@ -19,6 +19,11 @@ *

* It is designed to use native Talon SRX position PID control with a throttle bump, but the output * could instead be used to control a RIO-side PID loop. + *

+ * This class is stateful; it keeps track of the last time {@link #get(double, double)} get()} was + * called, and also has an integral accumulator for the gyro PI controller. With that in mind, if + * using a {@code FollowProfile} instance multiple times, call {@link #reset()} before beginning to + * execute the second, third, etc. profiles. */ public class ProfileFollower { @@ -113,6 +118,26 @@ public ProfileDriveSignal get(double heading, double time) { ); } + /** + * Reset the profile follower. + * + * This resets the follower so that it can be used multiple times. + */ + public void reset() { + gyroIAccum = 0; + lastTime = -1; + } + + + /** + * Get the current integral accumulator for the gyro PI controller. + * + * @return The integral accumulator. + */ + public double getGyroIAccum() { + return gyroIAccum; + } + /** * Gets the total time to execute the profile. * From 8a78f87725074c0fc9aa4503c6e1c3533cd0ab35 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Sat, 13 Oct 2018 00:20:06 -0700 Subject: [PATCH 035/145] Add diagnostic methods --- .../base/motionprofiling/ProfileFollower.java | 38 ++++++++++++++++++- 1 file changed, 37 insertions(+), 1 deletion(-) diff --git a/lib/src/main/java/org/team1540/base/motionprofiling/ProfileFollower.java b/lib/src/main/java/org/team1540/base/motionprofiling/ProfileFollower.java index 1bb87acb..faf2e05e 100644 --- a/lib/src/main/java/org/team1540/base/motionprofiling/ProfileFollower.java +++ b/lib/src/main/java/org/team1540/base/motionprofiling/ProfileFollower.java @@ -128,7 +128,6 @@ public void reset() { lastTime = -1; } - /** * Get the current integral accumulator for the gyro PI controller. * @@ -138,6 +137,43 @@ public double getGyroIAccum() { return gyroIAccum; } + + /** + * Get the current gyro error being fed into the PI controller. + * + * @param heading The current gyro heading reading, in radians from 0 to 2π. + * @param time The current time, in seconds. + * @return The heading error, in radians. + */ + public double getGyroError(double heading, double time) { + double tgtHeading = getCurrentPointLeft(time).heading; + + return atan2(sin(heading - tgtHeading), cos(heading - tgtHeading)); + } + + /** + * Get the currently executing {@code Point} on the left side. + * + * @param time The current time, in seconds. + * @return The current {@code Point}. + */ + @NotNull + public Point getCurrentPointLeft(double time) { + return getCurrentSegment(left, time); + } + + + /** + * Get the currently executing {@code Point} on the right side. + * + * @param time The current time, in seconds. + * @return The current {@code Point}. + */ + @NotNull + public Point getCurrentPointRight(double time) { + return getCurrentSegment(right, time); + } + /** * Gets the total time to execute the profile. * From 8c1a2fd41ba4fb29c7eb9a66af5ec78e77b0a1d9 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Sat, 13 Oct 2018 00:37:20 -0700 Subject: [PATCH 036/145] Add some information getters to FollowProfile --- .../base/motionprofiling/FollowProfile.java | 26 ++++++++++++++++--- 1 file changed, 23 insertions(+), 3 deletions(-) diff --git a/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java b/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java index bb01a16a..50f6ccbd 100644 --- a/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java +++ b/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java @@ -8,9 +8,9 @@ import org.team1540.base.util.AsyncCommand; /** - * {@link edu.wpi.first.wpilibj.command.Command Command} to execute a motion profile. This is - * an {@link AsyncCommand}-based wrapper around {@link ProfileFollower} which handles creating - * the instance, running it in a fast loop, and sending the output to the motors. + * {@link edu.wpi.first.wpilibj.command.Command Command} to execute a motion profile. This is an + * {@link AsyncCommand}-based wrapper around {@link ProfileFollower} which handles creating the + * instance, running it in a fast loop, and sending the output to the motors. * * @see FollowProfileFactory * @see ProfileFollower @@ -87,6 +87,26 @@ public FollowProfile(@NotNull MotionProfile left, @NotNull MotionProfile right, headingI); } + /** + * Get the time since starting the motion profile. + * + * @return The time, in seconds, since beginning to execute a motion profile, or 0 if not + * currently executing a profile. + */ + public double getExecutionTime() { + return isRunning() ? timer.get() : 0; + } + + /** + * Get the underlying {@link ProfileFollower} + * + * @return The underlying {@link ProfileFollower}. Any modifications to this object will affect + * this command. + */ + public ProfileFollower getFollower() { + return follower; + } + @Override protected void runPeriodic() { // check finish status From a42686421ef62e024b18257c8f6fea19539e1303 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Sat, 13 Oct 2018 00:37:35 -0700 Subject: [PATCH 037/145] Put heading info on SmartDashboard during testing --- .../base/testing/MotionProfileTesting.kt | 25 +++++++++++++------ 1 file changed, 18 insertions(+), 7 deletions(-) diff --git a/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt b/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt index 406c0db0..239a0bfa 100644 --- a/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt +++ b/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt @@ -14,16 +14,16 @@ import jaci.pathfinder.Trajectory import jaci.pathfinder.Waypoint import jaci.pathfinder.modifiers.TankModifier import org.team1540.base.Utilities +import org.team1540.base.motionprofiling.FollowProfile import org.team1540.base.motionprofiling.FollowProfileFactory import org.team1540.base.motionprofiling.MotionProfileUtils import org.team1540.base.preferencemanager.Preference import org.team1540.base.preferencemanager.PreferenceManager import org.team1540.base.util.Executable -import org.team1540.base.util.SimpleCommand import org.team1540.base.util.SimpleLoopCommand import org.team1540.base.wrappers.ChickenTalon import java.util.function.DoubleSupplier -import javax.rmi.CORBA.Util +import kotlin.math.PI private val driver = Joystick(0) @@ -32,6 +32,8 @@ class MotionProfileTestingRobot : IterativeRobot() { private val navx = AHRS(SPI.Port.kMXP) + private var command: FollowProfile? = null + @JvmField @Preference("kV", persistent = false) var kV = 0.0 @@ -108,9 +110,7 @@ class MotionProfileTestingRobot : IterativeRobot() { loopFreq = loopFreqMs headingP = hdgP headingI = hdgI - headingSupplier = DoubleSupplier { - Math.toRadians(navx.yaw.let { if (it < 0) it + 180 else it }.toDouble()) - } + headingSupplier = DoubleSupplier { processedHeading } } } @@ -121,6 +121,11 @@ class MotionProfileTestingRobot : IterativeRobot() { Scheduler.getInstance().run() if (DriveTrain.p != driveP) DriveTrain.p = driveP if (DriveTrain.d != driveP) DriveTrain.d = driveD + SmartDashboard.putNumber("Raw heading", navx.yaw.toDouble()) + SmartDashboard.putNumber("Processed heading", processedHeading) + SmartDashboard.putNumber("Heading Error", command?.follower?.getGyroError(processedHeading, command?.executionTime + ?: 0.0) ?: 0.0) + SmartDashboard.putNumber("Heading IAccum", command?.follower?.gyroIAccum ?: 0.0) } override fun disabledInit() { @@ -133,6 +138,12 @@ class MotionProfileTestingRobot : IterativeRobot() { override fun teleopPeriodic() { } + val processedHeading: Double + get() { + val yaw = Math.toRadians(navx.yaw.toDouble()) + return if (yaw < 0) PI - yaw else PI + yaw + } + override fun autonomousInit() { DriveTrain.brake = true factory.apply { @@ -163,9 +174,9 @@ class MotionProfileTestingRobot : IterativeRobot() { maxJerk ))).modify(wheelbase).trajectories - val command = factory.create(MotionProfileUtils.createProfile(leftTrajectory), MotionProfileUtils.createProfile(rightTrajectory)) + command = factory.create(MotionProfileUtils.createProfile(leftTrajectory), MotionProfileUtils.createProfile(rightTrajectory)) - command.start() + command?.start() } override fun teleopInit() { From 359dcbe7d409635fe660c032c314f532485d5356 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Sat, 13 Oct 2018 00:41:55 -0700 Subject: [PATCH 038/145] Put lots of drive info on the SmartDashboard --- .../team1540/base/testing/MotionProfileTesting.kt | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt b/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt index 239a0bfa..a9f08be5 100644 --- a/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt +++ b/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt @@ -262,6 +262,21 @@ private object DriveTrain : Subsystem() { } } + override fun periodic() { + SmartDashboard.putNumber("LPOS", left1.selectedSensorPosition) + SmartDashboard.putNumber("RPOS", right1.selectedSensorPosition) + SmartDashboard.putNumber("LVEL", left1.selectedSensorVelocity) + SmartDashboard.putNumber("RVEL", right1.selectedSensorVelocity) + SmartDashboard.putNumber("LTHROT", left1.motorOutputPercent) + SmartDashboard.putNumber("RTHROT", right1.motorOutputPercent) + SmartDashboard.putNumber("LCURR", left1.outputCurrent + left2.outputCurrent + left3.outputCurrent) + SmartDashboard.putNumber("RCURR", right1.outputCurrent + right2.outputCurrent + right3.outputCurrent) + SmartDashboard.putNumber("LVOLT", left1.motorOutputVoltage) + SmartDashboard.putNumber("RVOLT", right1.motorOutputVoltage) + SmartDashboard.putNumber("LERR", left1.closedLoopError.toDouble()) + SmartDashboard.putNumber("RERR", right1.closedLoopError.toDouble()) + } + init { reset() } From de5bf4e4781b213f13fd99b5cb3b251178aa3915 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Sat, 13 Oct 2018 13:10:12 -0700 Subject: [PATCH 039/145] Create WheelbaseTestRobot --- .../Deploy_WheelbaseTestRobot.xml | 21 ++ .../base/util/robots/WheelbaseTestRobot.java | 180 ++++++++++++++++++ 2 files changed, 201 insertions(+) create mode 100644 .idea/runConfigurations/Deploy_WheelbaseTestRobot.xml create mode 100644 lib/src/main/java/org/team1540/base/util/robots/WheelbaseTestRobot.java diff --git a/.idea/runConfigurations/Deploy_WheelbaseTestRobot.xml b/.idea/runConfigurations/Deploy_WheelbaseTestRobot.xml new file mode 100644 index 00000000..418fb69f --- /dev/null +++ b/.idea/runConfigurations/Deploy_WheelbaseTestRobot.xml @@ -0,0 +1,21 @@ + + + + + + + true + + + \ No newline at end of file diff --git a/lib/src/main/java/org/team1540/base/util/robots/WheelbaseTestRobot.java b/lib/src/main/java/org/team1540/base/util/robots/WheelbaseTestRobot.java new file mode 100644 index 00000000..d39f5fa4 --- /dev/null +++ b/lib/src/main/java/org/team1540/base/util/robots/WheelbaseTestRobot.java @@ -0,0 +1,180 @@ +package org.team1540.base.util.robots; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import edu.wpi.first.wpilibj.IterativeRobot; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.command.Scheduler; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import org.team1540.base.preferencemanager.Preference; +import org.team1540.base.preferencemanager.PreferenceManager; +import org.team1540.base.util.SimpleCommand; +import org.team1540.base.wrappers.ChickenTalon; + +/** + * Class to determine a robot's wheelbase width. For use instructions, load onto a robot and check + * the console. + */ +public class WheelbaseTestRobot extends IterativeRobot { + + @Preference(persistent = false) + public boolean logDataToCSV = false; + @Preference(persistent = false) + public int lMotor1ID = -1; + @Preference(persistent = false) + public int lMotor2ID = -1; + @Preference(persistent = false) + public int lMotor3ID = -1; + @Preference(persistent = false) + public int rMotor1ID = -1; + @Preference(persistent = false) + public int rMotor2ID = -1; + @Preference(persistent = false) + public int rMotor3ID = -1; + @Preference(persistent = false) + public boolean invertLeftMotor = false; + @Preference(persistent = false) + public boolean invertRightMotor = false; + @Preference(persistent = false) + public boolean invertLeftSensor = false; + @Preference(persistent = false) + public boolean invertRightSensor = false; + @Preference(persistent = false) + public boolean brake = false; + @Preference(persistent = false) + public double encoderTPU = 1; + @Preference(persistent = false) + public double setpoint = 0.5; + + private ChickenTalon lMotor1; + private ChickenTalon lMotor2; + private ChickenTalon lMotor3; + private ChickenTalon rMotor1; + private ChickenTalon rMotor2; + private ChickenTalon rMotor3; + + private Joystick joystick = new Joystick(0); + + @Override + public void robotInit() { + System.out.println("Initializing Wheelbase Test Robot"); + System.out.println( + "To change the motors to be used, change the preference values and then run the Reset command to " + + "allow the values to take effect. To disable a motor, set its motor ID to -1. Motor 1 will be " + + "configured as the master Talon and motors 2, 3, and 4 will be slaved to it in follower mode."); + + PreferenceManager.getInstance().add(this); + + Command reset = new SimpleCommand("Reset", () -> { + if (lMotor1ID != -1) { + lMotor1 = new ChickenTalon(lMotor1ID); + } else { + System.err.println("Left Motor 1 must be set!"); + return; + } + if (lMotor2ID != -1) { + lMotor2 = new ChickenTalon(lMotor2ID); + lMotor2.set(ControlMode.Follower, lMotor1.getDeviceID()); + } else { + if (lMotor2 != null) { + lMotor2.set(ControlMode.PercentOutput, 0); + } + lMotor2 = null; + } + if (lMotor3ID != -1) { + lMotor3 = new ChickenTalon(lMotor3ID); + lMotor3.set(ControlMode.Follower, lMotor1.getDeviceID()); + } else { + if (lMotor3 != null) { + lMotor3.set(ControlMode.PercentOutput, 0); + } + lMotor3 = null; + } + + if (rMotor1ID != -1) { + rMotor1 = new ChickenTalon(rMotor1ID); + } else { + System.err.println("Right Motor 1 must be set!"); + return; + } + if (rMotor2ID != -1) { + rMotor2 = new ChickenTalon(rMotor2ID); + rMotor2.set(ControlMode.Follower, rMotor1.getDeviceID()); + } else { + if (rMotor2 != null) { + rMotor2.set(ControlMode.PercentOutput, 0); + } + rMotor2 = null; + } + if (rMotor3ID != -1) { + rMotor3 = new ChickenTalon(rMotor3ID); + rMotor3.set(ControlMode.Follower, rMotor1.getDeviceID()); + } else { + if (rMotor3 != null) { + rMotor3.set(ControlMode.PercentOutput, 0); + } + rMotor3 = null; + } + for (ChickenTalon motor : new ChickenTalon[]{lMotor1, lMotor2, lMotor3, rMotor1, rMotor2, + rMotor3}) { + if (motor != null) { + motor.configClosedloopRamp(0); + motor.configOpenloopRamp(0); + motor.configPeakOutputForward(1); + motor.configPeakOutputReverse(-1); + motor.enableCurrentLimit(false); + motor.setBrake(brake); + } + } + }); + reset.setRunWhenDisabled(true); + reset.start(); + SmartDashboard.putData(reset); + + Command zero = new SimpleCommand("Zero", () -> { + if (lMotor1 != null) { + lMotor1.setSelectedSensorPosition(0); + } + + if (rMotor1 != null) { + rMotor1.setSelectedSensorPosition(0); + } + }); + zero.setRunWhenDisabled(true); + SmartDashboard.putData(zero); + } + + @Override + public void teleopInit() { + System.out.println("Zero encoders, then press A until the robot completes 10 revolutions"); + } + + @Override + public void teleopPeriodic() { + if (joystick.getRawButton(1)) { // button A + lMotor1.set(ControlMode.PercentOutput, setpoint); + rMotor1.set(ControlMode.PercentOutput, -setpoint); + } else { + lMotor1.set(ControlMode.PercentOutput, 0); + rMotor1.set(ControlMode.PercentOutput, 0); + } + } + + @Override + public void robotPeriodic() { + Scheduler.getInstance().run(); + if (lMotor1 != null && rMotor1 != null) { + SmartDashboard.putNumber("LPOS", lMotor1.getSelectedSensorPosition()); + SmartDashboard.putNumber("RPOS", rMotor1.getSelectedSensorPosition()); + + double leftDistance = lMotor1.getSelectedSensorPosition(); + double rightDistance = -rMotor1.getSelectedSensorPosition(); + + SmartDashboard.putNumber("Left Distance", leftDistance); + SmartDashboard.putNumber("Right Distance", rightDistance); + + SmartDashboard.putNumber("Calculated width (assuming 10 rots)", + (((leftDistance + rightDistance) / 2) / (10 * Math.PI)) / encoderTPU); + } + } +} From 6a5ec5bb8e427be231fabf18d792361e3cc932ca Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Wed, 10 Oct 2018 16:40:22 -0700 Subject: [PATCH 040/145] Create CharacterizedDriveHelper Uses Oblarg's CD drivetrain characterization. --- .../base/drive/CharacterizedDriveHelper.java | 43 +++++++++++++++++++ .../base/drive/OpenLoopDriveSignal.java | 23 ++++++++++ 2 files changed, 66 insertions(+) create mode 100644 lib/src/main/java/org/team1540/base/drive/CharacterizedDriveHelper.java create mode 100644 lib/src/main/java/org/team1540/base/drive/OpenLoopDriveSignal.java diff --git a/lib/src/main/java/org/team1540/base/drive/CharacterizedDriveHelper.java b/lib/src/main/java/org/team1540/base/drive/CharacterizedDriveHelper.java new file mode 100644 index 00000000..b66e25b9 --- /dev/null +++ b/lib/src/main/java/org/team1540/base/drive/CharacterizedDriveHelper.java @@ -0,0 +1,43 @@ +package org.team1540.base.drive; + + +/* +Implementation note: +This is not a command, as I realized that with PIDDrive so much of the configuration was simply +dealing with scaling the joysticks and creating a full pipeline from joystick to motor controller. +This just takes speed values in a method, and outputs a drive signal. This also means it can +be plugged into a motion profile executor. +*/ + +import org.jetbrains.annotations.Contract; + +public class CharacterizedDriveHelper { + + private final double kV; + private final double vIntercept; + + // TODO: Add explanation of units in class docs + + /** + * Creates a {@code CharacterizedDriveHelper} with the provided \(k_v\) and \(v_{Intercept}\) + * + * @param kV The velocity constant feed-forward, in output units per speed unit. + * @param vIntercept The velocity intercept, in output units + */ + public CharacterizedDriveHelper(double kV, double vIntercept) { + this.kV = kV; + this.vIntercept = vIntercept; + } + + @Contract(pure = true) + public OpenLoopDriveSignal getDriveSignal(double leftSpeed, double rightSpeed) { + double left = getThrottle(leftSpeed); + double right = getThrottle(rightSpeed); + return new OpenLoopDriveSignal(left, right); + } + + @Contract(pure = true) + private double getThrottle(double wantedSpeed) { + return (kV * wantedSpeed) + vIntercept; + } +} diff --git a/lib/src/main/java/org/team1540/base/drive/OpenLoopDriveSignal.java b/lib/src/main/java/org/team1540/base/drive/OpenLoopDriveSignal.java new file mode 100644 index 00000000..5d0d6aed --- /dev/null +++ b/lib/src/main/java/org/team1540/base/drive/OpenLoopDriveSignal.java @@ -0,0 +1,23 @@ +package org.team1540.base.drive; + +/** + * Data structure for a drive's left and right throttles. + */ +public class OpenLoopDriveSignal { + + private final double leftThrottle; + private final double rightThrottle; + + public OpenLoopDriveSignal(double leftThrottle, double rightThrottle) { + this.leftThrottle = leftThrottle; + this.rightThrottle = rightThrottle; + } + + public double getLeftThrottle() { + return leftThrottle; + } + + public double getRightThrottle() { + return rightThrottle; + } +} From 984d8d2caf2428fbef921da81875d0a8e35fbaa3 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 15 Oct 2018 10:23:04 -0700 Subject: [PATCH 041/145] Create DrivePipeline --- .../team1540/base/drive/DrivePipeline.java | 44 +++++++++++++++++++ 1 file changed, 44 insertions(+) create mode 100644 lib/src/main/java/org/team1540/base/drive/DrivePipeline.java diff --git a/lib/src/main/java/org/team1540/base/drive/DrivePipeline.java b/lib/src/main/java/org/team1540/base/drive/DrivePipeline.java new file mode 100644 index 00000000..d3a2f531 --- /dev/null +++ b/lib/src/main/java/org/team1540/base/drive/DrivePipeline.java @@ -0,0 +1,44 @@ +package org.team1540.base.drive; + +import java.util.function.Consumer; +import java.util.function.Function; +import java.util.function.Supplier; + +/** + * Pipeline for flexibly controlling a robot drivetrain. A {@code DrivePipeline} 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 stages: an input, a processor, and an + * output. Since inputs, processors, and outputs are just {@link Supplier Suppliers}, + * {@link Function Functions}, and {@link Consumer 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. 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.
    • + *
      + * + * @param The type given by the input stage. + * @param The type requested by the output stage. + */ +public class DrivePipeline { + + private Supplier input; + private Function processor; + private Consumer output; + + + public void execute() { + output.accept(processor.apply(input.get())); + } +} From 9ccff000ca0f9b78673389b8d96e33196e4f8d2a Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 15 Oct 2018 10:23:19 -0700 Subject: [PATCH 042/145] Create base data classes --- .../base/drive/pipeline/DriveData.java | 41 +++++++++++++++++++ .../base/drive/pipeline/TankDriveData.java | 36 ++++++++++++++++ 2 files changed, 77 insertions(+) create mode 100644 lib/src/main/java/org/team1540/base/drive/pipeline/DriveData.java create mode 100644 lib/src/main/java/org/team1540/base/drive/pipeline/TankDriveData.java diff --git a/lib/src/main/java/org/team1540/base/drive/pipeline/DriveData.java b/lib/src/main/java/org/team1540/base/drive/pipeline/DriveData.java new file mode 100644 index 00000000..e879164c --- /dev/null +++ b/lib/src/main/java/org/team1540/base/drive/pipeline/DriveData.java @@ -0,0 +1,41 @@ +package org.team1540.base.drive.pipeline; + +import java.util.OptionalDouble; + +@SuppressWarnings("OptionalUsedAsFieldOrParameterType") +public class DriveData { + + /** + * The desired position in position-units, or an empty optional if velocity should not be + * controlled. + */ + public final OptionalDouble position; + /** + * The desired velocity in position-units per second, or an empty optional if velocity should not + * be controlled. + */ + public final OptionalDouble velocity; + /** + * The desired acceleration in position-units per second squared, or an empty optional if + * acceleration should not be controlled. + */ + 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. + */ + public final OptionalDouble additionalFeedForward; + + public DriveData(OptionalDouble velocity) { + this(OptionalDouble.empty(), velocity, OptionalDouble.empty(), OptionalDouble.empty()); + } + + public DriveData(OptionalDouble position, OptionalDouble velocity, + OptionalDouble acceleration, OptionalDouble additionalFeedForward) { + this.position = position; + this.velocity = velocity; + this.acceleration = acceleration; + this.additionalFeedForward = additionalFeedForward; + } +} diff --git a/lib/src/main/java/org/team1540/base/drive/pipeline/TankDriveData.java b/lib/src/main/java/org/team1540/base/drive/pipeline/TankDriveData.java new file mode 100644 index 00000000..f5f5febf --- /dev/null +++ b/lib/src/main/java/org/team1540/base/drive/pipeline/TankDriveData.java @@ -0,0 +1,36 @@ +package org.team1540.base.drive.pipeline; + +import java.util.OptionalDouble; + +@SuppressWarnings("OptionalUsedAsFieldOrParameterType") +public class TankDriveData { + + /** + * The drive data for the left side. + */ + public final DriveData left; + /** + * The drive data for the right side. + */ + public final DriveData right; + /** + * The desired turning rate in radians from 0 (straight forward) to 2π, increasing clockwise, + * or an empty optional if heading should not be controlled. + */ + public final OptionalDouble heading; + /** + * The desired turning rate in radians/sec, or an empty optional if turning rate should not be + * controlled. + */ + public final OptionalDouble turningRate; + + public TankDriveData(DriveData left, DriveData right, OptionalDouble heading, + OptionalDouble turningRate) { + this.left = left; + this.right = right; + this.heading = heading; + this.turningRate = turningRate; + } + + +} From 4d680b4a7597a9920e9e25692a79e75c60db389e Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 15 Oct 2018 10:23:47 -0700 Subject: [PATCH 043/145] Convert CharacterizedDriveHelper to new pipeline --- .../base/drive/CharacterizedDriveHelper.java | 43 ----------- .../drive/OpenLoopCharacterizedProcessor.java | 74 +++++++++++++++++++ 2 files changed, 74 insertions(+), 43 deletions(-) delete mode 100644 lib/src/main/java/org/team1540/base/drive/CharacterizedDriveHelper.java create mode 100644 lib/src/main/java/org/team1540/base/drive/OpenLoopCharacterizedProcessor.java diff --git a/lib/src/main/java/org/team1540/base/drive/CharacterizedDriveHelper.java b/lib/src/main/java/org/team1540/base/drive/CharacterizedDriveHelper.java deleted file mode 100644 index b66e25b9..00000000 --- a/lib/src/main/java/org/team1540/base/drive/CharacterizedDriveHelper.java +++ /dev/null @@ -1,43 +0,0 @@ -package org.team1540.base.drive; - - -/* -Implementation note: -This is not a command, as I realized that with PIDDrive so much of the configuration was simply -dealing with scaling the joysticks and creating a full pipeline from joystick to motor controller. -This just takes speed values in a method, and outputs a drive signal. This also means it can -be plugged into a motion profile executor. -*/ - -import org.jetbrains.annotations.Contract; - -public class CharacterizedDriveHelper { - - private final double kV; - private final double vIntercept; - - // TODO: Add explanation of units in class docs - - /** - * Creates a {@code CharacterizedDriveHelper} with the provided \(k_v\) and \(v_{Intercept}\) - * - * @param kV The velocity constant feed-forward, in output units per speed unit. - * @param vIntercept The velocity intercept, in output units - */ - public CharacterizedDriveHelper(double kV, double vIntercept) { - this.kV = kV; - this.vIntercept = vIntercept; - } - - @Contract(pure = true) - public OpenLoopDriveSignal getDriveSignal(double leftSpeed, double rightSpeed) { - double left = getThrottle(leftSpeed); - double right = getThrottle(rightSpeed); - return new OpenLoopDriveSignal(left, right); - } - - @Contract(pure = true) - private double getThrottle(double wantedSpeed) { - return (kV * wantedSpeed) + vIntercept; - } -} diff --git a/lib/src/main/java/org/team1540/base/drive/OpenLoopCharacterizedProcessor.java b/lib/src/main/java/org/team1540/base/drive/OpenLoopCharacterizedProcessor.java new file mode 100644 index 00000000..9aaa840e --- /dev/null +++ b/lib/src/main/java/org/team1540/base/drive/OpenLoopCharacterizedProcessor.java @@ -0,0 +1,74 @@ +package org.team1540.base.drive; + + +/* +Implementation note: +This is not a command, as I realized that with PIDDrive so much of the configuration was simply +dealing with scaling the joysticks and creating a full pipeline from joystick to motor controller. +This just takes speed values in a method, and outputs a drive signal. This also means it can +be plugged into a motion profile executor. +*/ + +import java.util.OptionalDouble; +import java.util.function.Function; +import org.jetbrains.annotations.Contract; +import org.team1540.base.drive.pipeline.DriveData; +import org.team1540.base.drive.pipeline.TankDriveData; + +public class OpenLoopCharacterizedProcessor implements Function { + + private final double kV; + private final double vIntercept; + private final double kA; + + // TODO: Add explanation of units in class docs + + /** + * Creates a {@code CharacterizedDriveHelper} with the provided \(k_v\) and \(v_{Intercept}\) + * + * @param kV The velocity constant feed-forward, in output units per speed unit. + * @param vIntercept The velocity intercept, in output units + * @param kA The acceleration constant feed-forward, in output units per acceleration unit. + */ + public OpenLoopCharacterizedProcessor(double kV, double vIntercept, double kA) { + this.kV = kV; + this.vIntercept = vIntercept; + this.kA = kA; + } + + @Contract(pure = true) + private OpenLoopDriveSignal getDriveSignal(double leftSpeed, double leftAccel, double rightSpeed, + double rightAccel) { + double left = getThrottle(leftSpeed, leftAccel); + double right = getThrottle(rightSpeed, rightAccel); + return new OpenLoopDriveSignal(left, right); + } + + @Contract(pure = true) + private double getThrottle(double wantedSpeed, double wantedAccel) { + return (kV * wantedSpeed) + + (kA * wantedAccel) + + (wantedSpeed != 0 ? Math.copySign(vIntercept, wantedSpeed) : 0); + } + + @Override + public TankDriveData apply(TankDriveData command) { + OpenLoopDriveSignal signal = getDriveSignal( + command.left.velocity.orElse(0), + command.left.acceleration.orElse(0), + command.right.velocity.orElse(0), + command.right.acceleration.orElse(0)); + + return new TankDriveData( + new DriveData(command.left.position, + OptionalDouble.empty(), + OptionalDouble.empty(), + OptionalDouble.of(signal.getLeftThrottle())), + new DriveData(command.right.position, + OptionalDouble.empty(), + OptionalDouble.empty(), + OptionalDouble.of(signal.getRightThrottle())), + command.heading, + command.turningRate); + } +} From f173004c177da0cbfba463d14a142acd25fb97bd Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 15 Oct 2018 10:26:03 -0700 Subject: [PATCH 044/145] Move OpenLoopDriveSignal into OpenLoopCharacterizedProcessor --- .../drive/OpenLoopCharacterizedProcessor.java | 22 ++++++++++++++++++ .../base/drive/OpenLoopDriveSignal.java | 23 ------------------- 2 files changed, 22 insertions(+), 23 deletions(-) delete mode 100644 lib/src/main/java/org/team1540/base/drive/OpenLoopDriveSignal.java diff --git a/lib/src/main/java/org/team1540/base/drive/OpenLoopCharacterizedProcessor.java b/lib/src/main/java/org/team1540/base/drive/OpenLoopCharacterizedProcessor.java index 9aaa840e..d5964103 100644 --- a/lib/src/main/java/org/team1540/base/drive/OpenLoopCharacterizedProcessor.java +++ b/lib/src/main/java/org/team1540/base/drive/OpenLoopCharacterizedProcessor.java @@ -71,4 +71,26 @@ public TankDriveData apply(TankDriveData command) { command.heading, command.turningRate); } + + /** + * Data structure for a drive's left and right throttles. + */ + private static class OpenLoopDriveSignal { + + private final double leftThrottle; + private final double rightThrottle; + + public OpenLoopDriveSignal(double leftThrottle, double rightThrottle) { + this.leftThrottle = leftThrottle; + this.rightThrottle = rightThrottle; + } + + public double getLeftThrottle() { + return leftThrottle; + } + + public double getRightThrottle() { + return rightThrottle; + } + } } diff --git a/lib/src/main/java/org/team1540/base/drive/OpenLoopDriveSignal.java b/lib/src/main/java/org/team1540/base/drive/OpenLoopDriveSignal.java deleted file mode 100644 index 5d0d6aed..00000000 --- a/lib/src/main/java/org/team1540/base/drive/OpenLoopDriveSignal.java +++ /dev/null @@ -1,23 +0,0 @@ -package org.team1540.base.drive; - -/** - * Data structure for a drive's left and right throttles. - */ -public class OpenLoopDriveSignal { - - private final double leftThrottle; - private final double rightThrottle; - - public OpenLoopDriveSignal(double leftThrottle, double rightThrottle) { - this.leftThrottle = leftThrottle; - this.rightThrottle = rightThrottle; - } - - public double getLeftThrottle() { - return leftThrottle; - } - - public double getRightThrottle() { - return rightThrottle; - } -} From d9199d1f804e5b34d2c16ef54156c1da85aa9122 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 15 Oct 2018 22:35:04 -0700 Subject: [PATCH 045/145] Fix ClassCastException in createProfile() Seriously, why is turning a java stream into an array so stupid? --- .../base/motionprofiling/MotionProfileUtils.java | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/lib/src/main/java/org/team1540/base/motionprofiling/MotionProfileUtils.java b/lib/src/main/java/org/team1540/base/motionprofiling/MotionProfileUtils.java index 4bd7d5b4..0474538b 100644 --- a/lib/src/main/java/org/team1540/base/motionprofiling/MotionProfileUtils.java +++ b/lib/src/main/java/org/team1540/base/motionprofiling/MotionProfileUtils.java @@ -26,8 +26,8 @@ public static SetpointConsumer createSetpointConsumer(BaseMotorController motorC } /** - * Create a {@link SetpointConsumer} that sets the setpoint of the provided CTRE motor - * controller, multiplying the position setpoint by the adjustmant. + * Create a {@link SetpointConsumer} that sets the setpoint of the provided CTRE motor controller, + * multiplying the position setpoint by the adjustmant. * * @param motorController The motor controller. * @return A {@code SetpointConsumer} to use for motion profiling. @@ -45,12 +45,15 @@ public static SetpointConsumer createSetpointConsumer(BaseMotorController motorC * * @param trajectory The {@link Trajectory} to convert. * @return A {@link MotionProfile} containing the same points. Profile points are copied over, so - * subsequent changes to the {@link Trajectory} will not affect the produced {@link MotionProfile}. + * subsequent changes to the {@link Trajectory} will not affect the produced {@link + * MotionProfile}. */ @Contract("_ -> new") @NotNull public static MotionProfile createProfile(@NotNull Trajectory trajectory) { - return new MotionProfile((Point[]) Arrays.stream(trajectory.segments).map(s -> new Point(s.dt, s.x, s.y, s.position, s.velocity, s.acceleration, s.jerk, s.heading)).toArray()); + return new MotionProfile(Arrays.stream(trajectory.segments).map( + s -> new Point(s.dt, s.x, s.y, s.position, s.velocity, s.acceleration, s.jerk, s.heading)) + .toArray(Point[]::new)); } } From 447817c0253d1a733e499b6e1bf039610c7c9b9e Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Tue, 16 Oct 2018 08:08:53 -0700 Subject: [PATCH 046/145] Move OpenLoopCharacterizedProcessor to pipeline --- .../drive/{ => pipeline}/OpenLoopCharacterizedProcessor.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) rename lib/src/main/java/org/team1540/base/drive/{ => pipeline}/OpenLoopCharacterizedProcessor.java (95%) diff --git a/lib/src/main/java/org/team1540/base/drive/OpenLoopCharacterizedProcessor.java b/lib/src/main/java/org/team1540/base/drive/pipeline/OpenLoopCharacterizedProcessor.java similarity index 95% rename from lib/src/main/java/org/team1540/base/drive/OpenLoopCharacterizedProcessor.java rename to lib/src/main/java/org/team1540/base/drive/pipeline/OpenLoopCharacterizedProcessor.java index d5964103..44523e06 100644 --- a/lib/src/main/java/org/team1540/base/drive/OpenLoopCharacterizedProcessor.java +++ b/lib/src/main/java/org/team1540/base/drive/pipeline/OpenLoopCharacterizedProcessor.java @@ -1,4 +1,4 @@ -package org.team1540.base.drive; +package org.team1540.base.drive.pipeline; /* @@ -12,8 +12,6 @@ import java.util.OptionalDouble; import java.util.function.Function; import org.jetbrains.annotations.Contract; -import org.team1540.base.drive.pipeline.DriveData; -import org.team1540.base.drive.pipeline.TankDriveData; public class OpenLoopCharacterizedProcessor implements Function { From 9a1d0f7c719489e60bdfd9d75778c0c34857be4e Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Tue, 16 Oct 2018 08:10:47 -0700 Subject: [PATCH 047/145] Convert loop frequency to an int Longs aren't accepted by NetworkTables apparently --- .../org/team1540/base/testing/MotionProfileTesting.kt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt b/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt index a9f08be5..4a2b30e3 100644 --- a/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt +++ b/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt @@ -48,7 +48,7 @@ class MotionProfileTestingRobot : IterativeRobot() { @JvmField @Preference("MP Loop Freq", persistent = false) - var loopFreqMs = 0L + var loopFreqMs = 0 @JvmField @Preference("MP Heading P", persistent = false) @@ -107,7 +107,7 @@ class MotionProfileTestingRobot : IterativeRobot() { velIntercept = vIntercept velCoeff = kV accelCoeff = kA - loopFreq = loopFreqMs + loopFreq = loopFreqMs.toLong() headingP = hdgP headingI = hdgI headingSupplier = DoubleSupplier { processedHeading } @@ -150,7 +150,7 @@ class MotionProfileTestingRobot : IterativeRobot() { velIntercept = vIntercept velCoeff = kV accelCoeff = kA - loopFreq = loopFreqMs + loopFreq = loopFreqMs.toLong() headingP = hdgP headingI = hdgI leftSetpointConsumer = MotionProfileUtils.createSetpointConsumer(DriveTrain.left1, tpu) From 7d1b33599b1a66d93447e7683f4fae5b802f5b24 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Tue, 16 Oct 2018 08:11:16 -0700 Subject: [PATCH 048/145] Add function to zero drivetrain --- .../org/team1540/base/testing/MotionProfileTesting.kt | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt b/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt index 4a2b30e3..0fab5937 100644 --- a/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt +++ b/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt @@ -146,6 +146,7 @@ class MotionProfileTestingRobot : IterativeRobot() { override fun autonomousInit() { DriveTrain.brake = true + DriveTrain.zero() factory.apply { velIntercept = vIntercept velCoeff = kV @@ -227,6 +228,11 @@ private object DriveTrain : Subsystem() { }, this) } + fun zero() { + left1.setSelectedSensorPosition(0) + right1.setSelectedSensorPosition(0) + } + private fun reset() { left1.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder) right1.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder) From dbf842a09327fd34caea5d87126e629e9d491945 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Tue, 16 Oct 2018 08:11:48 -0700 Subject: [PATCH 049/145] Make sure voltage compensation is enabled --- .../kotlin/org/team1540/base/testing/MotionProfileTesting.kt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt b/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt index 0fab5937..9c21906b 100644 --- a/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt +++ b/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt @@ -265,6 +265,8 @@ private object DriveTrain : Subsystem() { talon.config_kD(0, d) talon.config_kF(0, 0.0) talon.config_IntegralZone(0, 0) + talon.configVoltageCompSaturation(12.0) + talon.enableVoltageCompensation(true) } } From 467975ca6897396b6177aa3e66f9f4c35e0b6e79 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Tue, 16 Oct 2018 08:12:18 -0700 Subject: [PATCH 050/145] Put target info on SmartDashboard --- .../kotlin/org/team1540/base/testing/MotionProfileTesting.kt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt b/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt index 9c21906b..8d7ae8d5 100644 --- a/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt +++ b/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt @@ -283,6 +283,8 @@ private object DriveTrain : Subsystem() { SmartDashboard.putNumber("RVOLT", right1.motorOutputVoltage) SmartDashboard.putNumber("LERR", left1.closedLoopError.toDouble()) SmartDashboard.putNumber("RERR", right1.closedLoopError.toDouble()) + SmartDashboard.putNumber("LTGT", right1.getClosedLoopTarget(0).toDouble()) + SmartDashboard.putNumber("RTGT", right1.getClosedLoopTarget(0).toDouble()) } init { From 2f3ba2fc7cb8c40f7664ab792008c24ba52d0f63 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Tue, 16 Oct 2018 08:13:52 -0700 Subject: [PATCH 051/145] Create input classes ProfileInput allows loading points from a motion profile; JoystickInput gets targets from, well, a joystick. --- .../base/drive/pipeline/JoystickInput.java | 37 ++++++++++++ .../base/drive/pipeline/ProfileInput.java | 60 +++++++++++++++++++ 2 files changed, 97 insertions(+) create mode 100644 lib/src/main/java/org/team1540/base/drive/pipeline/JoystickInput.java create mode 100644 lib/src/main/java/org/team1540/base/drive/pipeline/ProfileInput.java diff --git a/lib/src/main/java/org/team1540/base/drive/pipeline/JoystickInput.java b/lib/src/main/java/org/team1540/base/drive/pipeline/JoystickInput.java new file mode 100644 index 00000000..f6508653 --- /dev/null +++ b/lib/src/main/java/org/team1540/base/drive/pipeline/JoystickInput.java @@ -0,0 +1,37 @@ +package org.team1540.base.drive.pipeline; + +import edu.wpi.first.wpilibj.Joystick; +import java.util.OptionalDouble; +import java.util.function.Supplier; +import org.team1540.base.Utilities; + +public class JoystickInput implements Supplier { + + 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 maxVelocity; + private double deadzone = 0.1; + + @Override + public TankDriveData get() { + double triggerValue = Utilities.processDeadzone(joystick.getRawAxis(fwdAxis), deadzone) + - Utilities.processDeadzone(joystick.getRawAxis(backAxis), deadzone); + double leftThrottle = Utilities + .processDeadzone(Utilities.invertIf(invertLeft, joystick.getRawAxis(leftAxis)), deadzone); + double rightThrottle = Utilities + .processDeadzone(Utilities.invertIf(invertRight, joystick.getRawAxis(rightAxis)), deadzone); + + return new TankDriveData( + new DriveData(OptionalDouble.of(leftThrottle * maxVelocity)), + new DriveData(OptionalDouble.of(rightThrottle * maxVelocity)), + OptionalDouble.empty(), + OptionalDouble.empty() + ); + } +} diff --git a/lib/src/main/java/org/team1540/base/drive/pipeline/ProfileInput.java b/lib/src/main/java/org/team1540/base/drive/pipeline/ProfileInput.java new file mode 100644 index 00000000..3b5eee1a --- /dev/null +++ b/lib/src/main/java/org/team1540/base/drive/pipeline/ProfileInput.java @@ -0,0 +1,60 @@ +package org.team1540.base.drive.pipeline; + +import edu.wpi.first.wpilibj.Timer; +import java.util.OptionalDouble; +import java.util.function.Supplier; +import org.jetbrains.annotations.NotNull; +import org.team1540.base.motionprofiling.MotionProfile; +import org.team1540.base.motionprofiling.MotionProfile.Point; + +public class ProfileInput implements Supplier { + + private MotionProfile left; + private MotionProfile right; + + private Timer timer = new Timer(); + + public ProfileInput(@NotNull MotionProfile left, @NotNull MotionProfile right) { + this.left = left; + this.right = right; + } + + @NotNull + private static Point getCurrentSegment(@NotNull MotionProfile trajectory, double currentTime) { + // Start from the current time and find the closest point. + int startIndex = Math.toIntExact(Math.round(currentTime / trajectory.get(0).dt)); + + int length = trajectory.size(); + int index = startIndex; + if (startIndex >= length - 1) { + index = length - 1; + } + return trajectory.get(index); + } + + @Override + public TankDriveData get() { + if (timer.get() <= 0) { + timer.start(); + } + + double timeValue = timer.get(); + + Point leftPoint = getCurrentSegment(left, timeValue); + Point rightPoint = getCurrentSegment(right, timeValue); + + return new TankDriveData( + new DriveData( + OptionalDouble.of(leftPoint.position), + OptionalDouble.of(leftPoint.velocity), + OptionalDouble.of(leftPoint.acceleration), + OptionalDouble.empty()), + new DriveData( + OptionalDouble.of(rightPoint.position), + OptionalDouble.of(rightPoint.velocity), + OptionalDouble.of(rightPoint.acceleration), + OptionalDouble.empty()), + OptionalDouble.of(leftPoint.heading), + OptionalDouble.empty()); + } +} From 664d46e4556b8d2ad594a2022c91ee918be0b167 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Tue, 16 Oct 2018 08:36:17 -0700 Subject: [PATCH 052/145] Add a missing / This was causing javadoc to fail on Travis. --- lib/src/main/java/org/team1540/base/drive/DrivePipeline.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/src/main/java/org/team1540/base/drive/DrivePipeline.java b/lib/src/main/java/org/team1540/base/drive/DrivePipeline.java index d3a2f531..e1a42b5f 100644 --- a/lib/src/main/java/org/team1540/base/drive/DrivePipeline.java +++ b/lib/src/main/java/org/team1540/base/drive/DrivePipeline.java @@ -26,7 +26,7 @@ *
    • An output turns values from a processor into commands for motors. 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.
      • - *
        + *
      * * @param The type given by the input stage. * @param The type requested by the output stage. From be57e77a853ba6bd6df454c04c18629afaa1e96b Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Tue, 16 Oct 2018 08:46:36 -0700 Subject: [PATCH 053/145] Configure javadoc to ignore missing tags They were cluttering up the build logs and it annoyed me --- lib/build.gradle | 1 + 1 file changed, 1 insertion(+) diff --git a/lib/build.gradle b/lib/build.gradle index 0953b592..7c37bdb2 100644 --- a/lib/build.gradle +++ b/lib/build.gradle @@ -30,6 +30,7 @@ javadoc { options.addStringOption("doctitle", "ROOSTER API") options.addStringOption("windowtitle", "ROOSTER API") options.addBooleanOption("-allow-script-in-comments", true) + options.addBooleanOption('Xdoclint:all,-missing', true) } From 7ab42317401a79c630d2b5ca93b88fdba6a0e453 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Thu, 4 Oct 2018 12:25:17 -0700 Subject: [PATCH 054/145] Add .idea/jarRepositories.xml to .gitignore Not sure what this does but it seems to be generated from Gradle and is regenerated when deleted. Should be fine to ignore. Cherry-picked from commit 2358e3ad --- .gitignore | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.gitignore b/.gitignore index 74f34cb1..62e84356 100644 --- a/.gitignore +++ b/.gitignore @@ -153,5 +153,7 @@ gradle-app.setting # Cache of project .gradletasknamecache +.idea/jarRepositories.xml + # # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 # gradle/wrapper/gradle-wrapper.properties From 936e008eb9db0133ff71bad195e292d6251c5cc5 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Sat, 20 Oct 2018 21:44:52 -0700 Subject: [PATCH 055/145] Update WPILib dependencies --- lib/build.gradle | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/lib/build.gradle b/lib/build.gradle index 7c37bdb2..da41f6f7 100644 --- a/lib/build.gradle +++ b/lib/build.gradle @@ -35,11 +35,11 @@ javadoc { 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" From e94450dceabdabc9c736d57965a3e7a1ce602f70 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Sat, 20 Oct 2018 21:50:25 -0700 Subject: [PATCH 056/145] Remove manual installation instructions Since everyone is on Gradle now there's no need. --- README.md | 17 +---------------- 1 file changed, 1 insertion(+), 16 deletions(-) diff --git a/README.md b/README.md index 7e63b16b..acc70990 100644 --- a/README.md +++ b/README.md @@ -68,13 +68,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,20 +91,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 -- [Apache Commons Math 3](https://mvnrepository.com/artifact/org.apache.commons/commons-math3/3.6.1) version 3.6.1 or higher - ## Developing ROOSTER ### Project Structure From 2256869a20e3ae4a58b8772d5569f83e352d9964 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Fri, 26 Oct 2018 16:10:42 -0700 Subject: [PATCH 057/145] Add changeable motors to VelocityCharacterizationRobot This code is generally taken from the drive-data branch. It also adds TPU support. --- .../robots/VelocityCharacterizationRobot.java | 200 ++++++++++++------ 1 file changed, 139 insertions(+), 61 deletions(-) diff --git a/lib/src/main/java/org/team1540/base/util/robots/VelocityCharacterizationRobot.java b/lib/src/main/java/org/team1540/base/util/robots/VelocityCharacterizationRobot.java index 77f7dea4..32f78e85 100644 --- a/lib/src/main/java/org/team1540/base/util/robots/VelocityCharacterizationRobot.java +++ b/lib/src/main/java/org/team1540/base/util/robots/VelocityCharacterizationRobot.java @@ -1,41 +1,62 @@ package org.team1540.base.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.command.Command; import edu.wpi.first.wpilibj.command.Scheduler; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import org.apache.commons.math3.stat.regression.SimpleRegression; import org.jetbrains.annotations.NotNull; import org.team1540.base.preferencemanager.Preference; import org.team1540.base.preferencemanager.PreferenceManager; +import org.team1540.base.util.SimpleCommand; import org.team1540.base.wrappers.ChickenTalon; +//TODO MORE DOCS + /** * Self-contained robot class to characterize a drivetrain's velocity 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 a quasi-static velocity characterization as described in Eli Barnett's paper "FRC - * Drivetrain Characterization" until the button is released. The results (kV, vIntercept, and an - * R^2 value) are output to the SmartDashboard. + * Whenever the A button is pressed and held during teleop the robot will carry out a quasi-static + * velocity characterization as described in Eli Barnett's paper "FRC Drivetrain Characterization" + * until the button is released. The results (kV, vIntercept, and an R^2 value) are output to the + * SmartDashboard. */ public class VelocityCharacterizationRobot extends IterativeRobot { - 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}; + @Preference(persistent = false) + public int lMotor1ID = -1; + @Preference(persistent = false) + public int lMotor2ID = -1; + @Preference(persistent = false) + public int lMotor3ID = -1; + @Preference(persistent = false) + public int rMotor1ID = -1; + @Preference(persistent = false) + public int rMotor2ID = -1; + @Preference(persistent = false) + public int rMotor3ID = -1; + @Preference(persistent = false) + public boolean invertLeftMotor = false; + @Preference(persistent = false) + public boolean invertRightMotor = false; + @Preference(persistent = false) + public boolean invertLeftSensor = false; + @Preference(persistent = false) + public boolean invertRightSensor = false; + @Preference(persistent = false) + public boolean brake = false; + @Preference(persistent = false) + public double tpu = 1; + + + private ChickenTalon driveLeftMotorA; + private ChickenTalon driveLeftMotorB; + private ChickenTalon driveLeftMotorC; + private ChickenTalon driveRightMotorA; + private ChickenTalon driveRightMotorB; + private ChickenTalon driveRightMotorC; private SimpleRegression leftRegression = new SimpleRegression(); private SimpleRegression rightRegression = new SimpleRegression(); @@ -59,7 +80,84 @@ public class VelocityCharacterizationRobot extends IterativeRobot { @Override public void robotInit() { PreferenceManager.getInstance().add(this); - reset(); + Command reset = new SimpleCommand("Reset", () -> { + if (lMotor1ID != -1) { + driveLeftMotorA = new ChickenTalon(lMotor1ID); + } else { + System.err.println("Left Motor 1 must be set!"); + return; + } + if (lMotor2ID != -1) { + driveLeftMotorB = new ChickenTalon(lMotor2ID); + driveLeftMotorB.set(ControlMode.Follower, driveLeftMotorA.getDeviceID()); + } else { + if (driveLeftMotorB != null) { + driveLeftMotorB.set(ControlMode.PercentOutput, 0); + } + driveLeftMotorB = null; + } + if (lMotor3ID != -1) { + driveLeftMotorC = new ChickenTalon(lMotor3ID); + driveLeftMotorC.set(ControlMode.Follower, driveLeftMotorA.getDeviceID()); + } else { + if (driveLeftMotorC != null) { + driveLeftMotorC.set(ControlMode.PercentOutput, 0); + } + driveLeftMotorC = null; + } + + if (rMotor1ID != -1) { + driveRightMotorA = new ChickenTalon(rMotor1ID); + } else { + System.err.println("Right Motor 1 must be set!"); + return; + } + if (rMotor2ID != -1) { + driveRightMotorB = new ChickenTalon(rMotor2ID); + driveRightMotorB.set(ControlMode.Follower, driveRightMotorA.getDeviceID()); + } else { + if (driveRightMotorB != null) { + driveRightMotorB.set(ControlMode.PercentOutput, 0); + } + driveRightMotorB = null; + } + if (rMotor3ID != -1) { + driveRightMotorC = new ChickenTalon(rMotor3ID); + driveRightMotorC.set(ControlMode.Follower, driveRightMotorA.getDeviceID()); + } else { + if (driveRightMotorC != null) { + driveRightMotorC.set(ControlMode.PercentOutput, 0); + } + driveRightMotorC = null; + } + for (ChickenTalon motor : new ChickenTalon[]{driveLeftMotorA, driveLeftMotorB, + driveLeftMotorC, driveRightMotorA, driveRightMotorB, + driveRightMotorC}) { + if (motor != null) { + motor.configClosedloopRamp(0); + motor.configOpenloopRamp(0); + motor.configPeakOutputForward(1); + motor.configPeakOutputReverse(-1); + motor.enableCurrentLimit(false); + motor.setBrake(brake); + } + } + }); + reset.setRunWhenDisabled(true); + reset.start(); + SmartDashboard.putData(reset); + + Command zero = new SimpleCommand("Zero", () -> { + if (driveLeftMotorA != null) { + driveLeftMotorA.setSelectedSensorPosition(0); + } + + if (driveRightMotorA != null) { + driveRightMotorA.setSelectedSensorPosition(0); + } + }); + zero.setRunWhenDisabled(true); + SmartDashboard.putData(zero); } private static void putRegressionData(@NotNull SimpleRegression regression, String prefix) { @@ -80,6 +178,25 @@ public void robotPeriodic() { putRegressionData(leftRegression, "Left"); putRegressionData(rightRegression, "Right"); Scheduler.getInstance().run(); + if (driveLeftMotorA != null) { + driveLeftMotorA.setSensorPhase(invertLeftSensor); + } + for (ChickenTalon talon : new ChickenTalon[]{driveLeftMotorA, driveLeftMotorB, + driveLeftMotorC}) { + if (talon != null) { + talon.setInverted(invertLeftMotor); + } + } + + if (driveRightMotorA != null) { + driveRightMotorA.setSensorPhase(invertRightSensor); + } + for (ChickenTalon talon : new ChickenTalon[]{driveRightMotorA, driveRightMotorB, + driveRightMotorC}) { + if (talon != null) { + talon.setInverted(invertRightMotor); + } + } } @Override @@ -87,7 +204,6 @@ public void teleopPeriodic() { if (joystick.getRawButton(1)) { // if button A is pressed if (!running) { // reset everything - reset(); running = true; leftRegression.clear(); rightRegression.clear(); @@ -95,12 +211,12 @@ public void teleopPeriodic() { driveLeftMotorA.set(ControlMode.PercentOutput, 0); driveRightMotorA.set(ControlMode.PercentOutput, 0); } else { - double leftVelocity = driveLeftMotorA.getSelectedSensorVelocity(); + double leftVelocity = (driveLeftMotorA.getSelectedSensorVelocity() * 10) / tpu; if (leftVelocity != 0) { leftRegression.addData(leftVelocity, driveLeftMotorA.getMotorOutputVoltage()); } - double rightVelocity = driveRightMotorA.getSelectedSensorVelocity(); + double rightVelocity = (driveRightMotorA.getSelectedSensorVelocity() * 10) / tpu; if (rightVelocity != 0) { rightRegression.addData(leftVelocity, driveRightMotorA.getMotorOutputVoltage()); } @@ -119,42 +235,4 @@ public void teleopPeriodic() { } lastTime = System.currentTimeMillis(); } - - @SuppressWarnings("Duplicates") - public void reset() { - driveLeftMotorA.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); - driveRightMotorA.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); - - driveLeftMotorA.setSensorPhase(true); - - for (ChickenTalon talon : driveLeftMotors) { - talon.setInverted(invertLeft); - } - - driveRightMotorA.setSensorPhase(true); - - for (ChickenTalon talon : driveRightMotors) { - talon.setInverted(invertRight); - } - - driveLeftMotorB.set(ControlMode.Follower, driveLeftMotorA.getDeviceID()); - driveLeftMotorC.set(ControlMode.Follower, driveLeftMotorA.getDeviceID()); - - driveRightMotorB.set(ControlMode.Follower, driveRightMotorA.getDeviceID()); - driveRightMotorC.set(ControlMode.Follower, driveRightMotorA.getDeviceID()); - - for (ChickenTalon talon : driveMotorAll) { - talon.setBrake(true); - } - - for (ChickenTalon talon : driveMotorAll) { - talon.configClosedloopRamp(0); - talon.configOpenloopRamp(0); - talon.configPeakOutputForward(1); - talon.configPeakOutputReverse(-1); - talon.enableCurrentLimit(false); - talon.configVoltageCompSaturation(12); - talon.enableVoltageCompensation(true); - } - } } From 26dc0fa5aa6fd5b15e6616b52bcb78a8984765f1 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Fri, 26 Oct 2018 17:11:35 -0700 Subject: [PATCH 058/145] Add support for left and right profile coefficients --- .../base/motionprofiling/FollowProfile.java | 43 +++--- .../motionprofiling/FollowProfileFactory.java | 124 ++++++++++++++---- .../base/motionprofiling/ProfileFollower.java | 52 ++++---- .../base/testing/MotionProfileTesting.kt | 42 ++++-- 4 files changed, 188 insertions(+), 73 deletions(-) diff --git a/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java b/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java index 50f6ccbd..5aea2b36 100644 --- a/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java +++ b/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfile.java @@ -30,9 +30,12 @@ public class FollowProfile extends AsyncCommand { @NotNull private DoubleSupplier headingSupplier; - private double velCoeff; - private double velIntercept; - private double accelCoeff; + private double lVelCoeff; + private double lVelIntercept; + private double lAccelCoeff; + private double rVelCoeff; + private double rVelIntercept; + private double rAccelCoeff; private double headingP; private double headingI; @@ -54,10 +57,16 @@ public class FollowProfile extends AsyncCommand { * @param headingSupplier A {@link DoubleSupplier} that returns the robot's current heading in * radians from 0 to 2π. * @param loopFreq The interval, in milliseconds, between profile loop execution. - * @param velCoeff The velocity coefficient (kV), in bump units per profile unit per second. - * @param velIntercept The velocity intercept (VIntercept), in bump units. - * @param accelCoeff The acceleration coefficient (kA), in bump units per profile unit per second - * squared. + * @param lVelCoeff The left velocity coefficient (kV), in bump units per profile unit per + * second. + * @param lVelIntercept The left velocity intercept (VIntercept), in bump units. + * @param lAccelCoeff The left acceleration coefficient (kA), in bump units per profile unit per + * second squared. + * @param rVelCoeff The right velocity coefficient (kV), in bump units per profile unit per + * second. + * @param rVelIntercept The right velocity intercept (VIntercept), in bump units. + * @param rAccelCoeff The right acceleration coefficient (kA), in bump units per profile unit per + * second squared. * @param headingP The P coefficient for the heading controller, in profile units per radian. * @param headingI The I coefficient for the heading controller, in profile units per * radian-second. @@ -65,9 +74,10 @@ public class FollowProfile extends AsyncCommand { public FollowProfile(@NotNull MotionProfile left, @NotNull MotionProfile right, @NotNull Subsystem[] subsystems, @NotNull SetpointConsumer leftSetpointConsumer, - @NotNull SetpointConsumer rightSetpointConsumer, @NotNull DoubleSupplier headingSupplier, - long loopFreq, double velCoeff, double velIntercept, - double accelCoeff, double headingP, double headingI) { + @NotNull SetpointConsumer rightSetpointConsumer, + @NotNull DoubleSupplier headingSupplier, + long loopFreq, double lVelCoeff, double lVelIntercept, double lAccelCoeff, double rVelCoeff, + double rVelIntercept, double rAccelCoeff, double headingP, double headingI) { super(loopFreq); for (Subsystem subsystem : subsystems) { requires(subsystem); @@ -77,14 +87,17 @@ public FollowProfile(@NotNull MotionProfile left, @NotNull MotionProfile right, this.leftSetpointConsumer = leftSetpointConsumer; this.rightSetpointConsumer = rightSetpointConsumer; this.headingSupplier = headingSupplier; - this.velCoeff = velCoeff; - this.velIntercept = velIntercept; - this.accelCoeff = accelCoeff; + this.lVelCoeff = lVelCoeff; + this.lVelIntercept = lVelIntercept; + this.lAccelCoeff = lAccelCoeff; + this.rVelCoeff = rVelCoeff; + this.rVelIntercept = rVelIntercept; + this.rAccelCoeff = rAccelCoeff; this.headingP = headingP; this.headingI = headingI; - follower = new ProfileFollower(left, right, velCoeff, velIntercept, accelCoeff, headingP, - headingI); + follower = new ProfileFollower(left, right, lVelCoeff, lVelIntercept, lAccelCoeff, rVelCoeff, + rVelIntercept, rAccelCoeff, headingP, headingI); } /** diff --git a/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfileFactory.java b/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfileFactory.java index a4e98b27..8bc0c2cc 100644 --- a/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfileFactory.java +++ b/lib/src/main/java/org/team1540/base/motionprofiling/FollowProfileFactory.java @@ -33,9 +33,12 @@ public class FollowProfileFactory { private long loopFreq = 20; - private double velCoeff = 0; - private double velIntercept = 0; - private double accelCoeff = 0; + private double lVelCoeff; + private double lVelIntercept; + private double lAccelCoeff; + private double rVelCoeff; + private double rVelIntercept; + private double rAccelCoeff; private double headingP = 0; private double headingI = 0; @@ -169,17 +172,17 @@ public FollowProfileFactory setLoopFreq(long loopFreq) { } /** - * Gets the velocity feed-forward coefficient. This is equivalent to the kV term in drive + * Gets the left velocity feed-forward coefficient. This is equivalent to the kV term in drive * characterization. Defaults to 0 (no feed-forward). * * @return The velocity feed-forward coefficient, in bump units per profile unit per second. */ - public double getVelCoeff() { - return velCoeff; + public double getLVelCoeff() { + return lVelCoeff; } /** - * Sets the velocity feed-forward coefficent. This is equivalent to the kV term in drive * + * Sets the left velocity feed-forward coefficent. This is equivalent to the kV term in drive * * characterization. Defaults to 0 (no feed-forward). * * @param velCoeff The velocity feed-forward coefficient, in bump units per profile unit per @@ -187,43 +190,43 @@ public double getVelCoeff() { * @return This {@code FollowProfileFactory} in a builder pattern. */ @NotNull - public FollowProfileFactory setVelCoeff(double velCoeff) { - this.velCoeff = velCoeff; + public FollowProfileFactory setLVelCoeff(double velCoeff) { + this.lVelCoeff = velCoeff; return this; } /** - * Gets the velocity intercept, or VIntercept. (See Eli Barnett's drive characterization paper for - * an explanation of why this is needed.) + * Gets the left velocity intercept, or VIntercept. (See Eli Barnett's drive characterization + * paper for an explanation of why this is needed.) * * @return The velocity intercept, in bump units. */ - public double getVelIntercept() { - return velIntercept; + public double getLVelIntercept() { + return lVelIntercept; } /** - * Sets the velocity intercept, or VIntercept. (See Eli Barnett's drive characterization paper for - * an explanation of why this is needed.) Defaults to 0. + * Sets the left velocity intercept, or VIntercept. (See Eli Barnett's drive characterization + * paper for an explanation of why this is needed.) Defaults to 0. * * @param velIntercept The velocity intercept, in bump units. * @return This {@code FollowProfileFactory} in a builder pattern. */ @NotNull - public FollowProfileFactory setVelIntercept(double velIntercept) { - this.velIntercept = velIntercept; + public FollowProfileFactory setLVelIntercept(double velIntercept) { + this.lVelIntercept = velIntercept; return this; } /** - * Gets the acceleration feed-forward. This is equivalent to the kA term in drive + * Gets the left acceleration feed-forward. This is equivalent to the kA term in drive * characterization. Defaults to 0 (no feed-forward). * * @return The currently set acceleration coefficient, in bump units per profile unit per second * squared. */ - public double getAccelCoeff() { - return accelCoeff; + public double getLAccelCoeff() { + return lAccelCoeff; } /** @@ -235,8 +238,80 @@ public double getAccelCoeff() { * @return This {@code FollowProfileFactory} in a builder pattern. */ @NotNull - public FollowProfileFactory setAccelCoeff(double accelCoeff) { - this.accelCoeff = accelCoeff; + public FollowProfileFactory setLAccelCoeff(double accelCoeff) { + this.lAccelCoeff = accelCoeff; + return this; + } + + /** + * Gets the right velocity feed-forward coefficient. This is equivalent to the kV term in drive + * characterization. Defaults to 0 (no feed-forward). + * + * @return The velocity feed-forward coefficient, in bump units per profile unit per second. + */ + public double getRVelCoeff() { + return rVelCoeff; + } + + /** + * Sets the right velocity feed-forward coefficent. This is equivalent to the kV term in drive * + * characterization. Defaults to 0 (no feed-forward). + * + * @param velCoeff The velocity feed-forward coefficient, in bump units per profile unit per + * second. + * @return This {@code FollowProfileFactory} in a builder pattern. + */ + @NotNull + public FollowProfileFactory setRVelCoeff(double velCoeff) { + this.rVelCoeff = velCoeff; + return this; + } + + /** + * Gets the right velocity intercept, or VIntercept. (See Eli Barnett's drive characterization + * paper for an explanation of why this is needed.) + * + * @return The velocity intercept, in bump units. + */ + public double getRVelIntercept() { + return rVelIntercept; + } + + /** + * Sets the right velocity intercept, or VIntercept. (See Eli Barnett's drive characterization + * paper for an explanation of why this is needed.) Defaults to 0. + * + * @param velIntercept The velocity intercept, in bump units. + * @return This {@code FollowProfileFactory} in a builder pattern. + */ + @NotNull + public FollowProfileFactory setRVelIntercept(double velIntercept) { + this.rVelIntercept = velIntercept; + return this; + } + + /** + * Gets the right acceleration feed-forward. This is equivalent to the kA term in drive + * characterization. Defaults to 0 (no feed-forward). + * + * @return The currently set acceleration coefficient, in bump units per profile unit per second + * squared. + */ + public double getRAccelCoeff() { + return rAccelCoeff; + } + + /** + * Sets the acceleration feed-forward. This is equivalent to the kA term in drive + * characterization. Defaults to 0 (no feed-forward). + * + * @param accelCoeff The acceleration coefficient, in bump units per profile unit per second + * squared. + * @return This {@code FollowProfileFactory} in a builder pattern. + */ + @NotNull + public FollowProfileFactory setRAccelCoeff(double accelCoeff) { + this.rAccelCoeff = accelCoeff; return this; } @@ -324,12 +399,13 @@ public FollowProfile create(@NotNull File leftFile, @NotNull File rightFile) { * * @param left The profile for the left side. * @param right The profile for the right side. - * @return A new {@link FollowProfile} command with the previously configured settings following * + * @return A new {@link FollowProfile} command with the previously configured settings following * the provided profile. */ @NotNull public FollowProfile create(@NotNull MotionProfile left, @NotNull MotionProfile right) { return new FollowProfile(left, right, subsystems, leftSetpointConsumer, rightSetpointConsumer, - headingSupplier, loopFreq, velCoeff, velIntercept, accelCoeff, headingP, headingI); + headingSupplier, loopFreq, lVelCoeff, lVelIntercept, lAccelCoeff, rVelCoeff, + rVelIntercept, rAccelCoeff, headingP, headingI); } } diff --git a/lib/src/main/java/org/team1540/base/motionprofiling/ProfileFollower.java b/lib/src/main/java/org/team1540/base/motionprofiling/ProfileFollower.java index faf2e05e..b67b7a5e 100644 --- a/lib/src/main/java/org/team1540/base/motionprofiling/ProfileFollower.java +++ b/lib/src/main/java/org/team1540/base/motionprofiling/ProfileFollower.java @@ -4,7 +4,6 @@ import static java.lang.Math.cos; import static java.lang.Math.sin; -import java.util.Arrays; import org.jetbrains.annotations.NotNull; import org.team1540.base.motionprofiling.MotionProfile.Point; @@ -32,9 +31,12 @@ public class ProfileFollower { @NotNull private MotionProfile right; - private double velCoeff; - private double velIntercept; - private double accelCoeff; + private double lVelCoeff; + private double lVelIntercept; + private double lAccelCoeff; + private double rVelCoeff; + private double rVelIntercept; + private double rAccelCoeff; private double headingP; private double headingI; @@ -50,29 +52,35 @@ public class ProfileFollower { * For an explanation of units, see the {@linkplain org.team1540.base.motionprofiling package * docs}. * - * @param velCoeff The velocity coefficient (kV), in bump units per profile unit per second. - * @param velIntercept The velocity intercept (VIntercept), in bump units. - * @param accelCoeff The acceleration coefficient (kA), in bump units per profile unit per second + * @param lVelCoeff The left velocity coefficient (kV), in bump units per profile unit per second. + * @param lVelIntercept The left velocity intercept (VIntercept), in bump units. + * @param lAccelCoeff The left acceleration coefficient (kA), in bump units per profile unit per second + * squared. + * @param rVelCoeff The right velocity coefficient (kV), in bump units per profile unit per second. + * @param rVelIntercept The right velocity intercept (VIntercept), in bump units. + * @param rAccelCoeff The right acceleration coefficient (kA), in bump units per profile unit per second * squared. * @param headingP The P coefficient for the heading controller, in profile units per radian. * @param headingI The I coefficient for the heading controller, in profile units per * radian-second. */ - public ProfileFollower(@NotNull MotionProfile left, @NotNull MotionProfile right, double velCoeff, - double velIntercept, double accelCoeff, double headingP, double headingI) { + public ProfileFollower( + @NotNull MotionProfile left, + @NotNull MotionProfile right, double lVelCoeff, double lVelIntercept, double lAccelCoeff, + double rVelCoeff, double rVelIntercept, double rAccelCoeff, double headingP, + double headingI) { this.left = left; this.right = right; - this.velCoeff = velCoeff; - this.velIntercept = velIntercept; - this.accelCoeff = accelCoeff; + this.lVelCoeff = lVelCoeff; + this.lVelIntercept = lVelIntercept; + this.lAccelCoeff = lAccelCoeff; + this.rVelCoeff = rVelCoeff; + this.rVelIntercept = rVelIntercept; + this.rAccelCoeff = rAccelCoeff; this.headingP = headingP; this.headingI = headingI; - - profTime = Math.max(Arrays.stream(left.points).mapToDouble(s -> s.dt).sum(), - Arrays.stream(right.points).mapToDouble(s -> s.dt).sum()); } - /** * Get the output from the motion profile at a given time (usually the current time). * @@ -99,16 +107,16 @@ public ProfileDriveSignal get(double heading, double time) { double gyroPOut = headingError * headingP; double gyroIOut = gyroIAccum * headingI; - double leftVelFOut = velCoeff * leftSegment.velocity; - double rightVelFOut = velCoeff * rightSegment.velocity; + double leftVelFOut = lVelCoeff * leftSegment.velocity; + double rightVelFOut = rVelCoeff * rightSegment.velocity; double leftVelInterceptOut = - leftSegment.velocity == 0 ? 0 : Math.copySign(velIntercept, leftSegment.velocity); + leftSegment.velocity == 0 ? 0 : Math.copySign(lVelIntercept, leftSegment.velocity); double rightVelInterceptOut = - rightSegment.velocity == 0 ? 0 : Math.copySign(velIntercept, rightSegment.velocity); + rightSegment.velocity == 0 ? 0 : Math.copySign(rVelIntercept, rightSegment.velocity); - double leftAccelFOut = accelCoeff * leftSegment.acceleration; - double rightAccelFOut = accelCoeff * rightSegment.acceleration; + double leftAccelFOut = lAccelCoeff * leftSegment.acceleration; + double rightAccelFOut = rAccelCoeff * rightSegment.acceleration; return new ProfileDriveSignal( leftSegment.position - gyroPOut - gyroIOut, diff --git a/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt b/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt index 8d7ae8d5..b7337be3 100644 --- a/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt +++ b/test/src/main/kotlin/org/team1540/base/testing/MotionProfileTesting.kt @@ -35,16 +35,28 @@ class MotionProfileTestingRobot : IterativeRobot() { private var command: FollowProfile? = null @JvmField - @Preference("kV", persistent = false) - var kV = 0.0 + @Preference("lkV", persistent = false) + var lKV = 0.0 @JvmField - @Preference("kA", persistent = false) - var kA = 0.0 + @Preference("lkA", persistent = false) + var lKA = 0.0 @JvmField - @Preference("VIntercept", persistent = false) - var vIntercept = 0.0 + @Preference("lVIntercept", persistent = false) + var lVIntercept = 0.0 + + @JvmField + @Preference("rkV", persistent = false) + var rKV = 0.0 + + @JvmField + @Preference("rkA", persistent = false) + var rKA = 0.0 + + @JvmField + @Preference("rVIntercept", persistent = false) + var rVIntercept = 0.0 @JvmField @Preference("MP Loop Freq", persistent = false) @@ -104,9 +116,12 @@ class MotionProfileTestingRobot : IterativeRobot() { MotionProfileUtils.createSetpointConsumer(DriveTrain.left2, tpu), DriveTrain ).apply { - velIntercept = vIntercept - velCoeff = kV - accelCoeff = kA + lVelIntercept = lVIntercept + lVelCoeff = lKV + lAccelCoeff = lKA + rVelIntercept = rVIntercept + rVelCoeff = rKV + rAccelCoeff = rKA loopFreq = loopFreqMs.toLong() headingP = hdgP headingI = hdgI @@ -148,9 +163,12 @@ class MotionProfileTestingRobot : IterativeRobot() { DriveTrain.brake = true DriveTrain.zero() factory.apply { - velIntercept = vIntercept - velCoeff = kV - accelCoeff = kA + lVelIntercept = lVIntercept + lVelCoeff = lKV + lAccelCoeff = lKA + rVelIntercept = rVIntercept + rVelCoeff = rKV + rAccelCoeff = rKA loopFreq = loopFreqMs.toLong() headingP = hdgP headingI = hdgI From d9c8e92077bb7ffb511410959d67389a9d22d8b4 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 29 Oct 2018 11:00:51 -0700 Subject: [PATCH 059/145] Add SimpleJoystickInput Needs to be documented --- .../drive/pipeline/SimpleJoystickInput.java | 70 +++++++++++++++++++ 1 file changed, 70 insertions(+) create mode 100644 lib/src/main/java/org/team1540/base/drive/pipeline/SimpleJoystickInput.java diff --git a/lib/src/main/java/org/team1540/base/drive/pipeline/SimpleJoystickInput.java b/lib/src/main/java/org/team1540/base/drive/pipeline/SimpleJoystickInput.java new file mode 100644 index 00000000..ecdf1257 --- /dev/null +++ b/lib/src/main/java/org/team1540/base/drive/pipeline/SimpleJoystickInput.java @@ -0,0 +1,70 @@ +package org.team1540.base.drive.pipeline; + +import edu.wpi.first.wpilibj.Joystick; +import java.util.OptionalDouble; +import java.util.function.Supplier; +import org.team1540.base.Utilities; + +public class SimpleJoystickInput implements Supplier { + + 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 = 0.1; + + @Override + public TankDriveData get() { + double triggerValue = Utilities.processDeadzone(joystick.getRawAxis(fwdAxis), deadzone) + - Utilities.processDeadzone(joystick.getRawAxis(backAxis), deadzone); + 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() + ); + } + + 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); + } + + 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; + } +} From 4a8e736b7d6d707f22f44e603ba4dcfe59ad11dc Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 29 Oct 2018 11:01:09 -0700 Subject: [PATCH 060/145] Add TalonSRXOutput Also needs to be documented. Hmph. --- .../base/drive/pipeline/TalonSRXOutput.java | 50 +++++++++++++++++++ 1 file changed, 50 insertions(+) create mode 100644 lib/src/main/java/org/team1540/base/drive/pipeline/TalonSRXOutput.java diff --git a/lib/src/main/java/org/team1540/base/drive/pipeline/TalonSRXOutput.java b/lib/src/main/java/org/team1540/base/drive/pipeline/TalonSRXOutput.java new file mode 100644 index 00000000..257422b8 --- /dev/null +++ b/lib/src/main/java/org/team1540/base/drive/pipeline/TalonSRXOutput.java @@ -0,0 +1,50 @@ +package org.team1540.base.drive.pipeline; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.DemandType; +import java.util.function.Consumer; +import org.team1540.base.wrappers.ChickenTalon; + +public class TalonSRXOutput implements Consumer { + + private ChickenTalon left; + private ChickenTalon right; + + @SuppressWarnings("OptionalGetWithoutIsPresent") + @Override + public void accept(TankDriveData tankDriveData) { + if (tankDriveData.left.position.isPresent()) { + if (tankDriveData.left.additionalFeedForward.isPresent()) { + left.set(ControlMode.Position, tankDriveData.left.position.getAsDouble(), + DemandType.ArbitraryFeedForward, + tankDriveData.left.additionalFeedForward.getAsDouble()); + right.set(ControlMode.Position, tankDriveData.right.position.getAsDouble(), + DemandType.ArbitraryFeedForward, + tankDriveData.right.additionalFeedForward.getAsDouble()); + } else { + left.set(ControlMode.Position, tankDriveData.left.position.getAsDouble()); + right.set(ControlMode.Position, tankDriveData.right.position.getAsDouble()); + } + } else if (tankDriveData.left.velocity.isPresent()) { + if (tankDriveData.left.additionalFeedForward.isPresent()) { + left.set(ControlMode.Velocity, tankDriveData.left.velocity.getAsDouble(), + DemandType.ArbitraryFeedForward, + tankDriveData.left.additionalFeedForward.getAsDouble()); + right.set(ControlMode.Velocity, tankDriveData.right.velocity.getAsDouble(), + DemandType.ArbitraryFeedForward, + tankDriveData.right.additionalFeedForward.getAsDouble()); + } else { + left.set(ControlMode.Velocity, tankDriveData.left.velocity.getAsDouble()); + right.set(ControlMode.Velocity, tankDriveData.right.velocity.getAsDouble()); + } + } else { + left.set(ControlMode.PercentOutput, tankDriveData.left.additionalFeedForward.orElse(0)); + right.set(ControlMode.PercentOutput, tankDriveData.right.additionalFeedForward.orElse(0)); + } + } + + public TalonSRXOutput(ChickenTalon left, ChickenTalon right) { + this.left = left; + this.right = right; + } +} From 4b60024f30b523f19c19731440faad120b17f5b5 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 29 Oct 2018 11:02:54 -0700 Subject: [PATCH 061/145] Add a constructor for DrivePipeline --- .../main/java/org/team1540/base/drive/DrivePipeline.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/lib/src/main/java/org/team1540/base/drive/DrivePipeline.java b/lib/src/main/java/org/team1540/base/drive/DrivePipeline.java index e1a42b5f..cafa7590 100644 --- a/lib/src/main/java/org/team1540/base/drive/DrivePipeline.java +++ b/lib/src/main/java/org/team1540/base/drive/DrivePipeline.java @@ -41,4 +41,10 @@ public class DrivePipeline { public void execute() { output.accept(processor.apply(input.get())); } + + public DrivePipeline(Supplier input, Function processor, Consumer output) { + this.input = input; + this.processor = processor; + this.output = output; + } } From a037bcc7fb04e20873fee4973e7a5456f4c101cf Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 29 Oct 2018 11:03:24 -0700 Subject: [PATCH 062/145] Create DrivePipelineTesting file --- .../base/testing/DrivePipelineTesting.kt | 86 +++++++++++++++++++ 1 file changed, 86 insertions(+) create mode 100644 test/src/main/kotlin/org/team1540/base/testing/DrivePipelineTesting.kt diff --git a/test/src/main/kotlin/org/team1540/base/testing/DrivePipelineTesting.kt b/test/src/main/kotlin/org/team1540/base/testing/DrivePipelineTesting.kt new file mode 100644 index 00000000..b17cbd5d --- /dev/null +++ b/test/src/main/kotlin/org/team1540/base/testing/DrivePipelineTesting.kt @@ -0,0 +1,86 @@ +package org.team1540.base.testing + +import com.ctre.phoenix.motorcontrol.ControlMode +import edu.wpi.first.wpilibj.IterativeRobot +import edu.wpi.first.wpilibj.Joystick +import edu.wpi.first.wpilibj.command.Scheduler +import org.team1540.base.drive.DrivePipeline +import org.team1540.base.drive.pipeline.SimpleJoystickInput +import org.team1540.base.drive.pipeline.TalonSRXOutput +import org.team1540.base.drive.pipeline.TankDriveData +import org.team1540.base.wrappers.ChickenTalon +import java.util.function.Function + +class SimpleDrivePipelineTestRobot : IterativeRobot() { + private val left1 = ChickenTalon(1).apply { + setBrake(true) + configClosedloopRamp(0.0) + configOpenloopRamp(0.0) + configPeakOutputForward(1.0) + configPeakOutputReverse(-1.0) + enableCurrentLimit(false) + inverted = false + } + + 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()) + } + + private val right1 = ChickenTalon(4).apply { + setBrake(true) + configClosedloopRamp(0.0) + configOpenloopRamp(0.0) + configPeakOutputForward(1.0) + configPeakOutputReverse(-1.0) + enableCurrentLimit(false) + inverted = 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()) + } + + private val pipeline = DrivePipeline( + SimpleJoystickInput(Joystick(0), 1, 5, 3, 2, false, false), + Function.identity(), + TalonSRXOutput(left1, right1) + ) + + override fun teleopPeriodic() { + Scheduler.getInstance().run() + pipeline.execute() + } +} From 441cc45b3ef96e445fd2716d71949d0cee803f96 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 29 Oct 2018 15:04:37 -0700 Subject: [PATCH 063/145] Rename JoystickInput to TankJoystickInput This is for clarity and since I plan to create a different JoystickInput for arcade-based thingies. --- .../pipeline/{JoystickInput.java => TankJoystickInput.java} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename lib/src/main/java/org/team1540/base/drive/pipeline/{JoystickInput.java => TankJoystickInput.java} (94%) diff --git a/lib/src/main/java/org/team1540/base/drive/pipeline/JoystickInput.java b/lib/src/main/java/org/team1540/base/drive/pipeline/TankJoystickInput.java similarity index 94% rename from lib/src/main/java/org/team1540/base/drive/pipeline/JoystickInput.java rename to lib/src/main/java/org/team1540/base/drive/pipeline/TankJoystickInput.java index f6508653..f4c57596 100644 --- a/lib/src/main/java/org/team1540/base/drive/pipeline/JoystickInput.java +++ b/lib/src/main/java/org/team1540/base/drive/pipeline/TankJoystickInput.java @@ -5,7 +5,7 @@ import java.util.function.Supplier; import org.team1540.base.Utilities; -public class JoystickInput implements Supplier { +public class TankJoystickInput implements Supplier { private Joystick joystick; private int leftAxis; From 81b0ae69375a40af3377c20d0f7d5dee31479772 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 29 Oct 2018 16:06:34 -0700 Subject: [PATCH 064/145] Create extensions of functional interfaces --- .../team1540/base/drive/pipeline/Input.java | 31 ++++++++++++++ .../team1540/base/drive/pipeline/Output.java | 31 ++++++++++++++ .../base/drive/pipeline/Processor.java | 41 +++++++++++++++++++ 3 files changed, 103 insertions(+) create mode 100644 lib/src/main/java/org/team1540/base/drive/pipeline/Input.java create mode 100644 lib/src/main/java/org/team1540/base/drive/pipeline/Output.java create mode 100644 lib/src/main/java/org/team1540/base/drive/pipeline/Processor.java diff --git a/lib/src/main/java/org/team1540/base/drive/pipeline/Input.java b/lib/src/main/java/org/team1540/base/drive/pipeline/Input.java new file mode 100644 index 00000000..805e3bb9 --- /dev/null +++ b/lib/src/main/java/org/team1540/base/drive/pipeline/Input.java @@ -0,0 +1,31 @@ +package org.team1540.base.drive.pipeline; + +import java.util.function.Function; +import java.util.function.Supplier; + +/** + * Extension of a {@link Supplier} adding an additional composition method. + * + * {@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()); + } +} diff --git a/lib/src/main/java/org/team1540/base/drive/pipeline/Output.java b/lib/src/main/java/org/team1540/base/drive/pipeline/Output.java new file mode 100644 index 00000000..a602b850 --- /dev/null +++ b/lib/src/main/java/org/team1540/base/drive/pipeline/Output.java @@ -0,0 +1,31 @@ +package org.team1540.base.drive.pipeline; + +import java.util.function.Consumer; +import java.util.function.Function; + +/** + * 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)); + } +} diff --git a/lib/src/main/java/org/team1540/base/drive/pipeline/Processor.java b/lib/src/main/java/org/team1540/base/drive/pipeline/Processor.java new file mode 100644 index 00000000..68bc740d --- /dev/null +++ b/lib/src/main/java/org/team1540/base/drive/pipeline/Processor.java @@ -0,0 +1,41 @@ +package org.team1540.base.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)); + } +} From bb346d0484f02ecec09b33e8d1faa0ce6d5a3587 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 29 Oct 2018 16:11:10 -0700 Subject: [PATCH 065/145] Convert already-existing Inputs/Processors/Outputs They now all use the new interfaces. --- .../base/drive/pipeline/OpenLoopCharacterizedProcessor.java | 3 +-- .../java/org/team1540/base/drive/pipeline/ProfileInput.java | 3 +-- .../org/team1540/base/drive/pipeline/SimpleJoystickInput.java | 3 +-- .../java/org/team1540/base/drive/pipeline/TalonSRXOutput.java | 3 +-- .../org/team1540/base/drive/pipeline/TankJoystickInput.java | 3 +-- 5 files changed, 5 insertions(+), 10 deletions(-) diff --git a/lib/src/main/java/org/team1540/base/drive/pipeline/OpenLoopCharacterizedProcessor.java b/lib/src/main/java/org/team1540/base/drive/pipeline/OpenLoopCharacterizedProcessor.java index 44523e06..96ceddfd 100644 --- a/lib/src/main/java/org/team1540/base/drive/pipeline/OpenLoopCharacterizedProcessor.java +++ b/lib/src/main/java/org/team1540/base/drive/pipeline/OpenLoopCharacterizedProcessor.java @@ -10,10 +10,9 @@ */ import java.util.OptionalDouble; -import java.util.function.Function; import org.jetbrains.annotations.Contract; -public class OpenLoopCharacterizedProcessor implements Function { +public class OpenLoopCharacterizedProcessor implements Processor { private final double kV; private final double vIntercept; diff --git a/lib/src/main/java/org/team1540/base/drive/pipeline/ProfileInput.java b/lib/src/main/java/org/team1540/base/drive/pipeline/ProfileInput.java index 3b5eee1a..1ccb0292 100644 --- a/lib/src/main/java/org/team1540/base/drive/pipeline/ProfileInput.java +++ b/lib/src/main/java/org/team1540/base/drive/pipeline/ProfileInput.java @@ -2,12 +2,11 @@ import edu.wpi.first.wpilibj.Timer; import java.util.OptionalDouble; -import java.util.function.Supplier; import org.jetbrains.annotations.NotNull; import org.team1540.base.motionprofiling.MotionProfile; import org.team1540.base.motionprofiling.MotionProfile.Point; -public class ProfileInput implements Supplier { +public class ProfileInput implements Input { private MotionProfile left; private MotionProfile right; diff --git a/lib/src/main/java/org/team1540/base/drive/pipeline/SimpleJoystickInput.java b/lib/src/main/java/org/team1540/base/drive/pipeline/SimpleJoystickInput.java index ecdf1257..5867cf28 100644 --- a/lib/src/main/java/org/team1540/base/drive/pipeline/SimpleJoystickInput.java +++ b/lib/src/main/java/org/team1540/base/drive/pipeline/SimpleJoystickInput.java @@ -2,10 +2,9 @@ import edu.wpi.first.wpilibj.Joystick; import java.util.OptionalDouble; -import java.util.function.Supplier; import org.team1540.base.Utilities; -public class SimpleJoystickInput implements Supplier { +public class SimpleJoystickInput implements Input { private Joystick joystick; private int leftAxis; diff --git a/lib/src/main/java/org/team1540/base/drive/pipeline/TalonSRXOutput.java b/lib/src/main/java/org/team1540/base/drive/pipeline/TalonSRXOutput.java index 257422b8..bd5c68a4 100644 --- a/lib/src/main/java/org/team1540/base/drive/pipeline/TalonSRXOutput.java +++ b/lib/src/main/java/org/team1540/base/drive/pipeline/TalonSRXOutput.java @@ -2,10 +2,9 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.DemandType; -import java.util.function.Consumer; import org.team1540.base.wrappers.ChickenTalon; -public class TalonSRXOutput implements Consumer { +public class TalonSRXOutput implements Output { private ChickenTalon left; private ChickenTalon right; diff --git a/lib/src/main/java/org/team1540/base/drive/pipeline/TankJoystickInput.java b/lib/src/main/java/org/team1540/base/drive/pipeline/TankJoystickInput.java index f4c57596..15a6bed3 100644 --- a/lib/src/main/java/org/team1540/base/drive/pipeline/TankJoystickInput.java +++ b/lib/src/main/java/org/team1540/base/drive/pipeline/TankJoystickInput.java @@ -2,10 +2,9 @@ import edu.wpi.first.wpilibj.Joystick; import java.util.OptionalDouble; -import java.util.function.Supplier; import org.team1540.base.Utilities; -public class TankJoystickInput implements Supplier { +public class TankJoystickInput implements Input { private Joystick joystick; private int leftAxis; From 1e790525ce74dc855e6e0cf72b447c1c10183fb5 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 29 Oct 2018 16:12:00 -0700 Subject: [PATCH 066/145] Integrate JoystickScaling into pipeline framework This is all backwards-compatible; just adds a default method which allows it to be used as part of drive pipelines. --- .../java/org/team1540/base/drive/JoystickScaling.java | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/lib/src/main/java/org/team1540/base/drive/JoystickScaling.java b/lib/src/main/java/org/team1540/base/drive/JoystickScaling.java index 41a3cdc2..25da1a69 100644 --- a/lib/src/main/java/org/team1540/base/drive/JoystickScaling.java +++ b/lib/src/main/java/org/team1540/base/drive/JoystickScaling.java @@ -1,7 +1,13 @@ package org.team1540.base.drive; +import org.team1540.base.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); + } } From 7e09432b51367d990fee45728c0e43b134a750d4 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 29 Oct 2018 16:23:59 -0700 Subject: [PATCH 067/145] Create UnitScaler --- .../base/drive/pipeline/UnitScaler.java | 68 +++++++++++++++++++ 1 file changed, 68 insertions(+) create mode 100644 lib/src/main/java/org/team1540/base/drive/pipeline/UnitScaler.java diff --git a/lib/src/main/java/org/team1540/base/drive/pipeline/UnitScaler.java b/lib/src/main/java/org/team1540/base/drive/pipeline/UnitScaler.java new file mode 100644 index 00000000..06f45186 --- /dev/null +++ b/lib/src/main/java/org/team1540/base/drive/pipeline/UnitScaler.java @@ -0,0 +1,68 @@ +package org.team1540.base.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; + } +} From 81385bd296e654a9b3b8ea798343d31979c83380 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 29 Oct 2018 22:00:03 -0700 Subject: [PATCH 068/145] Get y-intercept properly --- .../base/util/robots/VelocityCharacterizationRobot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/src/main/java/org/team1540/base/util/robots/VelocityCharacterizationRobot.java b/lib/src/main/java/org/team1540/base/util/robots/VelocityCharacterizationRobot.java index 32f78e85..863e10ec 100644 --- a/lib/src/main/java/org/team1540/base/util/robots/VelocityCharacterizationRobot.java +++ b/lib/src/main/java/org/team1540/base/util/robots/VelocityCharacterizationRobot.java @@ -164,7 +164,7 @@ private static void putRegressionData(@NotNull SimpleRegression regression, Stri // getSlope, getIntercept, and getRSquare all have the same criteria for returning NaN if (!Double.isNaN(regression.getSlope())) { SmartDashboard.putNumber(prefix + " Calculated kV", regression.getSlope()); - SmartDashboard.putNumber(prefix + " Calculated vIntercept", regression.predict(0)); + SmartDashboard.putNumber(prefix + " Calculated vIntercept", regression.getIntercept()); SmartDashboard.putNumber(prefix + " rSquared", regression.getRSquare()); } else { SmartDashboard.putNumber(prefix + " Calculated kV", 0); From 73ad0afea408668b068db8c2b4c7cf4add7ca564 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 29 Oct 2018 22:01:06 -0700 Subject: [PATCH 069/145] Pass velocity through open loop This is in case we want to do an additional Talon-side PID on velocity (e.g. in a drivetrain scenario). --- .../base/drive/pipeline/OpenLoopCharacterizedProcessor.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lib/src/main/java/org/team1540/base/drive/pipeline/OpenLoopCharacterizedProcessor.java b/lib/src/main/java/org/team1540/base/drive/pipeline/OpenLoopCharacterizedProcessor.java index 96ceddfd..508755fb 100644 --- a/lib/src/main/java/org/team1540/base/drive/pipeline/OpenLoopCharacterizedProcessor.java +++ b/lib/src/main/java/org/team1540/base/drive/pipeline/OpenLoopCharacterizedProcessor.java @@ -58,11 +58,11 @@ public TankDriveData apply(TankDriveData command) { return new TankDriveData( new DriveData(command.left.position, - OptionalDouble.empty(), + command.left.velocity, OptionalDouble.empty(), OptionalDouble.of(signal.getLeftThrottle())), new DriveData(command.right.position, - OptionalDouble.empty(), + command.left.velocity, OptionalDouble.empty(), OptionalDouble.of(signal.getRightThrottle())), command.heading, From 874d9bc0a25af26e39346bb9cb1cbe17f6082c4d Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 29 Oct 2018 22:01:46 -0700 Subject: [PATCH 070/145] Create AdvancedArcadeJoystickInput This is 2471-style drive. Designed to be used with OpenLoopCharacterizedProcessor or similar. --- .../pipeline/AdvancedArcadeJoystickInput.java | 50 +++++++++++++++++++ 1 file changed, 50 insertions(+) create mode 100644 lib/src/main/java/org/team1540/base/drive/pipeline/AdvancedArcadeJoystickInput.java diff --git a/lib/src/main/java/org/team1540/base/drive/pipeline/AdvancedArcadeJoystickInput.java b/lib/src/main/java/org/team1540/base/drive/pipeline/AdvancedArcadeJoystickInput.java new file mode 100644 index 00000000..838a174a --- /dev/null +++ b/lib/src/main/java/org/team1540/base/drive/pipeline/AdvancedArcadeJoystickInput.java @@ -0,0 +1,50 @@ +package org.team1540.base.drive.pipeline; + +import java.util.OptionalDouble; +import java.util.function.Supplier; +import org.jetbrains.annotations.NotNull; + +public class AdvancedArcadeJoystickInput implements Input { + + private double maxVelocity; + private @NotNull Supplier throttleInput; + private @NotNull Supplier softTurnInput; + private @NotNull Supplier hardTurnInput; + + public AdvancedArcadeJoystickInput(double maxVelocity, + @NotNull Supplier throttleInput, + @NotNull Supplier softTurnInput, + @NotNull Supplier hardTurnInput) { + this.maxVelocity = maxVelocity; + this.throttleInput = throttleInput; + this.softTurnInput = softTurnInput; + this.hardTurnInput = hardTurnInput; + } + + @Override + public TankDriveData get() { + double throttle = throttleInput.get(); + double soft = softTurnInput.get(); + double hard = hardTurnInput.get(); + + double leftPowerRaw = throttle + (soft * Math.abs(throttle)) + hard; + double rightPowerRaw = throttle - (soft * Math.abs(throttle)) - hard; + + 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; + } + + return new TankDriveData( + new DriveData(OptionalDouble.of(leftPower * maxVelocity)), + new DriveData(OptionalDouble.of(rightPower * maxVelocity)), + OptionalDouble.empty(), + OptionalDouble.empty()); + } +} From 54db40a00fd3cf40acb120e97cd1c5d68440be06 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 29 Oct 2018 22:03:32 -0700 Subject: [PATCH 071/145] Retool CharacterizedProcessor into a FeedForwardProcessor This is more in line with what it actually does; it also now passes all targets through. --- ...or.java => OpenLoopFeedForwardProcessor.java} | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) rename lib/src/main/java/org/team1540/base/drive/pipeline/{OpenLoopCharacterizedProcessor.java => OpenLoopFeedForwardProcessor.java} (82%) diff --git a/lib/src/main/java/org/team1540/base/drive/pipeline/OpenLoopCharacterizedProcessor.java b/lib/src/main/java/org/team1540/base/drive/pipeline/OpenLoopFeedForwardProcessor.java similarity index 82% rename from lib/src/main/java/org/team1540/base/drive/pipeline/OpenLoopCharacterizedProcessor.java rename to lib/src/main/java/org/team1540/base/drive/pipeline/OpenLoopFeedForwardProcessor.java index 508755fb..5b0fadc0 100644 --- a/lib/src/main/java/org/team1540/base/drive/pipeline/OpenLoopCharacterizedProcessor.java +++ b/lib/src/main/java/org/team1540/base/drive/pipeline/OpenLoopFeedForwardProcessor.java @@ -12,7 +12,7 @@ import java.util.OptionalDouble; import org.jetbrains.annotations.Contract; -public class OpenLoopCharacterizedProcessor implements Processor { +public class OpenLoopFeedForwardProcessor implements Processor { private final double kV; private final double vIntercept; @@ -27,7 +27,7 @@ public class OpenLoopCharacterizedProcessor implements Processor Date: Mon, 29 Oct 2018 22:05:40 -0700 Subject: [PATCH 072/145] Extract pipeline drive train to an object --- .../base/testing/DrivePipelineTesting.kt | 29 ++++++++++--------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/test/src/main/kotlin/org/team1540/base/testing/DrivePipelineTesting.kt b/test/src/main/kotlin/org/team1540/base/testing/DrivePipelineTesting.kt index b17cbd5d..92001c76 100644 --- a/test/src/main/kotlin/org/team1540/base/testing/DrivePipelineTesting.kt +++ b/test/src/main/kotlin/org/team1540/base/testing/DrivePipelineTesting.kt @@ -12,7 +12,20 @@ import org.team1540.base.wrappers.ChickenTalon import java.util.function.Function class SimpleDrivePipelineTestRobot : IterativeRobot() { - private val left1 = ChickenTalon(1).apply { + private val pipeline = DrivePipeline( + SimpleJoystickInput(Joystick(0), 1, 5, 3, 2, false, false), + Function.identity(), + TalonSRXOutput(PipelineDriveTrain.left1, PipelineDriveTrain.right1) + ) + + override fun teleopPeriodic() { + Scheduler.getInstance().run() + pipeline.execute() + } +} + +private object PipelineDriveTrain { + val left1 = ChickenTalon(1).apply { setBrake(true) configClosedloopRamp(0.0) configOpenloopRamp(0.0) @@ -43,7 +56,7 @@ class SimpleDrivePipelineTestRobot : IterativeRobot() { set(ControlMode.Follower, left1.deviceID.toDouble()) } - private val right1 = ChickenTalon(4).apply { + val right1 = ChickenTalon(4).apply { setBrake(true) configClosedloopRamp(0.0) configOpenloopRamp(0.0) @@ -72,15 +85,5 @@ class SimpleDrivePipelineTestRobot : IterativeRobot() { inverted = true set(ControlMode.Follower, right1.deviceID.toDouble()) } - - private val pipeline = DrivePipeline( - SimpleJoystickInput(Joystick(0), 1, 5, 3, 2, false, false), - Function.identity(), - TalonSRXOutput(left1, right1) - ) - - override fun teleopPeriodic() { - Scheduler.getInstance().run() - pipeline.execute() - } } + From dfc09a6d76723ec243104019834b34a16d1612e1 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 29 Oct 2018 22:07:45 -0700 Subject: [PATCH 073/145] Extract DrivePipelineTestRobot to an abstract class Going to be doing a lot of testing with running a pipeline and not much else. This is more efficient and nicer. --- .../base/testing/DrivePipelineTesting.kt | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/test/src/main/kotlin/org/team1540/base/testing/DrivePipelineTesting.kt b/test/src/main/kotlin/org/team1540/base/testing/DrivePipelineTesting.kt index 92001c76..a98d5f28 100644 --- a/test/src/main/kotlin/org/team1540/base/testing/DrivePipelineTesting.kt +++ b/test/src/main/kotlin/org/team1540/base/testing/DrivePipelineTesting.kt @@ -11,12 +11,8 @@ import org.team1540.base.drive.pipeline.TankDriveData import org.team1540.base.wrappers.ChickenTalon import java.util.function.Function -class SimpleDrivePipelineTestRobot : IterativeRobot() { - private val pipeline = DrivePipeline( - SimpleJoystickInput(Joystick(0), 1, 5, 3, 2, false, false), - Function.identity(), - TalonSRXOutput(PipelineDriveTrain.left1, PipelineDriveTrain.right1) - ) +abstract class DrivePipelineTestRobot : IterativeRobot() { + protected abstract val pipeline: DrivePipeline override fun teleopPeriodic() { Scheduler.getInstance().run() @@ -24,6 +20,14 @@ class SimpleDrivePipelineTestRobot : IterativeRobot() { } } +class SimpleDrivePipelineTestRobot : DrivePipelineTestRobot() { + override val pipeline = DrivePipeline( + SimpleJoystickInput(Joystick(0), 1, 5, 3, 2, false, false), + Function.identity(), + TalonSRXOutput(PipelineDriveTrain.left1, PipelineDriveTrain.right1) + ) +} + private object PipelineDriveTrain { val left1 = ChickenTalon(1).apply { setBrake(true) From 0f31348c198451f0e64033d6e96d8daa08abb5aa Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Mon, 29 Oct 2018 22:20:17 -0700 Subject: [PATCH 074/145] Create AdvancedJoystickInputPipelineTestRobot Say that five times fast. Whew. Yes, the code here is ugly. It'll get cleaned up at some point. Code here doesn't have quite as high portability priority as you'll never be running it from a library. --- .../base/testing/DrivePipelineTesting.kt | 48 +++++++++++++++++-- 1 file changed, 45 insertions(+), 3 deletions(-) diff --git a/test/src/main/kotlin/org/team1540/base/testing/DrivePipelineTesting.kt b/test/src/main/kotlin/org/team1540/base/testing/DrivePipelineTesting.kt index a98d5f28..505fa946 100644 --- a/test/src/main/kotlin/org/team1540/base/testing/DrivePipelineTesting.kt +++ b/test/src/main/kotlin/org/team1540/base/testing/DrivePipelineTesting.kt @@ -1,15 +1,22 @@ package org.team1540.base.testing import com.ctre.phoenix.motorcontrol.ControlMode +import edu.wpi.first.wpilibj.GenericHID import edu.wpi.first.wpilibj.IterativeRobot import edu.wpi.first.wpilibj.Joystick +import edu.wpi.first.wpilibj.XboxController import edu.wpi.first.wpilibj.command.Scheduler +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard +import org.team1540.base.Utilities import org.team1540.base.drive.DrivePipeline -import org.team1540.base.drive.pipeline.SimpleJoystickInput -import org.team1540.base.drive.pipeline.TalonSRXOutput -import org.team1540.base.drive.pipeline.TankDriveData +import org.team1540.base.drive.pipeline.* +import org.team1540.base.preferencemanager.Preference +import org.team1540.base.preferencemanager.PreferenceManager +import org.team1540.base.util.Executable +import org.team1540.base.util.SimpleCommand import org.team1540.base.wrappers.ChickenTalon import java.util.function.Function +import java.util.function.Supplier abstract class DrivePipelineTestRobot : IterativeRobot() { protected abstract val pipeline: DrivePipeline @@ -28,6 +35,41 @@ class SimpleDrivePipelineTestRobot : DrivePipelineTestRobot() { ) } +class AdvancedJoystickInputPipelineTestRobot : DrivePipelineTestRobot() { + @JvmField + @Preference + var maxVelocity = 0.0 + + private val joystick = XboxController(0) + + override fun robotInit() { + PreferenceManager.getInstance().add(this) + val reset = SimpleCommand("reset", Executable { + _pipeline = DrivePipeline( + AdvancedArcadeJoystickInput( + maxVelocity, + Supplier { Utilities.processDeadzone(joystick.getY(GenericHID.Hand.kLeft), 0.1) }, + Supplier { Utilities.processDeadzone(joystick.getX(GenericHID.Hand.kRight), 0.1) }, + Supplier { + Utilities.processDeadzone(joystick.getTriggerAxis(GenericHID.Hand.kRight), 0.1) + Utilities.processDeadzone(-joystick.getTriggerAxis(GenericHID.Hand.kLeft), 0.1) + } + ), + OpenLoopFeedForwardProcessor(1 / maxVelocity, 0.0, 0.0), + TalonSRXOutput(PipelineDriveTrain.left1, PipelineDriveTrain.right1) + ) + }).apply { + setRunWhenDisabled(true) + start() + } + + SmartDashboard.putData(reset) + } + + private lateinit var _pipeline: DrivePipeline + override val pipeline get() = _pipeline +} + private object PipelineDriveTrain { val left1 = ChickenTalon(1).apply { setBrake(true) From b1a686cb924297ae7adaec00dd0cd7b54e1a41f9 Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Thu, 1 Nov 2018 08:21:24 -0700 Subject: [PATCH 075/145] Change some IntelliJ files Add encodings.xml and some weird changes to compiler.xml that I guess are from updating to the latest version of IntelliJ --- .idea/compiler.xml | 4 ++++ .idea/encodings.xml | 4 ++++ 2 files changed, 8 insertions(+) create mode 100644 .idea/encodings.xml 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 From cea9b379a3f2c3105115dacb2e9202edfa83255b Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Thu, 1 Nov 2018 08:24:28 -0700 Subject: [PATCH 076/145] Create run configurations for profile testing --- ...AdvancedJoystickInputPipelineTestRobot.xml | 21 +++++++++++++++++++ .../Deploy_SimpleDrivePipelineTestRobot.xml | 21 +++++++++++++++++++ 2 files changed, 42 insertions(+) create mode 100644 .idea/runConfigurations/Deploy_AdvancedJoystickInputPipelineTestRobot.xml create mode 100644 .idea/runConfigurations/Deploy_SimpleDrivePipelineTestRobot.xml diff --git a/.idea/runConfigurations/Deploy_AdvancedJoystickInputPipelineTestRobot.xml b/.idea/runConfigurations/Deploy_AdvancedJoystickInputPipelineTestRobot.xml new file mode 100644 index 00000000..37a08bbd --- /dev/null +++ b/.idea/runConfigurations/Deploy_AdvancedJoystickInputPipelineTestRobot.xml @@ -0,0 +1,21 @@ + + + + + + + true + + + \ No newline at end of file diff --git a/.idea/runConfigurations/Deploy_SimpleDrivePipelineTestRobot.xml b/.idea/runConfigurations/Deploy_SimpleDrivePipelineTestRobot.xml new file mode 100644 index 00000000..549dea37 --- /dev/null +++ b/.idea/runConfigurations/Deploy_SimpleDrivePipelineTestRobot.xml @@ -0,0 +1,21 @@ + + + + + + + true + + + \ No newline at end of file From a5f852d751ef6a777c1f0dc6780b5d4d8cef334e Mon Sep 17 00:00:00 2001 From: Zachary Robinson Date: Thu, 1 Nov 2018 08:34:03 -0700 Subject: [PATCH 077/145] Organize run configurations into folders --- .../Deploy_AccelerationCharacterizationRobot.xml | 2 +- .../Deploy_AdvancedJoystickInputPipelineTestRobot.xml | 2 +- .idea/runConfigurations/Deploy_SimpleDrivePipelineTestRobot.xml | 2 +- .../runConfigurations/Deploy_VelocityCharacterizationRobot.xml | 2 +- .idea/runConfigurations/Deploy_WheelbaseTestRobot.xml | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/.idea/runConfigurations/Deploy_AccelerationCharacterizationRobot.xml b/.idea/runConfigurations/Deploy_AccelerationCharacterizationRobot.xml index c8e0eb8a..8764883d 100644 --- a/.idea/runConfigurations/Deploy_AccelerationCharacterizationRobot.xml +++ b/.idea/runConfigurations/Deploy_AccelerationCharacterizationRobot.xml @@ -1,5 +1,5 @@ - +