diff --git a/wpilibc/src/main/native/cpp/Drive/DifferentialDrive.cpp b/wpilibc/src/main/native/cpp/Drive/DifferentialDrive.cpp new file mode 100644 index 0000000000..3aefce2774 --- /dev/null +++ b/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 + +#include + +#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"; +} diff --git a/wpilibc/src/main/native/cpp/Drive/KilloughDrive.cpp b/wpilibc/src/main/native/cpp/Drive/KilloughDrive.cpp new file mode 100644 index 0000000000..a038cbfca4 --- /dev/null +++ b/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 +#include + +#include + +#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"; +} diff --git a/wpilibc/src/main/native/cpp/Drive/MecanumDrive.cpp b/wpilibc/src/main/native/cpp/Drive/MecanumDrive.cpp new file mode 100644 index 0000000000..c00d2ec624 --- /dev/null +++ b/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 +#include + +#include + +#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"; +} diff --git a/wpilibc/src/main/native/cpp/Drive/RobotDriveBase.cpp b/wpilibc/src/main/native/cpp/Drive/RobotDriveBase.cpp new file mode 100644 index 0000000000..3f0067d2a0 --- /dev/null +++ b/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 +#include +#include + +#include + +#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 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; + } + } +} diff --git a/wpilibc/src/main/native/cpp/Drive/Vector2d.cpp b/wpilibc/src/main/native/cpp/Drive/Vector2d.cpp new file mode 100644 index 0000000000..d2ebe3dac7 --- /dev/null +++ b/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 + +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(); +} diff --git a/wpilibc/src/main/native/include/Drive/DifferentialDrive.h b/wpilibc/src/main/native/include/Drive/DifferentialDrive.h new file mode 100644 index 0000000000..d15475a88b --- /dev/null +++ b/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 + +#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: + * 
+ * |_______|
+ * | |   | |
+ *   |   |
+ * |_|___|_|
+ * |       |
+ *
+ * + * 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 diff --git a/wpilibc/src/main/native/include/Drive/KilloughDrive.h b/wpilibc/src/main/native/include/Drive/KilloughDrive.h new file mode 100644 index 0000000000..01913961c8 --- /dev/null +++ b/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 + +#include + +#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: + * 
+ *  /_____\
+ * / \   / \
+ *    \ /
+ *    ---
+ *
+ * + * 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 diff --git a/wpilibc/src/main/native/include/Drive/MecanumDrive.h b/wpilibc/src/main/native/include/Drive/MecanumDrive.h new file mode 100644 index 0000000000..2b8e515563 --- /dev/null +++ b/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 + +#include + +#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: + * 
+ * \\_______/
+ * \\ |   | /
+ *   |   |
+ * /_|___|_\\
+ * /       \\
+ *
+ * + * 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 diff --git a/wpilibc/src/main/native/include/Drive/RobotDriveBase.h b/wpilibc/src/main/native/include/Drive/RobotDriveBase.h new file mode 100644 index 0000000000..dc940d8a0e --- /dev/null +++ b/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 + +#include +#include + +#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 wheelSpeeds); + + double m_deadband = 0.02; + double m_maxOutput = 1.0; + MotorSafetyHelper m_safetyHelper{this}; +}; + +} // namespace frc diff --git a/wpilibc/src/main/native/include/Drive/Vector2d.h b/wpilibc/src/main/native/include/Drive/Vector2d.h new file mode 100644 index 0000000000..fc2aaa35d0 --- /dev/null +++ b/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 diff --git a/wpilibc/src/main/native/include/RobotDrive.h b/wpilibc/src/main/native/include/RobotDrive.h index af4dbf0104..5203816407 100644 --- a/wpilibc/src/main/native/include/RobotDrive.h +++ b/wpilibc/src/main/native/include/RobotDrive.h @@ -10,6 +10,7 @@ #include #include +#include #include "ErrorBase.h" #include "MotorSafety.h" @@ -30,7 +31,9 @@ class GenericHID; * function (intended for hand created drive code, such as autonomous) or with * the Tank/Arcade functions intended to be used for Operator Control driving. */ -class RobotDrive : public MotorSafety, public ErrorBase { +class WPI_DEPRECATED("Use DifferentialDrive or MecanumDrive classes instead.") + RobotDrive : public MotorSafety, + public ErrorBase { public: enum MotorType { kFrontLeftMotor = 0, diff --git a/wpilibc/src/main/native/include/WPILib.h b/wpilibc/src/main/native/include/WPILib.h index dca88b5444..ac2000fa89 100644 --- a/wpilibc/src/main/native/include/WPILib.h +++ b/wpilibc/src/main/native/include/WPILib.h @@ -41,6 +41,9 @@ #include "DigitalOutput.h" #include "DigitalSource.h" #include "DoubleSolenoid.h" +#include "Drive/DifferentialDrive.h" +#include "Drive/KilloughDrive.h" +#include "Drive/MecanumDrive.h" #include "DriverStation.h" #include "Encoder.h" #include "ErrorBase.h" diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java index 1b77bd1472..e42184eef9 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java @@ -20,7 +20,10 @@ import static java.util.Objects.requireNonNull; * Motor channel numbers are supplied on creation of the class. Those are used for either the drive * function (intended for hand created drive code, such as autonomous) or with the Tank/Arcade * functions intended to be used for Operator Control driving. + * + * @deprecated Use DifferentialDrive or MecanumDrive classes instead. */ +@Deprecated public class RobotDrive implements MotorSafety { protected MotorSafetyHelper m_safetyHelper; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java new file mode 100644 index 0000000000..7ac076d138 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java @@ -0,0 +1,286 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.drive; + +import edu.wpi.first.wpilibj.SpeedController; +import edu.wpi.first.wpilibj.hal.FRCNetComm.tInstances; +import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; +import edu.wpi.first.wpilibj.hal.HAL; + +/** + * 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 {@link SpeedControllerGroup} instances as follows. + * + *

