kaspar/RIOT
1
0
Fork 0
mirror of https://github.com/RIOT-OS/RIOT.git synced 2025-12-15 17:43:51 +01:00
Code Releases Activity

drivers/motor_driver: rework driver

Browse Source 
The motor_driver device driver is developed as a periph driver and it
should not.
Make this driver compliant with RIOT device driver development guide
[1].
Also make some cleanups and fix some typos.

[1] https://doc.riot-os.org/driver-guide.html

Signed-off-by: Gilles DOFFE <g.doffe@gmail.com>
Co-authored-by: crasbe <crasbe@gmail.com>
This commit is contained in:
Gilles DOFFE 2025-10-17 17:50:46 +02:00
parent 34ec109bb9
commit fc7f3cac4a
6 changed files with 447 additions and 310 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
boards/common/native/drivers/native-qdec.c
View File

@ -1,59 +0,0 @@
/*
* SPDX-FileCopyrightText: 2018 Gilles DOFFE <g.doffe@gmail.com>
* SPDX-License-Identifier: LGPL-2.1-only
*/
/*
* Native Board board implementation
*
* @ingroup boards_common_native
* @{
* @file
* @author Gilles DOFFE <g.doffe@gmail.com>
* @}
*
*/
#include <inttypes.h>
#include <stdio.h>
/* RIOT includes */
#include <board.h>
#include <log.h>
#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 */

4
dist/tools/doccheck/exclude_simple vendored
View File

@ -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.

130
drivers/include/motor_driver.h
View File

@ -1,15 +1,12 @@
/*
* Copyright (C) 2018 Gilles DOFFE <g.doffe@gmail.com>
*
* 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 <g.doffe@gmail.com>
* 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 <g.doffe@gmail.com>
*/
#include <stdbool.h>
#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
}

2
drivers/motor_driver/Makefile.include Normal file
View File

@ -0,0 +1,2 @@
USEMODULE_INCLUDES_motor_driver := $(LAST_MAKEFILEDIR)/include
USEMODULE_INCLUDES += $(USEMODULE_INCLUDES_motor_driver)

171
drivers/motor_driver/include/motor_driver_params.h Normal file
View File

@ -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 <g.doffe@gmail.com>
*
*/
#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
/** @} */

391
drivers/motor_driver/motor_driver.c
View File

@ -1,13 +1,10 @@
/*
* Copyright (C) 2018 Gilles DOFFE <g.doffe@gmail.com>
*
* 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 <g.doffe@gmail.com>
* SPDX-License-Identifier: LGPL-2.1-only
*/
/**
* @ingroup drivers_motor
* @ingroup drivers_motor_driver
* @{
*
* @file
@ -29,155 +26,168 @@
#define ENABLE_DEBUG 0
#include <debug.h>
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);
}
}