spheroncd: Throttle for speed scaling, refactoring.

ncd/examples/sphero/calculator.py

@@ -1,25 +1,26 @@
-from __future__ import print_function
 import sys
-import argparse
 import math
 
+def compute_rel_signed(val, minval, maxval):
+    return max(-1.0, min(1.0, (2.0 * ((val - minval) / (maxval - minval)) - 1.0)))
+
+def compute_rel_unsigned(val, minval, maxval):
+    return max(0.0, min(1.0, ((val - minval) / (maxval - minval))))
+
 def main():
-    parser = argparse.ArgumentParser()
-    args = parser.parse_args()
-    
     while True:
         comps = sys.stdin.readline().rstrip('\n').split(' ')
-        assert len(comps) == 11
         numbers = [float(x) for x in comps]
         
-        x_val, x_min, x_max, y_val, y_min, y_max, rz_val, rz_min, rz_max, turbo, adjust = numbers
+        x_val, x_min, x_max, y_val, y_min, y_max, rz_val, rz_min, rz_max, rz_degrees, throttle_pos, throttle_min, throttle_max, turbo, adjust = numbers
         
-        x_rel = 2.0 * ((x_val - x_min) / (x_max - x_min)) - 1.0
-        y_rel = 2.0 * ((y_val - y_min) / (y_max - y_min)) - 1.0
-        rz_rel = 2.0 * ((rz_val - rz_min) / (rz_max - rz_min)) - 1.0
+        x_rel  = compute_rel_signed(x_val, x_min, x_max)
+        y_rel  = compute_rel_signed(y_val, y_min, y_max)
+        rz_rel = compute_rel_signed(rz_val, rz_min, rz_max)
+        throttle_rel = compute_rel_unsigned(throttle_pos, throttle_min, throttle_max)
         
-        angle = math.atan2(y_rel, x_rel)
-        length = math.sqrt(x_rel**2 + y_rel**2)
+        angle = math.atan2(y_rel, x_rel) + (rz_rel * math.radians(rz_degrees))
+        length = math.sqrt(x_rel**2 + y_rel**2) * throttle_rel
         
         angle_fixed = (int(round(math.degrees(angle))) + 90) % 360
         speed_fixed = 0 if adjust else 255 if turbo else max(0, min(255, int(round(255.0 * length))))

ncd/examples/sphero/spheroncd.ncd

