Add replacement drive classes

These return the inverse kinematic values, so they can be composed with
feedback controllers. The internal implementation for DifferentialDrive
was massively simplified as well.

Input limits were removed since the normalization takes care of that.
The deadband functionality was moved to GenericHID::GetRawAxis() since
the deadband is generally applied to joystick axes.

MotorSafety was removed since that's handled by the drivetrain's motor
controllers already.

wpilibc/src/main/native/cpp/GenericHID.cpp

@@ -1,12 +1,14 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-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.                                                               */
 /*----------------------------------------------------------------------------*/
 
 #include "frc/GenericHID.h"
 
+#include <cmath>
+
 #include <hal/HAL.h>
 
 #include "frc/DriverStation.h"
@@ -34,7 +36,7 @@ bool GenericHID::GetRawButtonReleased(int button) {
 }
 
 double GenericHID::GetRawAxis(int axis) const {
-  return m_ds.GetStickAxis(m_port, axis);
+  return ApplyDeadband(m_ds.GetStickAxis(m_port, axis), m_deadband);
 }
 
 int GenericHID::GetPOV(int pov) const { return m_ds.GetStickPOV(m_port, pov); }
@@ -83,3 +85,17 @@ void GenericHID::SetRumble(RumbleType type, double value) {
   }
   HAL_SetJoystickOutputs(m_port, m_outputs, m_leftRumble, m_rightRumble);
 }
+
+void GenericHID::SetAxisDeadband(double deadband) { m_deadband = deadband; }
+
+double GenericHID::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;
+  }
+}

wpilibc/src/main/native/cpp/experimental/drive/DifferentialDrive.cpp

@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/experimental/drive/DifferentialDrive.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <hal/HAL.h>
+
+using namespace frc::experimental;
+
+DifferentialDrive::WheelSpeeds DifferentialDrive::ArcadeDrive(
+    double xSpeed, double zRotation) {
+  static bool reported = false;
+  if (!reported) {
+    HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 2,
+               HALUsageReporting::kRobotDrive2_DifferentialArcade);
+    reported = true;
+  }
+
+  WheelSpeeds speeds{xSpeed + zRotation, xSpeed - zRotation};
+
+  // Normalize the wheel speeds
+  double maxMagnitude = std::max(std::abs(speeds.left), std::abs(speeds.right));
+  if (maxMagnitude > 1.0) {
+    speeds.left /= maxMagnitude;
+    speeds.right /= maxMagnitude;
+  }
+
+  return speeds;
+}
+
+DifferentialDrive::WheelSpeeds DifferentialDrive::CurvatureDrive(
+    double xSpeed, double zRotation, bool isQuickTurn) {
+  static bool reported = false;
+  if (!reported) {
+    HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 2,
+               HALUsageReporting::kRobotDrive2_DifferentialCurvature);
+    reported = true;
+  }
+
+  WheelSpeeds speeds;
+
+  if (isQuickTurn) {
+    speeds.left = xSpeed + zRotation;
+    speeds.right = xSpeed - zRotation;
+  } else {
+    speeds.left = xSpeed + std::abs(xSpeed) * zRotation;
+    speeds.right = xSpeed - std::abs(xSpeed) * zRotation;
+  }
+
+  // Normalize the wheel speeds
+  double maxMagnitude = std::max(std::abs(speeds.left), std::abs(speeds.right));
+  if (maxMagnitude > 1.0) {
+    speeds.left /= maxMagnitude;
+    speeds.right /= maxMagnitude;
+  }
+
+  return speeds;
+}
+
+DifferentialDrive::WheelSpeeds DifferentialDrive::TankDrive(double leftSpeed,
+                                                            double rightSpeed) {
+  static bool reported = false;
+  if (!reported) {
+    HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 2,
+               HALUsageReporting::kRobotDrive2_DifferentialTank);
+    reported = true;
+  }
+
+  WheelSpeeds speeds{leftSpeed, rightSpeed};
+
+  // Normalize the wheel speeds
+  double maxMagnitude = std::max(std::abs(speeds.left), std::abs(speeds.right));
+  if (maxMagnitude > 1.0) {
+    speeds.left /= maxMagnitude;
+    speeds.right /= maxMagnitude;
+  }
+
+  return speeds;
+}

