Merge pull request #55 from blair-robot-project/jetson_communication

Jetson communication
blair-robot-project · Jan 26, 2018 · d55ba8f · d55ba8f
2 parents e4f37e3 + dd3f0aa
commit d55ba8f
Show file tree

Hide file tree

Showing 332 changed files with 1,150 additions and 743 deletions.
diff --git a/...ot/other/UnidirectionalPoseEstimator.java → ...stimator/UnidirectionalPoseEstimator.java b/...ot/other/UnidirectionalPoseEstimator.java → ...stimator/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;
@@ -7,7 +7,7 @@
 import org.jetbrains.annotations.NotNull;
 import org.usfirst.frc.team449.robot.drive.unidirectional.DriveUnidirectional;
 import org.usfirst.frc.team449.robot.generalInterfaces.loggable.Loggable;
-import org.usfirst.frc.team449.robot.generalInterfaces.poseEstimator.PoseEstimator;
+import org.usfirst.frc.team449.robot.other.Clock;
 import org.usfirst.frc.team449.robot.subsystem.interfaces.AHRS.SubsystemAHRS;
 
 import java.util.ArrayList;
@@ -18,7 +18,6 @@
  */
 @JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class)
 public class UnidirectionalPoseEstimator<T extends SubsystemAHRS & DriveUnidirectional> implements PoseEstimator, Loggable {
-
     /**
      * The subsystem to get gyro and encoder data from.
      */

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
@@ -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
@@ -69,6 +70,59 @@ 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;
+        this.pointTimeMillis = (int) (deltaTime / 1000.);
+        data = new double[pos.length][3];
+        for (int i = 0; i < pos.length; i++) {
+            data[i][0] = pos[i];
+            data[i][1] = vel[i];
+            data[i][2] = accel[i];
+        }
+    }
+
+    /**
+     * 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<Double> pos, @NotNull List<Double> vel, @NotNull List<Double> accel,
+                             double deltaTime, boolean inverted, boolean velocityOnly, boolean resetPosition) {
+        this.inverted = inverted;
+        this.velocityOnly = velocityOnly;
+        this.resetPosition = resetPosition;
+        this.pointTimeMillis = (int) (deltaTime / 1000.);
+        data = new double[pos.size()][3];
+        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);
+
+        }
+    }
+
     /**
      * Read the profile from the given file and store it in data.
      *

diff --git a/...t/subsystem/interfaces/motionProfile/TwoSideMPSubsystem/commands/LoadProfileTwoSides.java b/...t/subsystem/interfaces/motionProfile/TwoSideMPSubsystem/commands/LoadProfileTwoSides.java
@@ -6,10 +6,13 @@
 import com.fasterxml.jackson.annotation.ObjectIdGenerators;
 import edu.wpi.first.wpilibj.command.InstantCommand;
 import org.jetbrains.annotations.NotNull;
+import org.jetbrains.annotations.Nullable;
 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 InstantCommand {
     /**
      * 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<MotionProfileData> 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<MotionProfileData> leftSupplier,
+                               @NotNull Supplier<MotionProfileData> 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/...ot/subsystem/interfaces/motionProfile/TwoSideMPSubsystem/commands/RunProfileTwoSides.java b/...ot/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,
         addSequential(new LoadProfileTwoSides(subsystem, left, right));
         addSequential(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<MotionProfileData> leftSupplier,
+                              @NotNull Supplier<MotionProfileData> rightSupplier,
+                              double timeout) {
+        addSequential(new LoadProfileTwoSides(subsystem, leftSupplier, rightSupplier));
+        addSequential(new RunLoadedProfile<>(subsystem, timeout, true));
+    }
 }
diff --git a/...rg/usfirst/frc/team449/robot/subsystem/interfaces/motionProfile/commands/LoadProfile.java b/...rg/usfirst/frc/team449/robot/subsystem/interfaces/motionProfile/commands/LoadProfile.java
@@ -6,10 +6,13 @@
 import com.fasterxml.jackson.annotation.ObjectIdGenerators;
 import edu.wpi.first.wpilibj.command.InstantCommand;
 import org.jetbrains.annotations.NotNull;
+import org.jetbrains.annotations.Nullable;
 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 InstantCommand {
     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<MotionProfileData> 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<MotionProfileData> 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/...org/usfirst/frc/team449/robot/subsystem/interfaces/motionProfile/commands/RunProfile.java b/...org/usfirst/frc/team449/robot/subsystem/interfaces/motionProfile/commands/RunProfile.java
@@ -10,6 +10,8 @@
 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 and runs the given profile into the given subsystem.
  */
