Skip to content

Commit

Permalink
Merge pull request #493 from Luos-io/torque
Browse files Browse the repository at this point in the history 
Improve Torque management for motors
  • Loading branch information
@nicolas-rabault
nicolas-rabault authored Aug 14, 2024
2 parents 7407827 + 41bd12e commit 26ab1f8
Show file tree
Hide file tree
Showing 6 changed files with 149 additions and 104 deletions.
62 changes: 31 additions & 31 deletions engine/OD/od_force.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@
typedef struct
{
float raw;
} moment_t;
} torque_t;

typedef struct
{
Expand All @@ -28,124 +28,124 @@ typedef struct
* Function
******************************************************************************/

// moment are stored in Newton meter (Nm)
// torque are stored in Newton meter (Nm)
//******** Conversions ***********

// N.mm
static inline float ForceOD_MomentTo_N_mm(moment_t self)
static inline float ForceOD_TorqueTo_N_mm(torque_t self)
{
return self.raw * 1000.0f;
}

static inline moment_t ForceOD_MomentFrom_N_mm(float n_mm)
static inline torque_t ForceOD_TorqueFrom_N_mm(float n_mm)
{
moment_t self;
torque_t self;
self.raw = n_mm / 1000.0f;
return self;
}

// N.cm
static inline float ForceOD_MomentTo_N_cm(moment_t self)
static inline float ForceOD_TorqueTo_N_cm(torque_t self)
{
return self.raw * 100.0f;
}

static inline moment_t ForceOD_MomentFrom_N_cm(float n_cm)
static inline torque_t ForceOD_TorqueFrom_N_cm(float n_cm)
{
moment_t self;
torque_t self;
self.raw = n_cm / 100.0f;
return self;
}

// N.m
static inline float ForceOD_MomentTo_N_m(moment_t self)
static inline float ForceOD_TorqueTo_N_m(torque_t self)
{
return self.raw;
}

static inline moment_t ForceOD_MomentFrom_N_m(float n_m)
static inline torque_t ForceOD_TorqueFrom_N_m(float n_m)
{
moment_t self;
torque_t self;
self.raw = n_m;
return self;
}

// kgf.mm
static inline float ForceOD_MomentTo_kgf_mm(moment_t self)
static inline float ForceOD_TorqueTo_kgf_mm(torque_t self)
{
return self.raw * 101.97f;
}

static inline moment_t ForceOD_MomentFrom_kgf_mm(float kgf_mm)
static inline torque_t ForceOD_TorqueFrom_kgf_mm(float kgf_mm)
{
moment_t self;
torque_t self;
self.raw = kgf_mm / 101.97f;
return self;
}

// kgf.cm
static inline float ForceOD_MomentTo_kgf_cm(moment_t self)
static inline float ForceOD_TorqueTo_kgf_cm(torque_t self)
{
return self.raw * 10.2f;
}

static inline moment_t ForceOD_MomentFrom_kgf_cm(float kgf_cm)
static inline torque_t ForceOD_TorqueFrom_kgf_cm(float kgf_cm)
{
moment_t self;
torque_t self;
self.raw = kgf_cm / 10.2f;
return self;
}

// kgf.m
static inline float ForceOD_MomentTo_kgf_m(moment_t self)
static inline float ForceOD_TorqueTo_kgf_m(torque_t self)
{
return self.raw * 0.102f;
}

static inline moment_t ForceOD_MomentFrom_kgf_m(float kgf_m)
static inline torque_t ForceOD_TorqueFrom_kgf_m(float kgf_m)
{
moment_t self;
torque_t self;
self.raw = kgf_m / 0.102f;
return self;
}

// ozf.in
static inline float ForceOD_MomentTo_ozf_in(moment_t self)
static inline float ForceOD_TorqueTo_ozf_in(torque_t self)
{
return self.raw * 141.612f;
}

static inline moment_t ForceOD_MomentFrom_ozf_in(float ozf_in)
static inline torque_t ForceOD_TorqueFrom_ozf_in(float ozf_in)
{
moment_t self;
torque_t self;
self.raw = ozf_in / 141.612f;
return self;
}

// lbf.in
static inline float ForceOD_MomentTo_lbf_in(moment_t self)
static inline float ForceOD_TorqueTo_lbf_in(torque_t self)
{
return self.raw * 8.851f;
}

static inline moment_t ForceOD_MomentFrom_lbf_in(float lbf_in)
static inline torque_t ForceOD_TorqueFrom_lbf_in(float lbf_in)
{
moment_t self;
torque_t self;
self.raw = lbf_in / 8.851f;
return self;
}

//******** Messages management ***********
static inline void ForceOD_MomentToMsg(const moment_t *const self, msg_t *const msg)
static inline void ForceOD_TorqueToMsg(const torque_t *const self, msg_t *const msg)
{
LUOS_ASSERT(self);
LUOS_ASSERT(msg);
msg->header.cmd = MOMENT;
memcpy(msg->data, self, sizeof(moment_t));
msg->header.size = sizeof(moment_t);
msg->header.cmd = TORQUE;
memcpy(msg->data, self, sizeof(torque_t));
msg->header.size = sizeof(torque_t);
}

static inline void ForceOD_MomentFromMsg(moment_t *const self, const msg_t *const msg)
static inline void ForceOD_TorqueFromMsg(torque_t *const self, const msg_t *const msg)
{
LUOS_ASSERT(self);
LUOS_ASSERT(msg);
Expand Down
4 changes: 2 additions & 2 deletions engine/core/inc/luos_list.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ typedef enum
TEMPERATURE, // temperature_t (°C)
TIME, // time Second (float)
FORCE, // force_t (Newton N)
MOMENT, // moment_t (Newton meter N.m)
TORQUE, // torque_t (Newton meter N.m)
CONTROL, // control_mode (control_mode_t)
TEXT, // ASCII string
PRESSURE, // pressure_t (Pa)
Expand Down Expand Up @@ -87,7 +87,7 @@ typedef enum
CURRENT_LIMIT, // float(A)
ANGULAR_SPEED_LIMIT, // min angular_speed_t (deg/s), max angular_speed_t (deg/s)
LINEAR_SPEED_LIMIT, // min linear_speed_t (m/s), max linear_speed_t (m/s)
TORQUE_LIMIT, // max moment_t (Nm)
TORQUE_LIMIT, // max torque_t (Nm)
TEMPERATURE_LIMIT, // Max temperature_t (°C)

// Specific register
Expand Down
8 changes: 8 additions & 0 deletions engine/profiles/servo_motor/profile_servo_motor.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -152,6 +152,14 @@ void ProfileServo_Handler(service_t *service, const msg_t *msg)
}
}
break;
case TORQUE:
{
// set the motor target torque
if (servo_motor_profile->mode.mode_torque)
{
ForceOD_TorqueFromMsg((torque_t *)&servo_motor_profile->target_torque, msg);
}
}
case LINEAR_POSITION:
{
// set the motor target linear position
Expand Down
1 change: 1 addition & 0 deletions engine/profiles/servo_motor/profile_servo_motor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,7 @@ typedef struct
servo_motor_mode_t mode;
angular_position_t target_angular_position;
angular_speed_t target_angular_speed;
torque_t target_torque;

// limits
angular_position_t limit_angular_position[2];
Expand Down
138 changes: 69 additions & 69 deletions test/tests_core/tests_od/test_force/main.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,113 +2,113 @@
#include "unit_test.h"
#include "od_force.h"

void unittest_Od_forceMoment(void)
void unittest_Od_forceTorque(void)
{
NEW_TEST_CASE("Force moment FROM test");
NEW_TEST_CASE("Force morque FROM test");
{
moment_t moment;
moment_t moment_ref = {90.5};
torque_t morque;
torque_t morque_ref = {90.5};

NEW_STEP("Force moment FROM Nm test");
moment = ForceOD_MomentFrom_N_m(90.5);
TEST_ASSERT_EQUAL((uint32_t)moment_ref.raw, (uint32_t)moment.raw);
NEW_STEP("Force moment FROM Nmm test");
moment = ForceOD_MomentFrom_N_mm(90500);
TEST_ASSERT_EQUAL((uint32_t)moment_ref.raw, (uint32_t)moment.raw);
NEW_STEP("Force moment FROM Ncm test");
moment = ForceOD_MomentFrom_N_cm(9050);
TEST_ASSERT_EQUAL((uint32_t)moment_ref.raw, (uint32_t)moment.raw);
NEW_STEP("Force moment FROM Kgf/mm test");
moment = ForceOD_MomentFrom_kgf_mm(9228.43172745);
TEST_ASSERT_EQUAL((uint32_t)moment_ref.raw, (uint32_t)moment.raw);
NEW_STEP("Force moment FROM Kgf/cm test");
moment = ForceOD_MomentFrom_kgf_cm(922.843172745);
TEST_ASSERT_EQUAL((uint32_t)moment_ref.raw, (uint32_t)moment.raw);
NEW_STEP("Force moment FROM Kgf/m test");
moment = ForceOD_MomentFrom_kgf_m(9.22843172745);
TEST_ASSERT_EQUAL((uint32_t)moment_ref.raw, (uint32_t)moment.raw);
NEW_STEP("Force moment FROM ozf/in test");
moment = ForceOD_MomentFrom_ozf_in(12815.87956868);
TEST_ASSERT_EQUAL((uint32_t)moment_ref.raw, (uint32_t)moment.raw);
NEW_STEP("Force moment FROM lbf/in test");
moment = ForceOD_MomentFrom_lbf_in(800.9924635902);
TEST_ASSERT_EQUAL((uint32_t)moment_ref.raw, (uint32_t)moment.raw);
NEW_STEP("Force morque FROM Nm test");
morque = ForceOD_TorqueFrom_N_m(90.5);
TEST_ASSERT_EQUAL((uint32_t)morque_ref.raw, (uint32_t)morque.raw);
NEW_STEP("Force morque FROM Nmm test");
morque = ForceOD_TorqueFrom_N_mm(90500);
TEST_ASSERT_EQUAL((uint32_t)morque_ref.raw, (uint32_t)morque.raw);
NEW_STEP("Force morque FROM Ncm test");
morque = ForceOD_TorqueFrom_N_cm(9050);
TEST_ASSERT_EQUAL((uint32_t)morque_ref.raw, (uint32_t)morque.raw);
NEW_STEP("Force morque FROM Kgf/mm test");
morque = ForceOD_TorqueFrom_kgf_mm(9228.43172745);
TEST_ASSERT_EQUAL((uint32_t)morque_ref.raw, (uint32_t)morque.raw);
NEW_STEP("Force morque FROM Kgf/cm test");
morque = ForceOD_TorqueFrom_kgf_cm(922.843172745);
TEST_ASSERT_EQUAL((uint32_t)morque_ref.raw, (uint32_t)morque.raw);
NEW_STEP("Force morque FROM Kgf/m test");
morque = ForceOD_TorqueFrom_kgf_m(9.22843172745);
TEST_ASSERT_EQUAL((uint32_t)morque_ref.raw, (uint32_t)morque.raw);
NEW_STEP("Force morque FROM ozf/in test");
morque = ForceOD_TorqueFrom_ozf_in(12815.87956868);
TEST_ASSERT_EQUAL((uint32_t)morque_ref.raw, (uint32_t)morque.raw);
NEW_STEP("Force morque FROM lbf/in test");
morque = ForceOD_TorqueFrom_lbf_in(800.9924635902);
TEST_ASSERT_EQUAL((uint32_t)morque_ref.raw, (uint32_t)morque.raw);
}
NEW_TEST_CASE("Force moment TO test");
NEW_TEST_CASE("Force morque TO test");
{
moment_t moment = {90.5};
torque_t morque = {90.5};

NEW_STEP("Force moment TO Nm test");
float n_m = ForceOD_MomentTo_N_m(moment);
NEW_STEP("Force morque TO Nm test");
float n_m = ForceOD_TorqueTo_N_m(morque);
TEST_ASSERT_EQUAL(90.5, n_m);
NEW_STEP("Force moment TO Nmm test");
float n_mm = ForceOD_MomentTo_N_mm(moment);
NEW_STEP("Force morque TO Nmm test");
float n_mm = ForceOD_TorqueTo_N_mm(morque);
TEST_ASSERT_EQUAL(90500, n_mm);
NEW_STEP("Force moment TO Ncm test");
float n_cm = ForceOD_MomentTo_N_cm(moment);
NEW_STEP("Force morque TO Ncm test");
float n_cm = ForceOD_TorqueTo_N_cm(morque);
TEST_ASSERT_EQUAL(9050, n_cm);
NEW_STEP("Force moment TO Kgf/mm test");
float kgf_mm = ForceOD_MomentTo_kgf_mm(moment);
NEW_STEP("Force morque TO Kgf/mm test");
float kgf_mm = ForceOD_TorqueTo_kgf_mm(morque);
TEST_ASSERT_EQUAL(9228.43172745, kgf_mm);
NEW_STEP("Force moment TO Kgf/cm test");
float kgf_cm = ForceOD_MomentTo_kgf_cm(moment);
NEW_STEP("Force morque TO Kgf/cm test");
float kgf_cm = ForceOD_TorqueTo_kgf_cm(morque);
TEST_ASSERT_EQUAL(923, kgf_cm);
NEW_STEP("Force moment TO Kgf/m test");
float kgf_m = ForceOD_MomentTo_kgf_m(moment);
NEW_STEP("Force morque TO Kgf/m test");
float kgf_m = ForceOD_TorqueTo_kgf_m(morque);
TEST_ASSERT_EQUAL(9.22843172745, kgf_m);
NEW_STEP("Force moment TO ozf/in test");
float ozf_in = ForceOD_MomentTo_ozf_in(moment);
NEW_STEP("Force morque TO ozf/in test");
float ozf_in = ForceOD_TorqueTo_ozf_in(morque);
TEST_ASSERT_EQUAL(12815.87956868, ozf_in);
NEW_STEP("Force moment TO lbf/in test");
float lbf_in = ForceOD_MomentTo_lbf_in(moment);
NEW_STEP("Force morque TO lbf/in test");
float lbf_in = ForceOD_TorqueTo_lbf_in(morque);
TEST_ASSERT_EQUAL(801, lbf_in);
}
NEW_TEST_CASE("Force moment msg conversion test");
NEW_TEST_CASE("Force morque msg conversion test");
{
moment_t moment;
moment_t moment_ref = {90.5};
torque_t morque;
torque_t morque_ref = {90.5};
msg_t msg_ref;
msg_t msg;

NEW_STEP("Force moment msg conversion FROM test");
msg_ref.header.cmd = MOMENT;
msg_ref.header.size = sizeof(moment_t);
memcpy(msg_ref.data, &moment_ref, sizeof(moment_t));
ForceOD_MomentFromMsg(&moment, &msg_ref);
TEST_ASSERT_EQUAL((uint32_t)moment_ref.raw, (uint32_t)moment.raw);
NEW_STEP("Force moment msg conversion TO test");
ForceOD_MomentToMsg(&moment_ref, &msg);
NEW_STEP("Force morque msg conversion FROM test");
msg_ref.header.cmd = TORQUE;
msg_ref.header.size = sizeof(torque_t);
memcpy(msg_ref.data, &morque_ref, sizeof(torque_t));
ForceOD_TorqueFromMsg(&morque, &msg_ref);
TEST_ASSERT_EQUAL((uint32_t)morque_ref.raw, (uint32_t)morque.raw);
NEW_STEP("Force morque msg conversion TO test");
ForceOD_TorqueToMsg(&morque_ref, &msg);
TEST_ASSERT_EQUAL(msg_ref.header.cmd, msg.header.cmd);
TEST_ASSERT_EQUAL(msg_ref.header.size, msg.header.size);
TEST_ASSERT_EQUAL((uint32_t)((moment_t *)msg_ref.data)->raw, (uint32_t)((moment_t *)msg.data)->raw);
TEST_ASSERT_EQUAL((uint32_t)((torque_t *)msg_ref.data)->raw, (uint32_t)((torque_t *)msg.data)->raw);
}
NEW_TEST_CASE("Force moment msg conversion wrong values test");
NEW_TEST_CASE("Force morque msg conversion wrong values test");
{
RESET_ASSERT();
moment_t moment;
torque_t morque;
msg_t msg;
TRY
{
NEW_STEP("Force moment msg conversion TO wrong msg_t* value test");
ForceOD_MomentToMsg(&moment, NULL);
NEW_STEP("Force morque msg conversion TO wrong msg_t* value test");
ForceOD_TorqueToMsg(&morque, NULL);
}
TEST_ASSERT_TRUE(IS_ASSERT());
TRY
{
NEW_STEP("Force moment msg conversion TO wrong moment_t* value test");
ForceOD_MomentToMsg(NULL, &msg);
NEW_STEP("Force morque msg conversion TO wrong torque_t* value test");
ForceOD_TorqueToMsg(NULL, &msg);
}
TEST_ASSERT_TRUE(IS_ASSERT());
TRY
{
NEW_STEP("Force moment msg conversion FROM wrong msg_t* value test");
ForceOD_MomentFromMsg(&moment, NULL);
NEW_STEP("Force morque msg conversion FROM wrong msg_t* value test");
ForceOD_TorqueFromMsg(&morque, NULL);
}
TEST_ASSERT_TRUE(IS_ASSERT());
TRY
{
NEW_STEP("Force moment msg conversion FROM wrong moment_t* value test");
ForceOD_MomentFromMsg(NULL, &msg);
NEW_STEP("Force morque msg conversion FROM wrong torque_t* value test");
ForceOD_TorqueFromMsg(NULL, &msg);
}
TEST_ASSERT_TRUE(IS_ASSERT());
}
Expand Down Expand Up @@ -205,7 +205,7 @@ void unittest_Od_forceForce(void)
int main(int argc, char **argv)
{
UNITY_BEGIN();
UNIT_TEST_RUN(unittest_Od_forceMoment);
UNIT_TEST_RUN(unittest_Od_forceTorque);
UNIT_TEST_RUN(unittest_Od_forceForce);

UNITY_END();
Expand Down
Loading

0 comments on commit 26ab1f8

Please sign in to comment.