diff --git a/wpilibc/src/main/native/cpp/RobotDrive.cpp b/wpilibc/src/main/native/cpp/RobotDrive.cpp deleted file mode 100644 index 8293282873..0000000000 --- a/wpilibc/src/main/native/cpp/RobotDrive.cpp +++ /dev/null @@ -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 -#include - -#include - -#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 make_shared_nodelete( - SpeedController* ptr) { - return std::shared_ptr(ptr, NullDeleter()); -} - -RobotDrive::RobotDrive(int leftMotorChannel, int rightMotorChannel) { - InitRobotDrive(); - m_rearLeftMotor = std::make_shared(leftMotorChannel); - m_rearRightMotor = std::make_shared(rightMotorChannel); - SetLeftRightMotorOutputs(0.0, 0.0); -} - -RobotDrive::RobotDrive(int frontLeftMotor, int rearLeftMotor, - int frontRightMotor, int rearRightMotor) { - InitRobotDrive(); - m_rearLeftMotor = std::make_shared(rearLeftMotor); - m_rearRightMotor = std::make_shared(rearRightMotor); - m_frontLeftMotor = std::make_shared(frontLeftMotor); - m_frontRightMotor = std::make_shared(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 leftMotor, - std::shared_ptr 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 frontLeftMotor, - std::shared_ptr rearLeftMotor, - std::shared_ptr frontRightMotor, - std::shared_ptr 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; -} diff --git a/wpilibc/src/main/native/include/frc/RobotDrive.h b/wpilibc/src/main/native/include/frc/RobotDrive.h deleted file mode 100644 index fa22e0e8cc..0000000000 --- a/wpilibc/src/main/native/include/frc/RobotDrive.h +++ /dev/null @@ -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 - -#include -#include - -#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 leftMotor, - std::shared_ptr 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 frontLeftMotor, - std::shared_ptr rearLeftMotor, - std::shared_ptr frontRightMotor, - std::shared_ptr 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 m_frontLeftMotor; - std::shared_ptr m_frontRightMotor; - std::shared_ptr m_rearLeftMotor; - std::shared_ptr 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 diff --git a/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h b/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h index 4e0ff1e79e..dc24959a80 100644 --- a/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h +++ b/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h @@ -87,12 +87,12 @@ class SpeedController; * *

RobotDrive porting guide: *
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. *
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) *
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, diff --git a/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h b/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h index e478e3b11b..13d56c5aa8 100644 --- a/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h +++ b/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h @@ -53,12 +53,12 @@ class SpeedController; * inverted, while in RobotDrive, no speed controllers are automatically * inverted. *
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). *
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, diff --git a/wpilibc/src/test/native/cpp/drive/DriveTest.cpp b/wpilibc/src/test/native/cpp/drive/DriveTest.cpp deleted file mode 100644 index 643aac3f13..0000000000 --- a/wpilibc/src/test/native/cpp/drive/DriveTest.cpp +++ /dev/null @@ -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; - } - } - } -} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java deleted file mode 100644 index d038ac3d04..0000000000 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java +++ /dev/null @@ -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. - * - *

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. - * - *

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. - * - *

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. - */ - 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. - * - *

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. - * - *

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. - * - *

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; - } -} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java index cb16ce3fad..f68f36d7c7 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java @@ -86,14 +86,13 @@ import java.util.StringJoiner; * value can be changed with {@link #setDeadband}. * *

RobotDrive porting guide:
- * {@link #tankDrive(double, double)} is equivalent to {@link - * edu.wpi.first.wpilibj.RobotDrive#tankDrive(double, double)} if a deadband of 0 is used.
- * {@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)
- * {@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.
+ * {@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)
+ * {@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; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java index 8189e68a58..1930ecfabe 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java @@ -50,13 +50,12 @@ import java.util.StringJoiner; *

RobotDrive porting guide:
* In MecanumDrive, the right side speed controllers are automatically inverted, while in * RobotDrive, no speed controllers are automatically inverted.
- * {@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).
- * {@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).
+ * {@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; diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/DriveTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/DriveTest.java deleted file mode 100644 index 388801a9be..0000000000 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/DriveTest.java +++ /dev/null @@ -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); - } - } - } - } - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json index f188f103ea..005bd0f83f 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json @@ -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", diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java index c1f3d2d365..9bdb61b227 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java @@ -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() { diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java index 34bb9f5930..5b2ef43398 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java @@ -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; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrive/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrive/Robot.java index 71bf29084e..bf2a568e1a 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrive/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrive/Robot.java @@ -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;