/*----------------------------------------------------------------------------*/ /* Copyright (c) FIRST 2008-2016. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ /*----------------------------------------------------------------------------*/ #include "RobotDrive.h" //#include "CANJaguar.h" #include #include "GenericHID.h" #include "Joystick.h" #include "Talon.h" #include "Utility.h" #include "WPIErrors.h" #undef max #include const int32_t RobotDrive::kMaxNumberOfMotors; static auto make_shared_nodelete(SpeedController* ptr) { return std::shared_ptr(ptr, NullDeleter()); } /* * Driving functions. * * These functions provide an interface to multiple motors that is used for C * programming. * The Drive(speed, direction) function is the main part of the set that makes * it easy to set speeds and direction independently in one call. */ /** * 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 RobotDrive::InitRobotDrive() { // FIXME: m_safetyHelper = new MotorSafetyHelper(this); // FIXME: m_safetyHelper->SetSafetyEnabled(true); } /** * 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. */ RobotDrive::RobotDrive(uint32_t leftMotorChannel, uint32_t rightMotorChannel) { InitRobotDrive(); m_rearLeftMotor = std::make_shared(leftMotorChannel); m_rearRightMotor = std::make_shared(rightMotorChannel); SetLeftRightMotorOutputs(0.0, 0.0); m_deleteSpeedControllers = true; } /** * 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 */ RobotDrive::RobotDrive(uint32_t frontLeftMotor, uint32_t rearLeftMotor, uint32_t frontRightMotor, uint32_t 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); m_deleteSpeedControllers = true; } /** * 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. */ 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); } // TODO: Change to rvalue references & move syntax. 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; } /** * Constructor for RobotDrive with 4 motors specified as SpeedController * objects. * * Speed controller input version of RobotDrive (see previous comments). * * @param rearLeftMotor The back left SpeedController object used to drive the * robot. * @param frontLeftMotor The front left SpeedController object used to drive * the robot * @param rearRightMotor The back right SpeedController object used to drive * the robot. * @param frontRightMotor The front right SpeedController object used to drive * the robot. */ 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; } /** * 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 RobotDrive::Drive(float outputMagnitude, float curve) { float leftOutput, rightOutput; static bool reported = false; if (!reported) { reported = true; } if (curve < 0) { float value = log(-curve); float ratio = (value - m_sensitivity) / (value + m_sensitivity); if (ratio == 0) ratio = .0000000001; leftOutput = outputMagnitude / ratio; rightOutput = outputMagnitude; } else if (curve > 0) { float value = log(curve); float ratio = (value - m_sensitivity) / (value + m_sensitivity); if (ratio == 0) ratio = .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. * * @param leftStick The joystick to control the left side of the robot. * @param rightStick The joystick to control the right side of the robot. */ 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); } /** * 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. */ void RobotDrive::TankDrive(GenericHID* leftStick, uint32_t leftAxis, GenericHID* rightStick, uint32_t 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, uint32_t leftAxis, GenericHID& rightStick, uint32_t rightAxis, bool squaredInputs) { 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. */ void RobotDrive::TankDrive(float leftValue, float rightValue, bool squaredInputs) { static bool reported = false; if (!reported) { reported = true; } // square the inputs (while preserving the sign) to increase fine control // while permitting full power leftValue = Limit(leftValue); rightValue = Limit(rightValue); if (squaredInputs) { if (leftValue >= 0.0) { leftValue = (leftValue * leftValue); } else { leftValue = -(leftValue * leftValue); } if (rightValue >= 0.0) { rightValue = (rightValue * rightValue); } else { rightValue = -(rightValue * rightValue); } } SetLeftRightMotorOutputs(leftValue, rightValue); } /** * 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 increased for small * values */ void RobotDrive::ArcadeDrive(GenericHID* stick, bool 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.) * * @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 increased for small * values */ void RobotDrive::ArcadeDrive(GenericHID& stick, bool squaredInputs) { // simply call the full-featured ArcadeDrive with the appropriate values ArcadeDrive(stick.GetY(), stick.GetX(), 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. * * @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 RobotDrive::ArcadeDrive(GenericHID* moveStick, uint32_t moveAxis, GenericHID* rotateStick, uint32_t rotateAxis, bool squaredInputs) { float moveValue = moveStick->GetRawAxis(moveAxis); float 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. * * @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 RobotDrive::ArcadeDrive(GenericHID& moveStick, uint32_t moveAxis, GenericHID& rotateStick, uint32_t rotateAxis, bool squaredInputs) { float moveValue = moveStick.GetRawAxis(moveAxis); float rotateValue = rotateStick.GetRawAxis(rotateAxis); ArcadeDrive(moveValue, rotateValue, squaredInputs); } /** * 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 RobotDrive::ArcadeDrive(float moveValue, float rotateValue, bool squaredInputs) { static bool reported = false; if (!reported) { reported = true; } // local variables to hold the computed PWM values for the motors float leftMotorOutput; float rightMotorOutput; moveValue = Limit(moveValue); rotateValue = Limit(rotateValue); if (squaredInputs) { // square the inputs (while preserving the sign) to increase fine control // while permitting full power if (moveValue >= 0.0) { moveValue = (moveValue * moveValue); } else { moveValue = -(moveValue * moveValue); } if (rotateValue >= 0.0) { rotateValue = (rotateValue * rotateValue); } else { 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); } /** * 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 RobotDrive::MecanumDrive_Cartesian(float x, float y, float rotation, float gyroAngle) { static bool reported = false; if (!reported) { reported = true; } double xIn = x; double yIn = y; // Negate y for the joystick. yIn = -yIn; // Compenstate 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_invertedMotors[kFrontLeftMotor] * m_maxOutput); m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_invertedMotors[kFrontRightMotor] * m_maxOutput); m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_invertedMotors[kRearLeftMotor] * m_maxOutput); m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_invertedMotors[kRearRightMotor] * m_maxOutput); // FIXME: m_safetyHelper->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 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 RobotDrive::MecanumDrive_Polar(float magnitude, float direction, float rotation) { static bool reported = false; if (!reported) { reported = true; } // Normalized for full power along the Cartesian axes. magnitude = Limit(magnitude) * sqrt(2.0); // The rollers are at 45 degree angles. double dirInRad = (direction + 45.0) * 3.14159 / 180.0; double cosD = cos(dirInRad); double sinD = 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_invertedMotors[kFrontLeftMotor] * m_maxOutput); m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_invertedMotors[kFrontRightMotor] * m_maxOutput); m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_invertedMotors[kRearLeftMotor] * m_maxOutput); m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_invertedMotors[kRearRightMotor] * m_maxOutput); // FIXME: m_safetyHelper->Feed(); } /** * 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 RobotDrive::HolonomicDrive(float magnitude, float direction, float 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 "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. */ void RobotDrive::SetLeftRightMotorOutputs(float leftOutput, float rightOutput) { wpi_assert(m_rearLeftMotor != nullptr && m_rearRightMotor != nullptr); if (m_frontLeftMotor != nullptr) m_frontLeftMotor->Set( Limit(leftOutput) * m_invertedMotors[kFrontLeftMotor] * m_maxOutput); m_rearLeftMotor->Set( Limit(leftOutput) * m_invertedMotors[kRearLeftMotor] * m_maxOutput); if (m_frontRightMotor != nullptr) m_frontRightMotor->Set( -Limit(rightOutput) * m_invertedMotors[kFrontRightMotor] * m_maxOutput); m_rearRightMotor->Set( -Limit(rightOutput) * m_invertedMotors[kRearRightMotor] * m_maxOutput); // FIXME: m_safetyHelper->Feed(); } /** * Limit motor values to the -1.0 to +1.0 range. */ float RobotDrive::Limit(float num) { if (num > 1.0) { return 1.0; } if (num < -1.0) { return -1.0; } return num; } /** * Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0. */ void RobotDrive::Normalize(double* wheelSpeeds) { double maxMagnitude = fabs(wheelSpeeds[0]); int32_t i; for (i = 1; i < kMaxNumberOfMotors; i++) { double temp = fabs(wheelSpeeds[i]); if (maxMagnitude < temp) maxMagnitude = temp; } if (maxMagnitude > 1.0) { for (i = 0; i < kMaxNumberOfMotors; i++) { wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude; } } } /** * Rotate a vector in Cartesian space. */ void RobotDrive::RotateVector(double& x, double& y, double angle) { double cosA = cos(angle * (3.14159 / 180.0)); double sinA = sin(angle * (3.14159 / 180.0)); double xOut = x * cosA - y * sinA; double yOut = x * sinA + y * cosA; x = xOut; y = yOut; } /** * 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 RobotDrive::SetInvertedMotor(MotorType motor, bool isInverted) { if (motor < 0 || motor > 3) { wpi_setWPIError(InvalidMotorIndex); return; } m_invertedMotors[motor] = isInverted ? -1 : 1; } /** * 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 RobotDrive::SetSensitivity(float 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. */ void RobotDrive::SetMaxOutput(double maxOutput) { m_maxOutput = maxOutput; } void RobotDrive::SetExpiration(float timeout) { // FIXME: m_safetyHelper->SetExpiration(timeout); } float RobotDrive::GetExpiration() const { return -1; // FIXME: return m_safetyHelper->GetExpiration(); } bool RobotDrive::IsAlive() const { return true; // FIXME: m_safetyHelper->IsAlive(); } bool RobotDrive::IsSafetyEnabled() const { return false; // FIXME: return m_safetyHelper->IsSafetyEnabled(); } void RobotDrive::SetSafetyEnabled(bool enabled) { // FIXME: m_safetyHelper->SetSafetyEnabled(enabled); } void RobotDrive::GetDescription(std::ostringstream& desc) const { desc << "RobotDrive"; } void RobotDrive::StopMotor() { if (m_frontLeftMotor != nullptr) m_frontLeftMotor->Disable(); if (m_frontRightMotor != nullptr) m_frontRightMotor->Disable(); if (m_rearLeftMotor != nullptr) m_rearLeftMotor->Disable(); if (m_rearRightMotor != nullptr) m_rearRightMotor->Disable(); }