pbio/servo: restore duty limit

This was temporarily removed during revisions of the drivers, so restore it.
pybricks · Jan 6, 2021 · 5ab82ef · 5ab82ef
1 parent 7aeadf0
commit 5ab82ef
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 3 deletions.
diff --git a/lib/pbio/src/dcmotor.c b/lib/pbio/src/dcmotor.c
@@ -101,7 +101,7 @@ pbio_error_t pbio_dcmotor_set_duty_cycle_sys(pbio_dcmotor_t *dcmotor, int32_t du
 }
 
 pbio_error_t pbio_dcmotor_set_duty_cycle_usr(pbio_dcmotor_t *dcmotor, int32_t duty_steps) {
-    pbio_error_t err = pbio_dcmotor_set_duty_cycle_sys(dcmotor,  duty_steps * PBDRV_MAX_DUTY / PBIO_DUTY_USER_STEPS);
+    pbio_error_t err = pbio_dcmotor_set_duty_cycle_sys(dcmotor, duty_steps);
     if (err != PBIO_SUCCESS) {
         return err;
     }

diff --git a/lib/pbio/src/servo.c b/lib/pbio/src/servo.c
@@ -187,6 +187,7 @@ pbio_error_t pbio_servo_control_update(pbio_servo_t *srv) {
 
         // Convert torques to duty cycle based on model
         duty_cycle = pbio_observer_torque_to_duty(&srv->observer, feedback_torque + feedforward_torque, battery_voltage);
+        duty_cycle = max(-srv->control.settings.max_duty, min(duty_cycle, srv->control.settings.max_duty));
 
         // Actutate the servo
         err = pbio_servo_actuate(srv, actuation, duty_cycle);
@@ -220,6 +221,9 @@ pbio_error_t pbio_servo_set_duty_cycle(pbio_servo_t *srv, int32_t duty_steps) {
         return PBIO_ERROR_INVALID_OP;
     }
 
+    // Limit to maximum configured value
+    duty_steps = max(-srv->control.settings.max_duty, min(duty_steps, srv->control.settings.max_duty));
+
     pbio_control_stop(&srv->control);
     return pbio_dcmotor_set_duty_cycle_usr(srv->dcmotor, duty_steps);
 }

diff --git a/pybricks/common/pb_type_dcmotor.c b/pybricks/common/pb_type_dcmotor.c
@@ -71,10 +71,10 @@ STATIC mp_obj_t common_DCMotor_duty(size_t n_args, const mp_obj_t *pos_args, mp_
 
     if (is_servo) {
         common_Motor_obj_t *self = MP_OBJ_TO_PTR(pos_args[0]);
-        pb_assert(pbio_servo_set_duty_cycle(self->srv, duty));
+        pb_assert(pbio_servo_set_duty_cycle(self->srv, duty * 100));
     } else {
         common_DCMotor_obj_t *self = MP_OBJ_TO_PTR(pos_args[0]);
-        pb_assert(pbio_dcmotor_set_duty_cycle_usr(self->dcmotor, duty));
+        pb_assert(pbio_dcmotor_set_duty_cycle_usr(self->dcmotor, duty * 100));
     }
 
     return mp_const_none;