Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 4d6ec639024036a9a80a91c452ad8831db96671b
|
From: Jérémie D. <Ba...@us...> - 2010-02-03 14:12:21
|
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 4d6ec639024036a9a80a91c452ad8831db96671b (commit)
via bb8ab39552d9e5f3d8d28ea76ba5b39f9e92ae4c (commit)
from 817f11427cf4305cb7ee66ab1a248473c89e2992 (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 4d6ec639024036a9a80a91c452ad8831db96671b
Author: Jérémie Dimino <je...@di...>
Date: Wed Feb 3 15:11:28 2010 +0100
rename monitor to controller
commit bb8ab39552d9e5f3d8d28ea76ba5b39f9e92ae4c
Author: Jérémie Dimino <je...@di...>
Date: Wed Feb 3 15:09:28 2010 +0100
added per motor speed setting
-----------------------------------------------------------------------
Changes:
diff --git a/PC_Mainboard/clients/lib-krobot/krobot.ml b/PC_Mainboard/clients/lib-krobot/krobot.ml
index 05ef14d..58b2177 100644
--- a/PC_Mainboard/clients/lib-krobot/krobot.ml
+++ b/PC_Mainboard/clients/lib-krobot/krobot.ml
@@ -188,22 +188,25 @@ OP_method CloseGrip : unit
include MakeDevice(struct let name = "Motors" end)
+let obus_motor = OBus_type.mapping obus_int
+ [(`Left, -1);
+ (`Both, 0);
+ (`Right, 1)]
+
+let obus_stop_mode = OBus_type.mapping obus_int
+ [(`Off, 0);
+ (`Abrupt, 1);
+ (`Smooth, 2)]
+
OP_method Turn : int -> int -> int -> unit
OP_method Move : int -> int -> int -> unit
-OP_method StopMotors : int -> int -> unit
+OP_method StopMotors : motor -> stop_mode -> unit
+OP_method SetSpeed : motor -> int -> int -> unit
let turn krobot ~angle ~speed ~acc = turn krobot angle speed acc
let move krobot ~dist ~speed ~acc = turn krobot dist speed acc
-let stop_motors krobot ~motor ~mode =
- stop_motors krobot
- (match motor with
- | `Left -> -1
- | `Both -> 0
- | `Right -> 1)
- (match mode with
- | `Off -> 0
- | `Abrupt -> 1
- | `Smooth -> 2)
+let stop_motors krobot ~motor ~mode = stop_motors krobot motor mode
+let set_speed krobot ~motor ~mode = set_speed krobot motor mode
(* +-----------------------------------------------------------------+
| Cards |
diff --git a/PC_Mainboard/clients/lib-krobot/krobot.mli b/PC_Mainboard/clients/lib-krobot/krobot.mli
index cf75746..4c7046b 100644
--- a/PC_Mainboard/clients/lib-krobot/krobot.mli
+++ b/PC_Mainboard/clients/lib-krobot/krobot.mli
@@ -71,9 +71,15 @@ val close_grip : t -> unit Lwt.t
val turn : t -> angle : int -> speed : int -> acc : int -> unit Lwt.t
val move : t -> dist : int -> speed : int -> acc : int -> unit Lwt.t
-val stop_motors : t -> motor : [ `Left | `Right | `Both ] -> mode : [ `Off | `Abrupt | `Smooth ] -> unit Lwt.t
+type motor = [ `Left | `Right | `Both ]
+
+val stop_motors : t -> motor : motor -> mode : [ `Off | `Abrupt | `Smooth ] -> unit Lwt.t
(** [stop_motorw t motor mode] stop the given motor(s). *)
+val set_speed : t -> motor : motor -> speed : int -> acc : int -> unit Lwt.t
+ (** [set_speed krobot ~motor ~speed ~acc] set the speed and
+ acceleration for the specified motors. *)
+
(** {6 Cards} *)
module Card : sig
diff --git a/PC_Mainboard/clients/myocamlbuild.ml b/PC_Mainboard/clients/myocamlbuild.ml
index 9e3cd7c..ff52740 100644
--- a/PC_Mainboard/clients/myocamlbuild.ml
+++ b/PC_Mainboard/clients/myocamlbuild.ml
@@ -96,7 +96,7 @@ let _ =
"lib-krobot/krobot.cma";
"lib-krobot/krobot.cmxa";
"tools/status.native";
- "tools/monitor.native";
+ "tools/controller.native";
"remote/forward_dbus.native";
];
diff --git a/PC_Mainboard/clients/tools/controller.ml b/PC_Mainboard/clients/tools/controller.ml
new file mode 100644
index 0000000..0eed0f3
--- /dev/null
+++ b/PC_Mainboard/clients/tools/controller.ml
@@ -0,0 +1,300 @@
+(*
+ * 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)
+
+(* +-----------------------------------------------------------------+
+ | Drawing |
+ +-----------------------------------------------------------------+ *)
+
+type section = {
+ screen : point array array;
+ x : int;
+ y : int;
+ w : int;
+ h : int;
+}
+
+let get_point clip x y =
+ if x >= 0 && x < clip.w && y >= 0 && y < clip.h then
+ clip.screen.(clip.y + y).(clip.x + x)
+ else
+ blank
+
+let set_point clip x y point =
+ if x >= 0 && x < clip.w && y >= 0 && y < clip.h then
+ clip.screen.(clip.y + y).(clip.x + x) <- point
+
+let draw_text clip ?style x y txt =
+ if y >= 0 && y < clip.h && x < clip.w then
+ let rec loop x ptr = match Text.next ptr with
+ | None ->
+ ()
+ | Some(ch, ptr) ->
+ if x >= 0 && x < clip.w then begin
+ match style with
+ | Some style ->
+ clip.screen.(clip.y + y).(clip.x + x) <- { char = ch; style = style }
+ | None ->
+ clip.screen.(clip.y + y).(clip.x + x) <- { blank with char = ch };
+ end;
+ loop (x + 1) ptr
+ in
+ loop x (Text.pointer_l txt)
+
+(* Prevent concurrent drawing: *)
+let drawer_mutex = Lwt_mutex.create ()
+
+(* Draw the whole screen *)
+let rec draw size (compass, logic_sensors, range_finders, team, jack) (engine_state, box, logs) (state_interface, state_sensor, state_motor) =
+ Lwt_mutex.with_lock drawer_mutex begin fun () ->
+ if Lwt_mutex.is_empty drawer_mutex then begin
+ (* Redraw the screen only if there is no other thread waiting to
+ do it *)
+
+ let screen = Zone.make ~width:size.columns ~height:size.lines in
+ let points = Zone.points screen in
+
+ (* ===== Borders ===== *)
+
+ for i = 1 to size.columns - 2 do
+ points.(0).(i) <- { blank with char = "─" };
+ points.(size.lines - 1).(i) <- { blank with char = "─" }
+ done;
+ for i = 1 to size.lines - 2 do
+ points.(i).(0) <- { blank with char = "│" };
+ points.(i).(size.columns - 1) <- { blank with char = "│" }
+ done;
+ points.(0).(0) <- { blank with char = "┌" };
+ points.(size.lines - 1).(0) <- { blank with char = "└" };
+ points.(size.lines - 1).(size.columns - 1) <- { blank with char = "┘" };
+ points.(0).(size.columns - 1) <- { blank with char = "┐" };
+
+ (* ===== Status ===== *)
+
+ Draw.text screen 1 0 "─[ Range finders ]─┬─[ Logic Sensors ]─┬─[ Status ]─";
+ points.(9).(0) <- { blank with char = "├" };
+ points.(9).(size.columns - 1) <- { blank with char = "┤" };
+ for i = 1 to size.columns - 2 do
+ points.(9).(i) <- { blank with char = "─" }
+ done;
+ for i = 1 to 8 do
+ points.(i).(20) <- { blank with char = "│" };
+ points.(i).(40) <- { blank with char = "│" }
+ done;
+ Draw.text screen 1 9 "───────────────────┴───────────────────┴";
+
+ let zone = Zone.inner screen in
+ for i = 0 to Array.length range_finders - 1 do
+ Draw.textf zone 0 i "%d : %d" i range_finders.(i)
+ done;
+ 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 x = 40 in
+ Draw.textf zone x 0 "team = %s" (match team with Krobot.Team_red -> "red" | Krobot.Team_green -> "green");
+ Draw.textf zone x 1 "jack = %s" (if jack then "present" else "absent");
+ Draw.textf zone x 2 "compass = %d" compass;
+ let string_of_state = function
+ | `Running -> "running"
+ | `Opening -> "opening"
+ | `Closed -> "closed"
+ in
+ Draw.textf zone x 3 "interface card is %s" (string_of_state state_interface);
+ Draw.textf zone x 4 "sensor card is %s" (string_of_state state_sensor);
+ Draw.textf zone x 5 "motor card is %s" (string_of_state state_motor);
+
+ (* ===== 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.text zone 0 y line;
+ loop (y - 1) rest
+ end
+ in
+ loop (Zone.height zone - 1) logs;
+
+ (* ===== Read-line ===== *)
+
+ points.(size.lines - 3).(0) <- { blank with char = "├" };
+ points.(size.lines - 3).(size.columns - 1) <- { blank with char = "┤" };
+ points.(size.lines - 5).(0) <- { blank with char = "├" };
+ points.(size.lines - 5).(size.columns - 1) <- { blank with char = "┤" };
+ for i = 1 to size.columns - 2 do
+ points.(size.lines - 5).(i) <- { blank with char = "─" };
+ points.(size.lines - 3).(i) <- { blank with char = "─" }
+ done;
+
+ let zone = Zone.sub ~zone:screen ~x:1 ~y:(size.lines - 4) ~width:(size.columns - 2) ~height:1 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
+ | [] ->
+ 20 + 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;
+ ];
+ 20 + 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 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.text zone i 0 "┬";
+ Draw.text zone i 1 "│";
+ Draw.text zone i 2 "┴";
+ i + 1)
+ words 0)
+ end;
+
+ Lwt_term.render (Zone.points screen)
+ end else
+ return ()
+ end
+
+(* +-----------------------------------------------------------------+
+ | 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 logs, set_logs = React.S.create []
+
+let rec loop krobot history =
+ lwt key = read_key () in
+ if key = key_escape then
+ return ()
+ 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
+ return ()
+ else begin
+ let history = Lwt_read_line.add_entry line history in
+ set_engine_state (Engine.init history);
+ set_logs (line :: React.S.value logs);
+ ignore (Script.exec krobot line);
+ loop krobot history
+ end
+ | 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
+
+(* +-----------------------------------------------------------------+
+ | Entry point |
+ +-----------------------------------------------------------------+ *)
+
+lwt () =
+ lwt () = hide_cursor () in
+ try_lwt
+ lwt krobot = Krobot.create () in
+ let signal =
+ React.S.l4 draw
+ Lwt_term.size
+ (React.S.l5 (fun a b c d e -> (a, b, c, d, e))
+ (Krobot.compass krobot)
+ (Krobot.logic_sensors krobot)
+ (Krobot.range_finders krobot)
+ (Krobot.team krobot)
+ (Krobot.jack krobot))
+ (React.S.l3 (fun a b c -> (a, b, c))
+ engine_state
+ box
+ logs)
+ (React.S.l3 (fun a b c -> (a, b, c))
+ (Krobot.Card.state krobot `Interface)
+ (Krobot.Card.state krobot `Sensor)
+ (Krobot.Card.state krobot `Motor))
+ in
+ (* Make the compiler happy: *)
+ ignore signal;
+ Lwt_term.with_raw_mode (fun () -> loop krobot [])
+ finally
+ show_cursor ()
diff --git a/PC_Mainboard/clients/tools/monitor.ml b/PC_Mainboard/clients/tools/monitor.ml
deleted file mode 100644
index 7c6e0c3..0000000
--- a/PC_Mainboard/clients/tools/monitor.ml
+++ /dev/null
@@ -1,300 +0,0 @@
-(*
- * monitor.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)
-
-(* +-----------------------------------------------------------------+
- | Drawing |
- +-----------------------------------------------------------------+ *)
-
-type section = {
- screen : point array array;
- x : int;
- y : int;
- w : int;
- h : int;
-}
-
-let get_point clip x y =
- if x >= 0 && x < clip.w && y >= 0 && y < clip.h then
- clip.screen.(clip.y + y).(clip.x + x)
- else
- blank
-
-let set_point clip x y point =
- if x >= 0 && x < clip.w && y >= 0 && y < clip.h then
- clip.screen.(clip.y + y).(clip.x + x) <- point
-
-let draw_text clip ?style x y txt =
- if y >= 0 && y < clip.h && x < clip.w then
- let rec loop x ptr = match Text.next ptr with
- | None ->
- ()
- | Some(ch, ptr) ->
- if x >= 0 && x < clip.w then begin
- match style with
- | Some style ->
- clip.screen.(clip.y + y).(clip.x + x) <- { char = ch; style = style }
- | None ->
- clip.screen.(clip.y + y).(clip.x + x) <- { blank with char = ch };
- end;
- loop (x + 1) ptr
- in
- loop x (Text.pointer_l txt)
-
-(* Prevent concurrent drawing: *)
-let drawer_mutex = Lwt_mutex.create ()
-
-(* Draw the whole screen *)
-let rec draw size (compass, logic_sensors, range_finders, team, jack) (engine_state, box, logs) (state_interface, state_sensor, state_motor) =
- Lwt_mutex.with_lock drawer_mutex begin fun () ->
- if Lwt_mutex.is_empty drawer_mutex then begin
- (* Redraw the screen only if there is no other thread waiting to
- do it *)
-
- let screen = Zone.make ~width:size.columns ~height:size.lines in
- let points = Zone.points screen in
-
- (* ===== Borders ===== *)
-
- for i = 1 to size.columns - 2 do
- points.(0).(i) <- { blank with char = "─" };
- points.(size.lines - 1).(i) <- { blank with char = "─" }
- done;
- for i = 1 to size.lines - 2 do
- points.(i).(0) <- { blank with char = "│" };
- points.(i).(size.columns - 1) <- { blank with char = "│" }
- done;
- points.(0).(0) <- { blank with char = "┌" };
- points.(size.lines - 1).(0) <- { blank with char = "└" };
- points.(size.lines - 1).(size.columns - 1) <- { blank with char = "┘" };
- points.(0).(size.columns - 1) <- { blank with char = "┐" };
-
- (* ===== Status ===== *)
-
- Draw.text screen 1 0 "─[ Range finders ]─┬─[ Logic Sensors ]─┬─[ Status ]─";
- points.(9).(0) <- { blank with char = "├" };
- points.(9).(size.columns - 1) <- { blank with char = "┤" };
- for i = 1 to size.columns - 2 do
- points.(9).(i) <- { blank with char = "─" }
- done;
- for i = 1 to 8 do
- points.(i).(20) <- { blank with char = "│" };
- points.(i).(40) <- { blank with char = "│" }
- done;
- Draw.text screen 1 9 "───────────────────┴───────────────────┴";
-
- let zone = Zone.inner screen in
- for i = 0 to Array.length range_finders - 1 do
- Draw.textf zone 0 i "%d : %d" i range_finders.(i)
- done;
- 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 x = 40 in
- Draw.textf zone x 0 "team = %s" (match team with Krobot.Team_red -> "red" | Krobot.Team_green -> "green");
- Draw.textf zone x 1 "jack = %s" (if jack then "present" else "absent");
- Draw.textf zone x 2 "compass = %d" compass;
- let string_of_state = function
- | `Running -> "running"
- | `Opening -> "opening"
- | `Closed -> "closed"
- in
- Draw.textf zone x 3 "interface card is %s" (string_of_state state_interface);
- Draw.textf zone x 4 "sensor card is %s" (string_of_state state_sensor);
- Draw.textf zone x 5 "motor card is %s" (string_of_state state_motor);
-
- (* ===== 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.text zone 0 y line;
- loop (y - 1) rest
- end
- in
- loop (Zone.height zone - 1) logs;
-
- (* ===== Read-line ===== *)
-
- points.(size.lines - 3).(0) <- { blank with char = "├" };
- points.(size.lines - 3).(size.columns - 1) <- { blank with char = "┤" };
- points.(size.lines - 5).(0) <- { blank with char = "├" };
- points.(size.lines - 5).(size.columns - 1) <- { blank with char = "┤" };
- for i = 1 to size.columns - 2 do
- points.(size.lines - 5).(i) <- { blank with char = "─" };
- points.(size.lines - 3).(i) <- { blank with char = "─" }
- done;
-
- let zone = Zone.sub ~zone:screen ~x:1 ~y:(size.lines - 4) ~width:(size.columns - 2) ~height:1 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
- | [] ->
- 20 + 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;
- ];
- 20 + 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 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.text zone i 0 "┬";
- Draw.text zone i 1 "│";
- Draw.text zone i 2 "┴";
- i + 1)
- words 0)
- end;
-
- Lwt_term.render (Zone.points screen)
- end else
- return ()
- end
-
-(* +-----------------------------------------------------------------+
- | 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 logs, set_logs = React.S.create []
-
-let rec loop krobot history =
- lwt key = read_key () in
- if key = key_escape then
- return ()
- 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
- return ()
- else begin
- let history = Lwt_read_line.add_entry line history in
- set_engine_state (Engine.init history);
- set_logs (line :: React.S.value logs);
- ignore (Script.exec krobot line);
- loop krobot history
- end
- | 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
-
-(* +-----------------------------------------------------------------+
- | Entry point |
- +-----------------------------------------------------------------+ *)
-
-lwt () =
- lwt () = hide_cursor () in
- try_lwt
- lwt krobot = Krobot.create () in
- let signal =
- React.S.l4 draw
- Lwt_term.size
- (React.S.l5 (fun a b c d e -> (a, b, c, d, e))
- (Krobot.compass krobot)
- (Krobot.logic_sensors krobot)
- (Krobot.range_finders krobot)
- (Krobot.team krobot)
- (Krobot.jack krobot))
- (React.S.l3 (fun a b c -> (a, b, c))
- engine_state
- box
- logs)
- (React.S.l3 (fun a b c -> (a, b, c))
- (Krobot.Card.state krobot `Interface)
- (Krobot.Card.state krobot `Sensor)
- (Krobot.Card.state krobot `Motor))
- in
- (* Make the compiler happy: *)
- ignore signal;
- Lwt_term.with_raw_mode (fun () -> loop krobot [])
- finally
- show_cursor ()
diff --git a/PC_Mainboard/driver/src/driver.ml b/PC_Mainboard/driver/src/driver.ml
index 17b5236..ff0f8c7 100644
--- a/PC_Mainboard/driver/src/driver.ml
+++ b/PC_Mainboard/driver/src/driver.ml
@@ -367,6 +367,12 @@ struct
type t = {
obus : OBus_object.t;
card : Card.t;
+
+ mutable acceleration : int option;
+ (* The current acceleration *)
+
+ mutable state : unit React.signal;
+ (* Reinit [acceleraation] to None when the card state changes *)
}
module OBus = OBus_object.Make(struct
@@ -401,28 +407,76 @@ struct
else
move Protocol.traj_forward dev.card dist speed acc
+ let obus_motor = OBus_type.mapping obus_int
+ [(Protocol.motor_left, -1);
+ (Protocol.motor_both, 0);
+ (Protocol.motor_right, 1)]
+
+ let obus_stop_mode = OBus_type.mapping obus_int
+ [(Protocol.traj_stop_motor_off, 0);
+ (Protocol.traj_stop_abrupt, 1);
+ (Protocol.traj_stop_smooth, 2)]
+
let stop_motors dev motor mode =
let data = String.create 4 in
RW.set_uint8 data 0 Protocol.traj_stop;
- RW.set_uint8 data 1 (match motor with
- | -1 -> Protocol.motor_left
- | 0 -> Protocol.motor_both
- | 1 -> Protocol.motor_right
- | n -> Printf.ksprintf failwith "invalid motor (%d)" n);
- RW.set_uint16 data 2 (match mode with
- | 0 -> Protocol.traj_stop_motor_off
- | 1 -> Protocol.traj_stop_abrupt
- | 2 -> Protocol.traj_stop_smooth
- | n -> Printf.ksprintf failwith "invalid stop mode (%d)" n);
+ RW.set_uint8 data 1 motor;
+ RW.set_uint16 data 2 mode;
Card.send_command dev.card Protocol.cmd_traj data
+ (* Values comming from lm629.h *)
+ let forward = 0
+ let backward = 4096
+
+ let traj_new_velocity card motor vel acc dir =
+ let data = String.create 8 in
+ RW.set_uint8 data 0 Protocol.traj_new_velocity;
+ RW.set_uint8 data 1 motor;
+ RW.set_int16 data 2 vel;
+ RW.set_int16 data 4 acc;
+ RW.set_int16 data 6 dir;
+ Card.send_command card Protocol.cmd_traj data
+
+ let traj_change_velocity card motor vel dir =
+ let data = String.create 6 in
+ RW.set_uint8 data 0 Protocol.traj_change_velocity;
+ RW.set_uint8 data 1 motor;
+ RW.set_int16 data 2 vel;
+ RW.set_int16 data 4 dir;
+ Card.send_command card Protocol.cmd_traj data
+
+ let traj_start card motor =
+ let data = String.create 2 in
+ RW.set_uint8 data 0 Protocol.traj_start;
+ RW.set_uint8 data 1 motor;
+ Card.send_command card Protocol.cmd_traj data
+
+ let set_speed dev motor speed acc =
+ let dir, speed = if speed < 0 then (backward, -speed) else (forward, speed) in
+ lwt () =
+ if Some acc <> dev.acceleration then begin
+ dev.acceleration <- Some acc;
+ traj_new_velocity dev.card motor speed acc dir
+ end else
+ traj_change_velocity dev.card motor speed dir
+ in
+ traj_start dev.card motor
+
OL_method Turn : int -> int -> int -> unit
OL_method Move : int -> int -> int -> unit
+ OL_method StopMotors : motor -> stop_mode -> unit
+ OL_method SetSpeed : motor -> int -> int -> unit
- let make card path = {
- obus = OBus_object.make path;
- card = card;
- }
+ let make card path =
+ let dev = {
+ obus = OBus_object.make path;
+ card = card;
+ acceleration = None;
+ state = React.S.const ();
+ }
+ in
+ dev.state <- React.S.map (fun state -> dev.acceleration <- None) (Card.state card);
+ dev
end
(* +-----------------------------------------------------------------+
hooks/post-receive
--
krobot
|