@@ -30,4 +32,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<MotionProfileData> profileSupplier,
+                      double timeout) {
+        addSequential(new LoadProfile(subsystem, profileSupplier));
+        addSequential(new RunLoadedProfile<>(subsystem, timeout, true));
+    }
 }
diff --git a/docs/allclasses-frame.html b/docs/allclasses-frame.html
@@ -2,9 +2,9 @@
 <!-- NewPage -->
 <html lang="en">
 <head>
-<!-- Generated by javadoc (1.8.0_151) on Wed Jan 24 22:24:51 EST 2018 -->
+<!-- Generated by javadoc (1.8.0_121) on Fri Jan 26 16:27:50 EST 2018 -->
 <title>All Classes (RoboRIO API)</title>
-<meta name="date" content="2018-01-24">
+<meta name="date" content="2018-01-26">
 <link rel="stylesheet" type="text/css" href="stylesheet.css" title="Style">
 <script type="text/javascript" src="script.js"></script>
 </head>
@@ -159,7 +159,7 @@ <h1 class="bar">All&nbsp;Classes</h1>
 <li><a href="org/usfirst/frc/team449/robot/subsystem/interfaces/binaryMotor/commands/TurnMotorOn.html" title="class in org.usfirst.frc.team449.robot.subsystem.interfaces.binaryMotor.commands" target="classFrame">TurnMotorOn</a></li>
 <li><a href="org/usfirst/frc/team449/robot/commands/multiInterface/drive/UnidirectionalNavXDefaultDrive.html" title="class in org.usfirst.frc.team449.robot.commands.multiInterface.drive" target="classFrame">UnidirectionalNavXDefaultDrive</a></li>
 <li><a href="org/usfirst/frc/team449/robot/commands/multiInterface/drive/UnidirectionalNavXShiftingDefaultDrive.html" title="class in org.usfirst.frc.team449.robot.commands.multiInterface.drive" target="classFrame">UnidirectionalNavXShiftingDefaultDrive</a></li>
-<li><a href="org/usfirst/frc/team449/robot/other/UnidirectionalPoseEstimator.html" title="class in org.usfirst.frc.team449.robot.other" target="classFrame">UnidirectionalPoseEstimator</a></li>
+<li><a href="org/usfirst/frc/team449/robot/generalInterfaces/poseEstimator/UnidirectionalPoseEstimator.html" title="class in org.usfirst.frc.team449.robot.generalInterfaces.poseEstimator" target="classFrame">UnidirectionalPoseEstimator</a></li>
 <li><a href="org/usfirst/frc/team449/robot/generalInterfaces/updatable/Updatable.html" title="interface in org.usfirst.frc.team449.robot.generalInterfaces.updatable" target="classFrame"><span class="interfaceName">Updatable</span></a></li>
 <li><a href="org/usfirst/frc/team449/robot/other/Updater.html" title="class in org.usfirst.frc.team449.robot.other" target="classFrame">Updater</a></li>
 <li><a href="org/usfirst/frc/team449/robot/drive/unidirectional/commands/VoltageRamp.html" title="class in org.usfirst.frc.team449.robot.drive.unidirectional.commands" target="classFrame">VoltageRamp</a></li>

diff --git a/docs/allclasses-noframe.html b/docs/allclasses-noframe.html
@@ -2,9 +2,9 @@
 <!-- NewPage -->
 <html lang="en">
 <head>
-<!-- Generated by javadoc (1.8.0_151) on Wed Jan 24 22:24:51 EST 2018 -->
+<!-- Generated by javadoc (1.8.0_121) on Fri Jan 26 16:27:50 EST 2018 -->
 <title>All Classes (RoboRIO API)</title>
-<meta name="date" content="2018-01-24">
+<meta name="date" content="2018-01-26">
 <link rel="stylesheet" type="text/css" href="stylesheet.css" title="Style">
 <script type="text/javascript" src="script.js"></script>
 </head>
