DriveBoard Class Reference

This class handles communication with the drive board on the rover by sending RoveComm packets over the network. More...

#include <DriveBoard.h>

Collaboration diagram for DriveBoard:

Public Member Functions
	DriveBoard ()
	Construct a new Drive Board::DriveBoard object.
	~DriveBoard ()
	Destroy the Drive Board::DriveBoard object.
diffdrive::DrivePowers	CalculateMove (const double dGoalSpeed, const double dGoalHeading, const double dActualHeading, const diffdrive::DifferentialControlMethod eKinematicsMethod=diffdrive::DifferentialControlMethod::eArcadeDrive, const bool bDriveBackwards=false, const bool bAlwaysProgressForward=false, const bool bSquareControlInput=false, const bool bCurvatureDriveAllowTurningWhileStopped=true)
	This method determines drive powers to make the Rover drive towards a given heading at a given speed.
void	SendDrive (const diffdrive::DrivePowers &stDrivePowers, const bool bEnableVariableDriveEffort=true)
	Sets the left and right drive powers of the drive board.
void	SendStop ()
	Stop the drivetrain of the Rover.
float	VariableDriveEffort ()
	This method calculates a multiplier that is applied to SetMaxDriveEffort() to adjust the speed of the rover in relation to the risk of the terrain.
void	SetMaxDriveEffort (const float fMaxDriveEffortMultiplier)
	Set the max power limits of the drive.
diffdrive::DrivePowers	GetDrivePowers () const
	Accessor for the current drive powers of the robot.
float	GetMaxDriveEffort () const
	Accessor for the current max drive effort multiplier.

Private Attributes
diffdrive::DrivePowers	m_stDrivePowers
std::unique_ptr< controllers::PIDController >	m_pPID
float	m_fDriveEffortMultiplier
std::shared_mutex	m_muDriveEffortMutex
const float	m_fMinSlope = constants::DRIVE_BOARD_MIN_SLOPE
const float	m_fMaxSlope = constants::DRIVE_BOARD_MAX_SLOPE
const float	m_fMinDamp = constants::DRIVE_BOARD_MIN_DAMP
const float	m_fMaxDamp = constants::DRIVE_BOARD_MAX_DAMP
const float	m_fRoll_w = constants::DRIVE_BOARD_ROLL_WEIGHT
const float	m_fPitch_w = constants::DRIVE_BOARD_PITCH_WEIGHT
const float	m_fYaw_w = constants::DRIVE_BOARD_YAW_WEIGHT
const std::function< void(const rovecomm::RoveCommPacket< float > &, const sockaddr_in &)>	SetMaxSpeedCallback
	Callback function that is called whenever RoveComm receives a new SETMAXSPEED packet.

Detailed Description

This class handles communication with the drive board on the rover by sending RoveComm packets over the network.

Author

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

Date

2023-09-21

Constructor & Destructor Documentation

DriveBoard()

DriveBoard::DriveBoard

(

)

Construct a new Drive Board::DriveBoard object.

Author

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

Date

2023-09-21

{
    // Initialize member variables.
    m_stDrivePowers.dLeftDrivePower  = 0.0;
    m_stDrivePowers.dRightDrivePower = 0.0;
    m_fDriveEffortMultiplier         = 1.0f;
 
    // Configure PID controller for heading hold function.
    m_pPID = std::make_unique<controllers::PIDController>(constants::DRIVE_PID_PROPORTIONAL,
                                                          constants::DRIVE_PID_INTEGRAL,
                                                          constants::DRIVE_PID_DERIVATIVE,
                                                          constants::DRIVE_PID_FEEDFORWARD);
    m_pPID->SetMaxSetpointDifference(constants::DRIVE_PID_MAX_ERROR);
    m_pPID->SetMaxIntegralEffort(constants::DRIVE_PID_MAX_INTEGRAL_TERM);
    m_pPID->SetOutputLimits(1.0);    // Autonomy internally always uses -1.0, 1.0 for turning and drive powers.
    m_pPID->SetOutputRampRate(constants::DRIVE_PID_MAX_RAMP_RATE);
    m_pPID->SetOutputFilter(constants::DRIVE_PID_OUTPUT_FILTER);
    m_pPID->SetTolerance(constants::DRIVE_PID_TOLERANCE);
    m_pPID->SetDirection(constants::DRIVE_PID_OUTPUT_REVERSED);
    m_pPID->EnableContinuousInput(0, 360);
 
    // Set RoveComm callbacks.
    if (network::g_pRoveCommUDPNode)
    {
        network::g_pRoveCommUDPNode->AddUDPCallback<float>(SetMaxSpeedCallback, manifest::Autonomy::COMMANDS.find("SETMAXSPEED")->second.DATA_ID);
    }
}

