New servo ranges

Change-Id: Ia588afc6f648a381beba0958376a3ccf50de9da2
Signed-off-by: Michal Skorupinski <m.skorupinsk@samsung.com>

diff --git a/src/app.c b/src/app.c
index 01d8c73..60c5c20 100644 (file)
--- a/src/app.c
+++ b/src/app.c
@@ -32,11 +32,22 @@
 #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,
@@ -82,7 +93,7 @@ static inline double __map_round(double val)
        return floor(val + 0.5);
 }
 
-static int __map_range_val(int d_max, int d_min, int v_max, int v_min, int val)
+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;
@@ -93,26 +104,14 @@ static int __map_range_val(int d_max, int d_min, int v_max, int v_min, int val)
        return rval;
 }
 
-static int ___map_speed_val(int speed)
+static inline 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);
+       return __map_range_val(-MAX_UDP_INPUT, MAX_UDP_INPUT, -4095, 4095, speed);
 }
 
-static int ___map_servo_val(int servo)
+static inline int ___map_servo_val(int servo)
 {
-       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);
+       return __map_range_val(-MAX_UDP_INPUT, MAX_UDP_INPUT, STERING_SERVO_CENTER - STERING_SERVO_RANGE, STERING_SERVO_CENTER + STERING_SERVO_RANGE, servo);
 }
 
 static int __driving_motors(int servo, int speed)
@@ -136,20 +135,13 @@ static int __driving_motors(int servo, int speed)
 
 static void __camera(int azimuth, int elevation)
 {
-       static const int camer_input_min = -1000;
-       static const int camer_input_max = 1000;
-       static const int camer_azimuth_min = 200;
-       static const int camer_azimuth_max = 700;
-       static const int camer_elevation_min = 200;
-       static const int camer_elevation_max = 400;
-
-       int val_azimuth = __map_range_val(camer_input_min, camer_input_max, camer_azimuth_min, camer_azimuth_max, azimuth);
-       int val_elevation = __map_range_val(0, camer_input_max, camer_elevation_min, camer_elevation_max, elevation); // No need to look upside down
+       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(14, val_elevation);
-       resource_set_servo_motor_value(15, val_azimuth);
+       resource_set_servo_motor_value(ELEVATION_SERVO_PIN, val_elevation);
+       resource_set_servo_motor_value(AZIMUTH_SERVO_PIN, val_azimuth);
 #endif
 }
 
@@ -247,6 +239,8 @@ static void service_app_control(app_control_h app_control, void *data)
        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;
@@ -256,6 +250,10 @@ static void service_app_terminate(void *data)
 {
        app_data *ad = data;
 
+       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);
+
        if (ad->idle_h)
                g_source_remove(ad->idle_h);