wpilibc/src/main/native/cpp/experimental/drive/KilloughDrive.cpp

@@ -0,0 +1,98 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/experimental/drive/KilloughDrive.h"
+
+#include <algorithm>
+#include <array>
+#include <cmath>
+
+#include <hal/HAL.h>
+
+using namespace frc::experimental;
+
+constexpr double kPi = 3.14159265358979323846;
+
+KilloughDrive::KilloughDrive(double leftMotorAngle, double rightMotorAngle,
+                             double backMotorAngle) {
+  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))};
+}
+
+KilloughDrive::WheelSpeeds KilloughDrive::DriveCartesian(double xSpeed,
+                                                         double ySpeed,
+                                                         double zRotation,
+                                                         double gyroAngle) {
+  static bool reported = false;
+  if (!reported) {
+    HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 3,
+               HALUsageReporting::kRobotDrive2_KilloughCartesian);
+    reported = true;
+  }
+
+  // Compensate for gyro angle
+  Vector2d input{ySpeed, xSpeed};
+  input.Rotate(-gyroAngle);
+
+  WheelSpeeds speeds;
+  speeds.left = input.ScalarProject(m_leftVec) + zRotation;
+  speeds.right = input.ScalarProject(m_rightVec) + zRotation;
+  speeds.back = input.ScalarProject(m_backVec) + zRotation;
+
+  // Normalize the wheel speeds
+  std::array<double, 3> speedArray{{speeds.left, speeds.right, speeds.back}};
+  for (auto& speed : speedArray) {
+    speed = std::abs(speed);
+  }
+  double maxMagnitude = *std::max_element(speedArray.begin(), speedArray.end());
+  if (maxMagnitude > 1.0) {
+    speeds.left /= maxMagnitude;
+    speeds.right /= maxMagnitude;
+    speeds.back /= maxMagnitude;
+  }
+
+  return speeds;
+}
+
+KilloughDrive::WheelSpeeds KilloughDrive::DrivePolar(double magnitude,
+                                                     double angle,
+                                                     double zRotation) {
+  static bool reported = false;
+  if (!reported) {
+    HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 3,
+               HALUsageReporting::kRobotDrive2_KilloughPolar);
+    reported = true;
+  }
+
+  // Convert from polar to rectangular coordinates
+  double xSpeed = magnitude * std::cos(angle * (kPi / 180.0));
+  double ySpeed = magnitude * std::sin(angle * (kPi / 180.0));
+  Vector2d input{xSpeed, ySpeed};
+
+  WheelSpeeds speeds;
+  speeds.left = input.ScalarProject(m_leftVec) + zRotation;
+  speeds.right = input.ScalarProject(m_rightVec) + zRotation;
+  speeds.back = input.ScalarProject(m_backVec) + zRotation;
+
+  // Normalize the wheel speeds
+  std::array<double, 3> speedArray{{speeds.left, speeds.right, speeds.back}};
+  double maxMagnitude = *std::max_element(speedArray.begin(), speedArray.end(),
+                                          [](const double& i, const double& j) {
+                                            return std::abs(i) < std::abs(j);
+                                          });
+  if (maxMagnitude > 1.0) {
+    speeds.left /= maxMagnitude;
+    speeds.right /= maxMagnitude;
+    speeds.back /= maxMagnitude;
+  }
+
+  return speeds;
+}

wpilibc/src/main/native/cpp/experimental/drive/MecanumDrive.cpp

@@ -0,0 +1,96 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/experimental/drive/MecanumDrive.h"
+
+#include <algorithm>
+#include <array>
+#include <cmath>
+
+#include <hal/HAL.h>
+
+#include "frc/drive/Vector2d.h"
+
+using namespace frc::experimental;
+
+constexpr double kPi = 3.14159265358979323846;
+
+MecanumDrive::WheelSpeeds MecanumDrive::DriveCartesian(double xSpeed,
+                                                       double ySpeed,
+                                                       double zRotation,
+                                                       double gyroAngle) {
+  static bool reported = false;
+  if (!reported) {
+    HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 4,
+               HALUsageReporting::kRobotDrive2_MecanumCartesian);
+    reported = true;
+  }
+
+  // Compensate for gyro angle
+  Vector2d input{ySpeed, xSpeed};
+  input.Rotate(-gyroAngle);
+
+  WheelSpeeds speeds;
+  speeds.frontLeft = input.x + input.y + zRotation;
+  speeds.frontRight = -input.x + input.y - zRotation;
+  speeds.rearLeft = -input.x + input.y + zRotation;
+  speeds.rearRight = input.x + input.y - zRotation;
+
+  // Normalize the wheel speeds
+  std::array<double, 4> speedArray{
+      {speeds.frontLeft, speeds.frontRight, speeds.rearLeft, speeds.rearRight}};
+  for (auto& speed : speedArray) {
+    speed = std::abs(speed);
+  }
+  double maxMagnitude = *std::max_element(speedArray.begin(), speedArray.end());
+  if (maxMagnitude > 1.0) {
+    speeds.frontLeft /= maxMagnitude;
+    speeds.frontRight /= maxMagnitude;
+    speeds.rearLeft /= maxMagnitude;
+    speeds.rearRight /= maxMagnitude;
+  }
+
+  return speeds;
+}
+
+MecanumDrive::WheelSpeeds MecanumDrive::DrivePolar(double magnitude,
+                                                   double angle,
+                                                   double zRotation) {
+  static bool reported = false;
+  if (!reported) {
+    HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 4,
+               HALUsageReporting::kRobotDrive2_MecanumPolar);
+    reported = true;
+  }
+
+  // Convert from polar to rectangular coordinates
+  double xSpeed = magnitude * std::cos(angle * (kPi / 180.0));
+  double ySpeed = magnitude * std::sin(angle * (kPi / 180.0));
+  Vector2d input{xSpeed, ySpeed};
+
+  WheelSpeeds speeds;
+  speeds.frontLeft = input.x + input.y + zRotation;
+  speeds.frontRight = -input.x + input.y - zRotation;
+  speeds.rearLeft = -input.x + input.y + zRotation;
+  speeds.rearRight = input.x + input.y - zRotation;
+
+  // Normalize the wheel speeds
+  std::array<double, 4> speedArray{
+      {speeds.frontLeft, speeds.frontRight, speeds.rearLeft, speeds.rearRight}};
+  double maxMagnitude = *std::max_element(speedArray.begin(), speedArray.end(),
+                                          [](const double& i, const double& j) {
+                                            return std::abs(i) < std::abs(j);
+                                          });
+  if (maxMagnitude > 1.0) {
+    speeds.frontLeft /= maxMagnitude;
+    speeds.frontRight /= maxMagnitude;
+    speeds.rearLeft /= maxMagnitude;
+    speeds.rearRight /= maxMagnitude;
+  }
+
+  return speeds;
+}

wpilibc/src/main/native/include/frc/GenericHID.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-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.                                                               */
@@ -178,12 +178,32 @@ class GenericHID : public ErrorBase {
    */
   void SetRumble(RumbleType type, double value);
 
+  /**
+   * Sets the deadband applied to the joystick axes.
+   *
+   * Inputs smaller than the deadband are set to 0.0 while inputs larger than
+   * the deadband are scaled from 0.0 to 1.0. The default deadband is 0.0.
+   *
+   * @param deadband The deadband to set.
+   */
+  void SetAxisDeadband(double deadband);
+
  private:
   DriverStation& m_ds;
   int m_port;
   int m_outputs = 0;
   uint16_t m_leftRumble = 0;
   uint16_t m_rightRumble = 0;
+  double m_deadband = 0.0;
+
+  /**
+   * 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.
+   */
+  static double ApplyDeadband(double value, double deadband);
 };
 
 }  // namespace frc