Move CameraServer and WPILib headers into their own folder

The old headers were moved into folders because doing so avoids polluting
the system include directories.

Folder names were also normalized to lowercase.

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

@@ -0,0 +1,223 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 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/drive/DifferentialDrive.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <hal/HAL.h>
+
+#include "frc/SpeedController.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+DifferentialDrive::DifferentialDrive(SpeedController& leftMotor,
+                                     SpeedController& rightMotor)
+    : m_leftMotor(leftMotor), m_rightMotor(rightMotor) {
+  AddChild(&m_leftMotor);
+  AddChild(&m_rightMotor);
+  static int instances = 0;
+  ++instances;
+  SetName("DifferentialDrive", instances);
+}
+
+void DifferentialDrive::ArcadeDrive(double xSpeed, double zRotation,
+                                    bool squaredInputs) {
+  static bool reported = false;
+  if (!reported) {
+    HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 2,
+               HALUsageReporting::kRobotDrive_ArcadeStandard);
+    reported = true;
+  }
+
+  xSpeed = Limit(xSpeed);
+  xSpeed = ApplyDeadband(xSpeed, m_deadband);
+
+  zRotation = Limit(zRotation);
+  zRotation = ApplyDeadband(zRotation, m_deadband);
+
+  // Square the inputs (while preserving the sign) to increase fine control
+  // while permitting full power.
+  if (squaredInputs) {
+    xSpeed = std::copysign(xSpeed * xSpeed, xSpeed);
+    zRotation = std::copysign(zRotation * zRotation, zRotation);
+  }
+
+  double leftMotorOutput;
+  double rightMotorOutput;
+
+  double maxInput =
+      std::copysign(std::max(std::abs(xSpeed), std::abs(zRotation)), xSpeed);
+
+  if (xSpeed >= 0.0) {
+    // First quadrant, else second quadrant
+    if (zRotation >= 0.0) {
+      leftMotorOutput = maxInput;
+      rightMotorOutput = xSpeed - zRotation;
+    } else {
+      leftMotorOutput = xSpeed + zRotation;
+      rightMotorOutput = maxInput;
+    }
+  } else {
+    // Third quadrant, else fourth quadrant
+    if (zRotation >= 0.0) {
+      leftMotorOutput = xSpeed + zRotation;
+      rightMotorOutput = maxInput;
+    } else {
+      leftMotorOutput = maxInput;
+      rightMotorOutput = xSpeed - zRotation;
+    }
+  }
+
+  m_leftMotor.Set(Limit(leftMotorOutput) * m_maxOutput);
+  m_rightMotor.Set(Limit(rightMotorOutput) * m_maxOutput *
+                   m_rightSideInvertMultiplier);
+
+  m_safetyHelper.Feed();
+}
+
+void DifferentialDrive::CurvatureDrive(double xSpeed, double zRotation,
+                                       bool isQuickTurn) {
+  static bool reported = false;
+  if (!reported) {
+    // HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 2,
+    //            HALUsageReporting::kRobotDrive_Curvature);
+    reported = true;
+  }
+
+  xSpeed = Limit(xSpeed);
+  xSpeed = ApplyDeadband(xSpeed, m_deadband);
+
+  zRotation = Limit(zRotation);
+  zRotation = ApplyDeadband(zRotation, m_deadband);
+
+  double angularPower;
+  bool overPower;
+
+  if (isQuickTurn) {
+    if (std::abs(xSpeed) < m_quickStopThreshold) {
+      m_quickStopAccumulator = (1 - m_quickStopAlpha) * m_quickStopAccumulator +
+                               m_quickStopAlpha * Limit(zRotation) * 2;
+    }
+    overPower = true;
+    angularPower = zRotation;
+  } else {
+    overPower = false;
+    angularPower = std::abs(xSpeed) * zRotation - m_quickStopAccumulator;
+
+    if (m_quickStopAccumulator > 1) {
+      m_quickStopAccumulator -= 1;
+    } else if (m_quickStopAccumulator < -1) {
+      m_quickStopAccumulator += 1;
+    } else {
+      m_quickStopAccumulator = 0.0;
+    }
+  }
+
+  double leftMotorOutput = xSpeed + angularPower;
+  double rightMotorOutput = xSpeed - 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;
+    }
+  }
+
+  // Normalize the wheel speeds
+  double maxMagnitude =
+      std::max(std::abs(leftMotorOutput), std::abs(rightMotorOutput));
+  if (maxMagnitude > 1.0) {
+    leftMotorOutput /= maxMagnitude;
+    rightMotorOutput /= maxMagnitude;
+  }
+
+  m_leftMotor.Set(leftMotorOutput * m_maxOutput);
+  m_rightMotor.Set(rightMotorOutput * m_maxOutput *
+                   m_rightSideInvertMultiplier);
+
+  m_safetyHelper.Feed();
+}
+
+void DifferentialDrive::TankDrive(double leftSpeed, double rightSpeed,
+                                  bool squaredInputs) {
+  static bool reported = false;
+  if (!reported) {
+    HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 2,
+               HALUsageReporting::kRobotDrive_Tank);
+    reported = true;
+  }
+
+  leftSpeed = Limit(leftSpeed);
+  leftSpeed = ApplyDeadband(leftSpeed, m_deadband);
+
+  rightSpeed = Limit(rightSpeed);
+  rightSpeed = ApplyDeadband(rightSpeed, m_deadband);
+
+  // Square the inputs (while preserving the sign) to increase fine control
+  // while permitting full power.
+  if (squaredInputs) {
+    leftSpeed = std::copysign(leftSpeed * leftSpeed, leftSpeed);
+    rightSpeed = std::copysign(rightSpeed * rightSpeed, rightSpeed);
+  }
+
+  m_leftMotor.Set(leftSpeed * m_maxOutput);
+  m_rightMotor.Set(rightSpeed * m_maxOutput * m_rightSideInvertMultiplier);
+
+  m_safetyHelper.Feed();
+}
+
+void DifferentialDrive::SetQuickStopThreshold(double threshold) {
+  m_quickStopThreshold = threshold;
+}
+
+void DifferentialDrive::SetQuickStopAlpha(double alpha) {
+  m_quickStopAlpha = alpha;
+}
+
+bool DifferentialDrive::IsRightSideInverted() const {
+  return m_rightSideInvertMultiplier == -1.0;
+}
+
+void DifferentialDrive::SetRightSideInverted(bool rightSideInverted) {
+  m_rightSideInvertMultiplier = rightSideInverted ? -1.0 : 1.0;
+}
+
+void DifferentialDrive::StopMotor() {
+  m_leftMotor.StopMotor();
+  m_rightMotor.StopMotor();
+  m_safetyHelper.Feed();
+}
+
+void DifferentialDrive::GetDescription(wpi::raw_ostream& desc) const {
+  desc << "DifferentialDrive";
+}
+
+void DifferentialDrive::InitSendable(SendableBuilder& builder) {
+  builder.SetSmartDashboardType("DifferentialDrive");
+  builder.AddDoubleProperty("Left Motor Speed",
+                            [=]() { return m_leftMotor.Get(); },
+                            [=](double value) { m_leftMotor.Set(value); });
+  builder.AddDoubleProperty(
+      "Right Motor Speed",
+      [=]() { return m_rightMotor.Get() * m_rightSideInvertMultiplier; },
+      [=](double value) {
+        m_rightMotor.Set(value * m_rightSideInvertMultiplier);
+      });
+}

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

