From: Eunyoung Lee Date: Fri, 8 Dec 2017 06:27:38 +0000 (+0900) Subject: Implement infrared obstacle avoidance sensor X-Git-Url: http://review.tizen.org/git/?p=apps%2Fnative%2Fgear-racing-car.git;a=commitdiff_plain;h=2eda8411162daeb1dd518bd4d3052de9b93f3f03 Implement infrared obstacle avoidance sensor --- diff --git a/CMakeLists.txt b/CMakeLists.txt index 73084d3..9c386a9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -32,6 +32,8 @@ ADD_EXECUTABLE(${PROJECT_NAME} ${PROJECT_ROOT_DIR}/src/dc_motor.c ${PROJECT_ROOT_DIR}/src/servo_motor.c ${PROJECT_ROOT_DIR}/src/pca9685.c + ${PROJECT_ROOT_DIR}/src/resource.c + ${PROJECT_ROOT_DIR}/src/resource/resource_infrared_obstacle_avoidance_sensor.c ) TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${pkgs_LDFLAGS} -lm) diff --git a/inc/resource.h b/inc/resource.h new file mode 100644 index 0000000..1e06636 --- /dev/null +++ b/inc/resource.h @@ -0,0 +1,30 @@ +/* + * Copyright (c) 2017 Samsung Electronics Co., Ltd. + * + * Contact: Jin Yoon + * Geunsun Lee + * Eunyoung Lee + * Junkyu Han + * + * 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 __POSITION_FINDER_RESOURCE_H__ +#define __POSITION_FINDER_RESOURCE_H__ + +#include + +#include "resource_internal.h" +#include "resource/resource_infrared_obstacle_avoidance_sensor.h" + +#endif /* __POSITION_FINDER_RESOURCE_H__ */ diff --git a/inc/resource/resource_infrared_obstacle_avoidance_sensor.h b/inc/resource/resource_infrared_obstacle_avoidance_sensor.h new file mode 100644 index 0000000..fdd7482 --- /dev/null +++ b/inc/resource/resource_infrared_obstacle_avoidance_sensor.h @@ -0,0 +1,39 @@ +/* + * Copyright (c) 2017 Samsung Electronics Co., Ltd. + * + * Contact: Jin Yoon + * Geunsun Lee + * Eunyoung Lee + * Junkyu Han + * + * 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 __POSITION_FINDER_RESOURCE_INFRARED_OBSTACLE_AVOIDANCE_SENSOR_H__ +#define __POSITION_FINDER_RESOURCE_INFRARED_OBSTACLE_AVOIDANCE_SENSOR_H__ + + +#include +#include "resource_internal.h" +/** + * @brief Reads the value of gpio connected infrared obstacle avoidance sensor. + * @param[in] pin_num The number of the gpio pin connected to the infrared obstacle avoidance sensor + * @param[out] out_value The value of the gpio (zero or non-zero) + * @return 0 on success, otherwise a negative error value + * @see If the gpio pin is not open, creates gpio handle before reading the value of gpio. + */ +extern int resource_read_infrared_obstacle_avoidance_sensor(int pin_num, uint32_t *out_value); + +extern int resource_set_infrared_obstacle_avoidance_sensor_interrupted_cb(int pin_num, resource_changed_cb cb, void *data); + +#endif /* __POSITION_FINDER_RESOURCE_INFRARED_OBSTACLE_AVOIDANCE_SENSOR_H__ */ diff --git a/inc/resource/resource_infrared_obstacle_avoidance_sensor_internal.h b/inc/resource/resource_infrared_obstacle_avoidance_sensor_internal.h new file mode 100644 index 0000000..f52cdf1 --- /dev/null +++ b/inc/resource/resource_infrared_obstacle_avoidance_sensor_internal.h @@ -0,0 +1,31 @@ +/* + * Copyright (c) 2017 Samsung Electronics Co., Ltd. + * + * Contact: Jin Yoon + * Geunsun Lee + * Eunyoung Lee + * Junkyu Han + * + * 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 __POSITION_FINDER_RESOURCE_INFRARED_OBSTACLE_AVOIDANCE_SENSOR_INTERNAL_H__ +#define __POSITION_FINDER_RESOURCE_INFRARED_OBSTACLE_AVOIDANCE_SENSOR_INTERNAL_H__ + +/** + * @brief Releases the gpio handle and changes the gpio pin state to the close(0). + * @param[in] pin_num The number of the gpio pin connected to the infrared obstacle avoidance sensor + */ +extern void resource_close_infrared_obstacle_avoidance_sensor(int pin_num); + +#endif /* __POSITION_FINDER_RESOURCE_INFRARED_OBSTACLE_AVOIDANCE_SENSOR_INTERNAL_H__ */ diff --git a/inc/resource_internal.h b/inc/resource_internal.h new file mode 100644 index 0000000..af694a6 --- /dev/null +++ b/inc/resource_internal.h @@ -0,0 +1,58 @@ +/* + * Copyright (c) 2017 Samsung Electronics Co., Ltd. + * + * Contact: Jin Yoon + * Geunsun Lee + * Eunyoung Lee + * Junkyu Han + * + * 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 __POSITION_FINDER_RESOURCE_INTERNAL_H__ +#define __POSITION_FINDER_RESOURCE_INTERNAL_H__ + +#include +#include + +#define PIN_MAX 40 + +typedef void (*resource_read_cb)(double value, void *data); + +typedef struct _resource_read_cb_s { + resource_read_cb cb; + void *data; + int pin_num; +} resource_read_s; + +typedef void (*resource_changed_cb)(unsigned int value, void *data); + +typedef struct _resource_changed_s { + resource_changed_cb cb; + void *data; + int pin_num; +} resource_changed_s; + +typedef struct _resource_s { + int opened; + peripheral_gpio_h sensor_h; + void (*close) (int); + /*FIXME*/ + resource_changed_s *resource_changed_info; +} resource_s; + + +extern resource_s *resource_get_info(int pin_num); +extern void resource_close_all(void); + +#endif /* __POSITION_FINDER_RESOURCE_INTERNAL_H__ */ diff --git a/src/app.c b/src/app.c index e548ca3..5783915 100644 --- a/src/app.c +++ b/src/app.c @@ -22,15 +22,22 @@ #include #include "log.h" #include "dc_motor.h" +#include "resource.h" + +enum { + DIR_STATE_S, + DIR_STATE_F, + DIR_STATE_B, +}; typedef struct app_data_s { - guint timer_id; + unsigned int f_value; + unsigned int r_value; + unsigned int dir_state; } app_data; -static void service_app_control(app_control_h app_control, void *data) -{ - return; -} +#define FRONT_PIN 21 +#define REAR_PIN 4 static void service_app_lang_changed(app_event_info_h event_info, void *user_data) { @@ -52,64 +59,113 @@ static void service_app_low_memory(app_event_info_h event_info, void *user_data) return; } -static inline int __get_r_val(int val) +static void ___________control_motor(app_data *ad) { - if (val > 0) - return 0 - val; - else - return ABS(val); + _D("control motor, state(%u), f_val(%u), r_val(%u)", + ad->dir_state, ad->f_value, ad->r_value); + + + switch (ad->dir_state) { + case DIR_STATE_F: + if (ad->f_value) { + if (ad->r_value) { + ad->dir_state = DIR_STATE_S; + dc_motor_speed_set(DC_MOTOR_ID_L, 0); + dc_motor_speed_set(DC_MOTOR_ID_R, 0); + } else { + ad->dir_state = DIR_STATE_B; + dc_motor_speed_set(DC_MOTOR_ID_L, -2000); + dc_motor_speed_set(DC_MOTOR_ID_R, -2000); + } + } + break; + case DIR_STATE_B: + if (ad->r_value) { + if (ad->f_value) { + ad->dir_state = DIR_STATE_S; + dc_motor_speed_set(DC_MOTOR_ID_L, 0); + dc_motor_speed_set(DC_MOTOR_ID_R, 0); + } else { + ad->dir_state = DIR_STATE_F; + dc_motor_speed_set(DC_MOTOR_ID_L, 2000); + dc_motor_speed_set(DC_MOTOR_ID_R, 2000); + } + } + break; + case DIR_STATE_S: + if (!ad->f_value) { + ad->dir_state = DIR_STATE_F; + dc_motor_speed_set(DC_MOTOR_ID_L, 2000); + dc_motor_speed_set(DC_MOTOR_ID_R, 2000); + } else if (!ad->r_value) { + ad->dir_state = DIR_STATE_B; + dc_motor_speed_set(DC_MOTOR_ID_L, -2000); + dc_motor_speed_set(DC_MOTOR_ID_R, -2000); + } + break; + } + + return; } -static gboolean __control_dcmotor_cb(gpointer user_data) +static void _front_ioa_sensor_changed_cb(unsigned int value, void *data) { - static int value = -2000; - int value2 = 0; + app_data *ad = data; + + _D("FRONT has obstacle!"); - value = __get_r_val(value); + ad->f_value = value; - dc_motor_speed_set(DC_MOTOR_ID_L, 0); - dc_motor_speed_set(DC_MOTOR_ID_R, 0); + ___________control_motor(ad); + + return; +} - sleep(1); +static void _back_ioa_sensor_changed_cb(unsigned int value, void *data) +{ + app_data *ad = data; - dc_motor_speed_set(DC_MOTOR_ID_L, value); - dc_motor_speed_set(DC_MOTOR_ID_R, value); + _D("BACK has obstacle!"); - sleep(5); - if (value > 0) - value2 = value + 1000; - else - value2 = value - 1000; + ad->r_value = value; - dc_motor_speed_set(DC_MOTOR_ID_L, value2); - dc_motor_speed_set(DC_MOTOR_ID_R, value2); + ___________control_motor(ad); - return G_SOURCE_CONTINUE; + return; } static bool service_app_create(void *data) { - app_data *ad = data; int ret = 0; + ret = dc_motor_init(); if (ret) { _E("failed init motor, terminating this application"); service_app_exit(); } - ad->timer_id = g_timeout_add_seconds(10, __control_dcmotor_cb, ad); - return true; } -static void service_app_terminate(void *data) +static void service_app_control(app_control_h app_control, void *data) { app_data *ad = data; + int ret; - if (ad->timer_id) { - g_source_remove(ad->timer_id); - ad->timer_id = 0; - } + resource_read_infrared_obstacle_avoidance_sensor(FRONT_PIN, &ad->f_value); + resource_read_infrared_obstacle_avoidance_sensor(REAR_PIN, &ad->r_value); + + resource_set_infrared_obstacle_avoidance_sensor_interrupted_cb(FRONT_PIN, _front_ioa_sensor_changed_cb, ad); + resource_set_infrared_obstacle_avoidance_sensor_interrupted_cb(REAR_PIN, _back_ioa_sensor_changed_cb, ad); + + ___________control_motor(ad); + + return; +} + +static void service_app_terminate(void *data) +{ + app_data *ad = data; dc_motor_fini(); log_file_close(); diff --git a/src/resource.c b/src/resource.c new file mode 100644 index 0000000..bd6c085 --- /dev/null +++ b/src/resource.c @@ -0,0 +1,44 @@ +/* + * Copyright (c) 2017 Samsung Electronics Co., Ltd. + * + * Contact: Jin Yoon + * Geunsun Lee + * Eunyoung Lee + * Junkyu Han + * + * 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 "log.h" +#include "resource.h" + +static resource_s resource_info[PIN_MAX] = { {0, NULL, NULL}, }; + +resource_s *resource_get_info(int pin_num) +{ + return &resource_info[pin_num]; +} + +void resource_close_all(void) +{ + int i = 0; + for (i = 0; i < PIN_MAX; i++) { + if (!resource_info[i].opened) continue; + _I("GPIO[%d] is closing...", i); + + if (resource_info[i].close) + resource_info[i].close(i); + } +} diff --git a/src/resource/resource_infrared_obstacle_avoidance_sensor.c b/src/resource/resource_infrared_obstacle_avoidance_sensor.c new file mode 100644 index 0000000..573ee76 --- /dev/null +++ b/src/resource/resource_infrared_obstacle_avoidance_sensor.c @@ -0,0 +1,147 @@ +/* + * Copyright (c) 2017 Samsung Electronics Co., Ltd. + * + * Contact: Jin Yoon + * Geunsun Lee + * Eunyoung Lee + * Junkyu Han + * + * 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_internal.h" + +void resource_close_infrared_obstacle_avoidance_sensor(int pin_num) +{ + if (!resource_get_info(pin_num)->opened) return; + + _I("Infrared Obstacle Avoidance Sensor is finishing..."); + if (resource_get_info(pin_num)->resource_changed_info) { + free(resource_get_info(pin_num)->resource_changed_info); + resource_get_info(pin_num)->resource_changed_info = NULL; + } + peripheral_gpio_unset_interrupted_cb(resource_get_info(pin_num)->sensor_h); + peripheral_gpio_close(resource_get_info(pin_num)->sensor_h); + resource_get_info(pin_num)->sensor_h = NULL; + resource_get_info(pin_num)->opened = 0; + + return; +} + +static int _init_pin(int pin_num) +{ + int ret = PERIPHERAL_ERROR_NONE; + + ret = peripheral_gpio_open(pin_num, &resource_get_info(pin_num)->sensor_h); + retv_if(ret != PERIPHERAL_ERROR_NONE, -1); + + ret = peripheral_gpio_set_direction(resource_get_info(pin_num)->sensor_h, PERIPHERAL_GPIO_DIRECTION_IN); + if (ret != PERIPHERAL_ERROR_NONE) { + peripheral_gpio_close(resource_get_info(pin_num)->sensor_h); + resource_get_info(pin_num)->sensor_h = NULL; + return -1; + } + + resource_get_info(pin_num)->opened = 1; + resource_get_info(pin_num)->close = resource_close_infrared_obstacle_avoidance_sensor; + + return 0; +} + +int resource_read_infrared_obstacle_avoidance_sensor(int pin_num, uint32_t *out_value) +{ + int ret = PERIPHERAL_ERROR_NONE; + + if (!resource_get_info(pin_num)->opened) { + ret = _init_pin(pin_num); + retv_if(ret < 0, -1); + } + + ret = peripheral_gpio_read(resource_get_info(pin_num)->sensor_h, out_value); + retv_if(ret != PERIPHERAL_ERROR_NONE, -1); + + *out_value = !*out_value; + + _I("Infrared Obstacle Avoidance Sensor Value : %d", *out_value); + + return 0; +} + + +static void __ioa_sensor_interrupt_cb(peripheral_gpio_h gpio, peripheral_error_e error, void *user_data) +{ + uint32_t value; + resource_changed_s *resource_changed_info = (resource_changed_s *)user_data; + + /* Detected : 0, Non-detected : 1 */ + peripheral_gpio_read(gpio, &value); + _D("interrupt value = %d", !value); + + resource_changed_info->cb(!value, resource_changed_info->data); + + return; +} + +int resource_set_infrared_obstacle_avoidance_sensor_interrupted_cb(int pin_num, resource_changed_cb cb, void *data) +{ + int ret = PERIPHERAL_ERROR_NONE; + + if (resource_get_info(pin_num)->resource_changed_info == NULL) { + resource_get_info(pin_num)->resource_changed_info = calloc(1, sizeof(resource_changed_s)); + retv_if(!resource_get_info(pin_num)->resource_changed_info, -1); + } else { + if (resource_get_info(pin_num)->sensor_h) { + peripheral_gpio_unset_interrupted_cb(resource_get_info(resource_get_info(pin_num)->resource_changed_info->pin_num)->sensor_h); + } + } + + resource_get_info(pin_num)->resource_changed_info->cb = cb; + resource_get_info(pin_num)->resource_changed_info->data = data; + resource_get_info(pin_num)->resource_changed_info->pin_num = pin_num; + + if (!resource_get_info(pin_num)->opened) { + ret = _init_pin(pin_num); + goto_if(ret < 0, FREE_RESOURCE); + } + + if (resource_get_info(pin_num)->sensor_h) { + ret = peripheral_gpio_set_edge_mode(resource_get_info(pin_num)->sensor_h, PERIPHERAL_GPIO_EDGE_BOTH); + goto_if(ret != PERIPHERAL_ERROR_NONE, FREE_RESOURCE); + + ret = peripheral_gpio_set_interrupted_cb(resource_get_info(pin_num)->sensor_h, __ioa_sensor_interrupt_cb, resource_get_info(pin_num)->resource_changed_info); + goto_if(ret != PERIPHERAL_ERROR_NONE, FREE_RESOURCE); + } + + return 0; + +FREE_RESOURCE: + if (resource_get_info(pin_num)->resource_changed_info) { + free(resource_get_info(pin_num)->resource_changed_info); + resource_get_info(pin_num)->resource_changed_info = NULL; + } + + if (resource_get_info(pin_num)->sensor_h) { + peripheral_gpio_unset_interrupted_cb(resource_get_info(pin_num)->sensor_h); + peripheral_gpio_close(resource_get_info(pin_num)->sensor_h); + resource_get_info(pin_num)->sensor_h = NULL; + resource_get_info(pin_num)->opened = 0; + } + + return -1; +}