badvpn/ncd/examples/sphero/spheroncd.ncd

process main {
    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;
    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;
    
    blocker(@true) joy_event_signal;
    
    var("0") last_adjust;
    
    compile_search(" ") search_space;
    
    refhere() global;
    
    process_manager() mgr;
    mgr->start(@_spheroncd_joystick_events, {});
    mgr->start(@_spheroncd_sphero, {});
}

template _spheroncd_joystick_events {
    objref(^_caller.global) global;
    
    log(@notice, "Listening on joystick ", global.config.joy_dev);
    
    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) {
        global.joy_positions->replace(joy_event.code, joy_event.value);
        global.joy_event_signal->up();
        joy_event->nextevent();
    };
    
    global.joy_latch_buttons->try_get(joy_event.code) buttons_entry;
    If (buttons_entry.exists) {
        If (@val_equal(joy_event.value, "0")) {
            joy_event->nextevent();
        };
        println(joy_event.code);
        global.joy_latch_buttons->replace(joy_event.code, @true);
        global.joy_event_signal->up();
        joy_event->nextevent();
    };
    
    joy_event->nextevent();
}

template _spheroncd_sphero {
    objref(^_caller.global) global;
    
    log(@notice, "Using Sphero device ", global.config.sphero_dev);
    
    var(@true) first_time;
    backtrack_point() retry_connect;
    If (@not(first_time)) {
        sleep("5000");
    };
    first_time->set(@false);
    
    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.config.sphero_dev}) connection;
    If (connection.is_error) {
        log(@error, "Sphero device error");
        retry_connect->go();
    };
    
    log(@notice, "Opened Sphero device");
    
    value({}) send_queue;
    blocker(@true) send_event;
    
    depend_scope() depsc;
    
    refhere() dev;
    
    process_manager() mgr;
    mgr->start(@_spheroncd_read_task, {});
    mgr->start(@_spheroncd_send_task, {});
    mgr->start(@_spheroncd_ping_task, {});
    mgr->start(@_spheroncd_control_task, {});
    mgr->start(@_spheroncd_calculator_task, {});
    
    call(@_spheroncd_set_back_led, {^dev, "255"});
}

template _spheroncd_read_task {
    objref(^_caller.dev) dev;
    
    backtrack_point() read_again;
    
    dev.connection->read() recvd_data;
    If (recvd_data.eof) {
        log(@error, "Read EOF on Sphero device");
        dev.retry_connect->go();
    };
    
    read_again->go();
}

template _spheroncd_send_task {
    objref(^_caller.dev) dev;
    
    backtrack_point() next_message;
    
    Do {
        dev.send_event->use();
        If (@val_equal(dev.send_queue.length, "0")) {
            if(@false);
        };
    };
    
    dev.send_queue->get("0") message;
    dev.send_queue->remove("0");
    
    dev.connection->write(message);
    
    next_message->go();
}

template _spheroncd_enqueue_packet {
    objref_arg(_arg0) dev;
    alias(@_arg1) reset_timeout;
    alias(@_arg2) want_answer;
    alias(@_arg3) device_id;
    alias(@_arg4) command_id;
    alias(@_arg5) seq_num;
    value(_arg6) data;
    
    Do {
        If (@num_greater_equal(dev.send_queue.length, dev.global.config.max_sendq_len)) {
            log(@warning, "send queue exhausted");
            _do->break();
        };
        
        value("") message;
        
        message->append(@struct_encode({
            {@u8, "255"},
            {@u8, @num_add("252", @num_add(@if(reset_timeout, "2", "0"), @if(want_answer, "1", "0")))},
            {@u8, device_id},
            {@u8, command_id},
            {@u8, seq_num},
            {@u8, @num_add(data.length, "1")}
        }));
        
        message->append(data);
        
        message->substr("2") checksumed_data;
        message->append(@struct_encode({
            {@u8, @checksum(@inverted_sum_bytes, checksumed_data)}
        }));
        
        dev.send_queue->append(message);
        dev.send_event->downup();
    };
}

