--- /dev/null
+/*
+ * 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__ */
--- /dev/null
+/*
+ * 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__ */
--- /dev/null
+/*
+ * 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__ */
--- /dev/null
+/*
+ * 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__ */
--- /dev/null
+/*
+ * 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__ */
--- /dev/null
+/*
+ * 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;
+}
--- /dev/null
+/*
+ * 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;
+}
--- /dev/null
+/*
+ * 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;
+}
projects / apps / native / position-finder-server.git / commitdiff