From: Jeonghoon Park Date: Tue, 19 Dec 2017 05:27:46 +0000 (+0900) Subject: adds motor related modules X-Git-Url: http://review.tizen.org/git/?a=commitdiff_plain;h=ae765965b6522da30af70a365e25903ad2a9cae8;hp=d0cc43db3cd631a59b1bf6b0322fc86df88662dc;p=apps%2Fnative%2Fposition-finder-server.git adds motor related modules Change-Id: Ibfa78de491b4064b40224af125a0360034a0608b --- diff --git a/inc/resource/resource_PCA9685.h b/inc/resource/resource_PCA9685.h new file mode 100644 index 0000000..aa45477 --- /dev/null +++ b/inc/resource/resource_PCA9685.h @@ -0,0 +1,29 @@ +/* + * 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_PCA9685_H__ +#define __RESOURCE_PCA9685_H__ + +#define PCA9685_CH_MAX 15 + +int resource_pca9685_init(unsigned int ch); +int resource_pca9685_fini(unsigned int ch); +int resource_pca9685_set_frequency(unsigned int freq_hz); +int resource_pca9685_set_value_to_channel(unsigned int channel, int on, int off); + +#endif /* __RESOURCE_PCA9685_H__ */ diff --git a/inc/resource/resource_motor_driver_L298N.h b/inc/resource/resource_motor_driver_L298N.h new file mode 100644 index 0000000..1cf7bae --- /dev/null +++ b/inc/resource/resource_motor_driver_L298N.h @@ -0,0 +1,82 @@ +/* + * 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__ + +/** + * This module is sample codes to handling DC motors in Tizen platform. + * HW is configured with L298N(motor driver) and PCA9685(PWM controller). + * To control motor, we use two GPIO pins of IoT board(e.g. RPi 3) connected + * with L298N and a PWM channel of PCA9685 connected with L298N + */ + +/* Default GPIO pins of raspberry pi 3 connected 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 + +/* Default 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 + + +/** + * @brief Enumeration for motor id. + */ +typedef enum { + MOTOR_ID_1, + MOTOR_ID_2, + MOTOR_ID_3, + MOTOR_ID_4, + MOTOR_ID_MAX +} motor_id_e; + +/** + * @param[in] id The motor id + * @param[in] pin1 The first pin number to control motor + * @param[in] pin2 The second pin number to control motor + * @param[in] en_ch The enable channnel number to control PWM signal + * + * @return 0 on success, otherwise a negative error value + * @before resource_set_motor_driver_L298N_speed() : Optional + */ +int resource_set_motor_driver_L298N_configuration(motor_id_e id, + unsigned int pin1, unsigned int pin2, unsigned en_ch); + +/** + * @param[in] id The motor id + * @param[in] speed The speed to control motor, 0 to stop motor, + * positive value to rotate clockwise and higher value to rotate more fast + * negative value to rotate couterclockwise and lower value to rotate more fast + * @return 0 on success, otherwise a negative error value + * @before resource_set_motor_driver_L298N_speed() : Optional + */ +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..fa5beb1 --- /dev/null +++ b/inc/resource/resource_motor_driver_L298N_internal.h @@ -0,0 +1,25 @@ +/* + * 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__ + +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/inc/resource/resource_servo_motor.h b/inc/resource/resource_servo_motor.h new file mode 100644 index 0000000..924d6cc --- /dev/null +++ b/inc/resource/resource_servo_motor.h @@ -0,0 +1,36 @@ +/* + * 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_SERVO_MOTOR_H__ +#define __RESOURCE_SERVO_MOTOR_H__ + +/** + * This module is sample codes to handling Servo motors in Tizen platform. + * HW is configured with PCA9685(PWM controller). + */ + +/** + * @param[in] id The motor id + * @param[in] value The value to control servo motor + * + * @return 0 on success, otherwise a negative error value + * @remarks Must adjust servo motor with some value before use to fit your system. + */ +int resource_set_servo_motor_value(unsigned int motor_id, int value); + +#endif /* __RESOURCE_SERVO_MOTOR_H__ */ diff --git a/inc/resource/resource_servo_motor_internal.h b/inc/resource/resource_servo_motor_internal.h new file mode 100644 index 0000000..e038390 --- /dev/null +++ b/inc/resource/resource_servo_motor_internal.h @@ -0,0 +1,25 @@ +/* + * 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_SERVO_MOTOR_INTERNAL_H__ +#define __RESOURCE_SERVO_MOTOR_INTERNAL_H__ + +void resource_close_servo_motor(unsigned int ch); +void resource_close_servo_motor_all(void); + +#endif /* __RESOURCE_SERVO_MOTOR_INTERNAL_H__ */ diff --git a/src/resource/resource_PCA9685.c b/src/resource/resource_PCA9685.c new file mode 100644 index 0000000..20a5545 --- /dev/null +++ b/src/resource/resource_PCA9685.c @@ -0,0 +1,249 @@ +/* + * 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 +#include +#include "log.h" +#include "resource/resource_PCA9685.h" + +#define RPI3_I2C_BUS 1 + +/* Registers/etc: */ +#define PCA9685_ADDRESS 0x40 +#define MODE1 0x00 +#define MODE2 0x01 +#define SUBADR1 0x02 +#define SUBADR2 0x03 +#define SUBADR3 0x04 +#define PRESCALE 0xFE +#define LED0_ON_L 0x06 +#define LED0_ON_H 0x07 +#define LED0_OFF_L 0x08 +#define LED0_OFF_H 0x09 +#define ALL_LED_ON_L 0xFA +#define ALL_LED_ON_H 0xFB +#define ALL_LED_OFF_L 0xFC +#define ALL_LED_OFF_H 0xFD + +/* Bits: */ +#define RESTART 0x80 +#define SLEEP 0x10 +#define ALLCALL 0x01 +#define INVRT 0x10 +#define OUTDRV 0x04 + +typedef enum { + PCA9685_CH_STATE_NONE, + PCA9685_CH_STATE_USED, +} pca9685_ch_state_e; + +static peripheral_i2c_h g_i2c_h = NULL; +static unsigned int ref_count = 0; +static pca9685_ch_state_e ch_state[PCA9685_CH_MAX + 1] = {PCA9685_CH_STATE_NONE, }; + +int resource_pca9685_set_frequency(unsigned int freq_hz) +{ + int ret = PERIPHERAL_ERROR_NONE; + double prescale_value = 0.0; + int prescale = 0; + uint8_t oldmode = 0; + uint8_t newmode = 0; + + prescale_value = 25000000.0; // 25MHz + prescale_value /= 4096.0; // 12-bit + prescale_value /= (double)freq_hz; + prescale_value -= 1.0; + + prescale = (int)floor(prescale_value + 0.5); + + ret = peripheral_i2c_read_register_byte(g_i2c_h, MODE1, &oldmode); + retvm_if(ret != PERIPHERAL_ERROR_NONE, -1, "failed to read register"); + + newmode = (oldmode & 0x7F) | 0x10; // sleep + ret = peripheral_i2c_write_register_byte(g_i2c_h, MODE1, newmode); // go to sleep + retvm_if(ret != PERIPHERAL_ERROR_NONE, -1, "failed to write register"); + + ret = peripheral_i2c_write_register_byte(g_i2c_h, PRESCALE, prescale); + retvm_if(ret != PERIPHERAL_ERROR_NONE, -1, "failed to write register"); + + ret = peripheral_i2c_write_register_byte(g_i2c_h, MODE1, oldmode); + retvm_if(ret != PERIPHERAL_ERROR_NONE, -1, "failed to write register"); + + usleep(500); + + ret = peripheral_i2c_write_register_byte(g_i2c_h, MODE1, (oldmode | 0x80)); + retvm_if(ret != PERIPHERAL_ERROR_NONE, -1, "failed to write register"); + + return 0; +} + +int resource_pca9685_set_value_to_channel(unsigned int channel, int on, int off) +{ + int ret = PERIPHERAL_ERROR_NONE; + retvm_if(g_i2c_h == NULL, -1, "Not initialized yet"); + + retvm_if(ch_state[channel] == PCA9685_CH_STATE_NONE, -1, + "ch[%u] is not in used state", channel); + + ret = peripheral_i2c_write_register_byte(g_i2c_h, + LED0_ON_L + 4*channel, on & 0xFF); + retvm_if(ret != PERIPHERAL_ERROR_NONE, -1, "failed to write register"); + + ret = peripheral_i2c_write_register_byte(g_i2c_h, + LED0_ON_H + 4*channel, on >> 8); + retvm_if(ret != PERIPHERAL_ERROR_NONE, -1, "failed to write register"); + + ret = peripheral_i2c_write_register_byte(g_i2c_h, + LED0_OFF_L + 4*channel, off & 0xFF); + retvm_if(ret != PERIPHERAL_ERROR_NONE, -1, "failed to write register"); + + ret = peripheral_i2c_write_register_byte(g_i2c_h, + LED0_OFF_H + 4*channel, off >> 8); + retvm_if(ret != PERIPHERAL_ERROR_NONE, -1, "failed to write register"); + + return 0; +} + +static int resource_pca9685_set_value_to_all(int on, int off) +{ + int ret = PERIPHERAL_ERROR_NONE; + retvm_if(g_i2c_h == NULL, -1, "Not initialized yet"); + + ret = peripheral_i2c_write_register_byte(g_i2c_h, + ALL_LED_ON_L, on & 0xFF); + retvm_if(ret != PERIPHERAL_ERROR_NONE, -1, "failed to write register"); + + ret = peripheral_i2c_write_register_byte(g_i2c_h, + ALL_LED_ON_H, on >> 8); + retvm_if(ret != PERIPHERAL_ERROR_NONE, -1, "failed to write register"); + + ret = peripheral_i2c_write_register_byte(g_i2c_h, + ALL_LED_OFF_L, off & 0xFF); + retvm_if(ret != PERIPHERAL_ERROR_NONE, -1, "failed to write register"); + + ret = peripheral_i2c_write_register_byte(g_i2c_h, + ALL_LED_OFF_H, off >> 8); + retvm_if(ret != PERIPHERAL_ERROR_NONE, -1, "failed to write register"); + + return 0; +} + +int resource_pca9685_init(unsigned int ch) +{ + uint8_t mode1 = 0; + int ret = PERIPHERAL_ERROR_NONE; + + if (ch > PCA9685_CH_MAX) { + _E("channel[%u] is out of range", ch); + return -1; + } + + if (ch_state[ch] == PCA9685_CH_STATE_USED) { + _E("channel[%u] is already in used state", ch); + return -1; + } + + if (g_i2c_h) + goto PASS_OPEN_HANDLE; + + ret = peripheral_i2c_open(RPI3_I2C_BUS, PCA9685_ADDRESS, &g_i2c_h); + if (ret != PERIPHERAL_ERROR_NONE) { + _E("failed to open pca9685-[bus:%d, addr:%d]", + RPI3_I2C_BUS, PCA9685_ADDRESS); + return -1; + } + ret = resource_pca9685_set_value_to_all(0, 0); + if (ret) { + _E("failed to reset all value to register"); + goto ERROR; + } + + ret = peripheral_i2c_write_register_byte(g_i2c_h, MODE2, OUTDRV); + if (ret != PERIPHERAL_ERROR_NONE) { + _E("failed to write register"); + goto ERROR; + } + + ret = peripheral_i2c_write_register_byte(g_i2c_h, MODE1, ALLCALL); + if (ret != PERIPHERAL_ERROR_NONE) { + _E("failed to write register"); + goto ERROR; + } + + usleep(500); // wait for oscillator + + ret = peripheral_i2c_read_register_byte(g_i2c_h, MODE1, &mode1); + if (ret != PERIPHERAL_ERROR_NONE) { + _E("failed to read register"); + goto ERROR; + } + + mode1 = mode1 & (~SLEEP); // # wake up (reset sleep) + ret = peripheral_i2c_write_register_byte(g_i2c_h, MODE1, mode1); + if (ret != PERIPHERAL_ERROR_NONE) { + _E("failed to write register"); + goto ERROR; + } + + usleep(500); // wait for oscillator + + ret = resource_pca9685_set_frequency(60); + if (ret) { + _E("failed to set frequency"); + goto ERROR; + } + +PASS_OPEN_HANDLE: + ref_count++; + ch_state[ch] = PCA9685_CH_STATE_USED; + _D("pca9685 - ref_count[%u]", ref_count); + _D("sets ch[%u] used state", ch); + + return 0; + +ERROR: + if (g_i2c_h) + peripheral_i2c_close(g_i2c_h); + + g_i2c_h = NULL; + return -1; +} + +int resource_pca9685_fini(unsigned int ch) +{ + if (ch_state[ch] == PCA9685_CH_STATE_NONE) { + _E("channel[%u] is not in used state", ch); + return -1; + } + resource_pca9685_set_value_to_channel(ch, 0, 0); + ch_state[ch] = PCA9685_CH_STATE_NONE; + + ref_count--; + _D("ref count - %u", ref_count); + + if (ref_count == 0 && g_i2c_h) { + _D("finalizing pca9685"); + resource_pca9685_set_value_to_all(0, 0); + peripheral_i2c_close(g_i2c_h); + g_i2c_h = NULL; + } + + return 0; +} diff --git a/src/resource/resource_motor_driver_L298N.c b/src/resource/resource_motor_driver_L298N.c new file mode 100644 index 0000000..291b74b --- /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(g_md_h[id].en_ch); + + 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(g_md_h[id].en_ch); + 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(g_md_h[id].en_ch); + + 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; +} diff --git a/src/resource/resource_servo_motor.c b/src/resource/resource_servo_motor.c new file mode 100644 index 0000000..e16df2e --- /dev/null +++ b/src/resource/resource_servo_motor.c @@ -0,0 +1,75 @@ +/* + * 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 "log.h" +#include "resource/resource_PCA9685.h" + +#define SERVO_MOTOR_MAX PCA9685_CH_MAX + +static int servo_motor_index[SERVO_MOTOR_MAX + 1] = {0, }; + +static int resource_servo_motor_init(unsigned int ch) +{ + int ret = 0; + ret = resource_pca9685_init(ch); + if (ret) { + _E("failed to init PCA9685 with ch[%u]", ch); + return -1; + } + servo_motor_index[ch] = 1; + + return 0; +} + +void resource_close_servo_motor(unsigned int ch) +{ + if (servo_motor_index[ch] == 1) { + resource_pca9685_fini(ch); + servo_motor_index[ch] = 0; + } + + return; +} + +void resource_close_servo_motor_all(void) +{ + unsigned int i; + + for (i = 0 ; i <= SERVO_MOTOR_MAX; i++) + resource_close_servo_motor(i); + + return; +} + +int resource_set_servo_motor_value(unsigned int motor_id, int value) +{ + int ret = 0; + + if (motor_id > SERVO_MOTOR_MAX) + return -1; + + if (servo_motor_index[motor_id] == 0) { + ret = resource_servo_motor_init(motor_id); + if (ret) + return -1; + } + + ret = resource_pca9685_set_value_to_channel(motor_id, 0, value); + + return ret; +}