@@ -0,0 +1,113 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 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/drive/KilloughDrive.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <hal/HAL.h>
+
+#include "frc/SpeedController.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+constexpr double kPi = 3.14159265358979323846;
+
+KilloughDrive::KilloughDrive(SpeedController& leftMotor,
+                             SpeedController& rightMotor,
+                             SpeedController& backMotor)
+    : KilloughDrive(leftMotor, rightMotor, backMotor, kDefaultLeftMotorAngle,
+                    kDefaultRightMotorAngle, kDefaultBackMotorAngle) {}
+
+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))};
+  AddChild(&m_leftMotor);
+  AddChild(&m_rightMotor);
+  AddChild(&m_backMotor);
+  static int instances = 0;
+  ++instances;
+  SetName("KilloughDrive", instances);
+}
+
+void KilloughDrive::DriveCartesian(double ySpeed, double xSpeed,
+                                   double zRotation, double gyroAngle) {
+  if (!reported) {
+    // HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 3,
+    //            HALUsageReporting::kRobotDrive_KilloughCartesian);
+    reported = true;
+  }
+
+  ySpeed = Limit(ySpeed);
+  ySpeed = ApplyDeadband(ySpeed, m_deadband);
+
+  xSpeed = Limit(xSpeed);
+  xSpeed = ApplyDeadband(xSpeed, m_deadband);
+
+  // Compensate for gyro angle.
+  Vector2d input{ySpeed, xSpeed};
+  input.Rotate(-gyroAngle);
+
+  double wheelSpeeds[3];
+  wheelSpeeds[kLeft] = input.ScalarProject(m_leftVec) + zRotation;
+  wheelSpeeds[kRight] = input.ScalarProject(m_rightVec) + zRotation;
+  wheelSpeeds[kBack] = input.ScalarProject(m_backVec) + zRotation;
+
+  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();
+}
+
+void KilloughDrive::DrivePolar(double magnitude, double angle,
+                               double zRotation) {
+  if (!reported) {
+    // HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 3,
+    //            HALUsageReporting::kRobotDrive_KilloughPolar);
+    reported = true;
+  }
+
+  DriveCartesian(magnitude * std::sin(angle * (kPi / 180.0)),
+                 magnitude * std::cos(angle * (kPi / 180.0)), zRotation, 0.0);
+}
+
+void KilloughDrive::StopMotor() {
+  m_leftMotor.StopMotor();
+  m_rightMotor.StopMotor();
+  m_backMotor.StopMotor();
+  m_safetyHelper.Feed();
+}
+
+void KilloughDrive::GetDescription(wpi::raw_ostream& desc) const {
+  desc << "KilloughDrive";
+}
+
+void KilloughDrive::InitSendable(SendableBuilder& builder) {
+  builder.SetSmartDashboardType("KilloughDrive");
+  builder.AddDoubleProperty("Left Motor Speed",
+                            [=]() { return m_leftMotor.Get(); },
+                            [=](double value) { m_leftMotor.Set(value); });
+  builder.AddDoubleProperty("Right Motor Speed",
+                            [=]() { return m_rightMotor.Get(); },
+                            [=](double value) { m_rightMotor.Set(value); });
+  builder.AddDoubleProperty("Back Motor Speed",
+                            [=]() { return m_backMotor.Get(); },
+                            [=](double value) { m_backMotor.Set(value); });
+}

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

