mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-28 02:11:43 +00:00
[wpilib] Remove RobotDrive (#3295)
This has been deprecated for several years, and its functionality has been completely superseded by other drive classes (DifferentialDrive et al).
This commit is contained in:
Peter Johnson
@@ -1,444 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include "frc/RobotDrive.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
|
||||
#include <hal/FRCUsageReporting.h>
|
||||
|
||||
#include "frc/GenericHID.h"
|
||||
#include "frc/Joystick.h"
|
||||
#include "frc/Talon.h"
|
||||
#include "frc/Utility.h"
|
||||
#include "frc/WPIErrors.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
static std::shared_ptr<SpeedController> make_shared_nodelete(
|
||||
SpeedController* ptr) {
|
||||
return std::shared_ptr<SpeedController>(ptr, NullDeleter<SpeedController>());
|
||||
}
|
||||
|
||||
RobotDrive::RobotDrive(int leftMotorChannel, int rightMotorChannel) {
|
||||
InitRobotDrive();
|
||||
m_rearLeftMotor = std::make_shared<Talon>(leftMotorChannel);
|
||||
m_rearRightMotor = std::make_shared<Talon>(rightMotorChannel);
|
||||
SetLeftRightMotorOutputs(0.0, 0.0);
|
||||
}
|
||||
|
||||
RobotDrive::RobotDrive(int frontLeftMotor, int rearLeftMotor,
|
||||
int frontRightMotor, int rearRightMotor) {
|
||||
InitRobotDrive();
|
||||
m_rearLeftMotor = std::make_shared<Talon>(rearLeftMotor);
|
||||
m_rearRightMotor = std::make_shared<Talon>(rearRightMotor);
|
||||
m_frontLeftMotor = std::make_shared<Talon>(frontLeftMotor);
|
||||
m_frontRightMotor = std::make_shared<Talon>(frontRightMotor);
|
||||
SetLeftRightMotorOutputs(0.0, 0.0);
|
||||
}
|
||||
|
||||
RobotDrive::RobotDrive(SpeedController* leftMotor,
|
||||
SpeedController* rightMotor) {
|
||||
InitRobotDrive();
|
||||
if (leftMotor == nullptr || rightMotor == nullptr) {
|
||||
wpi_setWPIError(NullParameter);
|
||||
m_rearLeftMotor = m_rearRightMotor = nullptr;
|
||||
return;
|
||||
}
|
||||
m_rearLeftMotor = make_shared_nodelete(leftMotor);
|
||||
m_rearRightMotor = make_shared_nodelete(rightMotor);
|
||||
}
|
||||
|
||||
RobotDrive::RobotDrive(SpeedController& leftMotor,
|
||||
SpeedController& rightMotor) {
|
||||
InitRobotDrive();
|
||||
m_rearLeftMotor = make_shared_nodelete(&leftMotor);
|
||||
m_rearRightMotor = make_shared_nodelete(&rightMotor);
|
||||
}
|
||||
|
||||
RobotDrive::RobotDrive(std::shared_ptr<SpeedController> leftMotor,
|
||||
std::shared_ptr<SpeedController> rightMotor) {
|
||||
InitRobotDrive();
|
||||
if (leftMotor == nullptr || rightMotor == nullptr) {
|
||||
wpi_setWPIError(NullParameter);
|
||||
m_rearLeftMotor = m_rearRightMotor = nullptr;
|
||||
return;
|
||||
}
|
||||
m_rearLeftMotor = leftMotor;
|
||||
m_rearRightMotor = rightMotor;
|
||||
}
|
||||
|
||||
RobotDrive::RobotDrive(SpeedController* frontLeftMotor,
|
||||
SpeedController* rearLeftMotor,
|
||||
SpeedController* frontRightMotor,
|
||||
SpeedController* rearRightMotor) {
|
||||
InitRobotDrive();
|
||||
if (frontLeftMotor == nullptr || rearLeftMotor == nullptr ||
|
||||
frontRightMotor == nullptr || rearRightMotor == nullptr) {
|
||||
wpi_setWPIError(NullParameter);
|
||||
return;
|
||||
}
|
||||
m_frontLeftMotor = make_shared_nodelete(frontLeftMotor);
|
||||
m_rearLeftMotor = make_shared_nodelete(rearLeftMotor);
|
||||
m_frontRightMotor = make_shared_nodelete(frontRightMotor);
|
||||
m_rearRightMotor = make_shared_nodelete(rearRightMotor);
|
||||
}
|
||||
|
||||
RobotDrive::RobotDrive(SpeedController& frontLeftMotor,
|
||||
SpeedController& rearLeftMotor,
|
||||
SpeedController& frontRightMotor,
|
||||
SpeedController& rearRightMotor) {
|
||||
InitRobotDrive();
|
||||
m_frontLeftMotor = make_shared_nodelete(&frontLeftMotor);
|
||||
m_rearLeftMotor = make_shared_nodelete(&rearLeftMotor);
|
||||
m_frontRightMotor = make_shared_nodelete(&frontRightMotor);
|
||||
m_rearRightMotor = make_shared_nodelete(&rearRightMotor);
|
||||
}
|
||||
|
||||
RobotDrive::RobotDrive(std::shared_ptr<SpeedController> frontLeftMotor,
|
||||
std::shared_ptr<SpeedController> rearLeftMotor,
|
||||
std::shared_ptr<SpeedController> frontRightMotor,
|
||||
std::shared_ptr<SpeedController> rearRightMotor) {
|
||||
InitRobotDrive();
|
||||
if (frontLeftMotor == nullptr || rearLeftMotor == nullptr ||
|
||||
frontRightMotor == nullptr || rearRightMotor == nullptr) {
|
||||
wpi_setWPIError(NullParameter);
|
||||
return;
|
||||
}
|
||||
m_frontLeftMotor = frontLeftMotor;
|
||||
m_rearLeftMotor = rearLeftMotor;
|
||||
m_frontRightMotor = frontRightMotor;
|
||||
m_rearRightMotor = rearRightMotor;
|
||||
}
|
||||
|
||||
void RobotDrive::Drive(double outputMagnitude, double curve) {
|
||||
double leftOutput, rightOutput;
|
||||
static bool reported = false;
|
||||
if (!reported) {
|
||||
HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
|
||||
HALUsageReporting::kRobotDrive_ArcadeRatioCurve, GetNumMotors());
|
||||
reported = true;
|
||||
}
|
||||
|
||||
if (curve < 0) {
|
||||
double value = std::log(-curve);
|
||||
double ratio = (value - m_sensitivity) / (value + m_sensitivity);
|
||||
if (ratio == 0) {
|
||||
ratio = 0.0000000001;
|
||||
}
|
||||
leftOutput = outputMagnitude / ratio;
|
||||
rightOutput = outputMagnitude;
|
||||
} else if (curve > 0) {
|
||||
double value = std::log(curve);
|
||||
double ratio = (value - m_sensitivity) / (value + m_sensitivity);
|
||||
if (ratio == 0) {
|
||||
ratio = 0.0000000001;
|
||||
}
|
||||
leftOutput = outputMagnitude;
|
||||
rightOutput = outputMagnitude / ratio;
|
||||
} else {
|
||||
leftOutput = outputMagnitude;
|
||||
rightOutput = outputMagnitude;
|
||||
}
|
||||
SetLeftRightMotorOutputs(leftOutput, rightOutput);
|
||||
}
|
||||
|
||||
void RobotDrive::TankDrive(GenericHID* leftStick, GenericHID* rightStick,
|
||||
bool squaredInputs) {
|
||||
if (leftStick == nullptr || rightStick == nullptr) {
|
||||
wpi_setWPIError(NullParameter);
|
||||
return;
|
||||
}
|
||||
TankDrive(leftStick->GetY(), rightStick->GetY(), squaredInputs);
|
||||
}
|
||||
|
||||
void RobotDrive::TankDrive(GenericHID& leftStick, GenericHID& rightStick,
|
||||
bool squaredInputs) {
|
||||
TankDrive(leftStick.GetY(), rightStick.GetY(), squaredInputs);
|
||||
}
|
||||
|
||||
void RobotDrive::TankDrive(GenericHID* leftStick, int leftAxis,
|
||||
GenericHID* rightStick, int rightAxis,
|
||||
bool squaredInputs) {
|
||||
if (leftStick == nullptr || rightStick == nullptr) {
|
||||
wpi_setWPIError(NullParameter);
|
||||
return;
|
||||
}
|
||||
TankDrive(leftStick->GetRawAxis(leftAxis), rightStick->GetRawAxis(rightAxis),
|
||||
squaredInputs);
|
||||
}
|
||||
|
||||
void RobotDrive::TankDrive(GenericHID& leftStick, int leftAxis,
|
||||
GenericHID& rightStick, int rightAxis,
|
||||
bool squaredInputs) {
|
||||
TankDrive(leftStick.GetRawAxis(leftAxis), rightStick.GetRawAxis(rightAxis),
|
||||
squaredInputs);
|
||||
}
|
||||
|
||||
void RobotDrive::TankDrive(double leftValue, double rightValue,
|
||||
bool squaredInputs) {
|
||||
static bool reported = false;
|
||||
if (!reported) {
|
||||
HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
|
||||
HALUsageReporting::kRobotDrive_Tank, GetNumMotors());
|
||||
reported = true;
|
||||
}
|
||||
|
||||
leftValue = Limit(leftValue);
|
||||
rightValue = Limit(rightValue);
|
||||
|
||||
// square the inputs (while preserving the sign) to increase fine control
|
||||
// while permitting full power
|
||||
if (squaredInputs) {
|
||||
leftValue = std::copysign(leftValue * leftValue, leftValue);
|
||||
rightValue = std::copysign(rightValue * rightValue, rightValue);
|
||||
}
|
||||
|
||||
SetLeftRightMotorOutputs(leftValue, rightValue);
|
||||
}
|
||||
|
||||
void RobotDrive::ArcadeDrive(GenericHID* stick, bool squaredInputs) {
|
||||
// simply call the full-featured ArcadeDrive with the appropriate values
|
||||
ArcadeDrive(stick->GetY(), stick->GetX(), squaredInputs);
|
||||
}
|
||||
|
||||
void RobotDrive::ArcadeDrive(GenericHID& stick, bool squaredInputs) {
|
||||
// simply call the full-featured ArcadeDrive with the appropriate values
|
||||
ArcadeDrive(stick.GetY(), stick.GetX(), squaredInputs);
|
||||
}
|
||||
|
||||
void RobotDrive::ArcadeDrive(GenericHID* moveStick, int moveAxis,
|
||||
GenericHID* rotateStick, int rotateAxis,
|
||||
bool squaredInputs) {
|
||||
double moveValue = moveStick->GetRawAxis(moveAxis);
|
||||
double rotateValue = rotateStick->GetRawAxis(rotateAxis);
|
||||
|
||||
ArcadeDrive(moveValue, rotateValue, squaredInputs);
|
||||
}
|
||||
|
||||
void RobotDrive::ArcadeDrive(GenericHID& moveStick, int moveAxis,
|
||||
GenericHID& rotateStick, int rotateAxis,
|
||||
bool squaredInputs) {
|
||||
double moveValue = moveStick.GetRawAxis(moveAxis);
|
||||
double rotateValue = rotateStick.GetRawAxis(rotateAxis);
|
||||
|
||||
ArcadeDrive(moveValue, rotateValue, squaredInputs);
|
||||
}
|
||||
|
||||
void RobotDrive::ArcadeDrive(double moveValue, double rotateValue,
|
||||
bool squaredInputs) {
|
||||
static bool reported = false;
|
||||
if (!reported) {
|
||||
HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
|
||||
HALUsageReporting::kRobotDrive_ArcadeStandard, GetNumMotors());
|
||||
reported = true;
|
||||
}
|
||||
|
||||
// local variables to hold the computed PWM values for the motors
|
||||
double leftMotorOutput;
|
||||
double rightMotorOutput;
|
||||
|
||||
moveValue = Limit(moveValue);
|
||||
rotateValue = Limit(rotateValue);
|
||||
|
||||
// square the inputs (while preserving the sign) to increase fine control
|
||||
// while permitting full power
|
||||
if (squaredInputs) {
|
||||
moveValue = std::copysign(moveValue * moveValue, moveValue);
|
||||
rotateValue = std::copysign(rotateValue * rotateValue, rotateValue);
|
||||
}
|
||||
|
||||
if (moveValue > 0.0) {
|
||||
if (rotateValue > 0.0) {
|
||||
leftMotorOutput = moveValue - rotateValue;
|
||||
rightMotorOutput = std::max(moveValue, rotateValue);
|
||||
} else {
|
||||
leftMotorOutput = std::max(moveValue, -rotateValue);
|
||||
rightMotorOutput = moveValue + rotateValue;
|
||||
}
|
||||
} else {
|
||||
if (rotateValue > 0.0) {
|
||||
leftMotorOutput = -std::max(-moveValue, rotateValue);
|
||||
rightMotorOutput = moveValue + rotateValue;
|
||||
} else {
|
||||
leftMotorOutput = moveValue - rotateValue;
|
||||
rightMotorOutput = -std::max(-moveValue, -rotateValue);
|
||||
}
|
||||
}
|
||||
SetLeftRightMotorOutputs(leftMotorOutput, rightMotorOutput);
|
||||
}
|
||||
|
||||
void RobotDrive::MecanumDrive_Cartesian(double x, double y, double rotation,
|
||||
double gyroAngle) {
|
||||
static bool reported = false;
|
||||
if (!reported) {
|
||||
HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
|
||||
HALUsageReporting::kRobotDrive_MecanumCartesian, GetNumMotors());
|
||||
reported = true;
|
||||
}
|
||||
|
||||
double xIn = x;
|
||||
double yIn = y;
|
||||
// Negate y for the joystick.
|
||||
yIn = -yIn;
|
||||
// Compensate for gyro angle.
|
||||
RotateVector(xIn, yIn, gyroAngle);
|
||||
|
||||
double wheelSpeeds[kMaxNumberOfMotors];
|
||||
wheelSpeeds[kFrontLeftMotor] = xIn + yIn + rotation;
|
||||
wheelSpeeds[kFrontRightMotor] = -xIn + yIn - rotation;
|
||||
wheelSpeeds[kRearLeftMotor] = -xIn + yIn + rotation;
|
||||
wheelSpeeds[kRearRightMotor] = xIn + yIn - rotation;
|
||||
|
||||
Normalize(wheelSpeeds);
|
||||
|
||||
m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_maxOutput);
|
||||
m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_maxOutput);
|
||||
m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_maxOutput);
|
||||
m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_maxOutput);
|
||||
|
||||
Feed();
|
||||
}
|
||||
|
||||
void RobotDrive::MecanumDrive_Polar(double magnitude, double direction,
|
||||
double rotation) {
|
||||
static bool reported = false;
|
||||
if (!reported) {
|
||||
HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
|
||||
HALUsageReporting::kRobotDrive_MecanumPolar, GetNumMotors());
|
||||
reported = true;
|
||||
}
|
||||
|
||||
// Normalized for full power along the Cartesian axes.
|
||||
magnitude = Limit(magnitude) * std::sqrt(2.0);
|
||||
// The rollers are at 45 degree angles.
|
||||
double dirInRad = (direction + 45.0) * 3.14159 / 180.0;
|
||||
double cosD = std::cos(dirInRad);
|
||||
double sinD = std::sin(dirInRad);
|
||||
|
||||
double wheelSpeeds[kMaxNumberOfMotors];
|
||||
wheelSpeeds[kFrontLeftMotor] = sinD * magnitude + rotation;
|
||||
wheelSpeeds[kFrontRightMotor] = cosD * magnitude - rotation;
|
||||
wheelSpeeds[kRearLeftMotor] = cosD * magnitude + rotation;
|
||||
wheelSpeeds[kRearRightMotor] = sinD * magnitude - rotation;
|
||||
|
||||
Normalize(wheelSpeeds);
|
||||
|
||||
m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_maxOutput);
|
||||
m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_maxOutput);
|
||||
m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_maxOutput);
|
||||
m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_maxOutput);
|
||||
|
||||
Feed();
|
||||
}
|
||||
|
||||
void RobotDrive::HolonomicDrive(double magnitude, double direction,
|
||||
double rotation) {
|
||||
MecanumDrive_Polar(magnitude, direction, rotation);
|
||||
}
|
||||
|
||||
void RobotDrive::SetLeftRightMotorOutputs(double leftOutput,
|
||||
double rightOutput) {
|
||||
wpi_assert(m_rearLeftMotor != nullptr && m_rearRightMotor != nullptr);
|
||||
|
||||
if (m_frontLeftMotor != nullptr) {
|
||||
m_frontLeftMotor->Set(Limit(leftOutput) * m_maxOutput);
|
||||
}
|
||||
m_rearLeftMotor->Set(Limit(leftOutput) * m_maxOutput);
|
||||
|
||||
if (m_frontRightMotor != nullptr) {
|
||||
m_frontRightMotor->Set(-Limit(rightOutput) * m_maxOutput);
|
||||
}
|
||||
m_rearRightMotor->Set(-Limit(rightOutput) * m_maxOutput);
|
||||
|
||||
Feed();
|
||||
}
|
||||
|
||||
void RobotDrive::SetInvertedMotor(MotorType motor, bool isInverted) {
|
||||
if (motor < 0 || motor > 3) {
|
||||
wpi_setWPIError(InvalidMotorIndex);
|
||||
return;
|
||||
}
|
||||
switch (motor) {
|
||||
case kFrontLeftMotor:
|
||||
m_frontLeftMotor->SetInverted(isInverted);
|
||||
break;
|
||||
case kFrontRightMotor:
|
||||
m_frontRightMotor->SetInverted(isInverted);
|
||||
break;
|
||||
case kRearLeftMotor:
|
||||
m_rearLeftMotor->SetInverted(isInverted);
|
||||
break;
|
||||
case kRearRightMotor:
|
||||
m_rearRightMotor->SetInverted(isInverted);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void RobotDrive::SetSensitivity(double sensitivity) {
|
||||
m_sensitivity = sensitivity;
|
||||
}
|
||||
|
||||
void RobotDrive::SetMaxOutput(double maxOutput) {
|
||||
m_maxOutput = maxOutput;
|
||||
}
|
||||
|
||||
void RobotDrive::GetDescription(wpi::raw_ostream& desc) const {
|
||||
desc << "RobotDrive";
|
||||
}
|
||||
|
||||
void RobotDrive::StopMotor() {
|
||||
if (m_frontLeftMotor != nullptr) {
|
||||
m_frontLeftMotor->StopMotor();
|
||||
}
|
||||
if (m_frontRightMotor != nullptr) {
|
||||
m_frontRightMotor->StopMotor();
|
||||
}
|
||||
if (m_rearLeftMotor != nullptr) {
|
||||
m_rearLeftMotor->StopMotor();
|
||||
}
|
||||
if (m_rearRightMotor != nullptr) {
|
||||
m_rearRightMotor->StopMotor();
|
||||
}
|
||||
Feed();
|
||||
}
|
||||
|
||||
void RobotDrive::InitRobotDrive() {
|
||||
SetSafetyEnabled(true);
|
||||
}
|
||||
|
||||
double RobotDrive::Limit(double number) {
|
||||
if (number > 1.0) {
|
||||
return 1.0;
|
||||
}
|
||||
if (number < -1.0) {
|
||||
return -1.0;
|
||||
}
|
||||
return number;
|
||||
}
|
||||
|
||||
void RobotDrive::Normalize(double* wheelSpeeds) {
|
||||
double maxMagnitude = std::fabs(wheelSpeeds[0]);
|
||||
for (int i = 1; i < kMaxNumberOfMotors; i++) {
|
||||
double temp = std::fabs(wheelSpeeds[i]);
|
||||
if (maxMagnitude < temp) {
|
||||
maxMagnitude = temp;
|
||||
}
|
||||
}
|
||||
if (maxMagnitude > 1.0) {
|
||||
for (int i = 0; i < kMaxNumberOfMotors; i++) {
|
||||
wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void RobotDrive::RotateVector(double& x, double& y, double angle) {
|
||||
double cosA = std::cos(angle * (3.14159 / 180.0));
|
||||
double sinA = std::sin(angle * (3.14159 / 180.0));
|
||||
double xOut = x * cosA - y * sinA;
|
||||
double yOut = x * sinA + y * cosA;
|
||||
x = xOut;
|
||||
y = yOut;
|
||||
}
|
||||
@@ -1,461 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include <wpi/deprecated.h>
|
||||
#include <wpi/raw_ostream.h>
|
||||
|
||||
#include "frc/ErrorBase.h"
|
||||
#include "frc/MotorSafety.h"
|
||||
|
||||
namespace frc {
|
||||
|
||||
class SpeedController;
|
||||
class GenericHID;
|
||||
|
||||
/**
|
||||
* Utility class for handling Robot drive based on a definition of the motor
|
||||
* configuration.
|
||||
*
|
||||
* The robot drive class handles basic driving for a robot. Currently, 2 and 4
|
||||
* motor tank and mecanum drive trains are supported. In the future other drive
|
||||
* types like swerve might be implemented. Motor channel numbers are passed
|
||||
* 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.
|
||||
*
|
||||
*/
|
||||
class RobotDrive : public MotorSafety {
|
||||
public:
|
||||
enum MotorType {
|
||||
kFrontLeftMotor = 0,
|
||||
kFrontRightMotor = 1,
|
||||
kRearLeftMotor = 2,
|
||||
kRearRightMotor = 3
|
||||
};
|
||||
|
||||
/**
|
||||
* Constructor for RobotDrive with 2 motors specified with channel numbers.
|
||||
*
|
||||
* Set up parameters for a two wheel drive system where the
|
||||
* left and right motor pwm channels are specified in the call.
|
||||
* This call assumes Talons for controlling the motors.
|
||||
*
|
||||
* @param leftMotorChannel The PWM channel number that drives the left motor.
|
||||
* 0-9 are on-board, 10-19 are on the MXP port
|
||||
* @param rightMotorChannel The PWM channel number that drives the right
|
||||
* motor. 0-9 are on-board, 10-19 are on the MXP port
|
||||
*/
|
||||
WPI_DEPRECATED("Use DifferentialDrive or MecanumDrive classes instead.")
|
||||
RobotDrive(int leftMotorChannel, int rightMotorChannel);
|
||||
|
||||
/**
|
||||
* Constructor for RobotDrive with 4 motors specified with channel numbers.
|
||||
*
|
||||
* Set up parameters for a four wheel drive system where all four motor
|
||||
* pwm channels are specified in the call.
|
||||
* This call assumes Talons for controlling the motors.
|
||||
*
|
||||
* @param frontLeftMotor Front left motor channel number. 0-9 are on-board,
|
||||
* 10-19 are on the MXP port
|
||||
* @param rearLeftMotor Rear Left motor channel number. 0-9 are on-board,
|
||||
* 10-19 are on the MXP port
|
||||
* @param frontRightMotor Front right motor channel number. 0-9 are on-board,
|
||||
* 10-19 are on the MXP port
|
||||
* @param rearRightMotor Rear Right motor channel number. 0-9 are on-board,
|
||||
* 10-19 are on the MXP port
|
||||
*/
|
||||
WPI_DEPRECATED("Use DifferentialDrive or MecanumDrive classes instead.")
|
||||
RobotDrive(int frontLeftMotorChannel, int rearLeftMotorChannel,
|
||||
int frontRightMotorChannel, int rearRightMotorChannel);
|
||||
|
||||
/**
|
||||
* Constructor for RobotDrive with 2 motors specified as SpeedController
|
||||
* objects.
|
||||
*
|
||||
* The SpeedController version of the constructor enables programs to use the
|
||||
* RobotDrive classes with subclasses of the SpeedController objects, for
|
||||
* example, versions with ramping or reshaping of the curve to suit motor bias
|
||||
* or deadband elimination.
|
||||
*
|
||||
* @param leftMotor The left SpeedController object used to drive the robot.
|
||||
* @param rightMotor The right SpeedController object used to drive the robot.
|
||||
*/
|
||||
WPI_DEPRECATED("Use DifferentialDrive or MecanumDrive classes instead.")
|
||||
RobotDrive(SpeedController* leftMotor, SpeedController* rightMotor);
|
||||
|
||||
WPI_DEPRECATED("Use DifferentialDrive or MecanumDrive classes instead.")
|
||||
RobotDrive(SpeedController& leftMotor, SpeedController& rightMotor);
|
||||
|
||||
WPI_DEPRECATED("Use DifferentialDrive or MecanumDrive classes instead.")
|
||||
RobotDrive(std::shared_ptr<SpeedController> leftMotor,
|
||||
std::shared_ptr<SpeedController> rightMotor);
|
||||
|
||||
/**
|
||||
* Constructor for RobotDrive with 4 motors specified as SpeedController
|
||||
* objects.
|
||||
*
|
||||
* Speed controller input version of RobotDrive (see previous comments).
|
||||
*
|
||||
* @param frontLeftMotor The front left SpeedController object used to drive
|
||||
* the robot.
|
||||
* @param rearLeftMotor The back left SpeedController object used to drive
|
||||
* the robot.
|
||||
* @param frontRightMotor The front right SpeedController object used to drive
|
||||
* the robot.
|
||||
* @param rearRightMotor The back right SpeedController object used to drive
|
||||
* the robot.
|
||||
*/
|
||||
WPI_DEPRECATED("Use DifferentialDrive or MecanumDrive classes instead.")
|
||||
RobotDrive(SpeedController* frontLeftMotor, SpeedController* rearLeftMotor,
|
||||
SpeedController* frontRightMotor, SpeedController* rearRightMotor);
|
||||
|
||||
WPI_DEPRECATED("Use DifferentialDrive or MecanumDrive classes instead.")
|
||||
RobotDrive(SpeedController& frontLeftMotor, SpeedController& rearLeftMotor,
|
||||
SpeedController& frontRightMotor, SpeedController& rearRightMotor);
|
||||
|
||||
WPI_DEPRECATED("Use DifferentialDrive or MecanumDrive classes instead.")
|
||||
RobotDrive(std::shared_ptr<SpeedController> frontLeftMotor,
|
||||
std::shared_ptr<SpeedController> rearLeftMotor,
|
||||
std::shared_ptr<SpeedController> frontRightMotor,
|
||||
std::shared_ptr<SpeedController> rearRightMotor);
|
||||
|
||||
~RobotDrive() override = default;
|
||||
|
||||
RobotDrive(RobotDrive&&) = default;
|
||||
RobotDrive& operator=(RobotDrive&&) = default;
|
||||
|
||||
/**
|
||||
* Drive the motors at "outputMagnitude" and "curve".
|
||||
*
|
||||
* Both outputMagnitude and curve are -1.0 to +1.0 values, where 0.0
|
||||
* represents stopped and not turning. curve < 0 will turn left and curve > 0
|
||||
* will turn right.
|
||||
*
|
||||
* The algorithm for steering provides a constant turn radius for any normal
|
||||
* speed range, both forward and backward. Increasing m_sensitivity causes
|
||||
* sharper turns for fixed values of curve.
|
||||
*
|
||||
* This function will most likely be used in an autonomous routine.
|
||||
*
|
||||
* @param outputMagnitude The speed setting for the outside wheel in a turn,
|
||||
* forward or backwards, +1 to -1.
|
||||
* @param curve The rate of turn, constant for different forward
|
||||
* speeds. Set curve < 0 for left turn or curve > 0 for
|
||||
* right turn.
|
||||
*
|
||||
* Set curve = e^(-r/w) to get a turn radius r for wheelbase w of your robot.
|
||||
* Conversely, turn radius r = -ln(curve)*w for a given value of curve and
|
||||
* wheelbase w.
|
||||
*/
|
||||
void Drive(double outputMagnitude, double curve);
|
||||
|
||||
/**
|
||||
* Provide tank steering using the stored robot configuration.
|
||||
*
|
||||
* Drive the robot using two joystick inputs. The Y-axis will be selected from
|
||||
* each Joystick object.
|
||||
*
|
||||
* @param leftStick The joystick to control the left side of the robot.
|
||||
* @param rightStick The joystick to control the right side of the robot.
|
||||
* @param squaredInputs If true, the sensitivity will be decreased for small
|
||||
* values
|
||||
*/
|
||||
void TankDrive(GenericHID* leftStick, GenericHID* rightStick,
|
||||
bool squaredInputs = true);
|
||||
|
||||
/**
|
||||
* Provide tank steering using the stored robot configuration.
|
||||
*
|
||||
* Drive the robot using two joystick inputs. The Y-axis will be selected from
|
||||
* each Joystick object.
|
||||
*
|
||||
* @param leftStick The joystick to control the left side of the robot.
|
||||
* @param rightStick The joystick to control the right side of the robot.
|
||||
* @param squaredInputs If true, the sensitivity will be decreased for small
|
||||
* values
|
||||
*/
|
||||
void TankDrive(GenericHID& leftStick, GenericHID& rightStick,
|
||||
bool squaredInputs = true);
|
||||
|
||||
/**
|
||||
* Provide tank steering using the stored robot configuration.
|
||||
*
|
||||
* This function lets you pick the axis to be used on each Joystick object for
|
||||
* the left and right sides of the robot.
|
||||
*
|
||||
* @param leftStick The Joystick object to use for the left side of the
|
||||
* robot.
|
||||
* @param leftAxis The axis to select on the left side Joystick object.
|
||||
* @param rightStick The Joystick object to use for the right side of the
|
||||
* robot.
|
||||
* @param rightAxis The axis to select on the right side Joystick object.
|
||||
* @param squaredInputs If true, the sensitivity will be decreased for small
|
||||
* values
|
||||
*/
|
||||
void TankDrive(GenericHID* leftStick, int leftAxis, GenericHID* rightStick,
|
||||
int rightAxis, bool squaredInputs = true);
|
||||
|
||||
void TankDrive(GenericHID& leftStick, int leftAxis, GenericHID& rightStick,
|
||||
int rightAxis, bool squaredInputs = true);
|
||||
|
||||
/**
|
||||
* Provide tank steering using the stored robot configuration.
|
||||
*
|
||||
* This function lets you directly provide joystick values from any source.
|
||||
*
|
||||
* @param leftValue The value of the left stick.
|
||||
* @param rightValue The value of the right stick.
|
||||
* @param squaredInputs If true, the sensitivity will be decreased for small
|
||||
* values
|
||||
*/
|
||||
void TankDrive(double leftValue, double rightValue,
|
||||
bool squaredInputs = true);
|
||||
|
||||
/**
|
||||
* Arcade drive implements single stick driving.
|
||||
*
|
||||
* Given a single Joystick, the class assumes the Y axis for the move value
|
||||
* and the X axis for the rotate value. (Should add more information here
|
||||
* regarding the way that arcade drive works.)
|
||||
*
|
||||
* @param stick The joystick to use for Arcade single-stick driving.
|
||||
* The Y-axis will be selected for forwards/backwards and
|
||||
* the X-axis will be selected for rotation rate.
|
||||
* @param squaredInputs If true, the sensitivity will be decreased for small
|
||||
* values
|
||||
*/
|
||||
void ArcadeDrive(GenericHID* stick, bool squaredInputs = true);
|
||||
|
||||
/**
|
||||
* Arcade drive implements single stick driving.
|
||||
*
|
||||
* Given a single Joystick, the class assumes the Y axis for the move value
|
||||
* and the X axis for the rotate value. (Should add more information here
|
||||
* regarding the way that arcade drive works.)
|
||||
*
|
||||
* @param stick The joystick to use for Arcade single-stick driving.
|
||||
* The Y-axis will be selected for forwards/backwards and
|
||||
* the X-axis will be selected for rotation rate.
|
||||
* @param squaredInputs If true, the sensitivity will be decreased for small
|
||||
* values
|
||||
*/
|
||||
void ArcadeDrive(GenericHID& stick, bool squaredInputs = true);
|
||||
|
||||
/**
|
||||
* Arcade drive implements single stick driving.
|
||||
*
|
||||
* Given two joystick instances and two axis, compute the values to send to
|
||||
* either two or four motors.
|
||||
*
|
||||
* @param moveStick The Joystick object that represents the
|
||||
* forward/backward direction
|
||||
* @param moveAxis The axis on the moveStick object to use for
|
||||
* forwards/backwards (typically Y_AXIS)
|
||||
* @param rotateStick The Joystick object that represents the rotation value
|
||||
* @param rotateAxis The axis on the rotation object to use for the rotate
|
||||
* right/left (typically X_AXIS)
|
||||
* @param squaredInputs Setting this parameter to true increases the
|
||||
* sensitivity at lower speeds
|
||||
*/
|
||||
void ArcadeDrive(GenericHID* moveStick, int moveChannel,
|
||||
GenericHID* rotateStick, int rotateChannel,
|
||||
bool squaredInputs = true);
|
||||
|
||||
/**
|
||||
* Arcade drive implements single stick driving.
|
||||
*
|
||||
* Given two joystick instances and two axis, compute the values to send to
|
||||
* either two or four motors.
|
||||
*
|
||||
* @param moveStick The Joystick object that represents the
|
||||
* forward/backward direction
|
||||
* @param moveAxis The axis on the moveStick object to use for
|
||||
* forwards/backwards (typically Y_AXIS)
|
||||
* @param rotateStick The Joystick object that represents the rotation value
|
||||
* @param rotateAxis The axis on the rotation object to use for the rotate
|
||||
* right/left (typically X_AXIS)
|
||||
* @param squaredInputs Setting this parameter to true increases the
|
||||
* sensitivity at lower speeds
|
||||
*/
|
||||
void ArcadeDrive(GenericHID& moveStick, int moveChannel,
|
||||
GenericHID& rotateStick, int rotateChannel,
|
||||
bool squaredInputs = true);
|
||||
|
||||
/**
|
||||
* Arcade drive implements single stick driving.
|
||||
*
|
||||
* This function lets you directly provide joystick values from any source.
|
||||
*
|
||||
* @param moveValue The value to use for fowards/backwards
|
||||
* @param rotateValue The value to use for the rotate right/left
|
||||
* @param squaredInputs If set, increases the sensitivity at low speeds
|
||||
*/
|
||||
void ArcadeDrive(double moveValue, double rotateValue,
|
||||
bool squaredInputs = true);
|
||||
|
||||
/**
|
||||
* Drive method for Mecanum wheeled robots.
|
||||
*
|
||||
* A method for driving with Mecanum wheeled robots. There are 4 wheels
|
||||
* on the robot, arranged so that the front and back wheels are toed in 45
|
||||
* degrees.
|
||||
* When looking at the wheels from the top, the roller axles should form an X
|
||||
* across the robot.
|
||||
*
|
||||
* This is designed to be directly driven by joystick axes.
|
||||
*
|
||||
* @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.
|
||||
* This input is inverted to match the forward == -1.0 that
|
||||
* joysticks produce. [-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_Cartesian(double x, double y, double rotation,
|
||||
double gyroAngle = 0.0);
|
||||
|
||||
/**
|
||||
* Drive method for Mecanum wheeled robots.
|
||||
*
|
||||
* A method for driving with Mecanum wheeled robots. There are 4 wheels
|
||||
* on the robot, arranged so that the front and back wheels are toed in 45
|
||||
* degrees.
|
||||
* When looking at the wheels from the top, the roller axles should form an X
|
||||
* across the robot.
|
||||
*
|
||||
* @param magnitude The speed that the robot should drive in a given
|
||||
* direction. [-1.0..1.0]
|
||||
* @param direction The direction the robot should drive in degrees. The
|
||||
* direction and maginitute are independent of the rotation
|
||||
* rate.
|
||||
* @param rotation The rate of rotation for the robot that is completely
|
||||
* independent of the magnitute or direction. [-1.0..1.0]
|
||||
*/
|
||||
void MecanumDrive_Polar(double magnitude, double direction, double rotation);
|
||||
|
||||
/**
|
||||
* Holonomic Drive method for Mecanum wheeled robots.
|
||||
*
|
||||
* This is an alias to MecanumDrive_Polar() for backward compatibility
|
||||
*
|
||||
* @param magnitude The speed that the robot should drive in a given
|
||||
* direction. [-1.0..1.0]
|
||||
* @param direction The direction the robot should drive. The direction and
|
||||
* magnitude 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 HolonomicDrive(double magnitude, double direction, double rotation);
|
||||
|
||||
/**
|
||||
* Set the speed of the right and left motors.
|
||||
*
|
||||
* This is used once an appropriate drive setup function is called such as
|
||||
* TwoWheelDrive(). The motors are set to "leftOutput" and "rightOutput"
|
||||
* and includes flipping the direction of one side for opposing motors.
|
||||
*
|
||||
* @param leftOutput The speed to send to the left side of the robot.
|
||||
* @param rightOutput The speed to send to the right side of the robot.
|
||||
*/
|
||||
virtual void SetLeftRightMotorOutputs(double leftOutput, double rightOutput);
|
||||
|
||||
/*
|
||||
* Invert a motor direction.
|
||||
*
|
||||
* This is used when a motor should run in the opposite direction as the drive
|
||||
* code would normally run it. Motors that are direct drive would be inverted,
|
||||
* the Drive code assumes that the motors are geared with one reversal.
|
||||
*
|
||||
* @param motor The motor index to invert.
|
||||
* @param isInverted True if the motor should be inverted when operated.
|
||||
*/
|
||||
void SetInvertedMotor(MotorType motor, bool isInverted);
|
||||
|
||||
/**
|
||||
* Set the turning sensitivity.
|
||||
*
|
||||
* This only impacts the Drive() entry-point.
|
||||
*
|
||||
* @param sensitivity Effectively sets the turning sensitivity (or turn radius
|
||||
* for a given value)
|
||||
*/
|
||||
void SetSensitivity(double sensitivity);
|
||||
|
||||
/**
|
||||
* 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 SetMaxOutput(double maxOutput);
|
||||
|
||||
void StopMotor() override;
|
||||
void GetDescription(wpi::raw_ostream& desc) const override;
|
||||
|
||||
protected:
|
||||
/**
|
||||
* Common function to initialize all the robot drive constructors.
|
||||
*
|
||||
* Create a motor safety object (the real reason for the common code) and
|
||||
* initialize all the motor assignments. The default timeout is set for the
|
||||
* robot drive.
|
||||
*/
|
||||
void InitRobotDrive();
|
||||
|
||||
/**
|
||||
* Limit motor values to the -1.0 to +1.0 range.
|
||||
*/
|
||||
double Limit(double number);
|
||||
|
||||
/**
|
||||
* Normalize all wheel speeds if the magnitude of any wheel is greater than
|
||||
* 1.0.
|
||||
*/
|
||||
void Normalize(double* wheelSpeeds);
|
||||
|
||||
/**
|
||||
* Rotate a vector in Cartesian space.
|
||||
*/
|
||||
void RotateVector(double& x, double& y, double angle);
|
||||
|
||||
static constexpr int kMaxNumberOfMotors = 4;
|
||||
|
||||
double m_sensitivity = 0.5;
|
||||
double m_maxOutput = 1.0;
|
||||
|
||||
std::shared_ptr<SpeedController> m_frontLeftMotor;
|
||||
std::shared_ptr<SpeedController> m_frontRightMotor;
|
||||
std::shared_ptr<SpeedController> m_rearLeftMotor;
|
||||
std::shared_ptr<SpeedController> m_rearRightMotor;
|
||||
|
||||
private:
|
||||
int GetNumMotors() {
|
||||
int motors = 0;
|
||||
if (m_frontLeftMotor) {
|
||||
motors++;
|
||||
}
|
||||
if (m_frontRightMotor) {
|
||||
motors++;
|
||||
}
|
||||
if (m_rearLeftMotor) {
|
||||
motors++;
|
||||
}
|
||||
if (m_rearRightMotor) {
|
||||
motors++;
|
||||
}
|
||||
return motors;
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
@@ -87,12 +87,12 @@ class SpeedController;
|
||||
*
|
||||
* <p>RobotDrive porting guide:
|
||||
* <br>TankDrive(double, double, bool) is equivalent to
|
||||
* RobotDrive#TankDrive(double, double, bool) if a deadband of 0 is used.
|
||||
* RobotDrive's TankDrive(double, double, bool) if a deadband of 0 is used.
|
||||
* <br>ArcadeDrive(double, double, bool) is equivalent to
|
||||
* RobotDrive#ArcadeDrive(double, double, bool) if a deadband of 0 is used
|
||||
* RobotDrive's ArcadeDrive(double, double, bool) if a deadband of 0 is used
|
||||
* and the the rotation input is inverted eg ArcadeDrive(y, -rotation, false)
|
||||
* <br>CurvatureDrive(double, double, bool) is similar in concept to
|
||||
* RobotDrive#Drive(double, double) with the addition of a quick turn
|
||||
* RobotDrive's Drive(double, double) with the addition of a quick turn
|
||||
* mode. However, it is not designed to give exactly the same response.
|
||||
*/
|
||||
class DifferentialDrive : public RobotDriveBase,
|
||||
|
||||
@@ -53,12 +53,12 @@ class SpeedController;
|
||||
* inverted, while in RobotDrive, no speed controllers are automatically
|
||||
* inverted.
|
||||
* <br>DriveCartesian(double, double, double, double) is equivalent to
|
||||
* RobotDrive#MecanumDrive_Cartesian(double, double, double, double)
|
||||
* RobotDrive's MecanumDrive_Cartesian(double, double, double, double)
|
||||
* if a deadband of 0 is used, and the ySpeed and gyroAngle values are inverted
|
||||
* compared to RobotDrive (eg DriveCartesian(xSpeed, -ySpeed, zRotation,
|
||||
* -gyroAngle).
|
||||
* <br>DrivePolar(double, double, double) is equivalent to
|
||||
* RobotDrive#MecanumDrive_Polar(double, double, double) if a
|
||||
* RobotDrive's MecanumDrive_Polar(double, double, double) if a
|
||||
* deadband of 0 is used.
|
||||
*/
|
||||
class MecanumDrive : public RobotDriveBase,
|
||||
|
||||
@@ -1,187 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include "MockSpeedController.h"
|
||||
#include "frc/RobotDrive.h"
|
||||
#include "frc/drive/DifferentialDrive.h"
|
||||
#include "frc/drive/MecanumDrive.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
class DriveTest : public testing::Test {
|
||||
protected:
|
||||
MockSpeedController m_rdFrontLeft;
|
||||
MockSpeedController m_rdRearLeft;
|
||||
MockSpeedController m_rdFrontRight;
|
||||
MockSpeedController m_rdRearRight;
|
||||
MockSpeedController m_frontLeft;
|
||||
MockSpeedController m_rearLeft;
|
||||
MockSpeedController m_frontRight;
|
||||
MockSpeedController m_rearRight;
|
||||
frc::RobotDrive m_robotDrive{m_rdFrontLeft, m_rdRearLeft, m_rdFrontRight,
|
||||
m_rdRearRight};
|
||||
frc::DifferentialDrive m_differentialDrive{m_frontLeft, m_frontRight};
|
||||
frc::MecanumDrive m_mecanumDrive{m_frontLeft, m_rearLeft, m_frontRight,
|
||||
m_rearRight};
|
||||
|
||||
double m_testJoystickValues[9] = {-1.0, -0.9, -0.5, -0.01, 0.0,
|
||||
0.01, 0.5, 0.9, 1.0};
|
||||
double m_testGyroValues[19] = {0, 45, 90, 135, 180, 225, 270,
|
||||
305, 360, 540, -45, -90, -135, -180,
|
||||
-225, -270, -305, -360, -540};
|
||||
};
|
||||
|
||||
TEST_F(DriveTest, TankDrive) {
|
||||
int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
|
||||
double leftJoystick, rightJoystick;
|
||||
m_differentialDrive.SetDeadband(0.0);
|
||||
m_differentialDrive.SetSafetyEnabled(false);
|
||||
m_mecanumDrive.SetSafetyEnabled(false);
|
||||
m_robotDrive.SetSafetyEnabled(false);
|
||||
for (int i = 0; i < joystickSize; i++) {
|
||||
for (int j = 0; j < joystickSize; j++) {
|
||||
leftJoystick = m_testJoystickValues[i];
|
||||
rightJoystick = m_testJoystickValues[j];
|
||||
m_robotDrive.TankDrive(leftJoystick, rightJoystick, false);
|
||||
m_differentialDrive.TankDrive(leftJoystick, rightJoystick, false);
|
||||
ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01);
|
||||
ASSERT_NEAR(m_rdFrontRight.Get(), m_frontRight.Get(), 0.01);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(DriveTest, TankDriveSquared) {
|
||||
int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
|
||||
double leftJoystick, rightJoystick;
|
||||
m_differentialDrive.SetDeadband(0.0);
|
||||
m_differentialDrive.SetSafetyEnabled(false);
|
||||
m_mecanumDrive.SetSafetyEnabled(false);
|
||||
m_robotDrive.SetSafetyEnabled(false);
|
||||
for (int i = 0; i < joystickSize; i++) {
|
||||
for (int j = 0; j < joystickSize; j++) {
|
||||
leftJoystick = m_testJoystickValues[i];
|
||||
rightJoystick = m_testJoystickValues[j];
|
||||
m_robotDrive.TankDrive(leftJoystick, rightJoystick, true);
|
||||
m_differentialDrive.TankDrive(leftJoystick, rightJoystick, true);
|
||||
ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01);
|
||||
ASSERT_NEAR(m_rdFrontRight.Get(), m_frontRight.Get(), 0.01);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(DriveTest, ArcadeDriveSquared) {
|
||||
int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
|
||||
double moveJoystick, rotateJoystick;
|
||||
m_differentialDrive.SetDeadband(0.0);
|
||||
m_differentialDrive.SetSafetyEnabled(false);
|
||||
m_mecanumDrive.SetSafetyEnabled(false);
|
||||
m_robotDrive.SetSafetyEnabled(false);
|
||||
for (int i = 0; i < joystickSize; i++) {
|
||||
for (int j = 0; j < joystickSize; j++) {
|
||||
moveJoystick = m_testJoystickValues[i];
|
||||
rotateJoystick = m_testJoystickValues[j];
|
||||
m_robotDrive.ArcadeDrive(moveJoystick, rotateJoystick, true);
|
||||
m_differentialDrive.ArcadeDrive(moveJoystick, -rotateJoystick, true);
|
||||
ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01);
|
||||
ASSERT_NEAR(m_rdFrontRight.Get(), m_frontRight.Get(), 0.01);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(DriveTest, ArcadeDrive) {
|
||||
int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
|
||||
double moveJoystick, rotateJoystick;
|
||||
m_differentialDrive.SetDeadband(0.0);
|
||||
m_differentialDrive.SetSafetyEnabled(false);
|
||||
m_mecanumDrive.SetSafetyEnabled(false);
|
||||
m_robotDrive.SetSafetyEnabled(false);
|
||||
for (int i = 0; i < joystickSize; i++) {
|
||||
for (int j = 0; j < joystickSize; j++) {
|
||||
moveJoystick = m_testJoystickValues[i];
|
||||
rotateJoystick = m_testJoystickValues[j];
|
||||
m_robotDrive.ArcadeDrive(moveJoystick, rotateJoystick, false);
|
||||
m_differentialDrive.ArcadeDrive(moveJoystick, -rotateJoystick, false);
|
||||
ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01);
|
||||
ASSERT_NEAR(m_rdFrontRight.Get(), m_frontRight.Get(), 0.01);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(DriveTest, MecanumCartesian) {
|
||||
int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
|
||||
int gyroSize = sizeof(m_testGyroValues) / sizeof(double);
|
||||
double xJoystick, yJoystick, rotateJoystick, gyroValue;
|
||||
m_mecanumDrive.SetDeadband(0.0);
|
||||
m_mecanumDrive.SetSafetyEnabled(false);
|
||||
m_differentialDrive.SetSafetyEnabled(false);
|
||||
m_robotDrive.SetSafetyEnabled(false);
|
||||
for (int i = 0; i < joystickSize; i++) {
|
||||
for (int j = 0; j < joystickSize; j++) {
|
||||
for (int k = 0; k < joystickSize; k++) {
|
||||
for (int l = 0; l < gyroSize; l++) {
|
||||
xJoystick = m_testJoystickValues[i];
|
||||
yJoystick = m_testJoystickValues[j];
|
||||
rotateJoystick = m_testJoystickValues[k];
|
||||
gyroValue = m_testGyroValues[l];
|
||||
m_robotDrive.MecanumDrive_Cartesian(xJoystick, yJoystick,
|
||||
rotateJoystick, gyroValue);
|
||||
m_mecanumDrive.DriveCartesian(xJoystick, -yJoystick, rotateJoystick,
|
||||
-gyroValue);
|
||||
ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01)
|
||||
<< "X: " << xJoystick << " Y: " << yJoystick
|
||||
<< " Rotate: " << rotateJoystick << " Gyro: " << gyroValue;
|
||||
ASSERT_NEAR(m_rdFrontRight.Get(), -m_frontRight.Get(), 0.01)
|
||||
<< "X: " << xJoystick << " Y: " << yJoystick
|
||||
<< " Rotate: " << rotateJoystick << " Gyro: " << gyroValue;
|
||||
ASSERT_NEAR(m_rdRearLeft.Get(), m_rearLeft.Get(), 0.01)
|
||||
<< "X: " << xJoystick << " Y: " << yJoystick
|
||||
<< " Rotate: " << rotateJoystick << " Gyro: " << gyroValue;
|
||||
ASSERT_NEAR(m_rdRearRight.Get(), -m_rearRight.Get(), 0.01)
|
||||
<< "X: " << xJoystick << " Y: " << yJoystick
|
||||
<< " Rotate: " << rotateJoystick << " Gyro: " << gyroValue;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(DriveTest, MecanumPolar) {
|
||||
int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
|
||||
int gyroSize = sizeof(m_testGyroValues) / sizeof(double);
|
||||
double magnitudeJoystick, directionJoystick, rotateJoystick;
|
||||
m_mecanumDrive.SetDeadband(0.0);
|
||||
m_mecanumDrive.SetSafetyEnabled(false);
|
||||
m_differentialDrive.SetSafetyEnabled(false);
|
||||
m_robotDrive.SetSafetyEnabled(false);
|
||||
for (int i = 0; i < joystickSize; i++) {
|
||||
for (int j = 0; j < gyroSize; j++) {
|
||||
for (int k = 0; k < joystickSize; k++) {
|
||||
magnitudeJoystick = m_testJoystickValues[i];
|
||||
directionJoystick = m_testGyroValues[j];
|
||||
rotateJoystick = m_testJoystickValues[k];
|
||||
m_robotDrive.MecanumDrive_Polar(magnitudeJoystick, directionJoystick,
|
||||
rotateJoystick);
|
||||
m_mecanumDrive.DrivePolar(magnitudeJoystick, directionJoystick,
|
||||
rotateJoystick);
|
||||
ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01)
|
||||
<< "Magnitude: " << magnitudeJoystick
|
||||
<< " Direction: " << directionJoystick
|
||||
<< " Rotate: " << rotateJoystick;
|
||||
ASSERT_NEAR(m_rdFrontRight.Get(), -m_frontRight.Get(), 0.01)
|
||||
<< "Magnitude: " << magnitudeJoystick
|
||||
<< " Direction: " << directionJoystick
|
||||
<< " Rotate: " << rotateJoystick;
|
||||
ASSERT_NEAR(m_rdRearLeft.Get(), m_rearLeft.Get(), 0.01)
|
||||
<< "Magnitude: " << magnitudeJoystick
|
||||
<< " Direction: " << directionJoystick
|
||||
<< " Rotate: " << rotateJoystick;
|
||||
ASSERT_NEAR(m_rdRearRight.Get(), -m_rearRight.Get(), 0.01)
|
||||
<< "Magnitude: " << magnitudeJoystick
|
||||
<< " Direction: " << directionJoystick
|
||||
<< " Rotate: " << rotateJoystick;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,719 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.wpilibj;
|
||||
|
||||
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
|
||||
|
||||
import edu.wpi.first.hal.FRCNetComm.tInstances;
|
||||
import edu.wpi.first.hal.FRCNetComm.tResourceType;
|
||||
import edu.wpi.first.hal.HAL;
|
||||
|
||||
/**
|
||||
* Utility class for handling Robot drive based on a definition of the motor configuration. The
|
||||
* robot drive class handles basic driving for a robot. Currently, 2 and 4 motor tank and mecanum
|
||||
* drive trains are supported. In the future other drive types like swerve might be implemented.
|
||||
* 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 {@link edu.wpi.first.wpilibj.drive.DifferentialDrive} or {@link
|
||||
* edu.wpi.first.wpilibj.drive.MecanumDrive} classes instead.
|
||||
*/
|
||||
@Deprecated
|
||||
@SuppressWarnings("PMD.GodClass")
|
||||
public class RobotDrive extends MotorSafety implements AutoCloseable {
|
||||
/** The location of a motor on the robot for the purpose of driving. */
|
||||
public enum MotorType {
|
||||
kFrontLeft(0),
|
||||
kFrontRight(1),
|
||||
kRearLeft(2),
|
||||
kRearRight(3);
|
||||
|
||||
public final int value;
|
||||
|
||||
MotorType(int value) {
|
||||
this.value = value;
|
||||
}
|
||||
}
|
||||
|
||||
public static final double kDefaultExpirationTime = 0.1;
|
||||
public static final double kDefaultSensitivity = 0.5;
|
||||
public static final double kDefaultMaxOutput = 1.0;
|
||||
protected static final int kMaxNumberOfMotors = 4;
|
||||
protected double m_sensitivity;
|
||||
protected double m_maxOutput;
|
||||
protected SpeedController m_frontLeftMotor;
|
||||
protected SpeedController m_frontRightMotor;
|
||||
protected SpeedController m_rearLeftMotor;
|
||||
protected SpeedController m_rearRightMotor;
|
||||
protected boolean m_allocatedSpeedControllers;
|
||||
protected static boolean kArcadeRatioCurve_Reported;
|
||||
protected static boolean kTank_Reported;
|
||||
protected static boolean kArcadeStandard_Reported;
|
||||
protected static boolean kMecanumCartesian_Reported;
|
||||
protected static boolean kMecanumPolar_Reported;
|
||||
|
||||
/**
|
||||
* Constructor for RobotDrive with 2 motors specified with channel numbers. Set up parameters for
|
||||
* a two wheel drive system where the left and right motor pwm channels are specified in the call.
|
||||
* This call assumes Talons for controlling the motors.
|
||||
*
|
||||
* @param leftMotorChannel The PWM channel number that drives the left motor.
|
||||
* @param rightMotorChannel The PWM channel number that drives the right motor.
|
||||
*/
|
||||
public RobotDrive(final int leftMotorChannel, final int rightMotorChannel) {
|
||||
m_sensitivity = kDefaultSensitivity;
|
||||
m_maxOutput = kDefaultMaxOutput;
|
||||
m_frontLeftMotor = null;
|
||||
m_rearLeftMotor = new Talon(leftMotorChannel);
|
||||
m_frontRightMotor = null;
|
||||
m_rearRightMotor = new Talon(rightMotorChannel);
|
||||
m_allocatedSpeedControllers = true;
|
||||
setSafetyEnabled(true);
|
||||
drive(0, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructor for RobotDrive with 4 motors specified with channel numbers. Set up parameters for
|
||||
* a four wheel drive system where all four motor pwm channels are specified in the call. This
|
||||
* call assumes Talons for controlling the motors.
|
||||
*
|
||||
* @param frontLeftMotor Front left motor channel number
|
||||
* @param rearLeftMotor Rear Left motor channel number
|
||||
* @param frontRightMotor Front right motor channel number
|
||||
* @param rearRightMotor Rear Right motor channel number
|
||||
*/
|
||||
public RobotDrive(
|
||||
final int frontLeftMotor,
|
||||
final int rearLeftMotor,
|
||||
final int frontRightMotor,
|
||||
final int rearRightMotor) {
|
||||
m_sensitivity = kDefaultSensitivity;
|
||||
m_maxOutput = kDefaultMaxOutput;
|
||||
m_rearLeftMotor = new Talon(rearLeftMotor);
|
||||
m_rearRightMotor = new Talon(rearRightMotor);
|
||||
m_frontLeftMotor = new Talon(frontLeftMotor);
|
||||
m_frontRightMotor = new Talon(frontRightMotor);
|
||||
m_allocatedSpeedControllers = true;
|
||||
setSafetyEnabled(true);
|
||||
drive(0, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructor for RobotDrive with 2 motors specified as SpeedController objects. The
|
||||
* SpeedController version of the constructor enables programs to use the RobotDrive classes with
|
||||
* subclasses of the SpeedController objects, for example, versions with ramping or reshaping of
|
||||
* the curve to suit motor bias or dead-band elimination.
|
||||
*
|
||||
* @param leftMotor The left SpeedController object used to drive the robot.
|
||||
* @param rightMotor the right SpeedController object used to drive the robot.
|
||||
*/
|
||||
public RobotDrive(SpeedController leftMotor, SpeedController rightMotor) {
|
||||
requireNonNullParam(leftMotor, "leftMotor", "RobotDrive");
|
||||
requireNonNullParam(rightMotor, "rightMotor", "RobotDrive");
|
||||
|
||||
m_frontLeftMotor = null;
|
||||
m_rearLeftMotor = leftMotor;
|
||||
m_frontRightMotor = null;
|
||||
m_rearRightMotor = rightMotor;
|
||||
m_sensitivity = kDefaultSensitivity;
|
||||
m_maxOutput = kDefaultMaxOutput;
|
||||
m_allocatedSpeedControllers = false;
|
||||
setSafetyEnabled(true);
|
||||
drive(0, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructor for RobotDrive with 4 motors specified as SpeedController objects. Speed controller
|
||||
* input version of RobotDrive (see previous comments).
|
||||
*
|
||||
* @param frontLeftMotor The front left SpeedController object used to drive the robot
|
||||
* @param rearLeftMotor The back left SpeedController object used to drive the robot.
|
||||
* @param frontRightMotor The front right SpeedController object used to drive the robot.
|
||||
* @param rearRightMotor The back right SpeedController object used to drive the robot.
|
||||
*/
|
||||
public RobotDrive(
|
||||
SpeedController frontLeftMotor,
|
||||
SpeedController rearLeftMotor,
|
||||
SpeedController frontRightMotor,
|
||||
SpeedController rearRightMotor) {
|
||||
m_frontLeftMotor = requireNonNullParam(frontLeftMotor, "frontLeftMotor", "RobotDrive");
|
||||
m_rearLeftMotor = requireNonNullParam(rearLeftMotor, "rearLeftMotor", "RobotDrive");
|
||||
m_frontRightMotor = requireNonNullParam(frontRightMotor, "frontRightMotor", "RobotDrive");
|
||||
m_rearRightMotor = requireNonNullParam(rearRightMotor, "rearRightMotor", "RobotDrive");
|
||||
m_sensitivity = kDefaultSensitivity;
|
||||
m_maxOutput = kDefaultMaxOutput;
|
||||
m_allocatedSpeedControllers = false;
|
||||
setSafetyEnabled(true);
|
||||
drive(0, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Drive the motors at "outputMagnitude" and "curve". Both outputMagnitude and curve are -1.0 to
|
||||
* +1.0 values, where 0.0 represents stopped and not turning. {@literal curve < 0 will turn left
|
||||
* and curve > 0} will turn right.
|
||||
*
|
||||
* <p>The algorithm for steering provides a constant turn radius for any normal speed range, both
|
||||
* forward and backward. Increasing sensitivity causes sharper turns for fixed values of curve.
|
||||
*
|
||||
* <p>This function will most likely be used in an autonomous routine.
|
||||
*
|
||||
* @param outputMagnitude The speed setting for the outside wheel in a turn, forward or backwards,
|
||||
* +1 to -1.
|
||||
* @param curve The rate of turn, constant for different forward speeds. Set {@literal curve < 0
|
||||
* for left turn or curve > 0 for right turn.} Set curve = e^(-r/w) to get a turn radius r for
|
||||
* wheelbase w of your robot. Conversely, turn radius r = -ln(curve)*w for a given value of
|
||||
* curve and wheelbase w.
|
||||
*/
|
||||
public void drive(double outputMagnitude, double curve) {
|
||||
final double leftOutput;
|
||||
final double rightOutput;
|
||||
|
||||
if (!kArcadeRatioCurve_Reported) {
|
||||
HAL.report(
|
||||
tResourceType.kResourceType_RobotDrive,
|
||||
tInstances.kRobotDrive_ArcadeRatioCurve,
|
||||
getNumMotors());
|
||||
kArcadeRatioCurve_Reported = true;
|
||||
}
|
||||
if (curve < 0) {
|
||||
double value = Math.log(-curve);
|
||||
double ratio = (value - m_sensitivity) / (value + m_sensitivity);
|
||||
if (ratio == 0) {
|
||||
ratio = 0.0000000001;
|
||||
}
|
||||
leftOutput = outputMagnitude / ratio;
|
||||
rightOutput = outputMagnitude;
|
||||
} else if (curve > 0) {
|
||||
double value = Math.log(curve);
|
||||
double ratio = (value - m_sensitivity) / (value + m_sensitivity);
|
||||
if (ratio == 0) {
|
||||
ratio = 0.0000000001;
|
||||
}
|
||||
leftOutput = outputMagnitude;
|
||||
rightOutput = outputMagnitude / ratio;
|
||||
} else {
|
||||
leftOutput = outputMagnitude;
|
||||
rightOutput = outputMagnitude;
|
||||
}
|
||||
setLeftRightMotorOutputs(leftOutput, rightOutput);
|
||||
}
|
||||
|
||||
/**
|
||||
* Provide tank steering using the stored robot configuration. drive the robot using two joystick
|
||||
* inputs. The Y-axis will be selected from each Joystick object. The calculated values will be
|
||||
* squared to decrease sensitivity at low speeds.
|
||||
*
|
||||
* @param leftStick The joystick to control the left side of the robot.
|
||||
* @param rightStick The joystick to control the right side of the robot.
|
||||
*/
|
||||
public void tankDrive(GenericHID leftStick, GenericHID rightStick) {
|
||||
requireNonNullParam(leftStick, "leftStick", "tankDrive");
|
||||
requireNonNullParam(rightStick, "rightStick", "tankDrive");
|
||||
|
||||
tankDrive(leftStick.getY(), rightStick.getY(), true);
|
||||
}
|
||||
|
||||
/**
|
||||
* Provide tank steering using the stored robot configuration. drive the robot using two joystick
|
||||
* inputs. The Y-axis will be selected from each Joystick object.
|
||||
*
|
||||
* @param leftStick The joystick to control the left side of the robot.
|
||||
* @param rightStick The joystick to control the right side of the robot.
|
||||
* @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
|
||||
*/
|
||||
public void tankDrive(GenericHID leftStick, GenericHID rightStick, boolean squaredInputs) {
|
||||
requireNonNullParam(leftStick, "leftStick", "tankDrive");
|
||||
requireNonNullParam(rightStick, "rightStick", "tankDrive");
|
||||
|
||||
tankDrive(leftStick.getY(), rightStick.getY(), squaredInputs);
|
||||
}
|
||||
|
||||
/**
|
||||
* Provide tank steering using the stored robot configuration. This function lets you pick the
|
||||
* axis to be used on each Joystick object for the left and right sides of the robot. The
|
||||
* calculated values will be squared to decrease sensitivity at low speeds.
|
||||
*
|
||||
* @param leftStick The Joystick object to use for the left side of the robot.
|
||||
* @param leftAxis The axis to select on the left side Joystick object.
|
||||
* @param rightStick The Joystick object to use for the right side of the robot.
|
||||
* @param rightAxis The axis to select on the right side Joystick object.
|
||||
*/
|
||||
public void tankDrive(
|
||||
GenericHID leftStick, final int leftAxis, GenericHID rightStick, final int rightAxis) {
|
||||
requireNonNullParam(leftStick, "leftStick", "tankDrive");
|
||||
requireNonNullParam(rightStick, "rightStick", "tankDrive");
|
||||
|
||||
tankDrive(leftStick.getRawAxis(leftAxis), rightStick.getRawAxis(rightAxis), true);
|
||||
}
|
||||
|
||||
/**
|
||||
* Provide tank steering using the stored robot configuration. This function lets you pick the
|
||||
* axis to be used on each Joystick object for the left and right sides of the robot.
|
||||
*
|
||||
* @param leftStick The Joystick object to use for the left side of the robot.
|
||||
* @param leftAxis The axis to select on the left side Joystick object.
|
||||
* @param rightStick The Joystick object to use for the right side of the robot.
|
||||
* @param rightAxis The axis to select on the right side Joystick object.
|
||||
* @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
|
||||
*/
|
||||
public void tankDrive(
|
||||
GenericHID leftStick,
|
||||
final int leftAxis,
|
||||
GenericHID rightStick,
|
||||
final int rightAxis,
|
||||
boolean squaredInputs) {
|
||||
requireNonNullParam(leftStick, "leftStick", "tankDrive");
|
||||
requireNonNullParam(rightStick, "rightStick", "tankDrive");
|
||||
|
||||
tankDrive(leftStick.getRawAxis(leftAxis), rightStick.getRawAxis(rightAxis), squaredInputs);
|
||||
}
|
||||
|
||||
/**
|
||||
* Provide tank steering using the stored robot configuration. This function lets you directly
|
||||
* provide joystick values from any source.
|
||||
*
|
||||
* @param leftValue The value of the left stick.
|
||||
* @param rightValue The value of the right stick.
|
||||
* @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
|
||||
*/
|
||||
public void tankDrive(double leftValue, double rightValue, boolean squaredInputs) {
|
||||
if (!kTank_Reported) {
|
||||
HAL.report(
|
||||
tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive_Tank, getNumMotors());
|
||||
kTank_Reported = true;
|
||||
}
|
||||
|
||||
leftValue = limit(leftValue);
|
||||
rightValue = limit(rightValue);
|
||||
|
||||
// square the inputs (while preserving the sign) to increase fine control
|
||||
// while permitting full power
|
||||
if (squaredInputs) {
|
||||
leftValue = Math.copySign(leftValue * leftValue, leftValue);
|
||||
rightValue = Math.copySign(rightValue * rightValue, rightValue);
|
||||
}
|
||||
setLeftRightMotorOutputs(leftValue, rightValue);
|
||||
}
|
||||
|
||||
/**
|
||||
* Provide tank steering using the stored robot configuration. This function lets you directly
|
||||
* provide joystick values from any source. The calculated values will be squared to decrease
|
||||
* sensitivity at low speeds.
|
||||
*
|
||||
* @param leftValue The value of the left stick.
|
||||
* @param rightValue The value of the right stick.
|
||||
*/
|
||||
public void tankDrive(double leftValue, double rightValue) {
|
||||
tankDrive(leftValue, rightValue, true);
|
||||
}
|
||||
|
||||
/**
|
||||
* Arcade drive implements single stick driving. Given a single Joystick, the class assumes the Y
|
||||
* axis for the move value and the X axis for the rotate value. (Should add more information here
|
||||
* regarding the way that arcade drive works.)
|
||||
*
|
||||
* @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be selected
|
||||
* for forwards/backwards and the X-axis will be selected for rotation rate.
|
||||
* @param squaredInputs If true, the sensitivity will be decreased for small values
|
||||
*/
|
||||
public void arcadeDrive(GenericHID stick, boolean squaredInputs) {
|
||||
// simply call the full-featured arcadeDrive with the appropriate values
|
||||
arcadeDrive(stick.getY(), stick.getX(), squaredInputs);
|
||||
}
|
||||
|
||||
/**
|
||||
* Arcade drive implements single stick driving. Given a single Joystick, the class assumes the Y
|
||||
* axis for the move value and the X axis for the rotate value. (Should add more information here
|
||||
* regarding the way that arcade drive works.) The calculated values will be squared to decrease
|
||||
* sensitivity at low speeds.
|
||||
*
|
||||
* @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be selected
|
||||
* for forwards/backwards and the X-axis will be selected for rotation rate.
|
||||
*/
|
||||
public void arcadeDrive(GenericHID stick) {
|
||||
arcadeDrive(stick, true);
|
||||
}
|
||||
|
||||
/**
|
||||
* Arcade drive implements single stick driving. Given two joystick instances and two axis,
|
||||
* compute the values to send to either two or four motors.
|
||||
*
|
||||
* @param moveStick The Joystick object that represents the forward/backward direction
|
||||
* @param moveAxis The axis on the moveStick object to use for forwards/backwards (typically
|
||||
* Y_AXIS)
|
||||
* @param rotateStick The Joystick object that represents the rotation value
|
||||
* @param rotateAxis The axis on the rotation object to use for the rotate right/left (typically
|
||||
* X_AXIS)
|
||||
* @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
|
||||
*/
|
||||
public void arcadeDrive(
|
||||
GenericHID moveStick,
|
||||
final int moveAxis,
|
||||
GenericHID rotateStick,
|
||||
final int rotateAxis,
|
||||
boolean squaredInputs) {
|
||||
double moveValue = moveStick.getRawAxis(moveAxis);
|
||||
double rotateValue = rotateStick.getRawAxis(rotateAxis);
|
||||
|
||||
arcadeDrive(moveValue, rotateValue, squaredInputs);
|
||||
}
|
||||
|
||||
/**
|
||||
* Arcade drive implements single stick driving. Given two joystick instances and two axis,
|
||||
* compute the values to send to either two or four motors. The calculated values will be squared
|
||||
* to decrease sensitivity at low speeds.
|
||||
*
|
||||
* @param moveStick The Joystick object that represents the forward/backward direction
|
||||
* @param moveAxis The axis on the moveStick object to use for forwards/backwards (typically
|
||||
* Y_AXIS)
|
||||
* @param rotateStick The Joystick object that represents the rotation value
|
||||
* @param rotateAxis The axis on the rotation object to use for the rotate right/left (typically
|
||||
* X_AXIS)
|
||||
*/
|
||||
public void arcadeDrive(
|
||||
GenericHID moveStick, final int moveAxis, GenericHID rotateStick, final int rotateAxis) {
|
||||
arcadeDrive(moveStick, moveAxis, rotateStick, rotateAxis, true);
|
||||
}
|
||||
|
||||
/**
|
||||
* Arcade drive implements single stick driving. This function lets you directly provide joystick
|
||||
* values from any source.
|
||||
*
|
||||
* @param moveValue The value to use for forwards/backwards
|
||||
* @param rotateValue The value to use for the rotate right/left
|
||||
* @param squaredInputs If set, decreases the sensitivity at low speeds
|
||||
*/
|
||||
public void arcadeDrive(double moveValue, double rotateValue, boolean squaredInputs) {
|
||||
// local variables to hold the computed PWM values for the motors
|
||||
if (!kArcadeStandard_Reported) {
|
||||
HAL.report(
|
||||
tResourceType.kResourceType_RobotDrive,
|
||||
tInstances.kRobotDrive_ArcadeStandard,
|
||||
getNumMotors());
|
||||
kArcadeStandard_Reported = true;
|
||||
}
|
||||
|
||||
double leftMotorSpeed;
|
||||
double rightMotorSpeed;
|
||||
|
||||
moveValue = limit(moveValue);
|
||||
rotateValue = limit(rotateValue);
|
||||
|
||||
// square the inputs (while preserving the sign) to increase fine control
|
||||
// while permitting full power
|
||||
if (squaredInputs) {
|
||||
// square the inputs (while preserving the sign) to increase fine control
|
||||
// while permitting full power
|
||||
moveValue = Math.copySign(moveValue * moveValue, moveValue);
|
||||
rotateValue = Math.copySign(rotateValue * rotateValue, rotateValue);
|
||||
}
|
||||
|
||||
if (moveValue > 0.0) {
|
||||
if (rotateValue > 0.0) {
|
||||
leftMotorSpeed = moveValue - rotateValue;
|
||||
rightMotorSpeed = Math.max(moveValue, rotateValue);
|
||||
} else {
|
||||
leftMotorSpeed = Math.max(moveValue, -rotateValue);
|
||||
rightMotorSpeed = moveValue + rotateValue;
|
||||
}
|
||||
} else {
|
||||
if (rotateValue > 0.0) {
|
||||
leftMotorSpeed = -Math.max(-moveValue, rotateValue);
|
||||
rightMotorSpeed = moveValue + rotateValue;
|
||||
} else {
|
||||
leftMotorSpeed = moveValue - rotateValue;
|
||||
rightMotorSpeed = -Math.max(-moveValue, -rotateValue);
|
||||
}
|
||||
}
|
||||
|
||||
setLeftRightMotorOutputs(leftMotorSpeed, rightMotorSpeed);
|
||||
}
|
||||
|
||||
/**
|
||||
* Arcade drive implements single stick driving. This function lets you directly provide joystick
|
||||
* values from any source. The calculated values will be squared to decrease sensitivity at low
|
||||
* speeds.
|
||||
*
|
||||
* @param moveValue The value to use for forwards/backwards
|
||||
* @param rotateValue The value to use for the rotate right/left
|
||||
*/
|
||||
public void arcadeDrive(double moveValue, double rotateValue) {
|
||||
arcadeDrive(moveValue, rotateValue, true);
|
||||
}
|
||||
|
||||
/**
|
||||
* Drive method for Mecanum wheeled robots.
|
||||
*
|
||||
* <p>A method for driving with Mecanum wheeled robots. There are 4 wheels on the robot, arranged
|
||||
* so that the front and back wheels are toed in 45 degrees. When looking at the wheels from the
|
||||
* top, the roller axles should form an X across the robot.
|
||||
*
|
||||
* <p>This is designed to be directly driven by joystick axes.
|
||||
*
|
||||
* @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. This input is inverted to
|
||||
* match the forward == -1.0 that joysticks produce. [-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.
|
||||
*/
|
||||
public void mecanumDrive_Cartesian(double x, double y, double rotation, double gyroAngle) {
|
||||
if (!kMecanumCartesian_Reported) {
|
||||
HAL.report(
|
||||
tResourceType.kResourceType_RobotDrive,
|
||||
tInstances.kRobotDrive_MecanumCartesian,
|
||||
getNumMotors());
|
||||
kMecanumCartesian_Reported = true;
|
||||
}
|
||||
// Negate y for the joystick.
|
||||
y = -y;
|
||||
// Compensate for gyro angle.
|
||||
double[] rotated = rotateVector(x, y, gyroAngle);
|
||||
x = rotated[0];
|
||||
y = rotated[1];
|
||||
|
||||
double[] wheelSpeeds = new double[kMaxNumberOfMotors];
|
||||
wheelSpeeds[MotorType.kFrontLeft.value] = x + y + rotation;
|
||||
wheelSpeeds[MotorType.kFrontRight.value] = -x + y - rotation;
|
||||
wheelSpeeds[MotorType.kRearLeft.value] = -x + y + rotation;
|
||||
wheelSpeeds[MotorType.kRearRight.value] = x + 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);
|
||||
|
||||
feed();
|
||||
}
|
||||
|
||||
/**
|
||||
* Drive method for Mecanum wheeled robots.
|
||||
*
|
||||
* <p>A method for driving with Mecanum wheeled robots. There are 4 wheels on the robot, arranged
|
||||
* so that the front and back wheels are toed in 45 degrees. When looking at the wheels from the
|
||||
* top, the roller axles should form an X across the robot.
|
||||
*
|
||||
* @param magnitude The speed that the robot should drive in a given direction. [-1.0..1.0]
|
||||
* @param direction The angle the robot should drive in degrees. The direction and magnitude are
|
||||
* independent of the rotation rate. [-180.0..180.0]
|
||||
* @param rotation The rate of rotation for the robot that is completely independent of the
|
||||
* magnitude or direction. [-1.0..1.0]
|
||||
*/
|
||||
public void mecanumDrive_Polar(double magnitude, double direction, double rotation) {
|
||||
if (!kMecanumPolar_Reported) {
|
||||
HAL.report(
|
||||
tResourceType.kResourceType_RobotDrive,
|
||||
tInstances.kRobotDrive_MecanumPolar,
|
||||
getNumMotors());
|
||||
kMecanumPolar_Reported = true;
|
||||
}
|
||||
// Normalized for full power along the Cartesian axes.
|
||||
magnitude = limit(magnitude) * Math.sqrt(2.0);
|
||||
// The rollers are at 45 degree angles.
|
||||
double dirInRad = (direction + 45.0) * Math.PI / 180.0;
|
||||
double cosD = Math.cos(dirInRad);
|
||||
double sinD = Math.sin(dirInRad);
|
||||
|
||||
double[] wheelSpeeds = new double[kMaxNumberOfMotors];
|
||||
wheelSpeeds[MotorType.kFrontLeft.value] = sinD * magnitude + rotation;
|
||||
wheelSpeeds[MotorType.kFrontRight.value] = cosD * magnitude - rotation;
|
||||
wheelSpeeds[MotorType.kRearLeft.value] = cosD * magnitude + rotation;
|
||||
wheelSpeeds[MotorType.kRearRight.value] = sinD * magnitude - 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);
|
||||
|
||||
feed();
|
||||
}
|
||||
|
||||
/**
|
||||
* Holonomic Drive method for Mecanum wheeled robots.
|
||||
*
|
||||
* <p>This is an alias to mecanumDrive_Polar() for backward compatibility
|
||||
*
|
||||
* @param magnitude The speed that the robot should drive in a given direction. [-1.0..1.0]
|
||||
* @param direction The direction the robot should drive. 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
|
||||
* magnitute or direction. [-1.0..1.0]
|
||||
*/
|
||||
void holonomicDrive(double magnitude, double direction, double rotation) {
|
||||
mecanumDrive_Polar(magnitude, direction, rotation);
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the speed of the right and left motors. This is used once an appropriate drive setup
|
||||
* function is called such as twoWheelDrive(). The motors are set to "leftSpeed" and "rightSpeed"
|
||||
* and includes flipping the direction of one side for opposing motors.
|
||||
*
|
||||
* @param leftOutput The speed to send to the left side of the robot.
|
||||
* @param rightOutput The speed to send to the right side of the robot.
|
||||
*/
|
||||
public void setLeftRightMotorOutputs(double leftOutput, double rightOutput) {
|
||||
|
||||
if (m_frontLeftMotor != null) {
|
||||
m_frontLeftMotor.set(limit(leftOutput) * m_maxOutput);
|
||||
}
|
||||
m_rearLeftMotor.set(limit(leftOutput) * m_maxOutput);
|
||||
|
||||
if (m_frontRightMotor != null) {
|
||||
m_frontRightMotor.set(-limit(rightOutput) * m_maxOutput);
|
||||
}
|
||||
m_rearRightMotor.set(-limit(rightOutput) * m_maxOutput);
|
||||
|
||||
feed();
|
||||
}
|
||||
|
||||
/** Limit motor values to the -1.0 to +1.0 range. */
|
||||
protected static double limit(double number) {
|
||||
if (number > 1.0) {
|
||||
return 1.0;
|
||||
}
|
||||
if (number < -1.0) {
|
||||
return -1.0;
|
||||
}
|
||||
return number;
|
||||
}
|
||||
|
||||
/** Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0. */
|
||||
protected static void normalize(double[] wheelSpeeds) {
|
||||
double maxMagnitude = Math.abs(wheelSpeeds[0]);
|
||||
for (int i = 1; i < kMaxNumberOfMotors; i++) {
|
||||
double temp = Math.abs(wheelSpeeds[i]);
|
||||
if (maxMagnitude < temp) {
|
||||
maxMagnitude = temp;
|
||||
}
|
||||
}
|
||||
if (maxMagnitude > 1.0) {
|
||||
for (int i = 0; i < kMaxNumberOfMotors; i++) {
|
||||
wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/** Rotate a vector in Cartesian space. */
|
||||
protected static double[] rotateVector(double x, double y, 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;
|
||||
return out;
|
||||
}
|
||||
|
||||
/**
|
||||
* Invert a motor direction. This is used when a motor should run in the opposite direction as the
|
||||
* drive code would normally run it. Motors that are direct drive would be inverted, the drive
|
||||
* code assumes that the motors are geared with one reversal.
|
||||
*
|
||||
* @param motor The motor index to invert.
|
||||
* @param isInverted True if the motor should be inverted when operated.
|
||||
*/
|
||||
public void setInvertedMotor(MotorType motor, boolean isInverted) {
|
||||
switch (motor) {
|
||||
case kFrontLeft:
|
||||
m_frontLeftMotor.setInverted(isInverted);
|
||||
break;
|
||||
case kFrontRight:
|
||||
m_frontRightMotor.setInverted(isInverted);
|
||||
break;
|
||||
case kRearLeft:
|
||||
m_rearLeftMotor.setInverted(isInverted);
|
||||
break;
|
||||
case kRearRight:
|
||||
m_rearRightMotor.setInverted(isInverted);
|
||||
break;
|
||||
default:
|
||||
throw new IllegalArgumentException("Illegal motor type: " + motor);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the turning sensitivity.
|
||||
*
|
||||
* <p>This only impacts the drive() entry-point.
|
||||
*
|
||||
* @param sensitivity Effectively sets the turning sensitivity (or turn radius for a given value)
|
||||
*/
|
||||
public void setSensitivity(double sensitivity) {
|
||||
m_sensitivity = sensitivity;
|
||||
}
|
||||
|
||||
/**
|
||||
* 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;
|
||||
}
|
||||
|
||||
/** Free the speed controllers if they were allocated locally. */
|
||||
@Override
|
||||
public void close() {
|
||||
if (m_allocatedSpeedControllers) {
|
||||
if (m_frontLeftMotor != null) {
|
||||
((PWM) m_frontLeftMotor).close();
|
||||
}
|
||||
if (m_frontRightMotor != null) {
|
||||
((PWM) m_frontRightMotor).close();
|
||||
}
|
||||
if (m_rearLeftMotor != null) {
|
||||
((PWM) m_rearLeftMotor).close();
|
||||
}
|
||||
if (m_rearRightMotor != null) {
|
||||
((PWM) m_rearRightMotor).close();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public String getDescription() {
|
||||
return "Robot Drive";
|
||||
}
|
||||
|
||||
@Override
|
||||
public void stopMotor() {
|
||||
if (m_frontLeftMotor != null) {
|
||||
m_frontLeftMotor.stopMotor();
|
||||
}
|
||||
if (m_frontRightMotor != null) {
|
||||
m_frontRightMotor.stopMotor();
|
||||
}
|
||||
if (m_rearLeftMotor != null) {
|
||||
m_rearLeftMotor.stopMotor();
|
||||
}
|
||||
if (m_rearRightMotor != null) {
|
||||
m_rearRightMotor.stopMotor();
|
||||
}
|
||||
|
||||
feed();
|
||||
}
|
||||
|
||||
protected int getNumMotors() {
|
||||
int motors = 0;
|
||||
if (m_frontLeftMotor != null) {
|
||||
motors++;
|
||||
}
|
||||
if (m_frontRightMotor != null) {
|
||||
motors++;
|
||||
}
|
||||
if (m_rearLeftMotor != null) {
|
||||
motors++;
|
||||
}
|
||||
if (m_rearRightMotor != null) {
|
||||
motors++;
|
||||
}
|
||||
return motors;
|
||||
}
|
||||
}
|
||||
@@ -86,14 +86,13 @@ import java.util.StringJoiner;
|
||||
* value can be changed with {@link #setDeadband}.
|
||||
*
|
||||
* <p>RobotDrive porting guide: <br>
|
||||
* {@link #tankDrive(double, double)} is equivalent to {@link
|
||||
* edu.wpi.first.wpilibj.RobotDrive#tankDrive(double, double)} if a deadband of 0 is used. <br>
|
||||
* {@link #arcadeDrive(double, double)} is equivalent to {@link
|
||||
* edu.wpi.first.wpilibj.RobotDrive#arcadeDrive(double, double)} if a deadband of 0 is used and the
|
||||
* the rotation input is inverted eg arcadeDrive(y, -rotation) <br>
|
||||
* {@link #curvatureDrive(double, double, boolean)} is similar in concept to {@link
|
||||
* edu.wpi.first.wpilibj.RobotDrive#drive(double, double)} with the addition of a quick turn mode.
|
||||
* However, it is not designed to give exactly the same response.
|
||||
* {@link #tankDrive(double, double)} is equivalent to RobotDrive's tankDrive(double, double) if a
|
||||
* deadband of 0 is used. <br>
|
||||
* {@link #arcadeDrive(double, double)} is equivalent to RobotDrive's arcadeDrive(double, double) if
|
||||
* a deadband of 0 is used and the the rotation input is inverted eg arcadeDrive(y, -rotation) <br>
|
||||
* {@link #curvatureDrive(double, double, boolean)} is similar in concept to RobotDrive's
|
||||
* drive(double, double) with the addition of a quick turn mode. However, it is not designed to give
|
||||
* exactly the same response.
|
||||
*/
|
||||
public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoCloseable {
|
||||
public static final double kDefaultQuickStopThreshold = 0.2;
|
||||
|
||||
@@ -50,13 +50,12 @@ import java.util.StringJoiner;
|
||||
* <p>RobotDrive porting guide: <br>
|
||||
* In MecanumDrive, the right side speed controllers are automatically inverted, while in
|
||||
* RobotDrive, no speed controllers are automatically inverted. <br>
|
||||
* {@link #driveCartesian(double, double, double, double)} is equivalent to {@link
|
||||
* edu.wpi.first.wpilibj.RobotDrive#mecanumDrive_Cartesian(double, double, double, double)} if a
|
||||
* deadband of 0 is used, and the ySpeed and gyroAngle values are inverted compared to RobotDrive
|
||||
* (eg driveCartesian(xSpeed, -ySpeed, zRotation, -gyroAngle). <br>
|
||||
* {@link #drivePolar(double, double, double)} is equivalent to {@link
|
||||
* edu.wpi.first.wpilibj.RobotDrive#mecanumDrive_Polar(double, double, double)} if a deadband of 0
|
||||
* is used.
|
||||
* {@link #driveCartesian(double, double, double, double)} is equivalent to RobotDrive's
|
||||
* mecanumDrive_Cartesian(double, double, double, double) if a deadband of 0 is used, and the ySpeed
|
||||
* and gyroAngle values are inverted compared to RobotDrive (eg driveCartesian(xSpeed, -ySpeed,
|
||||
* zRotation, -gyroAngle). <br>
|
||||
* {@link #drivePolar(double, double, double)} is equivalent to RobotDrive's
|
||||
* mecanumDrive_Polar(double, double, double)} if a deadband of 0 is used.
|
||||
*/
|
||||
public class MecanumDrive extends RobotDriveBase implements Sendable, AutoCloseable {
|
||||
private static int instances;
|
||||
|
||||
@@ -1,265 +0,0 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.wpilibj.drive;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
import edu.wpi.first.wpilibj.MockSpeedController;
|
||||
import edu.wpi.first.wpilibj.RobotDrive;
|
||||
import org.junit.jupiter.api.BeforeEach;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
/** Tests DifferentialDrive and MecanumDrive. */
|
||||
public class DriveTest {
|
||||
private final MockSpeedController m_rdFrontLeft = new MockSpeedController();
|
||||
private final MockSpeedController m_rdRearLeft = new MockSpeedController();
|
||||
private final MockSpeedController m_rdFrontRight = new MockSpeedController();
|
||||
private final MockSpeedController m_rdRearRight = new MockSpeedController();
|
||||
private final MockSpeedController m_frontLeft = new MockSpeedController();
|
||||
private final MockSpeedController m_rearLeft = new MockSpeedController();
|
||||
private final MockSpeedController m_frontRight = new MockSpeedController();
|
||||
private final MockSpeedController m_rearRight = new MockSpeedController();
|
||||
private final RobotDrive m_robotDrive =
|
||||
new RobotDrive(m_rdFrontLeft, m_rdRearLeft, m_rdFrontRight, m_rdRearRight);
|
||||
private final DifferentialDrive m_differentialDrive =
|
||||
new DifferentialDrive(m_frontLeft, m_frontRight);
|
||||
private final MecanumDrive m_mecanumDrive =
|
||||
new MecanumDrive(m_frontLeft, m_rearLeft, m_frontRight, m_rearRight);
|
||||
|
||||
private final double[] m_testJoystickValues = {1.0, 0.9, 0.5, 0.01, 0.0, -0.01, -0.5, -0.9, -1.0};
|
||||
private final double[] m_testGyroValues = {
|
||||
0, 30, 45, 90, 135, 180, 225, 270, 305, 360, 540, -45, -90, -135, -180, -225, -270, -305, -360,
|
||||
-540
|
||||
};
|
||||
|
||||
@BeforeEach
|
||||
void setUp() {
|
||||
m_differentialDrive.setDeadband(0.0);
|
||||
m_differentialDrive.setSafetyEnabled(false);
|
||||
m_mecanumDrive.setDeadband(0.0);
|
||||
m_mecanumDrive.setSafetyEnabled(false);
|
||||
m_robotDrive.setSafetyEnabled(false);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testTankDriveSquared() {
|
||||
for (double leftJoystick : m_testJoystickValues) {
|
||||
for (double rightJoystick : m_testJoystickValues) {
|
||||
m_robotDrive.tankDrive(leftJoystick, rightJoystick);
|
||||
m_differentialDrive.tankDrive(leftJoystick, rightJoystick);
|
||||
assertEquals(
|
||||
m_rdFrontLeft.get(),
|
||||
m_frontLeft.get(),
|
||||
0.01,
|
||||
"Left Motor squared didn't match. Left Joystick: "
|
||||
+ leftJoystick
|
||||
+ " Right Joystick: "
|
||||
+ rightJoystick);
|
||||
assertEquals(
|
||||
m_rdFrontRight.get(),
|
||||
m_frontRight.get(),
|
||||
0.01,
|
||||
"Right Motor squared didn't match. Left Joystick: "
|
||||
+ leftJoystick
|
||||
+ " Right Joystick: "
|
||||
+ rightJoystick);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@Test
|
||||
void testTankDrive() {
|
||||
for (double leftJoystick : m_testJoystickValues) {
|
||||
for (double rightJoystick : m_testJoystickValues) {
|
||||
m_robotDrive.tankDrive(leftJoystick, rightJoystick, false);
|
||||
m_differentialDrive.tankDrive(leftJoystick, rightJoystick, false);
|
||||
assertEquals(
|
||||
m_rdFrontLeft.get(),
|
||||
m_frontLeft.get(),
|
||||
0.01,
|
||||
"Left Motor didn't match. Left Joystick: "
|
||||
+ leftJoystick
|
||||
+ " Right Joystick: "
|
||||
+ rightJoystick);
|
||||
assertEquals(
|
||||
m_rdFrontRight.get(),
|
||||
m_frontRight.get(),
|
||||
0.01,
|
||||
"Right Motor didn't match. Left Joystick: "
|
||||
+ leftJoystick
|
||||
+ " Right Joystick: "
|
||||
+ rightJoystick);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@Test
|
||||
void testArcadeDriveSquared() {
|
||||
for (double moveJoystick : m_testJoystickValues) {
|
||||
for (double rotateJoystick : m_testJoystickValues) {
|
||||
m_robotDrive.arcadeDrive(moveJoystick, rotateJoystick);
|
||||
m_differentialDrive.arcadeDrive(moveJoystick, -rotateJoystick);
|
||||
assertEquals(
|
||||
m_rdFrontLeft.get(),
|
||||
m_frontLeft.get(),
|
||||
0.01,
|
||||
"Left Motor squared didn't match. Move Joystick: "
|
||||
+ moveJoystick
|
||||
+ " Rotate Joystick: "
|
||||
+ rotateJoystick);
|
||||
assertEquals(
|
||||
m_rdFrontRight.get(),
|
||||
m_frontRight.get(),
|
||||
0.01,
|
||||
"Right Motor squared didn't match. Move Joystick: "
|
||||
+ moveJoystick
|
||||
+ " Rotate Joystick: "
|
||||
+ rotateJoystick);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@Test
|
||||
void testArcadeDrive() {
|
||||
for (double moveJoystick : m_testJoystickValues) {
|
||||
for (double rotateJoystick : m_testJoystickValues) {
|
||||
m_robotDrive.arcadeDrive(moveJoystick, rotateJoystick, false);
|
||||
m_differentialDrive.arcadeDrive(moveJoystick, -rotateJoystick, false);
|
||||
assertEquals(
|
||||
m_rdFrontLeft.get(),
|
||||
m_frontLeft.get(),
|
||||
0.01,
|
||||
"Left Motor didn't match. Move Joystick: "
|
||||
+ moveJoystick
|
||||
+ " Rotate Joystick: "
|
||||
+ rotateJoystick);
|
||||
assertEquals(
|
||||
m_rdFrontRight.get(),
|
||||
m_frontRight.get(),
|
||||
0.01,
|
||||
"Right Motor didn't match. Move Joystick: "
|
||||
+ moveJoystick
|
||||
+ " Rotate Joystick: "
|
||||
+ rotateJoystick);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@Test
|
||||
void testMecanumPolar() {
|
||||
for (double magnitudeJoystick : m_testJoystickValues) {
|
||||
for (double directionJoystick : m_testGyroValues) {
|
||||
for (double rotationJoystick : m_testJoystickValues) {
|
||||
m_robotDrive.mecanumDrive_Polar(magnitudeJoystick, directionJoystick, rotationJoystick);
|
||||
m_mecanumDrive.drivePolar(magnitudeJoystick, directionJoystick, rotationJoystick);
|
||||
assertEquals(
|
||||
m_rdFrontLeft.get(),
|
||||
m_frontLeft.get(),
|
||||
0.01,
|
||||
"Left Front Motor didn't match. Magnitude Joystick: "
|
||||
+ magnitudeJoystick
|
||||
+ " Direction Joystick: "
|
||||
+ directionJoystick
|
||||
+ " RotationJoystick: "
|
||||
+ rotationJoystick);
|
||||
assertEquals(
|
||||
m_rdFrontRight.get(),
|
||||
-m_frontRight.get(),
|
||||
0.01,
|
||||
"Right Front Motor didn't match. Magnitude Joystick: "
|
||||
+ magnitudeJoystick
|
||||
+ " Direction Joystick: "
|
||||
+ directionJoystick
|
||||
+ " RotationJoystick: "
|
||||
+ rotationJoystick);
|
||||
assertEquals(
|
||||
m_rdRearLeft.get(),
|
||||
m_rearLeft.get(),
|
||||
0.01,
|
||||
"Left Rear Motor didn't match. Magnitude Joystick: "
|
||||
+ magnitudeJoystick
|
||||
+ " Direction Joystick: "
|
||||
+ directionJoystick
|
||||
+ " RotationJoystick: "
|
||||
+ rotationJoystick);
|
||||
assertEquals(
|
||||
m_rdRearRight.get(),
|
||||
-m_rearRight.get(),
|
||||
0.01,
|
||||
"Right Rear Motor didn't match. Magnitude Joystick: "
|
||||
+ magnitudeJoystick
|
||||
+ " Direction Joystick: "
|
||||
+ directionJoystick
|
||||
+ " RotationJoystick: "
|
||||
+ rotationJoystick);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@Test
|
||||
@SuppressWarnings("checkstyle:LocalVariableName")
|
||||
void testMecanumCartesian() {
|
||||
for (double x_Joystick : m_testJoystickValues) {
|
||||
for (double y_Joystick : m_testJoystickValues) {
|
||||
for (double rotationJoystick : m_testJoystickValues) {
|
||||
for (double gyroValue : m_testGyroValues) {
|
||||
m_robotDrive.mecanumDrive_Cartesian(
|
||||
x_Joystick, y_Joystick, rotationJoystick, gyroValue);
|
||||
m_mecanumDrive.driveCartesian(x_Joystick, -y_Joystick, rotationJoystick, -gyroValue);
|
||||
assertEquals(
|
||||
m_rdFrontLeft.get(),
|
||||
m_frontLeft.get(),
|
||||
0.01,
|
||||
"Left Front Motor didn't match. X Joystick: "
|
||||
+ x_Joystick
|
||||
+ " Y Joystick: "
|
||||
+ y_Joystick
|
||||
+ " RotationJoystick: "
|
||||
+ rotationJoystick
|
||||
+ " Gyro: "
|
||||
+ gyroValue);
|
||||
assertEquals(
|
||||
m_rdFrontRight.get(),
|
||||
-m_frontRight.get(),
|
||||
0.01,
|
||||
"Right Front Motor didn't match. X Joystick: "
|
||||
+ x_Joystick
|
||||
+ " Y Joystick: "
|
||||
+ y_Joystick
|
||||
+ " RotationJoystick: "
|
||||
+ rotationJoystick
|
||||
+ " Gyro: "
|
||||
+ gyroValue);
|
||||
assertEquals(
|
||||
m_rdRearLeft.get(),
|
||||
m_rearLeft.get(),
|
||||
0.01,
|
||||
"Left Rear Motor didn't match. X Joystick: "
|
||||
+ x_Joystick
|
||||
+ " Y Joystick: "
|
||||
+ y_Joystick
|
||||
+ " RotationJoystick: "
|
||||
+ rotationJoystick
|
||||
+ " Gyro: "
|
||||
+ gyroValue);
|
||||
assertEquals(
|
||||
m_rdRearRight.get(),
|
||||
-m_rearRight.get(),
|
||||
0.01,
|
||||
"Right Rear Motor didn't match. X Joystick: "
|
||||
+ x_Joystick
|
||||
+ " Y Joystick: "
|
||||
+ y_Joystick
|
||||
+ " RotationJoystick: "
|
||||
+ rotationJoystick
|
||||
+ " Gyro: "
|
||||
+ gyroValue);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -12,7 +12,7 @@
|
||||
},
|
||||
{
|
||||
"name": "Tank Drive",
|
||||
"description": "Demonstrate the use of the RobotDrive class doing teleop driving with tank steering",
|
||||
"description": "Demonstrate the use of the DifferentialDrive class doing teleop driving with tank steering",
|
||||
"tags": [
|
||||
"Actuators",
|
||||
"Joystick",
|
||||
@@ -37,7 +37,7 @@
|
||||
},
|
||||
{
|
||||
"name": "Mecanum Drive",
|
||||
"description": "Demonstrate the use of the RobotDrive class doing teleop driving with Mecanum steering",
|
||||
"description": "Demonstrate the use of the MecanumDrive class doing teleop driving with Mecanum steering",
|
||||
"tags": [
|
||||
"Actuators",
|
||||
"Joystick",
|
||||
|
||||
@@ -39,8 +39,8 @@ public class Robot extends TimedRobot {
|
||||
}
|
||||
|
||||
/**
|
||||
* The motor speed is set from the joystick while the RobotDrive turning value is assigned from
|
||||
* the error between the setpoint and the gyro angle.
|
||||
* The motor speed is set from the joystick while the DifferentialDrive turning value is assigned
|
||||
* from the error between the setpoint and the gyro angle.
|
||||
*/
|
||||
@Override
|
||||
public void teleopPeriodic() {
|
||||
|
||||
@@ -9,7 +9,7 @@ import edu.wpi.first.wpilibj.PWMSparkMax;
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj.drive.MecanumDrive;
|
||||
|
||||
/** This is a demo program showing how to use Mecanum control with the RobotDrive class. */
|
||||
/** This is a demo program showing how to use Mecanum control with the MecanumDrive class. */
|
||||
public class Robot extends TimedRobot {
|
||||
private static final int kFrontLeftChannel = 2;
|
||||
private static final int kRearLeftChannel = 3;
|
||||
|
||||
@@ -10,8 +10,8 @@ import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
|
||||
|
||||
/**
|
||||
* This is a demo program showing the use of the RobotDrive class, specifically it contains the code
|
||||
* necessary to operate a robot with tank drive.
|
||||
* This is a demo program showing the use of the DifferentialDrive class, specifically it contains
|
||||
* the code necessary to operate a robot with tank drive.
|
||||
*/
|
||||
public class Robot extends TimedRobot {
|
||||
private DifferentialDrive m_myRobot;
|
||||
|
||||
Reference in New Issue
Block a user