@@ -1,18 +1,30 @@
 process main {
-    # CONFIG
-    var("/dev/input/by-id/usb-Logitech_Logitech_Extreme_3D-event-joystick") joy_dev;
-    value(["X": "1023", "Y": "1023", "RZ": "255"]) joy_axis_ranges;
-    var("/dev/rfcomm0") sphero_dev;
-    var("5") max_sendq_len;
-    var("20") max_roll_interval;
-    var("/run/current-system/sw/bin/stty") stty;
-    var("/run/current-system/sw/bin/python2.7") python;
+    If (@true) {
+        var("/dev/input/by-id/usb-Logitech_Logitech_Extreme_3D-event-joystick") joy_dev;
+        value(["X": "0",    "Y": "0",    "RZ": "0",   "THROTTLE": "255"]) joy_axis_min;
+        value(["X": "1023", "Y": "1023", "RZ": "255", "THROTTLE": "0"  ]) joy_axis_max;
+        var("60") rz_degrees;
+        var("/dev/rfcomm0") sphero_dev;
+        var("5") max_sendq_len;
+        var("20") max_roll_interval;
+        var(@false) print_joy_events;
+        var("/run/current-system/sw/bin/stty") stty;
+        var("/run/current-system/sw/bin/python2.7") python;
+    } config;
+    
+    call(@_spheroncd, {^config});
+}
+
+template _spheroncd {
+    objref_arg(_arg0) config;
     
     value([]) joy_positions;
-    joy_positions->insert("BTN_TRIGGER", "0");
-    joy_positions->insert("BTN_THUMB", "0");
-    Foreach (joy_axis_ranges As axis_name: range) {
-        joy_positions->insert(@concat("ABS_", axis_name), @num_divide(range, "2"));
+    Do {
+        joy_positions->insert("BTN_TRIGGER", "0");
+        joy_positions->insert("BTN_THUMB", "0");
+        Foreach (config.joy_axis_min As axis_name: range) {
+            joy_positions->insert(@concat("ABS_", axis_name), @num_divide(range, "2"));
+        };
     };
     
     value([]) joy_latch_buttons;
@@ -33,9 +45,13 @@ process main {
 template _spheroncd_joystick_events {
     objref(^_caller.global) global;
     
-    log(@notice, "Listening on joystick ", global.joy_dev);
+    log(@notice, "Listening on joystick ", global.config.joy_dev);
     
-    sys.evdev(global.joy_dev) joy_event;
+    sys.evdev(global.config.joy_dev) joy_event;
+    
+    If (global.config.print_joy_events) {
+        println(joy_event.code, " ", joy_event.value);
+    };
     
     global.joy_positions->try_get(joy_event.code) positions_entry;
     If (positions_entry.exists) {
@@ -61,7 +77,7 @@ template _spheroncd_joystick_events {
 template _spheroncd_sphero {
     objref(^_caller.global) global;
     
-    log(@notice, "Using Sphero device ", global.sphero_dev);
+    log(@notice, "Using Sphero device ", global.config.sphero_dev);
     
     var(@true) first_time;
     backtrack_point() retry_connect;
@@ -70,13 +86,13 @@ template _spheroncd_sphero {
     };
     first_time->set(@false);
     
-    runonce({global.stty, "-F", global.sphero_dev, "115200", "raw", "-echo"}, ["term_on_deinit":@true, "keep_stderr":@true]) stty_exec;
+    runonce({global.config.stty, "-F", global.config.sphero_dev, "115200", "raw", "-echo"}, ["term_on_deinit":@true, "keep_stderr":@true]) stty_exec;
     If (@num_different(stty_exec.exit_status, "0")) {
         log(@error, "Failed to stty the Sphero device");
         retry_connect->go();
     };
     
-    sys.connect({@device, global.sphero_dev}) connection;
+    sys.connect({@device, global.config.sphero_dev}) connection;
     If (connection.is_error) {
         log(@error, "Sphero device error");
         retry_connect->go();
@@ -145,7 +161,7 @@ template _spheroncd_enqueue_packet {
     value(_arg6) data;
     
     Do {
-        If (@num_greater_equal(dev.send_queue.length, dev.global.max_sendq_len)) {
+        If (@num_greater_equal(dev.send_queue.length, dev.global.config.max_sendq_len)) {
             log(@warning, "send queue exhausted");
             _do->break();
         };
@@ -186,23 +202,35 @@ template _spheroncd_control_task {
     objref(^_caller.dev) dev;
     objref(^dev.global) global;
     
-    backtrack_point() again;
+    backtrack_point() control_again;
     
     global.joy_event_signal->down();
     
     global.joy_positions->get("ABS_X") pos_x;
     global.joy_positions->get("ABS_Y") pos_y;
     global.joy_positions->get("ABS_RZ") pos_rz;
+    global.joy_positions->get("ABS_THROTTLE") pos_throttle;
     global.joy_positions->get("BTN_TRIGGER") turbo;
     global.joy_positions->get("BTN_THUMB") adjust;
-    global.joy_axis_ranges->get("X") max_x;
-    global.joy_axis_ranges->get("Y") max_y;
-    global.joy_axis_ranges->get("RZ") max_rz;
+    global.config.joy_axis_min->get("X") min_x;
+    global.config.joy_axis_min->get("Y") min_y;
+    global.config.joy_axis_min->get("RZ") min_rz;
+    global.config.joy_axis_min->get("THROTTLE") min_throttle;
+    global.config.joy_axis_max->get("X") max_x;
+    global.config.joy_axis_max->get("Y") max_y;
+    global.config.joy_axis_max->get("RZ") max_rz;
+    global.config.joy_axis_max->get("THROTTLE") max_throttle;
     
     var(global.last_adjust) old_adjust;
     global.last_adjust->set(adjust);
     
-    var(@concat(pos_x, " ", "0", " ", max_x, " ", pos_y, " ", "0", " ", max_y, " ", pos_rz, " ", "0", " ", max_rz, " ", turbo, " ", adjust, "\n")) calc_request;
+    var(@concat(
+        pos_x,        " ", min_x,        " ", max_x,        " ",
+        pos_y,        " ", min_y,        " ", max_y,        " ",
+        pos_rz,       " ", min_rz,       " ", max_rz,       " ", global.config.rz_degrees, " ",
+        pos_throttle, " ", min_throttle, " ", max_throttle, " ",
+        turbo, " ", adjust, "\n"
+    )) calc_request;
     
     call(@_spheroncd_calc_operation, {^dev, calc_request}) calc_op;
     global.search_space->explode(calc_op.response) resp_fields;
@@ -212,15 +240,15 @@ template _spheroncd_control_task {
     resp_fields->get("1") calc_speed;
     
     If (@and(@num_different(old_adjust, "0"), @num_equal(adjust, "0"))) {
-        call(@_spheroncd_set_heading, {^dev, "0"});
+        call(@_spheroncd_set_heading, {^dev, calc_heading});
     };
     
     call(@_spheroncd_roll, {^dev, calc_speed, calc_heading, "1"});
     
-    sleep(global.max_roll_interval);
+    sleep(global.config.max_roll_interval);
     
     global.joy_event_signal->use();
-    again->go();
+    control_again->go();
 }
 
 template _spheroncd_calculator_task {
@@ -234,7 +262,7 @@ template _spheroncd_calculator_task {
     };
     first_time->set(@false);
     
-    var({global.python, "-B", "calculator.py"}) cmd;
+    var({global.config.python, "-B", "calculator.py"}) cmd;
     
     sys.start_process(cmd, "rw", ["keep_stderr": @true]) proc;
     If (proc.is_error) {