|
@@ -1,362 +0,0 @@
|
|
|
-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}
|
|
|
|
|
- })});
|
|
|
|
|
-}
|
|
|