extmod/pbdevice: change power from bool to int

Some sensors require full power and have their builtin controls to
adjust it, while some lights have intensity controlled by the hub
directly, so we need more finegrained control than a bool.

Author: laurensvalk

bricks/ev3dev/pbdevice.c

@@ -218,7 +218,7 @@ void pbdevice_set_values(pbdevice_t *pbdev, uint8_t mode, int32_t *values, uint8
     pb_assert(PBIO_ERROR_NOT_SUPPORTED);
 }
 
-void pbdevice_set_power_supply(pbdevice_t *pbdev, bool on) {
+void pbdevice_set_power_supply(pbdevice_t *pbdev, int32_t duty) {
     pb_assert(PBIO_ERROR_NOT_SUPPORTED);
 }
 

bricks/stm32/pbdevice.c

@@ -178,14 +178,15 @@ void pbdevice_set_values(pbdevice_t *pbdev, uint8_t mode, int32_t *values, uint8
     wait(pbio_iodev_set_data_end, pbio_iodev_set_data_cancel, iodev);
 }
 
-// FIXME: iodev should take care of this based on device data
-void pbdevice_set_power_supply(pbdevice_t *pbdev, bool on) {
-    if (on) {
-        pb_assert(pbdrv_motor_set_duty_cycle(pbdev->iodev.port, -10000));
-        mp_hal_delay_ms(1000);
-    } else {
-        pb_assert(pbdrv_motor_coast(pbdev->iodev.port));
+void pbdevice_set_power_supply(pbdevice_t *pbdev, int32_t duty) {
+    // Bind user input to percentage
+    if (duty < 0) {
+        duty = 0;
+    } else if (duty > 100) {
+        duty = 100;
     }
+    // Apply duty cycle in reverse to activate power
+    pb_assert(pbdrv_motor_set_duty_cycle(pbdev->iodev.port, -100 * duty));
 }
 
 void pbdevice_get_info(pbdevice_t *pbdev, pbio_port_t *port, pbio_iodev_type_id_t *id, uint8_t *mode, uint8_t *num_values) {

extmod/modiodevices.c

@@ -50,8 +50,8 @@ STATIC mp_obj_t iodevices_LUMPDevice_make_new(const mp_obj_type_t *type, size_t
     #if PYBRICKS_PY_PUPDEVICES
     // FIXME: Read sensor capability flag to see which sensor uses power. As
     // a precaution, only enable power for selected known sensors for now.
-    bool power = (id == PBIO_IODEV_TYPE_ID_SPIKE_COLOR_SENSOR || id == PBIO_IODEV_TYPE_ID_SPIKE_ULTRASONIC_SENSOR);
-    pbdevice_set_power_supply(self->pbdev, power);
+    int32_t duty = (id == PBIO_IODEV_TYPE_ID_SPIKE_COLOR_SENSOR || id == PBIO_IODEV_TYPE_ID_SPIKE_ULTRASONIC_SENSOR) ? 100 : 0;
+    pbdevice_set_power_supply(self->pbdev, duty);
     #endif // PYBRICKS_PY_PUPDEVICES
 
     return MP_OBJ_FROM_PTR(self);

extmod/modpupdevices.c

@@ -2,6 +2,7 @@
 // Copyright (c) 2018-2020 The Pybricks Authors
 
 #include "py/mpconfig.h"
+#include "py/mphal.h"
 
 #if PYBRICKS_PY_PUPDEVICES
 
@@ -216,7 +217,7 @@ STATIC mp_obj_t pupdevices_ColorSensor_make_new(const mp_obj_type_t *type, size_
     self->pbdev = pbdevice_get_device(port_num, PBIO_IODEV_TYPE_ID_SPIKE_COLOR_SENSOR);
 
     // This sensor requires power, which iodevice does not do automatically yet
-    pbdevice_set_power_supply(self->pbdev, true);
+    pbdevice_set_power_supply(self->pbdev, 100);
 
     // Create an instance of the LightArray class
     self->lights = builtins_LightArray_obj_make_new(self->pbdev, PBIO_IODEV_MODE_PUP_COLOR_SENSOR__LIGHT, 3);
@@ -228,6 +229,9 @@ STATIC mp_obj_t pupdevices_ColorSensor_make_new(const mp_obj_type_t *type, size_
     // Save default settings
     pb_hsv_map_save_default(&self->color_map);
 
+    // This sensor needs some time to get values right after turning power on
+    mp_hal_delay_ms(1000);
+
     return MP_OBJ_FROM_PTR(self);
 }
 
@@ -353,7 +357,7 @@ STATIC mp_obj_t pupdevices_UltrasonicSensor_make_new(const mp_obj_type_t *type,
     self->pbdev = pbdevice_get_device(port_num, PBIO_IODEV_TYPE_ID_SPIKE_ULTRASONIC_SENSOR);
 
     // This sensor requires power, which iodevice does not do automatically yet
-    pbdevice_set_power_supply(self->pbdev, true);
+    pbdevice_set_power_supply(self->pbdev, 100);
 
     // Create an instance of the LightArray class
     self->lights = builtins_LightArray_obj_make_new(self->pbdev, PBIO_IODEV_MODE_PUP_ULTRASONIC_SENSOR__LIGHT, 4);

extmod/pbdevice.h

@@ -18,7 +18,7 @@ void pbdevice_get_values(pbdevice_t *pbdev, uint8_t mode, int32_t *values);
 
 void pbdevice_set_values(pbdevice_t *pbdev, uint8_t mode, int32_t *values, uint8_t num_values);
 
-void pbdevice_set_power_supply(pbdevice_t *pbdev, bool on);
+void pbdevice_set_power_supply(pbdevice_t *pbdev, int32_t duty);
 
 void pbdevice_get_info(pbdevice_t *pbdev, pbio_port_t *port, pbio_iodev_type_id_t *id, uint8_t *mode, uint8_t *num_values);