Add holonomic follower examples (#2052)

wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h

@@ -121,6 +121,22 @@ class SwerveDriveKinematics {
   template <typename... ModuleStates>
   ChassisSpeeds ToChassisSpeeds(ModuleStates&&... wheelStates);
 
+  /**
+   * Performs forward kinematics to return the resulting chassis state from the
+   * given module states. This method is often used for odometry -- determining
+   * the robot's position on the field using data from the real-world speed and
+   * angle of each module on the robot.
+   *
+   * @param moduleStates The state of the modules as an std::array of type
+   * SwerveModuleState, NumModules long as measured from respective encoders
+   * and gyros. The order of the swerve module states should be same as passed
+   * into the constructor of this class.
+   *
+   * @return The resulting chassis speed.
+   */
+  ChassisSpeeds ToChassisSpeeds(
+      std::array<SwerveModuleState, NumModules> moduleStates);
+
   /**
    * Normalizes the wheel speeds using some max attainable speed. Sometimes,
    * after inverse kinematics, the requested speed from a/several modules may be

wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc

@@ -63,6 +63,13 @@ ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
                 "locations provided in constructor.");
 
   std::array<SwerveModuleState, NumModules> moduleStates{wheelStates...};
+
+  return this->ToChassisSpeeds(moduleStates);
+}
+
+template <size_t NumModules>
+ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
+    std::array<SwerveModuleState, NumModules> moduleStates) {
   Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix;
 
   for (size_t i = 0; i < NumModules; i++) {