Four motor drivetrain: + * 


+ * public class Robot {
+ *   Talon m_frontLeft = new Talon(1);
+ *   Talon m_rearLeft = new Talon(2);
+ *   SpeedControllerGroup m_left = new SpeedControllerGroup(m_frontLeft, m_rearLeft);
+ *
+ *   Talon m_frontRight = new Talon(3);
+ *   Talon m_rearRight = new Talon(4);
+ *   SpeedControllerGroup m_right = new SpeedControllerGroup(m_frontRight, m_rearRight);
+ *
+ *   DifferentialDrive m_drive = new DifferentialDrive(m_left, m_right);
+ * }
+ *
+ * + *

Six motor drivetrain: + * 


+ * public class Robot {
+ *   Talon m_frontLeft = new Talon(1);
+ *   Talon m_midLeft = new Talon(2);
+ *   Talon m_rearLeft = new Talon(3);
+ *   SpeedControllerGroup m_left = new SpeedControllerGroup(m_frontLeft, m_midLeft, m_rearLeft);
+ *
+ *   Talon m_frontRight = new Talon(4);
+ *   Talon m_midRight = new Talon(5);
+ *   Talon m_rearRight = new Talon(6);
+ *   SpeedControllerGroup m_right = new SpeedControllerGroup(m_frontRight, m_midRight, m_rearRight);
+ *
+ *   DifferentialDrive m_drive = new DifferentialDrive(m_left, m_right);
+ * }
+ *
+ * + *

A differential drive robot has left and right wheels separated by an arbitrary width. + * + *

Drive base diagram: + * 

