kaspar/RIOT
1
0
Fork 0
mirror of https://github.com/RIOT-OS/RIOT.git synced 2025-12-13 08:33:49 +01:00
Code Releases Activity

Merge pull request #20429 from cogip/rework_motor_driver

Browse Source 
drivers/motor_driver: rework motor driver
This commit is contained in:
crasbe 2025-10-30 07:33:01 +00:00 committed by GitHub
commit 49b01c4284
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
16 changed files with 687 additions and 535 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
boards/bitcraze-crazyflie21-main/include/board.h
View File

@ -16,7 +16,6 @@
*/
#include "cpu.h"
#include "motor_driver.h"
#ifdef __cplusplus
extern "C" {
@ -51,78 +50,6 @@ extern "C" {
#define LED4_PORT_NUM PORT_C
/** @} */
/**
* @name Describe DC motors with PWM channel and GPIOs
* @{
*/
/** Motor driver config. Two driver with three and one motor attached respectively */
static const motor_driver_config_t motor_driver_config[] = {
{
.pwm_dev = 1,
.mode = MOTOR_DRIVER_1_DIR,
.mode_brake = MOTOR_BRAKE_HIGH,
.pwm_mode = PWM_LEFT,
.pwm_frequency = 20000U,
.pwm_resolution = 4200U,
.nb_motors = 3,
.motors = {
{
.pwm_channel = 1,
.gpio_enable = 0,
.gpio_dir0 = 0,
.gpio_dir1_or_brake = 0,
.gpio_dir_reverse = 0,
.gpio_enable_invert = 0,
.gpio_brake_invert = 0,
},
{
.pwm_channel = 3,
.gpio_enable = 0,
.gpio_dir0 = 0,
.gpio_dir1_or_brake = 0,
.gpio_dir_reverse = 0,
.gpio_enable_invert = 0,
.gpio_brake_invert = 0,
},
{
.pwm_channel = 0,
.gpio_enable = 0,
.gpio_dir0 = 0,
.gpio_dir1_or_brake = 0,
.gpio_dir_reverse = 0,
.gpio_enable_invert = 0,
.gpio_brake_invert = 0,
},
},
.cb = NULL,
},
{
.pwm_dev = 2,
.mode = MOTOR_DRIVER_1_DIR,
.mode_brake = MOTOR_BRAKE_HIGH,
.pwm_mode = PWM_LEFT,
.pwm_frequency = 20000U,
.pwm_resolution = 4200U,
.nb_motors = 1,
.motors = {
{
.pwm_channel = 0,
.gpio_enable = 0,
.gpio_dir0 = 0,
.gpio_dir1_or_brake = 0,
.gpio_dir_reverse = 0,
.gpio_enable_invert = 0,
.gpio_brake_invert = 0,
},
},
.cb = NULL,
}
};
/** Number of motor drivers */
#define MOTOR_DRIVER_NUMOF ARRAY_SIZE(motor_driver_config)
/** @} */
#ifdef __cplusplus
}
#endif

133
boards/bitcraze-crazyflie21-main/include/motor_driver_params.h Normal file
View File

@ -0,0 +1,133 @@
/*
* SPDX-FileCopyrightText: 2025 COGIP Robotics association
* SPDX-License-Identifier: LGPL-2.1-only
*/
#pragma once
/**
* @ingroup boards_bitcraze_crazyflie21_main
* @{
*
* @file
* @brief Configuration for motor driver on Crazyflie 2.1
*
* @author Gilles DOFFE <g.doffe@gmail.com>
*/
#include "board.h"
#include "motor_driver.h"
#include "saul_reg.h"
#ifdef __cplusplus
extern "C" {
#endif
/**
* @name Motor driver configuration for Crazyflie 2.1
* @{
*/
/**
* @brief Motor driver 0 parameters (3 motors on PWM device 1)
*/
#define MOTOR_DRIVER_0_PARAMS \
{ \
.mode = MOTOR_DRIVER_1_DIR, \
.pwm_dev = 1, \
.pwm_mode = PWM_LEFT, \
.pwm_frequency = 20000U, \
.pwm_resolution = 4200U, \
.brake_inverted = true, \
.enable_inverted = false, \
.nb_motors = 3, \
.motors = { \
{ \
.pwm_channel = 1, \
.gpio_enable = GPIO_UNDEF, \
.gpio_dir0 = GPIO_UNDEF, \
.gpio_dir1 = GPIO_UNDEF, \
.gpio_dir_reverse = GPIO_UNDEF, \
}, \
{ \
.pwm_channel = 3, \
.gpio_enable = GPIO_UNDEF, \
.gpio_dir0 = GPIO_UNDEF, \
.gpio_dir1 = GPIO_UNDEF, \
.gpio_dir_reverse = GPIO_UNDEF, \
}, \
{ \
.pwm_channel = 0, \
.gpio_enable = GPIO_UNDEF, \
.gpio_dir0 = GPIO_UNDEF, \
.gpio_dir1 = GPIO_UNDEF, \
.gpio_dir_reverse = GPIO_UNDEF, \
} \
}, \
.motor_set_post_cb = NULL \
}
/**
* @brief Motor driver 1 parameters (1 motor on PWM device 2)
*/
#define MOTOR_DRIVER_1_PARAMS \
{ \
.mode = MOTOR_DRIVER_1_DIR, \
.pwm_dev = 2, \
.pwm_mode = PWM_LEFT, \
.pwm_frequency = 20000U, \
.pwm_resolution = 4200U, \
.brake_inverted = true, \
.enable_inverted = false, \
.nb_motors = 1, \
.motors = { \
{ \
.pwm_channel = 0, \
.gpio_enable = GPIO_UNDEF, \
.gpio_dir0 = GPIO_UNDEF, \
.gpio_dir1 = GPIO_UNDEF, \
.gpio_dir_reverse = GPIO_UNDEF, \
} \
}, \
.motor_set_post_cb = NULL \
}
#ifndef MOTOR_DRIVER_PARAMS
/**
* @brief Motor driver configuration array
*/
# define MOTOR_DRIVER_PARAMS \
MOTOR_DRIVER_0_PARAMS, \
MOTOR_DRIVER_1_PARAMS
#endif
#ifndef MOTOR_DRIVER_SAUL_INFO
/**
* @brief SAUL registry information for motor drivers
*/
# define MOTOR_DRIVER_SAUL_INFO \
{ .name = "motor_driver_0" }, \
{ .name = "motor_driver_1" }
#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
/** @} */

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 */

63
boards/common/native/include/board.h
View File

@ -20,9 +20,6 @@
#include <stdint.h>
/* RIOT includes */
#include <motor_driver.h>
#ifdef __cplusplus
extern "C" {
#endif
@ -142,74 +139,14 @@ void _native_LED_RED_TOGGLE(void);
/** @} */
#endif
#if MODULE_PERIPH_QDEC
/**
* @brief Simulate QDEC on motor_set() calls
*
* @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
*/
void native_motor_driver_qdec_simulation( \
const motor_driver_t motor_driver, uint8_t motor_id, \
int32_t pwm_duty_cycle);
/* C++ standard do not support designated initializers */
#if !(defined __cplusplus) && (defined MODULE_PERIPH_QDEC)
/**
* @name Describe DC motors with PWM channel and GPIOs
* @{
*/
static const motor_driver_config_t motor_driver_config[] = {
{
.pwm_dev = 0,
.mode = MOTOR_DRIVER_1_DIR_BRAKE,
.mode_brake = MOTOR_BRAKE_LOW,
.pwm_mode = PWM_LEFT,
.pwm_frequency = 20000U,
.pwm_resolution = 1000U,
.nb_motors = 2,
.motors = {
{
.pwm_channel = 0,
.gpio_enable = GPIO_PIN(0, 0),
.gpio_dir0 = GPIO_PIN(0, 0),
.gpio_dir1_or_brake = GPIO_PIN(0, 0),
.gpio_dir_reverse = 0,
.gpio_enable_invert = 0,
.gpio_brake_invert = 0,
},
{
.pwm_channel = 1,
.gpio_enable = GPIO_PIN(0, 0),
.gpio_dir0 = GPIO_PIN(0, 0),
.gpio_dir1_or_brake = GPIO_PIN(0, 0),
.gpio_dir_reverse = 1,
.gpio_enable_invert = 0,
.gpio_brake_invert = 0,
},
},
.cb = native_motor_driver_qdec_simulation,
},
};
#define MOTOR_DRIVER_NUMOF ARRAY_SIZE(motor_driver_config)
/** @} */
#endif
/**
* @name ztimer configuration
* @{
*/
#define CONFIG_ZTIMER_USEC_TYPE ZTIMER_TYPE_PERIPH_TIMER
#define CONFIG_ZTIMER_USEC_DEV TIMER_DEV(0)
/* on native, anything can happen... */
#define CONFIG_ZTIMER_USEC_MIN (64)
/** @} */
#endif /* __cplusplus */
#ifdef __cplusplus
}
#endif

32
boards/common/nucleo64/include/board.h
View File

@ -18,7 +18,6 @@
#include "board_nucleo.h"
#include "arduino_pinmap.h"
#include "motor_driver.h"
#ifdef __cplusplus
extern "C" {
@ -52,37 +51,6 @@ extern "C" {
#endif
/** @} */
/**
* @name Describe DC motors with PWM channel and GPIOs
* @{
*/
static const motor_driver_config_t motor_driver_config[] = {
{
.pwm_dev = 1,
.mode = MOTOR_DRIVER_1_DIR,
.mode_brake = MOTOR_BRAKE_HIGH,
.pwm_mode = PWM_LEFT,
.pwm_frequency = 20000U,
.pwm_resolution = 2250U,
.nb_motors = 1,
.motors = {
{
.pwm_channel = 0,
.gpio_enable = 0,
.gpio_dir0 = ARDUINO_PIN_15,
.gpio_dir1_or_brake = 0,
.gpio_dir_reverse = 0,
.gpio_enable_invert = 0,
.gpio_brake_invert = 0,
},
},
.cb = NULL,
},
};
#define MOTOR_DRIVER_NUMOF ARRAY_SIZE(motor_driver_config)
/** @} */
/**
* @name Describe MRF24J40 radio
* @{

1
boards/nucleo-f446re/Makefile.features
View File

@ -14,7 +14,6 @@ FEATURES_PROVIDED += periph_uart
FEATURES_PROVIDED += periph_qdec
# Various other features (if any)
FEATURES_PROVIDED += motor_driver
FEATURES_PROVIDED += riotboot
# load the common Makefile.features for Nucleo boards

13
cpu/native/include/periph_conf.h
View File

@ -50,19 +50,6 @@ extern "C" {
#define TIMER_CHANNEL_NUMOF (1U) /**< Number of timer channels */
/** @} */
/* MARK: - xtimer configuration */
/**
* @name `xtimer` configuration
* @{
*
* @ref timer_set_absolute has a high margin for possible underflow if set with
* value not far in the future. To prevent this, we set high backoff values
* here.
*/
#define XTIMER_BACKOFF 200
#define XTIMER_ISR_BACKOFF 200
/** @} */
/**
* @brief UART configuration
*/

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);
}
}

8
tests/drivers/motor_driver/Makefile
View File

@ -1,13 +1,13 @@
BOARD ?= native
INCLUDES += -I$(APPDIR)
include ../Makefile.drivers_common
USEMODULE += motor_driver
USEMODULE += shell_cmds_default
USEMODULE += xtimer
FEATURES_REQUIRED += periph_qdec
FEATURES_REQUIRED += motor_driver
USEMODULE += ztimer
USEMODULE += ztimer_msec
CFLAGS += -DLOG_LEVEL=LOG_DEBUG
CFLAGS += -DDEBUG_ASSERT_VERBOSE

8
tests/drivers/motor_driver/Makefile.ci Normal file
View File

@ -0,0 +1,8 @@
BOARD_INSUFFICIENT_MEMORY := \
arduino-duemilanove \
arduino-nano \
arduino-uno \
atmega328p \
atmega328p-xplained-mini \
atmega8 \
#

45
tests/drivers/motor_driver/init_dev.h Normal file
View File

@ -0,0 +1,45 @@
/*
* SPDX-FileCopyrightText: 2023 COGIP Robotics association
* SPDX-License-Identifier: LGPL-2.1-only
*/
#pragma once
/**
* @ingroup tests
* @{
*
* @file
* @brief Motor driver test header file.
*
* @author Gilles DOFFE <g.doffe@gmail.com>
*
*/
#include <motor_driver.h>
#ifndef MOTOR_DRIVER_PARAM_MOTOR_SET_POST_CALLBACK
/** Default callback called at end of motor_set() */
# define MOTOR_DRIVER_PARAM_MOTOR_SET_POST_CALLBACK motor_driver_callback_example
#endif
#ifdef __cplusplus
extern "C" {
#endif
/**
* @brief Simple motor_set post callback example.
* Just print current PWM value.
*
* @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
*/
void motor_driver_callback_example(
const motor_driver_t *motor_driver, uint8_t motor_id,
int32_t pwm_duty_cycle);
#ifdef __cplusplus
}
#endif
/** @} */

89
tests/drivers/motor_driver/main.c
View File

@ -1,9 +1,6 @@
/*
* 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
*/
/**
@ -21,20 +18,36 @@
#include <string.h>
/* RIOT includes */
#include "init_dev.h"
#include "log.h"
#include "motor_driver.h"
#include "motor_driver_params.h"
#include "test_utils/expect.h"
#include "xtimer.h"
#include "time_units.h"
#include "ztimer.h"
/* set interval to 20 milli-second */
#define INTERVAL (3000 * US_PER_MS)
void motor_driver_callback_example(
const motor_driver_t *motor_driver, uint8_t motor_id,
int32_t pwm_duty_cycle)
{
LOG_DEBUG("MOTOR-DRIVER=%p" \
" MOTOR_ID = %"PRIu8 \
" PWM_VALUE = %"PRIi32"\n", \
(void*)motor_driver, motor_id,
pwm_duty_cycle);
}
#define MOTOR_0_ID 0
#define MOTOR_1_ID 1
/* Set interval to 3 seconds */
#define INTERVAL (3 * MS_PER_SEC)
#define MOTOR_0_ID ((uint8_t)0)
#define MOTOR_1_ID ((uint8_t)1)
static motor_driver_t motor_driver;
void motors_control(int32_t duty_cycle)
{
char str[6];
char str[4];
if (duty_cycle >= 0) {
strncpy(str, "CW", 3);
@ -43,26 +56,26 @@ void motors_control(int32_t duty_cycle)
strncpy(str, "CCW", 4);
}
printf("Duty cycle = %" PRId32 " Direction = %s\n", duty_cycle, str);
puts("\nActuate Motors");
if (motor_set(MOTOR_DRIVER_DEV(0), MOTOR_0_ID, duty_cycle)) {
printf("Cannot set PWM duty cycle for motor %" PRIu32 "\n", \
(uint32_t)MOTOR_0_ID);
if (motor_set(&motor_driver, MOTOR_0_ID, duty_cycle)) {
printf("Cannot set PWM duty cycle for motor %" PRIu8 "\n", \
MOTOR_0_ID);
}
if (motor_set(MOTOR_DRIVER_DEV(0), MOTOR_1_ID, duty_cycle)) {
printf("Cannot set PWM duty cycle for motor %" PRIu32 "\n", \
(uint32_t)MOTOR_1_ID);
if (motor_set(&motor_driver, MOTOR_1_ID, duty_cycle)) {
printf("Cannot set PWM duty cycle for motor %" PRIu8 "\n", \
MOTOR_1_ID);
}
}
void motors_brake(void)
{
puts("Brake motors !!!");
puts("\nBrake motors");
if (motor_brake(MOTOR_DRIVER_DEV(0), MOTOR_0_ID)) {
if (motor_brake(&motor_driver, MOTOR_0_ID)) {
printf("Cannot brake motor %" PRIu32 "\n", (uint32_t)MOTOR_0_ID);
}
if (motor_brake(MOTOR_DRIVER_DEV(0), MOTOR_1_ID)) {
if (motor_brake(&motor_driver, MOTOR_1_ID)) {
printf("Cannot brake motor %" PRIu32 "\n", (uint32_t)MOTOR_1_ID);
}
}
@ -71,42 +84,40 @@ void motion_control(void)
{
int8_t dir = 1;
int ret = 0;
xtimer_ticks32_t last_wakeup /*, start*/;
int32_t pwm_res = motor_driver_config[MOTOR_DRIVER_DEV(0)].pwm_resolution;
int32_t pwm_res = motor_driver_params->pwm_resolution;
ret = motor_driver_init(MOTOR_DRIVER_DEV(0));
ret = motor_driver_init(&motor_driver, &motor_driver_params[0]);
if (ret) {
LOG_ERROR("motor_driver_init failed with error code %d\n", ret);
}
expect(ret == 0);
for (;;) {
while (1) {
/* BRAKE - duty cycle 100% */
last_wakeup = xtimer_now();
motors_brake();
xtimer_periodic_wakeup(&last_wakeup, INTERVAL);
ztimer_sleep(ZTIMER_MSEC, INTERVAL);
/* CW - duty cycle 50% */
last_wakeup = xtimer_now();
motors_control(dir * pwm_res / 2);
xtimer_periodic_wakeup(&last_wakeup, INTERVAL);
ztimer_sleep(ZTIMER_MSEC, INTERVAL);
/* Disable motor during INTERVAL µs (motor driver must have enable
feature */
last_wakeup = xtimer_now();
motor_disable(MOTOR_DRIVER_DEV(0), MOTOR_0_ID);
motor_disable(MOTOR_DRIVER_DEV(0), MOTOR_1_ID);
xtimer_periodic_wakeup(&last_wakeup, INTERVAL);
motor_enable(MOTOR_DRIVER_DEV(0), MOTOR_0_ID);
motor_enable(MOTOR_DRIVER_DEV(0), MOTOR_1_ID);
* feature) */
puts("\nDisable motors");
motor_disable(&motor_driver, MOTOR_0_ID);
motor_disable(&motor_driver, MOTOR_1_ID);
ztimer_sleep(ZTIMER_MSEC, INTERVAL);
puts("\nEnable motors");
motor_enable(&motor_driver, MOTOR_0_ID);
motor_enable(&motor_driver, MOTOR_1_ID);
ztimer_sleep(ZTIMER_MSEC, INTERVAL);
/* CW - duty cycle 100% */
last_wakeup = xtimer_now();
motors_control(dir * pwm_res);
xtimer_periodic_wakeup(&last_wakeup, INTERVAL);
ztimer_sleep(ZTIMER_MSEC, INTERVAL);
/* Reverse direction */
dir = dir * -1;
dir *= -1;
}
}