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.
2026-06-21 01:01:43 +00:00 · 2018-07-20 00:03:45 -07:00
parent 31ced30c1e
commit d89b7dd412
728 changed files with 1876 additions and 1851 deletions
--- a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
+++ b/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);
+      });
+}