+ * |_______|
+ * | |   | |
+ *   |   |
+ * |_|___|_|
+ * |       |
+ *
+ * + *

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. + */ +public class DifferentialDrive extends RobotDriveBase { + private SpeedController m_leftMotor; + private SpeedController m_rightMotor; + + private double m_quickStopAccumulator = 0.0; + private boolean m_reported = false; + + /** + * Construct a DifferentialDrive. + * + *

To pass multiple motors per side, use a {@link SpeedControllerGroup}. If a motor needs to be + * inverted, do so before passing it in. + */ + public DifferentialDrive(SpeedController leftMotor, SpeedController rightMotor) { + m_leftMotor = leftMotor; + m_rightMotor = rightMotor; + } + + /** + * Arcade drive method for differential drive platform. + * + * @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] + */ + @SuppressWarnings("ParameterName") + public void arcadeDrive(double y, double rotation) { + arcadeDrive(y, rotation, true); + } + + /** + * Arcade drive method for differential drive platform. + * + * @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. + */ + @SuppressWarnings("ParameterName") + public void arcadeDrive(double y, double rotation, boolean squaredInputs) { + if (!m_reported) { + HAL.report(tResourceType.kResourceType_RobotDrive, 2, tInstances.kRobotDrive_ArcadeStandard); + m_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 = Math.copySign(y * y, y); + rotation = Math.copySign(rotation * rotation, rotation); + } + + double leftMotorOutput; + double rightMotorOutput; + + double maxInput = Math.copySign(Math.max(Math.abs(y), Math.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. + */ + @SuppressWarnings("ParameterName") + public void curvatureDrive(double y, double rotation, boolean isQuickTurn) { + if (!m_reported) { + // HAL.report(tResourceType.kResourceType_RobotDrive, 2, tInstances.kRobotDrive_Curvature); + m_reported = true; + } + + y = limit(y); + y = applyDeadband(y, m_deadband); + + rotation = limit(rotation); + rotation = applyDeadband(rotation, m_deadband); + + double angularPower; + boolean overPower; + + if (isQuickTurn) { + if (Math.abs(y) < 0.2) { + final double alpha = 0.1; + m_quickStopAccumulator = + (1 - alpha) * m_quickStopAccumulator + alpha * limit(rotation) * 2; + } + overPower = true; + angularPower = rotation; + } else { + overPower = false; + angularPower = Math.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] + */ + public void tankDrive(double left, double right) { + tankDrive(left, right, true); + } + + /** + * 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. + */ + public void tankDrive(double left, double right, boolean squaredInputs) { + if (!m_reported) { + HAL.report(tResourceType.kResourceType_RobotDrive, 2, tInstances.kRobotDrive_Tank); + m_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 = Math.copySign(left * left, left); + right = Math.copySign(right * right, right); + } + + m_leftMotor.set(left * m_maxOutput); + m_rightMotor.set(-right * m_maxOutput); + + m_safetyHelper.feed(); + } + + @Override + public void stopMotor() { + m_leftMotor.stopMotor(); + m_rightMotor.stopMotor(); + m_safetyHelper.feed(); + } + + @Override + public String getDescription() { + return "DifferentialDrive"; + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java new file mode 100644 index 0000000000..58fce96536 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java @@ -0,0 +1,181 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.drive; + +import edu.wpi.first.wpilibj.SpeedController; +// import edu.wpi.first.wpilibj.hal.FRCNetComm.tInstances; +// import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; +// import edu.wpi.first.wpilibj.hal.HAL; + +/** + * A class for driving Killough drive platforms. + * + *

Killough drives are triangular with one omni wheel on each corner. + * + *

Drive base diagram: + * 

+ *  /_____\
+ * / \   / \
+ *    \ /
+ *    ---
+ *
+ * + *

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. + */ +public class KilloughDrive extends RobotDriveBase { + private SpeedController m_leftMotor; + private SpeedController m_rightMotor; + private SpeedController m_backMotor; + + private Vector2d m_leftVec; + private Vector2d m_rightVec; + private Vector2d m_backVec; + + private boolean m_reported = false; + + /** + * 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. + */ + public KilloughDrive(SpeedController leftMotor, SpeedController rightMotor, + SpeedController backMotor) { + this(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. + */ + public KilloughDrive(SpeedController leftMotor, SpeedController rightMotor, + SpeedController backMotor, double leftMotorAngle, double rightMotorAngle, + double backMotorAngle) { + m_leftMotor = leftMotor; + m_rightMotor = rightMotor; + m_backMotor = backMotor; + m_leftVec = new Vector2d(Math.cos(leftMotorAngle * (Math.PI / 180.0)), + Math.sin(leftMotorAngle * (Math.PI / 180.0))); + m_rightVec = new Vector2d(Math.cos(rightMotorAngle * (Math.PI / 180.0)), + Math.sin(rightMotorAngle * (Math.PI / 180.0))); + m_backVec = new Vector2d(Math.cos(backMotorAngle * (Math.PI / 180.0)), + Math.sin(backMotorAngle * (Math.PI / 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] + */ + @SuppressWarnings("ParameterName") + public void driveCartesian(double x, double y, double rotation) { + driveCartesian(x, y, rotation, 0.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. + */ + @SuppressWarnings("ParameterName") + public void driveCartesian(double x, double y, double rotation, + double gyroAngle) { + if (!m_reported) { + // HAL.report(tResourceType.kResourceType_RobotDrive, 3, + // tInstances.kRobotDrive_KilloughCartesian); + m_reported = true; + } + + x = limit(x); + x = applyDeadband(x, m_deadband); + + y = limit(y); + y = applyDeadband(y, m_deadband); + + // Compensate for gyro angle. + Vector2d input = new Vector2d(x, y); + input.rotate(gyroAngle); + + double[] wheelSpeeds = new double[3]; + wheelSpeeds[MotorType.kLeft.value] = input.scalarProject(m_leftVec) + rotation; + wheelSpeeds[MotorType.kRight.value] = input.scalarProject(m_rightVec) + rotation; + wheelSpeeds[MotorType.kBack.value] = input.scalarProject(m_backVec) + rotation; + + normalize(wheelSpeeds); + + m_leftMotor.set(wheelSpeeds[MotorType.kLeft.value] * m_maxOutput); + m_rightMotor.set(wheelSpeeds[MotorType.kRight.value] * m_maxOutput); + m_backMotor.set(wheelSpeeds[MotorType.kBack.value] * 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] + */ + public void drivePolar(double magnitude, double angle, double rotation) { + if (!m_reported) { + // HAL.report(tResourceType.kResourceType_RobotDrive, 3, + // tInstances.kRobotDrive_KilloughPolar); + m_reported = true; + } + + // Normalized for full power along the Cartesian axes. + magnitude = limit(magnitude) * Math.sqrt(2.0); + + driveCartesian(magnitude * Math.cos(angle * (Math.PI / 180.0)), + magnitude * Math.sin(angle * (Math.PI / 180.0)), rotation, 0.0); + } + + @Override + public void stopMotor() { + m_leftMotor.stopMotor(); + m_rightMotor.stopMotor(); + m_backMotor.stopMotor(); + m_safetyHelper.feed(); + } + + @Override + public String getDescription() { + return "KilloughDrive"; + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java new file mode 100644 index 0000000000..ab3440114f --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java @@ -0,0 +1,149 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.drive; + +import edu.wpi.first.wpilibj.SpeedController; +import edu.wpi.first.wpilibj.hal.FRCNetComm.tInstances; +import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; +import edu.wpi.first.wpilibj.hal.HAL; + +/** + * 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. Each drive() function provides different inverse kinematic + * relations for a Mecanum drive robot. + * + *

Drive base diagram: + * 

+ * \\_______/
+ * \\ |   | /
+ *   |   |
+ * /_|___|_\\
+ * /       \\
+ *
+ * + *

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. + */ +public class MecanumDrive extends RobotDriveBase { + private SpeedController m_frontLeftMotor; + private SpeedController m_rearLeftMotor; + private SpeedController m_frontRightMotor; + private SpeedController m_rearRightMotor; + + private boolean m_reported = false; + + /** + * Construct a MecanumDrive. + * + *

If a motor needs to be inverted, do so before passing it in. + */ + public 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] + */ + @SuppressWarnings("ParameterName") + public void driveCartesian(double x, double y, double rotation) { + driveCartesian(x, y, rotation, 0.0); + } + + /** + * 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. + */ + @SuppressWarnings("ParameterName") + public void driveCartesian(double x, double y, double rotation, double gyroAngle) { + if (!m_reported) { + HAL.report(tResourceType.kResourceType_RobotDrive, 4, + tInstances.kRobotDrive_MecanumCartesian); + m_reported = true; + } + + x = limit(x); + x = applyDeadband(x, m_deadband); + + y = limit(y); + y = applyDeadband(y, m_deadband); + + // Compensate for gyro angle. + Vector2d input = new Vector2d(x, y); + input.rotate(gyroAngle); + + double[] wheelSpeeds = new double[4]; + wheelSpeeds[MotorType.kFrontLeft.value] = input.x + input.y + rotation; + wheelSpeeds[MotorType.kFrontRight.value] = input.x - input.y + rotation; + wheelSpeeds[MotorType.kRearLeft.value] = -input.x + input.y + rotation; + wheelSpeeds[MotorType.kRearRight.value] = -input.x - input.y + rotation; + + normalize(wheelSpeeds); + + m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft.value] * m_maxOutput); + m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight.value] * m_maxOutput); + m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft.value] * m_maxOutput); + m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight.value] * 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] + */ + public void drivePolar(double magnitude, double angle, double rotation) { + if (!m_reported) { + HAL.report(tResourceType.kResourceType_RobotDrive, 4, tInstances.kRobotDrive_MecanumPolar); + m_reported = true; + } + + // Normalized for full power along the Cartesian axes. + magnitude = limit(magnitude) * Math.sqrt(2.0); + + driveCartesian(magnitude * Math.cos(angle * (Math.PI / 180.0)), + magnitude * Math.sin(angle * (Math.PI / 180.0)), rotation, 0.0); + } + + @Override + public void stopMotor() { + m_frontLeftMotor.stopMotor(); + m_frontRightMotor.stopMotor(); + m_rearLeftMotor.stopMotor(); + m_rearRightMotor.stopMotor(); + m_safetyHelper.feed(); + } + + @Override + public String getDescription() { + return "MecanumDrive"; + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java new file mode 100644 index 0000000000..89494e23c2 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java @@ -0,0 +1,134 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.drive; + +import edu.wpi.first.wpilibj.MotorSafety; +import edu.wpi.first.wpilibj.MotorSafetyHelper; + +/** + * Common base class for drive platforms. + */ +public abstract class RobotDriveBase implements MotorSafety { + protected double m_deadband = 0.02; + protected double m_maxOutput = 1.0; + protected MotorSafetyHelper m_safetyHelper = new MotorSafetyHelper(this); + + /** + * The location of a motor on the robot for the purpose of driving. + */ + public enum MotorType { + kFrontLeft(0), kFrontRight(1), kRearLeft(2), kRearRight(3), kLeft(0), + kRight(1), kBack(2); + + @SuppressWarnings("MemberName") + public final int value; + + MotorType(int value) { + this.value = value; + } + } + + public RobotDriveBase() { + m_safetyHelper.setSafetyEnabled(true); + } + + public void 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. + */ + public void setMaxOutput(double maxOutput) { + m_maxOutput = maxOutput; + } + + @Override + public void setExpiration(double timeout) { + m_safetyHelper.setExpiration(timeout); + } + + @Override + public double getExpiration() { + return m_safetyHelper.getExpiration(); + } + + @Override + public boolean isAlive() { + return m_safetyHelper.isAlive(); + } + + @Override + public abstract void stopMotor(); + + @Override + public boolean isSafetyEnabled() { + return m_safetyHelper.isSafetyEnabled(); + } + + @Override + public void setSafetyEnabled(boolean enabled) { + m_safetyHelper.setSafetyEnabled(enabled); + } + + @Override + public abstract String getDescription(); + + /** + * Limit motor values to the -1.0 to +1.0 range. + */ + protected double 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 + */ + protected double applyDeadband(double value, double deadband) { + if (Math.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. + */ + protected void normalize(double[] wheelSpeeds) { + double maxMagnitude = Math.abs(wheelSpeeds[0]); + for (int i = 1; i < wheelSpeeds.length; i++) { + double temp = Math.abs(wheelSpeeds[i]); + if (maxMagnitude < temp) { + maxMagnitude = temp; + } + } + if (maxMagnitude > 1.0) { + for (int i = 0; i < wheelSpeeds.length; i++) { + wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude; + } + } + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/Vector2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/Vector2d.java new file mode 100644 index 0000000000..791cd35683 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/Vector2d.java @@ -0,0 +1,65 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.drive; + +/** + * This is a 2D vector struct that supports basic vector operations. + */ +@SuppressWarnings("MemberName") +public class Vector2d { + public double x = 0.0; + public double y = 0.0; + + public Vector2d() {} + + @SuppressWarnings("ParameterName") + public 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. + */ + public void rotate(double angle) { + double cosA = Math.cos(angle * (Math.PI / 180.0)); + double sinA = Math.sin(angle * (Math.PI / 180.0)); + double[] out = new double[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. + */ + public double dot(Vector2d vec) { + return x * vec.x + y * vec.y; + } + + /** + * Returns magnitude of vector. + */ + public double magnitude() { + return Math.sqrt(x * x + y * y); + } + + /** + * Returns scalar projection of this vector onto argument. + * + * @param vec Vector onto which to project this vector. + */ + public double scalarProject(Vector2d vec) { + return dot(vec) / vec.magnitude(); + } +}