@@ -0,0 +1,128 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2008-2018 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/drive/MecanumDrive.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <hal/HAL.h>
+
+#include "frc/SpeedController.h"
+#include "frc/drive/Vector2d.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+constexpr double kPi = 3.14159265358979323846;
+
+MecanumDrive::MecanumDrive(SpeedController& frontLeftMotor,
+                           SpeedController& rearLeftMotor,
+                           SpeedController& frontRightMotor,
+                           SpeedController& rearRightMotor)
+    : m_frontLeftMotor(frontLeftMotor),
+      m_rearLeftMotor(rearLeftMotor),
+      m_frontRightMotor(frontRightMotor),
+      m_rearRightMotor(rearRightMotor) {
+  AddChild(&m_frontLeftMotor);
+  AddChild(&m_rearLeftMotor);
+  AddChild(&m_frontRightMotor);
+  AddChild(&m_rearRightMotor);
+  static int instances = 0;
+  ++instances;
+  SetName("MecanumDrive", instances);
+}
+
+void MecanumDrive::DriveCartesian(double ySpeed, double xSpeed,
+                                  double zRotation, double gyroAngle) {
+  if (!reported) {
+    HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 4,
+               HALUsageReporting::kRobotDrive_MecanumCartesian);
+    reported = true;
+  }
+
+  ySpeed = Limit(ySpeed);
+  ySpeed = ApplyDeadband(ySpeed, m_deadband);
+
+  xSpeed = Limit(xSpeed);
+  xSpeed = ApplyDeadband(xSpeed, m_deadband);
+
+  // Compensate for gyro angle.
+  Vector2d input{ySpeed, xSpeed};
+  input.Rotate(-gyroAngle);
+
+  double wheelSpeeds[4];
+  wheelSpeeds[kFrontLeft] = input.x + input.y + zRotation;
+  wheelSpeeds[kFrontRight] = -input.x + input.y - zRotation;
+  wheelSpeeds[kRearLeft] = -input.x + input.y + zRotation;
+  wheelSpeeds[kRearRight] = input.x + input.y - zRotation;
+
+  Normalize(wheelSpeeds);
+
+  m_frontLeftMotor.Set(wheelSpeeds[kFrontLeft] * m_maxOutput);
+  m_frontRightMotor.Set(wheelSpeeds[kFrontRight] * m_maxOutput *
+                        m_rightSideInvertMultiplier);
+  m_rearLeftMotor.Set(wheelSpeeds[kRearLeft] * m_maxOutput);
+  m_rearRightMotor.Set(wheelSpeeds[kRearRight] * m_maxOutput *
+                       m_rightSideInvertMultiplier);
+
+  m_safetyHelper.Feed();
+}
+
+void MecanumDrive::DrivePolar(double magnitude, double angle,
+                              double zRotation) {
+  if (!reported) {
+    HAL_Report(HALUsageReporting::kResourceType_RobotDrive, 4,
+               HALUsageReporting::kRobotDrive_MecanumPolar);
+    reported = true;
+  }
+
+  DriveCartesian(magnitude * std::sin(angle * (kPi / 180.0)),
+                 magnitude * std::cos(angle * (kPi / 180.0)), zRotation, 0.0);
+}
+
+bool MecanumDrive::IsRightSideInverted() const {
+  return m_rightSideInvertMultiplier == -1.0;
+}
+
+void MecanumDrive::SetRightSideInverted(bool rightSideInverted) {
+  m_rightSideInvertMultiplier = rightSideInverted ? -1.0 : 1.0;
+}
+
+void MecanumDrive::StopMotor() {
+  m_frontLeftMotor.StopMotor();
+  m_frontRightMotor.StopMotor();
+  m_rearLeftMotor.StopMotor();
+  m_rearRightMotor.StopMotor();
+  m_safetyHelper.Feed();
+}
+
+void MecanumDrive::GetDescription(wpi::raw_ostream& desc) const {
+  desc << "MecanumDrive";
+}
+
+void MecanumDrive::InitSendable(SendableBuilder& builder) {
+  builder.SetSmartDashboardType("MecanumDrive");
+  builder.AddDoubleProperty("Front Left Motor Speed",
+                            [=]() { return m_frontLeftMotor.Get(); },
+                            [=](double value) { m_frontLeftMotor.Set(value); });
+  builder.AddDoubleProperty(
+      "Front Right Motor Speed",
+      [=]() { return m_frontRightMotor.Get() * m_rightSideInvertMultiplier; },
+      [=](double value) {
+        m_frontRightMotor.Set(value * m_rightSideInvertMultiplier);
+      });
+  builder.AddDoubleProperty("Rear Left Motor Speed",
+                            [=]() { return m_rearLeftMotor.Get(); },
+                            [=](double value) { m_rearLeftMotor.Set(value); });
+  builder.AddDoubleProperty(
+      "Rear Right Motor Speed",
+      [=]() { return m_rearRightMotor.Get() * m_rightSideInvertMultiplier; },
+      [=](double value) {
+        m_rearRightMotor.Set(value * m_rightSideInvertMultiplier);
+      });
+}

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

