TorqueControl.c File Reference

#include "../../../Inc/Systems/Controller/TorqueControl.h"
#include "../../../Inc/Utils/Common.h"

Include dependency graph for TorqueControl.c:

Go to the source code of this file.

Functions
void	initTorqueControl (TorqueControl *tc, int hz, float maxTorque)
	Initializes the Torque Control Actuator with initial settings. More...
int	startTorqueControl (TorqueControl *tc)
void	setDesiredTorque (TorqueControl *tc, float torque)
	Sets the desired torque for the Torque Control Actuator. More...
void	setActualTorque (TorqueControl *tc, float torque)
void	updateTorqueControl (void *tc)
	Updates the Torque Control Actuator based on sensor inputs and calculations. More...
TorqueStatus	checkTorqueLimits (TorqueControl *tc)
	Checks if the actual torque is within the defined limits and desired range. More...
int	torqueSafetyCheck (void *tc)
	Checks the safety of the torque with multiple monitors. More...

Function Documentation

checkTorqueLimits()

TorqueStatus checkTorqueLimits

(

TorqueControl *

tc

)

Checks if the actual torque is within the defined limits and desired range.

TODO: move into torque police

Note

FIXME: When should each torque condition be in place?

Parameters

tc	A pointer to the TorqueControl structure.

Returns

The actual status of the torque.

Definition at line 58 of file TorqueControl.c.

                                                  {
    if (tc->actualTorque > tc->maxAllowedTorque) {
        return TORQUE_OVER_LIMIT;
    }
    else if (tc->actualTorque < 0) {
        return TORQUE_UNDER_LIMIT;
    }
 
    float lowerBound = tc->desiredTorque * (1 - TORQUE_ERROR_MARGIN);
    float upperBound = tc->desiredTorque * (1 + TORQUE_ERROR_MARGIN);
 
    if (tc->actualTorque < lowerBound || tc->actualTorque > upperBound) {
        return TORQUE_SENSOR_ERROR;
    }
 
    return TORQUE_OK;
}

Here is the caller graph for this function:

initTorqueControl()

void initTorqueControl	(	TorqueControl *	tc,
		int	hz,
		float	maxTorque
	)

Initializes the Torque Control Actuator with initial settings.

Parameters

tc	A pointer to the TorqueControl structure.
hz	Rate at which the sensor is called (in hz).
maxTorque	The maximum torque limit set for the system (in Nm).

Definition at line 4 of file TorqueControl.c.

                                                                   {
    initControllerSystem(&tc->base, "Torque Control", hz, c_TORQUE);
    tc->base.system.updateable.update = updateTorqueControl;
    tc->actualTorque = 0;
    tc->desiredTorque = 0;
    tc->maxAllowedTorque = maxTorque;
    tc->status = TORQUE_OK;
    tc->base.safety = torqueSafetyCheck;
}

Here is the call graph for this function:

Here is the caller graph for this function:

setActualTorque()

void setActualTorque	(	TorqueControl *	tc,
		float	torque
	)

Definition at line 39 of file TorqueControl.c.

                                                      {
    tc->actualTorque = torque;
}

setDesiredTorque()

void setDesiredTorque	(	TorqueControl *	tc,
		float	torque
	)

Sets the desired torque for the Torque Control Actuator.

Note

FIXME: Update to this to a map in the future.

Parameters

tc	A pointer to the TorqueControl structure.
torque	The desired torque to be set (in Nm).

Definition at line 27 of file TorqueControl.c.

                                                       {
    if (tc->base.safety(tc) == FAILURE) {
        printf("Torque Control Actuator is not in a safe state\n");
        return;
    }
    if (torque > tc->maxAllowedTorque) {
        printf("Desired torque exceeds the maximum allowed torque\n");
        return;
    }
    tc->desiredTorque = torque;
}

startTorqueControl()

int startTorqueControl

(

TorqueControl *

tc

)

Definition at line 14 of file TorqueControl.c.

                                          {
    if (tc->base.safety == NULL) {
        printf("Safety system not set for Torque Control\n");
        return FAILURE;
    }
    else if (tc->base.safety(tc) == FAILURE) {
        printf("Torque Control Actuator is not in a safe state\n");
        return FAILURE;
    }
    ENABLE(tc->base.system);
    return SUCCESS;
}

torqueSafetyCheck()

int torqueSafetyCheck

(

void *

tc

)

Checks the safety of the torque with multiple monitors.

Parameters

tc	A pointer to the TorqueControl structure.

Definition at line 76 of file TorqueControl.c.

                                {
    TorqueControl* tcPtr = (TorqueControl*)tc;
    if(tcPtr->base.num_monitors == 0) {
        printf("No monitors set for Torque Control\n");
        return FAILURE;
    }
    else if (tcPtr->status != TORQUE_OK) {
        printf("Torque Control Actuator is not in a safe state\n");
        return FAILURE;
    }
    return SUCCESS;
}

Here is the caller graph for this function:

updateTorqueControl()

void updateTorqueControl

(

void *

tc

)

Updates the Torque Control Actuator based on sensor inputs and calculations.

Parameters

tc	A pointer to the TorqueControl structure.

Definition at line 43 of file TorqueControl.c.

                                   {
    TorqueControl* tcPtr = (TorqueControl*)tc;
    if (tcPtr->base.safety(tc) == FAILURE) {
        printf("Torque Control Actuator is not in a safe state\n");
        return;
    }
    tcPtr->status = checkTorqueLimits(tc);
    if (tcPtr->status != TORQUE_OK) {
        printf("Torque Control Actuator is not in OK\n");
        return;
    }
    // TODO: Implement some way to send the torque where it needs to go
    // sendTorqueCommand(tc);
}

Here is the call graph for this function:

Here is the caller graph for this function: