From 8a20936a9d724890128e86090d12611f04051904 Mon Sep 17 00:00:00 2001 From: Gilles DOFFE Date: Thu, 5 Apr 2018 15:51:43 +0200 Subject: [PATCH] drivers: add motor_driver implementation Implement motor_driver API to drive CC motors through hardware drivers. Signed-off-by: Gilles DOFFE --- drivers/Makefile.dep | 4 + drivers/motor_driver/Makefile | 3 + drivers/motor_driver/motor_driver.c | 259 ++++++++++++++++++++++++++++ 3 files changed, 266 insertions(+) create mode 100644 drivers/motor_driver/Makefile create mode 100644 drivers/motor_driver/motor_driver.c diff --git a/drivers/Makefile.dep b/drivers/Makefile.dep index 291b11469c..cdf6366c37 100644 --- a/drivers/Makefile.dep +++ b/drivers/Makefile.dep @@ -303,6 +303,10 @@ ifneq (,$(filter mma8x5x,$(USEMODULE))) FEATURES_REQUIRED += periph_i2c endif +ifneq (,$(filter motor_driver,$(USEMODULE))) + FEATURES_REQUIRED += periph_pwm +endif + ifneq (,$(filter mpl3115a2,$(USEMODULE))) FEATURES_REQUIRED += periph_i2c endif diff --git a/drivers/motor_driver/Makefile b/drivers/motor_driver/Makefile new file mode 100644 index 0000000000..6f68a56e21 --- /dev/null +++ b/drivers/motor_driver/Makefile @@ -0,0 +1,3 @@ +MODULE = motor_driver + +include $(RIOTBASE)/Makefile.base diff --git a/drivers/motor_driver/motor_driver.c b/drivers/motor_driver/motor_driver.c new file mode 100644 index 0000000000..24e3543976 --- /dev/null +++ b/drivers/motor_driver/motor_driver.c @@ -0,0 +1,259 @@ +#include + +/* RIOT includes */ +#include +#include +#include +#include +#include + +#define ENABLE_DEBUG (0) +#include + +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); +}