@@ -0,0 +1,82 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 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/drive/RobotDriveBase.h"
+
+#include <algorithm>
+#include <cmath>
+#include <cstddef>
+
+#include <hal/HAL.h>
+
+#include "frc/Base.h"
+#include "frc/SpeedController.h"
+
+using namespace frc;
+
+RobotDriveBase::RobotDriveBase() { m_safetyHelper.SetSafetyEnabled(true); }
+
+void RobotDriveBase::SetDeadband(double deadband) { m_deadband = deadband; }
+
+void RobotDriveBase::SetMaxOutput(double maxOutput) { m_maxOutput = maxOutput; }
+
+void RobotDriveBase::FeedWatchdog() { m_safetyHelper.Feed(); }
+
+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);
+}
+
+double RobotDriveBase::Limit(double value) {
+  if (value > 1.0) {
+    return 1.0;
+  }
+  if (value < -1.0) {
+    return -1.0;
+  }
+  return value;
+}
+
+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;
+  }
+}
+
+void RobotDriveBase::Normalize(wpi::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,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 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/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;
+}
+
+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];
+}
+
+double Vector2d::Dot(const Vector2d& vec) const {
+  return x * vec.x + y * vec.y;
+}
+
+double Vector2d::Magnitude() const { return std::sqrt(x * x + y * y); }
+
+double Vector2d::ScalarProject(const Vector2d& vec) const {
+  return Dot(vec) / vec.Magnitude();
+}