/*----------------------------------------------------------------------------*/ /* 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 #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. */ 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); 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 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