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/cpp/Drive/DifferentialDrive.cpp

@@ -0,0 +1,213 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Drive/DifferentialDrive.h"
+
+#include <cmath>
+
+#include <HAL/HAL.h>
+
+#include "SpeedController.h"
+
+using namespace frc;
+
+/**
+ * Construct a DifferentialDrive.
+ *
+ * To pass multiple motors per side, use a SpeedControllerGroup. If a motor
+ * needs to be inverted, do so before passing it in.
+ */
+DifferentialDrive::DifferentialDrive(SpeedController& leftMotor,
+                                     SpeedController& rightMotor)
+    : m_leftMotor(leftMotor), m_rightMotor(rightMotor) {}
+
+/**
+ * Arcade drive method for differential drive platform.
+ *
+ * Note: Some drivers may prefer inverted rotation controls. This can be done by
+ * negating the value passed for rotation.
+ *
+ * @param y             The value to use for forwards/backwards. [-1.0..1.0]
+ * @param rotation      The value to use for the rotation right/left.
+ *                      [-1.0..1.0]
+ * @param squaredInputs If set, decreases the input sensitivity at low speeds.
+ */
+void DifferentialDrive::ArcadeDrive(double y, double rotation,
+                                    bool squaredInputs) {
+  static bool reported = false;
+  if (!reported) {
+    HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 2,
+               HALUsageReporting::kRobotDrive_ArcadeStandard);
+    reported = true;
+  }
+
+  y = Limit(y);
+  y = ApplyDeadband(y, m_deadband);
+
+  rotation = Limit(rotation);
+  rotation = ApplyDeadband(rotation, m_deadband);
+
+  // square the inputs (while preserving the sign) to increase fine control
+  // while permitting full power
+  if (squaredInputs) {
+    y = std::copysign(y * y, y);
+    rotation = std::copysign(rotation * rotation, rotation);
+  }
+
+  double leftMotorOutput;
+  double rightMotorOutput;
+
+  double maxInput = std::copysign(std::max(std::abs(y), std::abs(rotation)), y);
+
+  if (y > 0.0) {
+    // First quadrant, else second quadrant
+    if (rotation > 0.0) {
+      leftMotorOutput = maxInput;
+      rightMotorOutput = y - rotation;
+    } else {
+      leftMotorOutput = y + rotation;
+      rightMotorOutput = maxInput;
+    }
+  } else {
+    // Third quadrant, else fourth quadrant
+    if (rotation > 0.0) {
+      leftMotorOutput = y + rotation;
+      rightMotorOutput = maxInput;
+    } else {
+      leftMotorOutput = maxInput;
+      rightMotorOutput = y - rotation;
+    }
+  }
+
+  m_leftMotor.Set(Limit(leftMotorOutput) * m_maxOutput);
+  m_rightMotor.Set(-Limit(rightMotorOutput) * m_maxOutput);
+
+  m_safetyHelper.Feed();
+}
+
+/**
+ * Curvature drive method for differential drive platform.
+ *
+ * The rotation argument controls the curvature of the robot's path rather than
+ * its rate of heading change. This makes the robot more controllable at high
+ * speeds. Also handles the robot's quick turn functionality - "quick turn"
+ * overrides constant-curvature turning for turn-in-place maneuvers.
+ *
+ * @param y           The value to use for forwards/backwards. [-1.0..1.0]
+ * @param rotation    The value to use for the rotation right/left. [-1.0..1.0]
+ * @param isQuickTurn If set, overrides constant-curvature turning for
+ *                    turn-in-place maneuvers.
+ */
+void DifferentialDrive::CurvatureDrive(double y, double rotation,
+                                       bool isQuickTurn) {
+  static bool reported = false;
+  if (!reported) {
+    // HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 2,
+    //            HALUsageReporting::kRobotDrive_Curvature);
+    reported = true;
+  }
+
+  y = Limit(y);
+  y = ApplyDeadband(y, m_deadband);
+
+  rotation = Limit(rotation);
+  rotation = ApplyDeadband(rotation, m_deadband);
+
+  double angularPower;
+  bool overPower;
+
+  if (isQuickTurn) {
+    if (std::abs(y) < 0.2) {
+      constexpr double alpha = 0.1;
+      m_quickStopAccumulator =
+          (1 - alpha) * m_quickStopAccumulator + alpha * Limit(rotation) * 2;
+    }
+    overPower = true;
+    angularPower = rotation;
+  } else {
+    overPower = false;
+    angularPower = std::abs(y) * rotation - m_quickStopAccumulator;
+
+    if (m_quickStopAccumulator > 1) {
+      m_quickStopAccumulator -= 1;
+    } else if (m_quickStopAccumulator < -1) {
+      m_quickStopAccumulator += 1;
+    } else {
+      m_quickStopAccumulator = 0.0;
+    }
+  }
+
+  double leftMotorOutput = y + angularPower;
+  double rightMotorOutput = y - angularPower;
+
+  // If rotation is overpowered, reduce both outputs to within acceptable range
+  if (overPower) {
+    if (leftMotorOutput > 1.0) {
+      rightMotorOutput -= leftMotorOutput - 1.0;
+      leftMotorOutput = 1.0;
+    } else if (rightMotorOutput > 1.0) {
+      leftMotorOutput -= rightMotorOutput - 1.0;
+      rightMotorOutput = 1.0;
+    } else if (leftMotorOutput < -1.0) {
+      rightMotorOutput -= leftMotorOutput + 1.0;
+      leftMotorOutput = -1.0;
+    } else if (rightMotorOutput < -1.0) {
+      leftMotorOutput -= rightMotorOutput + 1.0;
+      rightMotorOutput = -1.0;
+    }
+  }
+
+  m_leftMotor.Set(leftMotorOutput * m_maxOutput);
+  m_rightMotor.Set(-rightMotorOutput * m_maxOutput);
+
+  m_safetyHelper.Feed();
+}
+
+/**
+ * Tank drive method for differential drive platform.
+ *
+ * @param left  The value to use for left side motors. [-1.0..1.0]
+ * @param right The value to use for right side motors. [-1.0..1.0]
+ * @param squaredInputs If set, decreases the input sensitivity at low speeds.
+ */
+void DifferentialDrive::TankDrive(double left, double right,
+                                  bool squaredInputs) {
+  static bool reported = false;
+  if (!reported) {
+    HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 2,
+               HALUsageReporting::kRobotDrive_Tank);
+    reported = true;
+  }
+
+  left = Limit(left);
+  left = ApplyDeadband(left, m_deadband);
+
+  right = Limit(right);
+  right = ApplyDeadband(right, m_deadband);
+
+  // square the inputs (while preserving the sign) to increase fine control
+  // while permitting full power
+  if (squaredInputs) {
+    left = std::copysign(left * left, left);
+    right = std::copysign(right * right, right);
+  }
+
+  m_leftMotor.Set(left * m_maxOutput);
+  m_rightMotor.Set(-right * m_maxOutput);
+
+  m_safetyHelper.Feed();
+}
+
+void DifferentialDrive::StopMotor() {
+  m_leftMotor.StopMotor();
+  m_rightMotor.StopMotor();
+  m_safetyHelper.Feed();
+}
+
+void DifferentialDrive::GetDescription(llvm::raw_ostream& desc) const {
+  desc << "DifferentialDrive";
+}

wpilibc/src/main/native/cpp/Drive/KilloughDrive.cpp

@@ -0,0 +1,147 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Drive/KilloughDrive.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <HAL/HAL.h>
+
+#include "SpeedController.h"
+
+using namespace frc;
+
+constexpr double kPi = 3.14159265358979323846;
+
+/**
+ * Construct a Killough drive with the given motors and default motor angles.
+ *
+ * The default motor angles are 120, 60, and 270 degrees for the left, right,
+ * and back motors respectively, which make the wheels on each corner parallel
+ * to their respective opposite sides.
+ *
+ * If a motor needs to be inverted, do so before passing it in.
+ *
+ * @param leftMotor  The motor on the left corner.
+ * @param rightMotor The motor on the right corner.
+ * @param backMotor  The motor on the back corner.
+ */
+KilloughDrive::KilloughDrive(SpeedController& leftMotor,
+                             SpeedController& rightMotor,
+                             SpeedController& backMotor)
+    : KilloughDrive(leftMotor, rightMotor, backMotor, 120.0, 60.0, 270.0) {}
+
+/**
+ * Construct a Killough drive with the given motors.
+ *
+ * Angles are measured in counter-clockwise degrees where zero degrees is
+ * straight ahead.
+ *
+ * @param leftMotor       The motor on the left corner.
+ * @param rightMotor      The motor on the right corner.
+ * @param backMotor       The motor on the back corner.
+ * @param leftMotorAngle  The angle of the left wheel's forward direction of
+ *                        travel.
+ * @param rightMotorAngle The angle of the right wheel's forward direction of
+ *                        travel.
+ * @param backMotorAngle  The angle of the back wheel's forward direction of
+ *                        travel.
+ */
+KilloughDrive::KilloughDrive(SpeedController& leftMotor,
+                             SpeedController& rightMotor,
+                             SpeedController& backMotor, double leftMotorAngle,
+                             double rightMotorAngle, double backMotorAngle)
+    : m_leftMotor(leftMotor), m_rightMotor(rightMotor), m_backMotor(backMotor) {
+  m_leftVec = {std::cos(leftMotorAngle * (kPi / 180.0)),
+               std::sin(leftMotorAngle * (kPi / 180.0))};
+  m_rightVec = {std::cos(rightMotorAngle * (kPi / 180.0)),
+                std::sin(rightMotorAngle * (kPi / 180.0))};
+  m_backVec = {std::cos(backMotorAngle * (kPi / 180.0)),
+               std::sin(backMotorAngle * (kPi / 180.0))};
+}
+
+/**
+ * Drive method for Killough platform.
+ *
+ * @param x         The speed that the robot should drive in the X direction.
+ *                  [-1.0..1.0]
+ * @param y         The speed that the robot should drive in the Y direction.
+ *                  [-1.0..1.0]
+ * @param rotation  The rate of rotation for the robot that is completely
+ *                  independent of the translation. [-1.0..1.0]
+ * @param gyroAngle The current angle reading from the gyro.  Use this to
+ *                  implement field-oriented controls.
+ */
+void KilloughDrive::DriveCartesian(double x, double y, double rotation,
+                                   double gyroAngle) {
+  if (!reported) {
+    // HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 3,
+    //            HALUsageReporting::kRobotDrive_KilloughCartesian);
+    reported = true;
+  }
+
+  x = Limit(x);
+  x = ApplyDeadband(x, m_deadband);
+
+  y = Limit(y);
+  y = ApplyDeadband(y, m_deadband);
+
+  // Compensate for gyro angle.
+  Vector2d input{x, y};
+  input.Rotate(gyroAngle);
+
+  double wheelSpeeds[3];
+  wheelSpeeds[kLeft] = input.ScalarProject(m_leftVec) + rotation;
+  wheelSpeeds[kRight] = input.ScalarProject(m_rightVec) + rotation;
+  wheelSpeeds[kBack] = input.ScalarProject(m_backVec) + rotation;
+
+  Normalize(wheelSpeeds);
+
+  m_leftMotor.Set(wheelSpeeds[kLeft] * m_maxOutput);
+  m_rightMotor.Set(wheelSpeeds[kRight] * m_maxOutput);
+  m_backMotor.Set(wheelSpeeds[kBack] * m_maxOutput);
+
+  m_safetyHelper.Feed();
+}
+
+/**
+ * Drive method for Killough platform.
+ *
+ * @param magnitude The speed that the robot should drive in a given direction.
+ *                  [-1.0..1.0]
+ * @param angle     The direction the robot should drive in degrees. 0.0 is
+ *                  straight ahead. The direction and maginitude are independent
+ *                  of the rotation rate.
+ * @param rotation  The rate of rotation for the robot that is completely
+ *                  independent of the magnitude or direction. [-1.0..1.0]
+ */
+void KilloughDrive::DrivePolar(double magnitude, double angle,
+                               double rotation) {
+  if (!reported) {
+    // HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 3,
+    //            HALUsageReporting::kRobotDrive_KilloughPolar);
+    reported = true;
+  }
+
+  // Normalized for full power along the Cartesian axes.
+  magnitude = Limit(magnitude) * std::sqrt(2.0);
+
+  DriveCartesian(magnitude * std::cos(angle * (kPi / 180.0)),
+                 magnitude * std::sin(angle * (kPi / 180.0)), rotation, 0.0);
+}
+
+void KilloughDrive::StopMotor() {
+  m_leftMotor.StopMotor();
+  m_rightMotor.StopMotor();
+  m_backMotor.StopMotor();
+  m_safetyHelper.Feed();
+}
+
+void KilloughDrive::GetDescription(llvm::raw_ostream& desc) const {
+  desc << "KilloughDrive";
+}

wpilibc/src/main/native/cpp/Drive/MecanumDrive.cpp

@@ -0,0 +1,117 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Drive/MecanumDrive.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <HAL/HAL.h>
+
+#include "Drive/Vector2d.h"
+#include "SpeedController.h"
+
+using namespace frc;
+
+constexpr double kPi = 3.14159265358979323846;
+
+/**
+ * Construct a MecanumDrive.
+ *
+ * If a motor needs to be inverted, do so before passing it in.
+ */
+MecanumDrive::MecanumDrive(SpeedController& frontLeftMotor,
+                           SpeedController& rearLeftMotor,
+                           SpeedController& frontRightMotor,
+                           SpeedController& rearRightMotor)
+    : m_frontLeftMotor(frontLeftMotor),
+      m_rearLeftMotor(rearLeftMotor),
+      m_frontRightMotor(frontRightMotor),
+      m_rearRightMotor(rearRightMotor) {}
+
+/**
+ * Drive method for Mecanum platform.
+ *
+ * @param x         The speed that the robot should drive in the X direction.
+ *                  [-1.0..1.0]
+ * @param y         The speed that the robot should drive in the Y direction.
+ *                  [-1.0..1.0]
+ * @param rotation  The rate of rotation for the robot that is completely
+ *                  independent of the translation. [-1.0..1.0]
+ * @param gyroAngle The current angle reading from the gyro. Use this to
+ *                  implement field-oriented controls.
+ */
+void MecanumDrive::DriveCartesian(double x, double y, double rotation,
+                                  double gyroAngle) {
+  if (!reported) {
+    HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 4,
+               HALUsageReporting::kRobotDrive_MecanumCartesian);
+    reported = true;
+  }
+
+  x = Limit(x);
+  x = ApplyDeadband(x, m_deadband);
+
+  y = Limit(y);
+  y = ApplyDeadband(y, m_deadband);
+
+  // Compensate for gyro angle.
+  Vector2d input{x, y};
+  input.Rotate(gyroAngle);
+
+  double wheelSpeeds[4];
+  wheelSpeeds[kFrontLeft] = input.x + input.y + rotation;
+  wheelSpeeds[kFrontRight] = input.x - input.y + rotation;
+  wheelSpeeds[kRearLeft] = -input.x + input.y + rotation;
+  wheelSpeeds[kRearRight] = -input.x - input.y + rotation;
+
+  Normalize(wheelSpeeds);
+
+  m_frontLeftMotor.Set(wheelSpeeds[kFrontLeft] * m_maxOutput);
+  m_frontRightMotor.Set(wheelSpeeds[kFrontRight] * m_maxOutput);
+  m_rearLeftMotor.Set(wheelSpeeds[kRearLeft] * m_maxOutput);
+  m_rearRightMotor.Set(wheelSpeeds[kRearRight] * m_maxOutput);
+
+  m_safetyHelper.Feed();
+}
+
+/**
+ * Drive method for Mecanum platform.
+ *
+ * @param magnitude The speed that the robot should drive in a given direction.
+ *                  [-1.0..1.0]
+ * @param angle     The direction the robot should drive in degrees. 0.0 is
+ *                  straight ahead. The direction and maginitude are independent
+ *                  of the rotation rate.
+ * @param rotation  The rate of rotation for the robot that is completely
+ *                  independent of the magnitude or direction. [-1.0..1.0]
+ */
+void MecanumDrive::DrivePolar(double magnitude, double angle, double rotation) {
+  if (!reported) {
+    HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 4,
+               HALUsageReporting::kRobotDrive_MecanumPolar);
+    reported = true;
+  }
+
+  // Normalized for full power along the Cartesian axes.
+  magnitude = Limit(magnitude) * std::sqrt(2.0);
+
+  DriveCartesian(magnitude * std::cos(angle * (kPi / 180.0)),
+                 magnitude * std::sin(angle * (kPi / 180.0)), rotation, 0.0);
+}
+
+void MecanumDrive::StopMotor() {
+  m_frontLeftMotor.StopMotor();
+  m_frontRightMotor.StopMotor();
+  m_rearLeftMotor.StopMotor();
+  m_rearRightMotor.StopMotor();
+  m_safetyHelper.Feed();
+}
+
+void MecanumDrive::GetDescription(llvm::raw_ostream& desc) const {
+  desc << "MecanumDrive";
+}

wpilibc/src/main/native/cpp/Drive/RobotDriveBase.cpp

@@ -0,0 +1,100 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Drive/RobotDriveBase.h"
+
+#include <algorithm>
+#include <cmath>
+#include <cstddef>
+
+#include <HAL/HAL.h>
+
+#include "Base.h"
+#include "SpeedController.h"
+
+using namespace frc;
+
+RobotDriveBase::RobotDriveBase() { m_safetyHelper.SetSafetyEnabled(true); }
+
+void RobotDriveBase::SetDeadband(double deadband) { m_deadband = deadband; }
+
+/**
+ * Configure the scaling factor for using RobotDrive with motor controllers in a
+ * mode other than PercentVbus.
+ *
+ * @param maxOutput Multiplied with the output percentage computed by the drive
+ *                  functions.
+ */
+void RobotDriveBase::SetMaxOutput(double maxOutput) { m_maxOutput = maxOutput; }
+
+void RobotDriveBase::SetExpiration(double timeout) {
+  m_safetyHelper.SetExpiration(timeout);
+}
+
+double RobotDriveBase::GetExpiration() const {
+  return m_safetyHelper.GetExpiration();
+}
+
+bool RobotDriveBase::IsAlive() const { return m_safetyHelper.IsAlive(); }
+
+bool RobotDriveBase::IsSafetyEnabled() const {
+  return m_safetyHelper.IsSafetyEnabled();
+}
+
+void RobotDriveBase::SetSafetyEnabled(bool enabled) {
+  m_safetyHelper.SetSafetyEnabled(enabled);
+}
+
+/**
+ * Limit motor values to the -1.0 to +1.0 range.
+ */
+double RobotDriveBase::Limit(double value) {
+  if (value > 1.0) {
+    return 1.0;
+  }
+  if (value < -1.0) {
+    return -1.0;
+  }
+  return value;
+}
+
+/**
+ * Returns 0.0 if the given value is within the specified range around zero. The
+ * remaining range between the deadband and 1.0 is scaled from 0.0 to 1.0.
+ *
+ * @param value    value to clip
+ * @param deadband range around zero
+ */
+double RobotDriveBase::ApplyDeadband(double value, double deadband) {
+  if (std::abs(value) > deadband) {
+    if (value > 0.0) {
+      return (value - deadband) / (1.0 - deadband);
+    } else {
+      return (value + deadband) / (1.0 - deadband);
+    }
+  } else {
+    return 0.0;
+  }
+}
+
+/**
+ * Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0.
+ */
+void RobotDriveBase::Normalize(llvm::MutableArrayRef<double> wheelSpeeds) {
+  double maxMagnitude = std::abs(wheelSpeeds[0]);
+  for (size_t i = 1; i < wheelSpeeds.size(); i++) {
+    double temp = std::abs(wheelSpeeds[i]);
+    if (maxMagnitude < temp) {
+      maxMagnitude = temp;
+    }
+  }
+  if (maxMagnitude > 1.0) {
+    for (size_t i = 0; i < wheelSpeeds.size(); i++) {
+      wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
+    }
+  }
+}

