Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Improve Torque management for motors #493

Merged
merged 5 commits into from
Aug 14, 2024
Merged

Improve Torque management for motors #493

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
16ea5ee
replace moment_t by torque_t
nicolas-rabault Aug 1, 2024
7d32277
replace MOMENT command by TORQUE
nicolas-rabault Aug 1, 2024
2bbf414
[Gate] replace "moment" Json cmd by "torque"
nicolas-rabault Aug 1, 2024
e42e024
[Gate] Add target_torque management
nicolas-rabault Aug 1, 2024
41bd12e
Add target_torque management on motor profiles
nicolas-rabault Aug 1, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
62 changes: 31 additions & 31 deletions engine/OD/od_force.h
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading