diffdrive Namespace Reference

Namespace containing algorithms related to calculating drive powers, odometry, trajectories, kinematics, etc of differential drive (tank drive) robots. More...

Classes
struct	DrivePowers
	This struct is used to store the left and right drive powers for the robot. Storing these values in a struct allows for easy handling and access to said variables. More...

Enumerations
enum class	DifferentialControlMethod { eArcadeDrive , eCurvatureDrive }

Functions
DrivePowers	CalculateTankDrive (double dLeftSpeed, double dRightSpeed, bool bSquareInputs=false)
	Tank drive inverse kinematics for differential drive robots.
DrivePowers	CalculateArcadeDrive (double dSpeed, double dRotation, const bool bSquareInputs=false)
	Arcade drive inverse kinematics for differential drive robots.
DrivePowers	CalculateCurvatureDrive (double dSpeed, double dRotation, const bool bAllowTurnInPlace, const bool bSquareInputs=false)
	Curvature drive inverse kinematics for differential drive robots. The rotation parameter controls the curvature of the robot's path rather than it's rate of heading change. This makes the robot more controllable at high speeds.
DrivePowers	CalculateMotorPowerFromHeading (double dGoalSpeed, double dGoalHeading, double dActualHeading, const DifferentialControlMethod eDriveMethod, controllers::PIDController &PID, const bool bSquareControlInput=false, const bool bCurvatureDriveAllowTurningWhileStopped=true)
	This function will calculate the drive powers for a given speed and absolute heading. The method used to get the drive powers is determined by the given enumerator (must be arcade or curvature). The turning rate or curvature is determined by the given PID controller which must be properly configured.

Detailed Description

Namespace containing algorithms related to calculating drive powers, odometry, trajectories, kinematics, etc of differential drive (tank drive) robots.

Each drive function provides different inverse kinematic relations for a differential drive robot.

This library uses the NWU axes convention (North-West-Up as external reference in the world frame). The positive X axis points ahead, the positive Y axis points to the left, and the positive Z axis points up. Rotations follow the right-hand rule, so counterclockwise rotation around the Z axis is positive.

Inputs smaller then 0.02 will be set to 0, and larger values will be scaled so that the full range is still used. This deadband value can be changed with SetDeadband().

Referenced from: https://github.com/wpilibsuite/allwpilib/

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2023-09-21

Enumeration Type Documentation

DifferentialControlMethod

enum class diffdrive::DifferentialControlMethod

strong

    {
        eArcadeDrive,      // Typical drive control method for flightsticks. Uses speed and turn input to determine drive powers.
        eCurvatureDrive    // Similar to arcade drive with flightsticks, but the current turning speed of the robot is dampened when moving fast.
    };

Function Documentation

CalculateTankDrive()

DrivePowers diffdrive::CalculateTankDrive ( double  dLeftSpeed, double  dRightSpeed, bool  bSquareInputs = false  )

inline

Tank drive inverse kinematics for differential drive robots.

Parameters

dLeftSpeed	- The left drive power input for the drive. (-1.0 - 1.0)
dRightSpeed	- The right drive power input for the drive. (-1.0 - 1.0)
bSquareInputs	- Decreases the input sensitivity at low input speeds.

Returns

DrivePowers - The result drive powers. [left, right]

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2023-09-22

    {
        // Limit the input powers.
        dLeftSpeed  = std::clamp(dLeftSpeed, -1.0, 1.0);
        dRightSpeed = std::clamp(dRightSpeed, -1.0, 1.0);
 
        // Determine if we should square the inputs.
        if (bSquareInputs)
        {
            // Square the inputs but keep the sign.
            dLeftSpeed  = std::copysign(dLeftSpeed * dLeftSpeed, dLeftSpeed);
            dRightSpeed = std::copysign(dRightSpeed * dRightSpeed, dRightSpeed);
        }
 
        // Return result drive powers.
        return {dLeftSpeed, dRightSpeed};
    }

