Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 267bcf969f0866a9da25d3040e1ab0f589eb9566
|
From: Jérémie D. <Ba...@us...> - 2011-04-12 21:50:58
|
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 267bcf969f0866a9da25d3040e1ab0f589eb9566 (commit)
from ea122f2120a515a92164b114a030ece3485da728 (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 267bcf969f0866a9da25d3040e1ab0f589eb9566
Author: Jérémie Dimino <je...@di...>
Date: Tue Apr 12 23:50:16 2011 +0200
[info] split the viewer and the planner
-----------------------------------------------------------------------
Changes:
diff --git a/.gitignore b/.gitignore
index a443cd9..263aa3c 100644
--- a/.gitignore
+++ b/.gitignore
@@ -18,6 +18,7 @@ setup.data
setup.log
_build
info/control2011/src/driver/bus.conf
+info/control2011/src/services/*.service
buildrev.h
images
obj
diff --git a/info/control2011/_oasis b/info/control2011/_oasis
index e588a36..5b931a1 100644
--- a/info/control2011/_oasis
+++ b/info/control2011/_oasis
@@ -13,7 +13,9 @@ BuildTools: ocamlbuild
Plugins: DevFiles (0.2), META (0.2)
Synopsis: [Kro]bot
Description: Control program for the Eurobot robotic cup.
-FilesAB: src/driver/bus.conf.ab
+FilesAB:
+ src/driver/bus.conf.ab,
+ src/services/fr.krobot.Service.Planner.service.ab
# +-------------------------------------------------------------------+
# | Flags |
@@ -39,7 +41,8 @@ Library krobot
Krobot_bus,
Krobot_message,
Krobot_service,
- Krobot_geom
+ Krobot_geom,
+ Krobot_planner
Library "krobot-can"
FindlibName: can
@@ -62,7 +65,8 @@ Library "krobot-interfaces"
Install: true
Modules:
Krobot_interface_can,
- Krobot_interface_service
+ Krobot_interface_service,
+ Krobot_interface_planner
# +-------------------------------------------------------------------+
# | The driver |
@@ -161,6 +165,18 @@ Executable "krobot-hil-simulator"
BuildDepends: krobot, lwt.syntax
# +-------------------------------------------------------------------+
+# | Services |
+# +-------------------------------------------------------------------+
+
+Executable "krobot-service-planner"
+ Path: src/services
+ Install: true
+ CompiledObject: best
+ MainIs: krobot_service_planner.ml
+ BuildDepends: krobot, lwt.syntax
+ DataFiles: fr.krobot.Service.Planner.service ($datadir/$pkg_name/services)
+
+# +-------------------------------------------------------------------+
# | Examples |
# +-------------------------------------------------------------------+
diff --git a/info/control2011/_tags b/info/control2011/_tags
index 29a64d4..f190b59 100644
--- a/info/control2011/_tags
+++ b/info/control2011/_tags
@@ -2,7 +2,7 @@
<src/interfaces/*.ml>: -syntax_camlp4o
# OASIS_START
-# DO NOT EDIT (digest: 37f25f8d73f8ace7a5648c2d0f189bda)
+# DO NOT EDIT (digest: 19f32386260765eb3050ee674e3a3e18)
# Library krobot-interfaces
"src/interfaces": include
"src/interfaces/krobot-interfaces.cmxs": use_krobot-interfaces
@@ -84,6 +84,17 @@
<src/tools/krobot_plot_battery.{native,byte}>: pkg_lwt.syntax
<src/tools/krobot_plot_battery.{native,byte}>: pkg_lwt.glib
<src/tools/krobot_plot_battery.{native,byte}>: pkg_cairo.lablgtk2
+# Executable krobot-service-planner
+<src/services/krobot_service_planner.{native,byte}>: use_krobot
+<src/services/krobot_service_planner.{native,byte}>: use_krobot-interfaces
+<src/services/krobot_service_planner.{native,byte}>: pkg_obus
+<src/services/krobot_service_planner.{native,byte}>: pkg_lwt.unix
+<src/services/krobot_service_planner.{native,byte}>: pkg_lwt.syntax
+<src/services/*.ml{,i}>: use_krobot
+<src/services/*.ml{,i}>: use_krobot-interfaces
+<src/services/*.ml{,i}>: pkg_obus
+<src/services/*.ml{,i}>: pkg_lwt.unix
+<src/services/*.ml{,i}>: pkg_lwt.syntax
# Executable krobot-remote
<src/tools/krobot_remote.{native,byte}>: use_krobot
<src/tools/krobot_remote.{native,byte}>: use_krobot-interfaces
diff --git a/info/control2011/setup.ml b/info/control2011/setup.ml
index 1ab732c..d6cccba 100644
--- a/info/control2011/setup.ml
+++ b/info/control2011/setup.ml
@@ -1,7 +1,7 @@
(* setup.ml generated for the first time by OASIS v0.2.0~alpha1 *)
(* OASIS_START *)
-(* DO NOT EDIT (digest: 2fd774d555ea6bdcec2ed57e38e9bc94) *)
+(* DO NOT EDIT (digest: b36c743e22b6b1a7b932599bea11f68b) *)
(*
Regenerated by OASIS v0.2.0
Visit http://oasis.forge.ocamlcore.org for more information and
@@ -5049,7 +5049,11 @@ let setup_t =
pre_command = [(OASISExpr.EBool true, None)];
post_command = [(OASISExpr.EBool true, None)];
};
- files_ab = ["src/driver/bus.conf.ab"];
+ files_ab =
+ [
+ "src/driver/bus.conf.ab";
+ "src/services/fr.krobot.Service.Planner.service.ab"
+ ];
sections =
[
Library
@@ -5076,7 +5080,11 @@ let setup_t =
},
{
lib_modules =
- ["Krobot_interface_can"; "Krobot_interface_service"];
+ [
+ "Krobot_interface_can";
+ "Krobot_interface_service";
+ "Krobot_interface_planner"
+ ];
lib_internal_modules = [];
lib_findlib_parent = Some "krobot";
lib_findlib_name = Some "interfaces";
@@ -5430,6 +5438,40 @@ let setup_t =
});
Executable
({
+ cs_name = "krobot-service-planner";
+ cs_data = PropList.Data.create ();
+ cs_plugin_data = [];
+ },
+ {
+ bs_build = [(OASISExpr.EBool true, true)];
+ bs_install = [(OASISExpr.EBool true, true)];
+ bs_path = "src/services";
+ bs_compiled_object = Best;
+ bs_build_depends =
+ [
+ InternalLibrary "krobot";
+ FindlibPackage ("lwt.syntax", None)
+ ];
+ bs_build_tools = [ExternalTool "ocamlbuild"];
+ bs_c_sources = [];
+ bs_data_files =
+ [
+ ("fr.krobot.Service.Planner.service",
+ Some "$datadir/$pkg_name/services")
+ ];
+ bs_ccopt = [(OASISExpr.EBool true, [])];
+ bs_cclib = [(OASISExpr.EBool true, [])];
+ bs_dlllib = [(OASISExpr.EBool true, [])];
+ bs_dllpath = [(OASISExpr.EBool true, [])];
+ bs_byteopt = [(OASISExpr.EBool true, [])];
+ bs_nativeopt = [(OASISExpr.EBool true, [])];
+ },
+ {
+ exec_custom = false;
+ exec_main_is = "krobot_service_planner.ml";
+ });
+ Executable
+ ({
cs_name = "krobot-remote";
cs_data = PropList.Data.create ();
cs_plugin_data = [];
diff --git a/info/control2011/src/interfaces/krobot-interfaces.mllib b/info/control2011/src/interfaces/krobot-interfaces.mllib
index b25ba41..612c59f 100644
--- a/info/control2011/src/interfaces/krobot-interfaces.mllib
+++ b/info/control2011/src/interfaces/krobot-interfaces.mllib
@@ -1,5 +1,6 @@
# OASIS_START
-# DO NOT EDIT (digest: ca580479098ade9ac95ba1a5fb876b9b)
+# DO NOT EDIT (digest: 0fb519fe846821801a9941698a640eef)
Krobot_interface_can
Krobot_interface_service
+Krobot_interface_planner
# OASIS_STOP
diff --git a/info/control2011/src/interfaces/krobot_interface_planner.obus b/info/control2011/src/interfaces/krobot_interface_planner.obus
new file mode 100644
index 0000000..06c722a
--- /dev/null
+++ b/info/control2011/src/interfaces/krobot_interface_planner.obus
@@ -0,0 +1,36 @@
+(*
+ * krobot_interface_planner.obus
+ * -----------------------------
+ * Copyright : (c) 2011, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(** Interface for the planner. *)
+
+interface fr.krobot.Planner {
+ property_rw vertices : (double * double) array
+ (** Property holding the current list of vertices for the planned
+ trajectory. *)
+
+ method add_vertice : (vertice : (double * double)) -> ()
+ (** Add a new vertice after all current ones to [vertices]. *)
+
+ method simplify : (tolerance : double) -> ()
+ (** Simplify the trajectory using the given tolerance. *)
+
+ (** Move the robot according to the current trajectory. *)
+ method go : (
+ rotation_speed : double,
+ rotation_acceleration : double,
+ moving_speed : double,
+ moving_acceleration : double
+ ) -> ()
+
+ method stop : () -> ()
+ (** Stop the current trajectory. *)
+
+ property_r moving : boolean
+ (** Is the robot currently following a trajectory ? *)
+}
diff --git a/info/control2011/src/lib/krobot_planner.ml b/info/control2011/src/lib/krobot_planner.ml
new file mode 100644
index 0000000..918b836
--- /dev/null
+++ b/info/control2011/src/lib/krobot_planner.ml
@@ -0,0 +1,39 @@
+(*
+ * krobot_planner.ml
+ * -----------------
+ * Copyright : (c) 2011, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+open Lwt
+open Lwt_react
+open Krobot_geom
+open Krobot_interface_planner.Fr_krobot_Planner
+
+type t = Krobot_bus.t
+
+let proxy bus =
+ OBus_proxy.make (OBus_peer.make (Krobot_bus.to_bus bus) "fr.krobot.Service.Planner") ["fr"; "krobot"; "Planner"]
+
+let vertices bus =
+ OBus_property.map_rw
+ (List.map (fun (x, y) -> { x; y }))
+ (List.map (fun { x; y } -> (x, y)))
+ (OBus_property.make p_vertices (proxy bus))
+
+let add_vertice bus { x; y } =
+ OBus_method.call m_add_vertice (proxy bus) (x, y)
+
+let simplify bus tolerance =
+ OBus_method.call m_simplify (proxy bus) tolerance
+
+let moving bus =
+ OBus_property.make p_moving (proxy bus)
+
+let go bus a b c d =
+ OBus_method.call m_go (proxy bus) (a, b, c, d)
+
+let stop bus =
+ OBus_method.call m_stop (proxy bus) ()
diff --git a/info/control2011/src/lib/krobot_planner.mli b/info/control2011/src/lib/krobot_planner.mli
new file mode 100644
index 0000000..2287ee7
--- /dev/null
+++ b/info/control2011/src/lib/krobot_planner.mli
@@ -0,0 +1,30 @@
+(*
+ * krobot_planner.mli
+ * ------------------
+ * Copyright : (c) 2011, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+type t = Krobot_bus.t
+ (** Type of the planner. *)
+
+val vertices : t -> (Krobot_geom.vertice list, [ `readable | `writable ]) OBus_property.t
+ (** The property holding the current trajectory. *)
+
+val add_vertice : t -> Krobot_geom.vertice -> unit Lwt.t
+ (** Add a vertice at the end of the current trajectory. *)
+
+val simplify : t -> float -> unit Lwt.t
+ (** [simplify planner tolerance] simplify the trajectory. *)
+
+val moving : t -> (bool, [ `readable ]) OBus_property.t
+ (** Is the robot currently following a trajectory ? *)
+
+val go : t -> float -> float -> float -> float -> unit Lwt.t
+ (** [go planner rotation_speed rotation_acceleration moving_speed
+ moving_acceleration] make the robot to follow the trajectory. *)
+
+val stop : t -> unit Lwt.t
+ (** Stop the current trajectory. *)
diff --git a/info/control2011/src/services/fr.krobot.Service.Planner.service.ab b/info/control2011/src/services/fr.krobot.Service.Planner.service.ab
new file mode 100644
index 0000000..5fa58a5
--- /dev/null
+++ b/info/control2011/src/services/fr.krobot.Service.Planner.service.ab
@@ -0,0 +1,3 @@
+[D-BUS Service]
+Name=fr.krobot.Service.Planner
+Exec=$(bindir)/bin/krobot-service-planner
diff --git a/info/control2011/src/services/krobot_service_planner.ml b/info/control2011/src/services/krobot_service_planner.ml
new file mode 100644
index 0000000..f33d448
--- /dev/null
+++ b/info/control2011/src/services/krobot_service_planner.ml
@@ -0,0 +1,284 @@
+(*
+ * krobot_service_planner.ml
+ * -------------------------
+ * Copyright : (c) 2011, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(* The trajectory planner. *)
+
+open Lwt
+open Lwt_react
+open Krobot_geom
+open Krobot_config
+open Krobot_message
+open Krobot_interface_planner.Fr_krobot_Planner
+
+let pi = 4. *. atan 1.
+
+let math_mod_float a b =
+ let b2 = b /. 2. in
+ let modf = mod_float a b in
+ if modf > b2 then
+ modf -. b
+ else if modf < -. b2 then
+ modf +. b
+ else
+ modf;;
+
+(* +-----------------------------------------------------------------+
+ | Types |
+ +-----------------------------------------------------------------+ *)
+
+type planner = {
+ bus : Krobot_bus.t;
+ (* The message bus used to communicate with the robot. *)
+
+ obus : planner OBus_object.t;
+ (* The D-Bus object attached to this planner. *)
+
+ vertices : vertice list signal;
+ (* The list of vertices for the trajectory. *)
+
+ set_vertices : vertice list -> unit;
+ (* Set the list of vertices. *)
+
+ moving : bool signal;
+ (* Is the robot moving ? *)
+
+ set_moving : bool -> unit;
+ (* Set the moving status. *)
+
+ mutable motors_moving : bool;
+ (* Are the motor currently active ? *)
+
+ mutable mover : unit Lwt.t;
+ (* The thread moving the robot. *)
+
+ mutable position : vertice;
+ (* The position of the robot on the board. *)
+
+ mutable orientation : float;
+ (* The orientation of the robot. *)
+
+ mutable event : unit event;
+ (* Event kept in the planner. *)
+}
+
+(* +-----------------------------------------------------------------+
+ | Primitives |
+ +-----------------------------------------------------------------+ *)
+
+let add_vertice planner vertice =
+ planner.set_vertices (S.value planner.vertices @ [vertice])
+
+let simplify planner tolerance =
+ match S.value planner.vertices with
+ | [] ->
+ ()
+ | points ->
+ let points = Array.of_list points in
+ let rec loop = function
+ | i1 :: i2 :: rest ->
+ let { x = x1; y = y1 } = points.(i1) and { x = x2; y = y2 } = points.(i2) in
+ let a = y2 -. y1 and b = x1 -. x2 and c = x2 *. y1 -. x1 *. y2 in
+ let r = sqrt (a *. a +. b *. b) in
+ if r <> 0. then begin
+ (* Search the furthest point from the line passing by (x1,
+ y1) and (x2, y2) *)
+ let max_dist = ref 0. and at_max = ref i1 in
+ for i = i1 + 1 to i2 - 1 do
+ let { x; y } = points.(i) in
+ let d = abs_float (a *. x +. b *. y +. c) /. r in
+ if d > !max_dist then begin
+ max_dist := d;
+ at_max := i
+ end
+ done;
+ if !max_dist > tolerance then
+ (* The furthest point is out of tolerance, we split the
+ current region with it. *)
+ loop (i1 :: !at_max :: i2 :: rest)
+ else
+ (* The point is acceptable, we pass the next region. *)
+ i1:: loop (i2 :: rest)
+ end else
+ (* The two point are the same so we drop one. *)
+ loop (i2 :: rest)
+ | rest ->
+ rest
+ in
+ let result = loop [0; Array.length points - 1] in
+ planner.set_vertices (List.map (fun i -> points.(i)) result)
+
+let wait_done planner =
+ lwt () = Lwt_log.info "waiting for the robot to stop moving" in
+ lwt () = Lwt_unix.sleep 0.3 in
+ lwt () =
+ while_lwt planner.motors_moving do
+ Lwt_unix.sleep 0.2
+ done
+ in
+ Lwt_log.info "trajectory done"
+
+let go planner rotation_speed rotation_acceleration moving_speed moving_acceleration =
+ if S.value planner.moving then
+ return ()
+ else begin
+ let rec loop () =
+ match S.value planner.vertices with
+ | { x; y } :: rest ->
+ let sqr x = x *. x in
+ let radius = sqrt (sqr (max wheels_position (robot_size -. wheels_position)) +. sqr (robot_size /. 2.)) in
+ if x >= radius && x <= world_width -. radius && y >= radius && y <= world_height -. radius then begin
+ (* Turn the robot. *)
+ let alpha = math_mod_float (atan2 (y -. planner.position.y) (x -. planner.position.x) -. planner.orientation) (2. *. pi) in
+ lwt () = Lwt_log.info_f "turning by %f radiants" alpha in
+ lwt () = Krobot_message.send planner.bus (Unix.gettimeofday (),
+ Motor_turn(alpha,
+ rotation_speed,
+ rotation_acceleration)) in
+ lwt () = wait_done planner in
+
+ (* Move the robot. *)
+ let dist = sqrt (sqr (x -. planner.position.x) +. sqr (y -. planner.position.y)) in
+ lwt () = Lwt_log.info_f "moving by %f meters" dist in
+ lwt () = Krobot_message.send planner.bus (Unix.gettimeofday (),
+ Motor_move(dist,
+ moving_speed,
+ moving_acceleration)) in
+ lwt () = wait_done planner in
+
+ (* Remove the point. *)
+ (match S.value planner.vertices with
+ | _ :: l -> planner.set_vertices l
+ | [] -> ());
+
+ loop ()
+ end else
+ Lwt_log.warning_f "can not move to (%f, %f)" x y
+ | [] ->
+ return ()
+ in
+ planner.set_moving true;
+ planner.mover <- (
+ try_lwt
+ loop ()
+ finally
+ planner.set_moving false;
+ return ()
+ );
+ return ()
+ end
+
+let stop planner =
+ cancel planner.mover;
+ planner.set_moving false;
+ planner.set_vertices [];
+ Krobot_message.send planner.bus (Unix.gettimeofday (), Motor_stop)
+
+(* +-----------------------------------------------------------------+
+ | Object creation |
+ +-----------------------------------------------------------------+ *)
+
+let create bus =
+ let obus_object =
+ OBus_object.make
+ ~interfaces:[
+ Krobot_interface_planner.Fr_krobot_Planner.make {
+ p_vertices =
+ ((fun obj ->
+ S.map (List.map (fun { x; y } -> (x, y))) (OBus_object.get obj).vertices),
+ (fun obj vertices ->
+ (OBus_object.get obj).set_vertices (List.map (fun (x, y) -> { x; y }) vertices);
+ return ()));
+ p_moving =
+ (fun obj ->
+ (OBus_object.get obj).moving);
+ m_simplify =
+ (fun obj tolerance ->
+ simplify (OBus_object.get obj) tolerance;
+ return ());
+ m_add_vertice =
+ (fun obj (x, y) ->
+ add_vertice (OBus_object.get obj) { x; y };
+ return ());
+ m_go =
+ (fun obj (a, b, c, d) ->
+ go (OBus_object.get obj) a b c d);
+ m_stop =
+ (fun obj () ->
+ stop (OBus_object.get obj));
+ };
+ ]
+ ["fr"; "krobot"; "Planner"]
+ in
+ let vertices, set_vertices = S.create [] in
+ let moving, set_moving = S.create false in
+ let planner = {
+ bus;
+ obus = obus_object;
+ vertices;
+ set_vertices;
+ moving;
+ set_moving;
+ motors_moving = false;
+ mover = return ();
+ position = { x = 0.; y = 0. };
+ orientation = 0.;
+ event = E.never;
+ } in
+ OBus_object.attach obus_object planner;
+
+ planner.event <- (
+ E.map
+ (fun (ts, frame) ->
+ match frame with
+ | Odometry(x, y, theta) ->
+ planner.position <- { x; y };
+ planner.orientation <- math_mod_float theta (2. *. pi)
+ | Motor_status(m1, m2, m3, m4) ->
+ planner.motors_moving <- m1 || m2
+ | _ ->
+ ())
+ (Krobot_message.recv bus)
+ );
+
+ planner
+
+(* +-----------------------------------------------------------------+
+ | Command-line arguments |
+ +-----------------------------------------------------------------+ *)
+
+let fork = ref true
+let kill = ref false
+
+let options = Arg.align [
+ "-no-fork", Arg.Clear fork, " Run in foreground";
+ "-kill", Arg.Set kill, " Kill any running planner and exit";
+]
+
+let usage = "\
+Usage: krobot-planner [options]
+options are:"
+
+(* +-----------------------------------------------------------------+
+ | Entry point |
+ +-----------------------------------------------------------------+ *)
+
+lwt () =
+ Arg.parse options ignore usage;
+
+ lwt bus = Krobot_bus.get () in
+ lwt () = Krobot_service.init bus ~kill:!kill ~fork:!fork "Planner" in
+
+ (* Create a new planner. *)
+ let planner = create bus in
+
+ (* Export it on the krobot bus. *)
+ OBus_object.export (Krobot_bus.to_bus bus) planner.obus;
+
+ (* Wait forever. *)
+ fst (wait ())
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index 91ac856..d93c17f 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -194,13 +194,22 @@ module Board = struct
type t = {
bus : Krobot_bus.t;
+ (* The D-Bus message bus used by this board. *)
+
ui : Krobot_viewer_ui.window;
+ (* The UI of the board. *)
+
mutable state : state;
+ (* The state of the robot. *)
+
mutable beacon : beacon;
- mutable points : vertice list;
- mutable bezier : vertice array list;
- mutable event : unit event;
- mutable moving : bool;
+ (* The state of the beacon. *)
+
+ vertices : vertice list signal;
+ (* The current trajectory. *)
+
+ mutable events : unit event list;
+ (* List of events kept in the board. *)
}
type color =
@@ -406,11 +415,13 @@ module Board = struct
(* Draw points. *)
Cairo.set_source_rgb ctx 255. 255. 0.;
Cairo.move_to ctx board.state.pos.x board.state.pos.y;
- List.iter (fun { x; y } -> Cairo.line_to ctx x y) board.points;
+ List.iter (fun { x; y } -> Cairo.line_to ctx x y) (S.value board.vertices);
Cairo.stroke ctx;
+(*
Cairo.set_source_rgb ctx 255. 0. 255.;
List.iter (Array.iter (fun { x; y } -> Cairo.line_to ctx x y)) board.bezier;
Cairo.stroke ctx;
+*)
let ctx = Cairo_lablgtk.create board.ui#scene#misc#window in
Cairo.set_source_surface ctx surface 0. 0.;
@@ -428,15 +439,11 @@ module Board = struct
let scale = dw /. (world_width +. 0.204) in
let x0 = (width -. dw) /. 2. and y0 = (height -. dh) /. 2. in
let x = (x -. x0) /. scale -. 0.102 and y = world_height -. ((y -. y0) /. scale -. 0.102) in
- if x >= 0. && x < world_width && y >= 0. && y < world_height then begin
- board.points <- board.points @ [{ x; y }];
- queue_draw board
- end
+ if x >= 0. && x < world_width && y >= 0. && y < world_height then
+ ignore_result (Krobot_planner.add_vertice board.bus { x; y })
let clear board =
- board.points <- [];
- board.bezier <- [];
- queue_draw board
+ ignore_result (OBus_property.set (Krobot_planner.vertices board.bus) [])
let rec last = function
| [] -> failwith "Krobot_viewer.Board.last"
@@ -444,42 +451,10 @@ module Board = struct
| _ :: l -> last l
let smooth board =
- let points = Array.of_list ({ x = board.state.pos.x; y = board.state.pos.y } :: board.points) in
let tolerance = board.ui#tolerance#adjustment#value in
- let rec loop = function
- | i1 :: i2 :: rest ->
- let { x = x1; y = y1 } = points.(i1) and { x = x2; y = y2 } = points.(i2) in
- let a = y2 -. y1 and b = x1 -. x2 and c = x2 *. y1 -. x1 *. y2 in
- let r = sqrt (a *. a +. b *. b) in
- if r <> 0. then begin
- (* Search the furthest point from the line passing by (x1,
- y1) and (x2, y2) *)
- let max_dist = ref 0. and at_max = ref i1 in
- for i = i1 + 1 to i2 - 1 do
- let { x; y } = points.(i) in
- let d = abs_float (a *. x +. b *. y +. c) /. r in
- if d > !max_dist then begin
- max_dist := d;
- at_max := i
- end
- done;
- if !max_dist > tolerance then
- (* The furthest point is out of tolerance, we split the
- current region with it. *)
- loop (i1 :: !at_max :: i2 :: rest)
- else
- (* The point is acceptable, we pass the next region. *)
- i1:: loop (i2 :: rest)
- end else
- (* The two point are the same so we drop one. *)
- loop (i2 :: rest)
- | rest ->
- rest
- in
- let result = List.tl (loop [0; Array.length points - 1]); in
- board.points <- List.map (fun i -> points.(i)) result;
+ ignore_result (Krobot_planner.simplify board.bus tolerance)
- let bezier_vertices q r v1 v2 =
+(* let bezier_vertices q r v1 v2 =
(* Create the bezier curve. *)
let curve = Bezier.of_vertices q (translate q v1) (translate r v2) r in
@@ -530,62 +505,10 @@ module Board = struct
end;
queue_draw board
-
- let wait_done board =
- lwt () = Lwt_log.info "waiting for the robot to stop moving" in
- lwt () = Lwt_unix.sleep 0.3 in
- lwt () =
- while_lwt board.moving do
- Lwt_unix.sleep 0.2
- done
- in
- Lwt_log.info "trajectory done"
-
- let go board =
- let rec loop () =
- match board.points with
- | { x; y } :: rest ->
- let sqr x = x *. x in
- let radius = sqrt (sqr (max wheels_position (robot_size -. wheels_position)) +. sqr (robot_size /. 2.)) in
- if x >= radius && x <= world_width -. radius && y >= radius && y <= world_height -. radius then begin
- (* Turn the robot. *)
- let alpha = math_mod_float (atan2 (y -. board.state.pos.y) (x -. board.state.pos.x) -. board.state.theta) (2. *. pi) in
- lwt () = Lwt_log.info_f "turning by %f radiants" alpha in
- lwt () = Krobot_message.send board.bus (Unix.gettimeofday (),
- Motor_turn(alpha,
- board.ui#rotation_speed#adjustment#value,
- board.ui#rotation_acceleration#adjustment#value)) in
- lwt () = wait_done board in
-
- (* Move the robot. *)
- let dist = sqrt (sqr (x -. board.state.pos.x) +. sqr (y -. board.state.pos.y)) in
- lwt () = Lwt_log.info_f "moving by %f meters" dist in
- lwt () = Krobot_message.send board.bus (Unix.gettimeofday (),
- Motor_move(dist,
- board.ui#moving_speed#adjustment#value,
- board.ui#moving_acceleration#adjustment#value)) in
- lwt () = wait_done board in
-
- (* Remove the point. *)
- (match board.points with
- | _ :: l -> board.points <- l
- | [] -> ());
- (match board.bezier with
- | _ :: l -> board.bezier <- l
- | [] -> ());
-
- (* Redraw everything without the last point. *)
- queue_draw board;
-
- loop ()
- end else
- Lwt_log.warning_f "can not move to (%f, %f)" x y
- | [] ->
- return ()
- in
- loop ()
+*)
let create bus ui =
+ lwt vertices = OBus_property.monitor (Krobot_planner.vertices bus) in
let board ={
bus;
ui;
@@ -595,10 +518,8 @@ module Board = struct
theta = -0.5 *. pi
};
beacon = { xbeacon = 1.; ybeacon = 1.; valid = false };
- points = [];
- bezier = [];
- event = E.never;
- moving = false;
+ vertices;
+ events = [];
} in
board.ui#beacon_status#set_text "-";
board.ui#beacon_distance#set_text "-";
@@ -607,7 +528,8 @@ module Board = struct
queue_draw board;
(* Move the robot on the board when we receive odometry
informations. *)
- board.event <- (
+ board.events <- [
+ (* Handle CAN frames. *)
E.map
(fun (ts, frame) ->
match frame with
@@ -622,7 +544,6 @@ module Board = struct
queue_draw board
end
| Motor_status(m1, m2, m3, m4) ->
- board.moving <- m1 || m2;
board.ui#entry_moving1#set_text (if m1 then "yes" else "no");
board.ui#entry_moving2#set_text (if m2 then "yes" else "no");
board.ui#entry_moving3#set_text (if m3 then "yes" else "no");
@@ -648,9 +569,12 @@ module Board = struct
board.ui#menu_mode_normal#set_active true
| _ ->
())
- (Krobot_message.recv bus)
- );
- board
+ (Krobot_message.recv bus);
+
+ (* Redraw everything when the list of vertices changes. *)
+ E.map (fun _ -> queue_draw board) (S.changes vertices);
+ ];
+ return board
end
(* +-----------------------------------------------------------------+
@@ -696,7 +620,7 @@ lwt () =
let lcd = LCD.create ui in
ignore (ui#lcd#event#connect#expose (fun ev -> LCD.draw lcd; true));
- let board = Board.create bus ui in
+ lwt board = Board.create bus ui in
ignore (ui#scene#event#connect#expose (fun ev -> Board.draw board; true));
ignore
(ui#scene#event#connect#button_press
@@ -723,25 +647,22 @@ lwt () =
Board.smooth board;
false));
- let thread_go = ref (return ()) in
ignore
(ui#button_go#event#connect#button_release
(fun ev ->
if GdkEvent.Button.button ev = 1 then
ignore_result (
- ui#button_go#misc#set_sensitive false;
- try_lwt
- thread_go := Board.go board;
- !thread_go
- with
- | Canceled ->
- return ()
- finally
- ui#button_go#misc#set_sensitive true;
- return ()
+ Krobot_planner.go bus
+ ui#rotation_speed#adjustment#value
+ ui#rotation_acceleration#adjustment#value
+ ui#moving_speed#adjustment#value
+ ui#moving_acceleration#adjustment#value
);
false));
+ lwt moving = OBus_property.monitor (Krobot_planner.moving bus) in
+ S.keep (S.map (fun state -> ui#button_go#misc#set_sensitive (not state)) moving);
+
ignore
(ui#button_start_red#event#connect#button_release
(fun ev ->
@@ -767,13 +688,8 @@ lwt () =
ignore
(ui#button_stop#event#connect#button_release
(fun ev ->
- if GdkEvent.Button.button ev = 1 then begin
- Board.clear board;
- cancel !thread_go;
- ignore_result (
- Krobot_message.send bus (Unix.gettimeofday (), Motor_stop)
- )
- end;
+ if GdkEvent.Button.button ev = 1 then
+ ignore_result (Krobot_planner.stop bus);
false));
ignore
hooks/post-receive
--
krobot
|
