Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 7de615d13e5add106bc84cfb21118aa100d745f6
|
From: Jérémie D. <Ba...@us...> - 2010-02-10 14:19:02
|
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 7de615d13e5add106bc84cfb21118aa100d745f6 (commit)
via 2f104a859ee145da7d7bd2112875201bb46e123c (commit)
from d3193fa311b9c329e7b009ac3eadf26602f3628f (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 7de615d13e5add106bc84cfb21118aa100d745f6
Author: Jérémie Dimino <je...@di...>
Date: Wed Feb 10 15:18:22 2010 +0100
Add security (stop robot on colision)
commit 2f104a859ee145da7d7bd2112875201bb46e123c
Author: Jérémie Dimino <je...@di...>
Date: Wed Feb 10 14:37:01 2010 +0100
Remove USB calls from "Driver" and put them into "Commands"
-----------------------------------------------------------------------
Changes:
diff --git a/PC_Mainboard/clients/lib-krobot/krobot.ml b/PC_Mainboard/clients/lib-krobot/krobot.ml
index 25d225c..cb9490a 100644
--- a/PC_Mainboard/clients/lib-krobot/krobot.ml
+++ b/PC_Mainboard/clients/lib-krobot/krobot.ml
@@ -169,15 +169,12 @@ let obus_stop_mode = OBus_type.mapping obus_int
(`Abrupt, 1);
(`Smooth, 2)]
-OP_method Turn : int -> int -> int -> unit
-OP_method Move : int -> int -> int -> unit
-OP_method StopMotors : motor -> stop_mode -> unit
-OP_method SetSpeed : motor -> int -> int -> unit
-
-let turn krobot ~angle ~speed ~acc = turn krobot angle speed acc
-let move krobot ~dist ~speed ~acc = move krobot dist speed acc
-let stop_motors krobot ~motor ~mode = stop_motors krobot motor mode
-let set_speed krobot ~motor ~speed ~acc = set_speed krobot motor speed acc
+OP_method Turn : angle : int -> speed : int -> acc : int -> unit
+OP_method Move : dist : int -> speed : int -> acc : int -> unit
+OP_method StopMotors : motor : motor -> mode : stop_mode -> unit
+OP_method SetSpeed : motor : motor -> speed : int -> acc : int -> unit
+OP_method InhibitForward : float -> unit
+OP_method InhibitBackward : float -> unit
(* +-----------------------------------------------------------------+
| Cards |
diff --git a/PC_Mainboard/clients/lib-krobot/krobot.mli b/PC_Mainboard/clients/lib-krobot/krobot.mli
index 9d0134e..178779a 100644
--- a/PC_Mainboard/clients/lib-krobot/krobot.mli
+++ b/PC_Mainboard/clients/lib-krobot/krobot.mli
@@ -66,6 +66,14 @@ val set_speed : t -> motor : motor -> speed : int -> acc : int -> unit Lwt.t
(** [set_speed krobot ~motor ~speed ~acc] set the speed and
acceleration for the specified motors. *)
+val inhibit_forward : t -> float -> unit Lwt.t
+ (** [inhibit_forward krobot delay] forbid forward moves during
+ [delay] seconds *)
+
+val inhibit_backward : t -> float -> unit Lwt.t
+ (** [inhibit_backward krobot delay] forbid backward moves during
+ [delay] seconds *)
+
(** {6 Cards} *)
module Card : sig
diff --git a/PC_Mainboard/clients/myocamlbuild.ml b/PC_Mainboard/clients/myocamlbuild.ml
index 593f6b9..54d6a50 100644
--- a/PC_Mainboard/clients/myocamlbuild.ml
+++ b/PC_Mainboard/clients/myocamlbuild.ml
@@ -100,6 +100,7 @@ let _ =
"tools/controller.native";
"remote/forward_dbus.native";
"tools/joy_control.native";
+ "security/hard_stop.native";
];
(* +---------------------------------------------------------+
diff --git a/PC_Mainboard/clients/security/hard_stop.ml b/PC_Mainboard/clients/security/hard_stop.ml
new file mode 100644
index 0000000..4506154
--- /dev/null
+++ b/PC_Mainboard/clients/security/hard_stop.ml
@@ -0,0 +1,91 @@
+(*
+ * hard_stop.ml
+ * ------------
+ * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(* Stop the robot on collisions *)
+
+open Lwt
+
+let front_sensors = [0; 2; 3; 4; 5; 6; 7]
+let back_sensors = [8; 9; 10; 11; 12; 13; 14; 15]
+
+(* Duration of an inhibition: *)
+let duration = 1.0
+
+let foreground = ref false
+let args = [
+ "-n", Arg.Set foreground, "do not daemonize";
+]
+let usage = Printf.sprintf "Usage: %s [-n]\n\noptions are:" (Filename.basename (Sys.argv.(0)))
+
+type state = OK | Stopped
+
+let state_forward = ref OK
+let state_backward = ref OK
+
+let handle_colide krobot sensors =
+ let colide_front = ref false and colide_back = ref false in
+ for i = 0 to 15 do
+ if sensors.(i) then begin
+ if List.mem i front_sensors then
+ colide_front := true
+ else if List.mem i back_sensors then
+ colide_back := true
+ end
+ done;
+ join [
+ (if !colide_front then begin
+ lwt () = Krobot.inhibit_forward krobot duration in
+ if !state_forward = OK then begin
+ state_forward := Stopped;
+ Krobot.stop_motors krobot ~motor:`Both ~mode:`Off;
+ end else
+ return ()
+ end else begin
+ state_forward := OK;
+ return ()
+ end);
+ (if !colide_back then begin
+ lwt () = Krobot.inhibit_backward krobot duration in
+ if !state_backward = OK then begin
+ state_backward := Stopped;
+ Krobot.stop_motors krobot ~motor:`Both ~mode:`Off;
+ end else
+ return ()
+ end else begin
+ state_backward := OK;
+ return ()
+ end);
+ ]
+
+lwt () =
+ Arg.parse args ignore usage;
+
+ if !foreground then
+ Log#info "starting hard_stop in foreground mode"
+ else begin
+ Log#info "starting hard_stop in daemon mode";
+ Lwt_log.default := Lwt_log.syslog
+ ~level:(min Lwt_log.Info Lwt_log.default_level)
+ ~facility:`Daemon
+ ();
+ Lwt_unix.daemonize ~keep_stderr:true ()
+ end;
+
+ lwt krobot = Krobot.create () in
+
+ (* Stop motors as soon as possible: *)
+ Lwt_signal.always_notify_p (handle_colide krobot) (Krobot.logic_sensors krobot);
+
+ (* Continue the inhibition: *)
+ let rec loop () =
+ lwt () = handle_colide krobot (React.S.value (Krobot.logic_sensors krobot)) in
+ lwt () = Lwt_unix.sleep (duration /. 2.) in
+ loop ()
+ in
+ loop ()
diff --git a/PC_Mainboard/driver/src/commands.ml b/PC_Mainboard/driver/src/commands.ml
new file mode 100644
index 0000000..5404b77
--- /dev/null
+++ b/PC_Mainboard/driver/src/commands.ml
@@ -0,0 +1,150 @@
+(*
+ * commands.ml
+ * -----------
+ * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+open Lwt
+
+let string_of_azt data =
+ try
+ String.sub data 0 (String.index data '\000')
+ with Not_found ->
+ data
+
+let get_firmware_build card =
+ let data = Card.make_buffer () in
+ RW.set_uint8 data 0 Protocol.get_firmware_build;
+ Card.send_request card Protocol.cmd_get data >|= string_of_azt
+
+let get_board_info card =
+ let data = Card.make_buffer () in
+ RW.set_uint8 data 0 Protocol.get_board_info;
+ Card.send_request card Protocol.cmd_get data >|= string_of_azt
+
+let bootloader card =
+ Card.send_command card Protocol.cmd_bootloader ""
+
+module Compass =
+struct
+ let get card =
+ lwt data = Card.send_request card Protocol.get_cmp03_data "" in
+ return (RW.get_int16 data 2)
+end
+
+module AX12 =
+struct
+ let goto card ~id ~pos ~speed =
+ 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 ()
+end
+
+module Logic_sensors =
+struct
+ let get 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))
+end
+
+module Range_finders =
+struct
+ let get 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)))
+end
+
+module Motor =
+struct
+ type t = [ `Left | `Right | `Both ]
+ type stop_mode = [ `Off | `Abrupt | `Smooth ]
+ type direction = [ `Forward | `Backward ]
+
+ let backend cmd card arg1 arg2 arg3 =
+ 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 card ~dist ~speed ~acc =
+ if dist < 0 then
+ backend Protocol.traj_forward card (-dist) speed acc
+ else
+ backend Protocol.traj_backward card dist speed acc
+
+ let turn card ~angle ~speed ~acc =
+ if angle < 0 then
+ backend Protocol.traj_tl card (-angle) speed acc
+ else
+ backend Protocol.traj_tr card angle speed acc
+
+ (* Values comming from lm629.h *)
+ let int_of_direction = function
+ | `Forward -> 1
+ | `Backward -> -1
+
+ let int_of_motor = function
+ | `Left -> Protocol.motor_left
+ | `Both -> Protocol.motor_both
+ | `Right -> Protocol.motor_right
+
+ let int_of_stop_mode = function
+ | `Off -> Protocol.traj_stop_motor_off
+ | `Abrupt -> Protocol.traj_stop_abrupt
+ | `Smooth -> Protocol.traj_stop_smooth
+
+ let traj_new_velocity card ~motor ~speed ~acc ~dir =
+ let data = Card.make_buffer () in
+ RW.set_uint8 data 0 Protocol.traj_new_velocity;
+ RW.set_uint8 data 1 (int_of_motor motor);
+ RW.set_int16 data 2 speed;
+ RW.set_int16 data 4 acc;
+ RW.set_uint8 data 6 (int_of_direction dir);
+ Card.send_command card Protocol.cmd_traj data
+
+ let traj_change_velocity card ~motor ~speed ~dir =
+ let data = Card.make_buffer () in
+ RW.set_uint8 data 0 Protocol.traj_change_velocity;
+ RW.set_uint8 data 1 (int_of_motor motor);
+ RW.set_int16 data 2 speed;
+ RW.set_uint8 data 4 (int_of_direction dir);
+ Card.send_command card Protocol.cmd_traj data
+
+ let traj_start card ~motor =
+ let data = Card.make_buffer () in
+ RW.set_uint8 data 0 Protocol.traj_start;
+ RW.set_uint8 data 1 (int_of_motor motor);
+ Card.send_command card Protocol.cmd_traj data
+
+ let init_lm629 card =
+ let data = Card.make_buffer () in
+ RW.set_uint8 data 0 Protocol.traj_init;
+ Card.send_command card Protocol.cmd_traj data
+
+ let enable 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
+
+ let stop card ~motor ~mode =
+ let data = String.create 4 in
+ RW.set_uint8 data 0 Protocol.traj_stop;
+ RW.set_uint8 data 1 (int_of_motor motor);
+ RW.set_uint16 data 2 (int_of_stop_mode mode);
+ Card.send_command card Protocol.cmd_traj data
+end
diff --git a/PC_Mainboard/driver/src/commands.mli b/PC_Mainboard/driver/src/commands.mli
new file mode 100644
index 0000000..7c0862c
--- /dev/null
+++ b/PC_Mainboard/driver/src/commands.mli
@@ -0,0 +1,52 @@
+(*
+ * commands.mli
+ * ------------
+ * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(** All commands, by card *)
+
+(** {6 Common commands} *)
+
+val get_firmware_build : Card.t -> string Lwt.t
+val get_board_info : Card.t -> string Lwt.t
+val bootloader : Card.t -> unit Lwt.t
+
+module Compass : sig
+ val get : Card.t -> int Lwt.t
+end
+
+module AX12 : sig
+ val goto : Card.t -> id : int -> pos : int -> speed : int -> unit Lwt.t
+end
+
+module Logic_sensors : sig
+ val get : Card.t -> bool array Lwt.t
+end
+
+module Range_finders : sig
+ val get : Card.t -> int array Lwt.t
+end
+
+module Motor : sig
+ type t = [ `Left | `Right | `Both ]
+
+ val move : Card.t -> dist : int -> speed : int -> acc : int -> unit Lwt.t
+ val turn : Card.t -> angle : int -> speed : int -> acc : int -> unit Lwt.t
+
+ type stop_mode = [ `Off | `Abrupt | `Smooth ]
+
+ val stop : Card.t -> motor : t -> mode : stop_mode -> unit Lwt.t
+
+ type direction = [ `Forward | `Backward ]
+
+ val traj_new_velocity : Card.t -> motor : t -> speed : int -> acc : int -> dir : direction -> unit Lwt.t
+ val traj_change_velocity : Card.t -> motor : t -> speed : int -> dir : direction -> unit Lwt.t
+ val traj_start : Card.t -> motor : t -> unit Lwt.t
+
+ val enable : Card.t -> unit Lwt.t
+ val init_lm629 : Card.t -> unit Lwt.t
+end
diff --git a/PC_Mainboard/driver/src/driver.ml b/PC_Mainboard/driver/src/driver.ml
index 5d8b294..18d7203 100644
--- a/PC_Mainboard/driver/src/driver.ml
+++ b/PC_Mainboard/driver/src/driver.ml
@@ -51,20 +51,6 @@ let card_interface = Card.open_card ~name:"interface" ~vendor_id:Protocol.usb_vi
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
-
-let init_lm629 card =
- Log#info "enabling lm629 on %s card" (Card.name card);
- let data = Card.make_buffer () in
- RW.set_uint8 data 0 Protocol.traj_init;
- Card.send_command card Protocol.cmd_traj data
-
(* Enable motors when a card comes up *)
let on_running card f =
let stop = ref ignore in
@@ -83,9 +69,8 @@ let on_running card f =
stop := (fun () -> Lwt_signal.disable notifier)
let () =
- on_running card_interface motor_enable;
- on_running card_motor
- (fun card -> motor_enable card <&> init_lm629 card)
+ on_running card_interface Commands.Motor.enable;
+ on_running card_motor (fun card -> Commands.Motor.enable card <&> Commands.Motor.init_lm629 card)
(* +-----------------------------------------------------------------+
| Compass |
@@ -109,12 +94,8 @@ struct
OL_signal Value : int
OL_method Get : int = fun dev -> return dev.data
- let get card =
- 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
+ lwt data = Commands.Compass.get dev.card in
if data <> dev.data then begin
dev.data <- data;
lwt () = value dev data in
@@ -152,16 +133,9 @@ struct
include OBus.MakeInterface(struct let name = "fr.krobot.Device.AX12" end)
- let ax12_goto dev id pos speed =
- 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 dev.card Protocol.cmd_ax12 data in
- return ()
-
- OL_method AX12Goto : int -> int -> int -> unit
+ OL_method AX12Goto : int -> int -> int -> unit =
+ fun dev id pos speed ->
+ Commands.AX12.goto dev.card id pos speed
let make card path = {
obus = OBus_object.make path;
@@ -173,7 +147,7 @@ end
| Logic sensors |
+-----------------------------------------------------------------+ *)
-module LogicSensors =
+module Logic_sensors =
struct
type t = {
obus : OBus_object.t;
@@ -191,15 +165,8 @@ struct
OL_signal Value : bool array
OL_method Get : bool array = fun dev -> return dev.data
- let get 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))
-
let rec loop dev =
- lwt data = get dev.card in
+ lwt data = Commands.Logic_sensors.get dev.card in
if data <> dev.data then begin
dev.data <- data;
lwt () = value dev data in
@@ -223,7 +190,7 @@ end
| Range finders |
+-----------------------------------------------------------------+ *)
-module RangeFinders =
+module Range_finders =
struct
type t = {
obus : OBus_object.t;
@@ -241,14 +208,8 @@ struct
OL_signal Value : int array
OL_method Get : int array = fun dev -> return dev.data
- let get 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)))
-
let rec loop dev =
- lwt data = get dev.card in
+ lwt data = Commands.Range_finders.get dev.card in
if data <> dev.data then begin
dev.data <- data;
lwt () = value dev data in
@@ -281,11 +242,15 @@ struct
obus : OBus_object.t;
card : Card.t;
- mutable acceleration : int option;
+ mutable acceleration : int;
(* The current acceleration *)
mutable state : unit React.signal;
(* Reinit [acceleraation] to None when the card state changes *)
+
+ mutable inhibit_forward_until : float;
+ mutable inhibit_backward_until : float;
+ (* Date after which motor's inhition should be stopped *)
}
module OBus = OBus_object.Make(struct
@@ -295,27 +260,28 @@ struct
include OBus.MakeInterface(struct let name = "fr.krobot.Device.Motors" end)
- let move cmd card arg1 arg2 arg3 =
- 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 turn dev angle speed acc =
- Log#info "motor: turn(angle=%d, speed=%d, acc=%d)" angle speed acc;
- if angle < 0 then
- move Protocol.traj_tl dev.card (-angle) speed acc
- else
- move Protocol.traj_tr dev.card angle speed acc
+ (* +---------------------------------------------------------------+
+ | High-level movement |
+ +---------------------------------------------------------------+ *)
let move dev dist speed acc =
Log#info "motor: move(dist=%d, speed=%d, acc=%d)" dist speed acc;
- if dist < 0 then
- move Protocol.traj_forward dev.card (-dist) speed acc
+ let date = Unix.gettimeofday () in
+ if (dist > 0 && date < dev.inhibit_forward_until) || (dist < 0 && date < dev.inhibit_backward_until) then
+ return ()
else
- move Protocol.traj_backward dev.card dist speed acc
+ Commands.Motor.move dev.card dist speed acc
+
+ let turn dev angle speed acc =
+ Log#info "motor: turn(angle=%d, speed=%d, acc=%d)" angle speed acc;
+ Commands.Motor.turn dev.card angle speed acc
+
+ OL_method Turn : int -> int -> int -> unit
+ OL_method Move : int -> int -> int -> unit
+
+ (* +---------------------------------------------------------------+
+ | Low-level control of speed and acceleration of each motor |
+ +---------------------------------------------------------------+ *)
let obus_motor = OBus_type.mapping obus_int
[(`Left, -1);
@@ -327,21 +293,11 @@ struct
(`Abrupt, 1);
(`Smooth, 2)]
- let int_of_motor = function
- | `Left -> Protocol.motor_left
- | `Both -> Protocol.motor_both
- | `Right -> Protocol.motor_right
-
let string_of_motor = function
| `Left -> "left"
| `Both -> "both"
| `Right -> "right"
- let int_of_stop_mode = function
- | `Off -> Protocol.traj_stop_motor_off
- | `Abrupt -> Protocol.traj_stop_abrupt
- | `Smooth -> Protocol.traj_stop_smooth
-
let string_of_stop_mode = function
| `Off -> "off"
| `Abrupt -> "abrupt"
@@ -349,65 +305,52 @@ struct
let stop_motors dev motor mode =
Log#info "motor: stopping(motor=%s, mode=%s)" (string_of_motor motor) (string_of_stop_mode mode);
- let data = String.create 4 in
- RW.set_uint8 data 0 Protocol.traj_stop;
- RW.set_uint8 data 1 (int_of_motor motor);
- RW.set_uint16 data 2 (int_of_stop_mode mode);
- Card.send_command dev.card Protocol.cmd_traj data
-
- (* Values comming from lm629.h *)
- let forward = 1
- let backward = -1
-
- let traj_new_velocity card motor vel acc dir =
- let data = Card.make_buffer () in
- RW.set_uint8 data 0 Protocol.traj_new_velocity;
- RW.set_uint8 data 1 (int_of_motor motor);
- RW.set_int16 data 2 vel;
- RW.set_int16 data 4 acc;
- RW.set_uint8 data 6 dir;
- Card.send_command card Protocol.cmd_traj data
-
- let traj_change_velocity card motor vel dir =
- let data = Card.make_buffer () in
- RW.set_uint8 data 0 Protocol.traj_change_velocity;
- RW.set_uint8 data 1 (int_of_motor motor);
- RW.set_int16 data 2 vel;
- RW.set_uint8 data 4 dir;
- Card.send_command card Protocol.cmd_traj data
-
- let traj_start card motor =
- let data = Card.make_buffer () in
- RW.set_uint8 data 0 Protocol.traj_start;
- RW.set_uint8 data 1 (int_of_motor motor);
- Card.send_command card Protocol.cmd_traj data
+ Commands.Motor.stop dev.card motor mode
let set_speed dev motor speed acc =
Log#info "motor: set_speed(motor=%s, speed=%d, acc=%d)" (string_of_motor motor) speed acc;
- let dir, speed = if speed < 0 then (backward, -speed) else (forward, speed) in
- lwt () =
- if Some acc <> dev.acceleration then begin
- dev.acceleration <- Some acc;
- traj_new_velocity dev.card motor speed acc dir
- end else
- traj_change_velocity dev.card motor speed dir
- in
- traj_start dev.card motor
+ let dir, speed = if speed < 0 then (`Backward, -speed) else (`Forward, speed) in
+ let date = Unix.gettimeofday () in
+ if (dir = `Forward && date < dev.inhibit_forward_until) || (dir = `Backward && date < dev.inhibit_backward_until) then
+ return ()
+ else
+ lwt () =
+ if acc <> dev.acceleration then begin
+ dev.acceleration <- acc;
+ Commands.Motor.traj_new_velocity dev.card motor speed acc dir
+ end else
+ Commands.Motor.traj_change_velocity dev.card motor speed dir
+ in
+ Commands.Motor.traj_start dev.card motor
- OL_method Turn : int -> int -> int -> unit
- OL_method Move : int -> int -> int -> unit
OL_method StopMotors : motor -> stop_mode -> unit
OL_method SetSpeed : motor -> int -> int -> unit
+ (* +---------------------------------------------------------------+
+ | Motors inhbition |
+ +---------------------------------------------------------------+ *)
+
+ OL_method InhibitForward : float -> unit =
+ fun dev delay ->
+ dev.inhibit_forward_until <- Unix.gettimeofday () +. delay;
+ return ()
+
+ OL_method InhibitBackward : float -> unit =
+ fun dev delay ->
+ dev.inhibit_forward_until <- Unix.gettimeofday () +. delay;
+ return ()
+
let make card path =
let dev = {
obus = OBus_object.make path;
card = card;
- acceleration = None;
+ acceleration = 0;
state = React.S.const ();
+ inhibit_forward_until = 0.0;
+ inhibit_backward_until = 0.0;
}
in
- dev.state <- React.S.map (fun state -> dev.acceleration <- None) (Card.state card);
+ dev.state <- React.S.map (fun state -> dev.acceleration <- 0) (Card.state card);
dev
end
@@ -448,6 +391,7 @@ struct
type t = {
card : Card.t;
obus : OBus_object.t;
+ mutable state : unit Lwt.t React.signal;
}
module OBus = OBus_object.Make(struct
@@ -466,31 +410,28 @@ struct
with Not_found ->
data
- OL_method GetFirmwareBuild : string = fun card ->
- let data = Card.make_buffer () in
- RW.set_uint8 data 0 Protocol.get_firmware_build;
- Card.send_request card.card Protocol.cmd_get data >|= string_of_azt
+ OL_method GetFirmwareBuild : string = fun dev ->
+ Commands.get_firmware_build dev.card
- OL_method GetBoardInfo : string = fun card ->
- let data = Card.make_buffer () in
- RW.set_uint8 data 0 Protocol.get_board_info;
- Card.send_request card.card Protocol.cmd_get data >|= string_of_azt
+ OL_method GetBoardInfo : string = fun dev ->
+ Commands.get_board_info dev.card
- OL_method GetState : Card.state = fun card ->
- return (React.S.value (Card.state card.card))
+ OL_method GetState : Card.state = fun dev ->
+ return (React.S.value (Card.state dev.card))
- OL_method Bootloader : unit = fun card ->
- Card.send_command card.card Protocol.cmd_bootloader ""
+ OL_method Bootloader : unit = fun dev ->
+ Commands.bootloader dev.card
OL_signal StateChanged : Card.state
let make card path =
- let card = {
+ let dev = {
card = card;
obus = OBus_object.make path;
+ state = React.S.const (return ());
} in
- Lwt_signal.always_notify_s (state_changed card) (Card.state card.card);
- card
+ dev.state <- React.S.map (state_changed dev) (Card.state dev.card);
+ dev
end
(* +-----------------------------------------------------------------+
@@ -542,8 +483,8 @@ lwt () =
AX12.OBus.export bus (AX12.make card_interface ["fr"; "krobot"; "Devices"; "AX12"]);
(* Sensor card *)
- LogicSensors.OBus.export bus (LogicSensors.make card_sensor ["fr"; "krobot"; "Devices"; "LogicSensors"]);
- RangeFinders.OBus.export bus (RangeFinders.make card_sensor ["fr"; "krobot"; "Devices"; "RangeFinders"]);
+ Logic_sensors.OBus.export bus (Logic_sensors.make card_sensor ["fr"; "krobot"; "Devices"; "LogicSensors"]);
+ Range_finders.OBus.export bus (Range_finders.make card_sensor ["fr"; "krobot"; "Devices"; "RangeFinders"]);
(* Motor card *)
Motors.OBus.export bus (Motors.make card_motor ["fr"; "krobot"; "Devices"; "Motors"]);
hooks/post-receive
--
krobot
|
