adds motor related modules

Change-Id: Ibfa78de491b4064b40224af125a0360034a0608b

8 files changed, 854 insertions, 0 deletions

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 <jh1979.park@samsung.com>
+ *
+ * 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 <jh1979.park@samsung.com>
+ *
+ * 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 <jh1979.park@samsung.com>
+ *
+ * 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 <jh1979.park@samsung.com>
+ *
+ * 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 <jh1979.park@samsung.com>
+ *
+ * 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 <jh1979.park@samsung.com>
+ *
+ * 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 <stdio.h>
+#include <unistd.h>
+#include <math.h>
+#include <peripheral_io.h>
+#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 <jh1979.park@samsung.com>
+ *
+ * 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 <stdlib.h>
+#include <peripheral_io.h>
+#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 <jh1979.park@samsung.com>
+ *
+ * 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;
+}