Here is the caller graph for this function:

CalculateArcadeDrive()

DrivePowers diffdrive::CalculateArcadeDrive ( double  dSpeed, double  dRotation, const bool  bSquareInputs = false  )

inline

Arcade drive inverse kinematics for differential drive robots.

Parameters

dSpeed	- Speed at which the robot should drive forward/backward. Forward is positive. (-1.0 - 1.0)
dRotation	- The rotation rate of the robot. Clockwise is positive. (-1.0 - 1.0)
bSquareInputs	- Decreases the input sensitivity at low input speeds.

Returns

DrivePowers - The result drive powers. [left, right]

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2023-09-22

    {
        // Limit the input powers.
        dSpeed    = std::clamp(dSpeed, -1.0, 1.0);
        dRotation = std::clamp(dRotation, -1.0, 1.0);
 
        // Determine if we should square the inputs.
        if (bSquareInputs)
        {
            // Square the inputs but keep the sign.
            dSpeed    = std::copysign(dSpeed * dSpeed, dSpeed);
            dRotation = std::copysign(dRotation * dRotation, dRotation);
        }
 
        // Find the maximum possible value of throttle and turn along the vector that the speed and rotation is pointing.
        double dGreaterInput = std::max(std::abs(dSpeed), std::abs(dRotation));
        double dLesserInput  = std::min(std::abs(dSpeed), std::abs(dRotation));
        // If the biggest input is zero, then the wheel speeds should be zero.
        if (dGreaterInput == 0.0)
        {
            // Return zero drive power output.
            return {0.0, 0.0};
        }
 
        // Differential drive inverse kinematics for arcade drive.
        double dLeftSpeed  = dSpeed + dRotation;
        double dRightSpeed = dSpeed - dRotation;
        // Desaturate the input. Normalize to (-1.0, 1.0).
        double dSaturatedInput = (dGreaterInput + dLesserInput) / dGreaterInput;
        dLeftSpeed /= dSaturatedInput;
        dRightSpeed /= dSaturatedInput;
 
        // Return result drive powers.
        return {dLeftSpeed, dRightSpeed};
    }

Here is the caller graph for this function:

CalculateCurvatureDrive()

DrivePowers diffdrive::CalculateCurvatureDrive ( double  dSpeed, double  dRotation, const bool  bAllowTurnInPlace, const bool  bSquareInputs = false  )

inline

Curvature drive inverse kinematics for differential drive robots. The rotation parameter controls the curvature of the robot's path rather than it's rate of heading change. This makes the robot more controllable at high speeds.

Parameters

dSpeed	- Speed at which the robot should drive forward/backwards. Forward is positive. (-1.0, 1.0)
dRotation	- The normalized curvature of the robot. Clockwise is positive. (-1.0, 1.0)
bAllowTurnInPlace	- Whether or not forward input is required to turn. True makes this control exactly like a car.
bSquareInputs	- Decreases the input sensitivity at low input speeds.

Returns

DrivePowers - The result drive powers. [left, right]

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2023-09-22

    {
        // Create instance variables.
        double dLeftSpeed  = 0.0;
        double dRightSpeed = 0.0;
 
        // Limit the input powers.
        dSpeed    = std::clamp(dSpeed, -1.0, 1.0);
        dRotation = std::clamp(dRotation, -1.0, 1.0);
 
        // Determine if we should square the inputs.
        if (bSquareInputs)
        {
            // Square the inputs but keep the sign.
            dSpeed    = std::copysign(dSpeed * dSpeed, dSpeed);
            dRotation = std::copysign(dRotation * dRotation, dRotation);
        }
 
        // Check if turn-in-place is allowed.
        if (bAllowTurnInPlace && dSpeed <= 0.01)
        {
            // Differential drive inverse kinematics for curvature drive with turn while stopped.
            dLeftSpeed  = dSpeed + dRotation;
            dRightSpeed = dSpeed - dRotation;
        }
        else
        {
            // Differential drive inverse kinematics for curvature drive.
            dLeftSpeed  = dSpeed + std::abs(dSpeed) * dRotation;
            dRightSpeed = dSpeed - std::abs(dSpeed) * dRotation;
        }
 
        // Desaturate the input. Normalize to (-1.0, 1.0).
        double dMaxMagnitude = std::max(std::abs(dLeftSpeed), std::abs(dRightSpeed));
        if (dMaxMagnitude > 1.0)
        {
            dLeftSpeed /= dMaxMagnitude;
            dRightSpeed /= dMaxMagnitude;
        }
 
        // Return result drive powers.
        return {dLeftSpeed, dRightSpeed};
    }