template _spheroncd_ping_task {
    objref(^_caller.dev) dev;
    
    backtrack_point() again;
    call(@_spheroncd_enqueue_packet, {^dev, @true, @true, "0", "1", "0", ""});
    sleep("2000");
    again->go();
}

template _spheroncd_control_task {
    objref(^_caller.dev) dev;
    objref(^dev.global) global;
    
    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.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,        " ", 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;
    value(resp_fields) resp_fields;
    
    resp_fields->get("0") calc_heading;
    resp_fields->get("1") calc_speed;
    
    If (@and(@num_different(old_adjust, "0"), @num_equal(adjust, "0"))) {
        call(@_spheroncd_set_heading, {^dev, calc_heading});
    };
    
    call(@_spheroncd_roll, {^dev, calc_speed, calc_heading, "1"});
    
    sleep(global.config.max_roll_interval);
    
    global.joy_event_signal->use();
    control_again->go();
}

template _spheroncd_calculator_task {
    objref(^_caller.dev) dev;
    objref(^dev.global) global;

    var(@true) first_time;
    backtrack_point() try_again;
    If (@not(first_time)) {
        sleep("1000");
    };
    first_time->set(@false);
    
    var({global.config.python, "-B", "calculator.py"}) cmd;
    
    sys.start_process(cmd, "rw", ["keep_stderr": @true]) proc;
    If (proc.is_error) {
        log(@error, "Process error");
        try_again->go();
    };
    
    proc->read_pipe() read_pipe;
    If (read_pipe.is_error) {
        log(@error, "Read pipe error");
        try_again->go();
    };
    
    proc->write_pipe() write_pipe;
    If (write_pipe.is_error) {
        log(@error, "Write pipe error");
        try_again->go();
    };
    
    dev.depsc->provide(@calculator);
}

template _spheroncd_calc_operation {
    objref_arg(_arg0) dev;
    alias(@_arg1) request;
    
    value("") response;
    
    Do {
        dev.depsc->depend({@calculator}) calc;
        value("") buffer;
        
        calc.write_pipe->write(request);
        
        backtrack_point() read_again;
        calc.read_pipe->read() read_data;
        If (@not(read_data.not_eof)) {
            log(@error, "Got EOF from calculator");
            calc.try_again->go();
        };
        buffer->append(read_data);
        var(@num_subtract(buffer.length, "1")) len_minus_one;
        buffer->substr(len_minus_one) last_char;
        If (@val_different(last_char, "\n")) {
            read_again->go();
        };
        
        buffer->substr("0", len_minus_one) without_newline;
        response->reset(without_newline);
    };
}

template _spheroncd_set_heading {
    objref_arg(_arg0) dev;
    alias(@_arg1) heading;
    
    call(@_spheroncd_enqueue_packet, {^dev, @true, @false, "2", "1", "0", @struct_encode({
        {@u16b, heading}
    })});
}

template _spheroncd_roll {
    objref_arg(_arg0) dev;
    alias(@_arg1) speed;
    alias(@_arg2) heading;
    alias(@_arg3) state;
    
    call(@_spheroncd_enqueue_packet, {^dev, @true, @false, "2", "48", "0", @struct_encode({
        {@u8, speed},
        {@u16b, heading},
        {@u8, state}
    })});
}

template _spheroncd_set_led_output {
    objref_arg(_arg0) dev;
    alias(@_arg1) red;
    alias(@_arg2) green;
    alias(@_arg3) blue;
    alias(@_arg4) persist;
    
    call(@_spheroncd_enqueue_packet, {^dev, @true, @false, "2", "32", "0", @struct_encode({
        {@u8, red},
        {@u8, green},
        {@u8, blue},
        {@u8, @if(persist, "1", "0")}
    })});
}

template _spheroncd_set_back_led {
    objref_arg(_arg0) dev;
    alias(@_arg1) bright;
    
    call(@_spheroncd_enqueue_packet, {^dev, @true, @false, "2", "33", "0", @struct_encode({
        {@u8, bright}
    })});
}