*/
#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 "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_GRP_STEERING "Steering"
+#define CONFIG_GRP_CAMERA "Camera"
+#define CONFIG_GRP_ENGINE "Engine"
+#define CONFIG_GRP_RPI "Rpi"
+
+#define CONFIG_KEY_ID "Id"
+#define CONFIG_KEY_NAME "Name"
+
+#define CONFIG_KEY_STEERING_CENTER "Center"
+#define CONFIG_KEY_STEERING_RANGE "Range"
+#define CONFIG_KEY_CAMERA_AZIMUTH_CENTER "Azimuth.Center"
+#define CONFIG_KEY_CAMERA_AZIMUTH_RANGE "Azimuth.Range"
+#define CONFIG_KEY_CAMERA_ELEVATION_MIN "Elevation.Min"
+#define CONFIG_KEY_CAMERA_ELEVATION_MAX "Elevation.Max"
+#define CONFIG_KEY_CAMERA_ENGINE_MIN "Min"
+#define CONFIG_KEY_CAMERA_ENGINE_MAX "Max"
+
+#define CONFIG_KEY_RPI_PIN_STERING "Stering"
+#define CONFIG_KEY_RPI_PIN_CAMERA_AZIMUTH "Azimuth"
+#define CONFIG_KEY_RPI_PIN_CAMERA_ELEVATION "Elevation"
+
+#define CONFIG_KEY_RPI_PIN_ENGINE_1_FORWARD "1.forward"
+#define CONFIG_KEY_RPI_PIN_ENGINE_1_BACK "1.back"
+#define CONFIG_KEY_RPI_ENGINE_CHANNEL_1 "Channel.1"
+#define CONFIG_KEY_RPI_PIN_ENGINE_2_FORWARD "2.forward"
+#define CONFIG_KEY_RPI_PIN_ENGINE_2_BACK "2.back"
+#define CONFIG_KEY_RPI_ENGINE_CHANNEL_2 "Channel.2"
+
+#define CLOUD_REQUESTS_FREQUENCY 15
+
+#define ELEVATION_MIN 200
+#define ELEVATION_MAX 400
+#define AZIMUTH_MIN 200
+#define AZIMUTH_MAX 700
+
+#define CONFIG_LED_STATE_KEY_INIT "init"
+#define CONFIG_DEFAULT_LED_3BIT_INIT 1, 1, 0
+#define CONFIG_DEFAULT_LED_24BIT_INIT 255, 32, 0
+
+#define CONFIG_LED_STATE_KEY_OFF "off"
+#define CONFIG_DEFAULT_LED_3BIT_OFF 0, 0, 0
+#define CONFIG_DEFAULT_LED_24BIT_OFF 0, 0, 0
enum {
DIR_STATE_S,
unsigned int f_value;
unsigned int r_value;
unsigned int dir_state;
+ const char *user_name;
+ guint idle_h;
+
+ int stering_center;
+ int stering_range;
+
+ int camera_azimuth_center;
+ int camera_azimuth_range;
+
+ int camera_elevation_min;
+ int camera_elevation_max;
+
+ int engine_min;
+ int engine_max;
+
+ int stering_pin;
+
+ int elevation_pin;
+ int azimuth_pin;
+
+ int engine_1_forward_pin;
+ int engine_1_back_pin;
+ int engine_1_channel;
+
+ int engine_2_forward_pin;
+ int engine_2_back_pin;
+ int engine_2_channel;
} app_data;
-#define FRONT_PIN 21
-#define REAR_PIN 4
+static app_data s_info = {
+ .stering_center = STERING_SERVO_CENTER,
+ .stering_range = STERING_SERVO_RANGE,
+
+ .camera_azimuth_center = (AZIMUTH_MAX + AZIMUTH_MIN) /2,
+ .camera_azimuth_range = AZIMUTH_MAX - ((AZIMUTH_MAX + AZIMUTH_MIN) /2),
+
+ .camera_elevation_min = ELEVATION_MIN,
+ .camera_elevation_max = ELEVATION_MAX,
+
+ .engine_min = -4095,
+ .engine_max = 4095,
+
+ .stering_pin = 0,
+
+ .elevation_pin = 14,
+ .azimuth_pin = 15,
+
+ .engine_1_forward_pin = 6,
+ .engine_1_back_pin = 5,
+ .engine_1_channel = 5,
+
+ .engine_2_forward_pin = 21,
+ .engine_2_back_pin = 20,
+ .engine_2_channel = 4,
+};
+
+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 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 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)
+{
+ return __map_range_val(-MAX_UDP_INPUT, MAX_UDP_INPUT, s_info.engine_min, s_info.engine_max, speed);
+}
+
+static inline int ___map_servo_val(int servo)
+{
+ return __map_range_val(-MAX_UDP_INPUT, MAX_UDP_INPUT,
+ s_info.stering_center - s_info.stering_range,
+ s_info.stering_center + s_info.stering_range, 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;
}
-static void ___________control_motor(app_data *ad)
+static void __camera(int azimuth, int elevation)
{
- _D("control motor, state(%u), f_val(%u), r_val(%u)",
- ad->dir_state, ad->f_value, ad->r_value);
+ int val_azimuth = __map_range_val(-MAX_UDP_INPUT, MAX_UDP_INPUT,
+ s_info.camera_azimuth_center - s_info.camera_azimuth_range,
+ s_info.camera_azimuth_center + s_info.camera_azimuth_range, azimuth);
+ int val_elevation = __map_range_val(0, MAX_UDP_INPUT, s_info.camera_elevation_min, s_info.camera_elevation_max, elevation); // No need to look upside down
- 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);
- }
- }
+ _D("camera - azimuth[%4d : %4d], elevation[%4d : %4d]", azimuth, val_azimuth, elevation, val_elevation);
+#if ENABLE_MOTOR
+ resource_set_servo_motor_value(s_info.elevation_pin, val_elevation);
+ resource_set_servo_motor_value(s_info.azimuth_pin, val_azimuth);
+#endif
+}
+
+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 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);
- }
- }
+ 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 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);
- }
+ case COMMAND_TYPE_NONE:
+ break;
+ default:
+ _E("Unknown command type");
break;
}
+}
- return;
+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 _front_ioa_sensor_changed_cb(unsigned int value, void *data)
+static void _initialize_config()
{
- app_data *ad = data;
+ net_util_init();
- _D("FRONT has obstacle!");
+ config_init();
- ad->f_value = value;
+ char *id = NULL;
+ char *name = NULL;
+ gboolean modified = false;
- ___________control_motor(ad);
+ 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;
+ }
- return;
-}
+ char *uuid = g_uuid_string_random();
+ modified |= config_get_string_or_set_default(CONFIG_GRP_CAR, CONFIG_KEY_ID, uuid, &id);
+ g_free(uuid);
-static void _back_ioa_sensor_changed_cb(unsigned int value, void *data)
-{
- app_data *ad = data;
+ modified |= config_get_string_or_set_default(CONFIG_GRP_CAR, CONFIG_KEY_NAME, "Pink Car", &name);
- _D("BACK has obstacle!");
+ modified |= config_get_int_with_default(CONFIG_GRP_STEERING, CONFIG_KEY_STEERING_CENTER, s_info.stering_center, &s_info.stering_center);
+ modified |= config_get_int_with_default(CONFIG_GRP_STEERING, CONFIG_KEY_STEERING_RANGE, s_info.stering_range, &s_info.stering_range);
- ad->r_value = value;
+ modified |= config_get_int_with_default(CONFIG_GRP_CAMERA, CONFIG_KEY_CAMERA_AZIMUTH_CENTER, s_info.camera_azimuth_center, &s_info.camera_azimuth_center);
+ modified |= config_get_int_with_default(CONFIG_GRP_CAMERA, CONFIG_KEY_CAMERA_AZIMUTH_RANGE, s_info.camera_azimuth_range, &s_info.camera_azimuth_range);
+ modified |= config_get_int_with_default(CONFIG_GRP_CAMERA, CONFIG_KEY_CAMERA_ELEVATION_MIN, s_info.camera_elevation_min, &s_info.camera_elevation_min);
+ modified |= config_get_int_with_default(CONFIG_GRP_CAMERA, CONFIG_KEY_CAMERA_ELEVATION_MAX, s_info.camera_elevation_max, &s_info.camera_elevation_max);
- ___________control_motor(ad);
+ modified |= config_get_int_with_default(CONFIG_GRP_ENGINE, CONFIG_KEY_CAMERA_ENGINE_MIN, s_info.engine_min, &s_info.engine_min);
+ modified |= config_get_int_with_default(CONFIG_GRP_ENGINE, CONFIG_KEY_CAMERA_ENGINE_MAX, s_info.engine_max, &s_info.engine_max);
- return;
+ modified |= config_get_int_with_default(CONFIG_GRP_RPI, CONFIG_KEY_RPI_PIN_STERING, s_info.stering_pin, &s_info.stering_pin);
+ modified |= config_get_int_with_default(CONFIG_GRP_RPI, CONFIG_KEY_RPI_PIN_CAMERA_AZIMUTH, s_info.azimuth_pin, &s_info.azimuth_pin);
+ modified |= config_get_int_with_default(CONFIG_GRP_RPI, CONFIG_KEY_RPI_PIN_CAMERA_ELEVATION, s_info.elevation_pin, &s_info.elevation_pin);
+
+ modified |= config_get_int_with_default(CONFIG_GRP_RPI, CONFIG_KEY_RPI_PIN_ENGINE_1_FORWARD, s_info.engine_1_forward_pin, &s_info.engine_1_forward_pin);
+ modified |= config_get_int_with_default(CONFIG_GRP_RPI, CONFIG_KEY_RPI_PIN_ENGINE_1_BACK, s_info.engine_1_back_pin, &s_info.engine_1_back_pin);
+ modified |= config_get_int_with_default(CONFIG_GRP_RPI, CONFIG_KEY_RPI_ENGINE_CHANNEL_1, s_info.engine_1_channel, &s_info.engine_1_channel);
+
+ modified |= config_get_int_with_default(CONFIG_GRP_RPI, CONFIG_KEY_RPI_PIN_ENGINE_2_FORWARD, s_info.engine_2_forward_pin, &s_info.engine_2_forward_pin);
+ modified |= config_get_int_with_default(CONFIG_GRP_RPI, CONFIG_KEY_RPI_PIN_ENGINE_2_BACK, s_info.engine_2_back_pin, &s_info.engine_2_back_pin);
+ modified |= config_get_int_with_default(CONFIG_GRP_RPI, CONFIG_KEY_RPI_ENGINE_CHANNEL_2, s_info.engine_2_channel, &s_info.engine_2_channel);
+
+ if (modified == true) {
+ config_save();
+ }
+
+ free(id);
+ free(name);
+}
+
+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();
+ resource_led_init();
+
+
+ _D("Car settings: Stering[%d +/- %d] Engine[%d - %d], CamAzimuth[%d +/- %d], CamElev[%d - %d]",
+ s_info.stering_center, s_info.stering_range,
+ s_info.engine_min, s_info.engine_max,
+ s_info.camera_azimuth_center, s_info.camera_azimuth_range,
+ s_info.camera_elevation_min, s_info.camera_elevation_max);
+
+ _D("RPI: Stering[%d], Azimuth[%d], Elev[%d]; ENG_1[%d, %d, %d]; ENG_2[%d, %d, %d];",
+ s_info.stering_pin, s_info.azimuth_pin, s_info.elevation_pin,
+ s_info.engine_1_forward_pin, s_info.engine_1_back_pin, s_info.engine_1_channel,
+ s_info.engine_2_forward_pin, s_info.engine_2_back_pin, s_info.engine_2_channel);
}
static bool service_app_create(void *data)
{
int ret = 0;
+ 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(),
*
*/
- ret = resource_set_motor_driver_L298N_configuration(MOTOR_ID_1, 19, 16, 5);
+#if ENABLE_MOTOR
+ ret = resource_set_motor_driver_L298N_configuration(MOTOR_ID_1, s_info.engine_1_forward_pin, s_info.engine_1_back_pin, s_info.engine_1_channel);
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);
+ ret = resource_set_motor_driver_L298N_configuration(MOTOR_ID_2, s_info.engine_2_forward_pin, s_info.engine_2_back_pin, s_info.engine_2_channel);
if (ret) {
_E("resource_set_motor_driver_L298N_configuration()");
service_app_exit();
}
+#endif
+
+ _initialize_components(ad);
+ resource_lap_counter_init();
+ cloud_communication_start(CLOUD_REQUESTS_FREQUENCY);
+
+ controller_connection_manager_set_command_received_cb(__command_received_cb);
+ controller_connection_manager_set_user_name_received_cb(__user_name_received_cb);
+
+ resource_led_set_rgb_colors(CONFIG_LED_STATE_KEY_INIT,
+ CONFIG_DEFAULT_LED_3BIT_INIT,
+ CONFIG_DEFAULT_LED_24BIT_INIT,
+ LED_COLOR_RED);
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(s_info.stering_pin, STERING_SERVO_CENTER);
+ resource_set_servo_motor_value(s_info.elevation_pin, ELEVATION_MIN);
+ resource_set_servo_motor_value(s_info.azimuth_pin, (AZIMUTH_MIN + AZIMUTH_MAX) / 2);
+#endif
return;
}
static void service_app_terminate(void *data)
{
app_data *ad = data;
+ resource_led_set_rgb_colors(CONFIG_LED_STATE_KEY_OFF,
+ CONFIG_DEFAULT_LED_3BIT_OFF,
+ CONFIG_DEFAULT_LED_24BIT_OFF,
+ LED_COLOR_NONE);
+
+ resource_set_servo_motor_value(s_info.stering_pin, STERING_SERVO_CENTER);
+ resource_set_servo_motor_value(s_info.elevation_pin, ELEVATION_MIN);
+ resource_set_servo_motor_value(s_info.azimuth_pin, (AZIMUTH_MIN + AZIMUTH_MAX) / 2);
+
+ resource_lap_counter_destroy();
+ resource_led_destroy();
+ if (ad->idle_h)
+ g_source_remove(ad->idle_h);
+
+ 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