applys message and connection manager modules

diff --git a/src/app.c b/src/app.c
index 0de042d..cec9f78 100644 (file)
--- a/src/app.c
+++ b/src/app.c
@@ -17,11 +17,18 @@
  */
 
 #include <stdio.h>
+#include <stdlib.h>
 #include <unistd.h>
+#include <math.h>
 #include <glib.h>
 #include <service_app.h>
 #include "log.h"
 #include "resource.h"
+#include "receiver.h"
+#include "message.h"
+#include "connection_manager.h"
+
+#define ENABLE_MOTOR 1
 
 enum {
        DIR_STATE_S,
@@ -33,11 +40,9 @@ typedef struct app_data_s {
        unsigned int f_value;
        unsigned int r_value;
        unsigned int dir_state;
+       guint idle_h;
 } app_data;
 
-#define FRONT_PIN 21
-#define REAR_PIN 4
-
 static void service_app_lang_changed(app_event_info_h event_info, void *user_data)
 {
        return;
@@ -50,6 +55,9 @@ static void service_app_region_changed(app_event_info_h event_info, void *user_d
 
 static void service_app_low_battery(app_event_info_h event_info, void *user_data)
 {
+       _E("low battery! exit app");
+       service_app_exit();
+
        return;
 }
 
@@ -58,107 +66,157 @@ static void service_app_low_memory(app_event_info_h event_info, void *user_data)
        return;
 }
 
-static int __servo_motor_test(void)
+static inline double __map_round(double val)
 {
-       resource_set_motor_driver_L298N_speed(MOTOR_ID_1, 0);
-       resource_set_motor_driver_L298N_speed(MOTOR_ID_2, 0);
-       resource_set_servo_motor_value(0, 400);
-       sleep(1);
-       resource_set_servo_motor_value(0, 500);
-       sleep(1);
-       resource_set_servo_motor_value(0, 450);
-       sleep(1);
-       return 0;
+       return floor(val + 0.5);
 }
 
-static void ___________control_motor(app_data *ad)
+static int __map_range_val(int d_max, int d_min, int v_max, int v_min, int val)
 {
-       _D("control motor, state(%u), f_val(%u), r_val(%u)",
-                       ad->dir_state, ad->f_value, ad->r_value);
+       int rval = 0;
+       double slope = 0;
+       slope = 1.0 * (d_max - d_min) / (v_max - v_min);
 
+       rval = d_min + round(slope * (val - v_min));
 
-       switch (ad->dir_state) {
-       case DIR_STATE_F:
-               if (ad->f_value)  {
-                       if (ad->r_value) {
-                               ad->dir_state = DIR_STATE_S;
-                               resource_set_motor_driver_L298N_speed(MOTOR_ID_1, 0);
-                               resource_set_motor_driver_L298N_speed(MOTOR_ID_2, 0);
-                       } else {
-                               ad->dir_state = DIR_STATE_B;
-                               __servo_motor_test();
-                               resource_set_motor_driver_L298N_speed(MOTOR_ID_1, -2000);
-                               resource_set_motor_driver_L298N_speed(MOTOR_ID_2, -2000);
-                       }
-               }
-               break;
-       case DIR_STATE_B:
-               if (ad->r_value)  {
-                       if (ad->f_value) {
-                               ad->dir_state = DIR_STATE_S;
-                               resource_set_motor_driver_L298N_speed(MOTOR_ID_1, 0);
-                               resource_set_motor_driver_L298N_speed(MOTOR_ID_2, 0);
-                       } else {
-                               ad->dir_state = DIR_STATE_F;
-                               __servo_motor_test();
-                               resource_set_motor_driver_L298N_speed(MOTOR_ID_1, 2000);
-                               resource_set_motor_driver_L298N_speed(MOTOR_ID_2, 2000);
-                       }
-               }
-               break;
-       case DIR_STATE_S:
-               if (!ad->f_value)  {
-                       ad->dir_state = DIR_STATE_F;
-                       __servo_motor_test();
-                       resource_set_motor_driver_L298N_speed(MOTOR_ID_1, 2000);
-                       resource_set_motor_driver_L298N_speed(MOTOR_ID_2, 2000);
-               } else if (!ad->r_value) {
-                       ad->dir_state = DIR_STATE_B;
-                       __servo_motor_test();
-                       resource_set_motor_driver_L298N_speed(MOTOR_ID_1, -2000);
-                       resource_set_motor_driver_L298N_speed(MOTOR_ID_2, -2000);
-               }
-               break;
-       }
+       return rval;
+}
 
-       return;
+static int ___map_speed_val(int speed)
+{
+       static const int motor_max = 4095;
+       static const int motor_min = -4095;
+       static const int speed_max = 1000;
+       static const int speed_min = -1000;
+
+       return __map_range_val(motor_max, motor_min,
+               speed_max, speed_min, speed);
 }
 
-static void _front_ioa_sensor_changed_cb(unsigned int value, void *data)
+static int ___map_servo_val(int servo)
 {
-       app_data *ad = data;
+       static const int motor_max = 500;
+       static const int motor_min = 400;
+       static const int servo_max = 1000;
+       static const int servo_min = -1000;
+
+       return __map_range_val(motor_max, motor_min,
+               servo_max, servo_min, servo);
+}
+
+
+static int __driving_motors(int servo, int speed)
+{
+       int val_speed;
+       int val_servo;
+
+       val_servo = ___map_servo_val(servo);
+       val_speed = ___map_speed_val(speed);
+
+       _D("control motor - servo[%4d : %4d], speed[%4d : %4d]",
+               servo, val_servo, speed, val_speed);
+#if ENABLE_MOTOR
+       resource_set_servo_motor_value(0, val_servo);
+       resource_set_motor_driver_L298N_speed(MOTOR_ID_1, val_speed);
+       resource_set_motor_driver_L298N_speed(MOTOR_ID_2, val_speed);
+#endif
+
+       return 0;
+}
 
-       _D("FRONT has obstacle!");
+static gboolean __message_dispatcher(gpointer user_data)
+{
+       message_s *msg = NULL;
+
+       do {
+               msg = message_pop_from_inqueue();
+               if (msg) {
+                       switch (msg->cmd) {
+                       case MESSAGE_CMD_HELLO:
+                               /* TODO : say hello to sender */
+                               break;
+                       case MESSAGE_CMD_CALIBRATION:
+                               /* TODO : set calibration mode */
+                               break;
+                       case MESSAGE_CMD_DRIVE:
+                       /* TODO : driving car */
+                               __driving_motors(msg->servo, msg->speed);
+                               break;
+                       case MESSAGE_CMD_BYE:
+                               __driving_motors(0, 0);
+                               break;
+                       }
+               }
+               free(msg);
+       } while (msg);
 
-       ad->f_value = value;
+       return TRUE;
+}
 
-       ___________control_motor(ad);
+static void __recv_state_change(receiver_type_e type,
+       receiver_state_e state, void* user_data)
+{
+       app_data *ad = user_data;
+       ret_if(!ad);
+
+       _D("receiver type[%d] state changed[%d]", type, state);
+
+       if (state == RECEIVER_STATE_CONNECTED) {
+               if (!ad->idle_h)
+                       ad->idle_h = g_idle_add(__message_dispatcher, ad);
+       } else {
+               if (ad->idle_h) {
+                       g_source_remove(ad->idle_h);
+                       ad->idle_h = 0;
+               }
+               __driving_motors(0, 0);
+       }
 
        return;
 }
 
-static void _back_ioa_sensor_changed_cb(unsigned int value, void *data)
+static void __conn_state_changed_cb(connection_state_e state,
+       const char *ip, void* user_data)
 {
-       app_data *ad = data;
+       app_data *ad = user_data;
 
-       _D("BACK has obstacle!");
+       _D("connection state changed : %d", state);
 
-       ad->r_value = value;
+       if (state == CONNECTION_STATE_CONNECTED) {
+               receiver_start(RECEIVER_TYPE_UDP);
 
-       ___________control_motor(ad);
+       } else {
+               receiver_stop(RECEIVER_TYPE_UDP);
 
+               if (ad->idle_h) {
+                       g_source_remove(ad->idle_h);
+                       ad->idle_h = 0;
+               }
+
+               __driving_motors(0, 0);
+       }
        return;
 }
 
 static bool service_app_create(void *data)
 {
        int ret = 0;
+       app_data *ad = data;
+
+       receiver_init(RECEIVER_TYPE_UDP);
+       receiver_set_state_changed_cb(RECEIVER_TYPE_UDP, __recv_state_change, ad);
+
+       connection_manager_init();
+       connection_manager_set_state_changed_cb(__conn_state_changed_cb, ad);
+
+       message_queue_new();
 
        /*
         * if you want to use default configuration,
         * Do not need to call resource_set_motor_driver_L298N_configuration(),
         *
        */
+#if ENABLE_MOTOR
        ret = resource_set_motor_driver_L298N_configuration(MOTOR_ID_1, 19, 16, 5);
        if (ret) {
                _E("resource_set_motor_driver_L298N_configuration()");
@@ -169,28 +227,20 @@ static bool service_app_create(void *data)
                _E("resource_set_motor_driver_L298N_configuration()");
                service_app_exit();
        }
+#endif
 
        return true;
 }
 
 static void service_app_control(app_control_h app_control, void *data)
 {
-       app_data *ad = data;
-       int ret;
 
+#if ENABLE_MOTOR
        /* set speed 0, to reduce delay of initializing motor driver */
        resource_set_motor_driver_L298N_speed(MOTOR_ID_1, 0);
        resource_set_motor_driver_L298N_speed(MOTOR_ID_2, 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);
+       resource_set_servo_motor_value(0, 450);
+#endif
 
        return;
 }
@@ -199,6 +249,14 @@ static void service_app_terminate(void *data)
 {
        app_data *ad = data;
 
+       if (ad->idle_h)
+               g_source_remove(ad->idle_h);
+
+
+       connection_manager_fini();
+       receiver_fini(RECEIVER_TYPE_UDP);
+
+       resource_close_all();
        log_file_close();
 
        _D("Bye ~");
@@ -237,4 +295,3 @@ int main(int argc, char* argv[])
 
        return 0;
 }
-