Split RobotDrive class into a class for each drive type (#552)

DiffDrive.CurvatureDrive (aka CheesyDrive) and KilloughDrive were also added.
This reorganization paves the way for SwerveDrive.

wpilibc/src/main/native/include/Drive/DifferentialDrive.h

@@ -0,0 +1,99 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2017 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <llvm/raw_ostream.h>
+
+#include "Drive/RobotDriveBase.h"
+
+namespace frc {
+
+class SpeedController;
+
+/**
+ * A class for driving differential drive/skid-steer drive platforms such as
+ * the Kit of Parts drive base, "tank drive", or West Coast Drive.
+ *
+ * These drive bases typically have drop-center / skid-steer with two or more
+ * wheels per side (e.g., 6WD or 8WD). This class takes a SpeedController per
+ * side. For four and six motor drivetrains, construct and pass in
+ * SpeedControllerGroup instances as follows.
+ *
+ * Four motor drivetrain:
+ * @code{.cpp}
+ * class Robot {
+ *  public:
+ *   frc::Talon m_frontLeft{1};
+ *   frc::Talon m_rearLeft{2};
+ *   frc::SpeedControllerGroup m_left{m_frontLeft, m_rearLeft};
+ *
+ *   frc::Talon m_frontRight{3};
+ *   frc::Talon m_rearRight{4};
+ *   frc::SpeedControllerGroup m_right{m_frontRight, m_rearRight};
+ *
+ *   frc::DifferentialDrive m_drive{m_left, m_right};
+ * };
+ * @endcode
+ *
+ * Six motor drivetrain:
+ * @code{.cpp}
+ * class Robot {
+ *  public:
+ *   frc::Talon m_frontLeft{1};
+ *   frc::Talon m_midLeft{2};
+ *   frc::Talon m_rearLeft{3};
+ *   frc::SpeedControllerGroup m_left{m_frontLeft, m_midLeft, m_rearLeft};
+ *
+ *   frc::Talon m_frontRight{4};
+ *   frc::Talon m_midRight{5};
+ *   frc::Talon m_rearRight{6};
+ *   frc::SpeedControllerGroup m_right{m_frontRight, m_midRight, m_rearRight};
+ *
+ *   frc::DifferentialDrive m_drive{m_left, m_right};
+ * };
+ * @endcode
+ *
+ * A differential drive robot has left and right wheels separated by an
+ * arbitrary width.
+ *
+ * Drive base diagram:
+ * <pre>
+ * |_______|
+ * | |   | |
+ *   |   |
+ * |_|___|_|
+ * |       |
+ * </pre>
+ *
+ * Each Drive() function provides different inverse kinematic relations for a
+ * differential drive robot. Motor outputs for the right side are negated, so
+ * motor direction inversion by the user is usually unnecessary.
+ */
+class DifferentialDrive : public RobotDriveBase {
+ public:
+  DifferentialDrive(SpeedController& leftMotor, SpeedController& rightMotor);
+  virtual ~DifferentialDrive() = default;
+
+  DifferentialDrive(const DifferentialDrive&) = delete;
+  DifferentialDrive& operator=(const DifferentialDrive&) = delete;
+
+  void ArcadeDrive(double y, double rotation, bool squaredInputs = true);
+  void CurvatureDrive(double y, double rotation, bool isQuickTurn);
+  void TankDrive(double left, double right, bool squaredInputs = true);
+
+  void StopMotor() override;
+  void GetDescription(llvm::raw_ostream& desc) const override;
+
+ private:
+  SpeedController& m_leftMotor;
+  SpeedController& m_rightMotor;
+
+  double m_quickStopAccumulator = 0.0;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/Drive/KilloughDrive.h

@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <llvm/raw_ostream.h>
+
+#include "Drive/RobotDriveBase.h"
+#include "Drive/Vector2d.h"
+
+namespace frc {
+
+class SpeedController;
+
+/**
+ * A class for driving Killough drive platforms.
+ *
+ * Killough drives are triangular with one omni wheel on each corner.
+ *
+ * Drive base diagram:
+ * <pre>
+ *  /_____\
+ * / \   / \
+ *    \ /
+ *    ---
+ * </pre>
+ *
+ * Each Drive() function provides different inverse kinematic relations for a
+ * Killough drive. The default wheel vectors are parallel to their respective
+ * opposite sides, but can be overridden. See the constructor for more
+ * information.
+ */
+class KilloughDrive : public RobotDriveBase {
+ public:
+  KilloughDrive(SpeedController& leftMotor, SpeedController& rightMotor,
+                SpeedController& backMotor);
+  KilloughDrive(SpeedController& leftMotor, SpeedController& rightMotor,
+                SpeedController& backMotor, double leftMotorAngle,
+                double rightMotorAngle, double backMotorAngle);
+  virtual ~KilloughDrive() = default;
+
+  KilloughDrive(const KilloughDrive&) = delete;
+  KilloughDrive& operator=(const KilloughDrive&) = delete;
+
+  void DriveCartesian(double x, double y, double rotation,
+                      double gyroAngle = 0.0);
+  void DrivePolar(double magnitude, double angle, double rotation);
+
+  void StopMotor() override;
+  void GetDescription(llvm::raw_ostream& desc) const override;
+
+ private:
+  SpeedController& m_leftMotor;
+  SpeedController& m_rightMotor;
+  SpeedController& m_backMotor;
+
+  Vector2d m_leftVec;
+  Vector2d m_rightVec;
+  Vector2d m_backVec;
+
+  bool reported = false;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/Drive/MecanumDrive.h

@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2017 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <llvm/raw_ostream.h>
+
+#include "Drive/RobotDriveBase.h"
+
+namespace frc {
+
+class SpeedController;
+
+/**
+ * A class for driving Mecanum drive platforms.
+ *
+ * Mecanum drives are rectangular with one wheel on each corner. Each wheel has
+ * rollers toed in 45 degrees toward the front or back. When looking at the
+ * wheels from the top, the roller axles should form an X across the robot.
+ *
+ * Drive base diagram:
+ * <pre>
+ * \\_______/
+ * \\ |   | /
+ *   |   |
+ * /_|___|_\\
+ * /       \\
+ * </pre>
+ *
+ * Each Drive() function provides different inverse kinematic relations for a
+ * Mecanum drive robot. Motor outputs for the right side are negated, so motor
+ * direction inversion by the user is usually unnecessary.
+ */
+class MecanumDrive : public RobotDriveBase {
+ public:
+  MecanumDrive(SpeedController& frontLeftMotor, SpeedController& rearLeftMotor,
+               SpeedController& frontRightMotor,
+               SpeedController& rearRightMotor);
+  virtual ~MecanumDrive() = default;
+
+  MecanumDrive(const MecanumDrive&) = delete;
+  MecanumDrive& operator=(const MecanumDrive&) = delete;
+
+  void DriveCartesian(double x, double y, double rotation,
+                      double gyroAngle = 0.0);
+  void DrivePolar(double magnitude, double angle, double rotation);
+
+  void StopMotor() override;
+  void GetDescription(llvm::raw_ostream& desc) const override;
+
+ private:
+  SpeedController& m_frontLeftMotor;
+  SpeedController& m_rearLeftMotor;
+  SpeedController& m_frontRightMotor;
+  SpeedController& m_rearRightMotor;
+
+  bool reported = false;
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/Drive/RobotDriveBase.h

@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2017 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <llvm/ArrayRef.h>
+#include <llvm/raw_ostream.h>
+
+#include "ErrorBase.h"
+#include "MotorSafety.h"
+#include "MotorSafetyHelper.h"
+
+namespace frc {
+
+class SpeedController;
+
+/**
+ * Common base class for drive platforms.
+ */
+class RobotDriveBase : public MotorSafety, public ErrorBase {
+ public:
+  /**
+   * The location of a motor on the robot for the purpose of driving.
+   */
+  enum MotorType {
+    kFrontLeft = 0,
+    kFrontRight = 1,
+    kRearLeft = 2,
+    kRearRight = 3,
+    kLeft = 0,
+    kRight = 1,
+    kBack = 2
+  };
+
+  RobotDriveBase();
+  virtual ~RobotDriveBase() = default;
+
+  RobotDriveBase(const RobotDriveBase&) = delete;
+  RobotDriveBase& operator=(const RobotDriveBase&) = delete;
+
+  void SetDeadband(double deadband);
+  void SetMaxOutput(double maxOutput);
+
+  void SetExpiration(double timeout) override;
+  double GetExpiration() const override;
+  bool IsAlive() const override;
+  void StopMotor() override = 0;
+  bool IsSafetyEnabled() const override;
+  void SetSafetyEnabled(bool enabled) override;
+  void GetDescription(llvm::raw_ostream& desc) const override = 0;
+
+ protected:
+  double Limit(double number);
+  double ApplyDeadband(double number, double deadband);
+  void Normalize(llvm::MutableArrayRef<double> wheelSpeeds);
+
+  double m_deadband = 0.02;
+  double m_maxOutput = 1.0;
+  MotorSafetyHelper m_safetyHelper{this};
+};
+
+}  // namespace frc

wpilibc/src/main/native/include/Drive/Vector2d.h

@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+namespace frc {
+
+/**
+ * This is a 2D vector struct that supports basic vector operations.
+ */
+struct Vector2d {
+  Vector2d() = default;
+  Vector2d(double x, double y);
+
+  void Rotate(double angle);
+  double Dot(const Vector2d& vec) const;
+  double Magnitude() const;
+  double ScalarProject(const Vector2d& vec) const;
+
+  double x = 0.0;
+  double y = 0.0;
+};
+
+}  // namespace frc