Add target_torque management on motor profiles

Luos-io · Aug 1, 2024 · a0b9fa3 · a0b9fa3
1 parent 0c41aeb
commit a0b9fa3
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 0 deletions.
diff --git a/engine/profiles/servo_motor/profile_servo_motor.c b/engine/profiles/servo_motor/profile_servo_motor.c
@@ -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.current)
+            {
+                ForceOD_TorqueFromMsg((torque_t *)&servo_motor_profile->target_torque, msg);
+            }
+        }
         case LINEAR_POSITION:
         {
             // set the motor target linear position

diff --git a/engine/profiles/servo_motor/profile_servo_motor.h b/engine/profiles/servo_motor/profile_servo_motor.h
@@ -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];