wpilibc/src/main/native/cpp/Drive/Vector2d.cpp

@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Drive/Vector2d.h"
+
+#include <cmath>
+
+using namespace frc;
+
+constexpr double kPi = 3.14159265358979323846;
+
+Vector2d::Vector2d(double x, double y) {
+  this->x = x;
+  this->y = y;
+}
+
+/**
+ * Rotate a vector in Cartesian space.
+ *
+ * @param angle angle by which to rotate vector counter-clockwise.
+ */
+void Vector2d::Rotate(double angle) {
+  double cosA = std::cos(angle * (kPi / 180.0));
+  double sinA = std::sin(angle * (kPi / 180.0));
+  double out[2];
+  out[0] = x * cosA - y * sinA;
+  out[1] = x * sinA + y * cosA;
+  x = out[0];
+  y = out[1];
+}
+
+/**
+ * Returns dot product of this vector with argument.
+ *
+ * @param vec Vector with which to perform dot product.
+ */
+double Vector2d::Dot(const Vector2d& vec) const {
+  return x * vec.x + y * vec.y;
+}
+
+/**
+ * Returns magnitude of vector.
+ */
+double Vector2d::Magnitude() const { return std::sqrt(x * x + y * y); }
+
+/**
+ * Returns scalar projection of this vector onto argument.
+ *
+ * @param vec Vector onto which to project this vector.
+ */
+double Vector2d::ScalarProject(const Vector2d& vec) const {
+  return Dot(vec) / vec.Magnitude();
+}