~DriveBoard()

DriveBoard::~DriveBoard

(

)

Destroy the Drive Board::DriveBoard object.

Author

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

Date

2023-09-21

{
    // Stop drivetrain.
    this->SendStop();
}

Here is the call graph for this function:

Member Function Documentation

CalculateMove()

diffdrive::DrivePowers DriveBoard::CalculateMove	(	const double	dGoalSpeed,
		const double	dGoalHeading,
		const double	dActualHeading,
		const diffdrive::DifferentialControlMethod	eKinematicsMethod = diffdrive::DifferentialControlMethod::eArcadeDrive,
		const bool	bDriveBackwards = false,
		const bool	bPreventPointTurns = false,
		const bool	bSquareControlInput = false,
		const bool	bCurvatureDriveAllowTurningWhileStopped = true
	)

This method determines drive powers to make the Rover drive towards a given heading at a given speed.

Parameters

dGoalSpeed	- The speed to drive at (-1 to 1)
dGoalHeading	- The angle to drive towards. (0 - 360) 0 is North.
dActualHeading	- The real angle that the Rover is current facing.
eKinematicsMethod	- The kinematics model to use for differential drive control. Enum within DifferentialDrive.hpp
bDriveBackwards	- If true, the rover will drive backwards while trying to hit the heading. This is used for the "backwards" mode of the drive control.
bPreventPointTurns	- If true, the rover will always maintain linear movement (forward or backward). Point turns will not be allowed.
bSquareControlInput	- If true, the control input will be squared before being applied.
bCurvatureDriveAllowTurningWhileStopped	- If true, the curvature drive method will allow turning while the rover is stopped.

Returns

diffdrive::DrivePowers - A struct containing two values. (left power, right power)

Author

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

Date

2026-04-24

