TorqueControl.c File Reference

#include "../../../Inc/Systems/Controller/TorqueControl.h"
#include "../../../Inc/Systems/ControllerSystem.h"
#include "../../../Inc/Utils/Common.h"
#include <math.h>

Include dependency graph for TorqueControl.c:

Go to the source code of this file.

Functions
void	initTorqueControl (TorqueControl *tc, Apps *apps, int hz, float maxTorque)
	Initializes the Torque Control Actuator with initial settings. More...
int	startTorqueControl (TorqueControl *tc)
	Starts the Torque Control Actuator. More...
int	setDesiredTorque (ControllerSystem *controller)
	Sets the desired torque for the Torque Control Actuator. More...

Function Documentation

initTorqueControl()

void initTorqueControl	(	TorqueControl *	tc,
		Apps *	apps,
		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 6 of file TorqueControl.c.

                                                                               {
    initControllerSystem(&tc->base, "Torque Control", hz, c_TORQUE, setDesiredTorque, tc);
    tc->desiredTorque = 0;
    tc->maxAllowedTorque = maxTorque;
    tc->apps = apps;
}

Here is the call graph for this function:

Here is the caller graph for this function:

setDesiredTorque()

int setDesiredTorque

(

ControllerSystem *

controller

)

Sets the desired torque for the Torque Control Actuator.

Parameters

controller

A pointer to the ControllerSystem structure.

Returns

_SUCCESS if the desired torque was set, _FAILURE otherwise.

Definition at line 27 of file TorqueControl.c.

                                                   {
    TorqueControl* tc = (TorqueControl*)controller->child;
    float pedalposition = getAppsPosition(tc->apps);
    float torque = pedalposition * tc->maxAllowedTorque;
 
    if (torque > tc->maxAllowedTorque) {
        printf("Desired torque exceeds the maximum allowed torque, stepping down to max\n");
        torque = tc->maxAllowedTorque;
    }
 
    // Perform any desired mapping, i.e. fit to sigmoid function.
    double normalized = torque / tc->maxAllowedTorque;  // Normalize to range [0, 1]
    double s_curve = 1.0 / (1.0 + exp(-10 * (normalized - 0.5)));
    tc->desiredTorque = s_curve * tc->maxAllowedTorque;
    tc->base.state = c_computed;
    
    #ifdef DEBUGn
    printf("Desired Torque: %f\r\n", tc->desiredTorque);
    #endif
 
    return _SUCCESS;
}

Here is the call graph for this function:

Here is the caller graph for this function:

startTorqueControl()

int startTorqueControl

(

TorqueControl *

tc

)

Starts the Torque Control Actuator.

Parameters

tc	A pointer to the TorqueControl structure.

Returns

_SUCCESS if the actuator was started, _FAILURE otherwise.

Definition at line 13 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->base) == _FAILURE) {
        printf("Torque Control Actuator is not in a safe state\n");
        return _FAILURE;
    }
    ENABLE(tc->base.system);
    tc->base.state = c_idle;
    return _SUCCESS;
}

Here is the caller graph for this function: