Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 4895e2f16c55d98053d1849d4ae9a7c7711b89d2
|
From: Jérémie D. <Ba...@us...> - 2010-01-30 16:56:00
|
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 4895e2f16c55d98053d1849d4ae9a7c7711b89d2 (commit)
via cc55bd7091ec232ff6fd2a5cfecf74ce08d894a5 (commit)
from 5255dd22baf3b168c0761660b3dc61d99ec97188 (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 4895e2f16c55d98053d1849d4ae9a7c7711b89d2
Author: Jérémie Dimino <je...@di...>
Date: Sat Jan 30 17:55:32 2010 +0100
allow to specify the host for lib-krobot
commit cc55bd7091ec232ff6fd2a5cfecf74ce08d894a5
Author: Jérémie Dimino <je...@di...>
Date: Sat Jan 30 17:49:11 2010 +0100
handle card accessibility in Card
-----------------------------------------------------------------------
Changes:
diff --git a/PC_Mainboard/clients/lib-krobot/krobot.ml b/PC_Mainboard/clients/lib-krobot/krobot.ml
index 3329f21..cde6b32 100644
--- a/PC_Mainboard/clients/lib-krobot/krobot.ml
+++ b/PC_Mainboard/clients/lib-krobot/krobot.ml
@@ -11,27 +11,34 @@ open OBus_pervasives
open Lwt
lwt bus =
- match (try Sys.getenv "KROBOT" with Not_found -> ""), Unix.gethostname () with
- | "local", _ | _, "mrp" ->
- (* On the robot use the local connection *)
- Lazy.force OBus_bus.system
- | _ ->
- (* Start remove connection if on another computer *)
- try_lwt
- let ssh = Lwt_process.open_process ("ssh", [|"ssh"; "-l"; "olivier"; "mrp.wifi.crans.org"; "/home/olivier/bin/forward_dbus.native"|]) in
- let transport =
- OBus_transport.make
- ~send:(fun msg -> OBus_wire.write_message ssh#stdin msg)
- ~recv:(fun () -> OBus_wire.read_message ssh#stdout)
- ~shutdown:(fun () -> ssh#close >> return ())
- ()
- in
- let connection = OBus_connection.of_transport transport in
- lwt () = OBus_bus.register_connection connection in
- return connection
- with exn ->
- Log#exn exn "failed to create remote transport";
- fail exn
+ let host =
+ try
+ Sys.getenv "KROBOT"
+ with Not_found ->
+ if Unix.gethostname () = "mrp" then
+ "localhost"
+ else
+ "mrp.wifi.crans.org"
+ in
+ if host = "localhost" then
+ Lazy.force OBus_bus.system
+ else begin
+ try_lwt
+ let ssh = Lwt_process.open_process ("ssh", [|"ssh"; "-l"; "olivier"; host; "/home/olivier/bin/forward_dbus.native"|]) in
+ let transport =
+ OBus_transport.make
+ ~send:(fun msg -> OBus_wire.write_message ssh#stdin msg)
+ ~recv:(fun () -> OBus_wire.read_message ssh#stdout)
+ ~shutdown:(fun () -> ssh#close >> return ())
+ ()
+ in
+ let connection = OBus_connection.of_transport transport in
+ lwt () = OBus_bus.register_connection connection in
+ return connection
+ with exn ->
+ Log#exn exn "failed to create remote transport";
+ fail exn
+ end
let driver = OBus_peer.make bus "fr.krobot"
diff --git a/PC_Mainboard/driver/src/card.ml b/PC_Mainboard/driver/src/card.ml
index c781e78..4c2500a 100644
--- a/PC_Mainboard/driver/src/card.ml
+++ b/PC_Mainboard/driver/src/card.ml
@@ -9,7 +9,9 @@
open Lwt
-let fatal fmt = Printf.ksprintf (fun txt -> prerr_endline ("Fatal error: " ^ txt); exit 2) fmt
+(* Log an error message and exit: *)
+let fatal fmt =
+ Printf.ksprintf (fun txt -> Log#fatal "%s" txt; exit 2) fmt
(* +-----------------------------------------------------------------+
| Messages |
@@ -66,12 +68,17 @@ let forge_message msg =
end
(* +-----------------------------------------------------------------+
- | Connections |
+ | Definitions |
+-----------------------------------------------------------------+ *)
+exception Card_closed
+
module SerialMap = Map.Make(struct type t = serial let compare = compare end)
-type t = {
+type state = Running | Opening | Closed
+
+(* 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
@@ -90,89 +97,209 @@ type t = {
mutex : Lwt_mutex.t;
(* Mutex pour envoyer des commandes, les cartes n'aiment pas les
appels parallèles. *)
+}
+
+type wrapper_state =
+ | Ws_running of card
+ | Ws_opening of unit Lwt.t
+ | Ws_closed
+
+type wrapper = {
+ mutable state : wrapper_state;
+
+ public_state : state React.signal;
+ set_public_state : state -> unit;
+ (* The state that is returning to the user *)
+
+ vendor_id : int;
+ product_id : int;
+ (* Informations needed to reopen the card when it crashes *)
- mutable abort : bool;
+ name : string;
}
-let close k =
- lwt _ = USB.release_interface k.handle 0 in
- if k.kernel_active then USB.attach_kernel_driver k.handle 0;
- USB.close k.handle;
+type t = wrapper
+
+let name wrapper = wrapper.name
+
+(* Returns the public state of a wrapper *)
+let state wrapper = wrapper.public_state
+
+(* Set the state of a wrapper *)
+let set_state wrapper state =
+ wrapper.state <- state;
+ wrapper.set_public_state
+ (match state with
+ | Ws_running _ -> Running
+ | Ws_opening _ -> Opening
+ | Ws_closed -> Closed)
+
+(* +-----------------------------------------------------------------+
+ | Opening, closing and dispatching |
+ +-----------------------------------------------------------------+ *)
+
+(* Return a running card, if possible. *)
+let rec get_card wrapper = match wrapper.state with
+ | Ws_running card ->
+ return card
+ | Ws_opening waiter ->
+ lwt () = waiter in
+ get_card wrapper
+ | Ws_closed ->
+ fail Card_closed
+
+let close_card card =
+ 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 ()
-let rec dispatch k =
+let close wrapper = match wrapper.state with
+ | Ws_running card ->
+ set_state wrapper Ws_closed;
+ close_card card
+ | Ws_opening waiter ->
+ set_state wrapper Ws_closed;
+ Lwt.cancel waiter;
+ return ()
+ | Ws_closed ->
+ return ()
+
+(* Retry continuously to open a card until it succeed: *)
+let rec reopen_card wakener wrapper =
+ let rec loop is_first =
+ try_lwt
+ (* USB stuff *)
+ let handle = USB.open_device_with ~vendor_id:wrapper.vendor_id ~product_id:wrapper.product_id in
+ lwt () = USB.reset_device handle in
+ let kernel_active = USB.kernel_driver_active handle 0 in
+ if kernel_active then USB.detach_kernel_driver handle 0;
+ lwt () = USB.set_configuration handle 1 in
+ lwt () = USB.claim_interface handle 0 in
+
+ Log#info "%s card opened" wrapper.name;
+
+ let card = {
+ serial_pool = (let rec loop = function
+ | 256 -> []
+ | n -> n :: loop (n + 1)
+ in
+ loop 1);
+ reply_waiters = SerialMap.empty;
+ handle = handle;
+ kernel_active = kernel_active;
+ mutex = Lwt_mutex.create ();
+ } in
+ set_state wrapper (Ws_running card);
+ ignore (dispatch card);
+ Lwt.wakeup wakener ();
+ return ()
+ with exn ->
+ if is_first then Log#exn exn "failed to open %s card" wrapper.name;
+ lwt () = Lwt_unix.sleep Config.reopen_delay in
+ loop false
+ in
+ loop true
+
+(* Dispatch incomming messages *)
+and dispatch card =
let buffer = String.create 64 in
- lwt len = USB.interrupt_recv ~handle:k.handle ~endpoint:1 buffer 0 64 in
- if len <> 64 then
- fatal "message de moins de 64 octets reçu!"
- else begin
- let msg = parse_message buffer in
- if msg.command = Protocol.cmd_respond then begin
- (* Réponse à un message *)
- match try Some(SerialMap.find msg.host_serial k.reply_waiters) with Not_found -> None with
- | Some (_, w) ->
- k.reply_waiters <- SerialMap.remove msg.host_serial k.reply_waiters;
- k.serial_pool <- k.serial_pool @ [msg.host_serial];
- Lwt.wakeup w msg.data
- | None ->
- ()
- end;
- dispatch k
- end
+ lwt len = USB.interrupt_recv ~handle:card.handle ~endpoint:1 buffer 0 64 in
+ if len <> 64 then fatal "read <> 64!";
+ let msg = parse_message buffer in
+ if msg.command = Protocol.cmd_respond then begin
+ (* Réponse à un message *)
+ match try Some(SerialMap.find msg.host_serial card.reply_waiters) with Not_found -> None with
+ | Some (_, w) ->
+ card.reply_waiters <- SerialMap.remove msg.host_serial card.reply_waiters;
+ card.serial_pool <- card.serial_pool @ [msg.host_serial];
+ Lwt.wakeup w msg.data
+ | None ->
+ ()
+ end;
+ dispatch card
-let open_card ~vendor_id ~product_id =
- let handle = USB.open_device_with ~vendor_id ~product_id in
- lwt _ = USB.reset_device handle in
- let kernel_active = USB.kernel_driver_active handle 0 in
- if kernel_active then USB.detach_kernel_driver handle 0;
- lwt _ = USB.set_configuration handle 1 in
- lwt _ = USB.claim_interface handle 0 in
- let k = { serial_pool = (let rec loop = function
- | 256 -> []
- | n -> n :: loop (n + 1)
- in
- loop 1);
- reply_waiters = SerialMap.empty;
- handle = handle;
- kernel_active = kernel_active;
- mutex = Lwt_mutex.create ();
- abort = false } in
- let _ = Lwt_sequence.add_l (fun _ -> close k) Lwt_main.exit_hooks in
- ignore (dispatch k);
- return k
-
-(* Envoie une commande et attend une réponse: *)
-let send_request k command data =
- if k.abort then fail (Failure "abort") else begin
- let serial = match k.serial_pool with
- | [] ->
- fatal "plus aucun serial disponible!"
- | s :: l ->
- k.serial_pool <- l;
- s
- in
- let (w1, w2) as w = Lwt.wait () in
- k.reply_waiters <- SerialMap.add serial w k.reply_waiters;
- let buffer = forge_message { host_serial = serial;
- device_serial = 0;
- command = command;
- error = 0;
- data = data } in
- Lwt_mutex.with_lock k.mutex
- (fun _ -> USB.interrupt_send ~handle:k.handle ~endpoint:1 buffer 0 64 >> w1)
- end
+let open_card ~name ~vendor_id ~product_id =
+ let waiter, wakener = Lwt.task () in
+ let state, set_state = React.S.create Opening in
+ let wrapper = {
+ state = Ws_opening waiter;
+ public_state = state;
+ set_public_state = set_state;
+ vendor_id = vendor_id;
+ product_id = product_id;
+ name = name;
+ } in
+ ignore (reopen_card wakener wrapper);
+ wrapper
-(* Envoie une commande et attend une commande: *)
-let send_command k command data =
- if k.abort then fail (Failure "abort") else begin
- let buffer = forge_message { host_serial = 0;
- device_serial = 0;
- command = command;
- error = 0;
- data = data } in
- lwt _ = USB.interrupt_send ~handle:k.handle ~endpoint:1 buffer 0 64 in
- return ()
- end
+(* Close and reopen a card: *)
+let restart wrapper =
+ Log#info "restarting %s card" wrapper.name;
+ match wrapper.state with
+ | Ws_opening _ | Ws_closed ->
+ return ()
+ | Ws_running card ->
+ let waiter, wakener = Lwt.task () in
+ set_state wrapper (Ws_opening waiter);
+ lwt () =
+ (* Try to close the card before reopening it: *)
+ try_lwt
+ close_card card
+ with exn ->
+ Log#exn exn "cannot close %s card" wrapper.name;
+ return ()
+ in
+ ignore (reopen_card wakener wrapper);
+ return ()
+
+(* +-----------------------------------------------------------------+
+ | Sending/receiving messages |
+ +-----------------------------------------------------------------+ *)
+
+(* Send a command and wait for the response: *)
+let rec send_request wrapper command data =
+ lwt card = get_card wrapper in
+ let serial = match card.serial_pool with
+ | [] ->
+ fatal "no more serial available!"
+ | s :: l ->
+ card.serial_pool <- l;
+ s
+ in
+ let (waiter, wakener) as w = Lwt.wait () in
+ card.reply_waiters <- SerialMap.add serial w card.reply_waiters;
+ let buffer = forge_message { host_serial = serial;
+ device_serial = 0;
+ command = command;
+ error = 0;
+ data = data } in
+ try_lwt
+ Lwt_mutex.with_lock card.mutex
+ (fun () ->
+ lwt len = USB.interrupt_send ~handle:card.handle ~endpoint:1 buffer 0 64 in
+ if len <> 64 then fatal "write <> 64!";
+ waiter)
+ with exn ->
+ Log#exn exn "write to %s card failed" wrapper.name;
+ lwt () = restart wrapper in
+ send_request wrapper command data
+(* Send a command without waiting for the reply: *)
+let rec send_command wrapper command data =
+ lwt card = get_card wrapper in
+ let buffer = forge_message { host_serial = 0;
+ device_serial = 0;
+ command = command;
+ error = 0;
+ data = data } in
+ try_lwt
+ lwt len = USB.interrupt_send ~handle:card.handle ~endpoint:1 buffer 0 64 in
+ if len <> 64 then fatal "write <> 64!";
+ return ()
+ with exn ->
+ Log#exn exn "write to %s card failed" wrapper.name;
+ lwt () = restart wrapper in
+ send_command wrapper command data
let connect card command = failwith "not implemented"
diff --git a/PC_Mainboard/driver/src/card.mli b/PC_Mainboard/driver/src/card.mli
index 058d664..0e64417 100644
--- a/PC_Mainboard/driver/src/card.mli
+++ b/PC_Mainboard/driver/src/card.mli
@@ -10,12 +10,38 @@
(** Lowlevel card interfaces *)
type t
- (** Type of an opened card *)
+ (** Type of a card *)
-val open_card : vendor_id : int -> product_id : int -> t Lwt.t
- (** Opten the card with given product-id and vendor-id *)
+(** {6 Card state} *)
+
+(** State of a card: *)
+type state =
+ | Running
+ (** The card is up and running *)
+ | Opening
+ (** The card is being opened *)
+ | Closed
+ (** The card has been closed *)
+
+val state : t -> state React.signal
+ (** [state card] is the state of a card, as a reactive value *)
+
+val name : t -> string
+ (** Returns the name of a card *)
+
+(** {6 Card opening/closing} *)
+
+val open_card : name : string -> vendor_id : int -> product_id : int -> t
+ (** Opten the card with given product-id and vendor-id. [name] is
+ used for debug messages. *)
val close : t -> unit Lwt.t
+ (** Close the given card *)
+
+exception Card_closed
+ (** Exception raised when trying to use a closed card *)
+
+(** {6 Sending/receving messages} *)
val make_buffer : unit -> string
(** Creates a new buffer for serialization (with the right size) *)
diff --git a/PC_Mainboard/driver/src/config.ml b/PC_Mainboard/driver/src/config.ml
index e1ee58f..4d5953a 100644
--- a/PC_Mainboard/driver/src/config.ml
+++ b/PC_Mainboard/driver/src/config.ml
@@ -8,3 +8,4 @@
*)
let update_delay = 0.05
+let reopen_delay = 1.0
diff --git a/PC_Mainboard/driver/src/config.mli b/PC_Mainboard/driver/src/config.mli
index 08c5861..614cd16 100644
--- a/PC_Mainboard/driver/src/config.mli
+++ b/PC_Mainboard/driver/src/config.mli
@@ -10,3 +10,5 @@
val update_delay : float
(** Time to wait between updates *)
+val reopen_delay : float
+ (** Time to wait before retrying to open a card *)
diff --git a/PC_Mainboard/driver/src/driver.ml b/PC_Mainboard/driver/src/driver.ml
index 3d67efd..2bcb781 100644
--- a/PC_Mainboard/driver/src/driver.ml
+++ b/PC_Mainboard/driver/src/driver.ml
@@ -16,9 +16,38 @@ open Lwt
| Cards |
+-----------------------------------------------------------------+ *)
-let card_interface = ref None
-let card_sensor = ref None
-let card_motor = ref None
+let card_interface = Card.open_card ~name:"interface" ~vendor_id:Protocol.usb_vid ~product_id:Protocol.usb_pid_robot_interface
+let card_sensor = Card.open_card ~name:"sensor" ~vendor_id:Protocol.usb_vid ~product_id:Protocol.usb_pid_proximity_sensor
+let card_motor = Card.open_card ~name:"motor" ~vendor_id:Protocol.usb_vid ~product_id:Protocol.usb_pid_motor_controller
+
+(* Enable a motor *)
+let motor_enable card =
+ Log#info "enabling motors on %s card" (Card.name card);
+ let data = Card.make_buffer () in
+ RW.set_uint8 data 0 Protocol.motor_enable;
+ RW.set_uint8 data 1 Protocol.motor_both;
+ Card.send_command card Protocol.cmd_motor data
+
+(* Enable motors when a card comes up *)
+let enable_motor_on_running card =
+ let stop = ref ignore in
+ let notifier =
+ Lwt_signal.notify_p
+ (function
+ | Card.Running ->
+ motor_enable card
+ | Card.Opening ->
+ return ()
+ | Card.Closed ->
+ !stop ();
+ return ())
+ (Card.state card)
+ in
+ stop := (fun () -> Lwt_signal.disable notifier)
+
+let () =
+ enable_motor_on_running card_interface;
+ enable_motor_on_running card_motor
(* +-----------------------------------------------------------------+
| Compass |
@@ -28,7 +57,7 @@ module Compass =
struct
type t = {
obus : OBus_object.t;
- card : Card.t option ref;
+ card : Card.t;
mutable data : int;
}
@@ -43,12 +72,8 @@ struct
OL_method Get : int = fun dev -> return dev.data
let get card =
- match !card with
- | Some card ->
- lwt data = Card.send_request card Protocol.get_cmp03_data "" in
- return (RW.get_int16 data 2)
- | None ->
- return 0
+ lwt data = Card.send_request card Protocol.get_cmp03_data "" in
+ return (RW.get_int16 data 2)
let rec loop dev =
lwt data = get dev.card in
@@ -62,14 +87,13 @@ struct
loop dev
let make card path =
- lwt data = get card in
let dev = {
obus = OBus_object.make path;
card = card;
- data = data;
+ data = 0;
} in
ignore (loop dev);
- return dev
+ dev
end
(* +-----------------------------------------------------------------+
@@ -80,7 +104,7 @@ module AX12 =
struct
type t = {
obus : OBus_object.t;
- card : Card.t option ref;
+ card : Card.t;
}
module OBus = OBus_object.Make(struct
@@ -91,17 +115,13 @@ struct
include OBus.MakeInterface(struct let name = "fr.krobot.Device.AX12" end)
let ax12_goto card id pos speed =
- match !card with
- | Some card ->
- let data = Card.make_buffer () in
- RW.set_uint8 data 0 Protocol.ax12_goto;
- RW.set_uint8 data 1 id;
- RW.set_uint16 data 2 pos;
- RW.set_uint16 data 4 speed;
- lwt _ = Card.send_request card Protocol.cmd_ax12 data in
- return ()
- | None ->
- return ()
+ let data = Card.make_buffer () in
+ RW.set_uint8 data 0 Protocol.ax12_goto;
+ RW.set_uint8 data 1 id;
+ RW.set_uint16 data 2 pos;
+ RW.set_uint16 data 4 speed;
+ lwt _ = Card.send_request card Protocol.cmd_ax12 data in
+ return ()
OL_method OpenClaw : unit = fun dev ->
ax12_goto dev.card 1 1023 0
@@ -115,11 +135,10 @@ struct
OL_method CloseCylinder : unit = fun dev ->
ax12_goto dev.card 2 579 0
- let make card path =
- return {
- obus = OBus_object.make path;
- card = card;
- }
+ let make card path = {
+ obus = OBus_object.make path;
+ card = card;
+ }
end
(* +-----------------------------------------------------------------+
@@ -130,7 +149,7 @@ module Elevator =
struct
type t = {
obus : OBus_object.t;
- card : Card.t option ref;
+ card : Card.t;
}
module OBus = OBus_object.Make(struct
@@ -141,17 +160,13 @@ struct
include OBus.MakeInterface(struct let name = "fr.krobot.Device.Elevator" end)
let motor_move card sens speed duration =
- match !card with
- | Some card ->
- let data = Card.make_buffer () in
- RW.set_uint8 data 0 Protocol.motor_move;
- RW.set_uint8 data 1 Protocol.motor_right;
- RW.set_uint8 data 2 (if sens < 0 then 200 else sens);
- RW.set_uint8 data 3 (if speed < 0 then 2000 else speed);
- RW.set_uint32 data 4 duration;
- Card.send_command card Protocol.cmd_motor data
- | None ->
- return ()
+ let data = Card.make_buffer () in
+ RW.set_uint8 data 0 Protocol.motor_move;
+ RW.set_uint8 data 1 Protocol.motor_right;
+ RW.set_uint8 data 2 (if sens < 0 then 200 else sens);
+ RW.set_uint8 data 3 (if speed < 0 then 2000 else speed);
+ RW.set_uint32 data 4 duration;
+ Card.send_command card Protocol.cmd_motor data
OL_method ElevatorUp : int -> int -> unit = fun dev speed delay ->
motor_move dev.card 0 speed delay
@@ -159,11 +174,10 @@ struct
OL_method ElevatorDown : int -> int -> unit = fun dev speed delay ->
motor_move dev.card 1 speed delay
- let make card path =
- return {
- obus = OBus_object.make path;
- card = card;
- }
+ let make card path = {
+ obus = OBus_object.make path;
+ card = card;
+ }
end
(* +-----------------------------------------------------------------+
@@ -174,7 +188,7 @@ module Grip =
struct
type t = {
obus : OBus_object.t;
- card : Card.t option ref;
+ card : Card.t;
}
module OBus = OBus_object.Make(struct
@@ -186,15 +200,11 @@ struct
include OBus.MakeInterface(struct let name = "fr.krobot.Device.Grip" end)
let set_servo_state card angles =
- match !card with
- | Some card ->
- let data = Card.make_buffer () in
- RW.set_uint8 data 0 Protocol.set_servo_state;
- RW.set_uint8 data 1 (List.fold_left (fun acc (s, _) -> acc lor (1 lsl s)) 0 angles);
- List.iter (fun (s, a) -> RW.set_uint8 data (2 + s) a) angles;
- Card.send_command card Protocol.cmd_set data
- | None ->
- return ()
+ let data = Card.make_buffer () in
+ RW.set_uint8 data 0 Protocol.set_servo_state;
+ RW.set_uint8 data 1 (List.fold_left (fun acc (s, _) -> acc lor (1 lsl s)) 0 angles);
+ List.iter (fun (s, a) -> RW.set_uint8 data (2 + s) a) angles;
+ Card.send_command card Protocol.cmd_set data
let left = 1
let right = 2
@@ -209,11 +219,10 @@ struct
lwt () = Lwt_unix.sleep 0.2 in
set_servo_state dev.card [(left, 50)]
- let make card path =
- return {
- obus = OBus_object.make path;
- card = card;
- }
+ let make card path = {
+ obus = OBus_object.make path;
+ card = card;
+ }
end
(* +-----------------------------------------------------------------+
@@ -224,7 +233,7 @@ module LogicSensors =
struct
type t = {
obus : OBus_object.t;
- card : Card.t option ref;
+ card : Card.t;
mutable data : bool array;
}
@@ -239,15 +248,11 @@ struct
OL_method Get : bool array = fun dev -> return dev.data
let get card =
- match !card with
- | Some card ->
- let data = Card.make_buffer () in
- RW.set_uint8 data 0 Protocol.get_tor_state;
- lwt data = Card.send_request card Protocol.cmd_get data in
- let x = RW.get_uint16 data 0 in
- return (Array.init 16 (fun i -> x land (1 lsl i) <> 0))
- | None ->
- return (Array.make 16 false)
+ let data = Card.make_buffer () in
+ RW.set_uint8 data 0 Protocol.get_tor_state;
+ lwt data = Card.send_request card Protocol.cmd_get data in
+ let x = RW.get_uint16 data 0 in
+ return (Array.init 16 (fun i -> x land (1 lsl i) <> 0))
let rec loop dev =
lwt data = get dev.card in
@@ -261,14 +266,13 @@ struct
loop dev
let make card path =
- lwt data = get card in
let dev = {
obus = OBus_object.make path;
card = card;
- data = data;
+ data = Array.create 16 false;
} in
ignore (loop dev);
- return dev
+ dev
end
(* +-----------------------------------------------------------------+
@@ -279,7 +283,7 @@ module RangeFinders =
struct
type t = {
obus : OBus_object.t;
- card : Card.t option ref;
+ card : Card.t;
mutable data : int array;
}
@@ -294,14 +298,10 @@ struct
OL_method Get : int array = fun dev -> return dev.data
let get card =
- match !card with
- | Some card ->
- let data = Card.make_buffer () in
- RW.set_uint8 data 0 Protocol.get_rangefinder_state;
- lwt data = Card.send_request card Protocol.cmd_get data in
- return (Array.init 8 (fun i -> RW.get_int32 data (i * 4)))
- | None ->
- return (Array.create 8 0)
+ let data = Card.make_buffer () in
+ RW.set_uint8 data 0 Protocol.get_rangefinder_state;
+ lwt data = Card.send_request card Protocol.cmd_get data in
+ return (Array.init 8 (fun i -> RW.get_int32 data (i * 4)))
let rec loop dev =
lwt data = get dev.card in
@@ -315,14 +315,13 @@ struct
loop dev
let make card path =
- lwt data = get card in
let dev = {
obus = OBus_object.make path;
card = card;
- data = data;
+ data = Array.create 8 0;
} in
ignore (loop dev);
- return dev
+ dev
end
open OBus_pervasives
@@ -336,7 +335,7 @@ module Motors =
struct
type t = {
obus : OBus_object.t;
- card : Card.t option ref;
+ card : Card.t;
}
module OBus = OBus_object.Make(struct
@@ -347,16 +346,12 @@ struct
include OBus.MakeInterface(struct let name = "fr.krobot.Device.Motors" end)
let move cmd card arg1 arg2 arg3 =
- match !card with
- | Some card ->
- let data = String.create 7 in
- RW.set_uint8 data 0 cmd;
- RW.set_int16 data 1 arg1;
- RW.set_int16 data 3 arg2;
- RW.set_int16 data 5 arg3;
- Card.send_command card Protocol.cmd_traj data
- | None ->
- return ()
+ let data = String.create 7 in
+ RW.set_uint8 data 0 cmd;
+ RW.set_int16 data 1 arg1;
+ RW.set_int16 data 3 arg2;
+ RW.set_int16 data 5 arg3;
+ Card.send_command card Protocol.cmd_traj data
let move_forward = move Protocol.traj_forward
let move_backward = move Protocol.traj_backward
@@ -378,11 +373,10 @@ struct
OL_method Turn : int -> int -> int -> unit
OL_method Move : int -> int -> int -> unit
- let make card path =
- return {
- obus = OBus_object.make path;
- card = card;
- }
+ let make card path = {
+ obus = OBus_object.make path;
+ card = card;
+ }
end
(* +-----------------------------------------------------------------+
@@ -402,17 +396,11 @@ struct
include OBus.MakeInterface(struct let name = "fr.krobot.Manager" end)
- let close_card card = match !card with
- | Some card ->
- Card.close card
- | None ->
- return ()
-
OL_method Shutdown : unit = fun () ->
lwt bus = Lazy.force OBus_bus.system in
- lwt () = close_card card_interface
- and () = close_card card_sensor
- and () = close_card card_motor
+ lwt () = Card.close card_interface
+ and () = Card.close card_sensor
+ and () = Card.close card_motor
and _ = OBus_bus.release_name bus "fr.krobot" in
Lwt.wakeup done_wakener ();
return ()
@@ -422,26 +410,6 @@ end
| Entry point |
+-----------------------------------------------------------------+ *)
-let motor_enable card =
- match !card with
- | Some card ->
- let data = Card.make_buffer () in
- RW.set_uint8 data 0 Protocol.motor_enable;
- RW.set_uint8 data 1 Protocol.motor_both;
- Card.send_command card Protocol.cmd_motor data
- | None ->
- return ()
-
-let open_card store ~vendor_id ~product_id =
- try_lwt
- lwt card = Card.open_card ~vendor_id ~product_id in
- store := Some card;
- return ()
- with exn ->
- Log#exn exn "failed to open card with vendor_id %04x and product_id %04x" vendor_id product_id;
- store := None;
- return ()
-
(* Kill a running driver *)
let kill bus =
OBus_connection.method_call bus
@@ -502,40 +470,24 @@ lwt () =
Lwt_unix.daemonize ~keep_stderr:true ()
end;
- Log#info "opening cards";
- lwt () = open_card card_interface ~vendor_id:Protocol.usb_vid ~product_id:Protocol.usb_pid_robot_interface
- and () = open_card card_sensor ~vendor_id:Protocol.usb_vid ~product_id:Protocol.usb_pid_proximity_sensor
- and () = open_card card_motor ~vendor_id:Protocol.usb_vid ~product_id:Protocol.usb_pid_motor_controller in
-
- Log#info "enabling motors";
- lwt () = motor_enable card_interface
- and () = motor_enable card_motor in
-
- Log#info "creating objects";
+ Log#info "creating and exporting objects";
(* Interface card *)
- lwt compass = Compass.make card_interface [ "fr"; "krobot"; "Devices"; "Compass" ]
- and ax12 = AX12.make card_interface [ "fr"; "krobot"; "Devices"; "AX12" ]
- and elevator = Elevator.make card_interface [ "fr"; "krobot"; "Devices"; "Elevator" ]
- and grip = Grip.make card_interface [ "fr"; "krobot"; "Devices"; "Grip" ]
+ Compass.OBus.export bus (Compass.make card_interface ["fr"; "krobot"; "Devices"; "Compass"]);
+ AX12.OBus.export bus (AX12.make card_interface ["fr"; "krobot"; "Devices"; "AX12"]);
+ Elevator.OBus.export bus (Elevator.make card_interface ["fr"; "krobot"; "Devices"; "Elevator"]);
+ Grip.OBus.export bus (Grip.make card_interface ["fr"; "krobot"; "Devices"; "Grip"]);
(* Sensor card *)
- and logic_sensors = LogicSensors.make card_sensor [ "fr"; "krobot"; "Devices"; "LogicSensors" ]
- and range_finders = RangeFinders.make card_sensor [ "fr"; "krobot"; "Devices"; "RangeFinders" ]
+ LogicSensors.OBus.export bus (LogicSensors.make card_sensor ["fr"; "krobot"; "Devices"; "LogicSensors"]);
+ RangeFinders.OBus.export bus (RangeFinders.make card_sensor ["fr"; "krobot"; "Devices"; "RangeFinders"]);
(* Motor card *)
- and motors = Motors.make card_motor [ "fr"; "krobot"; "Devices"; "Motors" ] in
-
- Log#info "exporting objects";
- Compass.OBus.export bus compass;
- AX12.OBus.export bus ax12;
- Elevator.OBus.export bus elevator;
- Grip.OBus.export bus grip;
- LogicSensors.OBus.export bus logic_sensors;
- RangeFinders.OBus.export bus range_finders;
- Motors.OBus.export bus motors;
+ Motors.OBus.export bus (Motors.make card_motor ["fr"; "krobot"; "Devices"; "Motors"]);
+ (* Internal objects *)
Manager.OBus.export bus ();
+ Log#info "ready, waiting for requests";
done_waiter
end
hooks/post-receive
--
krobot
|
