From 24ff1dc06bf4665c2a38eb4c237eaa517293355d Mon Sep 17 00:00:00 2001 From: Noah Gleason Date: Mon, 4 Dec 2017 21:47:22 -0500 Subject: [PATCH 1/5] Add ability to pass data to MPD --- .../robot/other/MotionProfileData.java | 49 +++++++++++++++++++ 1 file changed, 49 insertions(+) diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/MotionProfileData.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/MotionProfileData.java index 7eb16050..02d8596f 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/MotionProfileData.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/MotionProfileData.java @@ -9,6 +9,7 @@ import java.io.BufferedReader; import java.io.FileReader; import java.io.IOException; +import java.util.List; /** * Data structure containing the array of points for the MP and a method to fill the MP from a csv file @@ -63,6 +64,54 @@ public MotionProfileData(@NotNull @JsonProperty(required = true) String filename } } + /** + * Data array constructor. + * @param pos An array with position setpoints, in feet. + * @param vel An array of corresponding velocity setpoints, in feet/sec. + * @param accel An array of corresponding acceleration setpoints, in feet/sec^2. + * @param deltaTime The time between setpoints, in seconds. + * @param inverted Whether or not the profile is inverted (would be inverted if we're driving it backwards) + * @param velocityOnly Whether or not to only use velocity feed-forward. Used for tuning kV and kA. Defaults to false. + * @param resetPosition Whether or not to reset the talon position when this profile starts. + */ + public MotionProfileData(@NotNull double[] pos, @NotNull double[] vel, @NotNull double[] accel, + double deltaTime, boolean inverted, boolean velocityOnly, boolean resetPosition){ + this.inverted = inverted; + this.velocityOnly = velocityOnly; + this.resetPosition = resetPosition; + data = new double[pos.length][4]; + for (int i = 0; i < pos.length; i++){ + data[i][0] = pos[i]; + data[i][1] = vel[i]; + data[i][2] = accel[i]; + data[i][3] = deltaTime; + } + } + + /** + * Data list constructor. + * @param pos A list with position setpoints, in feet. + * @param vel A list of corresponding velocity setpoints, in feet/sec. + * @param accel A list of corresponding acceleration setpoints, in feet/sec^2. + * @param deltaTime The time between setpoints, in seconds. + * @param inverted Whether or not the profile is inverted (would be inverted if we're driving it backwards) + * @param velocityOnly Whether or not to only use velocity feed-forward. Used for tuning kV and kA. Defaults to false. + * @param resetPosition Whether or not to reset the talon position when this profile starts. + */ + public MotionProfileData(@NotNull List pos, @NotNull List vel, @NotNull List accel, + double deltaTime, boolean inverted, boolean velocityOnly, boolean resetPosition){ + this.inverted = inverted; + this.velocityOnly = velocityOnly; + this.resetPosition = resetPosition; + data = new double[pos.size()][4]; + for (int i = 0; i < pos.size(); i++){ + data[i][0] = pos.get(i); + data[i][1] = vel.get(i); + data[i][2] = accel.get(i); + data[i][3] = deltaTime; + } + } + /** * Read the profile from the given file and store it in data. * From e3aabd3a12cc8cab1c70de49b3c9b518bff21576 Mon Sep 17 00:00:00 2001 From: Noah Gleason Date: Mon, 4 Dec 2017 21:57:48 -0500 Subject: [PATCH 2/5] Fix templates --- templates/GenericCommand.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/templates/GenericCommand.java b/templates/GenericCommand.java index 02be55db..4c8545b7 100644 --- a/templates/GenericCommand.java +++ b/templates/GenericCommand.java @@ -62,7 +62,7 @@ protected boolean isFinished() { */ @Override protected void end() { - Logger.addEvent("GenericSubsytemInterface end", this.getClass()); + Logger.addEvent("GenericCommand end", this.getClass()); } /** @@ -70,7 +70,7 @@ protected void end() { */ @Override protected void interrupted() { - Logger.addEvent("GenericSubsytemInterface interrupted!", this.getClass()); + Logger.addEvent("GenericCommand interrupted!", this.getClass()); } } From abc18148d2c4ef6c89fc29cb1b448c5558d086e1 Mon Sep 17 00:00:00 2001 From: Noah Gleason Date: Tue, 5 Dec 2017 21:49:00 -0500 Subject: [PATCH 3/5] Loosen requirement on RunProfile --- .../interfaces/motionProfile/commands/RunProfile.java | 3 ++- templates/GenericCommand.java | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/subsystem/interfaces/motionProfile/commands/RunProfile.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/subsystem/interfaces/motionProfile/commands/RunProfile.java index aab22d5d..37baa631 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/subsystem/interfaces/motionProfile/commands/RunProfile.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/subsystem/interfaces/motionProfile/commands/RunProfile.java @@ -8,13 +8,14 @@ import org.usfirst.frc.team449.robot.jacksonWrappers.YamlCommandGroupWrapper; import org.usfirst.frc.team449.robot.jacksonWrappers.YamlSubsystem; import org.usfirst.frc.team449.robot.other.MotionProfileData; +import org.usfirst.frc.team449.robot.subsystem.interfaces.motionProfile.SubsystemMP; import org.usfirst.frc.team449.robot.subsystem.interfaces.motionProfile.TwoSideMPSubsystem.SubsystemMPTwoSides; /** * Loads and runs the given profile into the given subsystem. */ @JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) -public class RunProfile extends YamlCommandGroupWrapper { +public class RunProfile extends YamlCommandGroupWrapper { /** * Default constructor. diff --git a/templates/GenericCommand.java b/templates/GenericCommand.java index 4c8545b7..18c40c4b 100644 --- a/templates/GenericCommand.java +++ b/templates/GenericCommand.java @@ -27,7 +27,7 @@ public class GenericCommand extends YamlCommandWrapper { * @param subsystem The subsystem to execute this command on. */ @JsonCreator - public RunMotorWhileConditonMet(@NotNull @JsonProperty(required = true) GenericSubsytemInterface subsystem) { + public GenericCommand(@NotNull @JsonProperty(required = true) GenericSubsytemInterface subsystem) { this.subsystem = subsystem; } From c7ecfa220df851985537efffe2c55f97154abdff Mon Sep 17 00:00:00 2001 From: Noah Gleason Date: Sat, 23 Dec 2017 14:05:25 -0500 Subject: [PATCH 4/5] Add poseEstimator interface --- .../poseEstimator/PoseEstimator.java | 20 +++++++++++++++++++ .../UnidirectionalPoseEstimator.java | 6 ++++-- .../other/UnidirectionalPoseEstimator.html | 2 +- 3 files changed, 25 insertions(+), 3 deletions(-) create mode 100644 RoboRIO/src/main/java/org/usfirst/frc/team449/robot/generalInterfaces/poseEstimator/PoseEstimator.java rename RoboRIO/src/main/java/org/usfirst/frc/team449/robot/{other => generalInterfaces/poseEstimator}/UnidirectionalPoseEstimator.java (98%) diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/generalInterfaces/poseEstimator/PoseEstimator.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/generalInterfaces/poseEstimator/PoseEstimator.java new file mode 100644 index 00000000..ff0032b1 --- /dev/null +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/generalInterfaces/poseEstimator/PoseEstimator.java @@ -0,0 +1,20 @@ +package org.usfirst.frc.team449.robot.generalInterfaces.poseEstimator; + +import com.fasterxml.jackson.annotation.JsonTypeInfo; +import org.jetbrains.annotations.NotNull; +import org.usfirst.frc.team449.robot.jacksonWrappers.MappedRunnable; + +/** + * Anything that estimates the absolute pose of the robot. + */ +@JsonTypeInfo(use = JsonTypeInfo.Id.CLASS, include = JsonTypeInfo.As.WRAPPER_OBJECT, property = "@class") +public interface PoseEstimator extends MappedRunnable{ + + /** + * Get the current absolute position of the robot + * + * @return The current x,y position in feet. + */ + @NotNull + double[] getPos(); +} diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/UnidirectionalPoseEstimator.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/generalInterfaces/poseEstimator/UnidirectionalPoseEstimator.java similarity index 98% rename from RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/UnidirectionalPoseEstimator.java rename to RoboRIO/src/main/java/org/usfirst/frc/team449/robot/generalInterfaces/poseEstimator/UnidirectionalPoseEstimator.java index 7284b2b9..5f579cb1 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/other/UnidirectionalPoseEstimator.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/generalInterfaces/poseEstimator/UnidirectionalPoseEstimator.java @@ -1,4 +1,4 @@ -package org.usfirst.frc.team449.robot.other; +package org.usfirst.frc.team449.robot.generalInterfaces.poseEstimator; import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIdentityInfo; @@ -8,6 +8,7 @@ import org.usfirst.frc.team449.robot.drive.unidirectional.DriveUnidirectional; import org.usfirst.frc.team449.robot.generalInterfaces.loggable.Loggable; import org.usfirst.frc.team449.robot.jacksonWrappers.MappedRunnable; +import org.usfirst.frc.team449.robot.other.Clock; import org.usfirst.frc.team449.robot.subsystem.interfaces.AHRS.SubsystemAHRS; import java.util.ArrayList; @@ -17,7 +18,7 @@ * A Runnable for pose estimation that can take absolute positions. */ @JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) -public class UnidirectionalPoseEstimator implements MappedRunnable, Loggable { +public class UnidirectionalPoseEstimator implements PoseEstimator, Loggable { /** * The subsystem to get gyro and encoder data from. @@ -277,6 +278,7 @@ public synchronized boolean addAbsolutePos(double x, double y, long time, double * @return The current x,y position in feet. */ @NotNull + @Override public double[] getPos() { return currentPos; } diff --git a/docs/org/usfirst/frc/team449/robot/other/UnidirectionalPoseEstimator.html b/docs/org/usfirst/frc/team449/robot/other/UnidirectionalPoseEstimator.html index 3289a418..4291913b 100644 --- a/docs/org/usfirst/frc/team449/robot/other/UnidirectionalPoseEstimator.html +++ b/docs/org/usfirst/frc/team449/robot/other/UnidirectionalPoseEstimator.html @@ -124,7 +124,7 @@

Class Unidirectional
  • java.lang.Object
    • -
    • org.usfirst.frc.team449.robot.other.UnidirectionalPoseEstimator<T>
      • +
    • org.usfirst.frc.team449.robot.generalInterfaces.poseEstimator.UnidirectionalPoseEstimator<T>
    • From dbed9fb6362bfafeabf806186dcbf678c760e9d6 Mon Sep 17 00:00:00 2001 From: Noah Gleason Date: Sat, 23 Dec 2017 23:50:57 -0500 Subject: [PATCH 5/5] Add lambda versions of MP run commands --- .../commands/LoadProfileTwoSides.java | 36 +++++++++++++++++-- .../commands/RunProfileTwoSides.java | 18 ++++++++++ .../motionProfile/commands/LoadProfile.java | 35 +++++++++++++++--- .../motionProfile/commands/RunProfile.java | 16 +++++++++ 4 files changed, 99 insertions(+), 6 deletions(-) diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/subsystem/interfaces/motionProfile/TwoSideMPSubsystem/commands/LoadProfileTwoSides.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/subsystem/interfaces/motionProfile/TwoSideMPSubsystem/commands/LoadProfileTwoSides.java index 8a4e8db6..d42aebf5 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/subsystem/interfaces/motionProfile/TwoSideMPSubsystem/commands/LoadProfileTwoSides.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/subsystem/interfaces/motionProfile/TwoSideMPSubsystem/commands/LoadProfileTwoSides.java @@ -5,11 +5,14 @@ import com.fasterxml.jackson.annotation.JsonProperty; import com.fasterxml.jackson.annotation.ObjectIdGenerators; import org.jetbrains.annotations.NotNull; +import org.jetbrains.annotations.Nullable; import org.usfirst.frc.team449.robot.jacksonWrappers.YamlCommandWrapper; import org.usfirst.frc.team449.robot.other.Logger; import org.usfirst.frc.team449.robot.other.MotionProfileData; import org.usfirst.frc.team449.robot.subsystem.interfaces.motionProfile.TwoSideMPSubsystem.SubsystemMPTwoSides; +import java.util.function.Supplier; + /** * Loads the given profiles into the subsystem, but doesn't run it. */ @@ -25,9 +28,15 @@ public class LoadProfileTwoSides extends YamlCommandWrapper { /** * The motion profiles for the left and right sides to execute, respectively. */ - @NotNull + @Nullable private final MotionProfileData left, right; + /** + * Suppliers for motion profiles for the left and right sides to execute, respectively. + */ + @Nullable + private final Supplier leftSupplier, rightSupplier; + /** * Default constructor * @@ -42,6 +51,25 @@ public LoadProfileTwoSides(@NotNull @JsonProperty(required = true) SubsystemMPTw this.subsystem = subsystem; this.left = left; this.right = right; + this.leftSupplier = null; + this.rightSupplier = null; + } + + /** + * Default constructor + * + * @param subsystem The subsystem to execute this command on. + * @param leftSupplier A supplier for the profile for the left side to run. + * @param rightSupplier A supplier for the profile for the right side to run. + */ + public LoadProfileTwoSides(@NotNull SubsystemMPTwoSides subsystem, + @NotNull Supplier leftSupplier, + @NotNull Supplier rightSupplier) { + this.subsystem = subsystem; + this.left = null; + this.right = null; + this.leftSupplier = leftSupplier; + this.rightSupplier = rightSupplier; } /** @@ -57,7 +85,11 @@ protected void initialize() { */ @Override protected void execute() { - subsystem.loadMotionProfile(left, right); + if(left!= null) { + subsystem.loadMotionProfile(left, right); + } else { + subsystem.loadMotionProfile(leftSupplier.get(), rightSupplier.get()); + } } /** diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/subsystem/interfaces/motionProfile/TwoSideMPSubsystem/commands/RunProfileTwoSides.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/subsystem/interfaces/motionProfile/TwoSideMPSubsystem/commands/RunProfileTwoSides.java index caf467f5..ab9d0b1f 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/subsystem/interfaces/motionProfile/TwoSideMPSubsystem/commands/RunProfileTwoSides.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/subsystem/interfaces/motionProfile/TwoSideMPSubsystem/commands/RunProfileTwoSides.java @@ -11,6 +11,8 @@ import org.usfirst.frc.team449.robot.subsystem.interfaces.motionProfile.TwoSideMPSubsystem.SubsystemMPTwoSides; import org.usfirst.frc.team449.robot.subsystem.interfaces.motionProfile.commands.RunLoadedProfile; +import java.util.function.Supplier; + /** * Loads and runs the given profiles into the given subsystem. */ @@ -33,4 +35,20 @@ public RunProfileTwoSides(@NotNull @JsonProperty(required = true) T subsystem, addParallel(new LoadProfileTwoSides(subsystem, left, right)); addParallel(new RunLoadedProfile<>(subsystem, timeout, true)); } + + /** + * Default constructor. + * + * @param subsystem The subsystem to execute this command on. + * @param leftSupplier A supplier for the motion profile for the left side to load and execute. + * @param rightSupplier A supplier for the motion profile for the right side to load and execute. + * @param timeout The maximum amount of time this command is allowed to take, in seconds. + */ + public RunProfileTwoSides(@NotNull T subsystem, + @NotNull Supplier leftSupplier, + @NotNull Supplier rightSupplier, + double timeout) { + addParallel(new LoadProfileTwoSides(subsystem, leftSupplier, rightSupplier)); + addParallel(new RunLoadedProfile<>(subsystem, timeout, true)); + } } diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/subsystem/interfaces/motionProfile/commands/LoadProfile.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/subsystem/interfaces/motionProfile/commands/LoadProfile.java index b36325b0..c611ed86 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/subsystem/interfaces/motionProfile/commands/LoadProfile.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/subsystem/interfaces/motionProfile/commands/LoadProfile.java @@ -5,11 +5,14 @@ import com.fasterxml.jackson.annotation.JsonProperty; import com.fasterxml.jackson.annotation.ObjectIdGenerators; import org.jetbrains.annotations.NotNull; +import org.jetbrains.annotations.Nullable; import org.usfirst.frc.team449.robot.jacksonWrappers.YamlCommandWrapper; import org.usfirst.frc.team449.robot.other.Logger; import org.usfirst.frc.team449.robot.other.MotionProfileData; import org.usfirst.frc.team449.robot.subsystem.interfaces.motionProfile.SubsystemMP; +import java.util.function.Supplier; + /** * Loads the given profile into the subsystem, but doesn't run it. */ @@ -23,22 +26,42 @@ public class LoadProfile extends YamlCommandWrapper { private final SubsystemMP subsystem; /** - * The profile to execute. + * The profile to load. Can be null if profileSupplier isn't. */ - @NotNull + @Nullable private final MotionProfileData profile; + /** + * A Supplier that gets the profile to load. Ignored if profile isn't null, must be non-null if profile is null. + */ + @Nullable + private final Supplier profileSupplier; + /** * Default constructor * * @param subsystem The subsystem to execute this command on. - * @param profile The profile to run. + * @param profile The profile to load. */ @JsonCreator public LoadProfile(@NotNull @JsonProperty(required = true) SubsystemMP subsystem, @NotNull @JsonProperty(required = true) MotionProfileData profile) { this.subsystem = subsystem; this.profile = profile; + this.profileSupplier = null; + } + + /** + * Constructor that takes a lambda, for use in dynamic commandGroups. + * + * @param subsystem The subsystem to execute this command on. + * @param profileSupplier A Supplier that gets the profile to load. + */ + public LoadProfile(@NotNull SubsystemMP subsystem, + @NotNull Supplier profileSupplier){ + this.subsystem = subsystem; + this.profileSupplier = profileSupplier; + this.profile = null; } /** @@ -54,7 +77,11 @@ protected void initialize() { */ @Override protected void execute() { - subsystem.loadMotionProfile(profile); + if (profile != null) { + subsystem.loadMotionProfile(profile); + } else { + subsystem.loadMotionProfile(profileSupplier.get()); + } } /** diff --git a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/subsystem/interfaces/motionProfile/commands/RunProfile.java b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/subsystem/interfaces/motionProfile/commands/RunProfile.java index 37baa631..b3fcc9c1 100644 --- a/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/subsystem/interfaces/motionProfile/commands/RunProfile.java +++ b/RoboRIO/src/main/java/org/usfirst/frc/team449/robot/subsystem/interfaces/motionProfile/commands/RunProfile.java @@ -11,6 +11,8 @@ import org.usfirst.frc.team449.robot.subsystem.interfaces.motionProfile.SubsystemMP; import org.usfirst.frc.team449.robot.subsystem.interfaces.motionProfile.TwoSideMPSubsystem.SubsystemMPTwoSides; +import java.util.function.Supplier; + /** * Loads and runs the given profile into the given subsystem. */ @@ -31,4 +33,18 @@ public RunProfile(@NotNull @JsonProperty(required = true) T subsystem, addSequential(new LoadProfile(subsystem, profile)); addSequential(new RunLoadedProfile<>(subsystem, timeout, true)); } + + /** + * Constructor that takes a lambda, for use in dynamic commandGroups. + * + * @param subsystem The subsystem to execute this command on. + * @param profileSupplier A supplier to get the profile to load and execute. + * @param timeout The maximum amount of time this command is allowed to take, in seconds. + */ + public RunProfile(@NotNull T subsystem, + @NotNull Supplier profileSupplier, + double timeout){ + addSequential(new LoadProfile(subsystem, profileSupplier)); + addSequential(new RunLoadedProfile<>(subsystem, timeout, true)); + } }