{
    // Create instance variables.
    diffdrive::DrivePowers stOutputPowers;
 
    // Invert the speed if we are driving backwards
    double dSpeed         = bDriveBackwards ? -dGoalSpeed : dGoalSpeed;
    double dTargetHeading = dGoalHeading;
 
    // If driving backwards, we must point the rear of the rover towards the goal.
    if (bDriveBackwards)
    {
        // Offset the goal heading by 180 degrees and wrap around 360 to stay in bounds.
        dTargetHeading = std::fmod(dTargetHeading + 180.0, 360.0);
    }
 
    // Get control output from PID controller using our target heading.
    double dTurnOutput = m_pPID->Calculate(dActualHeading, dTargetHeading);
 
    // Calculate drive powers from inverse kinematics of goal speed and turning adjustment.
    switch (eKinematicsMethod)
    {
        case diffdrive::DifferentialControlMethod::eArcadeDrive:
        {
            // Check if the rover is allowed to pivot in place.
            if (!bPreventPointTurns)
            {
                // Based on our turn output, inverse-proportionally scale down our goal speed along a squared curve profile.
                // Because we square dTurnOutput, this correctly scales down negative speeds when driving backwards as well.
                dSpeed *= 1.0 - std::pow(dTurnOutput, 2);
            }
            // Calculate drive power with inverse kinematics.
            stOutputPowers = diffdrive::CalculateArcadeDrive(dSpeed, dTurnOutput, bSquareControlInput);
            break;
        }
        case diffdrive::DifferentialControlMethod::eCurvatureDrive:
        {
            // Check if the rover is allowed to pivot in place.
            if (!bPreventPointTurns)
            {
                // Based on our turn output, inverse-proportionally scale down our goal speed along a squared curve profile.
                dSpeed *= 1.0 - std::pow(dTurnOutput, 2);
            }
            // Calculate drive power with inverse kinematics.
            stOutputPowers = diffdrive::CalculateCurvatureDrive(dSpeed, 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:

SendDrive()

void DriveBoard::SendDrive	(	const diffdrive::DrivePowers &	stDrivePowers,
		const bool	bEnableVariableDriveEffort = true
	)

Sets the left and right drive powers of the drive board.

Parameters

stDrivePowers

- A struct containing info about the desired drive powers. Drive powers are always in between -1.0 and 1.0 no matter what constants or RoveComm say. the -1.0 to 1.0 range is automatically mapped to the correct DriveBoard range in this method.

Author

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

Date

2023-09-21

{
    // Limit input values (-1.0 to 1.0).
    double dLeftInput  = std::clamp(stDrivePowers.dLeftDrivePower, -1.0, 1.0);
    double dRightInput = std::clamp(stDrivePowers.dRightDrivePower, -1.0, 1.0);
 
    // -------------------------------------------------------------------------
    // Decouple Linear and Angular components to fix low-speed turning.
    // -------------------------------------------------------------------------
    // Separate Linear (Forward/Back) and Angular (Turn) power.
    double dLinearPower  = (dLeftInput + dRightInput) / 2.0;
    double dAngularPower = (dLeftInput - dRightInput) / 2.0;
 
    // Fetch the base operator-set speed multiplier safely.
    float fCombinedMultiplier = 1.0f;
    {
        std::shared_lock<std::shared_mutex> lkDriveEffortLock(m_muDriveEffortMutex);
        fCombinedMultiplier = m_fDriveEffortMultiplier;
    }
 
    // If variable drive effort is enabled, scale the base RoveComm multiplier further based on terrain.
    if (bEnableVariableDriveEffort)
    {
        float fTerrainDampening = VariableDriveEffort();
        fCombinedMultiplier *= fTerrainDampening;
    }
 
    // Apply the Combined Speed Multiplier ONLY to the Linear component.
    // This slows down the travel speed based on operator limits AND terrain safety,
    // but keeps full turning torque available.
    dLinearPower *= fCombinedMultiplier;
 
    // Reconstruct Left and Right powers.
    double dLeftSpeed  = dLinearPower + dAngularPower;
    double dRightSpeed = dLinearPower - dAngularPower;
 
    // Desaturate the output to preserve the turning ratio if it exceeds the max.
    // If we commanded (1.0, 0.5) and scaled linear by 0.1, we might get (0.1, 0.05).
    // But if turning adds significant power, we might exceed 1.0.
    double dMaxMagnitude = std::max(std::abs(dLeftSpeed), std::abs(dRightSpeed));
    if (dMaxMagnitude > 1.0)
    {
        dLeftSpeed /= dMaxMagnitude;
        dRightSpeed /= dMaxMagnitude;
    }
 
    // -------------------------------------------------------------------------
    // If the min and max drive effort have been set to 0, then just send zero powers.
    // Limit the power to max and min effort defined in constants (Slope Safety).
    m_stDrivePowers.dLeftDrivePower  = std::clamp(float(dLeftSpeed), constants::DRIVE_MIN_POWER, constants::DRIVE_MAX_POWER);
    m_stDrivePowers.dRightDrivePower = std::clamp(float(dRightSpeed), constants::DRIVE_MIN_POWER, constants::DRIVE_MAX_POWER);
 
    // Construct a RoveComm packet with the drive data.
    rovecomm::RoveCommPacket<float> stPacket;
    stPacket.unDataId    = manifest::Core::COMMANDS.find("DRIVELEFTRIGHT")->second.DATA_ID;
    stPacket.unDataCount = manifest::Core::COMMANDS.find("DRIVELEFTRIGHT")->second.DATA_COUNT;
    stPacket.eDataType   = manifest::Core::COMMANDS.find("DRIVELEFTRIGHT")->second.DATA_TYPE;
    stPacket.vData.emplace_back(m_stDrivePowers.dLeftDrivePower);
    stPacket.vData.emplace_back(m_stDrivePowers.dRightDrivePower);
 
    // Send drive command over RoveComm to drive board.
    if (network::g_pRoveCommUDPNode)
    {
        // Check if we should send packets to the SIM or board.
        const char* cIPAddress = constants::MODE_SIM ? constants::SIM_IP_ADDRESS.c_str() : manifest::Core::IP_ADDRESS.IP_STR.c_str();
        // Send packet.
        network::g_pRoveCommUDPNode->SendUDPPacket(stPacket, cIPAddress, constants::ROVECOMM_OUTGOING_UDP_PORT);
    }
 
    // Submit logger message.
    LOG_DEBUG(logging::g_qSharedLogger, "Driving at: ({}, {})", m_stDrivePowers.dLeftDrivePower, m_stDrivePowers.dRightDrivePower);
}

Here is the call graph for this function:

Here is the caller graph for this function:

SendStop()

void DriveBoard::SendStop

(

)

Stop the drivetrain of the Rover.

Author

Eli Byrd (edbgk.nosp@m.k@ms.nosp@m.t.edu)

Date

2023-06-18

{
    // Update member variables with new target speeds.
    m_stDrivePowers.dLeftDrivePower  = 0.0;
    m_stDrivePowers.dRightDrivePower = 0.0;
 
    // Construct a RoveComm packet with the drive data.
    rovecomm::RoveCommPacket<float> stPacket;
    stPacket.unDataId    = manifest::Core::COMMANDS.find("DRIVELEFTRIGHT")->second.DATA_ID;
    stPacket.unDataCount = manifest::Core::COMMANDS.find("DRIVELEFTRIGHT")->second.DATA_COUNT;
    stPacket.eDataType   = manifest::Core::COMMANDS.find("DRIVELEFTRIGHT")->second.DATA_TYPE;
    stPacket.vData.emplace_back(m_stDrivePowers.dLeftDrivePower);
    stPacket.vData.emplace_back(m_stDrivePowers.dRightDrivePower);
    // Check if we should send packets to the SIM or board.
    const char* cIPAddress = constants::MODE_SIM ? constants::SIM_IP_ADDRESS.c_str() : manifest::Core::IP_ADDRESS.IP_STR.c_str();
    // Send drive command over RoveComm to drive board.
    if (network::g_pRoveCommUDPNode)
    {
        network::g_pRoveCommUDPNode->SendUDPPacket(stPacket, cIPAddress, constants::ROVECOMM_OUTGOING_UDP_PORT);
    }
    // Submit logger message.
    LOG_DEBUG(logging::g_qSharedLogger, "Sent stop powers to drivetrain.");
}

Here is the caller graph for this function:

VariableDriveEffort()

float DriveBoard::VariableDriveEffort

(

)

This method calculates a multiplier that is applied to SetMaxDriveEffort() to adjust the speed of the rover in relation to the risk of the terrain.

Returns

fMultiplier - A multiplier value between m_fMinDamp and m_fMaxDamp

Author

Hunter LeRette (hrlnp.nosp@m.c@ms.nosp@m.t.edu), Jordan Hoover (jh69n.nosp@m.@mst.nosp@m..edu), Aiden Buter (ab9hm.nosp@m.@mst.nosp@m..edu)

Date

2026-01-10

{
    // Get pointer to camera.
    std::shared_ptr<ZEDCamera> ExampleZEDCam1 = globals::g_pCameraHandler->GetZED(CameraHandler::ZEDCamName::eHeadMainCam);
    // Declare data structures to store data in.
    sl::SensorsData slSensorData;
 
    // Put in a request to have our empty sensors data variable filled with the most recent data from the camera.
    std::future<bool> fuCopyStatus = ExampleZEDCam1->RequestSensorsCopy(slSensorData);
    float fMultiplier              = 1;
 
    // Now we are ready to use the sensors data, let's make sure we have it or wait until we do.
    if (fuCopyStatus.get())
    {
        // Declare roll, pitch, yaw from sensor data
        float fRoll  = fabs(slSensorData.imu.pose.getEulerAngles(false).z);
        float fPitch = fabs(slSensorData.imu.pose.getEulerAngles(false).x);
        float fYaw   = slSensorData.imu.pose.getEulerAngles(false).y;
 
        // Calculate the risk factor to be applied to the linear polarization equation
        float fTheta = fRoll * (m_fRoll_w) + fPitch * (m_fPitch_w) + fYaw * (m_fYaw_w);
 
        // Clamp damping based on slope angle: Max damping on flat terrain, Min damping on risky terrain
        if (fTheta <= m_fMinSlope)
            fMultiplier = m_fMaxDamp;
        if (fTheta >= m_fMaxSlope)
            fMultiplier = m_fMinDamp;
 
        // Calculate multiplier using linear polarization
        const float fK = (m_fMaxDamp - m_fMinDamp) / (m_fMaxSlope - m_fMinSlope);
        float fD       = m_fMaxDamp - fK * (fTheta - m_fMinSlope);
 
        // Return multiplier
        fMultiplier = std::clamp(fD, m_fMinDamp, m_fMaxDamp);
    }
 
    return fMultiplier;
}

Here is the call graph for this function:

Here is the caller graph for this function:

SetMaxDriveEffort()

void DriveBoard::SetMaxDriveEffort

(

const float

fMaxDriveEffortMultiplier

)

Set the max power limits of the drive.

Parameters

fMaxDriveEffortMultiplier

- A multiplier from 0-1 for the max power output of the drive. Multiplier will be applied to constants::DRIVE_MIN_POWER and constants::DRIVE_MAX_POWER.

Author

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

Date

2024-03-15

{
    // Clamp the multiplier to the range [0, 1].
    float fClampedMaxDriveEffortMultiplier = std::clamp(fMaxDriveEffortMultiplier, 0.0f, constants::DRIVE_MAX_POWER);
 
    // Update member variables.
    std::unique_lock<std::shared_mutex> lkDriveEffortLock(m_muDriveEffortMutex);
    m_fDriveEffortMultiplier = fClampedMaxDriveEffortMultiplier;
}

GetDrivePowers()

diffdrive::DrivePowers DriveBoard::GetDrivePowers

(

)

const

Accessor for the current drive powers of the robot.

Returns

diffdrive::DrivePowers - A struct containing the left and right drive power of the drivetrain.

Author

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

Date

2023-10-20

{
    // Return the current drive powers.
    return m_stDrivePowers;
}

Here is the caller graph for this function:

GetMaxDriveEffort()

float DriveBoard::GetMaxDriveEffort

(

)

const

Accessor for the current max drive effort multiplier.

Returns

float - The current drive effort multiplier being applied to the drive powers. This is a value between 0 and 1 that is multiplied by the constants DRIVE_MIN_POWER and DRIVE_MAX_POWER to set the current power limits of the drive.

Author

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

Date

2026-05-17

{
    // Return the current max drive effort multiplier.
    std::shared_lock<std::shared_mutex> lkDriveEffortLock(m_muDriveEffortMutex);
    return m_fDriveEffortMultiplier;
}

Here is the caller graph for this function:

Member Data Documentation

SetMaxSpeedCallback

const std::function<void(const rovecomm::RoveCommPacket<float>&, const sockaddr_in&)> DriveBoard::SetMaxSpeedCallback

private

Initial value:

=
            [this](const rovecomm::RoveCommPacket<float>& stPacket, const sockaddr_in& stdAddr)
        {
            
            (void) stdAddr;
 
            
            float fClampedMultiplier = std::clamp(std::fabs(stPacket.vData[0]), 0.0f, 1.0f);
            
            {
                std::unique_lock<std::shared_mutex> lkDriveEffortLock(m_muDriveEffortMutex);
                m_fDriveEffortMultiplier = fClampedMultiplier;
            }
 
            
            LOG_NOTICE(logging::g_qSharedLogger, "Incoming SETMAXSPEED: {}", fClampedMultiplier);
        }

Callback function that is called whenever RoveComm receives a new SETMAXSPEED packet.

Author

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

Date

2024-03-03

        {
            // Not using this.
            (void) stdAddr;
 
            // Clamp the incoming multiplier to [0.0, 1.0].
            float fClampedMultiplier = std::clamp(std::fabs(stPacket.vData[0]), 0.0f, 1.0f);
            // Update member variable.
            {
                std::unique_lock<std::shared_mutex> lkDriveEffortLock(m_muDriveEffortMutex);
                m_fDriveEffortMultiplier = fClampedMultiplier;
            }
 
            // Submit logger message.
            LOG_NOTICE(logging::g_qSharedLogger, "Incoming SETMAXSPEED: {}", fClampedMultiplier);
        };

---

The documentation for this class was generated from the following files:

• src/drivers/DriveBoard.h
• src/drivers/DriveBoard.cpp