Add kinematics suite (#1787)

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: Matt <matthew.morley.ca@gmail.com>

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java

@@ -0,0 +1,121 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.differentialdrivebot;
+
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
+
+/**
+ * Represents a differential drive style drivetrain.
+ */
+public class Drivetrain {
+  public static final double kMaxSpeed = 3.0; // meters per second
+  public static final double kMaxAngularSpeed = 2 * Math.PI; // one rotation per second
+
+  private static final double kTrackWidth = 0.381 * 2; // meters
+  private static final double kWheelRadius = 0.0508; // meters
+  private static final int kEncoderResolution = 4096;
+
+  private final Spark m_leftMaster = new Spark(1);
+  private final Spark m_leftFollower = new Spark(2);
+  private final Spark m_rightMaster = new Spark(3);
+  private final Spark m_rightFollower = new Spark(4);
+
+  private final Encoder m_leftEncoder = new Encoder(0, 1);
+  private final Encoder m_rightEncoder = new Encoder(0, 1);
+
+  private final SpeedControllerGroup m_leftGroup
+      = new SpeedControllerGroup(m_leftMaster, m_leftFollower);
+  private final SpeedControllerGroup m_rightGroup
+      = new SpeedControllerGroup(m_rightMaster, m_rightFollower);
+
+  private final AnalogGyro m_gyro = new AnalogGyro(0);
+
+  private final PIDController m_leftPIDController = new PIDController(1, 0, 0);
+  private final PIDController m_rightPIDController = new PIDController(1, 0, 0);
+
+  private final DifferentialDriveKinematics m_kinematics
+      = new DifferentialDriveKinematics(kTrackWidth);
+
+  private final DifferentialDriveOdometry m_odometry
+      = new DifferentialDriveOdometry(m_kinematics);
+
+  /**
+   * Constructs a differential drive object.
+   * Sets the encoder distance per pulse and resets the gyro.
+   */
+  public Drivetrain() {
+    m_gyro.reset();
+
+    // Set the distance per pulse for the drive encoders. We can simply use the
+    // distance traveled for one rotation of the wheel divided by the encoder
+    // resolution.
+    m_leftEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
+    m_rightEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
+  }
+
+  /**
+   * Returns the angle of the robot as a Rotation2d.
+   *
+   * @return The angle of the robot.
+   */
+  public Rotation2d getAngle() {
+    // Negating the angle because WPILib gyros are CW positive.
+    return Rotation2d.fromDegrees(-m_gyro.getAngle());
+  }
+
+  /**
+   * Returns the current wheel speeds.
+   *
+   * @return The current wheel speeds.
+   */
+  public DifferentialDriveWheelSpeeds getCurrentSpeeds() {
+    return new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate());
+  }
+
+  /**
+   * Sets the desired wheel speeds.
+   *
+   * @param speeds The desired wheel speeds.
+   */
+  public void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
+    double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(),
+        speeds.leftMetersPerSecond);
+    double rightOutput = m_rightPIDController.calculate(m_rightEncoder.getRate(),
+        speeds.rightMetersPerSecond);
+    m_leftGroup.set(leftOutput);
+    m_rightGroup.set(rightOutput);
+  }
+
+  /**
+   * Drives the robot with the given linear velocity and angular velocity.
+   *
+   * @param xSpeed Linear velocity in m/s.
+   * @param rot    Angular velocity in rad/s.
+   */
+  @SuppressWarnings("ParameterName")
+  public void drive(double xSpeed, double rot) {
+    var wheelSpeeds = m_kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0.0, rot));
+    setSpeeds(wheelSpeeds);
+  }
+
+  /**
+   * Updates the field-relative position.
+   */
+  public void updateOdometry() {
+    m_odometry.update(getAngle(), getCurrentSpeeds());
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Main.java

@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.differentialdrivebot;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all.
+ * Unless you know what you are doing, do not modify this file except to
+ * change the parameter class to the startRobot call.
+ */
+public final class Main {
+  private Main() {
+  }
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java

@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.differentialdrivebot;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.XboxController;
+
+import static edu.wpi.first.wpilibj.examples.differentialdrivebot.Drivetrain.kMaxAngularSpeed;
+import static edu.wpi.first.wpilibj.examples.differentialdrivebot.Drivetrain.kMaxSpeed;
+
+public class Robot extends TimedRobot {
+  private final XboxController m_controller = new XboxController(0);
+  private final Drivetrain m_drive = new Drivetrain();
+
+  @Override
+  public void autonomousPeriodic() {
+    teleopPeriodic();
+    m_drive.updateOdometry();
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    // Get the x speed. We are inverting this because Xbox controllers return
+    // negative values when we push forward.
+    final var xSpeed = -m_controller.getY(GenericHID.Hand.kLeft) * kMaxSpeed;
+
+    // Get the rate of angular rotation. We are inverting this because we want a
+    // positive value when we pull to the left (remember, CCW is positive in
+    // mathematics). Xbox controllers return positive values when you pull to
+    // the right by default.
+    final var rot = -m_controller.getX(GenericHID.Hand.kRight) * kMaxAngularSpeed;
+
+    m_drive.drive(xSpeed, rot);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json

@@ -343,5 +343,35 @@
     ],
     "foldername": "gyrodrivecommands",
     "mainclass": "Main"
+  },
+  {
+    "name": "SwerveBot",
+    "description": "An example program for a swerve drive that uses swerve drive kinematics and odometry.",
+    "tags": [
+      "SwerveBot"
+    ],
+    "foldername": "swervebot",
+    "gradlebase": "java",
+    "mainclass": "Main"
+  },
+  {
+    "name": "MecanumBot",
+    "description": "An example program for a mecanum drive that uses mecanum drive kinematics and odometry.",
+    "tags": [
+      "MecanumBot"
+    ],
+    "foldername": "mecanumbot",
+    "gradlebase": "java",
+    "mainclass": "Main"
+  },
+  {
+    "name": "DifferentialDriveBot",
+    "description": "An example program for a differential drive that uses differential drive kinematics and odometry.",
+    "tags": [
+      "MecanumBot"
+    ],
+    "foldername": "differentialdrivebot",
+    "gradlebase": "java",
+    "mainclass": "Main"
   }
 ]

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java

@@ -0,0 +1,138 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.mecanumbot;
+
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveOdometry;
+import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds;
+
+/**
+ * Represents a mecanum drive style drivetrain.
+ */
+@SuppressWarnings("PMD.TooManyFields")
+public class Drivetrain {
+  public static final double kMaxSpeed = 3.0; // 3 meters per second
+  public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second
+
+  private final Spark m_frontLeftMotor = new Spark(1);
+  private final Spark m_frontRightMotor = new Spark(2);
+  private final Spark m_backLeftMotor = new Spark(3);
+  private final Spark m_backRightMotor = new Spark(4);
+
+  private final Encoder m_frontLeftEncoder = new Encoder(0, 1);
+  private final Encoder m_frontRightEncoder = new Encoder(0, 1);
+  private final Encoder m_backLeftEncoder = new Encoder(0, 1);
+  private final Encoder m_backRightEncoder = new Encoder(0, 1);
+
+  private final Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381);
+  private final Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381);
+  private final Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381);
+  private final Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381);
+
+  private final PIDController m_frontLeftPIDController = new PIDController(1, 0, 0);
+  private final PIDController m_frontRightPIDController = new PIDController(1, 0, 0);
+  private final PIDController m_backLeftPIDController = new PIDController(1, 0, 0);
+  private final PIDController m_backRightPIDController = new PIDController(1, 0, 0);
+
+  private final AnalogGyro m_gyro = new AnalogGyro(0);
+
+  private final MecanumDriveKinematics m_kinematics = new MecanumDriveKinematics(
+      m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation
+  );
+
+  private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics);
+
+  /**
+   * Constructs a MecanumDrive and resets the gyro.
+   */
+  public Drivetrain() {
+    m_gyro.reset();
+  }
+
+  /**
+   * Returns the angle of the robot as a Rotation2d.
+   *
+   * @return The angle of the robot.
+   */
+  public Rotation2d getAngle() {
+    // Negating the angle because WPILib gyros are CW positive.
+    return Rotation2d.fromDegrees(-m_gyro.getAngle());
+  }
+
+  /**
+   * Returns the current state of the drivetrain.
+   *
+   * @return The current state of the drivetrain.
+   */
+  public MecanumDriveWheelSpeeds getCurrentState() {
+    return new MecanumDriveWheelSpeeds(
+        m_frontLeftEncoder.getRate(),
+        m_frontRightEncoder.getRate(),
+        m_backLeftEncoder.getRate(),
+        m_backRightEncoder.getRate()
+    );
+  }
+
+  /**
+   * Set the desired speeds for each wheel.
+   *
+   * @param speeds The desired wheel speeds.
+   */
+  public void setSpeeds(MecanumDriveWheelSpeeds speeds) {
+    final var frontLeftOutput = m_frontLeftPIDController.calculate(
+        m_frontLeftEncoder.getRate(), speeds.frontLeftMetersPerSecond
+    );
+    final var frontRightOutput = m_frontRightPIDController.calculate(
+        m_frontRightEncoder.getRate(), speeds.frontRightMetersPerSecond
+    );
+    final var backLeftOutput = m_backLeftPIDController.calculate(
+        m_backLeftEncoder.getRate(), speeds.rearLeftMetersPerSecond
+    );
+    final var backRightOutput = m_backRightPIDController.calculate(
+        m_backRightEncoder.getRate(), speeds.rearRightMetersPerSecond
+    );
+
+    m_frontLeftMotor.set(frontLeftOutput);
+    m_frontRightMotor.set(frontRightOutput);
+    m_backLeftMotor.set(backLeftOutput);
+    m_backRightMotor.set(backRightOutput);
+  }
+
+  /**
+   * Method to drive the robot using joystick info.
+   *
+   * @param xSpeed        Speed of the robot in the x direction (forward).
+   * @param ySpeed        Speed of the robot in the y direction (sideways).
+   * @param rot           Angular rate of the robot.
+   * @param fieldRelative Whether the provided x and y speeds are relative to the field.
+   */
+  @SuppressWarnings("ParameterName")
+  public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
+    var mecanumDriveWheelSpeeds = m_kinematics.toWheelSpeeds(
+        fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(
+            xSpeed, ySpeed, rot, getAngle()
+        ) : new ChassisSpeeds(xSpeed, ySpeed, rot)
+    );
+    mecanumDriveWheelSpeeds.normalize(kMaxSpeed);
+    setSpeeds(mecanumDriveWheelSpeeds);
+  }
+
+  /**
+   * Updates the field relative position of the robot.
+   */
+  public void updateOdometry() {
+    m_odometry.update(getAngle(), getCurrentState());
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Main.java

@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.mecanumbot;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all.
+ * Unless you know what you are doing, do not modify this file except to
+ * change the parameter class to the startRobot call.
+ */
+public final class Main {
+  private Main() {
+  }
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java

@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.mecanumbot;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.XboxController;
+
+import static edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxAngularSpeed;
+import static edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxSpeed;
+
+public class Robot extends TimedRobot {
+  private final XboxController m_controller = new XboxController(0);
+  private final Drivetrain m_mecanum = new Drivetrain();
+
+  @Override
+  public void autonomousPeriodic() {
+    driveWithJoystick(false);
+    m_mecanum.updateOdometry();
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    driveWithJoystick(true);
+  }
+
+  private void driveWithJoystick(boolean fieldRelative) {
+    // Get the x speed. We are inverting this because Xbox controllers return
+    // negative values when we push forward.
+    final var xSpeed = -m_controller.getY(GenericHID.Hand.kLeft) * kMaxSpeed;
+
+    // Get the y speed or sideways/strafe speed. We are inverting this because
+    // we want a positive value when we pull to the left. Xbox controllers
+    // return positive values when you pull to the right by default.
+    final var ySpeed = -m_controller.getX(GenericHID.Hand.kLeft) * kMaxSpeed;
+
+    // Get the rate of angular rotation. We are inverting this because we want a
+    // positive value when we pull to the left (remember, CCW is positive in
+    // mathematics). Xbox controllers return positive values when you pull to
+    // the right by default.
+    final var rot = -m_controller.getX(GenericHID.Hand.kRight) * kMaxAngularSpeed;
+
+    m_mecanum.drive(xSpeed, ySpeed, rot, fieldRelative);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java

@@ -0,0 +1,90 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.swervebot;
+
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.wpilibj.kinematics.SwerveDriveOdometry;
+
+/**
+ * Represents a swerve drive style drivetrain.
+ */
+public class Drivetrain {
+  public static final double kMaxSpeed = 3.0; // 3 meters per second
+  public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second
+
+  private final Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381);
+  private final Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381);
+  private final Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381);
+  private final Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381);
+
+  private final SwerveModule m_frontLeft = new SwerveModule(1, 2);
+  private final SwerveModule m_frontRight = new SwerveModule(3, 4);
+  private final SwerveModule m_backLeft = new SwerveModule(5, 6);
+  private final SwerveModule m_backRight = new SwerveModule(7, 8);
+
+  private final AnalogGyro m_gyro = new AnalogGyro(0);
+
+  private final SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(
+      m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation
+  );
+
+  private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics);
+
+  public Drivetrain() {
+    m_gyro.reset();
+  }
+
+  /**
+   * Returns the angle of the robot as a Rotation2d.
+   *
+   * @return The angle of the robot.
+   */
+  public Rotation2d getAngle() {
+    // Negating the angle because WPILib gyros are CW positive.
+    return Rotation2d.fromDegrees(-m_gyro.getAngle());
+  }
+
+  /**
+   * Method to drive the robot using joystick info.
+   *
+   * @param xSpeed        Speed of the robot in the x direction (forward).
+   * @param ySpeed        Speed of the robot in the y direction (sideways).
+   * @param rot           Angular rate of the robot.
+   * @param fieldRelative Whether the provided x and y speeds are relative to the field.
+   */
+  @SuppressWarnings("ParameterName")
+  public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
+    var swerveModuleStates = m_kinematics.toSwerveModuleStates(
+        fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(
+            xSpeed, ySpeed, rot, getAngle())
+            : new ChassisSpeeds(xSpeed, ySpeed, rot)
+    );
+    SwerveDriveKinematics.normalizeWheelSpeeds(swerveModuleStates, kMaxSpeed);
+    m_frontLeft.setDesiredState(swerveModuleStates[0]);
+    m_frontRight.setDesiredState(swerveModuleStates[1]);
+    m_backLeft.setDesiredState(swerveModuleStates[2]);
+    m_backRight.setDesiredState(swerveModuleStates[3]);
+  }
+
+  /**
+   * Updates the field relative position of the robot.
+   */
+  public void updateOdometry() {
+    m_odometry.update(
+        getAngle(),
+        m_frontLeft.getState(),
+        m_frontRight.getState(),
+        m_backLeft.getState(),
+        m_backRight.getState()
+    );
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Main.java

@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.swervebot;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all.
+ * Unless you know what you are doing, do not modify this file except to
+ * change the parameter class to the startRobot call.
+ */
+public final class Main {
+  private Main() {
+  }
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java

@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.swervebot;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.XboxController;
+
+import static edu.wpi.first.wpilibj.examples.swervebot.Drivetrain.kMaxAngularSpeed;
+import static edu.wpi.first.wpilibj.examples.swervebot.Drivetrain.kMaxSpeed;
+
+public class Robot extends TimedRobot {
+  private final XboxController m_controller = new XboxController(0);
+  private final Drivetrain m_swerve = new Drivetrain();
+
+  @Override
+  public void autonomousPeriodic() {
+    driveWithJoystick(false);
+    m_swerve.updateOdometry();
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    driveWithJoystick(true);
+  }
+
+  private void driveWithJoystick(boolean fieldRelative) {
+    // Get the x speed. We are inverting this because Xbox controllers return
+    // negative values when we push forward.
+    final var xSpeed = -m_controller.getY(GenericHID.Hand.kLeft) * kMaxSpeed;
+
+    // Get the y speed or sideways/strafe speed. We are inverting this because
+    // we want a positive value when we pull to the left. Xbox controllers
+    // return positive values when you pull to the right by default.
+    final var ySpeed = -m_controller.getX(GenericHID.Hand.kLeft) * kMaxSpeed;
+
+    // Get the rate of angular rotation. We are inverting this because we want a
+    // positive value when we pull to the left (remember, CCW is positive in
+    // mathematics). Xbox controllers return positive values when you pull to
+    // the right by default.
+    final var rot = -m_controller.getX(GenericHID.Hand.kRight) * kMaxAngularSpeed;
+
+    m_swerve.drive(xSpeed, ySpeed, rot, fieldRelative);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java

@@ -0,0 +1,91 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.swervebot;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+public class SwerveModule {
+  private static final double kWheelRadius = 0.0508;
+  private static final int kEncoderResolution = 4096;
+
+  private static final double kModuleMaxAngularVelocity = Drivetrain.kMaxAngularSpeed;
+  private static final double kModuleMaxAngularAcceleration
+      = 2 * Math.PI; // radians per second squared
+
+  private final Spark m_driveMotor;
+  private final Spark m_turningMotor;
+
+  private final Encoder m_driveEncoder = new Encoder(0, 1);
+  private final Encoder m_turningEncoder = new Encoder(0, 1);
+
+  private final PIDController m_drivePIDController = new PIDController(1, 0, 0);
+
+  private final ProfiledPIDController m_turningPIDController
+      = new ProfiledPIDController(1, 0, 0,
+      new TrapezoidProfile.Constraints(kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration));
+
+  /**
+   * Constructs a SwerveModule.
+   *
+   * @param driveMotorChannel   ID for the drive motor.
+   * @param turningMotorChannel ID for the turning motor.
+   */
+  public SwerveModule(int driveMotorChannel, int turningMotorChannel) {
+    m_driveMotor = new Spark(driveMotorChannel);
+    m_turningMotor = new Spark(turningMotorChannel);
+
+    // Set the distance per pulse for the drive encoder. We can simply use the
+    // distance traveled for one rotation of the wheel divided by the encoder
+    // resolution.
+    m_driveEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
+
+    // Set the distance (in this case, angle) per pulse for the turning encoder.
+    // This is the the angle through an entire rotation (2 * wpi::math::pi)
+    // divided by the encoder resolution.
+    m_turningEncoder.setDistancePerPulse(2 * Math.PI / kEncoderResolution);
+
+    // Limit the PID Controller's input range between -pi and pi and set the input
+    // to be continuous.
+    m_turningPIDController.enableContinuousInput(-Math.PI, Math.PI);
+  }
+
+  /**
+   * Returns the current state of the module.
+   *
+   * @return The current state of the module.
+   */
+  public SwerveModuleState getState() {
+    return new SwerveModuleState(m_driveEncoder.getRate(), new Rotation2d(m_turningEncoder.get()));
+  }
+
+  /**
+   * Sets the desired state for the module.
+   *
+   * @param state Desired state with speed and angle.
+   */
+  public void setDesiredState(SwerveModuleState state) {
+    // Calculate the drive output from the drive PID controller.
+    final var driveOutput = m_drivePIDController.calculate(
+        m_driveEncoder.getRate(), state.speedMetersPerSecond);
+
+    // Calculate the turning motor output from the turning PID controller.
+    final var turnOutput = m_turningPIDController.calculate(
+        m_turningEncoder.get(), state.angle.getRadians()
+    );
+
+    // Calculate the turning motor output from the turning PID controller.
+    m_driveMotor.set(driveOutput);
+    m_turningMotor.set(turnOutput);
+  }
+}