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.

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.05)
        {
            // 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: