mirror of
https://github.com/RIOT-OS/RIOT.git
synced 2024-12-29 04:50:03 +01:00
drivers: add motor_driver implementation
Implement motor_driver API to drive CC motors through hardware drivers. Signed-off-by: Gilles DOFFE <g.doffe@gmail.com>
This commit is contained in:
Gilles DOFFE
parent
dd6a37517b
commit
8a20936a9d
@ -303,6 +303,10 @@ ifneq (,$(filter mma8x5x,$(USEMODULE)))
|
|||||||
FEATURES_REQUIRED += periph_i2c
|
FEATURES_REQUIRED += periph_i2c
|
||||||
endif
|
endif
|
||||||
|
|
||||||
|
ifneq (,$(filter motor_driver,$(USEMODULE)))
|
||||||
|
FEATURES_REQUIRED += periph_pwm
|
||||||
|
endif
|
||||||
|
|
||||||
ifneq (,$(filter mpl3115a2,$(USEMODULE)))
|
ifneq (,$(filter mpl3115a2,$(USEMODULE)))
|
||||||
FEATURES_REQUIRED += periph_i2c
|
FEATURES_REQUIRED += periph_i2c
|
||||||
endif
|
endif
|
||||||
|
|||||||
3
drivers/motor_driver/Makefile
Normal file
3
drivers/motor_driver/Makefile
Normal file
@ -0,0 +1,3 @@
|
|||||||
|
MODULE = motor_driver
|
||||||
|
|
||||||
|
include $(RIOTBASE)/Makefile.base
|
||||||
259
drivers/motor_driver/motor_driver.c
Normal file
259
drivers/motor_driver/motor_driver.c
Normal file
@ -0,0 +1,259 @@
|
|||||||
|
#include <errno.h>
|
||||||
|
|
||||||
|
/* RIOT includes */
|
||||||
|
#include <assert.h>
|
||||||
|
#include <board.h>
|
||||||
|
#include <irq.h>
|
||||||
|
#include <log.h>
|
||||||
|
#include <motor_driver.h>
|
||||||
|
|
||||||
|
#define ENABLE_DEBUG (0)
|
||||||
|
#include <debug.h>
|
||||||
|
|
||||||
|
int motor_driver_init(motor_driver_t motor_driver)
|
||||||
|
{
|
||||||
|
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 ret_pwm = pwm_init(pwm_dev, mode, freq, resol);
|
||||||
|
if (ret_pwm != freq) {
|
||||||
|
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 ((motor_driver_conf->motors[i].gpio_dir0 != GPIO_UNDEF)
|
||||||
|
&& (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 ((motor_driver_conf->motors[i].gpio_dir1_or_brake != GPIO_UNDEF)
|
||||||
|
&& (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 (motor_driver_conf->motors[i].gpio_enable != GPIO_UNDEF) {
|
||||||
|
if (gpio_init(motor_driver_conf->motors[i].gpio_enable,
|
||||||
|
GPIO_OUT)) {
|
||||||
|
err = EIO;
|
||||||
|
LOG_ERROR("gpio_enable init failed\n");
|
||||||
|
goto motor_init_err;
|
||||||
|
}
|
||||||
|
motor_enable(motor_driver, i);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
|
||||||
|
motor_init_err:
|
||||||
|
return -err;
|
||||||
|
}
|
||||||
|
|
||||||
|
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);
|
||||||
|
|
||||||
|
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;
|
||||||
|
|
||||||
|
motor_direction_t direction = (pwm_duty_cycle < 0) ? MOTOR_CCW : MOTOR_CW;
|
||||||
|
|
||||||
|
direction = direction ^ dev->gpio_dir_reverse;
|
||||||
|
|
||||||
|
/* Two direction GPIO, handling brake */
|
||||||
|
if (motor_driver_conf->mode == MOTOR_DRIVER_2_DIRS) {
|
||||||
|
if ((dev->gpio_dir0 == GPIO_UNDEF) || \
|
||||||
|
(dev->gpio_dir1_or_brake == GPIO_UNDEF)) {
|
||||||
|
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 (dev->gpio_dir0 == GPIO_UNDEF) {
|
||||||
|
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 ((dev->gpio_dir0 == GPIO_UNDEF) || \
|
||||||
|
(dev->gpio_dir1_or_brake == GPIO_UNDEF)) {
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* 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;
|
||||||
|
|
||||||
|
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_enable();
|
||||||
|
|
||||||
|
motor_driver_cb_t cb = motor_driver_conf->cb;
|
||||||
|
if (cb) {
|
||||||
|
cb(motor_driver, motor_id, pwm_duty_cycle);
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
|
||||||
|
motor_set_err:
|
||||||
|
return -err;
|
||||||
|
}
|
||||||
|
|
||||||
|
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 ((dev->gpio_dir0 == GPIO_UNDEF) || \
|
||||||
|
(dev->gpio_dir1_or_brake == GPIO_UNDEF)) {
|
||||||
|
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 (dev->gpio_dir1_or_brake == GPIO_UNDEF) {
|
||||||
|
err = ENODEV;
|
||||||
|
goto motor_brake_err;
|
||||||
|
}
|
||||||
|
/* Brake */
|
||||||
|
gpio_dir1_or_brake_value = 1 ^ dev->gpio_brake_invert;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
err = EINVAL;
|
||||||
|
goto motor_brake_err;
|
||||||
|
}
|
||||||
|
|
||||||
|
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_enable();
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
|
||||||
|
motor_brake_err:
|
||||||
|
return -err;
|
||||||
|
}
|
||||||
|
|
||||||
|
void motor_enable(const motor_driver_t motor_driver, uint8_t motor_id)
|
||||||
|
{
|
||||||
|
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];
|
||||||
|
|
||||||
|
assert(dev->gpio_enable != GPIO_UNDEF);
|
||||||
|
|
||||||
|
gpio_write(dev->gpio_enable, 1 ^ dev->gpio_enable_invert);
|
||||||
|
}
|
||||||
|
|
||||||
|
void motor_disable(const motor_driver_t motor_driver, uint8_t motor_id)
|
||||||
|
{
|
||||||
|
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];
|
||||||
|
|
||||||
|
assert(dev->gpio_enable != GPIO_UNDEF);
|
||||||
|
|
||||||
|
gpio_write(dev->gpio_enable, dev->gpio_enable_invert);
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue
Block a user