DifferentialDrive.hpp

Go to the documentation of this file.

 
#ifndef DIFFERENTIAL_DRIVE_HPP
#define DIFFERENTIAL_DRIVE_HPP
 
#include "../AutonomyLogging.h"
#include "../util/NumberOperations.hpp"
#include "controllers/PIDController.h"
 
#include <array>
#include <cmath>
 
 
 
namespace diffdrive
{
    // Declare public enums that are specific to and used within this namespace.
 
    // Enum for choosing the differential drive method for certain functions.
    // Enumerator used to specify what method of drive control to use.
    enum class DifferentialControlMethod
    {
        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.
    };
 
 
    struct DrivePowers
    {
        public:
            // Define public struct attributes.
            double dLeftDrivePower;
            double dRightDrivePower;
    };
 
 
    inline DrivePowers CalculateTankDrive(double dLeftSpeed, double dRightSpeed, bool bSquareInputs = false)
    {
        // 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};
    }
 
 
    inline DrivePowers CalculateArcadeDrive(double dSpeed, double dRotation, const bool bSquareInputs = false)
    {
        // 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};
    }
 
 
    inline DrivePowers CalculateCurvatureDrive(double dSpeed, double dRotation, const bool bAllowTurnInPlace, const bool bSquareInputs = false)
    {
        // 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};
    }
 
 
    inline DrivePowers CalculateMotorPowerFromHeading(double dGoalSpeed,
                                                      double dGoalHeading,
                                                      double dActualHeading,
                                                      const DifferentialControlMethod eDriveMethod,
                                                      controllers::PIDController& PID,
                                                      const bool bSquareControlInput                     = false,
                                                      const bool bCurvatureDriveAllowTurningWhileStopped = true)
    {
        // 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;
    }
}    // namespace diffdrive
#endif