mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-22 01:11:42 +00:00
acb786a791
Most of the MotorSafety implementation was moved into the MotorSafety base class. SafePWM's inheritance of MotorSafety was moved into PWM to eliminate Java needing a helper class. In Java, a helper class for Sendable (SendableImpl) was added due to lack of multiple inheritance.
454 lines
18 KiB
C++
454 lines
18 KiB
C++
/*----------------------------------------------------------------------------*/
|
|
/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
|
|
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
|
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
|
/* the project. */
|
|
/*----------------------------------------------------------------------------*/
|
|
|
|
#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.
|
|
*/
|
|
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);
|
|
|
|
virtual ~RobotDrive() = 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 compatability
|
|
*
|
|
* @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
|