mirror of
https://github.com/RIOT-OS/RIOT.git
synced 2024-12-29 04:50:03 +01:00
6d61381d2a
The expandable GPIO API requires the comparison of structured GPIO types. This means that inline functions must be used instead of direct comparisons. For the migration process, drivers must first be changed so that they use the inline comparison functions.
279 lines
7.7 KiB
C
279 lines
7.7 KiB
C
/*
|
|
* 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.
|
|
*/
|
|
|
|
/**
|
|
* @ingroup drivers_motor
|
|
* @{
|
|
*
|
|
* @file
|
|
* @brief High-level driver for DC motors
|
|
*
|
|
* @author Gilles DOFFE <g.doffe@gmail.com>
|
|
* @}
|
|
*/
|
|
|
|
#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 (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,
|
|
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 (!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;
|
|
}
|
|
|
|
/* 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 (!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 {
|
|
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(gpio_is_valid(dev->gpio_enable));
|
|
|
|
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(gpio_is_valid(dev->gpio_enable));
|
|
|
|
gpio_write(dev->gpio_enable, dev->gpio_enable_invert);
|
|
}
|