From 5ab82ef48a39258c7fc18538e95c5f932a20d69e Mon Sep 17 00:00:00 2001
From: Laurens Valk <laurens@pybricks.com>
Date: Wed, 6 Jan 2021 16:33:28 +0100
Subject: [PATCH] pbio/servo: restore duty limit

This was temporarily removed during revisions of the drivers, so restore it.
---
 lib/pbio/src/dcmotor.c            | 2 +-
 lib/pbio/src/servo.c              | 4 ++++
 pybricks/common/pb_type_dcmotor.c | 4 ++--
 3 files changed, 7 insertions(+), 3 deletions(-)

diff --git a/lib/pbio/src/dcmotor.c b/lib/pbio/src/dcmotor.c
index b5c357cf9..bc6e949d5 100644
--- 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
index 09e4a1376..c1e5b8543 100644
--- 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
index 03ef0a44b..07e3ec1f8 100644
--- 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;