Here is the caller graph for this function:

CalculateMotorPowerFromHeading()

DrivePowers diffdrive::CalculateMotorPowerFromHeading ( double  dGoalSpeed, double  dGoalHeading, double  dActualHeading, const DifferentialControlMethod  eDriveMethod, controllers::PIDController &  PID, const bool  bSquareControlInput = false, const bool  bCurvatureDriveAllowTurningWhileStopped = true  )

inline

This function will calculate the drive powers for a given speed and absolute heading. The method used to get the drive powers is determined by the given enumerator (must be arcade or curvature). The turning rate or curvature is determined by the given PID controller which must be properly configured.

Parameters

dGoalSpeed	- The goal speed for the robot.
dGoalHeading	- The goal absolute heading for the robot. (0-360 degrees, CW positive.)
dActualHeading	- The actual current heading of the robot.
eDriveMethod	- The differential drive method to use for navigation. MUST NOT BE TANK.
PID	- A reference to the PID controller to use for hitting the heading setpoint.
bSquareControlInput	- Can provide smoother control at goal heading.
bCurvatureDriveAllowTurningWhileStopped	- Allow DifferentialControlMethod::eCurvatureDrive to turn in-place.

Returns

DrivePowers - The resultant drive powers.

Note

This method does not support tank drive as a control method.

Author

clayjay3 (clayt.nosp@m.onra.nosp@m.ycowe.nosp@m.n@gm.nosp@m.ail.c.nosp@m.om)

Date

2023-10-19

    {
        // Create instance variables.
        DrivePowers stOutputPowers;
 
        // Get control output from PID controller.
        double dTurnOutput = PID.Calculate(dActualHeading, dGoalHeading);
 
        // Calculate drive powers from inverse kinematics of goal speed and turning adjustment.
        switch (eDriveMethod)
        {
            case DifferentialControlMethod::eArcadeDrive:
            {
                // Based on our turn output, inverse-proportionally scale down our goal speed along a squared curve profile. This helps with pivot turns.
                dGoalSpeed *= 1.0 - std::pow(dTurnOutput, 2);
                // Calculate drive power with inverse kinematics.
                stOutputPowers = CalculateArcadeDrive(dGoalSpeed, dTurnOutput, bSquareControlInput);
                break;
            }
            case DifferentialControlMethod::eCurvatureDrive:
            {
                // Based on our turn output, inverse-proportionally scale down our goal speed along a squared curve profile. This helps with pivot turns.
                dGoalSpeed *= 1.0 - std::pow(dTurnOutput, 2);
                // Calculate drive power with inverse kinematics.
                stOutputPowers = CalculateCurvatureDrive(dGoalSpeed, dTurnOutput, bCurvatureDriveAllowTurningWhileStopped, bSquareControlInput);
                break;
            }
            default:
            {
                // Submit logger message.
                LOG_ERROR(logging::g_qSharedLogger, "eTankDrive is not supported for the CalculateMotorPowerFromHeading() method!");
                break;
            }
        }
 
        // Return result powers.
        return stOutputPowers;
    }

Here is the call graph for this function:

Here is the caller graph for this function: