From c234fb1c324cb9df0ba9ee4c6f6223eeef80bbb4 Mon Sep 17 00:00:00 2001 From: Jeonghoon Park Date: Fri, 15 Dec 2017 10:02:50 +0900 Subject: [PATCH] add l298n(motor driver) api in resource module --- inc/resource/resource_motor_driver_L298N.h | 51 ++++ .../resource_motor_driver_L298N_internal.h | 27 ++ src/resource/resource_motor_driver_L298N.c | 333 +++++++++++++++++++++ 3 files changed, 411 insertions(+) create mode 100644 inc/resource/resource_motor_driver_L298N.h create mode 100644 inc/resource/resource_motor_driver_L298N_internal.h create mode 100644 src/resource/resource_motor_driver_L298N.c diff --git a/inc/resource/resource_motor_driver_L298N.h b/inc/resource/resource_motor_driver_L298N.h new file mode 100644 index 0000000..11ddf8b --- /dev/null +++ b/inc/resource/resource_motor_driver_L298N.h @@ -0,0 +1,51 @@ +/* + * Copyright (c) 2017 Samsung Electronics Co., Ltd. + * + * Contact: Jeonghoon Park + * + * Licensed under the Flora License, Version 1.1 (the License); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://floralicense.org/license/ + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef __RESOURCE_MOTOR_DRIVER_L298N_H__ +#define __RESOURCE_MOTOR_DRIVER_L298N_H__ + +/* connected GPIO pin numbers of raspberry pi 3 with IN pins of L298N */ +#define DEFAULT_MOTOR1_PIN1 26 +#define DEFAULT_MOTOR1_PIN2 20 + +#define DEFAULT_MOTOR2_PIN1 19 +#define DEFAULT_MOTOR2_PIN2 16 + +#define DEFAULT_MOTOR3_PIN1 6 +#define DEFAULT_MOTOR3_PIN2 12 + +#define DEFAULT_MOTOR4_PIN1 22 +#define DEFAULT_MOTOR4_PIN2 23 + +/* connected channel numbers of PCA9685 with enable pins of L298N */ +#define DEFAULT_MOTOR1_EN_CH 1 +#define DEFAULT_MOTOR2_EN_CH 2 +#define DEFAULT_MOTOR3_EN_CH 3 +#define DEFAULT_MOTOR4_EN_CH 4 + +typedef enum { + MOTOR_ID_1, + MOTOR_ID_2, + MOTOR_ID_3, + MOTOR_ID_4, + MOTOR_ID_MAX +} motor_id_e; + +int resource_set_motor_driver_L298N_speed(motor_id_e id, int speed); + +#endif /* __RESOURCE_MOTOR_DRIVER_L298N_H__ */ diff --git a/inc/resource/resource_motor_driver_L298N_internal.h b/inc/resource/resource_motor_driver_L298N_internal.h new file mode 100644 index 0000000..7808d59 --- /dev/null +++ b/inc/resource/resource_motor_driver_L298N_internal.h @@ -0,0 +1,27 @@ +/* + * Copyright (c) 2017 Samsung Electronics Co., Ltd. + * + * Contact: Jeonghoon Park + * + * Licensed under the Flora License, Version 1.1 (the License); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://floralicense.org/license/ + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef __RESOURCE_MOTOR_DRIVER_L298N_INTERNAL_H__ +#define __RESOURCE_MOTOR_DRIVER_L298N_INTERNAL_H__ + +int resource_set_motor_driver_L298N_configuration(motor_id_e id, + unsigned int pin1, unsigned int pin2, unsigned en_ch); +void resource_close_motor_driver_L298N(motor_id_e id); +void resource_close_motor_driver_L298N_all(void); + +#endif /* __RESOURCE_MOTOR_DRIVER_L298N_INTERNAL_H__ */ diff --git a/src/resource/resource_motor_driver_L298N.c b/src/resource/resource_motor_driver_L298N.c new file mode 100644 index 0000000..a8a9726 --- /dev/null +++ b/src/resource/resource_motor_driver_L298N.c @@ -0,0 +1,333 @@ +/* + * Copyright (c) 2017 Samsung Electronics Co., Ltd. + * + * Contact: Jeonghoon Park + * + * Licensed under the Flora License, Version 1.1 (the License); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://floralicense.org/license/ + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include "log.h" +#include "resource/resource_PCA9685.h" +#include "resource/resource_motor_driver_L298N.h" + +typedef enum { + MOTOR_STATE_NONE, + MOTOR_STATE_CONFIGURED, + MOTOR_STATE_STOP, + MOTOR_STATE_FORWARD, + MOTOR_STATE_BACKWARD, +} motor_state_e; + +typedef struct __motor_driver_s { + unsigned int pin_1; + unsigned int pin_2; + unsigned int en_ch; + motor_state_e motor_state; + peripheral_gpio_h pin1_h; + peripheral_gpio_h pin2_h; +} motor_driver_s; + +static motor_driver_s g_md_h[MOTOR_ID_MAX] = { + {0, 0, 0, MOTOR_STATE_NONE, NULL, NULL}, +}; + + +/* see Principle section in http://wiki.sunfounder.cc/index.php?title=Motor_Driver_Module-L298N */ + +static int __motor_brake_n_stop_by_id(motor_id_e id) +{ + int ret = PERIPHERAL_ERROR_NONE; + int motor1_v = 0; + int motor2_v = 0; + + if (g_md_h[id].motor_state <= MOTOR_STATE_CONFIGURED) { + _E("motor[%d] are not initialized - state(%d)", + id, g_md_h[id].motor_state); + return -1; + } + + if (g_md_h[id].motor_state == MOTOR_STATE_STOP) { + _D("motor[%d] is already stopped", id); + return 0; + } + + if (g_md_h[id].motor_state == MOTOR_STATE_FORWARD) { + motor1_v = 0; + motor2_v = 0; + } else if (g_md_h[id].motor_state == MOTOR_STATE_BACKWARD) { + motor1_v = 1; + motor2_v = 1; + } + + /* Brake DC motor */ + ret = peripheral_gpio_write(g_md_h[id].pin1_h, motor1_v); + if (ret != PERIPHERAL_ERROR_NONE) { + _E("Failed to set value[%d] Motor[%d] pin 1", motor1_v, id); + return -1; + } + + ret = peripheral_gpio_write(g_md_h[id].pin2_h, motor2_v); + if (ret != PERIPHERAL_ERROR_NONE) { + _E("Failed to set value[%d] Motor[%d] pin 2", motor2_v, id); + return -1; + } + + /* set stop DC motor */ + // need to stop motor or not?, it may stop motor to free running + resource_pca9685_set_value_to_channel(g_md_h[id].en_ch, 0, 0); + + g_md_h[id].motor_state = MOTOR_STATE_STOP; + + return 0; +} + +static int __set_default_configuration_by_id(motor_id_e id) +{ + unsigned int pin_1, pin_2, en_ch; + + switch (id) { + case MOTOR_ID_1: + pin_1 = DEFAULT_MOTOR1_PIN1; + pin_2 = DEFAULT_MOTOR1_PIN2; + en_ch = DEFAULT_MOTOR1_EN_CH; + break; + case MOTOR_ID_2: + pin_1 = DEFAULT_MOTOR2_PIN1; + pin_2 = DEFAULT_MOTOR2_PIN2; + en_ch = DEFAULT_MOTOR2_EN_CH; + break; + case MOTOR_ID_3: + pin_1 = DEFAULT_MOTOR3_PIN1; + pin_2 = DEFAULT_MOTOR3_PIN2; + en_ch = DEFAULT_MOTOR3_EN_CH; + break; + case MOTOR_ID_4: + pin_1 = DEFAULT_MOTOR4_PIN1; + pin_2 = DEFAULT_MOTOR4_PIN2; + en_ch = DEFAULT_MOTOR4_EN_CH; + break; + case MOTOR_ID_MAX: + default: + _E("Unkwon ID[%d]", id); + return -1; + break; + } + + g_md_h[id].pin_1 = pin_1; + g_md_h[id].pin_2 = pin_2; + g_md_h[id].en_ch = en_ch; + g_md_h[id].motor_state = MOTOR_STATE_CONFIGURED; + + return 0; +} + +static int __fini_motor_by_id(motor_id_e id) +{ + retv_if(id == MOTOR_ID_MAX, -1); + + if (g_md_h[id].motor_state <= MOTOR_STATE_CONFIGURED) + return 0; + + if (g_md_h[id].motor_state > MOTOR_STATE_STOP) + __motor_brake_n_stop_by_id(id); + + resource_pca9685_fini(); + + if (g_md_h[id].pin1_h) { + peripheral_gpio_close(g_md_h[id].pin1_h); + g_md_h[id].pin1_h = NULL; + } + + if (g_md_h[id].pin2_h) { + peripheral_gpio_close(g_md_h[id].pin2_h); + g_md_h[id].pin2_h = NULL; + } + + g_md_h[id].motor_state = MOTOR_STATE_CONFIGURED; + + return 0; +} + +static int __init_motor_by_id(motor_id_e id) +{ + int ret = 0; + + retv_if(id == MOTOR_ID_MAX, -1); + + if (g_md_h[id].motor_state == MOTOR_STATE_NONE) + __set_default_configuration_by_id(id); + + ret = resource_pca9685_init(); + if (ret) { + _E("failed to init PCA9685"); + return -1; + } + + /* open pins for Motor */ + ret = peripheral_gpio_open(g_md_h[id].pin_1, &g_md_h[id].pin1_h); + if (ret == PERIPHERAL_ERROR_NONE) + peripheral_gpio_set_direction(g_md_h[id].pin1_h, + PERIPHERAL_GPIO_DIRECTION_OUT_INITIALLY_LOW); + else { + _E("failed to open Motor[%d] gpio pin1[%u]", id, g_md_h[id].pin_1); + goto ERROR; + } + + ret = peripheral_gpio_open(g_md_h[id].pin_2, &g_md_h[id].pin2_h); + if (ret == PERIPHERAL_ERROR_NONE) + peripheral_gpio_set_direction(g_md_h[id].pin2_h, + PERIPHERAL_GPIO_DIRECTION_OUT_INITIALLY_LOW); + else { + _E("failed to open Motor[%d] gpio pin2[%u]", id, g_md_h[id].pin_2); + goto ERROR; + } + + g_md_h[id].motor_state = MOTOR_STATE_STOP; + + return 0; + +ERROR: + resource_pca9685_fini(); + + if (g_md_h[id].pin1_h) { + peripheral_gpio_close(g_md_h[id].pin1_h); + g_md_h[id].pin1_h = NULL; + } + + if (g_md_h[id].pin2_h) { + peripheral_gpio_close(g_md_h[id].pin2_h); + g_md_h[id].pin2_h = NULL; + } + + return -1; +} + +void resource_close_motor_driver_L298N(motor_id_e id) +{ + __fini_motor_by_id(id); + return; +} + +void resource_close_motor_driver_L298N_all(void) +{ + int i; + for (i = MOTOR_ID_1; i < MOTOR_ID_MAX; i++) + __fini_motor_by_id(i); + + return; +} + +int resource_set_motor_driver_L298N_configuration(motor_id_e id, + unsigned int pin1, unsigned int pin2, unsigned en_ch) +{ + + if (g_md_h[id].motor_state > MOTOR_STATE_CONFIGURED) { + _E("cannot set configuration motor[%d] in this state[%d]", + id, g_md_h[id].motor_state); + return -1; + } + + g_md_h[id].pin_1 = pin1; + g_md_h[id].pin_2 = pin2; + g_md_h[id].en_ch = en_ch; + g_md_h[id].motor_state = MOTOR_STATE_CONFIGURED; + + return 0; +} + +int resource_set_motor_driver_L298N_speed(motor_id_e id, int speed) +{ + int ret = 0; + const int value_max = 4095; + int value = 0; + int e_state = MOTOR_STATE_NONE; + int motor_v_1 = 0; + int motor_v_2 = 0; + + if (g_md_h[id].motor_state <= MOTOR_STATE_CONFIGURED) { + ret = __init_motor_by_id(id); + if (ret) { + _E("failed to __init_motor_by_id()"); + return -1; + } + } + + value = abs(speed); + + if (value > value_max) { + value = value_max; + _D("max speed is %d", value_max); + } + _D("set speed %d", value); + + if (speed == 0) { + /* brake and stop */ + ret = __motor_brake_n_stop_by_id(id); + if (ret) { + _E("failed to stop motor[%d]", id); + return -1; + } + return 0; /* done */ + } + + if (speed > 0) + e_state = MOTOR_STATE_FORWARD; /* will be set forward */ + else + e_state = MOTOR_STATE_BACKWARD; /* will be set backward */ + + if (g_md_h[id].motor_state == e_state) + goto SET_SPEED; + else { + /* brake and stop */ + ret = __motor_brake_n_stop_by_id(id); + if (ret) { + _E("failed to stop motor[%d]", id); + return -1; + } + } + + switch (e_state) { + case MOTOR_STATE_FORWARD: + motor_v_1 = 1; + motor_v_2 = 0; + break; + case MOTOR_STATE_BACKWARD: + motor_v_1 = 0; + motor_v_2 = 1; + break; + } + ret = peripheral_gpio_write(g_md_h[id].pin1_h, motor_v_1); + if (ret != PERIPHERAL_ERROR_NONE) { + _E("failed to set value[%d] Motor[%d] pin 1", motor_v_1, id); + return -1; + } + + ret = peripheral_gpio_write(g_md_h[id].pin2_h, motor_v_2); + if (ret != PERIPHERAL_ERROR_NONE) { + _E("failed to set value[%d] Motor[%d] pin 2", motor_v_2, id); + return -1; + } + +SET_SPEED: + ret = resource_pca9685_set_value_to_channel(g_md_h[id].en_ch, 0, value); + if (ret) { + _E("failed to set speed - %d", speed); + return -1; + } + + g_md_h[id].motor_state = e_state; + + return 0; +} -- 2.7.4