Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 42199563c1632e7b522d74b53d16628b310d589f
|
From: Jérémie D. <Ba...@us...> - 2010-04-29 15:16: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, master has been updated
via 42199563c1632e7b522d74b53d16628b310d589f (commit)
via 4ca440465d8ca4535c5f698c59745333b3d02af6 (commit)
from 36cc6e499035eb900b2b0ec85e4fc6cd59601fd7 (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 42199563c1632e7b522d74b53d16628b310d589f
Author: Jérémie Dimino <je...@di...>
Date: Thu Apr 29 17:15:43 2010 +0200
various enhancement
commit 4ca440465d8ca4535c5f698c59745333b3d02af6
Author: Jérémie Dimino <je...@di...>
Date: Wed Apr 28 09:11:24 2010 +0200
replace speed 200 by speed 0 for ax12s
-----------------------------------------------------------------------
Changes:
diff --git a/info/control/clients/controller.ml b/info/control/clients/controller.ml
index cf5792f..f038d41 100644
--- a/info/control/clients/controller.ml
+++ b/info/control/clients/controller.ml
@@ -7,8 +7,6 @@
* This file is a part of [kro]bot.
*)
-(* Prints status continuously *)
-
open Lwt
open Lwt_term
open Lwt_read_line
@@ -135,15 +133,16 @@ let rec loop krobot history =
| Service monitoring |
+-----------------------------------------------------------------+ *)
-let services, set_services = React.S.create ~eq:TextSet.equal TextSet.empty
+let services, set_services = React.S.create []
+let set_services l = set_services (List.sort Pervasives.compare l)
let check_services bus =
lwt l = OBus_bus.list_names bus in
- set_services (List.fold_left (fun set name ->
+ set_services (List.fold_left (fun acc name ->
if Text.starts_with name "fr.krobot." then
- TextSet.add (String.sub name 10 (String.length name - 10)) set
+ String.sub name 10 (String.length name - 10) :: acc
else
- set) TextSet.empty l);
+ acc) [] l);
return ()
(* +-----------------------------------------------------------------+
@@ -151,7 +150,8 @@ let check_services bus =
+-----------------------------------------------------------------+ *)
(* Draw the whole screen *)
-let rec draw krobot =
+let draw krobot =
+
let size = React.S.value Lwt_term.size in
let screen = Zone.make ~width:size.columns ~height:size.lines in
@@ -161,6 +161,21 @@ let rec draw krobot =
let line = { blank with style = { blank.style with foreground = line_color } } in
let name_color = lwhite in
+ let driver = List.mem "Driver" (React.S.value services) in
+ lwt devices =
+ if driver then
+ OBus_property.get (Krobot.devices_status krobot)
+ else
+ return {
+ Krobot.dev_compass = false;
+ Krobot.dev_range_finders = false;
+ Krobot.dev_logic_sensors = false;
+ Krobot.dev_motor = false;
+ Krobot.dev_ax12 = false;
+ Krobot.dev_lcd = false;
+ }
+ in
+
(* ===== Borders ===== *)
for i = 1 to size.columns - 2 do
@@ -205,34 +220,36 @@ let rec draw krobot =
let zone = Zone.inner screen in
lwt () =
- try_lwt
+ if driver && devices.Krobot.dev_range_finders then begin
lwt range_finders = OBus_property.get (Krobot.range_finders krobot) in
- for i = 0 to Array.length range_finders - 1 do
+ for i = 0 to 7 do
Draw.textc zone 0 i [textf "%d : " i; text (Text.repeat (range_finders.(i) * 14 / 3146) "=")]
done;
return ()
- with exn ->
+ end else begin
for i = 0 to 7 do
- Draw.textc zone 0 i [fg red; text "error"]
+ Draw.textc zone 0 i [fg red; text "unavailable"]
done;
return ()
+ end
in
lwt () =
- try_lwt
+ if driver && devices.Krobot.dev_logic_sensors then begin
lwt logic_sensors = OBus_property.get (Krobot.logic_sensors krobot) in
- for i = 0 to Array.length logic_sensors / 2 - 1 do
+ for i = 0 to 7 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;
return ()
- with exn ->
+ end else begin
for i = 0 to 7 do
- Draw.textc zone 20 i [fg red; text "error"]
+ Draw.textc zone 20 i [fg red; text "unavailable"]
done;
return ()
+ end
in
let zone' = Zone.sub ~zone ~x:40 ~y:0 ~width:14 ~height:8 in
@@ -243,7 +260,7 @@ let rec draw krobot =
Draw.text ~zone:zone' ~x:0 ~y ~text:name;
loop (y + 1) rest
in
- loop 0 (TextSet.elements (React.S.value services));
+ loop 0 (React.S.value services);
let x = 55 in
let text_of_state name = function
@@ -251,31 +268,34 @@ let rec draw krobot =
| `Present -> [text name]
in
lwt () =
- try_lwt
- lwt interface_state = OBus_property.get (Krobot.Card.state krobot `Interface)
- and sensor_state = OBus_property.get (Krobot.Card.state krobot `Sensor)
- and motor_state = OBus_property.get (Krobot.Card.state krobot `Motor)
- and monitoring_state = OBus_property.get (Krobot.Card.state krobot `Monitoring) in
- Draw.textc zone x 0 (text_of_state "interface" interface_state);
- Draw.textc zone x 1 (text_of_state "sensor" sensor_state);
- Draw.textc zone x 2 (text_of_state "motor" motor_state);
- Draw.textc zone x 3 (text_of_state "monitoring" monitoring_state);
+ if driver then begin
+ lwt card_interface = OBus_property.get (Krobot.Card.state krobot `Interface)
+ and card_sensor = OBus_property.get (Krobot.Card.state krobot `Sensor)
+ and card_motor = OBus_property.get (Krobot.Card.state krobot `Motor)
+ and card_monitoring = OBus_property.get (Krobot.Card.state krobot `Monitoring) in
+ Draw.textc zone x 0 (text_of_state "interface" card_interface);
+ Draw.textc zone x 1 (text_of_state "sensor" card_sensor);
+ Draw.textc zone x 2 (text_of_state "motor" card_motor);
+ Draw.textc zone x 3 (text_of_state "monitoring" card_monitoring);
return ()
- with exn ->
- for i = 0 to 3 do
- Draw.textc zone x i [fg red; text "error"]
- done;
+ end else begin
+ Draw.textc zone x 0 (text_of_state "interface" `Absent);
+ Draw.textc zone x 1 (text_of_state "sensor" `Absent);
+ Draw.textc zone x 2 (text_of_state "motor" `Absent);
+ Draw.textc zone x 3 (text_of_state "monitoring" `Absent);
return ()
+ end
in
let x = x + 12 in
lwt () =
- try_lwt
+ if driver && devices.Krobot.dev_compass then begin
lwt compass = OBus_property.get (Krobot.compass krobot) in
Draw.textf zone x 2 "compass = %d" compass;
return ()
- with exn ->
- Draw.textc zone x 2 [fg red; text "error"];
+ end else begin
+ Draw.textc zone x 2 [fg red; text "unavailable"];
return ()
+ end
in
let date = Unix.gettimeofday () in
let text_of_motor_state mode until =
@@ -285,17 +305,18 @@ let rec draw krobot =
[text mode; text "OK"]
in
lwt () =
- try_lwt
+ if driver && devices.Krobot.dev_motor then begin
lwt forward = OBus_property.get (Krobot.inhibit_forward_until krobot)
and backward = OBus_property.get (Krobot.inhibit_backward_until krobot) in
Draw.textc zone x 3 (text_of_motor_state "move forward: " forward);
Draw.textc zone x 4 (text_of_motor_state "move backward: " backward);
return ()
- with exn ->
+ end else begin
for i = 3 to 4 do
- Draw.textc zone x i [fg red; text "error"]
+ Draw.textc zone x i [fg red; text "unavailable"]
done;
return ()
+ end
in
(* ===== History ===== *)
@@ -399,6 +420,8 @@ let rec draw krobot =
(* Wether the screen need to be refreshed *)
let refresh_needed = ref false
+let draw_mutex = Lwt_mutex.create ()
+
(* Program a refresh before the next main loop iteration *)
let refresh krobot =
if !refresh_needed then
@@ -407,7 +430,7 @@ let refresh krobot =
refresh_needed := true;
lwt () = Lwt.pause () in
refresh_needed := false;
- draw krobot
+ Lwt_mutex.with_lock draw_mutex (fun () -> draw krobot)
end
(* +-----------------------------------------------------------------+
@@ -452,19 +475,35 @@ lwt () =
notify box;
notify logs;
notify services;
+ let notifiers = ref [] in
let notify_property property =
- Lwt_event.always_notify
- (fun _ -> push ())
- (OBus_property.updates property)
+ try_lwt
+ lwt signal = OBus_property.monitor property in
+ notifiers := Lwt_signal.notify (fun _ -> push ()) signal :: !notifiers;
+ return ()
+ with exn ->
+ Lwt_log.error_f ~exn "cannot monitor property"
in
- notify_property (Krobot.compass krobot);
- notify_property (Krobot.logic_sensors krobot);
- notify_property (Krobot.range_finders krobot);
- notify_property (Krobot.inhibit_forward_until krobot);
- notify_property (Krobot.inhibit_backward_until krobot);
- notify_property (Krobot.Card.state krobot `Interface);
- notify_property (Krobot.Card.state krobot `Sensor);
- notify_property (Krobot.Card.state krobot `Motor);
+ Lwt_signal.always_notify_s
+ (function
+ | true ->
+ join [
+ notify_property (Krobot.compass krobot);
+ notify_property (Krobot.logic_sensors krobot);
+ notify_property (Krobot.range_finders krobot);
+ notify_property (Krobot.inhibit_forward_until krobot);
+ notify_property (Krobot.inhibit_backward_until krobot);
+ notify_property (Krobot.Card.state krobot `Interface);
+ notify_property (Krobot.Card.state krobot `Sensor);
+ notify_property (Krobot.Card.state krobot `Motor);
+ notify_property (Krobot.Card.state krobot `Monitoring);
+ notify_property (Krobot.devices_status krobot);
+ ]
+ | false ->
+ List.iter Lwt_signal.disable !notifiers;
+ notifiers := [];
+ return ())
+ (React.S.map (fun services -> List.mem "Driver" services) services);
List.iter
(fun card ->
diff --git a/info/control/clients/script.ml b/info/control/clients/script.ml
index 4649704..f96ea83 100644
--- a/info/control/clients/script.ml
+++ b/info/control/clients/script.ml
@@ -562,14 +562,17 @@ let rec text_of_value typ value = match typ, value with
| _ ->
failwith "Script.text_of_value: types mismatch"
+let convert_name name =
+ String.concat "-" (List.map String.lowercase (OBus_name.split name))
+
let make_command cmd = {
c_args =
List.map2
(fun name typ -> (name, arg_type_of_type typ))
(Value.arg_names cmd.Commands.send)
(Value.C.type_sequence (Value.arg_types cmd.Commands.send));
- c_name = cmd.Commands.name;
- c_path = ["unsafe"; String.lowercase cmd.Commands.section];
+ c_name = convert_name cmd.Commands.name;
+ c_path = ["unsafe"; convert_name cmd.Commands.section];
c_exec =
fun args logger krobot ->
let args =
diff --git a/info/control/common/error.ml b/info/control/common/error.ml
new file mode 100644
index 0000000..5bcb78c
--- /dev/null
+++ b/info/control/common/error.ml
@@ -0,0 +1,15 @@
+(*
+ * error.ml
+ * --------
+ * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+exception Card_unavailable
+
+let () =
+ List.iter (fun (name, exn) -> OBus_error.register name exn) [
+ "fr.krobot.CardUnavailable", Card_unavailable;
+ ]
diff --git a/info/control/common/error.mli b/info/control/common/error.mli
new file mode 100644
index 0000000..b04e2b1
--- /dev/null
+++ b/info/control/common/error.mli
@@ -0,0 +1,13 @@
+(*
+ * error.mli
+ * ---------
+ * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(** Errors that may be reported by the driver *)
+
+exception Card_unavailable
+ (** The card is currently not available *)
diff --git a/info/control/driver/driver.ml b/info/control/driver/driver.ml
index 170f447..9dca6b4 100644
--- a/info/control/driver/driver.ml
+++ b/info/control/driver/driver.ml
@@ -14,15 +14,118 @@ let section = Lwt_log.Section.make "driver"
open Types
open Lwt
-let make_signal ?(update_delay=Config.update_delay) get card =
- lwt value = get card in
- return (React.S.hold value (Lwt_event.from
- (fun () ->
- lwt () = Lwt_unix.sleep update_delay in
- get card)))
+module String_map = Map.Make(String)
(* THe notification mode for all interfaces: *)
-let notify_mode () = OBus_object.notify_update "PropertiesChanged"
+let notify_mode = OBus_object.notify_update "PropertiesChanged"
+
+(* +-----------------------------------------------------------------+
+ | Cards |
+ +-----------------------------------------------------------------+ *)
+
+type card = Export_unsafe.card = {
+ card_card : USBCard.t option React.signal;
+ card_name : string;
+}
+
+let card_interface, set_card_interface =
+ let signal, set = React.S.create ~eq:(==) None in
+ ({ card_card = signal; card_name = "interface" }, set)
+
+let card_monitoring, set_card_monitoring =
+ let signal, set = React.S.create ~eq:(==) None in
+ ({ card_card = signal; card_name = "monitoring" }, set)
+
+let card_sensor, set_card_sensor =
+ let signal, set = React.S.create ~eq:(==) None in
+ ({ card_card = signal; card_name = "sensor" }, set)
+
+let card_motor, set_card_motor =
+ let signal, set = React.S.create ~eq:(==) None in
+ ({ card_card = signal; card_name = "motor" }, set)
+
+let get_card card =
+ match React.S.value card.card_card with
+ | Some card ->
+ card
+ | None ->
+ Printf.ksprintf
+ (OBus_error.raise Error.Card_unavailable)
+ "the %s card is currently unavaiables"
+ card.card_name
+
+(* List of services, by cards *)
+let services = [
+ card_interface, ["Servo"; "Compass"; "AX12"; "LCD"];
+ card_monitoring, ["Power"];
+ card_sensor, ["LogicSensors"; "RangeFinders"];
+ card_motor, ["Motors"];
+]
+
+let devices_status, set_devices_status =
+ React.S.create ~eq:(String_map.equal (=))
+ (List.fold_left
+ (fun map service -> String_map.add service false map)
+ String_map.empty
+ (List.concat (List.map snd services)))
+
+let set_device_status device status =
+ set_devices_status (String_map.add device status (React.S.value devices_status))
+
+(* +-----------------------------------------------------------------+
+ | USB card polling |
+ +-----------------------------------------------------------------+ *)
+
+(* Poll a card forever *)
+let poll card ?(update_delay=Config.update_delay) initial get =
+ let value, set_value = React.S.create initial in
+ let rec loop () =
+ lwt () =
+ match React.S.value card.card_card with
+ | Some usb_card -> begin
+ try_lwt
+ lwt x = get usb_card in
+ set_value x;
+ return ()
+ with exn ->
+ lwt () = Lwt_log.error_f ~exn "reading from card %s failed with" card.card_name in
+ return ()
+ end
+ | None ->
+ return ()
+ in
+ lwt () = Lwt_unix.sleep update_delay in
+ loop ()
+ in
+ ignore (loop ());
+ value
+
+(* +-----------------------------------------------------------------+
+ | Service template |
+ +-----------------------------------------------------------------+ *)
+
+module Service(Name : sig val name : string end) =
+struct
+ let card =
+ fst (List.find (function (card, l) -> List.mem Name.name l) services)
+
+ let obus =
+ OBus_object.make
+ ~interfaces:[Export_unsafe.interface Name.name card]
+ ["fr"; "krobot"; "Devices"; Name.name]
+
+ let () =
+ OBus_object.attach obus ()
+
+ let () =
+ Lwt_signal.always_notify
+ (function
+ | Some _ ->
+ set_device_status Name.name true
+ | None ->
+ set_device_status Name.name false)
+ card.card_card
+end
(* +-----------------------------------------------------------------+
| Power |
@@ -30,21 +133,7 @@ let notify_mode () = OBus_object.notify_update "PropertiesChanged"
module Power =
struct
- type t = {
- obus : t OBus_object.t;
- card : USBCard.t;
- }
-
- let make card path =
- let dev = {
- obus =
- OBus_object.make
- ~interfaces:[Export_unsafe.interface "power" (fun dev -> dev.card)]
- path;
- card = card;
- } in
- OBus_object.attach dev.obus dev;
- return dev
+ include Service(struct let name = "Power" end)
end
(* +-----------------------------------------------------------------+
@@ -53,31 +142,18 @@ end
module Compass =
struct
- type t = {
- obus : t OBus_object.t;
- card : USBCard.t;
- value : int React.signal;
- }
+ include Service(struct let name = "Compass" end)
open Krobot_interfaces.Fr_krobot_Device_Compass
- let make card path =
- lwt value = make_signal ~update_delay:(Config.update_delay *. 2.) (fun card -> snd =|< USBCard.call Commands.Compass.get card ()) card in
- let dev = {
- obus =
- OBus_object.make
- ~interfaces:[Export_unsafe.interface "compass" (fun dev -> dev.card);
- Krobot_interfaces.Fr_krobot_Device_Compass.make
- ~notify_mode:(OBus_object.notify_update "PropertiesChanged")
- {
- p_Value = (fun dev -> React.S.map Int32.of_int dev.value);
- }]
- path;
- card = card;
- value = value;
- } in
- OBus_object.attach dev.obus dev;
- return dev
+ let value = poll ~update_delay:1.0 card 0 (fun card -> snd =|< USBCard.call Commands.Compass.get card ())
+
+ let () =
+ OBus_object.add_interface obus
+ (make ~notify_mode
+ {
+ p_Value = (fun () -> React.S.map Int32.of_int value);
+ })
end
(* +-----------------------------------------------------------------+
@@ -86,49 +162,37 @@ end
module LCD =
struct
- type t = {
- obus : t OBus_object.t;
- card : USBCard.t;
- }
+ include Service(struct let name = "LCD" end)
open Krobot_interfaces.Fr_krobot_Device_LCD
- let set_lcd dev lines =
+ let set_lcd lines =
if List.length lines > 4 || List.exists (fun line -> String.length line > 20) lines then
invalid_arg "SetLCD"
else begin
- lwt () = USBCard.call Commands.LCD.clear dev.card () in
- lwt () = USBCard.call Commands.LCD.cursor_off dev.card () in
+ lwt () = USBCard.call Commands.LCD.clear (get_card card) () in
+ lwt () = USBCard.call Commands.LCD.cursor_off (get_card card) () in
let rec loop i = function
| [] ->
return ()
| line :: lines ->
- lwt () = USBCard.call Commands.LCD.write_line dev.card (i, line) in
+ lwt () = USBCard.call Commands.LCD.write_line (get_card card) (i, line) in
loop (i + 1) lines
in
loop 1 lines
end
- let backlight_on dev = USBCard.call Commands.LCD.backlight_on dev.card ()
- let backlight_off dev = USBCard.call Commands.LCD.backlight_off dev.card ()
-
- let make card path =
- let dev = {
- obus =
- OBus_object.make
- ~interfaces:[Export_unsafe.interface "LCD" (fun dev -> dev.card);
- Krobot_interfaces.Fr_krobot_Device_LCD.make
- ~notify_mode:(OBus_object.notify_update "PropertiesChanged")
- {
- m_SetLCD = (fun ctx -> set_lcd);
- m_BacklightOn = (fun ctx obj () -> backlight_on obj);
- m_BacklightOff = (fun ctx obj () -> backlight_off obj);
- }]
- path;
- card = card;
- } in
- OBus_object.attach dev.obus dev;
- return dev
+ let backlight_on () = USBCard.call Commands.LCD.backlight_on (get_card card) ()
+ let backlight_off () = USBCard.call Commands.LCD.backlight_off (get_card card) ()
+
+ let () =
+ OBus_object.add_interface obus
+ (make ~notify_mode
+ {
+ m_SetLCD = (fun ctx () lines -> set_lcd lines);
+ m_BacklightOn = (fun ctx () () -> backlight_on ());
+ m_BacklightOff = (fun ctx () () -> backlight_off ());
+ })
end
(* +-----------------------------------------------------------------+
@@ -137,47 +201,35 @@ end
module Servo =
struct
- type t = {
- obus : t OBus_object.t;
- card : USBCard.t;
- }
+ include Service(struct let name = "Servo" end)
open Krobot_interfaces.Fr_krobot_Device_Servo
- let claws_enable dev =
- USBCard.call Commands.Servo.set_config dev.card (0b10100, 0)
+ let claws_enable () =
+ USBCard.call Commands.Servo.set_config (get_card card) (0b10100, 0)
- let claws_disable dev =
- USBCard.call Commands.Servo.set_config dev.card (0, 0xff)
+ let claws_disable () =
+ USBCard.call Commands.Servo.set_config (get_card card) (0, 0xff)
- let claws_open dev =
- USBCard.call Commands.Servo.set_state dev.card (0b10100, 0, 0, 18, 0, -69)
+ let claws_open () =
+ USBCard.call Commands.Servo.set_state (get_card card) (0b10100, 0, 0, 18, 0, -69)
- let claws_close dev =
- USBCard.call Commands.Servo.set_state dev.card (0b10100, 0, 0, -100, 0, 45)
+ let claws_close () =
+ USBCard.call Commands.Servo.set_state (get_card card) (0b10100, 0, 0, -100, 0, 45)
- let claws_take dev =
- USBCard.call Commands.Servo.set_state dev.card (0b10100, 0, 0, -40, 0, -20)
+ let claws_take () =
+ USBCard.call Commands.Servo.set_state (get_card card) (0b10100, 0, 0, -40, 0, -20)
- let make card path =
- let dev = {
- obus =
- OBus_object.make
- ~interfaces:[Export_unsafe.interface "servo" (fun dev -> dev.card);
- Krobot_interfaces.Fr_krobot_Device_Servo.make
- ~notify_mode:(OBus_object.notify_update "PropertiesChanged")
- {
- m_ClawsEnable = (fun ctx obj () -> claws_enable obj);
- m_ClawsDisable = (fun ctx obj () -> claws_disable obj);
- m_ClawsOpen = (fun ctx obj () -> claws_open obj);
- m_ClawsClose = (fun ctx obj () -> claws_close obj);
- m_ClawsTake = (fun ctx obj () -> claws_take obj);
- }]
- path;
- card = card;
- } in
- OBus_object.attach dev.obus dev;
- return dev
+ let () =
+ OBus_object.add_interface obus
+ (make ~notify_mode
+ {
+ m_ClawsEnable = (fun ctx () () -> claws_enable ());
+ m_ClawsDisable = (fun ctx () () -> claws_disable ());
+ m_ClawsOpen = (fun ctx () () -> claws_open ());
+ m_ClawsClose = (fun ctx () () -> claws_close ());
+ m_ClawsTake = (fun ctx () () -> claws_take ());
+ })
end
(* +-----------------------------------------------------------------+
@@ -186,17 +238,10 @@ end
module AX12 =
struct
- type t = {
- obus : t OBus_object.t;
- card : USBCard.t;
- }
+ include Service(struct let name = "AX12" end)
open Krobot_interfaces.Fr_krobot_Device_AX12
- let grip_up_position = 880
- let grip_down_position = 580
- let ax12_default_velocity = 50
-
(* +---------------------------------------------------------------+
| High-level commands |
+---------------------------------------------------------------+ *)
@@ -207,88 +252,78 @@ struct
aa_velocity : int;
}
- let set_ax12 dev actions =
+ let set_ax12 actions =
lwt () =
Lwt_list.iter_p
(fun action ->
USBCard.call Commands.AX12.goto
- dev.card
+ (get_card card)
(action.aa_id,
action.aa_position,
action.aa_velocity,
`Action))
actions
in
- USBCard.call Commands.AX12.action dev.card 0xfe
-
- let grip_up dev =
- set_ax12 dev [{ aa_id = 1;
- aa_position = 190;
- aa_velocity = 50 };
- { aa_id = 2;
- aa_position = 180;
- aa_velocity = 50 }]
-
- let grip_down dev =
- set_ax12 dev [{ aa_id = 1;
- aa_position = 490;
- aa_velocity = 50 };
- { aa_id = 2;
- aa_position = 510;
- aa_velocity = 50 };
- { aa_id = 3;
- aa_position = 390;
- aa_velocity = 200 }]
-
- let grip_open dev =
- set_ax12 dev [{ aa_id = 2;
- aa_position = 650;
- aa_velocity = 100 };
- { aa_id = 3;
- aa_position = 0;
- aa_velocity = 0 }]
-
- let grip_close dev =
- set_ax12 dev [{ aa_id = 2;
- aa_position = 510;
- aa_velocity = 100 };
- { aa_id = 3;
- aa_position = 390;
- aa_velocity = 200 }]
-
- let grip_release dev =
- USBCard.call Commands.AX12.goto dev.card (3, 200, 200, `Now)
-
- let make card path =
- let dev = {
- obus =
- OBus_object.make
- ~interfaces:[Export_unsafe.interface "AX12" (fun dev -> dev.card);
- Krobot_interfaces.Fr_krobot_Device_AX12.make
- ~notify_mode:(OBus_object.notify_update "PropertiesChanged")
- {
- m_SetAX12 = (fun ctx obj positions ->
- let positions =
- List.map
- (fun (x1, x2, x3) ->
- { aa_id = Int32.to_int x1;
- aa_position = Int32.to_int x2;
- aa_velocity = Int32.to_int x3 })
- positions
- in
- set_ax12 obj positions);
- m_GripUp = (fun ctx obj () -> grip_up obj);
- m_GripDown = (fun ctx obj () -> grip_down obj);
- m_GripOpen = (fun ctx obj () -> grip_open obj);
- m_GripClose = (fun ctx obj () -> grip_close obj);
- m_GripRelease = (fun ctx obj () -> grip_release obj);
- }]
- path;
- card = card;
- }
- in
- OBus_object.attach dev.obus dev;
- return dev
+ USBCard.call Commands.AX12.action (get_card card) 0xfe
+
+ let grip_up () =
+ set_ax12 [{ aa_id = 1;
+ aa_position = 190;
+ aa_velocity = 50 };
+ { aa_id = 2;
+ aa_position = 180;
+ aa_velocity = 50 }]
+
+ let grip_down () =
+ set_ax12 [{ aa_id = 1;
+ aa_position = 490;
+ aa_velocity = 50 };
+ { aa_id = 2;
+ aa_position = 510;
+ aa_velocity = 50 };
+ { aa_id = 3;
+ aa_position = 390;
+ aa_velocity = 0 }]
+
+ let grip_open () =
+ set_ax12 [{ aa_id = 2;
+ aa_position = 650;
+ aa_velocity = 100 };
+ { aa_id = 3;
+ aa_position = 0;
+ aa_velocity = 0 }]
+
+ let grip_close () =
+ set_ax12 [{ aa_id = 2;
+ aa_position = 510;
+ aa_velocity = 100 };
+ { aa_id = 3;
+ aa_position = 390;
+ aa_velocity = 0 }]
+
+ let grip_release () =
+ USBCard.call Commands.AX12.goto (get_card card) (3, 200, 0, `Now)
+
+ let () =
+ OBus_object.add_interface obus
+ (make ~notify_mode
+ {
+ m_SetAX12 = (fun ctx () positions ->
+ let positions =
+ List.map
+ (fun (x1, x2, x3) ->
+ { aa_id = Int32.to_int x1;
+ aa_position = Int32.to_int x2;
+ aa_velocity = Int32.to_int x3 })
+ positions
+ in
+ set_ax12 positions);
+ m_GripUp = (fun ctx () () -> grip_up ());
+ m_GripDown = (fun ctx () () -> grip_down ());
+ m_GripOpen = (fun ctx () () -> grip_open ());
+ m_GripClose = (fun ctx () () -> grip_close ());
+ m_GripRelease = (fun ctx () () -> grip_release ());
+ })
end
(* +-----------------------------------------------------------------+
@@ -297,31 +332,18 @@ end
module Logic_sensors =
struct
- type t = {
- obus : t OBus_object.t;
- card : USBCard.t;
- value : bool array React.signal;
- }
+ include Service(struct let name = "LogicSensors" end)
open Krobot_interfaces.Fr_krobot_Device_LogicSensors
- let make card path =
- lwt value = make_signal (fun card -> USBCard.call Commands.Logic_sensors.get_state card ()) card in
- let dev = {
- obus =
- OBus_object.make
- ~interfaces:[Export_unsafe.interface "logic-sensors" (fun dev -> dev.card);
- Krobot_interfaces.Fr_krobot_Device_LogicSensors.make
- ~notify_mode:(OBus_object.notify_update "PropertiesChanged")
- {
- p_Value = (fun dev -> React.S.map Array.to_list dev.value);
- }]
- path;
- card = card;
- value = value;
- } in
- OBus_object.attach dev.obus dev;
- return dev
+ let value = poll card (Array.make 16 false) (fun card -> USBCard.call Commands.Logic_sensors.get_state card ())
+
+ let () =
+ OBus_object.add_interface obus
+ (make ~notify_mode
+ {
+ p_Value = (fun () -> React.S.map Array.to_list value);
+ })
end
(* +-----------------------------------------------------------------+
@@ -330,51 +352,38 @@ end
module Range_finders =
struct
- type t = {
- obus : t OBus_object.t;
- card : USBCard.t;
- value : int array React.signal;
- }
+ include Service(struct let name = "RangeFinders" end)
open Krobot_interfaces.Fr_krobot_Device_RangeFinders
- let get_calibration dev id =
- USBCard.call Commands.Range_finders.get_calibration dev.card id
-
- let calibration_start dev id skip_meas =
- USBCard.call Commands.Range_finders.calibration_start dev.card (id, skip_meas)
-
- let calibration_stop dev =
- USBCard.call Commands.Range_finders.calibration_stop dev.card ()
-
- let calibration_continue dev =
- USBCard.call Commands.Range_finders.calibration_continue dev.card ()
-
- let make card path =
- lwt value = make_signal (fun card -> USBCard.call Commands.Range_finders.get_state card ()) card in
- let dev = {
- obus =
- OBus_object.make
- ~interfaces:[Export_unsafe.interface "range-finders" (fun dev -> dev.card);
- Krobot_interfaces.Fr_krobot_Device_RangeFinders.make
- ~notify_mode:(OBus_object.notify_update "PropertiesChanged")
- {
- p_Value = (fun dev -> React.S.map (fun a -> List.map Int32.of_int (Array.to_list a)) dev.value);
- m_GetCalibration = (fun ctx obj id ->
- lwt result = get_calibration obj (Int32.to_int id) in
- return (List.map Int32.of_int (Array.to_list result)));
- m_CalibrationStart = (fun ctx obj (id, skip_measure) ->
- let id = Int32.to_int id in
- calibration_start obj id skip_measure);
- m_CalibrationStop = (fun ctx obj () -> calibration_stop obj);
- m_CalibrationContinue = (fun ctx obj () -> calibration_continue obj);
- }]
- path;
- card = card;
- value = value;
- } in
- OBus_object.attach dev.obus dev;
- return dev
+ let get_calibration id =
+ USBCard.call Commands.Range_finders.get_calibration (get_card card) id
+
+ let calibration_start id skip_meas =
+ USBCard.call Commands.Range_finders.calibration_start (get_card card) (id, skip_meas)
+
+ let calibration_stop () =
+ USBCard.call Commands.Range_finders.calibration_stop (get_card card) ()
+
+ let calibration_continue () =
+ USBCard.call Commands.Range_finders.calibration_continue (get_card card) ()
+
+ let value = poll card (Array.make 8 0) (fun card -> USBCard.call Commands.Range_finders.get_state card ())
+
+ let () =
+ OBus_object.add_interface obus
+ (make ~notify_mode
+ {
+ p_Value = (fun () -> React.S.map (fun x -> List.map Int32.of_int (Array.to_list x)) value);
+ m_GetCalibration = (fun ctx () id ->
+ lwt result = get_calibration (Int32.to_int id) in
+ return (List.map Int32.of_int (Array.to_list result)));
+ m_CalibrationStart = (fun ctx () (id, skip_measure) ->
+ let id = Int32.to_int id in
+ calibration_start id skip_measure);
+ m_CalibrationStop = (fun ctx () () -> calibration_stop ());
+ m_CalibrationContinue = (fun ctx () () -> calibration_continue ());
+ })
end
(* +-----------------------------------------------------------------+
@@ -383,6 +392,8 @@ end
module Motors =
struct
+ include Service(struct let name = "Motors" end)
+
open Krobot_interfaces.Fr_krobot_Device_Motors
type trajectory = {
@@ -410,27 +421,45 @@ struct
(* Manual settings. The first argument is a threads which
suspend itself, then resume and stop the motors *)
- type t = {
- obus : t OBus_object.t;
- card : USBCard.t;
+ let state = Var.create Static
+ (* The current state of the two motors *)
- inhibit_forward_until : float Var.t;
- inhibit_backward_until : float Var.t;
- (* Date after which motor's inhibition should be stopped *)
+ let () =
+ (* Reset the state each the card goes down *)
+ Lwt_signal.always_notify
+ (fun _ -> Var.set state Static)
+ card.card_card
- traj_completed : (int * string) React.event;
- (* Event which occurs each time a TRAJ_COMPLETED command is
- received *)
+ let inhibit_forward_until = Var.create 0.0
+ let inhibit_backward_until = Var.create 0.0
+ (* Date after which motor's inhibition should be stopped *)
- state : state Var.t;
- (* The current state of the two motors *)
- }
+ (* Event which occurs each time a TRAJ_COMPLETED command is
+ received *)
+ let traj_completed =
+ let event_of_card card =
+ React.E.filter
+ (fun (cmd, data) ->
+ cmd = PcInterface.cmd_traj
+ && Char.code data.[0] = PcInterface.traj_completed)
+ (USBCard.commands card)
+ in
+ React.E.switch
+ (match React.S.value card.card_card with
+ | Some card -> event_of_card card
+ | None -> React.E.never)
+ (React.E.fmap
+ (function
+ | Some card -> Some(event_of_card card)
+ | None -> None)
+ (React.S.changes card.card_card))
let string_of_direction = function
| `Forward -> "forward"
| `Backward -> "backward"
- let state dev =
+ (* String describing the current state *)
+ let state_info =
React.S.map
(function
| Static ->
@@ -448,18 +477,18 @@ struct
Printf.sprintf "manual: left={ direction=%s; velocity=%d; acceleration=%d } right={ direction=%s; velocity=%d; acceleration=%d }"
(string_of_direction settings_l.direction) settings_l.velocity settings_l.acceleration
(string_of_direction settings_r.direction) settings_r.velocity settings_r.acceleration)
- (Var.signal dev.state)
+ (Var.signal state)
(* +---------------------------------------------------------------+
| High-level movement |
+---------------------------------------------------------------+ *)
- let move dev dist velocity acc =
+ let move dist velocity acc =
lwt () =
Lwt_log.info_f ~section "move: distance=%d velocity=%d acceleration=%d"
dist velocity acc
in
- match Var.get dev.state with
+ match Var.get state with
| Trajectory _ ->
lwt () = Lwt_log.info ~section "move: state=trajectory" in
fail (Failure "already in a trajectory")
@@ -469,7 +498,7 @@ struct
| Static ->
lwt () = Lwt_log.info ~section "move: state=static" in
let date = Unix.gettimeofday () in
- if (dist > 0 && date < Var.get dev.inhibit_forward_until) || (dist < 0 && date < Var.get dev.inhibit_backward_until) then
+ if (dist > 0 && date < Var.get inhibit_forward_until) || (dist < 0 && date < Var.get inhibit_backward_until) then
fail (Failure "inhibited move")
else begin
let waiter, wakener = Lwt.wait () in
@@ -478,28 +507,28 @@ struct
trajectory = if dist > 0 then `Forward else `Backward;
stopped = false
} in
- Var.set dev.state (Trajectory trajectory);
- let thread = Lwt_event.next dev.traj_completed >> return () in
+ Var.set state (Trajectory trajectory);
+ let thread = Lwt_event.next traj_completed >> return () in
lwt () =
pick [waiter;
(lwt () =
if dist > 0 then
- USBCard.call Commands.Motors.forward dev.card (dist, velocity, acc)
+ USBCard.call Commands.Motors.forward (get_card card) (dist, velocity, acc)
else
- USBCard.call Commands.Motors.backward dev.card (-dist, velocity, acc)
+ USBCard.call Commands.Motors.backward (get_card card) (-dist, velocity, acc)
in
thread)]
in
- Var.set dev.state Static;
+ Var.set state Static;
return (if trajectory.stopped then `Stopped else `OK)
end
- let turn dev angle velocity acc =
+ let turn angle velocity acc =
lwt () =
Lwt_log.info_f ~section "turn: angle=%d velocity=%d acceleration=%d"
angle velocity acc
in
- match Var.get dev.state with
+ match Var.get state with
| Trajectory _ ->
lwt () = Lwt_log.info ~section "turn: state=trajectory" in
fail (Failure "already in a trajectory")
@@ -514,19 +543,19 @@ struct
trajectory = if angle > 0 then `Left else `Right;
stopped = false
} in
- Var.set dev.state (Trajectory trajectory);
- let thread = Lwt_event.next dev.traj_completed >> return () in
+ Var.set state (Trajectory trajectory);
+ let thread = Lwt_event.next traj_completed >> return () in
lwt () =
pick [waiter;
(lwt () =
if angle > 0 then
- USBCard.call Commands.Motors.left dev.card (angle, velocity, acc)
+ USBCard.call Commands.Motors.left (get_card card) (angle, velocity, acc)
else
- USBCard.call Commands.Motors.right dev.card (-angle, velocity, acc)
+ USBCard.call Commands.Motors.right (get_card card) (-angle, velocity, acc)
in
thread)]
in
- Var.set dev.state Static;
+ Var.set state Static;
return (if trajectory.stopped then `Stopped else `OK)
let string_of_goto_mode = function
@@ -534,12 +563,12 @@ struct
| `Curve_right -> "curve-right"
| `Straight -> "straight"
- let goto dev x y velocity acc mode bypass_distance =
+ let goto x y velocity acc mode bypass_distance =
lwt () =
Lwt_log.info_f ~section "goto: x=%d y=%d velocity=%d acceleration=%d mode=%s bypass_distance=%d"
x y velocity acc (string_of_goto_mode mode) bypass_distance
in
- match Var.get dev.state with
+ match Var.get state with
| Trajectory _ ->
lwt () = Lwt_log.info ~section "goto: state=trajectory" in
fail (Failure "already in a trajectory")
@@ -549,7 +578,7 @@ struct
| Static ->
lwt () = Lwt_log.info ~section "goto: state=static" in
let date = Unix.gettimeofday () in
- if date < Var.get dev.inhibit_forward_until then
+ if date < Var.get inhibit_forward_until then
fail (Failure "inhibited move")
else begin
let waiter, wakener = Lwt.wait () in
@@ -558,17 +587,17 @@ struct
trajectory = `Goto;
stopped = false
} in
- Var.set dev.state (Trajectory trajectory);
- let thread = Lwt_event.next dev.traj_completed >> return () in
+ Var.set state (Trajectory trajectory);
+ let thread = Lwt_event.next traj_completed >> return () in
lwt () =
pick [waiter;
(lwt () =
- USBCard.call Commands.Motors.goto dev.card
+ USBCard.call Commands.Motors.goto (get_card card)
(x, y, velocity, acc, mode, bypass_distance)
in
thread)]
in
- Var.set dev.state Static;
+ Var.set state Static;
return (if trajectory.stopped then `Stopped else `OK)
end
@@ -581,158 +610,140 @@ struct
| `Smooth -> "smooth"
| `Off -> "off"
- let stop_motors dev mode =
+ let stop_motors mode =
lwt () = Lwt_log.info_f ~section "stop-motors: mode=%s" (string_of_stop_mode mode) in
- match Var.get dev.state with
+ match Var.get state with
| Trajectory trajectory ->
lwt () = Lwt_log.info ~section "stop-motors: state=trajectory" in
trajectory.stopped <- true;
- lwt () = USBCard.call Commands.Motors.traj_stop dev.card (`Both, mode) in
+ lwt () = USBCard.call Commands.Motors.traj_stop (get_card card) (`Both, mode) in
wakeup trajectory.abort ();
return ()
| Manual(stopper, left, right) ->
lwt () = Lwt_log.info ~section "stop-motors: state=manual" in
- lwt () = USBCard.call Commands.Motors.traj_stop dev.card (`Both, mode) in
- Var.set dev.state Static;
+ lwt () = USBCard.call Commands.Motors.traj_stop (get_card card) (`Both, mode) in
+ Var.set state Static;
cancel stopper;
return ()
| Static ->
lwt () = Lwt_log.info ~section "stop-motors: state=static" in
- USBCard.call Commands.Motors.traj_stop dev.card (`Both, mode)
+ USBCard.call Commands.Motors.traj_stop (get_card card) (`Both, mode)
- let _set_velocities dev stopper (settings_l, settings_r) (velocity_l, velocity_r) (acceleration_l, acceleration_r) duration =
+ let _set_velocities stopper (settings_l, settings_r) (velocity_l, velocity_r) (acceleration_l, acceleration_r) duration =
let direction_l, velocity_l = if velocity_l < 0 then (`Backward, -velocity_l) else (`Forward, velocity_l) in
let direction_r, velocity_r = if velocity_r < 0 then (`Backward, -velocity_r) else (`Forward, velocity_r) in
let date = Unix.gettimeofday () in
- if (((direction_l = `Forward && velocity_l > 0) || (direction_r = `Forward && velocity_r > 0)) && date < Var.get dev.inhibit_forward_until)
- || (((direction_l = `Backward && velocity_l > 0) || (direction_r = `Backward && velocity_r > 0)) && date < Var.get dev.inhibit_backward_until) then
+ if (((direction_l = `Forward && velocity_l > 0) || (direction_r = `Forward && velocity_r > 0)) && date < Var.get inhibit_forward_until)
+ || (((direction_l = `Backward && velocity_l > 0) || (direction_r = `Backward && velocity_r > 0)) && date < Var.get inhibit_backward_until) then
fail (Failure "inhibited move")
else begin
cancel stopper;
if velocity_l <> 0 || velocity_r <> 0 then begin
- Var.set dev.state (Manual(Lwt_unix.sleep duration >> stop_motors dev `Smooth,
+ Var.set state (Manual(Lwt_unix.sleep duration >> stop_motors `Smooth,
{ velocity = velocity_l; acceleration = acceleration_l; direction = direction_l },
{ velocity = velocity_r; acceleration = acceleration_r; direction = direction_r }));
lwt () =
if acceleration_l <> settings_l.acceleration then
- USBCard.call Commands.Motors.traj_new_velocity dev.card (`Left, velocity_l, acceleration_l, direction_l)
+ USBCard.call Commands.Motors.traj_new_velocity (get_card card) (`Left, velocity_l, acceleration_l, direction_l)
else
- USBCard.call Commands.Motors.traj_change_velocity dev.card (`Left, velocity_l, direction_l)
+ USBCard.call Commands.Motors.traj_change_velocity (get_card card) (`Left, velocity_l, direction_l)
and () =
if acceleration_r <> settings_r.acceleration then
- USBCard.call Commands.Motors.traj_new_velocity dev.card (`Right, velocity_r, acceleration_r, direction_r)
+ USBCard.call Commands.Motors.traj_new_velocity (get_card card) (`Right, velocity_r, acceleration_r, direction_r)
else
- USBCard.call Commands.Motors.traj_change_velocity dev.card (`Right, velocity_r, direction_r)
+ USBCard.call Commands.Motors.traj_change_velocity (get_card card) (`Right, velocity_r, direction_r)
in
- USBCard.call Commands.Motors.traj_start dev.card `Both
+ USBCard.call Commands.Motors.traj_start (get_card card) `Both
end else
- stop_motors dev `Smooth
+ stop_motors `Smooth
end
- let set_velocities dev velocities accelerations duration =
+ let set_velocities velocities accelerations duration =
lwt () =
Lwt_log.info_f ~section "set-velocities: velocities=(%d, %d) accelerations=(%d, %d) duration=%f"
(fst velocities) (snd velocities)
(fst accelerations) (snd accelerations)
duration
in
- match Var.get dev.state with
+ match Var.get state with
| Trajectory _ ->
lwt () = Lwt_log.info ~section "set-velocities: state=trajectory" in
fail (Failure "currently in trajectory mode")
| Manual(stopper, left, right) ->
lwt () = Lwt_log.info ~section "set-velocities: state=manual" in
- _set_velocities dev stopper (left, right) velocities accelerations duration
+ _set_velocities stopper (left, right) velocities accelerations duration
| Static ->
lwt () = Lwt_log.info ~section "set-velocities: state=static" in
let static = { velocity = 0; acceleration = 0; direction = `Forward } in
- _set_velocities dev (return ()) (static, static) velocities accelerations duration
+ _set_velocities (return ()) (static, static) velocities accelerations duration
(* +---------------------------------------------------------------+
| Motors inhbition |
+---------------------------------------------------------------+ *)
- let set_inhibit_forward_until dev delay =
+ let set_inhibit_forward_until delay =
let until = Unix.gettimeofday () +. delay in
- Var.set dev.inhibit_forward_until until;
- match Var.get dev.state with
+ Var.set inhibit_forward_until until;
+ match Var.get state with
| Trajectory{ trajectory = (`Forward | `Goto) } ->
- stop_motors dev `Abrupt
+ stop_motors `Abrupt
| Manual(_, settings_l, settings_r)
when (settings_l.direction = `Forward && settings_l.velocity > 0)
|| (settings_r.direction = `Forward && settings_r.velocity > 0) ->
- stop_motors dev `Abrupt
+ stop_motors `Abrupt
| _ ->
return ()
- let set_inhibit_backward_until dev delay =
+ let set_inhibit_backward_until delay =
let until = Unix.gettimeofday () +. delay in
- Var.set dev.inhibit_backward_until until;
- match Var.get dev.state with
+ Var.set inhibit_backward_until until;
+ match Var.get state with
| Trajectory{ trajectory = `Backward } ->
- stop_motors dev `Abrupt
+ stop_motors `Abrupt
| Manual(_, settings_l, settings_r)
when (settings_l.direction = `Backward && settings_l.velocity > 0)
|| (settings_r.direction = `Backward && settings_r.velocity > 0) ->
- stop_motors dev `Abrupt
+ stop_motors `Abrupt
| _ ->
return ()
- let make card path =
- let dev = {
- obus =
- OBus_object.make
- ~interfaces:[Export_unsafe.interface "motors" (fun dev -> dev.card);
- Krobot_interfaces.Fr_krobot_Device_Motors.make
- ~notify_mode:(OBus_object.notify_update "PropertiesChanged")
- {
- m_Turn = (fun ctx obj (angle, velocity, acceleration) ->
- let angle = Int32.to_int angle in
- let velocity = Int32.to_int velocity in
- let acceleration = Int32.to_int acceleration in
- turn obj angle velocity acceleration >|= int32_of_move_result);
- m_Move = (fun ctx obj (distance, velocity, acceleration) ->
- let distance = Int32.to_int distance in
- let velocity = Int32.to_int velocity in
- let acceleration = Int32.to_int acceleration in
- move obj distance velocity acceleration >|= int32_of_move_result);
- m_Goto = (fun ctx obj (x, y, velocity, acceleration, mode, bypass_distance) ->
- let x = Int32.to_int x in
- let y = Int32.to_int y in
- let velocity = Int32.to_int velocity in
- let acceleration = Int32.to_int acceleration in
- let mode = goto_mode_of_int32 mode in
- let bypass_distance = Int32.to_int bypass_distance in
- goto obj x y velocity acceleration mode bypass_distance >|= int32_of_move_result);
- m_StopMotors = (fun ctx obj mode ->
- let mode = stop_mode_of_int32 mode in
- stop_motors obj mode);
- m_SetVelocities = (fun ctx obj (velocity_l, acceleration_l, velocity_r, acceleration_r, duration) ->
- let velocity_l = Int32.to_int velocity_l in
- let acceleration_l = Int32.to_int acceleration_l in
- let velocity_r = Int32.to_int velocity_r in
- let acceleration_r = Int32.to_int acceleration_r in
- set_velocities obj (velocity_l, velocity_r) (acceleration_l, acceleration_r) duration);
- p_InhibitForwardUntil = ((fun obj -> Var.signal obj.inhibit_forward_until),
- (fun ctx -> set_inhibit_forward_until));
- p_InhibitBackwardUntil = ((fun obj -> Var.signal obj.inhibit_backward_until),
- (fun ctx -> set_inhibit_backward_until));
- p_State = (fun obj -> state obj);
- }]
- path;
- card = card;
- inhibit_forward_until = Var.create 0.0;
- inhibit_backward_until = Var.create 0.0;
- state = Var.create Static;
- traj_completed =
- React.E.filter
- (fun (cmd, data) ->
- cmd = PcInterface.cmd_traj
- && Char.code data.[0] = PcInterface.traj_completed)
- (USBCard.commands card);
- } in
- OBus_object.attach dev.obus dev;
- return dev
+ let () =
+ OBus_object.add_interface obus
+ (make ~notify_mode
+ {
+ m_Turn = (fun ctx () (angle, velocity, acceleration) ->
+ let angle = Int32.to_int angle in
+ let velocity = Int32.to_int velocity in
+ let acceleration = Int32.to_int acceleration in
+ turn angle velocity acceleration >|= int32_of_move_result);
+ m_Move = (fun ctx () (distance, velocity, acceleration) ->
+ let distance = Int32.to_int distance in
+ let velocity = Int32.to_int velocity in
+ let acceleration = Int32.to_int acceleration in
+ move distance velocity acceleration >|= int32_of_move_result);
+ m_Goto = (fun ctx () (x, y, velocity, acceleration, mode, bypass_distance) ->
+ let x = Int32.to_int x in
+ let y = Int32.to_int y in
+ let velocity = Int32.to_int velocity in
+ let acceleration = Int32.to_int acceleration in
+ let mode = goto_mode_of_int32 mode in
+ let bypass_distance = Int32.to_int bypass_distance in
+ goto x y velocity acceleration mode bypass_distance >|= int32_of_move_result);
+ m_StopMotors = (fun ctx () mode ->
+ let mode = stop_mode_of_int32 mode in
+ stop_motors mode);
+ m_SetVelocities = (fun ctx () (velocity_l, acceleration_l, velocity_r, acceleration_r, duration) ->
+ let velocity_l = Int32.to_int velocity_l in
+ let acceleration_l = Int32.to_int acceleration_l in
+ let velocity_r = Int32.to_int velocity_r in
+ let acceleration_r = Int32.to_int acceleration_r in
+ set_velocities (velocity_l, velocity_r) (acceleration_l, acceleration_r) duration);
+ p_InhibitForwardUntil = ((fun () -> Var.signal inhibit_forward_until),
+ (fun ctx () delay -> set_inhibit_forward_until delay));
+ p_InhibitBackwardUntil = ((fun () -> Var.signal inhibit_backward_until),
+ (fun ctx () delay -> set_inhibit_backward_until delay));
+ p_State = (fun () -> state_info);
+ })
end
(* +-----------------------------------------------------------------+
@@ -741,31 +752,19 @@ end
let done_waiter, done_wakener = Lwt.wait ()
let quit = ref false
-let card_interface, set_card_interface = React.S.create None
-let card_monitoring, set_card_monitoring = React.S.create None
-let card_sensor, set_card_sensor = React.S.create None
-let card_motor, set_card_motor = React.S.create None
module Manager =
struct
+ let obus = OBus_object.make ["fr"; "krobot"; "Manager"]
+
open Krobot_interfaces.Fr_krobot_Manager
- let close card = match React.S.value card with
+ let close card = match React.S.value card.card_card with
| None ->
return ()
| Some card ->
USBCard.close card
- let card_states manager =
- let state card = match React.S.value card with
- | Some _ -> `Present
- | None -> `Absent
- in
- return (int32_of_card_state (state card_interface),
- int32_of_card_state (state card_sensor),
- int32_of_card_state (state card_motor),
- int32_of_card_state (state card_monitoring))
-
let shutdown ctx manager =
lwt () = Lwt_log.info ~section "exiting" in
quit := true;
@@ -777,19 +776,22 @@ struct
Lwt.wakeup done_wakener ();
return ()
- let manager =
- let manager =
- OBus_object.make
- ~interfaces:[Krobot_interfaces.Fr_krobot_Manager.make
- ~notify_mode:(OBus_object.notify_update "PropertiesChanged")
- {
- m_CardStates = (fun ctx obj () -> card_states obj);
- m_Shutdown = (fun ctx obj () -> shutdown ctx obj);
- }]
- ["fr"; "krobot"; "Manager"]
- in
- OBus_object.attach manager ();
- manager
+ let devices_status =
+ React.S.map
+ (fun map ->
+ String_map.fold
+ (fun device status acc -> (device, status) :: acc)
+ map [])
+ devices_status
+
+ let () =
+ OBus_object.attach obus ();
+ OBus_object.add_interface obus
+ (make ~notify_mode
+ {
+ p_DevicesStatus = (fun () -> devices_status);
+ m_Shutdown = (fun ctx () () -> shutdown ctx ());
+ })
end
(* +-----------------------------------------------------------------+
@@ -801,46 +803,39 @@ struct
open Krobot_interfaces.Fr_krobot_Card
type t = {
- card : USBCard.t option React.signal;
+ card : card;
obus : t OBus_object.t;
- name : string;
state : Types.card_state React.signal;
mutable errors : unit React.event;
}
- let get_card dev = match React.S.value dev.card with
- | None ->
- Printf.ksprintf failwith "%s card is not available" dev.name
- | Some card ->
- card
-
let error dev msg =
OBus_signal.emit Krobot_interfaces.Fr_krobot_Card.s_Error dev.obus msg
let get_firmware_build dev =
- USBCard.call Commands.Card.get_firmware_build (get_card dev) ()
+ USBCard.call Commands.Card.get_firmware_build (get_card dev.card) ()
let get_board_info dev =
- USBCard.call Commands.Card.get_board_info (get_card dev) ()
+ USBCard.call Commands.Card.get_board_info (get_card dev.card) ()
let bootloader dev =
- USBCard.call Commands.Card.bootloader (get_card dev) ()
+ USBCard.call Commands.Card.bootloader (get_card dev.card) ()
let reset dev =
- USBCard.call Commands.Card.reset (get_card dev) ()
+ USBCard.call Commands.Card.reset (get_card dev.card) ()
let test dev =
- USBCard.call Commands.Card.test (get_card dev) ()
+ USBCard.call Commands.Card.test (get_card dev.card) ()
- let make name card path =
+ let make card path =
let dev = {
obus =
OBus_object.make
- ~interfaces:[Export_unsafe.interface "card" get_card;
+ ~interfaces:[Export_unsafe.interface "Card" card;
Krobot_interfaces.Fr_krobot_Card.make
~notify_mode:(OBus_object.notify_update "PropertiesChanged")
{
- p_Name = (fun obj -> React.S.const name);
+ p_Name = (fun obj -> React.S.const card.card_name);
p_State = (fun obj -> React.S.map int32_of_card_state obj.state);
...
[truncated message content] |
