diff --git a/boards/common/native/drivers/native-qdec.c b/boards/common/native/drivers/native-qdec.c deleted file mode 100644 index 7e87b83198..0000000000 --- a/boards/common/native/drivers/native-qdec.c +++ /dev/null @@ -1,59 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2018 Gilles DOFFE - * SPDX-License-Identifier: LGPL-2.1-only - */ - -/* - * Native Board board implementation - * - * @ingroup boards_common_native - * @{ - * @file - * @author Gilles DOFFE - * @} - * - */ - -#include -#include - -/* RIOT includes */ -#include -#include - -#ifdef MODULE_PERIPH_QDEC - -extern int32_t qdecs_value[QDEC_NUMOF]; - -void native_motor_driver_qdec_simulation( - const motor_driver_t motor_driver, uint8_t motor_id, - int32_t pwm_duty_cycle) -{ - uint32_t id = 0; - - for (uint32_t i = 0; i < motor_driver; i++) { - const motor_driver_config_t motor_driver_conf = - motor_driver_config[motor_driver]; - id += motor_driver_conf.nb_motors; - } - id += motor_id; - - if (id < QDEC_NUMOF) { - qdecs_value[id] = pwm_duty_cycle; - - LOG_DEBUG("MOTOR-DRIVER=%u" \ - " MOTOR_ID = %u" \ - " PWM_VALUE = %d" \ - " QDEC_ID = %"PRIu32"" \ - " QDEC_VALUE = %d\n", \ - motor_driver, motor_id, pwm_duty_cycle, id, pwm_duty_cycle); - } - else { - LOG_ERROR("MOTOR-DRIVER=%u" \ - " MOTOR_ID = %u" \ - " no QDEC device associated\n", \ - motor_driver, motor_id); - } -} - -#endif /* MODULE_PERIPH_QDEC */ diff --git a/dist/tools/doccheck/exclude_simple b/dist/tools/doccheck/exclude_simple index 0100479f64..4a4ec97cd6 100644 --- a/dist/tools/doccheck/exclude_simple +++ b/dist/tools/doccheck/exclude_simple @@ -18,8 +18,6 @@ warning: found documented return type for mcp47xx_dac_get that does not return a warning: found documented return type for mcp47xx_dac_poweroff that does not return anything warning: found documented return type for mcp47xx_dac_poweron that does not return anything warning: found documented return type for mcp47xx_dac_set that does not return anything -warning: found documented return type for motor_disable that does not return anything -warning: found documented return type for motor_enable that does not return anything warning: found documented return type for pthread_exit that does not return anything warning: found documented return type for senml_set_duration_ms that does not return anything warning: found documented return type for senml_set_duration_us that does not return anything @@ -4699,8 +4697,6 @@ warning: Member MODULE_PIN_P6 (macro definition) of file board_module.h is not d warning: Member MODULE_PIN_P7 (macro definition) of file board_module.h is not documented. warning: Member MODULE_PIN_P8 (macro definition) of file board_module.h is not documented. warning: Member MODULE_PIN_P9 (macro definition) of file board_module.h is not documented. -warning: Member motor_driver_config[] (variable) of file board.h is not documented. -warning: Member MOTOR_DRIVER_NUMOF (macro definition) of file board.h is not documented. warning: Member MPL3115A2_CTRL_REG1_ALT (macro definition) of file mpl3115a2_reg.h is not documented. warning: Member MPL3115A2_CTRL_REG1_OS_MASK (macro definition) of file mpl3115a2_reg.h is not documented. warning: Member MPL3115A2_CTRL_REG1_OS_SHIFT (macro definition) of file mpl3115a2_reg.h is not documented. diff --git a/drivers/include/motor_driver.h b/drivers/include/motor_driver.h index 9850098b85..3bf46bf936 100644 --- a/drivers/include/motor_driver.h +++ b/drivers/include/motor_driver.h @@ -1,15 +1,12 @@ /* - * Copyright (C) 2018 Gilles DOFFE - * - * This file is subject to the terms and conditions of the GNU Lesser - * General Public License v2.1. See the file LICENSE in the top level - * directory for more details. + * SPDX-FileCopyrightText: 2018 Gilles DOFFE + * SPDX-License-Identifier: LGPL-2.1-only */ #pragma once /** - * @defgroup drivers_motor DC Motor Driver + * @defgroup drivers_motor_driver DC Motor Driver * @ingroup drivers_actuators * @brief High-level driver for DC motors * @@ -73,8 +70,9 @@ * * BRAKE LOW is functionally the same than BRAKE HIGH but some H-bridge only * brake on BRAKE HIGH due to hardware. - * In case of single direction GPIO, there is no BRAKE, PWM duty cycle is set - * to 0. + * In case of single direction GPIO, there is no BRAKE. + * + * In case of brake, PWM duty cycle is always set to 0. * @{ * @file @@ -83,6 +81,8 @@ * @author Gilles DOFFE */ +#include + #include "periph/pwm.h" #include "periph/gpio.h" @@ -91,7 +91,7 @@ extern "C" { #endif /** - * @defgroup drivers_motor_driver_config Motor_Driver driver compile configuration + * @defgroup drivers_motor_driver_config motor_driver driver build configuration * @ingroup config_drivers_actuators * @{ */ @@ -99,35 +99,22 @@ extern "C" { * @brief Maximum number of motors by motor driver */ #ifndef CONFIG_MOTOR_DRIVER_MAX -#define CONFIG_MOTOR_DRIVER_MAX (2) +# define CONFIG_MOTOR_DRIVER_MAX (2) #endif /** @} */ -/** - * @brief Macro to return motor driver id - */ -#define MOTOR_DRIVER_DEV(x) (x) - /** * @brief Describe DC motor driver modes */ typedef enum { - MOTOR_DRIVER_2_DIRS = 0, /**< 2 GPIOS for direction, \ - handling BRAKE */ - MOTOR_DRIVER_1_DIR = 1, /**< Single GPIO for direction, \ - no BRAKE */ - MOTOR_DRIVER_1_DIR_BRAKE = 2 /**< Single GPIO for direction, \ - Single GPIO for BRAKE */ + MOTOR_DRIVER_2_DIRS = 0, /**< 2 GPIOs for direction, \ + handling brake */ + MOTOR_DRIVER_1_DIR = 1, /**< single GPIO for direction, \ + no brake */ + MOTOR_DRIVER_1_DIR_BRAKE = 2 /**< single GPIO for direction, \ + single GPIO for brake */ } motor_driver_mode_t; -/** - * @brief Describe DC motor driver brake modes - */ -typedef enum { - MOTOR_BRAKE_LOW = 0, /**< Low stage brake */ - MOTOR_BRAKE_HIGH = 1, /**< High stage brake */ -} motor_driver_mode_brake_t; - /** * @brief Describe DC motor direction states */ @@ -142,61 +129,84 @@ typedef enum { typedef struct { int pwm_channel; /**< PWM channel the motor is connected to */ gpio_t gpio_enable; /**< GPIO to enable/disable motor */ - gpio_t gpio_dir0; /**< GPIO to control rotation direction */ - gpio_t gpio_dir1_or_brake; /**< GPIO to control rotation direction */ - uint8_t gpio_dir_reverse; /**< flag to reverse direction */ - uint8_t gpio_enable_invert; /**< flag to set enable GPIO inverted mode */ - uint8_t gpio_brake_invert; /**< flag to make brake active low */ + gpio_t gpio_dir0; /**< GPIO to control direction */ + union { + gpio_t gpio_dir1; /**< GPIO to control direction */ + gpio_t gpio_brake; /**< GPIO to control brake */ + }; + bool gpio_dir_reverse; /**< flag to reverse direction */ } motor_t; /** - * @brief Default motor driver type definition + * @brief Motor driver */ -typedef unsigned int motor_driver_t; +typedef struct _motor_driver_t motor_driver_t; /** * @brief Motor callback. It is called at end of motor_set() */ -typedef void (*motor_driver_cb_t)(const motor_driver_t motor_driver, +typedef void (*motor_set_post_cb_t)(const motor_driver_t *motor_driver, uint8_t motor_id, int32_t pwm_duty_cycle); +/** + * @brief Motor set callback. Called to set motor speed. + */ +typedef void (*motor_set_cb_t)(const motor_t *motor, + motor_direction_t direction); + +/** + * @brief Motor brake callback. Called to brake a motor. + */ +typedef void (*motor_brake_cb_t)(const motor_t *motor, + bool brake); + /** * @brief Describe DC motor driver with PWM device and motors array */ typedef struct { - pwm_t pwm_dev; /**< PWM device driving motors */ - motor_driver_mode_t mode; /**< driver mode */ - motor_driver_mode_brake_t mode_brake; /**< driver brake mode */ - pwm_mode_t pwm_mode; /**< PWM mode */ - uint32_t pwm_frequency; /**< PWM device frequency */ - uint32_t pwm_resolution; /**< PWM device resolution */ - uint8_t nb_motors; /**< number of moros */ - motor_t motors[CONFIG_MOTOR_DRIVER_MAX]; /**< motors array */ - motor_driver_cb_t cb; /**< callback on motor_set */ -} motor_driver_config_t; + motor_driver_mode_t mode; /**< driver mode */ + pwm_t pwm_dev; /**< PWM device driving motors */ + pwm_mode_t pwm_mode; /**< PWM mode */ + uint32_t pwm_frequency; /**< PWM device frequency */ + uint32_t pwm_resolution; /**< PWM device resolution */ + bool brake_inverted; /**< if false, brake high (1), low (0) otherwise */ + bool enable_inverted; /**< if false, enable high (1), low (0) otherwise */ + uint8_t nb_motors; /**< number of motors */ + motor_t motors[CONFIG_MOTOR_DRIVER_MAX]; /**< motors array */ + motor_set_post_cb_t motor_set_post_cb; /**< callback post to motor_set */ +} motor_driver_params_t; + +/** + * @brief Motor driver + */ +struct _motor_driver_t { + const motor_driver_params_t *params; /**< parameters */ +}; /** * @brief Initialize DC motor driver board * - * @param[out] motor_driver motor driver to initialize + * @param[out] motor_driver motor driver to initialize + * @param[in] params motor driver parameters * - * @return 0 on success - * @return -1 on error with errno set + * @retval 0 on success + * @retval -EINVAL on bad parameter value + * @retval -EIO on failed GPIO init */ -int motor_driver_init(const motor_driver_t motor_driver); +int motor_driver_init(motor_driver_t *motor_driver, const motor_driver_params_t *params); /** * @brief Set motor speed and direction * * @param[in] motor_driver motor driver to which motor is attached * @param[in] motor_id motor ID on driver - * @param[in] pwm_duty_cycle Signed PWM duty_cycle to set motor speed and direction + * @param[in] pwm_duty_cycle signed PWM duty_cycle to set motor speed and direction * - * @return 0 on success - * @return -1 on error with errno set + * @retval 0 on success + * @retval -EINVAL on bad motor ID */ -int motor_set(const motor_driver_t motor_driver, uint8_t motor_id, \ +int motor_set(const motor_driver_t *motor_driver, uint8_t motor_id, \ int32_t pwm_duty_cycle); /** @@ -205,10 +215,10 @@ int motor_set(const motor_driver_t motor_driver, uint8_t motor_id, \ * @param[in] motor_driver motor driver to which motor is attached * @param[in] motor_id motor ID on driver * - * @return 0 on success - * @return -1 on error with errno set + * @retval 0 on success + * @retval -EINVAL on bad motor ID */ -int motor_brake(const motor_driver_t motor_driver, uint8_t motor_id); +int motor_brake(const motor_driver_t *motor_driver, uint8_t motor_id); /** * @brief Enable a motor of a given motor driver @@ -216,7 +226,7 @@ int motor_brake(const motor_driver_t motor_driver, uint8_t motor_id); * @param[in] motor_driver motor driver to which motor is attached * @param[in] motor_id motor ID on driver */ -void motor_enable(const motor_driver_t motor_driver, uint8_t motor_id); +void motor_enable(const motor_driver_t *motor_driver, uint8_t motor_id); /** * @brief Disable a motor of a given motor driver @@ -224,7 +234,7 @@ void motor_enable(const motor_driver_t motor_driver, uint8_t motor_id); * @param[in] motor_driver motor driver to which motor is attached * @param[in] motor_id motor ID on driver */ -void motor_disable(const motor_driver_t motor_driver, uint8_t motor_id); +void motor_disable(const motor_driver_t *motor_driver, uint8_t motor_id); #ifdef __cplusplus } diff --git a/drivers/motor_driver/Makefile.include b/drivers/motor_driver/Makefile.include new file mode 100644 index 0000000000..40d3127958 --- /dev/null +++ b/drivers/motor_driver/Makefile.include @@ -0,0 +1,2 @@ +USEMODULE_INCLUDES_motor_driver := $(LAST_MAKEFILEDIR)/include +USEMODULE_INCLUDES += $(USEMODULE_INCLUDES_motor_driver) diff --git a/drivers/motor_driver/include/motor_driver_params.h b/drivers/motor_driver/include/motor_driver_params.h new file mode 100644 index 0000000000..9ce8db93e7 --- /dev/null +++ b/drivers/motor_driver/include/motor_driver_params.h @@ -0,0 +1,171 @@ +/* + * SPDX-FileCopyrightText: 2023 COGIP Robotics association + * SPDX-License-Identifier: LGPL-2.1-only + */ + +#pragma once + +/** + * @ingroup drivers_motor_driver + * @{ + * + * @file + * @brief Default configuration for motor driver. + * + * @author Gilles DOFFE + * + */ + +#include "board.h" +#include "motor_driver.h" +#include "saul_reg.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @name Set default configuration parameters for motor_driver + * @{ + */ + +#ifndef MOTOR_DRIVER_PARAM_MODE +/** Default motor driver mode */ +# define MOTOR_DRIVER_PARAM_MODE MOTOR_DRIVER_1_DIR +#endif +#ifndef MOTOR_DRIVER_PARAM_BRAKE_INVERTED +/** Default brake level */ +# define MOTOR_DRIVER_PARAM_BRAKE_INVERTED false +#endif +#ifndef MOTOR_DRIVER_PARAM_ENABLE_INVERTED +/** Default enable level */ +# define MOTOR_DRIVER_PARAM_ENABLE_INVERTED false +#endif +#ifndef MOTOR_DRIVER_PARAM_PWM +/** Default PWM device */ +# define MOTOR_DRIVER_PARAM_PWM 0 +#endif +#ifndef MOTOR_DRIVER_PARAM_PWM_MODE +/** Default PWM mode */ +# define MOTOR_DRIVER_PARAM_PWM_MODE PWM_LEFT +#endif +#ifndef MOTOR_DRIVER_PARAM_PWM_FREQUENCY +/** Default PWM frequency */ +# define MOTOR_DRIVER_PARAM_PWM_FREQUENCY 20000U +#endif +#ifndef MOTOR_DRIVER_PARAM_PWM_RESOLUTION +/** Default PWM resolution */ +# define MOTOR_DRIVER_PARAM_PWM_RESOLUTION 100U +#endif +#ifndef MOTOR_DRIVER_PARAM_NB_MOTORS +/** Default number of motors */ +# define MOTOR_DRIVER_PARAM_NB_MOTORS 2U +#endif +#ifndef MOTOR_DRIVER_PARAM_MOTOR_SET_POST_CALLBACK +/** Default callback called at end of motor_set() */ +# define MOTOR_DRIVER_PARAM_MOTOR_SET_POST_CALLBACK NULL +#endif + +/* Motor 1 */ +#ifndef MOTOR_DRIVER_PARAM_MOTOR1_PWM_CHANNEL +/** Default motor 1 PWM channel */ +# define MOTOR_DRIVER_PARAM_MOTOR1_PWM_CHANNEL 1U +#endif +#ifndef MOTOR_DRIVER_PARAM_MOTOR1_GPIO_ENABLE +/** Default motor 1 enable GPIO */ +# define MOTOR_DRIVER_PARAM_MOTOR1_GPIO_ENABLE GPIO_UNDEF +#endif +#ifndef MOTOR_DRIVER_PARAM_MOTOR1_GPIO_DIR0 +/** Default motor 1 direction GPIO */ +# define MOTOR_DRIVER_PARAM_MOTOR1_GPIO_DIR0 GPIO_UNDEF +#endif +#ifndef MOTOR_DRIVER_PARAM_MOTOR1_GPIO_DIR1_OR_BRAKE +/** Default motor 1 direction or brake GPIO */ +# define MOTOR_DRIVER_PARAM_MOTOR1_GPIO_DIR1_OR_BRAKE GPIO_UNDEF +#endif +#ifndef MOTOR_DRIVER_PARAM_MOTOR1_GPIO_DIR_REVERSE +/** Default motor 1 direction GPIO(s) reverse */ +# define MOTOR_DRIVER_PARAM_MOTOR1_GPIO_DIR_REVERSE 0 +#endif + +/* Motor 2 */ +#ifndef MOTOR_DRIVER_PARAM_MOTOR2_PWM_CHANNEL +/** Default motor 2 PWM channel */ +# define MOTOR_DRIVER_PARAM_MOTOR2_PWM_CHANNEL 2U +#endif +#ifndef MOTOR_DRIVER_PARAM_MOTOR2_GPIO_ENABLE +/** Default motor 2 enable GPIO */ +# define MOTOR_DRIVER_PARAM_MOTOR2_GPIO_ENABLE GPIO_UNDEF +#endif +#ifndef MOTOR_DRIVER_PARAM_MOTOR2_GPIO_DIR0 +/** Default motor 2 direction GPIO */ +# define MOTOR_DRIVER_PARAM_MOTOR2_GPIO_DIR0 GPIO_UNDEF +#endif +#ifndef MOTOR_DRIVER_PARAM_MOTOR2_GPIO_DIR1_OR_BRAKE +/** Default motor 2 direction or brake GPIO */ +# define MOTOR_DRIVER_PARAM_MOTOR2_GPIO_DIR1_OR_BRAKE GPIO_UNDEF +#endif +#ifndef MOTOR_DRIVER_PARAM_MOTOR2_GPIO_DIR_REVERSE +/** Default motor 2 direction GPIO(s) reverse */ +# define MOTOR_DRIVER_PARAM_MOTOR2_GPIO_DIR_REVERSE 0 +#endif + +#ifndef MOTOR_DRIVER_PARAMS +/** Default motor driver parameters */ +# define MOTOR_DRIVER_PARAMS \ + { \ + .mode = MOTOR_DRIVER_PARAM_MODE, \ + .pwm_dev = MOTOR_DRIVER_PARAM_PWM, \ + .pwm_mode = MOTOR_DRIVER_PARAM_PWM_MODE, \ + .pwm_frequency = MOTOR_DRIVER_PARAM_PWM_FREQUENCY, \ + .pwm_resolution = MOTOR_DRIVER_PARAM_PWM_RESOLUTION, \ + .brake_inverted = MOTOR_DRIVER_PARAM_BRAKE_INVERTED, \ + .enable_inverted = MOTOR_DRIVER_PARAM_ENABLE_INVERTED, \ + .nb_motors = MOTOR_DRIVER_PARAM_NB_MOTORS, \ + .motors = { \ + { \ + .pwm_channel = MOTOR_DRIVER_PARAM_MOTOR1_PWM_CHANNEL, \ + .gpio_enable = MOTOR_DRIVER_PARAM_MOTOR1_GPIO_ENABLE, \ + .gpio_dir0 = MOTOR_DRIVER_PARAM_MOTOR1_GPIO_DIR0, \ + .gpio_dir1 = MOTOR_DRIVER_PARAM_MOTOR1_GPIO_DIR1_OR_BRAKE, \ + .gpio_dir_reverse = MOTOR_DRIVER_PARAM_MOTOR1_GPIO_DIR_REVERSE, \ + }, \ + { \ + .pwm_channel = MOTOR_DRIVER_PARAM_MOTOR2_PWM_CHANNEL, \ + .gpio_enable = MOTOR_DRIVER_PARAM_MOTOR2_GPIO_ENABLE, \ + .gpio_dir0 = MOTOR_DRIVER_PARAM_MOTOR2_GPIO_DIR0, \ + .gpio_dir1 = MOTOR_DRIVER_PARAM_MOTOR2_GPIO_DIR1_OR_BRAKE, \ + .gpio_dir_reverse = MOTOR_DRIVER_PARAM_MOTOR2_GPIO_DIR_REVERSE, \ + } \ + }, \ + .motor_set_post_cb = MOTOR_DRIVER_PARAM_MOTOR_SET_POST_CALLBACK \ + } +#endif + +/* SAUL */ +#ifndef MOTOR_DRIVER_SAUL_INFO +/** SAUL information */ +# define MOTOR_DRIVER_SAUL_INFO { .name = "motor_driver" } +#endif +/**@}*/ + +/** + * @brief MOTOR_DRIVER configuration + */ +static const motor_driver_params_t motor_driver_params[] = +{ + MOTOR_DRIVER_PARAMS, +}; + +/** + * @brief Additional meta information to keep in the SAUL registry + */ +static const saul_reg_info_t motor_driver_saul_info[] = +{ + MOTOR_DRIVER_SAUL_INFO +}; + +#ifdef __cplusplus +} +#endif +/** @} */ diff --git a/drivers/motor_driver/motor_driver.c b/drivers/motor_driver/motor_driver.c index 0198c18412..c453de2d00 100644 --- a/drivers/motor_driver/motor_driver.c +++ b/drivers/motor_driver/motor_driver.c @@ -1,13 +1,10 @@ /* - * Copyright (C) 2018 Gilles DOFFE - * - * This file is subject to the terms and conditions of the GNU Lesser - * General Public License v2.1. See the file LICENSE in the top level - * directory for more details. + * SPDX-FileCopyrightText: 2018 Gilles DOFFE + * SPDX-License-Identifier: LGPL-2.1-only */ /** - * @ingroup drivers_motor + * @ingroup drivers_motor_driver * @{ * * @file @@ -29,155 +26,168 @@ #define ENABLE_DEBUG 0 #include -int motor_driver_init(motor_driver_t motor_driver) +/** + * @brief Callback to set direction on two directions pins driver + * + * @param motor motor + * @param direction direction + */ +static void _motor_set_two_dirs(const motor_t *motor, motor_direction_t direction); + +/** + * @brief Callback to set direction on one direction pin driver + * + * @param motor motor + * @param direction direction + */ + +static void _motor_set_one_dir(const motor_t *motor, motor_direction_t direction); + +static void _motor_brake_two_dirs(const motor_t *motor, bool brake); +static void _motor_brake_one_dir_brake(const motor_t *motor, bool brake); + +int motor_driver_init(motor_driver_t *motor_driver, const motor_driver_params_t *params) { int err = 0; - assert(motor_driver < MOTOR_DRIVER_NUMOF); - - const motor_driver_config_t *motor_driver_conf = \ - &motor_driver_config[motor_driver]; - - pwm_t pwm_dev = motor_driver_conf->pwm_dev; - pwm_mode_t mode = motor_driver_conf->pwm_mode; - uint32_t freq = motor_driver_conf->pwm_frequency; - uint16_t resol = motor_driver_conf->pwm_resolution; - - uint32_t pwm_freq = pwm_init(pwm_dev, mode, freq, resol); - if (pwm_freq == 0) { + uint32_t ret_pwm = pwm_init(params->pwm_dev, + params->pwm_mode, + params->pwm_frequency, + params->pwm_resolution); + if (ret_pwm == 0) { err = EINVAL; LOG_ERROR("pwm_init failed\n"); goto motor_init_err; } - for (uint8_t i = 0; i < motor_driver_conf->nb_motors; i++) { - if (gpio_is_valid(motor_driver_conf->motors[i].gpio_dir0) - && (gpio_init(motor_driver_conf->motors[i].gpio_dir0, - GPIO_OUT))) { - err = EIO; - LOG_ERROR("gpio_dir0 init failed\n"); - goto motor_init_err; - } - if (gpio_is_valid(motor_driver_conf->motors[i].gpio_dir1_or_brake) - && (gpio_init(motor_driver_conf->motors[i].gpio_dir1_or_brake, - GPIO_OUT))) { - err = EIO; - LOG_ERROR("gpio_dir1_or_brake init failed\n"); - goto motor_init_err; - } - if (gpio_is_valid(motor_driver_conf->motors[i].gpio_enable)) { - if (gpio_init(motor_driver_conf->motors[i].gpio_enable, + /* Init GPIO */ + err = 0; + for (uint8_t i = 0; i < params->nb_motors; i++) { + /* Init motor GPIOs, if one fails, motor is not setup */ + if (gpio_is_valid(params->motors[i].gpio_dir0)) { + if (gpio_init(params->motors[i].gpio_dir0, GPIO_OUT)) { err = EIO; - LOG_ERROR("gpio_enable init failed\n"); + LOG_ERROR("gpio_dir0 init failed for motor %d\n", i); + goto motor_init_err; + } + } + if (gpio_is_valid(params->motors[i].gpio_dir1)) { + if (gpio_init(params->motors[i].gpio_dir1, + GPIO_OUT)) { + err = EIO; + LOG_ERROR("gpio_dir1/brake init failed for motor %d\n", i); + goto motor_init_err; + } + } + if (gpio_is_valid(params->motors[i].gpio_enable)) { + if (gpio_init(params->motors[i].gpio_enable, + GPIO_OUT)) { + err = EIO; + LOG_ERROR("gpio_enable init failed for motor %d\n", i); goto motor_init_err; } - motor_enable(motor_driver, i); } } - return 0; + motor_driver->params = params; motor_init_err: return -err; } -int motor_set(const motor_driver_t motor_driver, uint8_t motor_id, \ +static void _motor_set_two_dirs(const motor_t *motor, motor_direction_t direction) +{ + if ((gpio_is_valid(motor->gpio_dir0)) + && (gpio_is_valid(motor->gpio_dir1))) { + gpio_write(motor->gpio_dir0, direction); + gpio_write(motor->gpio_dir1, direction ^ 1); + } +} + +static void _motor_set_one_dir(const motor_t *motor, motor_direction_t direction) +{ + if (gpio_is_valid(motor->gpio_dir0)) { + gpio_write(motor->gpio_dir0, direction); + } +} + +int motor_set(const motor_driver_t *motor_driver, uint8_t motor_id, \ int32_t pwm_duty_cycle) { int err = 0; - assert(motor_driver < MOTOR_DRIVER_NUMOF); + if (motor_id >= motor_driver->params->nb_motors) { + err = EINVAL; + LOG_ERROR("Motor ID %u greater than number of motors %u\n", + motor_id, motor_driver->params->nb_motors); + goto motor_set_err; + } - const motor_driver_config_t *motor_driver_conf = - &motor_driver_config[motor_driver]; - - assert(motor_id < motor_driver_conf->nb_motors); - - const motor_t *dev = &motor_driver_conf->motors[motor_id]; - - int gpio_dir0_value = 0; - int gpio_dir1_or_brake_value = 0; + const motor_t *motor = &motor_driver->params->motors[motor_id]; motor_direction_t direction = (pwm_duty_cycle < 0) ? MOTOR_CCW : MOTOR_CW; + direction = direction ^ motor->gpio_dir_reverse; - direction = direction ^ dev->gpio_dir_reverse; - - /* Two direction GPIO, handling brake */ - if (motor_driver_conf->mode == MOTOR_DRIVER_2_DIRS) { - if (!gpio_is_valid(dev->gpio_dir0) || \ - !gpio_is_valid(dev->gpio_dir1_or_brake)) { - err = ENODEV; - goto motor_set_err; - } - switch (direction) { - case MOTOR_CW: - case MOTOR_CCW: - /* Direction */ - gpio_dir0_value = direction; - gpio_dir1_or_brake_value = direction ^ 0x1; - break; - default: - pwm_duty_cycle = 0; - break; - } - } - /* Single direction GPIO */ - else if (motor_driver_conf->mode == MOTOR_DRIVER_1_DIR) { - if (!gpio_is_valid(dev->gpio_dir0)) { - err = ENODEV; - goto motor_set_err; - } - switch (direction) { - case MOTOR_CW: - case MOTOR_CCW: - /* Direction */ - gpio_dir0_value = direction; - break; - default: - pwm_duty_cycle = 0; - break; - } - } - /* Single direction GPIO and brake GPIO */ - else if (motor_driver_conf->mode == MOTOR_DRIVER_1_DIR_BRAKE) { - if (!gpio_is_valid(dev->gpio_dir0) || \ - !gpio_is_valid(dev->gpio_dir1_or_brake)) { - err = ENODEV; - goto motor_set_err; - } - switch (direction) { - case MOTOR_CW: - case MOTOR_CCW: - /* Direction */ - gpio_dir0_value = direction; - /* No brake */ - gpio_dir1_or_brake_value = dev->gpio_brake_invert; - break; - default: - pwm_duty_cycle = 0; - break; - } - } - else { - err = EINVAL; - goto motor_set_err; + if (direction != MOTOR_CW && direction != MOTOR_CCW) { + pwm_duty_cycle = 0; } /* Absolute value of pwm_duty_cycle */ int32_t pwm_duty_cycle_abs = pwm_duty_cycle; pwm_duty_cycle_abs *= (pwm_duty_cycle < 0) ? -1 : 1; - unsigned irqstate = irq_disable(); - gpio_write(dev->gpio_dir0, gpio_dir0_value); - gpio_write(dev->gpio_dir1_or_brake, gpio_dir1_or_brake_value); - pwm_set(motor_driver_conf->pwm_dev, dev->pwm_channel, \ - (uint16_t)pwm_duty_cycle_abs); - irq_restore(irqstate); + /* Critical section */ + int state = irq_disable(); - motor_driver_cb_t cb = motor_driver_conf->cb; - if (cb) { - cb(motor_driver, motor_id, pwm_duty_cycle); + /* Set direction */ + switch (motor_driver->params->mode) { + /* Two direction GPIO, handling brake */ + case MOTOR_DRIVER_2_DIRS: + _motor_set_two_dirs(motor, direction); + break; + /* Single direction GPIO */ + case MOTOR_DRIVER_1_DIR: + case MOTOR_DRIVER_1_DIR_BRAKE: + _motor_set_one_dir(motor, direction); + break; + /* Error */ + default: + LOG_ERROR("Invalid mode to set direction %u\n", + motor_driver->params->mode); + err = EINVAL; + goto motor_set_err; + break; + } + + /* Apply PWM duty cycle */ + pwm_set(motor_driver->params->pwm_dev, motor->pwm_channel, \ + (uint16_t)pwm_duty_cycle_abs); + + /* Remove brake */ + switch (motor_driver->params->mode) { + /* Two direction GPIO, handling brake */ + case MOTOR_DRIVER_2_DIRS: + _motor_brake_two_dirs(motor, motor_driver->params->brake_inverted); + break; + case MOTOR_DRIVER_1_DIR: + break; + case MOTOR_DRIVER_1_DIR_BRAKE: + _motor_brake_one_dir_brake(motor, motor_driver->params->brake_inverted); + break; + /* Error */ + default: + LOG_ERROR("Invalid mode to unbrake %u\n", motor_driver->params->mode); + err = EINVAL; + goto motor_set_err; + break; + }; + + /* End of critical section */ + irq_restore(state); + + if (motor_driver->params->motor_set_post_cb) { + motor_driver->params->motor_set_post_cb(motor_driver, motor_id, pwm_duty_cycle); } return 0; @@ -186,58 +196,63 @@ motor_set_err: return -err; } -int motor_brake(const motor_driver_t motor_driver, uint8_t motor_id) +static void _motor_brake_two_dirs(const motor_t *motor, bool brake) +{ + if ((gpio_is_valid(motor->gpio_dir0)) + && (gpio_is_valid(motor->gpio_dir1))) { + gpio_write(motor->gpio_dir0, brake); + gpio_write(motor->gpio_dir1, brake); + } +} + +static void _motor_brake_one_dir_brake(const motor_t *motor, bool brake) +{ + if (gpio_is_valid(motor->gpio_brake)) { + gpio_write(motor->gpio_brake, brake); + } +} + +int motor_brake(const motor_driver_t *motor_driver, uint8_t motor_id) { int err = 0; - assert(motor_driver < MOTOR_DRIVER_NUMOF); - - const motor_driver_config_t *motor_driver_conf = - &motor_driver_config[motor_driver]; - - assert(motor_id < motor_driver_conf->nb_motors); - - const motor_t *dev = &motor_driver_conf->motors[motor_id]; - - int gpio_dir0_value = 0; - int gpio_dir1_or_brake_value = 0; - - /* Two direction GPIO, handling brake */ - if (motor_driver_conf->mode == MOTOR_DRIVER_2_DIRS) { - if (!gpio_is_valid(dev->gpio_dir0) || \ - !gpio_is_valid(dev->gpio_dir1_or_brake)) { - err = ENODEV; - goto motor_brake_err; - } - /* Brake */ - gpio_dir0_value = - motor_driver_conf->mode_brake; - gpio_dir1_or_brake_value = - motor_driver_conf->mode_brake; - } - /* Single direction GPIO */ - else if (motor_driver_conf->mode == MOTOR_DRIVER_1_DIR) { - /* Nothing to do here */ - } - /* Single direction GPIO and brake GPIO */ - else if (motor_driver_conf->mode == MOTOR_DRIVER_1_DIR_BRAKE) { - if (!gpio_is_valid(dev->gpio_dir1_or_brake)) { - err = ENODEV; - goto motor_brake_err; - } - /* Brake */ - gpio_dir1_or_brake_value = 1 ^ dev->gpio_brake_invert; - } - else { + if (motor_id >= motor_driver->params->nb_motors) { err = EINVAL; + LOG_ERROR("Motor ID %u greater than number of motors %u\n", + motor_id, motor_driver->params->nb_motors); goto motor_brake_err; } - unsigned irqstate = irq_disable(); - gpio_write(dev->gpio_dir0, gpio_dir0_value); - gpio_write(dev->gpio_dir1_or_brake, gpio_dir1_or_brake_value); - pwm_set(motor_driver_conf->pwm_dev, dev->pwm_channel, 0); - irq_restore(irqstate); + const motor_t *motor = &motor_driver->params->motors[motor_id]; + + /* Critical section */ + int state = irq_disable(); + + /* Apply brake */ + switch (motor_driver->params->mode) { + /* Two direction GPIO, handling brake */ + case MOTOR_DRIVER_2_DIRS: + _motor_brake_two_dirs(motor, !motor_driver->params->brake_inverted); + break; + /* Single direction GPIO */ + case MOTOR_DRIVER_1_DIR: + DEBUG("%s: cannot brake with only one direction pin, just set PWM to 0\n", __func__); + break; + case MOTOR_DRIVER_1_DIR_BRAKE: + _motor_brake_one_dir_brake(motor, !motor_driver->params->brake_inverted); + break; + /* Error */ + default: + LOG_ERROR("Invalid mode to brake %u\n", motor_driver->params->mode); + err = EINVAL; + goto motor_brake_err; + break; + } + + /* Reset PWM duty cycle */ + pwm_set(motor_driver->params->pwm_dev, motor->pwm_channel, 0); + + irq_restore(state); return 0; @@ -245,34 +260,36 @@ motor_brake_err: return -err; } -void motor_enable(const motor_driver_t motor_driver, uint8_t motor_id) +void motor_enable(const motor_driver_t *motor_driver, uint8_t motor_id) { - assert(motor_driver < MOTOR_DRIVER_NUMOF); + if (motor_id >= motor_driver->params->nb_motors) { + LOG_ERROR("Motor ID greater than number of motors\n"); + return; + } - const motor_driver_config_t *motor_driver_conf = - &motor_driver_config[motor_driver]; + const motor_t *motor = &motor_driver->params->motors[motor_id]; - assert(motor_id < motor_driver_conf->nb_motors); - - const motor_t *dev = &motor_driver_conf->motors[motor_id]; - - assert(gpio_is_valid(dev->gpio_enable)); - - gpio_write(dev->gpio_enable, 1 ^ dev->gpio_enable_invert); + if (gpio_is_valid(motor->gpio_enable)) { + gpio_write(motor->gpio_enable, !motor_driver->params->enable_inverted); + } + else { + LOG_WARNING("Enable GPIO is not valid for motor %u, skipping enable\n", motor_id); + } } -void motor_disable(const motor_driver_t motor_driver, uint8_t motor_id) +void motor_disable(const motor_driver_t *motor_driver, uint8_t motor_id) { - assert(motor_driver < MOTOR_DRIVER_NUMOF); + if (motor_id >= motor_driver->params->nb_motors) { + LOG_ERROR("Motor ID greater than number of motors\n"); + return; + } - const motor_driver_config_t *motor_driver_conf = - &motor_driver_config[motor_driver]; + const motor_t *motor = &motor_driver->params->motors[motor_id]; - assert(motor_id < motor_driver_conf->nb_motors); - - const motor_t *dev = &motor_driver_conf->motors[motor_id]; - - assert(gpio_is_valid(dev->gpio_enable)); - - gpio_write(dev->gpio_enable, dev->gpio_enable_invert); + if (gpio_is_valid(motor->gpio_enable)) { + gpio_write(motor->gpio_enable, motor_driver->params->enable_inverted); + } + else { + LOG_WARNING("Enable GPIO is not valid for motor %u, skipping disable\n", motor_id); + } }