diff --git a/bricks/nxt/Makefile b/bricks/nxt/Makefile index 31a938c24..54a0565ef 100644 --- a/bricks/nxt/Makefile +++ b/bricks/nxt/Makefile @@ -211,6 +211,7 @@ PBIO_SRC_C = $(addprefix lib/pbio/,\ src/dcmotor.c \ src/drivebase.c \ src/error.c \ + src/iodev.c \ src/integrator.c \ src/logger.c \ src/main.c \ diff --git a/bricks/stm32/stm32.mk b/bricks/stm32/stm32.mk index 052aaa779..3aeae579d 100644 --- a/bricks/stm32/stm32.mk +++ b/bricks/stm32/stm32.mk @@ -433,6 +433,7 @@ PBIO_SRC_C = $(addprefix lib/pbio/,\ drv/clock/clock_stm32.c \ drv/core.c \ drv/counter/counter_core.c \ + drv/counter/counter_lpf2.c \ drv/counter/counter_stm32f0_gpio_quad_enc.c \ drv/gpio/gpio_stm32f0.c \ drv/gpio/gpio_stm32f4.c \ diff --git a/lib/pbio/drv/counter/counter_core.c b/lib/pbio/drv/counter/counter_core.c index 661837079..f2a6f93b4 100644 --- a/lib/pbio/drv/counter/counter_core.c +++ b/lib/pbio/drv/counter/counter_core.c @@ -10,8 +10,8 @@ #include #include -#include "../src/uartdev.h" #include "counter_ev3dev_stretch_iio.h" +#include "counter_lpf2.h" #include "counter_nxt.h" #include "counter_stm32f0_gpio_quad_enc.h" #include "counter_test.h" @@ -22,11 +22,11 @@ static pbdrv_counter_dev_t pbdrv_counter_devs[PBDRV_CONFIG_COUNTER_NUM_DEV]; void pbdrv_counter_init(void) { pbdrv_counter_ev3dev_stretch_iio_init(pbdrv_counter_devs); + pbdrv_counter_lpf2_init(pbdrv_counter_devs); pbdrv_counter_nxt_init(pbdrv_counter_devs); pbdrv_counter_stm32f0_gpio_quad_enc_init(pbdrv_counter_devs); pbdrv_counter_test_init(pbdrv_counter_devs); pbdrv_counter_virtual_init(pbdrv_counter_devs); - pbio_uartdev_counter_init(pbdrv_counter_devs); } pbio_error_t pbdrv_counter_get_dev(uint8_t id, pbdrv_counter_dev_t **dev) { diff --git a/lib/pbio/drv/counter/counter_lpf2.c b/lib/pbio/drv/counter/counter_lpf2.c new file mode 100644 index 000000000..13981ce3f --- /dev/null +++ b/lib/pbio/drv/counter/counter_lpf2.c @@ -0,0 +1,124 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2022 The Pybricks Authors + +// LPF2 Counter driver +// +// This driver parses type-specific position readouts from LPF2 motors. +// +// This driver currently requires that pbdrv_counter_lpf2_get_count gets +// called often enough to observe full-rotation transitions of motors that +// use absolute encoders. + +#include + +#if PBDRV_CONFIG_COUNTER_LPF2 + +#include + +#include +#include +#include +#include + +#include "counter.h" +#include "counter_lpf2.h" + +typedef struct { + int32_t rotations; + int32_t last_abs_pos; +} private_data_t; + +#define PBDRV_COUNTER_UNKNOWN_ABS_ANGLE (INT32_MAX) + +static private_data_t private_data[PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV]; + +static pbio_error_t pbdrv_counter_lpf2_get_count(pbdrv_counter_dev_t *dev, int32_t *count) { + + private_data_t *priv = dev->priv; + const pbdrv_counter_lpf2_platform_data_t *pdata = dev->pdata; + + // Get iodev, and test if still connected. + pbio_iodev_t *iodev; + pbio_error_t err = pbdrv_ioport_get_iodev(pdata->port_id, &iodev); + if (err != PBIO_SUCCESS) { + return err; + } + + // Ensure we are still dealing with a motor. + if (!PBIO_IODEV_IS_FEEDBACK_MOTOR(iodev)) { + return PBIO_ERROR_NO_DEV; + } + + // Ensure that we are on expected mode. + uint8_t mode_id = PBIO_IODEV_IS_ABS_MOTOR(iodev) ? + PBIO_IODEV_MODE_PUP_ABS_MOTOR__APOS: + PBIO_IODEV_MODE_PUP_REL_MOTOR__POS; + if (iodev->mode != mode_id) { + return PBIO_ERROR_INVALID_OP; + } + + // Get LPF2 data buffer. + uint8_t *data; + pbio_iodev_get_data(iodev, &data); + + // For incremental encoders, we can just return the LPF2 data directly. + if (!PBIO_IODEV_IS_ABS_MOTOR(iodev)) { + *count = pbio_get_uint32_le(data); + priv->last_abs_pos = PBDRV_COUNTER_UNKNOWN_ABS_ANGLE; + return PBIO_SUCCESS; + } + + // For absolute encoders, we need to keep track of whole rotations. + int32_t abs_pos = (int16_t)pbio_get_uint16_le(data); + + // Update rotation counter as encoder passes through -180 + if (abs_pos < -90 && priv->last_abs_pos > 90) { + priv->rotations += 1; + } + if (abs_pos > 90 && priv->last_abs_pos < -90) { + priv->rotations -= 1; + } + priv->last_abs_pos = abs_pos; + + // The total count is the position in this circle plus full rotations. + *count = priv->rotations * 360 + abs_pos; + return PBIO_SUCCESS; +} + +static pbio_error_t pbdrv_counter_lpf2_get_abs_count(pbdrv_counter_dev_t *dev, int32_t *count) { + private_data_t *priv = dev->priv; + + // Get total count value, which also updates absolute count value. + pbio_error_t err = pbdrv_counter_lpf2_get_count(dev, count); + if (err != PBIO_SUCCESS) { + return err; + } + + if (priv->last_abs_pos == PBDRV_COUNTER_UNKNOWN_ABS_ANGLE) { + return PBIO_ERROR_NOT_SUPPORTED; + } + + *count = priv->last_abs_pos; + + return PBIO_SUCCESS; +} + +static const pbdrv_counter_funcs_t pbdrv_counter_lpf2_funcs = { + .get_count = pbdrv_counter_lpf2_get_count, + .get_abs_count = pbdrv_counter_lpf2_get_abs_count, +}; + +void pbdrv_counter_lpf2_init(pbdrv_counter_dev_t *devs) { + for (int i = 0; i < PBIO_ARRAY_SIZE(private_data); i++) { + const pbdrv_counter_lpf2_platform_data_t *pdata = + &pbdrv_counter_lpf2_platform_data[i]; + private_data_t *priv = &private_data[i]; + pbdrv_counter_dev_t *dev = &devs[pdata->counter_id]; + + dev->pdata = pdata; + dev->funcs = &pbdrv_counter_lpf2_funcs; + dev->priv = priv; + } +} + +#endif // PBDRV_CONFIG_COUNTER_LPF2 diff --git a/lib/pbio/drv/counter/counter_lpf2.h b/lib/pbio/drv/counter/counter_lpf2.h new file mode 100644 index 000000000..d9e8e9268 --- /dev/null +++ b/lib/pbio/drv/counter/counter_lpf2.h @@ -0,0 +1,39 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2022 The Pybricks Authors + +#ifndef _INTERNAL_PVDRV_COUNTER_LPF2_H_ +#define _INTERNAL_PVDRV_COUNTER_LPF2_H_ + +#include + +#if PBDRV_CONFIG_COUNTER_LPF2 + +#if !PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV +#error Platform must define PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV +#endif + +#include + +#include +#include + +typedef struct { + /** The ID of the counter device provided by this instance. */ + uint8_t counter_id; + /** The I/O port identifier this counter is associated with. */ + pbio_port_id_t port_id; +} pbdrv_counter_lpf2_platform_data_t; + +// defined in platform/*/platform.c +extern const pbdrv_counter_lpf2_platform_data_t + pbdrv_counter_lpf2_platform_data[PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV]; + +void pbdrv_counter_lpf2_init(pbdrv_counter_dev_t *devs); + +#else // PBDRV_CONFIG_COUNTER_LPF2 + +#define pbdrv_counter_lpf2_init(devs) + +#endif // PBDRV_CONFIG_COUNTER_LPF2 + +#endif // _INTERNAL_PVDRV_COUNTER_LPF2_H_ diff --git a/lib/pbio/include/pbio/iodev.h b/lib/pbio/include/pbio/iodev.h index 22c337dc7..784cc0383 100644 --- a/lib/pbio/include/pbio/iodev.h +++ b/lib/pbio/include/pbio/iodev.h @@ -344,6 +344,28 @@ enum { PBIO_IODEV_MODE_PUP_COLOR_LIGHT_MATRIX__TRANS = 3, }; +/** + * Modes for Powered Up motors with relative encoders + */ +enum { + PBIO_IODEV_MODE_PUP_REL_MOTOR__POWER = 0, + PBIO_IODEV_MODE_PUP_REL_MOTOR__SPEED = 1, + PBIO_IODEV_MODE_PUP_REL_MOTOR__POS = 2, + PBIO_IODEV_MODE_PUP_REL_MOTOR__TEST = 3, +}; + +/** + * Modes for Powered Up motors with absolute encoders + */ +enum { + PBIO_IODEV_MODE_PUP_ABS_MOTOR__POWER = 0, + PBIO_IODEV_MODE_PUP_ABS_MOTOR__SPEED = 1, + PBIO_IODEV_MODE_PUP_ABS_MOTOR__POS = 2, + PBIO_IODEV_MODE_PUP_ABS_MOTOR__APOS = 3, + PBIO_IODEV_MODE_PUP_ABS_MOTOR__CALIB = 4, + PBIO_IODEV_MODE_PUP_ABS_MOTOR__STATS = 5, +}; + /** * Data types used by I/O devices. */ @@ -441,6 +463,14 @@ typedef enum { */ #define PBIO_IODEV_IS_FEEDBACK_MOTOR(d) ((d)->info->capability_flags & PBIO_IODEV_CAPABILITY_FLAG_HAS_MOTOR_REL_POS) +/** + * Macro for testing if I/O device is a motor with absolute position feedback. + * + * @param [in] d Pointer to pbio_iodev_t. + */ +#define PBIO_IODEV_IS_ABS_MOTOR(d) ((d)->info->capability_flags & PBIO_IODEV_CAPABILITY_FLAG_HAS_MOTOR_ABS_POS) + + /** * Mapping flags that describe the input and output values of an I/O device. diff --git a/lib/pbio/include/pbio/uartdev.h b/lib/pbio/include/pbio/uartdev.h index 74b50a186..975f7d809 100644 --- a/lib/pbio/include/pbio/uartdev.h +++ b/lib/pbio/include/pbio/uartdev.h @@ -13,7 +13,6 @@ typedef struct { uint8_t uart_id; /**< The ID of a UART device used by this uartdev */ - uint8_t counter_id; /**< The ID of a counter device provided by this uartdev */ } pbio_uartdev_platform_data_t; #if PBIO_CONFIG_UARTDEV diff --git a/lib/pbio/platform/city_hub/pbdrvconfig.h b/lib/pbio/platform/city_hub/pbdrvconfig.h index 8769ba300..fc6d83fa5 100644 --- a/lib/pbio/platform/city_hub/pbdrvconfig.h +++ b/lib/pbio/platform/city_hub/pbdrvconfig.h @@ -31,6 +31,8 @@ #define PBDRV_CONFIG_COUNTER (1) #define PBDRV_CONFIG_COUNTER_NUM_DEV (2) +#define PBDRV_CONFIG_COUNTER_LPF2 (1) +#define PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV (2) #define PBDRV_CONFIG_GPIO (1) #define PBDRV_CONFIG_GPIO_STM32F0 (1) diff --git a/lib/pbio/platform/city_hub/platform.c b/lib/pbio/platform/city_hub/platform.c index f776c1e8c..f1cdd5d9a 100644 --- a/lib/pbio/platform/city_hub/platform.c +++ b/lib/pbio/platform/city_hub/platform.c @@ -9,6 +9,7 @@ #include "../../drv/bluetooth/bluetooth_stm32_cc2640.h" #include "../../drv/button/button_gpio.h" +#include "../../drv/counter/counter_lpf2.h" #include "../../drv/ioport/ioport_lpf2.h" #include "../../drv/led/led_pwm.h" #include "../../drv/motor_driver/motor_driver_hbridge_pwm.h" @@ -161,6 +162,19 @@ void EXTI2_3_IRQHandler(void) { EXTI->PR = EXTI_PR_PR2; } +// counters + +const pbdrv_counter_lpf2_platform_data_t pbdrv_counter_lpf2_platform_data[PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV] = { + [COUNTER_PORT_A] = { + .counter_id = COUNTER_PORT_A, + .port_id = PBIO_PORT_ID_A, + }, + [COUNTER_PORT_B] = { + .counter_id = COUNTER_PORT_B, + .port_id = PBIO_PORT_ID_B, + }, +}; + // I/O ports const pbdrv_ioport_lpf2_platform_data_t pbdrv_ioport_lpf2_platform_data = { @@ -340,11 +354,9 @@ void USART3_6_IRQHandler(void) { const pbio_uartdev_platform_data_t pbio_uartdev_platform_data[PBIO_CONFIG_UARTDEV_NUM_DEV] = { [0] = { .uart_id = UART_ID_0, - .counter_id = COUNTER_PORT_A, }, [1] = { .uart_id = UART_ID_1, - .counter_id = COUNTER_PORT_B, }, }; diff --git a/lib/pbio/platform/debug/pbdrvconfig.h b/lib/pbio/platform/debug/pbdrvconfig.h index 8a28fd6e8..2d6ab9755 100644 --- a/lib/pbio/platform/debug/pbdrvconfig.h +++ b/lib/pbio/platform/debug/pbdrvconfig.h @@ -20,6 +20,8 @@ #define PBDRV_CONFIG_COUNTER (1) #define PBDRV_CONFIG_COUNTER_NUM_DEV (1) +#define PBDRV_CONFIG_COUNTER_LPF2 (1) +#define PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV (1) #define PBDRV_CONFIG_GPIO (1) #define PBDRV_CONFIG_GPIO_STM32F4 (1) diff --git a/lib/pbio/platform/debug/platform.c b/lib/pbio/platform/debug/platform.c index 8c81687d0..9612f2ed2 100644 --- a/lib/pbio/platform/debug/platform.c +++ b/lib/pbio/platform/debug/platform.c @@ -13,6 +13,7 @@ #include "../../drv/adc/adc_stm32_hal.h" #include "../../drv/button/button_gpio.h" +#include "../../drv/counter/counter_lpf2.h" #include "../../drv/led/led_pwm.h" #include "../../drv/pwm/pwm_stm32_tim.h" #include "../../drv/uart/uart_stm32f4_ll_irq.h" @@ -133,6 +134,8 @@ void DMA2_Stream0_IRQHandler(void) { pbdrv_adc_stm32_hal_handle_irq(); } +// buttons + const pbdrv_button_gpio_platform_t pbdrv_button_gpio_platform[PBDRV_CONFIG_BUTTON_GPIO_NUM_BUTTON] = { [0] = { .gpio = { .bank = GPIOC, .pin = 13 }, @@ -140,6 +143,15 @@ const pbdrv_button_gpio_platform_t pbdrv_button_gpio_platform[PBDRV_CONFIG_BUTTO } }; +// counters + +const pbdrv_counter_lpf2_platform_data_t pbdrv_counter_lpf2_platform_data[PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV] = { + [0] = { + .counter_id = 0, + .port_id = PBIO_PORT_ID_1, + }, +}; + // LED // TODO: these are move/city hub values @@ -230,7 +242,6 @@ const pbdrv_uart_stm32f4_ll_irq_platform_data_t const pbio_uartdev_platform_data_t pbio_uartdev_platform_data[PBIO_CONFIG_UARTDEV_NUM_DEV] = { [0] = { .uart_id = UART_ID_0, - .counter_id = COUNTER_DEV_0, }, }; diff --git a/lib/pbio/platform/essential_hub/pbdrvconfig.h b/lib/pbio/platform/essential_hub/pbdrvconfig.h index 5a5828ea8..272cbee04 100644 --- a/lib/pbio/platform/essential_hub/pbdrvconfig.h +++ b/lib/pbio/platform/essential_hub/pbdrvconfig.h @@ -44,6 +44,8 @@ #define PBDRV_CONFIG_COUNTER (1) #define PBDRV_CONFIG_COUNTER_NUM_DEV (2) +#define PBDRV_CONFIG_COUNTER_LPF2 (1) +#define PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV (2) #define PBDRV_CONFIG_GPIO (1) #define PBDRV_CONFIG_GPIO_STM32F4 (1) diff --git a/lib/pbio/platform/essential_hub/platform.c b/lib/pbio/platform/essential_hub/platform.c index 6f02a7ba3..a17d5c6c0 100644 --- a/lib/pbio/platform/essential_hub/platform.c +++ b/lib/pbio/platform/essential_hub/platform.c @@ -17,6 +17,7 @@ #include "../../drv/bluetooth/bluetooth_btstack.h" #include "../../drv/button/button_gpio.h" #include "../../drv/charger/charger_mp2639a.h" +#include "../../drv/counter/counter_lpf2.h" #include "../../drv/ioport/ioport_lpf2.h" #include "../../drv/led/led_pwm.h" #include "../../drv/motor_driver/motor_driver_hbridge_pwm.h" @@ -150,6 +151,19 @@ const pbdrv_charger_mp2639a_platform_data_t pbdrv_charger_mp2639a_platform_data .ib_adc_ch = 3, }; +// counters + +const pbdrv_counter_lpf2_platform_data_t pbdrv_counter_lpf2_platform_data[PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV] = { + [COUNTER_PORT_A] = { + .counter_id = COUNTER_PORT_A, + .port_id = PBIO_PORT_ID_A, + }, + [COUNTER_PORT_B] = { + .counter_id = COUNTER_PORT_B, + .port_id = PBIO_PORT_ID_B, + }, +}; + // I/O ports const pbdrv_ioport_lpf2_platform_data_t pbdrv_ioport_lpf2_platform_data = { @@ -381,15 +395,12 @@ void USART3_IRQHandler(void) { const pbio_uartdev_platform_data_t pbio_uartdev_platform_data[PBIO_CONFIG_UARTDEV_NUM_DEV] = { [COUNTER_PORT_A] = { .uart_id = UART_PORT_A, - .counter_id = COUNTER_PORT_A, }, [COUNTER_PORT_B] = { .uart_id = UART_PORT_B, - .counter_id = COUNTER_PORT_B, }, }; - // STM32 HAL integration // bootloader gives us 16MHz clock diff --git a/lib/pbio/platform/move_hub/pbdrvconfig.h b/lib/pbio/platform/move_hub/pbdrvconfig.h index aadaf2a22..50da36cf1 100644 --- a/lib/pbio/platform/move_hub/pbdrvconfig.h +++ b/lib/pbio/platform/move_hub/pbdrvconfig.h @@ -32,6 +32,8 @@ #define PBDRV_CONFIG_COUNTER_NUM_DEV (4) #define PBDRV_CONFIG_COUNTER_STM32F0_GPIO_QUAD_ENC (1) #define PBDRV_CONFIG_COUNTER_STM32F0_GPIO_QUAD_ENC_NUM_DEV (2) +#define PBDRV_CONFIG_COUNTER_LPF2 (1) +#define PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV (2) #define PBDRV_CONFIG_GPIO (1) #define PBDRV_CONFIG_GPIO_STM32F0 (1) diff --git a/lib/pbio/platform/move_hub/platform.c b/lib/pbio/platform/move_hub/platform.c index 50ba05133..df6751612 100644 --- a/lib/pbio/platform/move_hub/platform.c +++ b/lib/pbio/platform/move_hub/platform.c @@ -8,6 +8,7 @@ #include #include "../../drv/button/button_gpio.h" +#include "../../drv/counter/counter_lpf2.h" #include "../../drv/counter/counter_stm32f0_gpio_quad_enc.h" #include "../../drv/ioport/ioport_lpf2.h" #include "../../drv/led/led_pwm.h" @@ -67,6 +68,17 @@ const pbdrv_counter_stm32f0_gpio_quad_enc_platform_data_t }, }; +const pbdrv_counter_lpf2_platform_data_t pbdrv_counter_lpf2_platform_data[PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV] = { + [0] = { + .counter_id = COUNTER_PORT_C, + .port_id = PBIO_PORT_ID_C, + }, + [1] = { + .counter_id = COUNTER_PORT_D, + .port_id = PBIO_PORT_ID_D, + }, +}; + // I/O ports const pbdrv_ioport_lpf2_platform_data_t pbdrv_ioport_lpf2_platform_data = { @@ -289,13 +301,12 @@ void USART3_4_IRQHandler(void) { const pbio_uartdev_platform_data_t pbio_uartdev_platform_data[PBIO_CONFIG_UARTDEV_NUM_DEV] = { [0] = { .uart_id = UART_ID_0, - .counter_id = COUNTER_PORT_C, }, [1] = { .uart_id = UART_ID_1, - .counter_id = COUNTER_PORT_D, }, }; + #endif // PBIO_CONFIG_UARTDEV // special memory addresses defined in linker script diff --git a/lib/pbio/platform/prime_hub/pbdrvconfig.h b/lib/pbio/platform/prime_hub/pbdrvconfig.h index 5fa23762e..2944602bb 100644 --- a/lib/pbio/platform/prime_hub/pbdrvconfig.h +++ b/lib/pbio/platform/prime_hub/pbdrvconfig.h @@ -46,6 +46,8 @@ #define PBDRV_CONFIG_COUNTER (1) #define PBDRV_CONFIG_COUNTER_NUM_DEV (6) +#define PBDRV_CONFIG_COUNTER_LPF2 (1) +#define PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV (6) #define PBDRV_CONFIG_GPIO (1) #define PBDRV_CONFIG_GPIO_STM32F4 (1) diff --git a/lib/pbio/platform/prime_hub/platform.c b/lib/pbio/platform/prime_hub/platform.c index bcbfc8323..cdee36a71 100644 --- a/lib/pbio/platform/prime_hub/platform.c +++ b/lib/pbio/platform/prime_hub/platform.c @@ -15,6 +15,7 @@ #include "../../drv/bluetooth/bluetooth_btstack_uart_block_stm32_hal.h" #include "../../drv/bluetooth/bluetooth_btstack.h" #include "../../drv/charger/charger_mp2639a.h" +#include "../../drv/counter/counter_lpf2.h" #include "../../drv/ioport/ioport_lpf2.h" #include "../../drv/led/led_array_pwm.h" #include "../../drv/led/led_dual.h" @@ -144,6 +145,35 @@ const pbdrv_charger_mp2639a_platform_data_t pbdrv_charger_mp2639a_platform_data .iset_pwm_ch = 1, }; +// counters + +const pbdrv_counter_lpf2_platform_data_t pbdrv_counter_lpf2_platform_data[PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV] = { + [COUNTER_PORT_A] = { + .counter_id = COUNTER_PORT_A, + .port_id = PBIO_PORT_ID_A, + }, + [COUNTER_PORT_B] = { + .counter_id = COUNTER_PORT_B, + .port_id = PBIO_PORT_ID_B, + }, + [COUNTER_PORT_C] = { + .counter_id = COUNTER_PORT_C, + .port_id = PBIO_PORT_ID_C, + }, + [COUNTER_PORT_D] = { + .counter_id = COUNTER_PORT_D, + .port_id = PBIO_PORT_ID_D, + }, + [COUNTER_PORT_E] = { + .counter_id = COUNTER_PORT_E, + .port_id = PBIO_PORT_ID_E, + }, + [COUNTER_PORT_F] = { + .counter_id = COUNTER_PORT_F, + .port_id = PBIO_PORT_ID_F, + }, +}; + // I/O ports const pbdrv_ioport_lpf2_platform_data_t pbdrv_ioport_lpf2_platform_data = { @@ -622,31 +652,24 @@ void UART10_IRQHandler(void) { const pbio_uartdev_platform_data_t pbio_uartdev_platform_data[PBIO_CONFIG_UARTDEV_NUM_DEV] = { [COUNTER_PORT_A] = { .uart_id = UART_PORT_A, - .counter_id = COUNTER_PORT_A, }, [COUNTER_PORT_B] = { .uart_id = UART_PORT_B, - .counter_id = COUNTER_PORT_B, }, [COUNTER_PORT_C] = { .uart_id = UART_PORT_C, - .counter_id = COUNTER_PORT_C, }, [COUNTER_PORT_D] = { .uart_id = UART_PORT_D, - .counter_id = COUNTER_PORT_D, }, [COUNTER_PORT_E] = { .uart_id = UART_PORT_E, - .counter_id = COUNTER_PORT_E, }, [COUNTER_PORT_F] = { .uart_id = UART_PORT_F, - .counter_id = COUNTER_PORT_F, }, }; - // STM32 HAL integration // bootloader gives us 16MHz clock diff --git a/lib/pbio/platform/technic_hub/pbdrvconfig.h b/lib/pbio/platform/technic_hub/pbdrvconfig.h index ac410cf4b..51ad75fc6 100644 --- a/lib/pbio/platform/technic_hub/pbdrvconfig.h +++ b/lib/pbio/platform/technic_hub/pbdrvconfig.h @@ -36,6 +36,8 @@ #define PBDRV_CONFIG_COUNTER (1) #define PBDRV_CONFIG_COUNTER_NUM_DEV (4) +#define PBDRV_CONFIG_COUNTER_LPF2 (1) +#define PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV (4) #define PBDRV_CONFIG_GPIO (1) #define PBDRV_CONFIG_GPIO_STM32L4 (1) diff --git a/lib/pbio/platform/technic_hub/platform.c b/lib/pbio/platform/technic_hub/platform.c index 618ab3e5e..ea35d8d08 100644 --- a/lib/pbio/platform/technic_hub/platform.c +++ b/lib/pbio/platform/technic_hub/platform.c @@ -9,6 +9,7 @@ #include "../../drv/battery/battery_adc.h" #include "../../drv/bluetooth/bluetooth_stm32_cc2640.h" #include "../../drv/button/button_gpio.h" +#include "../../drv/counter/counter_lpf2.h" #include "../../drv/ioport/ioport_lpf2.h" #include "../../drv/led/led_pwm.h" #include "../../drv/motor_driver/motor_driver_hbridge_pwm.h" @@ -183,6 +184,27 @@ const pbdrv_button_gpio_platform_t pbdrv_button_gpio_platform[PBDRV_CONFIG_BUTTO }, }; +// counters + +const pbdrv_counter_lpf2_platform_data_t pbdrv_counter_lpf2_platform_data[PBDRV_CONFIG_COUNTER_LPF2_NUM_DEV] = { + [COUNTER_PORT_A] = { + .counter_id = COUNTER_PORT_A, + .port_id = PBIO_PORT_ID_A, + }, + [COUNTER_PORT_B] = { + .counter_id = COUNTER_PORT_B, + .port_id = PBIO_PORT_ID_B, + }, + [COUNTER_PORT_C] = { + .counter_id = COUNTER_PORT_C, + .port_id = PBIO_PORT_ID_C, + }, + [COUNTER_PORT_D] = { + .counter_id = COUNTER_PORT_D, + .port_id = PBIO_PORT_ID_D, + }, +}; + // I/O ports const pbdrv_ioport_lpf2_platform_data_t pbdrv_ioport_lpf2_platform_data = { @@ -548,23 +570,18 @@ void LPUART1_IRQHandler(void) { const pbio_uartdev_platform_data_t pbio_uartdev_platform_data[PBIO_CONFIG_UARTDEV_NUM_DEV] = { [0] = { .uart_id = UART_PORT_A, - .counter_id = COUNTER_PORT_A, }, [1] = { .uart_id = UART_PORT_B, - .counter_id = COUNTER_PORT_B, }, [2] = { .uart_id = UART_PORT_C, - .counter_id = COUNTER_PORT_C, }, [3] = { .uart_id = UART_PORT_D, - .counter_id = COUNTER_PORT_D, }, }; - // STM32 HAL integration // Default clock is 4MHz on boot diff --git a/lib/pbio/src/uartdev.c b/lib/pbio/src/uartdev.c index 0525eb2a8..cdb3fe76a 100644 --- a/lib/pbio/src/uartdev.c +++ b/lib/pbio/src/uartdev.c @@ -41,7 +41,6 @@ #include "pbio/port.h" #include "pbio/uartdev.h" #include "pbio/util.h" -#include "../drv/counter/counter.h" #include #define EV3_UART_MAX_MESSAGE_SIZE (LUMP_MAX_MSG_SIZE + 3) @@ -139,16 +138,12 @@ typedef enum { * @new_baud_rate: New baud rate that will be set with ev3_uart_change_bitrate * @info_flags: Flags indicating what information has already been read * from the data. - * @tacho_count: The tacho count received from an LPF2 motor - * @tacho_offset: Tacho count offset to account for unexpected jumps in LPF2 tacho data * @abs_pos: The absolute position received from an LPF2 motor * @tx_msg: Buffer to hold messages transmitted to the device * @rx_msg: Buffer to hold messages received from the device * @rx_msg_size: Size of the current message being received * @ext_mode: Extra mode adder for Powered Up devices (for modes > LUMP_MAX_MODE) * @write_cmd_size: The size parameter received from a WRITE command - * @tacho_rate: The tacho rate received from an LPF2 motor - * @max_tacho_rate: The "100%" rate received from an LPF2 motor * @last_err: data->msg to be printed in case of an error. * @err_count: Total number of errors that have occurred * @num_data_err: Number of bad reads when receiving DATA data->msgs. @@ -158,8 +153,6 @@ typedef enum { * @mode_change_tx_done: Flag to keep ev3_uart_set_mode_end() blocked until * mode has actually changed * @speed_payload: Buffer for holding baud rate change message data - * @mode_combo_payload: Buffer for holding mode combo message data - * @mode_combo_size: Actual size of mode combo message */ typedef struct { pbio_iodev_t iodev; @@ -176,16 +169,11 @@ typedef struct { uint8_t new_mode; uint32_t new_baud_rate; uint32_t info_flags; - int32_t tacho_count; - int32_t tacho_offset; - int16_t abs_pos; uint8_t *tx_msg; uint8_t *rx_msg; uint8_t rx_msg_size; uint8_t ext_mode; uint8_t write_cmd_size; - int8_t tacho_rate; - int32_t max_tacho_rate; DBG_ERR(const char *last_err); uint32_t err_count; uint32_t num_data_err; @@ -193,8 +181,6 @@ typedef struct { bool tx_busy; bool mode_change_tx_done; uint8_t speed_payload[4]; - uint8_t mode_combo_payload[5]; - uint8_t mode_combo_size; } uartdev_port_data_t; enum { @@ -214,8 +200,6 @@ static uint8_t bufs[PBIO_CONFIG_UARTDEV_NUM_DEV][NUM_BUF][EV3_UART_MAX_MESSAGE_S static uartdev_port_data_t dev_data[PBIO_CONFIG_UARTDEV_NUM_DEV]; -static pbdrv_counter_dev_t *counter_devs; - #define PBIO_PT_WAIT_READY(pt, expr) PT_WAIT_UNTIL((pt), (expr) != PBIO_ERROR_AGAIN) pbio_error_t pbio_uartdev_get(uint8_t id, pbio_iodev_t **iodev) { @@ -490,9 +474,7 @@ static void pbio_uartdev_parse_msg(uartdev_port_data_t *data) { } // REVISIT: this is potentially an array of combos - data->info->mode_combos = data->rx_msg[3] << 8 | data->rx_msg[2]; - - debug_pr("mode combos: %04x\n", data->info->mode_combos); + debug_pr("mode combos: %04x\n", data->rx_msg[3] << 8 | data->rx_msg[2]); break; case LUMP_INFO_UNK9: @@ -505,12 +487,10 @@ static void pbio_uartdev_parse_msg(uartdev_port_data_t *data) { goto err; } - // first 3 parameters look like PID constants - data->max_tacho_rate = pbio_get_uint32_le(data->rx_msg + 14); - + // first 3 parameters look like PID constants, 4th is max tacho_rate debug_pr("motor parameters: %" PRIu32 " %" PRIu32 " %" PRIu32 " %" PRIu32 "\n", pbio_get_uint32_le(data->rx_msg + 2), pbio_get_uint32_le(data->rx_msg + 6), - pbio_get_uint32_le(data->rx_msg + 10), data->max_tacho_rate); + pbio_get_uint32_le(data->rx_msg + 10), pbio_get_uint32_le(data->rx_msg + 14)); break; case LUMP_INFO_UNK11: @@ -563,37 +543,16 @@ static void pbio_uartdev_parse_msg(uartdev_port_data_t *data) { goto err; } - if (PBIO_IODEV_IS_FEEDBACK_MOTOR(&data->iodev) && data->write_cmd_size > 0) { - data->tacho_rate = data->rx_msg[1]; - - // Decode the tacho count data message - int32_t tacho_count_msg = pbio_get_uint32_le(data->rx_msg + 2); - - // Sometimes, the incremental tacho data unexpectedly jumps by multiples - // of -/+360, so add a correction if an impossibly high change is detected. - while (tacho_count_msg - data->tacho_offset - data->tacho_count < -270) { - data->tacho_offset -= 360; - } - while (tacho_count_msg - data->tacho_offset - data->tacho_count > 270) { - data->tacho_offset += 360; - } - // The counter driver must return the corrected count - data->tacho_count = tacho_count_msg - data->tacho_offset; - - if (data->info->capability_flags & PBIO_IODEV_CAPABILITY_FLAG_HAS_MOTOR_ABS_POS) { - data->abs_pos = data->rx_msg[7] << 8 | data->rx_msg[6]; - } - } else { - if (mode >= data->info->num_modes) { - DBG_ERR(data->last_err = "Invalid mode received"); - goto err; - } - data->iodev.mode = mode; - if (mode == data->new_mode) { - memcpy(data->iodev.bin_data, data->rx_msg + 1, msg_size - 2); - } + if (mode >= data->info->num_modes) { + DBG_ERR(data->last_err = "Invalid mode received"); + goto err; + } + data->iodev.mode = mode; + if (mode == data->new_mode) { + memcpy(data->iodev.bin_data, data->rx_msg + 1, msg_size - 2); } + // setting type_id in info struct lets external modules know a device is connected and receiving good data data->info->type_id = data->type_id; @@ -742,9 +701,6 @@ static PT_THREAD(pbio_uartdev_update(uartdev_port_data_t * data)) { data->info->capability_flags = PBIO_IODEV_CAPABILITY_FLAG_NONE; data->ext_mode = 0; data->status = PBIO_UARTDEV_STATUS_SYNCING; - // default max tacho rate for BOOST external motor since it is the only - // motor that does not send this info - data->max_tacho_rate = 1400; pbdrv_uart_flush(data->uart); @@ -899,37 +855,6 @@ static PT_THREAD(pbio_uartdev_update(uartdev_port_data_t * data)) { // reset data rx thread PT_INIT(&data->data_pt); - if (PBIO_IODEV_IS_FEEDBACK_MOTOR(&data->iodev)) { - data->mode_combo_size = __builtin_popcount(data->info->mode_combos) + 2; - data->mode_combo_payload[0] = 0x20 | (data->mode_combo_size - 2); // mode combo command, x modes - data->mode_combo_payload[1] = 0; // combo index - data->mode_combo_payload[2] = 1 << 4 | 0; // mode 1, dataset 0 - data->mode_combo_payload[3] = 2 << 4 | 0; // mode 2, dataset 0 - data->mode_combo_payload[4] = 3 << 4 | 0; // mode 3, dataset 0 - // HACK: we are cheating here and assuming that all mode combinations - // are consecutive, starting with mode 1 and data->mode_combo_size - // chops off any unused mode (i.e. APOS) - - // setup motor to send position and speed data - PBIO_PT_WAIT_READY(&data->pt, - err = ev3_uart_begin_tx_msg(data, LUMP_MSG_TYPE_CMD, LUMP_CMD_WRITE, - data->mode_combo_payload, data->mode_combo_size)); - if (err != PBIO_SUCCESS) { - DBG_ERR(data->last_err = "UART Tx begin error during motor"); - goto err; - } - PBIO_PT_WAIT_READY(&data->pt, err = pbdrv_uart_write_end(data->uart)); - if (err != PBIO_SUCCESS) { - data->tx_busy = false; - DBG_ERR(data->last_err = "UART Tx end error during motor"); - goto err; - } - data->tx_busy = false; - - // Reset tacho offset - data->tacho_offset = 0; - } - // Turn on power for devices that need it if (data->motor_driver) { if (data->info->capability_flags & PBIO_IODEV_CAPABILITY_FLAG_NEEDS_SUPPLY_PIN1) { @@ -1051,10 +976,6 @@ static pbio_error_t ev3_uart_set_mode_begin(pbio_iodev_t *iodev, uint8_t mode) { uartdev_port_data_t *port_data = PBIO_CONTAINER_OF(iodev, uartdev_port_data_t, iodev); pbio_error_t err; - // User mode change for motors is not supported - if (PBIO_IODEV_IS_FEEDBACK_MOTOR(iodev)) { - return PBIO_ERROR_NOT_SUPPORTED; - } err = ev3_uart_begin_tx_msg(port_data, LUMP_MSG_TYPE_CMD, LUMP_CMD_SELECT, &mode, 1); if (err != PBIO_SUCCESS) { @@ -1146,45 +1067,6 @@ static const pbio_iodev_ops_t pbio_uartdev_ops = { .write_cancel = ev3_uart_write_cancel, }; -static pbio_error_t pbio_uartdev_get_count(pbdrv_counter_dev_t *dev, int32_t *count) { - uartdev_port_data_t *port_data = dev->priv; - - if (!PBIO_IODEV_IS_FEEDBACK_MOTOR(&port_data->iodev)) { - return PBIO_ERROR_NO_DEV; - } - - *count = port_data->tacho_count; - - return PBIO_SUCCESS; -} - -static pbio_error_t pbio_uartdev_get_abs_count(pbdrv_counter_dev_t *dev, int32_t *count) { - uartdev_port_data_t *port_data = dev->priv; - - if (!PBIO_IODEV_IS_FEEDBACK_MOTOR(&port_data->iodev)) { - return PBIO_ERROR_NO_DEV; - } - - if (!(port_data->info->capability_flags & PBIO_IODEV_CAPABILITY_FLAG_HAS_MOTOR_ABS_POS)) { - return PBIO_ERROR_NOT_SUPPORTED; - } - - *count = port_data->abs_pos; - - return PBIO_SUCCESS; -} - -static const pbdrv_counter_funcs_t pbio_uartdev_counter_funcs = { - .get_count = pbio_uartdev_get_count, - .get_abs_count = pbio_uartdev_get_abs_count, -}; - -void pbio_uartdev_counter_init(pbdrv_counter_dev_t *devs) { - // Since this has async init via contiki process, we need to save this - // pointer for later. - counter_devs = devs; -} - static PT_THREAD(pbio_uartdev_init(struct pt *pt, uint8_t id)) { const pbio_uartdev_platform_data_t *pdata = &pbio_uartdev_platform_data[id]; uartdev_port_data_t *port_data = &dev_data[id]; @@ -1200,12 +1082,6 @@ static PT_THREAD(pbio_uartdev_init(struct pt *pt, uint8_t id)) { port_data->tx_msg = &bufs[id][BUF_TX_MSG][0]; port_data->rx_msg = &bufs[id][BUF_RX_MSG][0]; - // It is not guaranteed that pbio_uartdev_counter_init() is called before pbio_uartdev_init() - PT_WAIT_UNTIL(pt, counter_devs != NULL); - pbdrv_counter_dev_t *counter = &counter_devs[pdata->counter_id]; - counter->funcs = &pbio_uartdev_counter_funcs; - counter->priv = port_data; - PT_END(pt); } diff --git a/lib/pbio/src/uartdev.h b/lib/pbio/src/uartdev.h deleted file mode 100644 index 047e10805..000000000 --- a/lib/pbio/src/uartdev.h +++ /dev/null @@ -1,23 +0,0 @@ -// SPDX-License-Identifier: MIT -// Copyright (c) 2020 The Pybricks Authors - -// Internal functions for LEGO UART I/O devices. - -#ifndef _PBIO_UARTDEV_H_ -#define _PBIO_UARTDEV_H_ - -#include - -#if PBIO_CONFIG_UARTDEV - -#include - -void pbio_uartdev_counter_init(pbdrv_counter_dev_t *devs); - -#else // PBIO_CONFIG_UARTDEV - -#define pbio_uartdev_counter_init(devs) - -#endif // PBIO_CONFIG_UARTDEV - -#endif // _PBIO_UARTDEV_H_ diff --git a/lib/pbio/test/src/uartdev.c b/lib/pbio/test/src/uartdev.c index 9f42e4438..2b50fbe49 100644 --- a/lib/pbio/test/src/uartdev.c +++ b/lib/pbio/test/src/uartdev.c @@ -1,5 +1,5 @@ // SPDX-License-Identifier: MIT -// Copyright (c) 2019-2021 The Pybricks Authors +// Copyright (c) 2019-2022 The Pybricks Authors #include #include @@ -12,7 +12,6 @@ #include #include -#include #include #include #include @@ -332,12 +331,6 @@ static PT_THREAD(test_boost_color_distance_sensor(struct pt *pt)) { PT_YIELD(pt); - pbdrv_counter_dev_t *counter; - tt_want_uint_op(pbdrv_counter_get_dev(0, &counter), ==, PBIO_SUCCESS); - int32_t count; - tt_want_uint_op(pbdrv_counter_get_count(counter, &count), ==, PBIO_ERROR_NO_DEV); - tt_want_uint_op(pbdrv_counter_get_abs_count(counter, &count), ==, PBIO_ERROR_NO_DEV); - // should be synced now are receive regular pings static int i; for (i = 0; i < 10; i++) { @@ -354,7 +347,6 @@ static PT_THREAD(test_boost_color_distance_sensor(struct pt *pt)) { tt_want_uint_op(iodev->info->type_id, ==, PBIO_IODEV_TYPE_ID_COLOR_DIST_SENSOR); tt_want_uint_op(iodev->info->num_modes, ==, 11); // TODO: verify fw/hw versions - tt_want_uint_op(iodev->info->mode_combos, ==, 1 << 6 | 1 << 3 | 1 << 2 | 1 << 1 | 1 << 0); tt_want_uint_op(iodev->info->capability_flags, ==, PBIO_IODEV_CAPABILITY_FLAG_NONE); tt_want_uint_op(iodev->mode, ==, 0); @@ -494,9 +486,7 @@ static PT_THREAD(test_boost_interactive_motor(struct pt *pt)) { static const uint8_t msg34[] = { 0x04 }; // ACK - static const uint8_t msg35[] = { 0x54, 0x22, 0x00, 0x10, 0x20, 0xB9 }; // WRITE mode combo - - static const uint8_t msg36[] = { 0xD8, 0x9C, 0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0xBB }; // DATA speed and position combo + static const uint8_t msg36[] = { 0xD0, 0xFF, 0xFF, 0xFF, 0xFF, 0x2F }; // DATA length 4, mode 0 static const uint8_t msg37[] = { 0x02 }; // NACK @@ -566,22 +556,8 @@ static PT_THREAD(test_boost_interactive_motor(struct pt *pt)) { test_uart_dev.baud == 115200; })); - // motors get WRITE message to setup mode combos - SIMULATE_TX_MSG(msg35); - - // same message is received in respose along with data message - SIMULATE_RX_MSG(msg35); - SIMULATE_RX_MSG(msg36); - PT_YIELD(pt); - pbdrv_counter_dev_t *counter; - tt_want_uint_op(pbdrv_counter_get_dev(0, &counter), ==, PBIO_SUCCESS); - int32_t count; - tt_want_uint_op(pbdrv_counter_get_count(counter, &count), ==, PBIO_SUCCESS); - tt_want_int_op(count, ==, -1); - tt_want_uint_op(pbdrv_counter_get_abs_count(counter, &count), ==, PBIO_ERROR_NOT_SUPPORTED); - // should be synced now are receive regular pings static int i; for (i = 0; i < 10; i++) { @@ -597,7 +573,6 @@ static PT_THREAD(test_boost_interactive_motor(struct pt *pt)) { tt_want_uint_op(iodev->info->type_id, ==, PBIO_IODEV_TYPE_ID_INTERACTIVE_MOTOR); tt_want_uint_op(iodev->info->num_modes, ==, 4); // TODO: verify fw/hw versions - tt_want_uint_op(iodev->info->mode_combos, ==, 1 << 2 | 1 << 1); tt_want_uint_op(iodev->info->capability_flags, ==, PBIO_IODEV_CAPABILITY_FLAG_IS_DC_OUTPUT | PBIO_IODEV_CAPABILITY_FLAG_HAS_MOTOR_SPEED | PBIO_IODEV_CAPABILITY_FLAG_HAS_MOTOR_REL_POS); tt_want_uint_op(iodev->mode, ==, 0); @@ -680,9 +655,7 @@ static PT_THREAD(test_technic_large_motor(struct pt *pt)) { static const uint8_t msg55[] = { 0x04 }; // ACK - static const uint8_t msg56[] = { 0x5C, 0x23, 0x00, 0x10, 0x20, 0x30, 0x00, 0x00, 0x00, 0x80 }; // WRITE mode combo - - static const uint8_t msg57[] = { 0xD8, 0x64, 0xFF, 0xFF, 0xFF, 0xFF, 0x17, 0x00, 0x00, 0x54 }; // DATA speed and position combo + static const uint8_t msg57[] = { 0xD0, 0xFF, 0xFF, 0xFF, 0xFF, 0x2F }; // DATA length 4, mode 0 static const uint8_t msg58[] = { 0x02 }; // NACK @@ -765,23 +738,8 @@ static PT_THREAD(test_technic_large_motor(struct pt *pt)) { // wait for ACK SIMULATE_TX_MSG(msg55); - // motors get WRITE message to setup mode combos - SIMULATE_TX_MSG(msg56); - - // same message is received in respose along with data message - SIMULATE_RX_MSG(msg56); - SIMULATE_RX_MSG(msg57); - PT_YIELD(pt); - pbdrv_counter_dev_t *counter; - tt_want_uint_op(pbdrv_counter_get_dev(0, &counter), ==, PBIO_SUCCESS); - int32_t count; - tt_want_uint_op(pbdrv_counter_get_count(counter, &count), ==, PBIO_SUCCESS); - tt_want_int_op(count, ==, -1); - tt_want_uint_op(pbdrv_counter_get_abs_count(counter, &count), ==, PBIO_SUCCESS); - tt_want_int_op(count, ==, 23); - // should be synced now are receive regular pings static int i; for (i = 0; i < 10; i++) { @@ -797,7 +755,6 @@ static PT_THREAD(test_technic_large_motor(struct pt *pt)) { tt_want_uint_op(iodev->info->type_id, ==, PBIO_IODEV_TYPE_ID_TECHNIC_L_MOTOR); tt_want_uint_op(iodev->info->num_modes, ==, 6); // TODO: verify fw/hw versions - tt_want_uint_op(iodev->info->mode_combos, ==, 1 << 3 | 1 << 2 | 1 << 1); tt_want_uint_op(iodev->info->capability_flags, ==, PBIO_IODEV_CAPABILITY_FLAG_IS_DC_OUTPUT | PBIO_IODEV_CAPABILITY_FLAG_HAS_MOTOR_SPEED | PBIO_IODEV_CAPABILITY_FLAG_HAS_MOTOR_REL_POS | PBIO_IODEV_CAPABILITY_FLAG_HAS_MOTOR_ABS_POS); tt_want_uint_op(iodev->mode, ==, 0); @@ -886,9 +843,7 @@ static PT_THREAD(test_technic_xl_motor(struct pt *pt)) { static const uint8_t msg55[] = { 0x04 }; // ACK - static const uint8_t msg56[] = { 0x5C, 0x23, 0x00, 0x10, 0x20, 0x30, 0x00, 0x00, 0x00, 0x80 }; // WRITE mode combo - - static const uint8_t msg57[] = { 0xD8, 0x64, 0xFF, 0xFF, 0xFF, 0xFF, 0x41, 0x00, 0x00, 0x02 }; // DATA speed and position combo + static const uint8_t msg57[] = { 0xD0, 0xFF, 0xFF, 0xFF, 0xFF, 0x2F }; // DATA length 4, mode 0 static const uint8_t msg58[] = { 0x02 }; // NACK @@ -971,23 +926,8 @@ static PT_THREAD(test_technic_xl_motor(struct pt *pt)) { // wait for ACK SIMULATE_TX_MSG(msg55); - // motors get WRITE message to setup mode combos - SIMULATE_TX_MSG(msg56); - - // same message is received in respose along with data message - SIMULATE_RX_MSG(msg56); - SIMULATE_RX_MSG(msg57); - PT_YIELD(pt); - pbdrv_counter_dev_t *counter; - tt_want_uint_op(pbdrv_counter_get_dev(0, &counter), ==, PBIO_SUCCESS); - int32_t count; - tt_want_uint_op(pbdrv_counter_get_count(counter, &count), ==, PBIO_SUCCESS); - tt_want_int_op(count, ==, -1); - tt_want_uint_op(pbdrv_counter_get_abs_count(counter, &count), ==, PBIO_SUCCESS); - tt_want_int_op(count, ==, 65); - // should be synced now are receive regular pings static int i; for (i = 0; i < 10; i++) { @@ -1003,7 +943,6 @@ static PT_THREAD(test_technic_xl_motor(struct pt *pt)) { tt_want_uint_op(iodev->info->type_id, ==, PBIO_IODEV_TYPE_ID_TECHNIC_XL_MOTOR); tt_want_uint_op(iodev->info->num_modes, ==, 6); // TODO: verify fw/hw versions - tt_want_uint_op(iodev->info->mode_combos, ==, 1 << 3 | 1 << 2 | 1 << 1); tt_want_uint_op(iodev->info->capability_flags, ==, PBIO_IODEV_CAPABILITY_FLAG_IS_DC_OUTPUT | PBIO_IODEV_CAPABILITY_FLAG_HAS_MOTOR_SPEED | PBIO_IODEV_CAPABILITY_FLAG_HAS_MOTOR_REL_POS | PBIO_IODEV_CAPABILITY_FLAG_HAS_MOTOR_ABS_POS); tt_want_uint_op(iodev->mode, ==, 0); @@ -1045,7 +984,6 @@ struct testcase_t pbio_uartdev_tests[] = { const pbio_uartdev_platform_data_t pbio_uartdev_platform_data[] = { [0] = { .uart_id = 0, - .counter_id = 0, }, }; diff --git a/pybricks/util_pb/pb_device_stm32.c b/pybricks/util_pb/pb_device_stm32.c index c378d25ac..2cb0c8bf3 100644 --- a/pybricks/util_pb/pb_device_stm32.c +++ b/pybricks/util_pb/pb_device_stm32.c @@ -225,4 +225,32 @@ int8_t pb_device_get_mode_id_from_str(pb_device_t *pbdev, const char *mode_str) } void pb_device_setup_motor(pbio_port_id_t port, bool is_servo) { + + // Get the iodevice + pbio_iodev_t *iodev; + pbio_error_t err; + + // Set up device + while ((err = pbdrv_ioport_get_iodev(port, &iodev)) == PBIO_ERROR_AGAIN) { + mp_hal_delay_ms(50); + } + pb_assert(err); + + // Only motors are allowed. + if (!PBIO_IODEV_IS_DC_OUTPUT(iodev)) { + pb_assert(PBIO_ERROR_NO_DEV); + } + + // If it's a DC motor, no further setup is needed. + if (!PBIO_IODEV_IS_FEEDBACK_MOTOR(iodev)) { + return; + } + + // Choose mode based on device capabilities. + uint8_t mode_id = PBIO_IODEV_IS_ABS_MOTOR(iodev) ? + PBIO_IODEV_MODE_PUP_ABS_MOTOR__APOS: + PBIO_IODEV_MODE_PUP_REL_MOTOR__POS; + + // Activate mode. + set_mode(iodev, mode_id); }