Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-02 02:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Split RobotDrive class into a class for each drive type (#552)

Browse Source 
DiffDrive.CurvatureDrive (aka CheesyDrive) and KilloughDrive were also added.
This reorganization paves the way for SwerveDrive.
This commit is contained in:
Tyler Veness
2017-09-28 23:30:00 -07:00
committed by Peter Johnson
parent abb66d3e4b
commit 19addb04cf
18 changed files with 1790 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

213
wpilibc/src/main/native/cpp/Drive/DifferentialDrive.cpp Normal file
View File

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

147
wpilibc/src/main/native/cpp/Drive/KilloughDrive.cpp Normal file
View File

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

117
wpilibc/src/main/native/cpp/Drive/MecanumDrive.cpp Normal file
View File

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

100
wpilibc/src/main/native/cpp/Drive/RobotDriveBase.cpp Normal file
View File

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

57
wpilibc/src/main/native/cpp/Drive/Vector2d.cpp Normal file
View File

@@ -0,0 +1,57 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "Drive/Vector2d.h"
#include <cmath>
using namespace frc;
constexpr double kPi = 3.14159265358979323846;
Vector2d::Vector2d(double x, double y) {
this->x = x;
this->y = y;
}
/**
* Rotate a vector in Cartesian space.
*
* @param angle angle by which to rotate vector counter-clockwise.
*/
void Vector2d::Rotate(double angle) {
double cosA = std::cos(angle * (kPi / 180.0));
double sinA = std::sin(angle * (kPi / 180.0));
double out[2];
out[0] = x * cosA - y * sinA;
out[1] = x * sinA + y * cosA;
x = out[0];
y = out[1];
}
/**
* Returns dot product of this vector with argument.
*
* @param vec Vector with which to perform dot product.
*/
double Vector2d::Dot(const Vector2d& vec) const {
return x * vec.x + y * vec.y;
}
/**
* Returns magnitude of vector.
*/
double Vector2d::Magnitude() const { return std::sqrt(x * x + y * y); }
/**
* Returns scalar projection of this vector onto argument.
*
* @param vec Vector onto which to project this vector.
*/
double Vector2d::ScalarProject(const Vector2d& vec) const {
return Dot(vec) / vec.Magnitude();
}

99
wpilibc/src/main/native/include/Drive/DifferentialDrive.h Normal file
View File

@@ -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 <llvm/raw_ostream.h>
#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:
* <pre>
* |_______|
* | | | |
* | |
* |_|___|_|
* | |
* </pre>
*
* 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

70
wpilibc/src/main/native/include/Drive/KilloughDrive.h Normal file
View File

@@ -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 <memory>
#include <llvm/raw_ostream.h>
#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:
* <pre>
* /_____\
* / \ / \
* \ /
* ---
* </pre>
*
* 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

66
wpilibc/src/main/native/include/Drive/MecanumDrive.h Normal file
View File

@@ -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 <memory>
#include <llvm/raw_ostream.h>
#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:
* <pre>
* \\_______/
* \\ | | /
* | |
* /_|___|_\\
* / \\
* </pre>
*
* 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

68
wpilibc/src/main/native/include/Drive/RobotDriveBase.h Normal file
View File

@@ -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 <memory>
#include <llvm/ArrayRef.h>
#include <llvm/raw_ostream.h>
#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<double> wheelSpeeds);
double m_deadband = 0.02;
double m_maxOutput = 1.0;
MotorSafetyHelper m_safetyHelper{this};
};
} // namespace frc

28
wpilibc/src/main/native/include/Drive/Vector2d.h Normal file
View File

@@ -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

5
wpilibc/src/main/native/include/RobotDrive.h
View File

@@ -10,6 +10,7 @@
#include <memory>
#include <llvm/raw_ostream.h>
#include <support/deprecated.h>
#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,

3
wpilibc/src/main/native/include/WPILib.h
View File

@@ -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"

3
wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java
View File

@@ -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;

286
wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java Normal file
View File

@@ -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.
*
* <p>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.
*
* <p>Four motor drivetrain:
* <pre><code>
* 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);
* }
* </code></pre>
*
* <p>Six motor drivetrain:
* <pre><code>
* 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);
* }
* </code></pre>
*
* <p>A differential drive robot has left and right wheels separated by an arbitrary width.
*
* <p>Drive base diagram:
* <pre>
* |_______|
* | | | |
* | |
* |_|___|_|
* | |
* </pre>
*
* <p>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.
*
* <p>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.
*
* <p>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";
}
}

181
wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java Normal file
View File

@@ -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.
*
* <p>Killough drives are triangular with one omni wheel on each corner.
*
* <p>Drive base diagram:
* <pre>
* /_____\
* / \ / \
* \ /
* ---
* </pre>
*
* <p>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.
*
* <p>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.
*
* <p>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.
*
* <p>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";
}
}

149
wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java Normal file
View File

@@ -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.
*
* <p>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.
*
* <p>Drive base diagram:
* <pre>
* \\_______/
* \\ | | /
* | |
* /_|___|_\\
* / \\
* </pre>
*
* <p>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.
*
* <p>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";
}
}

134
wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java Normal file
View File

@@ -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;
}
}
}
}

65
wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/Vector2d.java Normal file
View File

@@ -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();
}
}