Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, motor-nurbs, updated. 03ce4e475cf42b892c90fc4f9278e7866c82911d
|
From: Xavier L. <Ba...@us...> - 2010-02-24 22:47:33
|
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, motor-nurbs has been updated
via 03ce4e475cf42b892c90fc4f9278e7866c82911d (commit)
via d3e48b7aed5c05658abf5cd9c7465f3990f4b9e4 (commit)
via 085efdbed4f0703aa3d6a2e3be36ed0cc993c32d (commit)
via f763bb5ede549e946affa6f5f8b1817de9656595 (commit)
via 4b6f331e6e1c0a2c4e91e5f13838a77ab7110ad7 (commit)
via d5cabf5265f1cf7f5d47fd193ada0bd3b4c36e68 (commit)
via c8338ac1db819595ad7883625879bc2180fe872c (commit)
via ec6a88b803f8747e048f74bd305d67e32be01ac7 (commit)
via 0a529d96f005d2befe3919348127e415a4bbd8bc (commit)
via 374911310949605868ed9c075f8958a9dcda27db (commit)
via dde1617b6333d9005a9bb79542d09fa17e023b3d (commit)
via e237a87733351c0f01e319bab5ff335166fa6094 (commit)
via 63f14c692283128c4438374445fd1298b31c811e (commit)
via 59b4b288ae853d691e5293a9054a765aeeb3a8e8 (commit)
via ff69ea42068f9527975208553f04d369e1ba7f5c (commit)
via 09b96167adb43821ec7c1a4023be8cb1686a1a27 (commit)
via b816f48eab76c6b03cde9297fdf561dd83168995 (commit)
via 25eabbea383d06ad591978ba725b9e9b8081599f (commit)
via 03b2d41ebef871da47dc3cc3a72773b838fc5c5d (commit)
via 421de24efb03bcaa1919f0799f9aa8637cfcb88d (commit)
via 6d97e2de14f3e17bba824ae17e4630ed4a5ff5b8 (commit)
via 0e26131ab8db136ad132bee9e796b91bcfa54ef8 (commit)
via f444918324cd99ce343190d5b122295f6ce31e45 (commit)
via 1704a1ff79e3504fe1e5121f576c559b2046766d (commit)
via 4a734d6396172ea06a22a0b8c0300c48ea0dbf48 (commit)
via 874ddbf56c24c94c9805d5faa15e242b0f7eefba (commit)
via cb562ea4bd61d42e8cd13999561155ba29a95952 (commit)
via c2a2cd1daade5c0e5d52d2791998f874088988d9 (commit)
via a56ae2921c0bad51ebc5c2cd11f1d9ba336c67d5 (commit)
via 3192e559c2ab36e98d3b408a015a95d24f6f2cd0 (commit)
via 694a02719b4b016a8c206177f9d6d70e00ddee6d (commit)
via 4435faf7b86a66387821cbeecbb930e3c575ef67 (commit)
via 5e643c4777ad4c8746695c8c566e06e6056e79fa (commit)
via b4e019644eb1a56de3a006e2136fb99e9c9517db (commit)
via fa42027035da13c7ecdd2a9eebda5f869db6bb63 (commit)
via d5dd29f7cee70a51a4bbfb294b48c970927ace06 (commit)
via 55485c76b7ae61643fb73003c6f7c02ec144c984 (commit)
via 82f453512edd3c3b653149f7025735b943249d93 (commit)
via dc7f31e7038f1ac3f97cdad9b76de424fbbab041 (commit)
via 4559f9bbbab6518f243fd21f6e406354ac8705ec (commit)
via e4578a9f84417da74eafeb2d81d2ab8079cdb6b3 (commit)
via f3530f39db8f3f91c4e699cdb2b62dc4deecbd23 (commit)
via b8f7544dccaa2e3b7fae129e3aab7753be831561 (commit)
via 04715e243dcdaeee3de34246586171c3b4c9883a (commit)
via 819b0a90fd152d2390eea791f5a3ab115b2629a3 (commit)
via c6ca4d9dab2c4646c8d8e0b5bb9670fb30a3b8af (commit)
via c07439dbd3a468aa4347ee5c03fa9905637426d1 (commit)
via c0d1d60462dfc4bf44bdc73eb14937bc048a6afc (commit)
via 6abe826b5278a9447436ef4209a6db4e8804799d (commit)
via 00d6bb9fa4015d784fa097f489b8621d392078f6 (commit)
via 6957a1aa373af9614710b18c193c3d062d61d14e (commit)
via 57ed3996de596341ae3996edf376de366d39eb2f (commit)
via 6945591b460b0b5106b07685b69b78d6407cacf1 (commit)
from c72950af55e8c202c2aa7fab117b946af69b1c45 (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 03ce4e475cf42b892c90fc4f9278e7866c82911d
Author: Xavier Lagorce <Xav...@cr...>
Date: Wed Feb 24 23:46:19 2010 +0100
Building new firmware after fixing merge errors
commit d3e48b7aed5c05658abf5cd9c7465f3990f4b9e4
Merge: c72950af55e8c202c2aa7fab117b946af69b1c45 085efdbed4f0703aa3d6a2e3be36ed0cc993c32d
Author: Xavier Lagorce <Xav...@cr...>
Date: Wed Feb 24 23:34:11 2010 +0100
Merge branch 'master' into motor-nurbs
Conflicts:
USB_Module/Motor_Controller/Firmware/Motor_Controller.mcw
USB_Module/Motor_Controller/Firmware/output/Motor_controller.cof
USB_Module/Motor_Controller/Firmware/output/Motor_controller.hex
USB_Module/Motor_Controller/Firmware/output/Motor_controller.map
-> keeping binary files from the master branch
(/!\ Firmwares need recompilation)
-----------------------------------------------------------------------
Changes:
diff --git a/PC_Mainboard/_tags b/PC_Mainboard/_tags
index 818fd97..2b3245a 100644
--- a/PC_Mainboard/_tags
+++ b/PC_Mainboard/_tags
@@ -25,8 +25,7 @@
<clients/**>: pkg_lwt.unix, pkg_obus
<clients/joy_control.*>: pkg_sdl
<clients/controller.*>: pkg_lwt.text
-<clients/script_lexer.*>: pkg_text
-<clients/script.*>: pkg_text
+<clients/script*>: pkg_text
# +------------------------------------------------------------------+
# | Services |
@@ -39,8 +38,8 @@
# | Common |
# +------------------------------------------------------------------+
-<common/types.{ml,mli}>: syntax_camlp4o, pkg_obus.syntax
-<common/types.*>: pkg_obus
+<common/{types,util}.{ml,mli}>: syntax_camlp4o, pkg_obus.syntax, pkg_lwt.syntax
+<common/{types,util}.*>: pkg_obus
# +------------------------------------------------------------------+
# | Driver |
diff --git a/PC_Mainboard/card_tools/bootloader.ml b/PC_Mainboard/card_tools/bootloader.ml
index 28044cc..35b407e 100644
--- a/PC_Mainboard/card_tools/bootloader.ml
+++ b/PC_Mainboard/card_tools/bootloader.ml
@@ -54,8 +54,8 @@ let close k =
let open_card () =
let handle = USB.open_device_with
- ~vendor_id:Protocol.usb_vid
- ~product_id:Protocol.usb_pid_bootloader
+ ~vendor_id:PcInterface.usb_vid
+ ~product_id:PcInterface.usb_pid_bootloader
in
let kernel_active = USB.kernel_driver_active handle 0 in
if kernel_active then USB.detach_kernel_driver handle 0;
@@ -109,7 +109,7 @@ let get_flash k ~address ~length =
let length = min increment total_length in
let response_length = length+header_length in
let address = address+offset in
- put_message send_buffer Protocol.read_flash length address "";
+ put_message send_buffer PcInterface.read_flash length address "";
lwt () = send_receive_packet k send_buffer header_length receive_buffer response_length 1. 3. in
let receive_header = String.sub receive_buffer 0 header_length in
if receive_header <> send_buffer then
@@ -132,10 +132,10 @@ let erase_flash k ~address ~length =
return ()
else begin
let address = address+offset in
- put_message send_buffer Protocol.erase_flash 1 address "";
+ put_message send_buffer PcInterface.erase_flash 1 address "";
lwt () = send_receive_packet k send_buffer header_length receive_buffer response_length 1. 5. in
- if int_of_char receive_buffer.[0] <> Protocol.erase_flash then
- failwith (UnexpectedReply (receive_buffer, String.make 1 (char_of_int Protocol.erase_flash)))
+ if int_of_char receive_buffer.[0] <> PcInterface.erase_flash then
+ failwith (UnexpectedReply (receive_buffer, String.make 1 (char_of_int PcInterface.erase_flash)))
else
loop (offset+increment) (total_length-increment);
end
@@ -162,10 +162,10 @@ let write_flash k ~address data offset length =
loop (address+increment) (offset+increment) (total_length-increment)
end else begin
(* lwt () = printf "Processing address 0x%06X...\n" address in *)
- put_message send_buffer Protocol.write_flash increment address packet;
+ put_message send_buffer PcInterface.write_flash increment address packet;
lwt () = send_receive_packet k send_buffer send_length receive_buffer receive_length 0.5 1. in
- if int_of_char receive_buffer.[0] <> Protocol.write_flash then
- failwith (UnexpectedReply (receive_buffer, String.make 1 (char_of_int Protocol.erase_flash)))
+ if int_of_char receive_buffer.[0] <> PcInterface.write_flash then
+ failwith (UnexpectedReply (receive_buffer, String.make 1 (char_of_int PcInterface.erase_flash)))
else begin
lwt written = get_flash k ~address ~length:increment in
if written <> packet then
@@ -179,6 +179,6 @@ let write_flash k ~address data offset length =
let reset_board k =
let send_buffer = String.create 64 and receive_buffer = String.create 64 in
- send_buffer.[0] <- char_of_int Protocol.reset;
+ send_buffer.[0] <- char_of_int PcInterface.reset;
lwt () = send_receive_packet k send_buffer 1 receive_buffer 64 5. 5. in
return ()
diff --git a/PC_Mainboard/clients/controller.ml b/PC_Mainboard/clients/controller.ml
index 1d45d75..a253a34 100644
--- a/PC_Mainboard/clients/controller.ml
+++ b/PC_Mainboard/clients/controller.ml
@@ -116,13 +116,14 @@ let rec loop krobot history =
let line = Text.strip (Engine.all_input (React.S.value engine_state)) in
if line = "exit" then
save_history history
- else begin
+ else if line <> "" then begin
let history = Lwt_read_line.add_entry line history in
set_engine_state (Engine.init history);
lwt () = Log.notice line in
ignore (Script.exec ~krobot ~logger:(fun line -> log_add_line line; return ()) ~command:line);
loop krobot history
- end
+ 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
@@ -134,6 +135,21 @@ let rec loop krobot history =
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 |
+-----------------------------------------------------------------+ *)
@@ -173,6 +189,8 @@ let rec draw krobot =
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 "Status";
fg line_color; text " ]─"];
points.(9).(0) <- { line with char = "├" };
@@ -182,9 +200,10 @@ let rec draw krobot =
done;
for i = 1 to 8 do
points.(i).(20) <- { line with char = "│" };
- points.(i).(40) <- { line with char = "│" }
+ points.(i).(40) <- { line with char = "│" };
+ points.(i).(55) <- { line with char = "│" }
done;
- Draw.textc screen 1 9 [fg line_color; text "───────────────────┴───────────────────┴"];
+ Draw.textc screen 1 9 [fg line_color; text "───────────────────┴───────────────────┴──────────────┴"];
let zone = Zone.inner screen in
@@ -200,8 +219,18 @@ let rec draw krobot =
(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
+ 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
Draw.textf zone x 0 "team = %s" (match React.S.value (Krobot.team krobot) with
| Krobot.Team_red -> "red"
| Krobot.Team_green -> "green");
@@ -344,6 +373,7 @@ let refresh krobot =
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: *)
@@ -362,6 +392,10 @@ lwt () =
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: *)
@@ -384,6 +418,7 @@ lwt () =
notify (Krobot.Card.state krobot `Interface);
notify (Krobot.Card.state krobot `Sensor);
notify (Krobot.Card.state krobot `Motor);
+ notify services;
(* Redraw immedlatly the screen when [signal] changes: *)
let urgent signal = Lwt_signal.always_notify_p (fun _ -> refresh krobot) signal in
diff --git a/PC_Mainboard/clients/init_position.ml b/PC_Mainboard/clients/init_position.ml
index 94073a3..0a4e6d9 100644
--- a/PC_Mainboard/clients/init_position.ml
+++ b/PC_Mainboard/clients/init_position.ml
@@ -15,7 +15,7 @@ open Lwt
let move_backward_slowly krobot =
lwt () = Log.notice "moving backward" in
- Krobot.move krobot ~dist:(-1000) ~speed:100 ~acc:100 >>= function
+ Krobot.move krobot ~distance:(-1000) ~velocity:100 ~acceleration:100 >>= function
| `OK ->
lwt () = Log.error "where am i ???" in
exit 1
@@ -28,16 +28,16 @@ lwt () =
lwt () = move_backward_slowly krobot in
lwt () = Log.notice "going to initial position on first axis" in
- lwt _ = Krobot.move krobot ~dist:Config.initial_position ~speed:400 ~acc:800 in
+ lwt _ = Krobot.move krobot ~distance:Config.initial_position ~velocity:400 ~acceleration:800 in
lwt () = Log.notice "turning" in
- lwt _ = Krobot.turn krobot ~angle:(-90) ~speed:400 ~acc:800 in
+ lwt _ = Krobot.turn krobot ~angle:(-90) ~velocity:400 ~acceleration:800 in
lwt () = move_backward_slowly krobot in
lwt () = Log.notice "going to initial position on second axis" in
- lwt _ = Krobot.move krobot ~dist:Config.initial_position ~speed:400 ~acc:800 in
+ lwt _ = Krobot.move krobot ~distance:Config.initial_position ~velocity:400 ~acceleration:800 in
lwt () = Log.notice "turning" in
- lwt _ = Krobot.turn krobot ~angle:45 ~speed:400 ~acc:800 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
index 7c84d21..a31a0b7 100644
--- a/PC_Mainboard/clients/joy_control.ml
+++ b/PC_Mainboard/clients/joy_control.ml
@@ -128,7 +128,8 @@ let child_loop pipe joy =
let axis_coef = 6
let axis_coef_turn = 4
-let acceleration = 800
+let accelerations = (800, 800)
+let duration = 0.2
let try_call action f =
try_lwt
@@ -137,38 +138,34 @@ let try_call action f =
lwt () = Log.error_f "action %s failed with: %s" action msg in
return ()
-let rec set_speed action krobot motor speed abort_waiter =
- lwt () = choose [abort_waiter; try_call action (fun () -> Krobot.set_speed krobot ~motor ~speed ~acc:acceleration)] in
- if speed = 0 then
+let rec set_velocities krobot velocities =
+ 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 () = select [abort_waiter; Lwt_unix.sleep (Config.stop_motors_delay /. 2.)] in
- set_speed action krobot motor speed abort_waiter
+ lwt () = Lwt_unix.sleep (duration /. 2.) in
+ set_velocities krobot velocities
end
let parent_loop krobot pipe =
- let rstop = ref false and lstop = ref false in
- let rthread = ref (return ()) and lthread = ref (return ()) in
- let rabort_wakener = ref None and labort_wakener = ref None in
+ 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_speeds () =
- begin match !labort_wakener with
- | None -> ()
- | Some w -> wakeup_exn w Exit
- end;
- begin match !rabort_wakener with
- | None -> ()
- | Some w -> wakeup_exn w Exit
- end;
- let waiter, wakener = Lwt.wait () in
- labort_wakener := Some wakener;
- lthread := set_speed "set-speed-left" krobot `Left (!laxis_v * axis_coef - !raxis_h * axis_coef_turn) waiter;
- let waiter, wakener = Lwt.wait () in
- rabort_wakener := Some wakener;
- rthread := set_speed "set-speed-right" krobot `Right (!laxis_v * axis_coef + !raxis_h * axis_coef_turn) waiter
+ 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
@@ -176,35 +173,27 @@ let parent_loop krobot pipe =
return ()
| JoyLAxisV n ->
laxis_v := n;
- set_speeds ();
+ set_velocities ();
loop ()
| JoyLAxisH n ->
laxis_h := n;
- set_speeds ();
+ set_velocities ();
loop ()
| JoyRAxisV n ->
raxis_v := n;
- set_speeds ();
+ set_velocities ();
loop ()
| JoyRAxisH n ->
raxis_h := n;
- set_speeds ();
- loop ()
- | JoyButtonPressed ButtonL2 ->
- lstop := true;
- cancel !lthread;
- lwt () = try_call "stop-left-motor" (fun () -> Krobot.stop_motors krobot ~motor:`Left ~mode:`Abrupt) in
- ignore (lwt () = Lwt_unix.sleep 1.0 in lstop := false; return ());
+ set_velocities ();
loop ()
- | JoyButtonPressed ButtonR2 ->
- rstop := true;
- cancel !rthread;
- lwt () = try_call "stop-right-motor" (fun () -> Krobot.stop_motors krobot ~motor:`Right ~mode:`Abrupt) in
- ignore (lwt () = Lwt_unix.sleep 1.0 in rstop := false; return ());
+ | JoyButtonPressed ButtonSquare ->
+ stop := true;
+ cancel !thread;
+ lwt () = try_call "stop-motors" (fun () -> Krobot.stop_motors krobot ~mode:`Abrupt) in
loop ()
- | JoyButtonReleased ButtonCircle ->
- rstop := false;
- lstop := false;
+ | JoyButtonReleased ButtonSquare ->
+ stop := false;
loop ()
| _ ->
loop ()
@@ -234,6 +223,6 @@ let () =
Unix.close fd_w;
Lwt_main.run begin
lwt krobot = Krobot.create () in
- lwt() = Log.notice "ready to process event" in
+ 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/script.ml b/PC_Mainboard/clients/script.ml
index 2d6a19b..614e73c 100644
--- a/PC_Mainboard/clients/script.ml
+++ b/PC_Mainboard/clients/script.ml
@@ -9,133 +9,86 @@
open Lwt
open Lwt_term
+open Script_commands
module TextSet = Set.Make(Text)
let set_of_list l = List.fold_left (fun set x -> TextSet.add x set) TextSet.empty l
(* +-----------------------------------------------------------------+
- | Commands |
- +-----------------------------------------------------------------+ *)
-
-type logger = Lwt_term.styled_text -> unit Lwt.t
-
-(* Type of an argument *)
-type arg_type =
- | Int
- | Keyword of string list
-
-type command = {
- c_name : string;
- (* The command name *)
-
- c_exec : (string * string) list -> logger -> Krobot.t -> unit Lwt.t;
- (* The command implementation. It takes as argument the list of
- parameters. *)
-
- c_args : (string * arg_type) list;
- (* Argument description, used for completion. *)
-}
-
-(* An argument description *)
-type 'a arg = {
- a_type : arg_type;
- a_name : string;
- a_cast : string -> 'a;
- a_default : 'a option;
-}
-
-(* A function description *)
-type 'a func = {
- f_args : (string * arg_type) list;
- (* Arguments of the function, for completion *)
-
- f_func : (string * string) list -> 'a -> unit Lwt.t;
- (* [f_func args f] parses arguments [args] and apply them to [f] *)
-}
-
-(* All registred commands *)
-let commands = ref []
-
-(* Register a command *)
-let register name func f =
- let command = {
- c_name = name;
- c_exec = (fun args logger krobot -> func.f_func args (f logger krobot));
- c_args = func.f_args;
- } in
- commands := command :: !commands
-
-exception Argument_error of string
- (* Exception raised when there is a problem with an argument *)
-
-let arg_error msg = raise (Argument_error msg)
-
-(* Returns the value associated to [key] if any, and the list without
- the first occurence of [key] *)
-let rec assoc_remove key = function
- | [] ->
- (None, [])
- | (key', value) :: rest when key = key' ->
- (Some value, rest)
- | pair :: rest ->
- let result, l = assoc_remove key rest in
- (result, pair :: l)
-
-let ( --> ) arg func = {
- f_args = (arg.a_name, arg.a_type) :: func.f_args;
- f_func =
- fun args f ->
- let result, args = assoc_remove arg.a_name args in
- match result with
- | Some str ->
- func.f_func args (f (arg.a_cast str))
- | None ->
- match arg.a_default with
- | Some value ->
- func.f_func args (f value)
- | None ->
- Printf.ksprintf arg_error "argument '%s' is mandatory" arg.a_name
-}
-
-let f0 = {
- f_args = [];
- f_func =
- fun args f ->
- match args with
- | [] ->
- f
- | (key, _) :: _ ->
- Printf.ksprintf arg_error "unused argument '%s'" key
-}
-
-let f1 arg0 = arg0 --> f0
-let f2 arg0 arg1 = arg0 --> (f1 arg1)
-let f3 arg0 arg1 arg2 = arg0 --> (f2 arg1 arg2)
-let f4 arg0 arg1 arg2 arg3 = arg0 --> (f3 arg1 arg2 arg3)
-let f5 arg0 arg1 arg2 arg3 arg4 = arg0 --> (f4 arg1 arg2 arg3 arg4)
-let f6 arg0 arg1 arg2 arg3 arg4 arg5 = arg0 --> (f5 arg1 arg2 arg3 arg4 arg5)
-
-(* +-----------------------------------------------------------------+
| Completion |
+-----------------------------------------------------------------+ *)
-let rec args_of_command command = function
- | { c_name = name; c_args = args } :: _ when name = command ->
- Some args
- | _ :: rest ->
- args_of_command command rest
- | [] ->
- None
+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
+ | [], p -> Some p
+ | e1 :: p1, e2 :: p2 when e1 = e2 -> after_prefix p1 p2
+ | _ -> None
let complete ~before ~after =
try
match Script_lexer.partial_command (Lexing.from_string before) with
- | `Command(before, name) ->
- Lwt_read_line.complete ~suffix:" " before name after
- (set_of_list (List.map (fun command -> command.c_name) !commands))
+ | `Command(before, name) -> begin
+ let full_path, path, name =
+ 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 name names in
+ match TextSet.cardinal words with
+ | 0 ->
+ Lwt_read_line.complete ~suffix:"." (before ^ full_path) name after paths
+ | 1 ->
+ { Lwt_read_line.comp_state = (before ^ full_path ^ prefix ^ " ", 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 !commands with
+ match args_of_command name with
| None ->
raise Exit
| Some args' ->
@@ -145,7 +98,7 @@ let complete ~before ~after =
Lwt_read_line.complete ~suffix:"=" before key after args
end
| `Arg(before, name, args, `Value(key, value)) -> begin
- match args_of_command name !commands with
+ match args_of_command name with
| None ->
raise Exit
| Some args' ->
@@ -175,11 +128,12 @@ let exec ~krobot ~logger ~command =
| `Fail exn ->
logger [fg lred; textf "parse failure: %s" (Printexc.to_string exn)]
| `OK(name, args) ->
- try
+ let path, name = decompose name in
+ try_lwt
let rec search = function
| [] ->
logger [fg lred; textf "unknown command '%s'" name]
- | cmd :: rest when cmd.c_name <> name ->
+ | cmd :: rest when cmd.c_path <> path || cmd.c_name <> name ->
search rest
| cmd :: _ ->
cmd.c_exec args logger krobot
@@ -194,32 +148,6 @@ let exec ~krobot ~logger ~command =
logger [fg lred; textf "command '%s' failed with: %s" name (Printexc.to_string exn)]
(* +-----------------------------------------------------------------+
- | Arguments |
- +-----------------------------------------------------------------+ *)
-
-let int ?default name = {
- a_name = name;
- a_type = Int;
- a_cast = (fun str ->
- try
- int_of_string str
- with Failure _ ->
- Printf.ksprintf arg_error "invalid value for argument '%s': an integer was expected" name);
- a_default = default;
-}
-
-let keyword ?default name keywords = {
- a_name = name;
- a_type = Keyword(List.map fst keywords);
- a_cast = (fun key ->
- try
- List.assoc key keywords
- with Not_found ->
- Printf.ksprintf arg_error "invalid value for '%s'" name);
- a_default = default;
-}
-
-(* +-----------------------------------------------------------------+
| All commands |
+-----------------------------------------------------------------+ *)
@@ -231,10 +159,10 @@ let () =
| Movement |
+---------------------------------------------------------------+ *)
- let dist = int ~default:100 "dist"
+ let distance = int ~default:100 "distance"
and angle = int ~default:90 "angle"
- and speed = int ~default:400 "speed"
- and acc = int ~default:800 "acc" in
+ and velocity = int ~default:400 "velocity"
+ and acceleration = int ~default:800 "acceleration" in
let move_result logger = function
| `OK ->
@@ -243,40 +171,50 @@ let () =
logger [fg lyellow; text "move stopped"]
in
- register "forward" (f3 dist speed acc)
- (fun logger krobot dist speed acc ->
- Krobot.move krobot dist speed acc >>= move_result logger);
- register "backward" (f3 dist speed acc)
- (fun logger krobot dist speed acc ->
- Krobot.move krobot (-dist) speed acc >>= move_result logger);
- register "left" (f3 angle speed acc)
- (fun logger krobot angle speed acc ->
- Krobot.turn krobot angle speed acc >>= move_result logger);
- register "right" (f3 angle speed acc)
- (fun logger krobot angle speed acc ->
- Krobot.turn krobot (-angle) speed acc >>= move_result logger);
+ register "forward" (f3 distance velocity acceleration)
+ (fun logger krobot distance velocity acceleration ->
+ Krobot.move krobot distance velocity acceleration >>= move_result logger);
+ register "backward" (f3 distance velocity acceleration)
+ (fun logger krobot distance velocity acceleration ->
+ Krobot.move krobot (-distance) velocity acceleration >>= move_result logger);
+ register "left" (f3 angle velocity acceleration)
+ (fun logger krobot angle velocity acceleration ->
+ Krobot.turn krobot angle velocity acceleration >>= move_result logger);
+ register "right" (f3 angle velocity acceleration)
+ (fun logger krobot angle velocity acceleration ->
+ Krobot.turn krobot (-angle) velocity acceleration >>= move_result logger);
register "goto" (f6
- (int ~default:0 "x") (int ~default:0 "y") speed acc
+ (int ~default:0 "x") (int ~default:0 "y") velocity acceleration
(keyword ~default:`Straight "mode" [("straight", `Straight);
("curve-left", `Curve_left);
("curve-right", `Curve_right)])
- (int ~default:0 "bypass-dist"))
- (fun logger krobot x y speed acc mode bypass ->
- Krobot.goto krobot x y speed acc mode bypass >>= move_result logger);
+ (int ~default:0 "bypass-distance"))
+ (fun logger krobot x y velocity acceleration mode bypass ->
+ Krobot.goto krobot x y velocity acceleration mode bypass >>= move_result logger);
(* +---------------------------------------------------------------+
| Motors low-level conrol |
+---------------------------------------------------------------+ *)
- let motor = keyword ~default:`Both "motor"
- [("left", `Left); ("right", `Right); ("both", `Both)]
- and stop_mode = keyword ~default:`Smooth "stop-mode"
- [("off", `Off); ("abrupt", `Abrupt); ("smooth", `Smooth)] in
+ let stop_mode = keyword ~default:`Smooth "stop-mode"
+ [("off", `Off); ("abrupt", `Abrupt); ("smooth", `Smooth)]
+ and duration = float ~default:1.0 "duration"
+ and velocity motor = int ~default:400 ("velocity" ^ motor)
+ and acceleration motor = int ~default:800 ("acceleration" ^ motor) in
- register "stop-motors" (f2 motor stop_mode)
- (fun logger krobot motor mode -> Krobot.stop_motors krobot ~motor ~mode);
- register "set-speed" (f3 motor speed acc)
- (fun logger krobot motor speed acc -> Krobot.set_speed krobot ~motor ~speed ~acc);
+ register "motors-state" f0
+ (fun logger krobot ->
+ lwt state = Krobot.motors_state krobot in
+ logger [text "motors state: "; text state]);
+ register "stop-motors" (f1 stop_mode)
+ (fun logger krobot mode ->
+ Krobot.stop_motors krobot ~mode);
+ register "set-velocities" (f5 (velocity "-left") (velocity "-right") (acceleration "-left") (acceleration "-right") duration)
+ (fun logger krobot velocity_left velocity_right acceleration_left acceleration_right duration ->
+ Krobot.set_velocities krobot
+ ~velocities:(velocity_left, velocity_right)
+ ~accelerations:(acceleration_left, acceleration_right)
+ ~duration);
(* +---------------------------------------------------------------+
| Cards control |
@@ -316,9 +254,9 @@ let () =
register "calibration-start" (f2 (int "range-finder") (keyword "skip-meas" [("true", true); ("false", false)]))
(fun logger -> Krobot.calibration_start);
- register "calibartion-stop" f0
+ register "calibration-stop" f0
(fun logger -> Krobot.calibration_stop);
- register "calibartion-continue" f0
+ register "calibration-continue" f0
(fun logger -> Krobot.calibration_continue);
(* +---------------------------------------------------------------+
@@ -326,64 +264,44 @@ let () =
+---------------------------------------------------------------+ *)
let id = int "id"
- and pos = int "pos"
- and speed = int ~default:50 "speed"
+ and position = int "position"
+ and velocity = int ~default:50 "velocity"
and timeout = int ~default:100 "timeout"
- and reg = int "reg"
- and value = int "value"
- and now = keyword ~default:true "now" [("true", true); ("false", false)] in
+ and goto_mode = keyword ~default:`Now "mode" [("now", `Now); ("action", `Action)] in
- register "ax12-goto" (f4 id pos speed now)
- (fun logger krobot id pos speed now ->
- Krobot.AX12.goto krobot id pos speed now);
- register "ax12-ping" (f2 id timeout)
+ register ~path:["ax12"] "goto" (f4 id position velocity goto_mode)
+ (fun logger krobot id position velocity goto_mode ->
+ Krobot_unsafe.AX12.goto krobot id position velocity goto_mode);
+ register ~path:["ax12"] "ping" (f2 id timeout)
(fun logger krobot id timeout ->
- Krobot.AX12.ping krobot id timeout >>= function
- | 0 -> logger [textf "ax12-ping[%d] reply: " id; fg lred; text "timeout"]
- | _ -> logger [textf "ax12-ping[%d] reply: " id; fg lgreen; text "success"]);
- register "ax12-read8" (f3 id reg timeout)
- (fun logger krobot id reg timeout ->
- lwt x = Krobot.AX12.read8 krobot id reg timeout in
- logger [textf "ax12-read8[%d] reply: %d" id x]);
- register "ax12-read16" (f3 id reg timeout)
- (fun logger krobot id reg timeout ->
- lwt x = Krobot.AX12.read16 krobot id reg timeout in
- logger [textf "ax12-read16[%d] reply: %d" id x]);
- register "ax12-write8" (f3 id reg value)
- (fun logger krobot id reg value ->
- Krobot.AX12.write8 krobot id reg value);
- register "ax12-write16" (f3 id reg value)
- (fun logger krobot id reg value ->
- Krobot.AX12.write16 krobot id reg value);
- register "ax12-get-pos" (f2 id timeout)
+ Krobot_unsafe.AX12.ping krobot id timeout >>= function
+ | 0 -> logger [textf "ping[%d] reply: " id; fg lred; text "timeout"]
+ | _ -> logger [textf "ping[%d] reply: " id; fg lgreen; text "success"]);
+ register ~path:["ax12"] "get-position" (f2 id timeout)
(fun logger krobot id timeout ->
- lwt x = Krobot.AX12.get_pos krobot id timeout in
- logger [textf "ax12-position[%d]: %d" id x]);
- register "ax12-get-speed" (f2 id timeout)
+ lwt x = Krobot_unsafe.AX12.get_position krobot id timeout in
+ logger [textf "positionition[%d]: %d" id x]);
+ register ~path:["ax12"] "get-velocity" (f2 id timeout)
(fun logger krobot id timeout ->
- lwt x = Krobot.AX12.get_speed krobot id timeout in
- logger [textf "ax12-speed[%d]: %d" id x]);
- register "ax12-get-load" (f2 id timeout)
+ lwt x = Krobot_unsafe.AX12.get_velocity krobot id timeout in
+ logger [textf "velocity[%d]: %d" id x]);
+ register ~path:["ax12"] "get-load" (f2 id timeout)
(fun logger krobot id timeout ->
- lwt x = Krobot.AX12.get_load krobot id timeout in
- logger [textf "ax12-load[%d]: %d" id x]);
- register "ax12-stats" (f2 id timeout)
+ lwt x = Krobot_unsafe.AX12.get_load krobot id timeout in
+ logger [textf "load[%d]: %d" id x]);
+ register ~path:["ax12"] "get-stats" (f2 id timeout)
(fun logger krobot id timeout ->
- lwt stats = Krobot.AX12.stats krobot id timeout in
+ lwt stats = Krobot_unsafe.AX12.get_stats krobot id timeout in
lwt () = logger [textf "ax12[%d] position = %d" id stats.Types.ax12_position] in
- lwt () = logger [textf "ax12[%d] speed = %d" id stats.Types.ax12_speed] in
+ lwt () = logger [textf "ax12[%d] velocity = %d" id stats.Types.ax12_velocity] in
lwt () = logger [textf "ax12[%d] torque = %d" id stats.Types.ax12_torque] in
lwt () = logger [textf "ax12[%d] voltage = %d" id stats.Types.ax12_voltage] in
lwt () = logger [textf "ax12[%d] temperature = %d" id stats.Types.ax12_temperature] in
lwt () = logger [textf "ax12[%d] cw-angle-limit = %d" id stats.Types.ax12_cw_angle_limit] in
lwt () = logger [textf "ax12[%d] ccw-angle-limit = %d" id stats.Types.ax12_ccw_angle_limit] in
return ());
- register "ax12-write-reg8" (f3 id reg value)
- (fun logger krobot id reg value ->
- Krobot.AX12.write_reg8 krobot id reg value);
- register "ax12-write-reg16" (f3 id reg value)
- (fun logger krobot id reg value ->
- Krobot.AX12.write_reg16 krobot id reg value);
- register "ax12-action" (f1 (int ~default:254 "id"))
+ register ~path:["ax12"] "action" (f1 (int ~default:254 "id"))
(fun logger krobot id ->
- Krobot.AX12.action krobot id)
+ Krobot_unsafe.AX12.action krobot id)
+
+let () = Script_unsafe.register ()
diff --git a/PC_Mainboard/clients/script_commands.ml b/PC_Mainboard/clients/script_commands.ml
new file mode 100644
index 0000000..b7369a4
--- /dev/null
+++ b/PC_Mainboard/clients/script_commands.ml
@@ -0,0 +1,153 @@
+(*
+ * script_commands.ml
+ * ------------------
+ * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+
+(* +-----------------------------------------------------------------+
+ | Types |
+ +-----------------------------------------------------------------+ *)
+
+type logger = Lwt_term.styled_text -> unit Lwt.t
+
+(* Type of an argument *)
+type arg_type =
+ | Int
+ | Float
+ | Keyword of string list
+
+type command = {
+ c_name : string;
+ (* The command name *)
+
+ c_path : string list;
+ (* The path of the command *)
+
+ c_exec : (string * string) list -> logger -> Krobot.t -> unit Lwt.t;
+ (* The command implementation. It takes as argument the list of
+ parameters. *)
+
+ c_args : (string * arg_type) list;
+ (* Argument description, used for completion. *)
+}
+
+(* An argument description *)
+type 'a arg = {
+ a_type : arg_type;
+ a_name : string;
+ a_cast : string -> 'a;
+ a_default : 'a option;
+}
+
+(* A function description *)
+type 'a func = {
+ f_args : (string * arg_type) list;
+ (* Arguments of the function, for completion *)
+
+ f_func : (string * string) list -> 'a -> unit Lwt.t;
+ (* [f_func args f] parses arguments [args] and apply them to [f] *)
+}
+
+(* All registred commands *)
+let commands = ref []
+
+(* Register a command *)
+let register ?(path=[]) name func f =
+ let command = {
+ c_name = name;
+ c_path = path;
+ c_exec = (fun args logger krobot -> func.f_func args (f logger krobot));
+ c_args = func.f_args;
+ } in
+ commands := command :: !commands
+
+exception Argument_error of string
+ (* Exception raised when there is a problem with an argument *)
+
+let arg_error msg = raise (Argument_error msg)
+
+(* Returns the value associated to [key] if any, and the list without
+ the first occurence of [key] *)
+let rec assoc_remove key = function
+ | [] ->
+ (None, [])
+ | (key', value) :: rest when key = key' ->
+ (Some value, rest)
+ | pair :: rest ->
+ let result, l = assoc_remove key rest in
+ (result, pair :: l)
+
+let ( --> ) arg func = {
+ f_args = (arg.a_name, arg.a_type) :: func.f_args;
+ f_func =
+ fun args f ->
+ let result, args = assoc_remove arg.a_name args in
+ match result with
+ | Some str ->
+ func.f_func args (f (arg.a_cast str))
+ | None ->
+ match arg.a_default with
+ | Some value ->
+ func.f_func args (f value)
+ | None ->
+ Printf.ksprintf a
rg_error "argument '%s' is mandatory" arg.a_name
+}
+
+let f0 = {
+ f_args = [];
+ f_func =
+ fun args f ->
+ match args with
+ | [] ->
+ f
+ | (key, _) :: _ ->
+ Printf.ksprintf arg_error "unused argument '%s'" key
+}
+
+let f1 arg0 = arg0 --> f0
+let f2 arg0 arg1 = arg0 --> (f1 arg1)
+let f3 arg0 arg1 arg2 = arg0 --> (f2 arg1 arg2)
+let f4 arg0 arg1 arg2 arg3 = arg0 --> (f3 arg1 arg2 arg3)
+let f5 arg0 arg1 arg2 arg3 arg4 = arg0 --> (f4 arg1 arg2 arg3 arg4)
+let f6 arg0 arg1 arg2 arg3 arg4 arg5 = arg0 --> (f5 arg1 arg2 arg3 arg4 arg5)
+
+(* +-----------------------------------------------------------------+
+ | Arguments |
+ +-----------------------------------------------------------------+ *)
+
+let int ?default name = {
+ a_name = name;
+ a_type = Int;
+ a_cast = (fun str ->
+ try
+ int_of_string str
+ with Failure _ ->
+ Printf.ksprintf arg_error "invalid value for argument '%s': an integer was expected" name);
+ a_default = default;
+}
+
+let float ?default name = {
+ a_name = name;
+ a_type = Float;
+ a_cast = (fun str ->
+ try
+ float_of_string str
+ with Failure _ ->
+ Printf.ksprintf arg_error "invalid value for argument '%s': a float was expected" name);
+ a_default = default;
+}
+
+let keyword ?default name keywords = {
+ a_name = name;
+ a_type = Keyword(List.map fst keywords);
+ a_cast = (fun key ->
+ try
+ List.assoc key keywords
+ with Not_found ->
+ Printf.ksprintf arg_error "invalid value for '%s'" name);
+ a_default = default;
+}
diff --git a/PC_Mainboard/clients/script_lexer.mll b/PC_Mainboard/clients/script_lexer.mll
index 46d64fc..fa5a9a9 100644
--- a/PC_Mainboard/clients/script_lexer.mll
+++ b/PC_Mainboard/clients/script_lexer.mll
@@ -26,7 +26,7 @@ let xdigit = digit | ['a'-'f' 'A'-'F']
let space = blank | ['\n' '\x0b' '\x0c' '\r']
let identstart = [ 'A'-'Z' 'a'-'z' '_' ]
-let identbody = [ 'A'-'Z' 'a'-'z' '_' '-' '\'' '0' - '9' ]
+let identbody = [ 'A'-'Z' 'a'-'z' '_' '-' '\'' '0' - '9' '.' ]
let ident = identstart identbody*
let maybe_ident = "" | ident
diff --git a/PC_Mainboard/common/PcInterface.h b/PC_Mainboard/common/PcInterface.h
index 8fd98e0..e79e8fa 100644
--- a/PC_Mainboard/common/PcInterface.h
+++ b/PC_Mainboard/common/PcInterface.h
@@ -78,6 +78,7 @@ typedef struct _UP {
#define GET_CURRENT 12 ///< Valeur instantandu courant dtar la batterie [Battery Monitoring]
#define GET_POWER_STATE 13 ///< Etat de l'alimentation de puissance (On/Off) [Battery Monitoring]
#define GET_BATTERY_STATE 14 ///< Etat des batteries (Pleine charge/Charge moyenne/Charge faible) [Battery Monitoring]
+#define GET_CURRENT_SPEED 15 ///< Demande au PIC la valeur actuelle d'un moteur [Carte d'asservissement]
// CMD_ERR arguments
#define ERR_UNKNOWN_CMD 1 ///< Commande inconnue
diff --git a/PC_Mainboard/common/config.ml b/PC_Mainboard/common/config.ml
index 6c73d1d..c248d6e 100644
--- a/PC_Mainboard/common/config.ml
+++ b/PC_Mainboard/common/config.ml
@@ -13,4 +13,3 @@ let initial_position = 200
let bus_address = "unix:abstract=krobot"
let update_delay = 0.05
let reopen_delay = 1.0
-let stop_motors_delay = 0.2
diff --git a/PC_Mainboard/common/config.mli b/PC_Mainboard/common/config.mli
index f43c1eb..645ac10 100644
--- a/PC_Mainboard/common/config.mli
+++ b/PC_Mainboard/common/config.mli
@@ -26,7 +26,3 @@ val update_delay : float
val reopen_delay : float
(** Time to wait before retrying to open a card *)
-
-val stop_motors_delay : float
- (** Amount of time to wait after a set-speed command to stop the
- motors *)
diff --git a/PC_Mainboard/common/types.ml b/PC_Mainboard/common/types.ml
index 60ab4ce..3f970ca 100644
--- a/PC_Mainboard/common/types.ml
+++ b/PC_Mainboard/common/types.ml
@@ -43,10 +43,26 @@ let obus_goto_mode = OBus_type.mapping obus_int
type ax12_stats = {
ax12_position : int;
- ax12_speed : int;
+ ax12_velocity : int;
ax12_torque : int;
ax12_voltage : int;
ax12_temperature : int;
ax12_cw_angle_limit : int;
ax12_ccw_angle_limit : int;
} with obus
+
+type exec_mode = [ `Now | `Action ]
+
+let obus_exec_mode = OBus_type.mapping obus_int
+ [(`Now, 0); (`Action, 1)]
+
+type direction = [ `Forward | `Backward ]
+
+let obus_direction = OBus_type.mapping obus_int
+ [(`Forward, 0); (`Backward, 1)]
+
+type ax12_action = {
+ aa_id : int;
+ aa_position : int;
+ aa_velocity : int;
+} with obus
diff --git a/PC_Mainboard/common/types.mli b/PC_Mainboard/common/types.mli
index 164f85a..6fa04c8 100644
--- a/PC_Mainboard/common/types.mli
+++ b/PC_Mainboard/common/types.mli
@@ -25,18 +25,30 @@ type motor = [ `Left | `Right | `Both ] with obus(basic)
type stop_mode = [ `Off | `Abrupt | `Smooth ] with obus(basic)
(** Mode for stopping motors *)
+type direction = [ `Forward | `Backward ] with obus(basic)
+
type card_state = [ `Present | `Absent ] with obus(basic)
(** State of a card *)
type goto_mode = [ `Straight | `Curve_right | `Curve_left ] with obus(basic)
(** Form of the trajectory for the goto command *)
+type exec_mode = [ `Now | `Action ] with obus(basic)
+ (** Mode of execution of the goto command for ax12 *)
+
type ax12_stats = {
ax12_position : int;
- ax12_speed : int;
+ ax12_velocity : int;
ax12_torque : int;
ax12_voltage : int;
ax12_temperature : int;
ax12_cw_angle_limit : int;
ax12_ccw_angle_limit : int;
} with obus(sequence)
+
+(** Action on an AX12: *)
+type ax12_action = {
+ aa_id : int;
+ aa_position : int;
+ aa_velocity : int;
+} with obus(sequence)
diff --git a/PC_Mainboard/common/util.ml b/PC_Mainboard/common/util.ml
index c40e26f..705623f 100644
--- a/PC_Mainboard/common/util.ml
+++ b/PC_Mainboard/common/util.ml
@@ -7,6 +7,8 @@
* This file is a part of [kro]bot.
*)
+open Lwt
+
let front_collide sensors =
if Array.length sensors <> 16 then invalid_arg "Until.front_collide";
let rec loop = function
@@ -22,3 +24,23 @@ let back_collide sensors =
| n -> (sensors.(n) && List.mem n Config.back_sensors) || loop (n + 1)
in
loop 0
+
+let unexpected_reply name reply =
+ lwt () = Lwt_log.log_f ~level:Lwt_log.Fatal "unexpected reply for request_name(%S): %s" name reply in
+ exit 1
+
+let single_instance bus name =
+ Lwt_event.always_notify_p
+ (fun name ->
+ lwt () = Lwt_log.log ~level:Lwt_log.Notice "service restarted, exiting" in
+ exit 0)
+ (React.E.filter ((=) name) (OBus_bus.name_lost bus)#event);
+ OBus_bus.request_name bus ~allow_replacement:true ~replace_existing:true name >>= function
+ | `Primary_owner ->
+ return ()
+ | `In_queue ->
+ unexpected_reply name "in-queue"
+ | `Exists ->
+ unexpected_reply name "exists"
+ | `Already_owner ->
+ unexpected_reply name "already-owner"
diff --git a/PC_Mainboard/common/util.mli b/PC_Mainboard/common/util.mli
index f652bb8..36ad923 100644
--- a/PC_Mainboard/common/util.mli
+++ b/PC_Mainboard/common/util.mli
@@ -16,3 +16,9 @@ val front_collide : bool array -> bool
val back_collide : bool array -> bool
(** [front_collide sensors] returns whether on of the back sensors is
activated *)
+
+val single_instance : OBus_bus.t -> OBus_name.bus -> unit Lwt.t
+ (** [single_instance bus name] requests the bus name [name], and
+ exit the program when this name is lost. This permit to easily
+ restart the service and ensures that there is only one running
+ instance. *)
diff --git a/PC_Mainboard/driver/RW.ml b/PC_Mainboard/driver/RW.ml
index f7c417a..c43640f 100644
--- a/PC_Mainboard/driver/RW.ml
+++ b/PC_Mainboard/driver/RW.ml
@@ -7,31 +7,75 @@
* This file is a part of [kro]bot.
*)
-let get_uint8 data ofs = Char.code data.[ofs]
-let set_uint8 data ofs v = data.[ofs] <- Char.unsafe_chr v
-let get_int8 = get_uint8
-let set_int8 = set_uint8
-
-let get_int16 data ofs =
- (get_uint8 data ofs lsl 8)
- lor (get_uint8 data (ofs + 1))
-let get_uint16 = get_int16
-
-let set_int16 data ofs v =
- set_uint8 data ofs ((v lsr 8) land 0xff);
- set_uint8 data (ofs + 1) (v land 0xff)
-let set_uint16 = set_int16
-
-let get_int32 data ofs =
- (get_uint8 data ofs lsl 24)
- lor (get_uint8 data (ofs + 1) lsl 16)
- lor (get_uint8 data (ofs + 2) lsl 8)
- lor (get_uint8 data (ofs + 3))
-let get_uint32 = get_int32
-
-let set_int32 data ofs v =
- set_uint8 data ofs ((v lsr 24) land 0xff);
- set_uint8 data (ofs + 1) ((v lsr 16) land 0xff);
- set_uint8 data (ofs + 2) ((v lsr 8) land 0xff);
- set_uint8 data (ofs + 3) (v land 0xff)
-let set_uint32 = set_int32
+type pointer = {
+ mutable offset : int;
+ buffer : string;
+}
+
+type writer = pointer
+type reader = pointer
+
+let writer buffer = { buffer = buffer; offset = 0 }
+let reader buffer = { buffer = buffer; offset = 0 }
+
+let get_uint8 pointer =
+ let offset = pointer.offset in
+ pointer.offset <- offset + 1;
+ Char.code pointer.buffer.[offset]
+let put_uint8 pointer value =
+ let offset = pointer.offset in
+ pointer.offset <- offset + 1;
+ pointer.buffer.[offset] <- Char.unsafe_chr value
+let get_sint8 = get_uint8
+let put_sint8 = put_uint8
+
+let get_sint16 pointer =
+ let v0 = get_uint8 pointer in
+ let v1 = get_uint8 pointer in
+ (v0 lsl 8) lor v1
+let get_uint16 = get_sint16
+
+let put_sint16 pointer value =
+ put_uint8 pointer ((value lsr 8) land 0xff);
+ put_uint8 pointer (value land 0xff)
+let put_uint16 = put_sint16
+
+let get_sint32 pointer =
+ let v0 = get_uint8 pointer in
+ let v1 = get_uint8 pointer in
+ let v2 = get_uint8 pointer in
+ let v3 = get_uint8 pointer in
+ (v0 lsl 24) lor (v1 lsl 16) lor (v2 lsl 8) lor v3
+let get_uint32 = get_sint32
+
+let put_sint32 pointer value =
+ put_uint8 pointer ((value lsr 24) land 0xff);
+ put_uint8 pointer ((value lsr 16) land 0xff);
+ put_uint8 pointer ((value lsr 8) land 0xff);
+ put_uint8 pointer (value land 0xff)
+let put_uint32 = put_sint32
+
+let get_string pointer =
+ let index =
+ try
+ String.index_from pointer.buffer pointer.offset '\000'
+ with Not_found ->
+ String.length pointer.buffer
+ in
+ let offset = pointer.offset in
+ pointer.offset <- index + 1;
+ String.sub pointer.buffer offset (index - offset)
+
+let put_string pointer value =
+ let len = String.length value in
+ if len > String.length pointer.buffer - pointer.offset then
+ invalid_arg "RW.put_string: string too long"
+ else begin
+ String.blit value 0 pointer.buffer pointer.offset len;
+ let offset = pointer.offset + len in
+ if offset < String.length pointer.buffer then begin
+ pointer.buffer.[offset] <- '\x00';
+ pointer.offset <- offset + 1
+ end else
+ pointer.offset <- offset
+ end
diff --git a/PC_Mainboard/driver/RW.mli b/PC_Mainboard/driver/RW.mli
index 4fe39fa..92948c0 100644
--- a/PC_Mainboard/driver/RW.mli
+++ b/PC_Mainboard/driver/RW.mli
@@ -7,20 +7,38 @@
* This file is a part of [kro]bot.
*)
-val get_int8 : string -> int -> int
-val set_int8 : string -> int -> int -> unit
+(** Serialisation/deserialisation *)
-val get_int16 : string -> int -> int
-val set_int16 : string -> int -> int -> unit
+(** {6 Writing} *)
-val get_int32 : string -> int -> int
-val set_int32 : string -> int -> int -> unit
+type writer
-val get_uint8 : string -> int -> int
-val set_uint8 : string -> int -> int -> unit
+val writer : string -> writer
+ (** [writer buffer] creates a writer which writes into [buffer] *)
-val get_uint16 : string -> int -> int
-val set_uint16 : string -> int -> int -> unit
+val put_sint8 : writer -> int -> unit
+val put_sint16 : writer -> int -> unit
+val put_sint32 : writer -> int -> unit
-val get_uint32 : string -> int -> int
-val set_uint32 : string -> int -> int -> unit
+val put_uint8 : writer -> int -> unit
+val put_uint16 : writer -> int -> unit
+val put_uint32 : writer -> int -> unit
+
+val put_string : writer -> string -> unit
+
+(** {6 Reading} *)
+
+type reader
+
+val reader : string -> reader
+ (** [reader buffer] creates a reader which reads from [buffer] *)
+
+val get_sint8 : reader -> int
+val get_sint16 : reader -> int
+val get_sint32 : reader -> int
+
+val get_uint8 : reader -> int
+val get_uint16 : reader -> int
+val get_uint32 : reader -> int
+
+val get_string : reader -> string
diff --git a/PC_Mainboard/driver/card.ml b/PC_Mainboard/driver/card.ml
deleted file mode 100644
index 5ecf347..0000000
--- a/PC_Mainboard/driver/card.ml
+++ /dev/null
@@ -1,350 +0,0 @@
-(*
- * card.ml
- * -------
- * Copyright : (c) 2009, Jeremie Dimino <je...@di...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- *)
-
-module Log = Lwt_log.Make(struct let section = "card" end)
-
-open Lwt
-
-(* +-----------------------------------------------------------------+
- | Messages |
- +-----------------------------------------------------------------+ *)
-
-let data_length = 52
- (* Taille en octet du corps d'un message *)
-
-type serial = int
- (* Type d'un numéro de série d'un message *)
-
-type message = {
- host_serial : serial;
- (* Le numéro de série du message, émis par l'ordinateur. Vaut 0 pour
- les messages émis par le PIC. *)
-
- device_serial : serial;
- (* Le numéro de série du message, émis par le PIC. Vaut 0 pour les
- messages émis par l'ordinateur. *)
-
- command : int;
- (* La commande, en fait c'est plutôt le type du message *)
-
- error : int;
- (* Si c'est un message d'erreur ce flag est non-nul *)
-
- data : string;
- (* Les données du messages, il y a 52 octets. *)
-}
-
-let make_buffer () = String.make data_length '\000'
-
-(* Parse un message depuis un buffer brut: *)
-let parse_message buf = {
- host_serial = Char.code buf.[Protocol.up_hseq];
- device_serial = Char.code buf.[Protocol.up_dseq];
- command = Char.code buf.[Protocol.up_cmd];
- error = Char.code buf.[Protocol.up_err];
- data = String.sub buf Protocol.up_data 52;
-}
-
-(* Créé un buffer brut depuis un message: *)
-let forge_message msg =
- let buf = String.make 64 '\000' in
- buf.[Protocol.up_hseq] <- Char.chr msg.host_serial;
- buf.[Protocol.up_dseq] <- Char.chr msg.device_serial;
- buf.[Protocol.up_cmd] <- Char.chr msg.command;
- buf.[Protocol.up_err] <- Char.chr msg.error;
- if String.length msg.data > 52 then
- Printf.ksprintf invalid_arg "message body too big: %d > 52" (String.length msg.data)
- else begin
- String.blit msg.data 0 buf Protocol.up_data (String.length msg.data);
- buf
- end
-
-(* +-----------------------------------------------------------------+
- | Definitions |
- +-----------------------------------------------------------------+ *)
-
-exception Card_closed
-exception Card_crashed of string
-
-module Int_map = Map.Make(struct type t = int let compare = compare end)
-
-(* Type of a up and running card *)
-type card = {
- mutable serial_pool : serial list;
- (* Pool de serial disponibles, comme il n'y a que 256 serial
- disponibles on évite de juste incrémenter un compteur au pif. *)
-
- mutable reply_waiters : string Lwt.u Int_map.t;
- (* Threads en attente d'une réponse *)
-
- handle : USB.handle;
- (* Handle pour le périphérique usb *)
-
- kernel_active : bool;
- (* Est-ce qu'un driver noyau était attaché à la carte avant qu'on
- l'utilise ? *)
-
- mutex : Lwt_mutex.t;
- (* Mutex pour envoyer des commandes, les cartes n'aiment pas les
- appels parallèles. *)
-
- mutable events : (string -> unit) Lwt_sequence.t Int_map.t;
- (* Connected events *)
-
- abort_waiter : int Lwt.t;
- abort_wakener : int Lwt.u;
- (* Sleeping thread which is wakeup when the card is closed *)
-
- wrapper : wrapper;
- (* The associated wrapper *)
-}
-
-and state =
- | Opened of card
- | Closed of exn
-
-and wrapper = {
- mutable state : state;
- name : string;
- watch : [ `Error of exn | `Closed ] Lwt.t;
-}
-
-type t = wrapper
-
-let name wrapper = wrapper.name
-let closed wrapper = match wrapper.state with
- | Opened _ -> false
- | Closed _ -> true
-
-let watch wrapper = wrapper.watch
-
-(* Return a running card, if possible. *)
-let rec get_card wrapper = match wrapper.state with
- | Opened card ->
- return card
- | Closed exn ->
- fail exn
-
-(* +-----------------------------------------------------------------+
- | Aborting |
- +-----------------------------------------------------------------+ *)
-
-let abort wrapper exn =
- match wrapper.state with
- | Closed exn ->
- return exn
- | Opened card ->
- wrapper.state <- Closed exn;
- try_lwt
- lwt () = USB.release_interface card.handle 0 in
- if card.kernel_active then USB.attach_kernel_driver card.handle 0;
- USB.close card.handle;
- return exn
- finally
- wakeup_exn card.abort_wakener exn;
- Int_map.iter (fun serial w -> wakeup_exn w exn) card.reply_waiters;
- return ()
-
-(* +-----------------------------------------------------------------+
- | Dispatching |
- +-----------------------------------------------------------------+ *)
-
-let dropped typ msg =
- lwt () = Log.warning_f "%s dropped" typ in
- lwt () = Log.warning_f "=====
-host_serial = %d
-device_serial = %d
-command = %d
-error = %d
-data:" msg.host_serial msg.device_serial msg.command msg.error in
- Lwt_stream.iter_s (fun line -> Log.warning line) (Lwt_stream.hexdump (Lwt_stream.of_string msg.data))
-
-(* Dispatch incomming messages continously *)
-let rec dispatch card =
- let buffer = String.create 64 in
- begin
- try_lwt
- select [card.abort_waiter;
- USB.interrupt_recv
- ~handle:card.handle
- ~endpoint:1
- buffer 0 64] >|= fun len -> `OK len
- with exn ->
- return (`Error exn)
- end >>= function
- | `Error exn ->
- lwt () = Log.exn_f exn "stop dispatching on %s card" card.wrapper.name in
- lwt _ = abort card.wrapper exn in
- return ()
- | `OK len ->
- if len <> 64 then begin
- let msg = Printf.sprintf "read on %s card returned %d instead of 64" card.wrapper.name len in
- lwt () = Log.error msg in
- lwt _ = abort card.wrapper (Card_crashed msg) in
- return ()
- end else begin
- let msg = parse_message buffer in
- if msg.command = Protocol.cmd_respond then begin
- match try Some(Int_map.find msg.host_serial card.reply_waiters) with Not_found -> None with
- | Some wakener ->
- card.reply_waiters <- Int_map.remove msg.host_serial card.reply_waiters;
- card.serial_pool <- card.serial_pool @ [msg.host_serial];
- Lwt.wakeup wakener msg.data
- | None ->
- ignore (dropped "response" msg)
- end else begin
- match try Some(Int_map.find msg...
[truncated message content] |