@@ -159,7 +159,7 @@ <h1 class="bar">All&nbsp;Classes</h1>
 <li><a href="org/usfirst/frc/team449/robot/subsystem/interfaces/binaryMotor/commands/TurnMotorOn.html" title="class in org.usfirst.frc.team449.robot.subsystem.interfaces.binaryMotor.commands">TurnMotorOn</a></li>
 <li><a href="org/usfirst/frc/team449/robot/commands/multiInterface/drive/UnidirectionalNavXDefaultDrive.html" title="class in org.usfirst.frc.team449.robot.commands.multiInterface.drive">UnidirectionalNavXDefaultDrive</a></li>
 <li><a href="org/usfirst/frc/team449/robot/commands/multiInterface/drive/UnidirectionalNavXShiftingDefaultDrive.html" title="class in org.usfirst.frc.team449.robot.commands.multiInterface.drive">UnidirectionalNavXShiftingDefaultDrive</a></li>
-<li><a href="org/usfirst/frc/team449/robot/other/UnidirectionalPoseEstimator.html" title="class in org.usfirst.frc.team449.robot.other">UnidirectionalPoseEstimator</a></li>
+<li><a href="org/usfirst/frc/team449/robot/generalInterfaces/poseEstimator/UnidirectionalPoseEstimator.html" title="class in org.usfirst.frc.team449.robot.generalInterfaces.poseEstimator">UnidirectionalPoseEstimator</a></li>
 <li><a href="org/usfirst/frc/team449/robot/generalInterfaces/updatable/Updatable.html" title="interface in org.usfirst.frc.team449.robot.generalInterfaces.updatable"><span class="interfaceName">Updatable</span></a></li>
 <li><a href="org/usfirst/frc/team449/robot/other/Updater.html" title="class in org.usfirst.frc.team449.robot.other">Updater</a></li>
 <li><a href="org/usfirst/frc/team449/robot/drive/unidirectional/commands/VoltageRamp.html" title="class in org.usfirst.frc.team449.robot.drive.unidirectional.commands">VoltageRamp</a></li>

diff --git a/docs/constant-values.html b/docs/constant-values.html
@@ -2,9 +2,9 @@
 <!-- NewPage -->
 <html lang="en">
 <head>
-<!-- Generated by javadoc (1.8.0_151) on Wed Jan 24 22:24:51 EST 2018 -->
+<!-- Generated by javadoc (1.8.0_121) on Fri Jan 26 16:27:49 EST 2018 -->
 <title>Constant Field Values (RoboRIO API)</title>
-<meta name="date" content="2018-01-24">
+<meta name="date" content="2018-01-26">
 <link rel="stylesheet" type="text/css" href="stylesheet.css" title="Style">
 <script type="text/javascript" src="script.js"></script>
 </head>

diff --git a/docs/deprecated-list.html b/docs/deprecated-list.html
@@ -2,9 +2,9 @@
 <!-- NewPage -->
 <html lang="en">
 <head>
-<!-- Generated by javadoc (1.8.0_151) on Wed Jan 24 22:24:51 EST 2018 -->
+<!-- Generated by javadoc (1.8.0_121) on Fri Jan 26 16:27:50 EST 2018 -->
 <title>Deprecated List (RoboRIO API)</title>
-<meta name="date" content="2018-01-24">
+<meta name="date" content="2018-01-26">
 <link rel="stylesheet" type="text/css" href="stylesheet.css" title="Style">
 <script type="text/javascript" src="script.js"></script>
 </head>

diff --git a/docs/help-doc.html b/docs/help-doc.html
@@ -2,9 +2,9 @@
 <!-- NewPage -->
 <html lang="en">
 <head>
-<!-- Generated by javadoc (1.8.0_151) on Wed Jan 24 22:24:51 EST 2018 -->
+<!-- Generated by javadoc (1.8.0_121) on Fri Jan 26 16:27:50 EST 2018 -->
 <title>API Help (RoboRIO API)</title>
-<meta name="date" content="2018-01-24">
+<meta name="date" content="2018-01-26">
 <link rel="stylesheet" type="text/css" href="stylesheet.css" title="Style">
 <script type="text/javascript" src="script.js"></script>
 </head>