Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 88fdac751bdee68f38724e833e8091651cec68d6
|
From: Jérémie D. <Ba...@us...> - 2010-03-27 00:08:15
|
This is an automated email from the git hooks/post-receive script. It was
generated because a ref change was pushed to the repository containing
the project "krobot".
The branch, master has been updated
via 88fdac751bdee68f38724e833e8091651cec68d6 (commit)
from f46b6a4fb008c1e1abd85d728f02cd2c6dfe0deb (commit)
Those revisions listed above that are new to this repository have
not appeared on any other notification email; so we list those
revisions in full, below.
- Log -----------------------------------------------------------------
commit 88fdac751bdee68f38724e833e8091651cec68d6
Author: Jérémie Dimino <je...@di...>
Date: Sat Mar 27 01:07:25 2010 +0100
refactoring
-----------------------------------------------------------------------
Changes:
diff --git a/PC_Mainboard/_tags b/PC_Mainboard/_tags
index 0d628c0..6eaf178 100644
--- a/PC_Mainboard/_tags
+++ b/PC_Mainboard/_tags
@@ -23,9 +23,9 @@
<clients/**/*.ml>: syntax_camlp4o, pkg_lwt.syntax, pkg_lwt.syntax.log, pkg_obus.syntax
<clients/**>: pkg_lwt.unix, pkg_obus
-<clients/joy_control.*>: pkg_sdl
-<clients/controller.*>: pkg_lwt.text
-<clients/script*>: pkg_text
+<clients/krobot_joy_control.*>: pkg_sdl
+<clients/krobot_controller.*>: pkg_lwt.text
+<clients/krobot_script*>: pkg_text
# +------------------------------------------------------------------+
# | Services |
@@ -38,8 +38,8 @@
# | Common |
# +------------------------------------------------------------------+
-<common/{krobot_types,util}.{ml,mli}>: syntax_camlp4o, pkg_obus.syntax, pkg_lwt.syntax
-<common/{krobot_types,util}.*>: pkg_obus
+<common/krobot_{types,util}.{ml,mli}>: syntax_camlp4o, pkg_obus.syntax, pkg_lwt.syntax
+<common/krobot_{types,util}.*>: pkg_obus
# +------------------------------------------------------------------+
# | Driver |
diff --git a/PC_Mainboard/clients/controller.ml b/PC_Mainboard/clients/controller.ml
deleted file mode 100644
index bd8bba0..0000000
--- a/PC_Mainboard/clients/controller.ml
+++ /dev/null
@@ -1,447 +0,0 @@
-(*
- * controller.ml
- * -------------
- * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- *)
-
-(* Prints status continuously *)
-
-open Lwt
-open Lwt_term
-open Lwt_read_line
-
-module TextSet = Set.Make(Text)
-
-(* Maximum number of refresh by seconds: *)
-let refresh_rate = 10
-
-(* +-----------------------------------------------------------------+
- | Logging |
- +-----------------------------------------------------------------+ *)
-
-(* Maximum number of lines to keep in logs: *)
-let log_count = 16384
-
-(* Signal holding the current logs: *)
-let logs, set_logs = React.S.create []
-
-let add_date line =
- let buffer = Buffer.create 42 in
- Lwt_log.render ~buffer ~level:Lwt_log.Info ~message:"" ~template:"$(date): " ~section:Lwt_log.Section.main;
- text (Buffer.contents buffer) :: line
-
-(* Add a list of lines to logs *)
-let log_add_lines lines =
- let rec truncate n = function
- | [] ->
- []
- | line :: rest ->
- if n = log_count then
- []
- else
- line :: truncate (n + 1) rest
- in
- set_logs (truncate 0 (List.rev_map add_date lines @ (React.S.value logs)))
-
-let log_add_line line =
- log_add_lines [line]
-
-(* Redirect stderr to logs *)
-let redirect_stderr () =
- let rec copy_logs ic =
- lwt line = Lwt_io.read_line ic in
- log_add_line [text line];
- copy_logs ic
- in
- let fdr, fdw = Unix.pipe () in
- Unix.dup2 fdw Unix.stderr;
- Unix.close fdw;
- ignore (copy_logs (Lwt_io.of_unix_fd ~mode:Lwt_io.input fdr))
-
-(* Make the default logger to logs into the log buffer *)
-let init_logger () =
- Lwt_log.default :=
- Lwt_log.make
- ~output:(fun section level lines ->
- log_add_lines
- (List.map
- (fun line ->
- if level >= Lwt_log.Warning then
- (* Colorize error in red: *)
- [fg lred; text line]
- else
- [text line])
- lines);
- return ())
- ~close:return
-
-(* +-----------------------------------------------------------------+
- | Read-line |
- +-----------------------------------------------------------------+ *)
-
-let engine_state, set_engine_state = React.S.create (Engine.init [])
-let box, set_box = React.S.create Terminal.Box_empty
-
-let () =
- Lwt_signal.always_notify
- (function
- | Engine.Edition(before, after) ->
- let comp = Script.complete before after in
- set_box (Terminal.Box_words(comp.comp_words, 0))
- | Engine.Selection _ ->
- set_box (Terminal.Box_message "<selection>")
- | Engine.Search _ ->
- set_box (Terminal.Box_message "<backward search>"))
- (React.S.map (fun state -> state.Engine.mode) engine_state)
-
-let history_file_name =
- Filename.concat (try Unix.getenv "HOME" with _ -> "") ".krobot-controller-history"
-
-let save_history history =
- Lwt_read_line.save_history history_file_name history
-
-let rec loop krobot history =
- lwt key = read_key () in
- if key = key_escape then
- save_history history
- else
- match Command.of_key key with
- | Command.Accept_line ->
- let line = Text.strip (Engine.all_input (React.S.value engine_state)) in
- if line = "exit" then
- save_history history
- else if line <> "" then begin
- let history = Lwt_read_line.add_entry line history in
- set_engine_state (Engine.init history);
- lwt () = Lwt_log.notice line in
- ignore (Script.exec ~krobot ~logger:(fun line -> log_add_line line; return ()) ~command:line);
- loop krobot history
- end else
- loop krobot history
- | Command.Complete ->
- let engine_state = Engine.reset (React.S.value engine_state) in
- let before, after = Engine.edition_state engine_state in
- let comp = Script.complete before after in
- set_engine_state { engine_state with Engine.mode = Engine.Edition comp.comp_state };
- loop krobot history
- | command ->
- set_engine_state (Engine.update (React.S.value engine_state) command ());
- loop krobot history
-
-(* +-----------------------------------------------------------------+
- | Service monitoring |
- +-----------------------------------------------------------------+ *)
-
-let services, set_services = React.S.create ~eq:TextSet.equal TextSet.empty
-
-let check_services bus =
- lwt l = OBus_bus.list_names bus in
- set_services (List.fold_left (fun set name ->
- if Text.starts_with name "fr.krobot." then
- TextSet.add (String.sub name 10 (String.length name - 10)) set
- else
- set) TextSet.empty l);
- return ()
-
-(* +-----------------------------------------------------------------+
- | Drawing |
- +-----------------------------------------------------------------+ *)
-
-(* Thread currently redrawing the screen: *)
-let renderer = ref (return ())
-
-(* Draw the whole screen *)
-let rec draw krobot =
- let size = React.S.value Lwt_term.size in
-
- let screen = Zone.make ~width:size.columns ~height:size.lines in
- let points = Zone.points screen in
-
- let line_color = lblue in
- let line = { blank with style = { blank.style with foreground = line_color } } in
- let name_color = lwhite in
-
- (* ===== Borders ===== *)
-
- for i = 1 to size.columns - 2 do
- points.(0).(i) <- { line with char = "─" };
- points.(size.lines - 1).(i) <- { line with char = "─" }
- done;
- for i = 1 to size.lines - 2 do
- points.(i).(0) <- { line with char = "│" };
- points.(i).(size.columns - 1) <- { line with char = "│" }
- done;
- points.(0).(0) <- { line with char = "┌" };
- points.(size.lines - 1).(0) <- { line with char = "└" };
- points.(size.lines - 1).(size.columns - 1) <- { line with char = "┘" };
- points.(0).(size.columns - 1) <- { line with char = "┐" };
-
- (* ===== Status ===== *)
-
- Draw.textc screen 1 0 [fg line_color; text "─[ ";
- fg name_color; text "Range finders";
- fg line_color; text " ]─┬─[ ";
- fg name_color; text "Logic Sensors";
- fg line_color; text " ]─┬─[ ";
- fg name_color; text "Services";
- fg line_color; text " ]─┬─[ ";
- fg name_color; text "Cards";
- fg line_color; text " ]─┬─[ ";
- fg name_color; text "Status";
- fg line_color; text " ]─"];
- points.(9).(0) <- { line with char = "├" };
- points.(9).(size.columns - 1) <- { line with char = "┤" };
- for i = 1 to size.columns - 2 do
- points.(9).(i) <- { line with char = "─" }
- done;
- for i = 1 to 8 do
- points.(i).(20) <- { line with char = "│" };
- points.(i).(40) <- { line with char = "│" };
- points.(i).(55) <- { line with char = "│" };
- points.(i).(67) <- { line with char = "│" }
- done;
- Draw.textc screen 1 9 [fg line_color; text "───────────────────┴───────────────────┴──────────────┴───────────┴"];
-
- let zone = Zone.inner screen in
-
- let range_finders = React.S.value (Krobot.range_finders krobot) in
- for i = 0 to Array.length range_finders - 1 do
- Draw.textc zone 0 i [textf "%d : " i; text (Text.repeat (range_finders.(i) * 14 / 3146) "=")]
- done;
-
- let logic_sensors = React.S.value (Krobot.logic_sensors krobot) in
- for i = 0 to Array.length logic_sensors / 2 - 1 do
- let j = i * 2 in
- Draw.textf zone 20 i "%02d : %s %02d : %s"
- (j + 0) (if logic_sensors.(j + 0) then "O" else ".")
- (j + 1) (if logic_sensors.(j + 1) then "O" else ".")
- done;
-
- let zone' = Zone.sub ~zone ~x:40 ~y:0 ~width:14 ~height:8 in
- let rec loop y = function
- | [] ->
- ()
- | name :: rest ->
- Draw.text ~zone:zone' ~x:0 ~y ~text:name;
- loop (y + 1) rest
- in
- loop 0 (TextSet.elements (React.S.value services));
-
- let x = 55 in
- let text_of_state name = function
- | `Absent -> [fg lred; text name]
- | `Present -> [text name]
- in
- Draw.textc zone x 0 (text_of_state "interface" (React.S.value (Krobot.Card.state (krobot, `Interface))));
- Draw.textc zone x 1 (text_of_state "sensor" (React.S.value (Krobot.Card.state (krobot, `Sensor))));
- Draw.textc zone x 2 (text_of_state "motor" (React.S.value (Krobot.Card.state (krobot, `Motor))));
- Draw.textc zone x 3 (text_of_state "monitoring" (React.S.value (Krobot.Card.state (krobot, `Monitoring))));
- let x = x + 12 in
- Draw.textf zone x 0 "team = %s" (match React.S.value (Krobot.team krobot) with
- | Krobot.Team_red -> "red"
- | Krobot.Team_green -> "green");
- Draw.textf zone x 1 "jack = %s" (if React.S.value (Krobot.jack krobot) then "present" else "absent");
- Draw.textf zone x 2 "compass = %d" (React.S.value (Krobot.compass krobot));
- let date = Unix.gettimeofday () in
- let text_of_motor_state mode until =
- if date < until then
- [text mode; fg lyellow; text "inhibited"]
- else
- [text mode; text "OK"]
- in
- Draw.textc zone x 3 (text_of_motor_state "move forward: " (React.S.value (Krobot.inhibited_forward_until krobot)));
- Draw.textc zone x 4 (text_of_motor_state "move backward: " (React.S.value (Krobot.inhibited_backward_until krobot)));
-
- (* ===== History ===== *)
-
- let zone = Zone.sub ~zone:screen ~x:1 ~y:10 ~width:(Zone.width screen - 2) ~height:(Zone.height screen - 15) in
- let rec loop y = function
- | [] ->
- ()
- | line :: rest ->
- if y < 0 then
- ()
- else begin
- Draw.textc zone 0 y line;
- loop (y - 1) rest
- end
- in
- loop (Zone.height zone - 1) (React.S.value logs);
-
- (* ===== Read-line ===== *)
-
- points.(size.lines - 3).(0) <- { line with char = "├" };
- points.(size.lines - 3).(size.columns - 1) <- { line with char = "┤" };
- points.(size.lines - 5).(0) <- { line with char = "├" };
- points.(size.lines - 5).(size.columns - 1) <- { line with char = "┤" };
- for i = 1 to size.columns - 2 do
- points.(size.lines - 5).(i) <- { line with char = "─" };
- points.(size.lines - 3).(i) <- { line with char = "─" }
- done;
-
- let zone = Zone.sub ~zone:screen ~x:1 ~y:(size.lines - 4) ~width:(size.columns - 2) ~height:1 in
- let engine_state = React.S.value engine_state in
- let cursor_position =
- match engine_state.Engine.mode with
- | Engine.Edition(before, after) ->
- let len = Text.length before in
- Draw.textc zone 0 0 [Text before; Text after];
- len
- | Engine.Selection state ->
- let a = min state.Engine.sel_cursor state.Engine.sel_mark
- and b = max state.Engine.sel_cursor state.Engine.sel_mark in
- let part_before = Text.chunk (Text.pointer_l state.Engine.sel_text) a
- and part_selected = Text.chunk a b
- and part_after = Text.chunk (Text.pointer_r state.Engine.sel_text) b in
- Draw.textc zone 0 0 [Text part_before; Underlined; Text part_selected; Reset; Text part_after];
- if state.Engine.sel_cursor < state.Engine.sel_mark then
- Text.length part_before
- else
- Text.length part_before + Text.length part_selected
- | Engine.Search state ->
- let len = Text.length state.Engine.search_word in
- Draw.text zone 0 0 (Printf.sprintf "(reverse-i-search)'%s'" state.Engine.search_word);
- begin match state.Engine.search_history with
- | [] ->
- 19 + len
- | phrase :: _ ->
- let ptr_start = match Text.find phrase state.Engine.search_word with
- | Some ptr ->
- ptr
- | None ->
- assert false
- in
- let ptr_end = Text.move len ptr_start in
- let before = Text.chunk (Text.pointer_l phrase) ptr_start
- and selected = Text.chunk ptr_start ptr_end
- and after = Text.chunk ptr_end (Text.pointer_r phrase) in
- Draw.textc zone (20 + len) 0 [
- Text ": ";
- Text before;
- Underlined;
- Text selected;
- Reset;
- Text after;
- ];
- 19 + len
- end
- in
- Draw.map zone cursor_position 0 (fun point -> { point with style = { point.style with inverse = true } });
-
- let zone = Zone.sub ~zone:screen ~x:1 ~y:(size.lines - 3) ~width:(size.columns - 2) ~height:3 in
- begin
- match React.S.value box with
- | Terminal.Box_none | Terminal.Box_empty ->
- ()
- | Terminal.Box_message msg ->
- Draw.text zone 0 1 msg
- | Terminal.Box_words(words, _) ->
- ignore (TextSet.fold
- (fun word i ->
- let len = Text.length word in
- Draw.text zone i 1 word;
- let i = i + len in
- Draw.textc zone i 0 [fg line_color; text "┬"];
- Draw.textc zone i 1 [fg line_color; text "│"];
- Draw.textc zone i 2 [fg line_color; text "┴"];
- i + 1)
- words 0)
- end;
-
- Lwt.cancel !renderer;
- renderer := Lwt_term.render (Zone.points screen)
-
-(* Wether the screen need to be refreshed *)
-let refresh_needed = ref false
-
-(* Program a refresh before the next main loop iteration *)
-let refresh krobot =
- if !refresh_needed then
- return ()
- else begin
- refresh_needed := true;
- lwt () = Lwt.pause () in
- refresh_needed := false;
- draw krobot;
- return ()
- end
-
-(* +-----------------------------------------------------------------+
- | Entry point |
- +-----------------------------------------------------------------+ *)
-
-lwt () =
- lwt () = Lwt_log.notice "connecting to the krobot bus..." in
- lwt bus = Lazy.force Krobot.bus in
- lwt krobot = Krobot.create () in
-
- (* Put the terminal into drawing mode: *)
- lwt () = Lwt_term.enter_drawing_mode () in
- lwt () = Lwt_term.hide_cursor () in
-
- init_logger ();
- redirect_stderr ();
-
- (* Dump all logs to stdout on abnormal exit: *)
- let node =
- Lwt_sequence.add_l
- (fun () ->
- lwt () = Lwt_term.leave_drawing_mode () in
- Lwt_list.iter_s printlc (List.rev (React.S.value logs)))
- Lwt_main.exit_hooks
- in
-
- (* Service monitoring *)
- lwt () = check_services bus in
- Lwt_event.always_notify_p (fun _ -> check_services bus) (OBus_bus.name_owner_changed bus)#event;
-
- (* Minimum delay to wait between two screen redrawing *)
- let delay = 1.0 /. (float_of_int refresh_rate) in
- (* Event which refresh the screen when it occurs: *)
- let event, push = React.E.create () in
- Lwt_event.always_notify_p
- (fun () -> refresh krobot)
- (* Limit the number of redraw per seconds: *)
- (Lwt_event.limit (fun () -> Lwt_unix.sleep delay) event);
- (* Ask for a refresh when [signal] changes: *)
- let notify signal = Lwt_signal.always_notify (fun _ -> push ()) signal in
- notify box;
- notify logs;
- notify (Krobot.compass krobot);
- notify (Krobot.logic_sensors krobot);
- notify (Krobot.range_finders krobot);
- notify (Krobot.team krobot);
- notify (Krobot.jack krobot);
- notify (Krobot.inhibited_forward_until krobot);
- notify (Krobot.inhibited_backward_until krobot);
- notify (Krobot.Card.state (krobot, `Interface));
- notify (Krobot.Card.state (krobot, `Sensor));
- notify (Krobot.Card.state (krobot, `Motor));
- notify services;
-
- List.iter
- (fun card ->
- Lwt_event.always_notify
- (fun error -> log_add_line [fg lred; textf "error on card %s: %s" (Krobot.Card.name card) error])
- (Krobot.Card.errors (krobot, card)))
- [`Interface; `Sensor; `Motor; `Monitoring];
-
- (* Redraw immedlatly the screen when [signal] changes: *)
- let urgent signal = Lwt_signal.always_notify_p (fun _ -> refresh krobot) signal in
- urgent Lwt_term.size;
- urgent engine_state;
-
- lwt history = Lwt_read_line.load_history history_file_name in
- set_engine_state (Engine.init history);
-
- (* User input loop *)
- lwt () = Lwt_term.with_raw_mode (fun () -> loop krobot history) in
-
- (* Normal exit, do not dump logs on stdout: *)
- Lwt_sequence.remove node;
-
- (* Leave drawing mode *)
- Lwt_term.leave_drawing_mode ()
diff --git a/PC_Mainboard/clients/info.ml b/PC_Mainboard/clients/info.ml
deleted file mode 100644
index f2e8501..0000000
--- a/PC_Mainboard/clients/info.ml
+++ /dev/null
@@ -1,36 +0,0 @@
-(*
- * info.ml
- * -------
- * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- *)
-
-(* Print cards informations *)
-
-open Lwt
-open Lwt_io
-
-let print_card krobot card =
- match React.S.value (Krobot.Card.state (krobot, card)) with
- | `Present ->
- lwt firmware_build = Krobot.Card.get_firmware_build (krobot, card)
- and board_info = Krobot.Card.get_board_info (krobot, card) in
- let name = Krobot.Card.name card in
- lwt () = printl "==========" in
- lwt () = printlf "card.%s.state = present" name in
- lwt () = printlf "card.%s.firmware_build = %s" name firmware_build in
- lwt () = printlf "card.%s.board_info = %s" name board_info in
- return ()
- | `Absent ->
- lwt () = printl "==========" in
- printlf "card.%s.state = absent" (Krobot.Card.name card)
-
-lwt () =
- lwt krobot = Krobot.create () in
- lwt () = print_card krobot `Interface in
- lwt () = print_card krobot `Sensor in
- lwt () = print_card krobot `Motor in
- lwt () = print_card krobot `Monitoring in
- return ()
diff --git a/PC_Mainboard/clients/init_position.ml b/PC_Mainboard/clients/init_position.ml
deleted file mode 100644
index 85a69bd..0000000
--- a/PC_Mainboard/clients/init_position.ml
+++ /dev/null
@@ -1,41 +0,0 @@
-(*
- * init_position.ml
- * ----------------
- * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- *)
-
-(* Put the robot into its initial state *)
-
-open Lwt
-
-let move_backward_slowly krobot =
- lwt () = Lwt_log.notice "moving backward" in
- Krobot.move krobot ~distance:(-1000) ~velocity:100 ~acceleration:100 >>= function
- | `OK ->
- lwt () = Lwt_log.error "where am i ???" in
- exit 1
- | `Stopped ->
- Lwt_log.notice "backward colisiton dectected"
-
-lwt () =
- lwt krobot = Krobot.create () in
-
- lwt () = move_backward_slowly krobot in
-
- lwt () = Lwt_log.notice "going to initial position on first axis" in
- lwt _ = Krobot.move krobot ~distance:Krobot_config.initial_position ~velocity:400 ~acceleration:800 in
-
- lwt () = Lwt_log.notice "turning" in
- lwt _ = Krobot.turn krobot ~angle:(-90) ~velocity:400 ~acceleration:800 in
-
- lwt () = move_backward_slowly krobot in
-
- lwt () = Lwt_log.notice "going to initial position on second axis" in
- lwt _ = Krobot.move krobot ~distance:Krobot_config.initial_position ~velocity:400 ~acceleration:800 in
-
- lwt () = Lwt_log.notice "turning" in
- lwt _ = Krobot.turn krobot ~angle:45 ~velocity:400 ~acceleration:800 in
- return ()
diff --git a/PC_Mainboard/clients/joy_control.ml b/PC_Mainboard/clients/joy_control.ml
deleted file mode 100644
index 2247147..0000000
--- a/PC_Mainboard/clients/joy_control.ml
+++ /dev/null
@@ -1,226 +0,0 @@
-(*
- * joy_control.ml
- * --------------
- * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- *)
-
-(* Control the robot with a joystick *)
-
-open Lwt
-open Sdljoystick
-open Sdlevent
-open Sdlkey
-
-(* +-----------------------------------------------------------------+
- | Joystick events |
- +-----------------------------------------------------------------+ *)
-
-type button =
- | ButtonCross
- | ButtonSquare
- | ButtonTriangle
- | ButtonCircle
- | ButtonDown
- | ButtonLeft
- | ButtonUp
- | ButtonRight
- | ButtonSelect
- | ButtonStart
- | ButtonR1
- | ButtonR2
- | ButtonL1
- | ButtonL2
- | ButtonPS3
- | ButtonLAxis
- | ButtonRAxis
-
-type event =
- | JoyRAxisV of int
- | JoyLAxisV of int
- | JoyRAxisH of int
- | JoyLAxisH of int
- | JoyButtonPressed of button
- | JoyButtonReleased of button
- | KeyPressed of Sdlkey.t
- | KeyReleased of Sdlkey.t
-
-(* +-----------------------------------------------------------------+
- | int --> button |
- +-----------------------------------------------------------------+ *)
-
-let raxis_v = 3
-let raxis_h = 2
-let laxis_v = 1
-let laxis_h = 0
-
-let axis_min = -32768
-let axis_max = 32767
-
-let button_of_num = function
- | 14 -> ButtonCross
- | 15 -> ButtonSquare
- | 12 -> ButtonTriangle
- | 13 -> ButtonCircle
- | 6 -> ButtonDown
- | 7 -> ButtonLeft
- | 4 -> ButtonUp
- | 5 -> ButtonRight
- | 0 -> ButtonSelect
- | 3 -> ButtonStart
- | 11 -> ButtonR1
- | 9 -> ButtonR2
- | 10 -> ButtonL1
- | 8 -> ButtonL2
- | 16 -> ButtonPS3
- | 1 -> ButtonLAxis
- | 2 -> ButtonRAxis
- | n -> Printf.ksprintf failwith "unknown button %d" n
-
-(* +-----------------------------------------------------------------+
- | SDL events (executed in a child process) |
- +-----------------------------------------------------------------+ *)
-
-let child_loop pipe joy =
- let axis_state = Array.init (num_axes joy) (get_axis joy) in
- let send ev =
- Pervasives.output_value pipe ev;
- Pervasives.flush pipe
- in
- while true do
- match wait_event () with
- | KEYDOWN { keysym = key } ->
- send (KeyPressed key);
- if key = KEY_ESCAPE then begin
- Sdl.quit ();
- exit 0
- end
- | JOYAXISMOTION { jae_axis = axis; jae_value = value } ->
- let value = 100 - ((value - axis_min) * 200 / (axis_max - axis_min)) in
- if value <> axis_state.(axis) then begin
- axis_state.(axis) <- value;
- if axis = laxis_h then
- send (JoyLAxisH value)
- else if axis = laxis_v then
- send (JoyLAxisV value)
- else if axis = raxis_h then
- send (JoyRAxisH value)
- else if axis = raxis_v then
- send (JoyRAxisV value)
- else
- ()
- end
- | JOYBUTTONUP { jbe_button = button } ->
- send (JoyButtonPressed(button_of_num button))
- | JOYBUTTONDOWN { jbe_button = button } ->
- send (JoyButtonReleased(button_of_num button))
- | _ ->
- ()
- done
-
-(* +-----------------------------------------------------------------+
- | Handling events (in the parent process) |
- +-----------------------------------------------------------------+ *)
-
-let axis_coef = 6
-let axis_coef_turn = 4
-let accelerations = (800, 800)
-let duration = 0.2
-
-let try_call action f =
- try_lwt
- f ()
- with Failure msg ->
- lwt () = Lwt_log.error_f "action %s failed with: %s" action msg in
- return ()
-
-let rec set_velocities krobot velocities =
- lwt () = Lwt_log.info_f "set-velocities: left=%d right=%d" (fst velocities) (snd velocities) in
- lwt () =
- try_call "set-velocities"
- (fun () ->
- Krobot.set_velocities krobot ~velocities ~accelerations ~duration)
- in
- if velocities = (0, 0) then
- return ()
- else begin
- lwt () = Lwt_unix.sleep (duration /. 2.) in
- set_velocities krobot velocities
- end
-
-let parent_loop krobot pipe =
- let stop = ref false in
- let thread = ref (return ()) in
- let raxis_h = ref 0
- and raxis_v = ref 0
- and laxis_h = ref 0
- and laxis_v = ref 0 in
- let set_velocities () =
- cancel !thread;
- if not !stop then
- thread :=
- set_velocities krobot
- (!laxis_v * axis_coef - !raxis_h * axis_coef_turn,
- !laxis_v * axis_coef + !raxis_h * axis_coef_turn)
- in
- let rec loop () =
- Lwt_io.read_value pipe >>= function
- | KeyPressed KEY_ESCAPE ->
- return ()
- | JoyLAxisV n ->
- laxis_v := n;
- set_velocities ();
- loop ()
- | JoyLAxisH n ->
- laxis_h := n;
- set_velocities ();
- loop ()
- | JoyRAxisV n ->
- raxis_v := n;
- set_velocities ();
- loop ()
- | JoyRAxisH n ->
- raxis_h := n;
- set_velocities ();
- loop ()
- | JoyButtonPressed ButtonSquare ->
- stop := true;
- cancel !thread;
- lwt () = try_call "stop-motors" (fun () -> Krobot.stop_motors krobot ~mode:`Abrupt) in
- loop ()
- | JoyButtonReleased ButtonSquare ->
- stop := false;
- loop ()
- | _ ->
- loop ()
- in
- loop ()
-
-(* +-----------------------------------------------------------------+
- | Entry-point |
- +-----------------------------------------------------------------+ *)
-
-let () =
- let fd_r, fd_w = Unix.pipe () in
- match Unix.fork () with
- | 0 ->
- Unix.close fd_r;
- Sdl.init [`JOYSTICK;`VIDEO];
- Sdljoystick.set_event_state true;
- let joy =
- try
- open_joystick 0
- with exn ->
- Printf.eprintf "cannot open joystick: %s\n%!" (Printexc.to_string exn);
- raise exn
- in
- child_loop (Unix.out_channel_of_descr fd_w) joy
- | pid ->
- Unix.close fd_w;
- Lwt_main.run begin
- lwt krobot = Krobot.create () in
- lwt () = Lwt_log.notice "ready to process event" in
- parent_loop krobot (Lwt_io.of_unix_fd ~mode:Lwt_io.input fd_r)
- end
diff --git a/PC_Mainboard/clients/krobot_controller.ml b/PC_Mainboard/clients/krobot_controller.ml
new file mode 100644
index 0000000..f40fcf8
--- /dev/null
+++ b/PC_Mainboard/clients/krobot_controller.ml
@@ -0,0 +1,447 @@
+(*
+ * krobot_controller.ml
+ * --------------------
+ * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(* Prints status continuously *)
+
+open Lwt
+open Lwt_term
+open Lwt_read_line
+
+module TextSet = Set.Make(Text)
+
+(* Maximum number of refresh by seconds: *)
+let refresh_rate = 10
+
+(* +-----------------------------------------------------------------+
+ | Logging |
+ +-----------------------------------------------------------------+ *)
+
+(* Maximum number of lines to keep in logs: *)
+let log_count = 16384
+
+(* Signal holding the current logs: *)
+let logs, set_logs = React.S.create []
+
+let add_date line =
+ let buffer = Buffer.create 42 in
+ Lwt_log.render ~buffer ~level:Lwt_log.Info ~message:"" ~template:"$(date): " ~section:Lwt_log.Section.main;
+ text (Buffer.contents buffer) :: line
+
+(* Add a list of lines to logs *)
+let log_add_lines lines =
+ let rec truncate n = function
+ | [] ->
+ []
+ | line :: rest ->
+ if n = log_count then
+ []
+ else
+ line :: truncate (n + 1) rest
+ in
+ set_logs (truncate 0 (List.rev_map add_date lines @ (React.S.value logs)))
+
+let log_add_line line =
+ log_add_lines [line]
+
+(* Redirect stderr to logs *)
+let redirect_stderr () =
+ let rec copy_logs ic =
+ lwt line = Lwt_io.read_line ic in
+ log_add_line [text line];
+ copy_logs ic
+ in
+ let fdr, fdw = Unix.pipe () in
+ Unix.dup2 fdw Unix.stderr;
+ Unix.close fdw;
+ ignore (copy_logs (Lwt_io.of_unix_fd ~mode:Lwt_io.input fdr))
+
+(* Make the default logger to logs into the log buffer *)
+let init_logger () =
+ Lwt_log.default :=
+ Lwt_log.make
+ ~output:(fun section level lines ->
+ log_add_lines
+ (List.map
+ (fun line ->
+ if level >= Lwt_log.Warning then
+ (* Colorize error in red: *)
+ [fg lred; text line]
+ else
+ [text line])
+ lines);
+ return ())
+ ~close:return
+
+(* +-----------------------------------------------------------------+
+ | Read-line |
+ +-----------------------------------------------------------------+ *)
+
+let engine_state, set_engine_state = React.S.create (Engine.init [])
+let box, set_box = React.S.create Terminal.Box_empty
+
+let () =
+ Lwt_signal.always_notify
+ (function
+ | Engine.Edition(before, after) ->
+ let comp = Krobot_script.complete before after in
+ set_box (Terminal.Box_words(comp.comp_words, 0))
+ | Engine.Selection _ ->
+ set_box (Terminal.Box_message "<selection>")
+ | Engine.Search _ ->
+ set_box (Terminal.Box_message "<backward search>"))
+ (React.S.map (fun state -> state.Engine.mode) engine_state)
+
+let history_file_name =
+ Filename.concat (try Unix.getenv "HOME" with _ -> "") ".krobot-controller-history"
+
+let save_history history =
+ Lwt_read_line.save_history history_file_name history
+
+let rec loop krobot history =
+ lwt key = read_key () in
+ if key = key_escape then
+ save_history history
+ else
+ match Command.of_key key with
+ | Command.Accept_line ->
+ let line = Text.strip (Engine.all_input (React.S.value engine_state)) in
+ if line = "exit" then
+ save_history history
+ else if line <> "" then begin
+ let history = Lwt_read_line.add_entry line history in
+ set_engine_state (Engine.init history);
+ lwt () = Lwt_log.notice line in
+ ignore (Krobot_script.exec ~krobot ~logger:(fun line -> log_add_line line; return ()) ~command:line);
+ loop krobot history
+ end else
+ loop krobot history
+ | Command.Complete ->
+ let engine_state = Engine.reset (React.S.value engine_state) in
+ let before, after = Engine.edition_state engine_state in
+ let comp = Krobot_script.complete before after in
+ set_engine_state { engine_state with Engine.mode = Engine.Edition comp.comp_state };
+ loop krobot history
+ | command ->
+ set_engine_state (Engine.update (React.S.value engine_state) command ());
+ loop krobot history
+
+(* +-----------------------------------------------------------------+
+ | Service monitoring |
+ +-----------------------------------------------------------------+ *)
+
+let services, set_services = React.S.create ~eq:TextSet.equal TextSet.empty
+
+let check_services bus =
+ lwt l = OBus_bus.list_names bus in
+ set_services (List.fold_left (fun set name ->
+ if Text.starts_with name "fr.krobot." then
+ TextSet.add (String.sub name 10 (String.length name - 10)) set
+ else
+ set) TextSet.empty l);
+ return ()
+
+(* +-----------------------------------------------------------------+
+ | Drawing |
+ +-----------------------------------------------------------------+ *)
+
+(* Thread currently redrawing the screen: *)
+let renderer = ref (return ())
+
+(* Draw the whole screen *)
+let rec draw krobot =
+ let size = React.S.value Lwt_term.size in
+
+ let screen = Zone.make ~width:size.columns ~height:size.lines in
+ let points = Zone.points screen in
+
+ let line_color = lblue in
+ let line = { blank with style = { blank.style with foreground = line_color } } in
+ let name_color = lwhite in
+
+ (* ===== Borders ===== *)
+
+ for i = 1 to size.columns - 2 do
+ points.(0).(i) <- { line with char = "─" };
+ points.(size.lines - 1).(i) <- { line with char = "─" }
+ done;
+ for i = 1 to size.lines - 2 do
+ points.(i).(0) <- { line with char = "│" };
+ points.(i).(size.columns - 1) <- { line with char = "│" }
+ done;
+ points.(0).(0) <- { line with char = "┌" };
+ points.(size.lines - 1).(0) <- { line with char = "└" };
+ points.(size.lines - 1).(size.columns - 1) <- { line with char = "┘" };
+ points.(0).(size.columns - 1) <- { line with char = "┐" };
+
+ (* ===== Status ===== *)
+
+ Draw.textc screen 1 0 [fg line_color; text "─[ ";
+ fg name_color; text "Range finders";
+ fg line_color; text " ]─┬─[ ";
+ fg name_color; text "Logic Sensors";
+ fg line_color; text " ]─┬─[ ";
+ fg name_color; text "Services";
+ fg line_color; text " ]─┬─[ ";
+ fg name_color; text "Cards";
+ fg line_color; text " ]─┬─[ ";
+ fg name_color; text "Status";
+ fg line_color; text " ]─"];
+ points.(9).(0) <- { line with char = "├" };
+ points.(9).(size.columns - 1) <- { line with char = "┤" };
+ for i = 1 to size.columns - 2 do
+ points.(9).(i) <- { line with char = "─" }
+ done;
+ for i = 1 to 8 do
+ points.(i).(20) <- { line with char = "│" };
+ points.(i).(40) <- { line with char = "│" };
+ points.(i).(55) <- { line with char = "│" };
+ points.(i).(67) <- { line with char = "│" }
+ done;
+ Draw.textc screen 1 9 [fg line_color; text "───────────────────┴───────────────────┴──────────────┴───────────┴"];
+
+ let zone = Zone.inner screen in
+
+ let range_finders = React.S.value (Krobot.range_finders krobot) in
+ for i = 0 to Array.length range_finders - 1 do
+ Draw.textc zone 0 i [textf "%d : " i; text (Text.repeat (range_finders.(i) * 14 / 3146) "=")]
+ done;
+
+ let logic_sensors = React.S.value (Krobot.logic_sensors krobot) in
+ for i = 0 to Array.length logic_sensors / 2 - 1 do
+ let j = i * 2 in
+ Draw.textf zone 20 i "%02d : %s %02d : %s"
+ (j + 0) (if logic_sensors.(j + 0) then "O" else ".")
+ (j + 1) (if logic_sensors.(j + 1) then "O" else ".")
+ done;
+
+ let zone' = Zone.sub ~zone ~x:40 ~y:0 ~width:14 ~height:8 in
+ let rec loop y = function
+ | [] ->
+ ()
+ | name :: rest ->
+ Draw.text ~zone:zone' ~x:0 ~y ~text:name;
+ loop (y + 1) rest
+ in
+ loop 0 (TextSet.elements (React.S.value services));
+
+ let x = 55 in
+ let text_of_state name = function
+ | `Absent -> [fg lred; text name]
+ | `Present -> [text name]
+ in
+ Draw.textc zone x 0 (text_of_state "interface" (React.S.value (Krobot.Card.state (krobot, `Interface))));
+ Draw.textc zone x 1 (text_of_state "sensor" (React.S.value (Krobot.Card.state (krobot, `Sensor))));
+ Draw.textc zone x 2 (text_of_state "motor" (React.S.value (Krobot.Card.state (krobot, `Motor))));
+ Draw.textc zone x 3 (text_of_state "monitoring" (React.S.value (Krobot.Card.state (krobot, `Monitoring))));
+ let x = x + 12 in
+ Draw.textf zone x 0 "team = %s" (match React.S.value (Krobot.team krobot) with
+ | Krobot.Team_red -> "red"
+ | Krobot.Team_green -> "green");
+ Draw.textf zone x 1 "jack = %s" (if React.S.value (Krobot.jack krobot) then "present" else "absent");
+ Draw.textf zone x 2 "compass = %d" (React.S.value (Krobot.compass krobot));
+ let date = Unix.gettimeofday () in
+ let text_of_motor_state mode until =
+ if date < until then
+ [text mode; fg lyellow; text "inhibited"]
+ else
+ [text mode; text "OK"]
+ in
+ Draw.textc zone x 3 (text_of_motor_state "move forward: " (React.S.value (Krobot.inhibited_forward_until krobot)));
+ Draw.textc zone x 4 (text_of_motor_state "move backward: " (React.S.value (Krobot.inhibited_backward_until krobot)));
+
+ (* ===== History ===== *)
+
+ let zone = Zone.sub ~zone:screen ~x:1 ~y:10 ~width:(Zone.width screen - 2) ~height:(Zone.height screen - 15) in
+ let rec loop y = function
+ | [] ->
+ ()
+ | line :: rest ->
+ if y < 0 then
+ ()
+ else begin
+ Draw.textc zone 0 y line;
+ loop (y - 1) rest
+ end
+ in
+ loop (Zone.height zone - 1) (React.S.value logs);
+
+ (* ===== Read-line ===== *)
+
+ points.(size.lines - 3).(0) <- { line with char = "├" };
+ points.(size.lines - 3).(size.columns - 1) <- { line with char = "┤" };
+ points.(size.lines - 5).(0) <- { line with char = "├" };
+ points.(size.lines - 5).(size.columns - 1) <- { line with char = "┤" };
+ for i = 1 to size.columns - 2 do
+ points.(size.lines - 5).(i) <- { line with char = "─" };
+ points.(size.lines - 3).(i) <- { line with char = "─" }
+ done;
+
+ let zone = Zone.sub ~zone:screen ~x:1 ~y:(size.lines - 4) ~width:(size.columns - 2) ~height:1 in
+ let engine_state = React.S.value engine_state in
+ let cursor_position =
+ match engine_state.Engine.mode with
+ | Engine.Edition(before, after) ->
+ let len = Text.length before in
+ Draw.textc zone 0 0 [Text before; Text after];
+ len
+ | Engine.Selection state ->
+ let a = min state.Engine.sel_cursor state.Engine.sel_mark
+ and b = max state.Engine.sel_cursor state.Engine.sel_mark in
+ let part_before = Text.chunk (Text.pointer_l state.Engine.sel_text) a
+ and part_selected = Text.chunk a b
+ and part_after = Text.chunk (Text.pointer_r state.Engine.sel_text) b in
+ Draw.textc zone 0 0 [Text part_before; Underlined; Text part_selected; Reset; Text part_after];
+ if state.Engine.sel_cursor < state.Engine.sel_mark then
+ Text.length part_before
+ else
+ Text.length part_before + Text.length part_selected
+ | Engine.Search state ->
+ let len = Text.length state.Engine.search_word in
+ Draw.text zone 0 0 (Printf.sprintf "(reverse-i-search)'%s'" state.Engine.search_word);
+ begin match state.Engine.search_history with
+ | [] ->
+ 19 + len
+ | phrase :: _ ->
+ let ptr_start = match Text.find phrase state.Engine.search_word with
+ | Some ptr ->
+ ptr
+ | None ->
+ assert false
+ in
+ let ptr_end = Text.move len ptr_start in
+ let before = Text.chunk (Text.pointer_l phrase) ptr_start
+ and selected = Text.chunk ptr_start ptr_end
+ and after = Text.chunk ptr_end (Text.pointer_r phrase) in
+ Draw.textc zone (20 + len) 0 [
+ Text ": ";
+ Text before;
+ Underlined;
+ Text selected;
+ Reset;
+ Text after;
+ ];
+ 19 + len
+ end
+ in
+ Draw.map zone cursor_position 0 (fun point -> { point with style = { point.style with inverse = true } });
+
+ let zone = Zone.sub ~zone:screen ~x:1 ~y:(size.lines - 3) ~width:(size.columns - 2) ~height:3 in
+ begin
+ match React.S.value box with
+ | Terminal.Box_none | Terminal.Box_empty ->
+ ()
+ | Terminal.Box_message msg ->
+ Draw.text zone 0 1 msg
+ | Terminal.Box_words(words, _) ->
+ ignore (TextSet.fold
+ (fun word i ->
+ let len = Text.length word in
+ Draw.text zone i 1 word;
+ let i = i + len in
+ Draw.textc zone i 0 [fg line_color; text "┬"];
+ Draw.textc zone i 1 [fg line_color; text "│"];
+ Draw.textc zone i 2 [fg line_color; text "┴"];
+ i + 1)
+ words 0)
+ end;
+
+ Lwt.cancel !renderer;
+ renderer := Lwt_term.render (Zone.points screen)
+
+(* Wether the screen need to be refreshed *)
+let refresh_needed = ref false
+
+(* Program a refresh before the next main loop iteration *)
+let refresh krobot =
+ if !refresh_needed then
+ return ()
+ else begin
+ refresh_needed := true;
+ lwt () = Lwt.pause () in
+ refresh_needed := false;
+ draw krobot;
+ return ()
+ end
+
+(* +-----------------------------------------------------------------+
+ | Entry point |
+ +-----------------------------------------------------------------+ *)
+
+lwt () =
+ lwt () = Lwt_log.notice "connecting to the krobot bus..." in
+ lwt bus = Lazy.force Krobot.bus in
+ lwt krobot = Krobot.create () in
+
+ (* Put the terminal into drawing mode: *)
+ lwt () = Lwt_term.enter_drawing_mode () in
+ lwt () = Lwt_term.hide_cursor () in
+
+ init_logger ();
+ redirect_stderr ();
+
+ (* Dump all logs to stdout on abnormal exit: *)
+ let node =
+ Lwt_sequence.add_l
+ (fun () ->
+ lwt () = Lwt_term.leave_drawing_mode () in
+ Lwt_list.iter_s printlc (List.rev (React.S.value logs)))
+ Lwt_main.exit_hooks
+ in
+
+ (* Service monitoring *)
+ lwt () = check_services bus in
+ Lwt_event.always_notify_p (fun _ -> check_services bus) (OBus_bus.name_owner_changed bus)#event;
+
+ (* Minimum delay to wait between two screen redrawing *)
+ let delay = 1.0 /. (float_of_int refresh_rate) in
+ (* Event which refresh the screen when it occurs: *)
+ let event, push = React.E.create () in
+ Lwt_event.always_notify_p
+ (fun () -> refresh krobot)
+ (* Limit the number of redraw per seconds: *)
+ (Lwt_event.limit (fun () -> Lwt_unix.sleep delay) event);
+ (* Ask for a refresh when [signal] changes: *)
+ let notify signal = Lwt_signal.always_notify (fun _ -> push ()) signal in
+ notify box;
+ notify logs;
+ notify (Krobot.compass krobot);
+ notify (Krobot.logic_sensors krobot);
+ notify (Krobot.range_finders krobot);
+ notify (Krobot.team krobot);
+ notify (Krobot.jack krobot);
+ notify (Krobot.inhibited_forward_until krobot);
+ notify (Krobot.inhibited_backward_until krobot);
+ notify (Krobot.Card.state (krobot, `Interface));
+ notify (Krobot.Card.state (krobot, `Sensor));
+ notify (Krobot.Card.state (krobot, `Motor));
+ notify services;
+
+ List.iter
+ (fun card ->
+ Lwt_event.always_notify
+ (fun error -> log_add_line [fg lred; textf "error on card %s: %s" (Krobot.Card.name card) error])
+ (Krobot.Card.errors (krobot, card)))
+ [`Interface; `Sensor; `Motor; `Monitoring];
+
+ (* Redraw immedlatly the screen when [signal] changes: *)
+ let urgent signal = Lwt_signal.always_notify_p (fun _ -> refresh krobot) signal in
+ urgent Lwt_term.size;
+ urgent engine_state;
+
+ lwt history = Lwt_read_line.load_history history_file_name in
+ set_engine_state (Engine.init history);
+
+ (* User input loop *)
+ lwt () = Lwt_term.with_raw_mode (fun () -> loop krobot history) in
+
+ (* Normal exit, do not dump logs on stdout: *)
+ Lwt_sequence.remove node;
+
+ (* Leave drawing mode *)
+ Lwt_term.leave_drawing_mode ()
diff --git a/PC_Mainboard/clients/krobot_info.ml b/PC_Mainboard/clients/krobot_info.ml
new file mode 100644
index 0000000..b0906dc
--- /dev/null
+++ b/PC_Mainboard/clients/krobot_info.ml
@@ -0,0 +1,36 @@
+(*
+ * krobot_info.ml
+ * --------------
+ * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(* Print cards informations *)
+
+open Lwt
+open Lwt_io
+
+let print_card krobot card =
+ match React.S.value (Krobot.Card.state (krobot, card)) with
+ | `Present ->
+ lwt firmware_build = Krobot.Card.get_firmware_build (krobot, card)
+ and board_info = Krobot.Card.get_board_info (krobot, card) in
+ let name = Krobot.Card.name card in
+ lwt () = printl "==========" in
+ lwt () = printlf "card.%s.state = present" name in
+ lwt () = printlf "card.%s.firmware_build = %s" name firmware_build in
+ lwt () = printlf "card.%s.board_info = %s" name board_info in
+ return ()
+ | `Absent ->
+ lwt () = printl "==========" in
+ printlf "card.%s.state = absent" (Krobot.Card.name card)
+
+lwt () =
+ lwt krobot = Krobot.create () in
+ lwt () = print_card krobot `Interface in
+ lwt () = print_card krobot `Sensor in
+ lwt () = print_card krobot `Motor in
+ lwt () = print_card krobot `Monitoring in
+ return ()
diff --git a/PC_Mainboard/clients/krobot_init_position.ml b/PC_Mainboard/clients/krobot_init_position.ml
new file mode 100644
index 0000000..9343f77
--- /dev/null
+++ b/PC_Mainboard/clients/krobot_init_position.ml
@@ -0,0 +1,41 @@
+(*
+ * krobot_init_position.ml
+ * -----------------------
+ * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(* Put the robot into its initial state *)
+
+open Lwt
+
+let move_backward_slowly krobot =
+ lwt () = Lwt_log.notice "moving backward" in
+ Krobot.move krobot ~distance:(-1000) ~velocity:100 ~acceleration:100 >>= function
+ | `OK ->
+ lwt () = Lwt_log.error "where am i ???" in
+ exit 1
+ | `Stopped ->
+ Lwt_log.notice "backward colisiton dectected"
+
+lwt () =
+ lwt krobot = Krobot.create () in
+
+ lwt () = move_backward_slowly krobot in
+
+ lwt () = Lwt_log.notice "going to initial position on first axis" in
+ lwt _ = Krobot.move krobot ~distance:Krobot_config.initial_position ~velocity:400 ~acceleration:800 in
+
+ lwt () = Lwt_log.notice "turning" in
+ lwt _ = Krobot.turn krobot ~angle:(-90) ~velocity:400 ~acceleration:800 in
+
+ lwt () = move_backward_slowly krobot in
+
+ lwt () = Lwt_log.notice "going to initial position on second axis" in
+ lwt _ = Krobot.move krobot ~distance:Krobot_config.initial_position ~velocity:400 ~acceleration:800 in
+
+ lwt () = Lwt_log.notice "turning" in
+ lwt _ = Krobot.turn krobot ~angle:45 ~velocity:400 ~acceleration:800 in
+ return ()
diff --git a/PC_Mainboard/clients/krobot_joy_control.ml b/PC_Mainboard/clients/krobot_joy_control.ml
new file mode 100644
index 0000000..cfdd224
--- /dev/null
+++ b/PC_Mainboard/clients/krobot_joy_control.ml
@@ -0,0 +1,226 @@
+(*
+ * krobot_joy_control.ml
+ * ---------------------
+ * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(* Control the robot with a joystick *)
+
+open Lwt
+open Sdljoystick
+open Sdlevent
+open Sdlkey
+
+(* +-----------------------------------------------------------------+
+ | Joystick events |
+ +-----------------------------------------------------------------+ *)
+
+type button =
+ | ButtonCross
+ | ButtonSquare
+ | ButtonTriangle
+ | ButtonCircle
+ | ButtonDown
+ | ButtonLeft
+ | ButtonUp
+ | ButtonRight
+ | ButtonSelect
+ | ButtonStart
+ | ButtonR1
+ | ButtonR2
+ | ButtonL1
+ | ButtonL2
+ | ButtonPS3
+ | ButtonLAxis
+ | ButtonRAxis
+
+type event =
+ | JoyRAxisV of int
+ | JoyLAxisV of int
+ | JoyRAxisH of int
+ | JoyLAxisH of int
+ | JoyButtonPressed of button
+ | JoyButtonReleased of button
+ | KeyPressed of Sdlkey.t
+ | KeyReleased of Sdlkey.t
+
+(* +-----------------------------------------------------------------+
+ | int --> button |
+ +-----------------------------------------------------------------+ *)
+
+let raxis_v = 3
+let raxis_h = 2
+let laxis_v = 1
+let laxis_h = 0
+
+let axis_min = -32768
+let axis_max = 32767
+
+let button_of_num = function
+ | 14 -> ButtonCross
+ | 15 -> ButtonSquare
+ | 12 -> ButtonTriangle
+ | 13 -> ButtonCircle
+ | 6 -> ButtonDown
+ | 7 -> ButtonLeft
+ | 4 -> ButtonUp
+ | 5 -> ButtonRight
+ | 0 -> ButtonSelect
+ | 3 -> ButtonStart
+ | 11 -> ButtonR1
+ | 9 -> ButtonR2
+ | 10 -> ButtonL1
+ | 8 -> ButtonL2
+ | 16 -> ButtonPS3
+ | 1 -> ButtonLAxis
+ | 2 -> ButtonRAxis
+ | n -> Printf.ksprintf failwith "unknown button %d" n
+
+(* +-----------------------------------------------------------------+
+ | SDL events (executed in a child process) |
+ +-----------------------------------------------------------------+ *)
+
+let child_loop pipe joy =
+ let axis_state = Array.init (num_axes joy) (get_axis joy) in
+ let send ev =
+ Pervasives.output_value pipe ev;
+ Pervasives.flush pipe
+ in
+ while true do
+ match wait_event () with
+ | KEYDOWN { keysym = key } ->
+ send (KeyPressed key);
+ if key = KEY_ESCAPE then begin
+ Sdl.quit ();
+ exit 0
+ end
+ | JOYAXISMOTION { jae_axis = axis; jae_value = value } ->
+ let value = 100 - ((value - axis_min) * 200 / (axis_max - axis_min)) in
+ if value <> axis_state.(axis) then begin
+ axis_state.(axis) <- value;
+ if axis = laxis_h then
+ send (JoyLAxisH value)
+ else if axis = laxis_v then
+ send (JoyLAxisV value)
+ else if axis = raxis_h then
+ send (JoyRAxisH value)
+ else if axis = raxis_v then
+ send (JoyRAxisV value)
+ else
+ ()
+ end
+ | JOYBUTTONUP { jbe_button = button } ->
+ send (JoyButtonPressed(button_of_num button))
+ | JOYBUTTONDOWN { jbe_button = button } ->
+ send (JoyButtonReleased(button_of_num button))
+ | _ ->
+ ()
+ done
+
+(* +-----------------------------------------------------------------+
+ | Handling events (in the parent process) |
+ +-----------------------------------------------------------------+ *)
+
+let axis_coef = 6
+let axis_coef_turn = 4
+let accelerations = (800, 800)
+let duration = 0.2
+
+let try_call action f =
+ try_lwt
+ f ()
+ with Failure msg ->
+ lwt () = Lwt_log.error_f "action %s failed with: %s" action msg in
+ return ()
+
+let rec set_velocities krobot velocities =
+ lwt () = Lwt_log.info_f "set-velocities: left=%d right=%d" (fst velocities) (snd velocities) in
+ lwt () =
+ try_call "set-velocities"
+ (fun () ->
+ Krobot.set_velocities krobot ~velocities ~accelerations ~duration)
+ in
+ if velocities = (0, 0) then
+ return ()
+ else begin
+ lwt () = Lwt_unix.sleep (duration /. 2.) in
+ set_velocities krobot velocities
+ end
+
+let parent_loop krobot pipe =
+ let stop = ref false in
+ let thread = ref (return ()) in
+ let raxis_h = ref 0
+ and raxis_v = ref 0
+ and laxis_h = ref 0
+ and laxis_v = ref 0 in
+ let set_velocities () =
+ cancel !thread;
+ if not !stop then
+ thread :=
+ set_velocities krobot
+ (!laxis_v * axis_coef - !raxis_h * axis_coef_turn,
+ !laxis_v * axis_coef + !raxis_h * axis_coef_turn)
+ in
+ let rec loop () =
+ Lwt_io.read_value pipe >>= function
+ | KeyPressed KEY_ESCAPE ->
+ return ()
+ | JoyLAxisV n ->
+ laxis_v := n;
+ set_velocities ();
+ loop ()
+ | JoyLAxisH n ->
+ laxis_h := n;
+ set_velocities ();
+ loop ()
+ | JoyRAxisV n ->
+ raxis_v := n;
+ set_velocities ();
+ loop ()
+ | JoyRAxisH n ->
+ raxis_h := n;
+ set_velocities ();
+ loop ()
+ | JoyButtonPressed ButtonSquare ->
+ stop := true;
+ cancel !thread;
+ lwt () = try_call "stop-motors" (fun () -> Krobot.stop_motors krobot ~mode:`Abrupt) in
+ loop ()
+ | JoyButtonReleased ButtonSquare ->
+ stop := false;
+ loop ()
+ | _ ->
+ loop ()
+ in
+ loop ()
+
+(* +-----------------------------------------------------------------+
+ | Entry-point |
+ +-----------------------------------------------------------------+ *)
+
+let () =
+ let fd_r, fd_w = Unix.pipe () in
+ match Unix.fork () with
+ | 0 ->
+ Unix.close fd_r;
+ Sdl.init [`JOYSTICK;`VIDEO];
+ Sdljoystick.set_event_state true;
+ let joy =
+ try
+ open_joystick 0
+ with exn ->
+ Printf.eprintf "cannot open joystick: %s\n%!" (Printexc.to_string exn);
+ raise exn
+ in
+ child_loop (Unix.out_channel_of_descr fd_w) joy
+ | pid ->
+ Unix.close fd_w;
+ Lwt_main.run begin
+ lwt krobot = Krobot.create () in
+ lwt () = Lwt_log.notice "ready to process event" in
+ parent_loop krobot (Lwt_io.of_unix_fd ~mode:Lwt_io.input fd_r)
+ end
diff --git a/PC_Mainboard/clients/krobot_script.ml b/PC_Mainboard/clients/krobot_script.ml
new file mode 100644
index 0000000..6f55c8a
--- /dev/null
+++ b/PC_Mainboard/clients/krobot_script.ml
@@ -0,0 +1,329 @@
+(*
+ * krobot_script.ml
+ * ----------------
+ * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+open Lwt
+open Lwt_term
+open Krobot_script_types
+
+module TextSet = Set.Make(Text)
+
+let set_of_list l = List.fold_left (fun set x -> TextSet.add x set) TextSet.empty l
+
+(* +-----------------------------------------------------------------+
+ | Completion |
+ +-----------------------------------------------------------------+ *)
+
+let decompose name =
+ match Text.rev_split ~sep:"." ~max:2 name with
+ | [] ->
+ ([], "")
+ | [name] ->
+ ([], name)
+ | [path; name] ->
+ (Text.split ~sep:"." path, name)
+ | _ ->
+ assert false
+
+let args_of_command name =
+ let path, name = decompose name in
+ let rec loop = function
+ | cmd :: _ when cmd.c_path = path && cmd.c_name = name ->
+ Some cmd.c_args
+ | _ :: rest ->
+ loop rest
+ | [] ->
+ None
+ in
+ loop !commands
+
+let rec after_prefix prefix path =
+ match prefix, path with
+ | [], path ->
+ Some path
+ | e1 :: p1, e2 :: p2 when e1 = e2 ->
+ after_prefix p1 p2
+ | [e1], e2 :: p2 when Text.starts_with e2 e1 ->
+ Some path
+ | _ ->
+ None
+
+let complete ~before ~after =
+ try
+ match Krobot_script_lexer.partial_command (Lexing.from_string before) with
+ | `Command(before, name) -> begin
+ let full_path, path, basename =
+ match Text.rev_split ~sep:"." ~max:2 name with
+ | [] ->
+ ("", [], "")
+ | [name] ->
+ ("", [], name)
+ | [path; name] ->
+ (path ^ ".", Text.split ~sep:"." path, name)
+ | _ ->
+ assert false
+ in
+ let paths, names =
+ (List.fold_left
+ (fun (paths, names) command ->
+ match after_prefix path command.c_path with
+ | None ->
+ (paths, names)
+ | Some [] ->
+ (paths, TextSet.add command.c_name names)
+ | Some (name :: rest) ->
+ (TextSet.add name paths, names))
+ (TextSet.empty, TextSet.empty) !commands)
+ in
+ let prefix, words = Lwt_read_line.lookup basename (TextSet.union paths names) in
+ match TextSet.cardinal words with
+ | 0 ->
+ { Lwt_read_line.comp_state = (before ^ name, after);
+ Lwt_read_line.comp_words = words }
+ | 1 ->
+ { Lwt_read_line.comp_state = (before ^ full_path ^ prefix ^
+ (if TextSet.mem (TextSet.choose words) paths then "." else " "),
+ after);
+ Lwt_read_line.comp_words = words }
+ | _ ->
+ { Lwt_read_line.comp_state = (before ^ full_path ^ prefix, after);
+ Lwt_read_line.comp_words = words }
+ end
+
+ | `Arg(before, name, args, `Key key) -> begin
+ match args_of_command name with
+ | None ->
+ raise Exit
+ | Some args' ->
+ let args' = set_of_list (List.map fst args') in
+ (* Remove already passed arguments *)
+ let args = TextSet.diff args' args in
+ Lwt_read_line.complete ~suffix:"=" before key after args
+ end
+ | `Arg(before, name, args, `Value(key, value)) -> begin
+ match args_of_command name with
+ | None ->
+ raise Exit
+ | Some args' ->
+ try
+ match List.assoc key args' with
+ | Keyword words ->
+ Lwt_read_line.complete ~suffix:" " before value after (set_of_list words)
+ | _ ->
+ raise Exit
+ with Not_found ->
+ raise Exit
+ end
+ | `Arg(before, name, args, `Nothing) ->
+ raise Exit
+ with Exit | Krobot_script_lexer.Parse_failure _ ->
+ { Lwt_read_line.comp_state = (before, after);
+ Lwt_read_line.comp_words = TextSet.empty }
+
+(* +-----------------------------------------------------------------+
+ | Execution |
+ +-----------------------------------------------------------------+ *)
+
+let exec ~krobot ~logger ~comma...
[truncated message content] |
