*/
#include <stdio.h>
+#include <stdlib.h>
#include <unistd.h>
+#include <math.h>
#include <glib.h>
#include <service_app.h>
#include "log.h"
-#include "dc_motor.h"
+#include "resource.h"
+#include "net-util.h"
+#include "config.h"
+#include "cloud/cloud_communication.h"
+#include "messages/message_manager.h"
+#include "controller_connection_manager.h"
+#include "lap_counter/lap_counter.h"
+#include "command.h"
+
+#define ENABLE_MOTOR 1
+#define STERING_SERVO_CENTER 340
+#define STERING_SERVO_RANGE 125
+
+#define MAX_UDP_INPUT 10000
+
+#define CONFIG_GRP_CAR "Car"
+#define CONFIG_KEY_ID "Id"
+#define CONFIG_KEY_NAME "Name"
+#define CLOUD_REQUESTS_FREQUENCY 15
+#define AZIMUTH_SERVO_PIN 15
+#define ELEVATION_SERVO_PIN 14
+
+#define ELEVATION_MIN 200
+#define ELEVATION_MAX 400
+#define AZIMUTH_MIN 200
+#define AZIMUTH_MAX 700
+
+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;
+ const char *user_name;
+ guint idle_h;
} app_data;
-static void service_app_control(app_control_h app_control, void *data)
-{
- return;
-}
+static void _initialize_components(app_data *ad);
+static void _initialize_config();
static void service_app_lang_changed(app_event_info_h event_info, void *user_data)
{
static void service_app_low_battery(app_event_info_h event_info, void *user_data)
{
+ _E("low battery! exit app");
+ service_app_exit();
+
return;
}
return;
}
-static inline int __get_r_val(int val)
+static inline double __map_round(double val)
+{
+ return floor(val + 0.5);
+}
+
+static int __map_range_val(int v_min, int v_max, int d_min, int d_max, int val)
+{
+ int rval = 0;
+ double slope = 0;
+ slope = 1.0 * (d_max - d_min) / (v_max - v_min);
+
+ rval = d_min + __map_round(slope * (val - v_min));
+
+ return rval;
+}
+
+static inline int ___map_speed_val(int speed)
{
- if (val > 0)
- return 0 - val;
- else
- return ABS(val);
+ return __map_range_val(-MAX_UDP_INPUT, MAX_UDP_INPUT, -4095, 4095, speed);
}
-static gboolean __control_dcmotor_cb(gpointer user_data)
+static inline int ___map_servo_val(int servo)
{
- static int value = -2000;
- int value2 = 0;
+ return __map_range_val(-MAX_UDP_INPUT, MAX_UDP_INPUT, STERING_SERVO_CENTER - STERING_SERVO_RANGE, STERING_SERVO_CENTER + STERING_SERVO_RANGE, servo);
+}
- value = __get_r_val(value);
+static int __driving_motors(int servo, int speed)
+{
+ int val_speed;
+ int val_servo;
- dc_motor_speed_set(DC_MOTOR_ID_L, 0);
- dc_motor_speed_set(DC_MOTOR_ID_R, 0);
+ val_servo = ___map_servo_val(servo);
+ val_speed = ___map_speed_val(speed);
- sleep(1);
+ _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
- dc_motor_speed_set(DC_MOTOR_ID_L, value);
- dc_motor_speed_set(DC_MOTOR_ID_R, value);
+ return 0;
+}
- sleep(5);
- if (value > 0)
- value2 = value + 1000;
- else
- value2 = value - 1000;
+static void __camera(int azimuth, int elevation)
+{
+ int val_azimuth = __map_range_val(-MAX_UDP_INPUT, MAX_UDP_INPUT, AZIMUTH_MIN, AZIMUTH_MAX, azimuth);
+ int val_elevation = __map_range_val(0, MAX_UDP_INPUT, ELEVATION_MIN, ELEVATION_MAX, elevation); // No need to look upside down
+
+ _D("camera - azimuth[%4d : %4d], elevation[%4d : %4d]", azimuth, val_azimuth, elevation, val_elevation);
+#if ENABLE_MOTOR
+ resource_set_servo_motor_value(ELEVATION_SERVO_PIN, val_elevation);
+ resource_set_servo_motor_value(AZIMUTH_SERVO_PIN, val_azimuth);
+#endif
+}
- dc_motor_speed_set(DC_MOTOR_ID_L, value2);
- dc_motor_speed_set(DC_MOTOR_ID_R, value2);
+static void __command_received_cb(command_s command) {
+ switch(command.type) {
+ case COMMAND_TYPE_DRIVE:
+ __driving_motors(command.data.steering.direction, command.data.steering.speed);
+ break;
+ case COMMAND_TYPE_CAMERA:
+ __camera(command.data.camera_position.camera_azimuth, command.data.camera_position.camera_elevation);
+ break;
+ case COMMAND_TYPE_DRIVE_AND_CAMERA:
+ __driving_motors(command.data.steering_and_camera.direction, command.data.steering_and_camera.speed);
+ __camera(command.data.steering_and_camera.camera_azimuth, command.data.steering_and_camera.camera_elevation);
+ break;
+ case COMMAND_TYPE_NONE:
+ break;
+ default:
+ _E("Unknown command type");
+ break;
+ }
+}
+
+static void __user_name_received_cb(const char *name)
+{
+ _D("User name received: %s", name);
+ lap_counter_set_user_name(name);
+ lap_counter_set_start_lap();
+}
+
+static void _initialize_config()
+{
+ net_util_init();
+
+ config_init();
+
+ char *id = NULL;
+ char *name = NULL;
+ gboolean modified = false;
+ if (config_get_string(CONFIG_GRP_CAR, CONFIG_KEY_ID, &id) != 0) {
+ char *uuid = g_uuid_string_random();
+ config_set_string(CONFIG_GRP_CAR, CONFIG_KEY_ID, uuid);
+ g_free(uuid);
+ modified = true;
+ }
+ if (config_get_string(CONFIG_GRP_CAR, CONFIG_KEY_NAME, &id) != 0) {
+ config_set_string(CONFIG_GRP_CAR, CONFIG_KEY_NAME, "Passerati");
+ modified = true;
+ }
+ if (modified == true) {
+ config_save();
+ }
+ free(id);
+ free(name);
+}
- return G_SOURCE_CONTINUE;
+static void _initialize_components(app_data *ad)
+{
+ net_util_init();
+ _initialize_config();
+ cloud_communication_init();
+ message_manager_init();
+ controller_connection_manager_listen();
+ lap_counter_init();
}
static bool service_app_create(void *data)
{
- app_data *ad = data;
int ret = 0;
- ret = dc_motor_init();
+ app_data *ad = data;
+
+ _D("-----------------------=======================================================================-----------------------");
+ _D("-----------------------=======================================================================-----------------------");
+ _D("-----------------------============================== APP %s %s ==============================-----------------------", __DATE__, __TIME__);
+ _D("-----------------------=======================================================================-----------------------");
+ _D("-----------------------=======================================================================-----------------------");
+
+ /*
+ * 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()");
+ service_app_exit();
+ }
+ ret = resource_set_motor_driver_L298N_configuration(MOTOR_ID_2, 26, 20, 4);
if (ret) {
- _E("failed init motor, terminating this application");
+ _E("resource_set_motor_driver_L298N_configuration()");
service_app_exit();
}
+#endif
+
+ resource_lap_counter_init();
+
+ _initialize_components(ad);
+ cloud_communication_start(CLOUD_REQUESTS_FREQUENCY);
- ad->timer_id = g_timeout_add_seconds(10, __control_dcmotor_cb, ad);
+ controller_connection_manager_set_command_received_cb(__command_received_cb);
+ controller_connection_manager_set_user_name_received_cb(__user_name_received_cb);
+ lap_counter_init();
return true;
}
+static void service_app_control(app_control_h app_control, void *data)
+{
+
+#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_set_servo_motor_value(0, 450);
+ resource_set_servo_motor_value(ELEVATION_SERVO_PIN, ELEVATION_MIN);
+ resource_set_servo_motor_value(AZIMUTH_SERVO_PIN, (AZIMUTH_MIN + AZIMUTH_MAX) / 2);
+#endif
+
+ return;
+}
+
static void service_app_terminate(void *data)
{
app_data *ad = data;
- if (ad->timer_id) {
- g_source_remove(ad->timer_id);
- ad->timer_id = 0;
- }
+ resource_set_servo_motor_value(0, STERING_SERVO_CENTER);
+ resource_set_servo_motor_value(ELEVATION_SERVO_PIN, ELEVATION_MIN);
+ resource_set_servo_motor_value(AZIMUTH_SERVO_PIN, (AZIMUTH_MIN + AZIMUTH_MAX) / 2);
+
+ resource_lap_counter_destroy();
+
+ if (ad->idle_h)
+ g_source_remove(ad->idle_h);
- dc_motor_fini();
+ lap_counter_shutdown();
+ controller_connection_manager_release();
+ message_manager_shutdown();
+
+ cloud_communication_stop();
+ cloud_communication_fini();
+ config_shutdown();
+ net_util_fini();
+
+ resource_close_all();
log_file_close();
_D("Bye ~");
return 0;
}
-
projects / apps / native / gear-racing-car.git / blobdiff