Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 3f6b2f56b27416cf35ffbc0d5fa6d67eb8b6b0ea
|
From: Jérémie D. <Ba...@us...> - 2011-06-12 15:23:42
|
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 3f6b2f56b27416cf35ffbc0d5fa6d67eb8b6b0ea (commit)
from 86cafa05a128e93f4e8630479c9669bc495fb006 (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 3f6b2f56b27416cf35ffbc0d5fa6d67eb8b6b0ea
Author: Jérémie Dimino <je...@di...>
Date: Sat Jun 11 22:29:20 2011 +0200
[info] rewrite path planning using a system of actions
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/_oasis b/info/control2011/_oasis
index acedcc8..1f4cca0 100644
--- a/info/control2011/_oasis
+++ b/info/control2011/_oasis
@@ -41,7 +41,8 @@ Library krobot
Krobot_solve,
Krobot_pathfinding,
Krobot_config,
- Krobot_path
+ Krobot_path,
+ Krobot_action
CSources:
krobot_message_stubs.c
@@ -193,11 +194,18 @@ Executable "krobot-sharps"
MainIs: krobot_sharps.ml
BuildDepends: krobot, lwt.syntax
-Executable "krobot-homo"
+Executable "krobot-vm"
Path: src/tools
Install: true
CompiledObject: best
- MainIs: krobot_homo.ml
+ MainIs: krobot_vm.ml
+ BuildDepends: krobot, lwt.syntax
+
+Executable "krobot-ia"
+ Path: src/tools
+ Install: true
+ CompiledObject: best
+ MainIs: krobot_ia.ml
BuildDepends: krobot, lwt.syntax
# +-------------------------------------------------------------------+
diff --git a/info/control2011/_tags b/info/control2011/_tags
index 2f33846..79de459 100644
--- a/info/control2011/_tags
+++ b/info/control2011/_tags
@@ -2,7 +2,7 @@
<src/interfaces/*.ml>: -syntax_camlp4o
# OASIS_START
-# DO NOT EDIT (digest: 5752a329a86d17ea144b965fdf9cbca2)
+# DO NOT EDIT (digest: 4e49eb45e4c273e67ec75220ea03b734)
# Ignore VCS directories, you can use the same kind of rule outside
# OASIS_START/STOP if you want to exclude directories that contains
# useless stuff for the build process
@@ -100,6 +100,11 @@
<examples/*.ml{,i}>: pkg_lwt.unix
<examples/*.ml{,i}>: pkg_lwt.syntax
<examples/*.ml{,i}>: pkg_lwt.react
+# Executable krobot-vm
+<src/tools/krobot_vm.{native,byte}>: use_krobot
+<src/tools/krobot_vm.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_vm.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_vm.{native,byte}>: pkg_lwt.react
# Executable krobot-webcam
<src/tools/krobot_webcam.{native,byte}>: use_krobot
<src/tools/krobot_webcam.{native,byte}>: pkg_lwt.unix
@@ -110,6 +115,11 @@
<src/tools/krobot_objects.{native,byte}>: pkg_lwt.unix
<src/tools/krobot_objects.{native,byte}>: pkg_lwt.syntax
<src/tools/krobot_objects.{native,byte}>: pkg_lwt.react
+# Executable krobot-ia
+<src/tools/krobot_ia.{native,byte}>: use_krobot
+<src/tools/krobot_ia.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_ia.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_ia.{native,byte}>: pkg_lwt.react
# Executable krobot-replay
<src/tools/krobot_replay.{native,byte}>: use_krobot
<src/tools/krobot_replay.{native,byte}>: pkg_lwt.unix
@@ -146,11 +156,6 @@
<src/tools/krobot_dump_encoders.{native,byte}>: pkg_lwt.unix
<src/tools/krobot_dump_encoders.{native,byte}>: pkg_lwt.syntax
<src/tools/krobot_dump_encoders.{native,byte}>: pkg_lwt.react
-# Executable krobot-homo
-<src/tools/krobot_homo.{native,byte}>: use_krobot
-<src/tools/krobot_homo.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_homo.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_homo.{native,byte}>: pkg_lwt.react
<src/tools/*.ml{,i}>: use_krobot
<src/tools/*.ml{,i}>: pkg_lwt.unix
<src/tools/*.ml{,i}>: pkg_lwt.syntax
diff --git a/info/control2011/krobot-api.odocl b/info/control2011/krobot-api.odocl
index 21068ef..e19a38b 100644
--- a/info/control2011/krobot-api.odocl
+++ b/info/control2011/krobot-api.odocl
@@ -1,5 +1,5 @@
# OASIS_START
-# DO NOT EDIT (digest: 8446681ae962223329ec305b5442f818)
+# DO NOT EDIT (digest: 7edbd90b2355975a6b906edaa0a69b3e)
src/lib/Krobot_can
src/lib/Krobot_bus
src/lib/Krobot_message
@@ -8,5 +8,6 @@ src/lib/Krobot_solve
src/lib/Krobot_pathfinding
src/lib/Krobot_config
src/lib/Krobot_path
+src/lib/Krobot_action
src/can/Krobot_can_bus
# OASIS_STOP
diff --git a/info/control2011/setup.ml b/info/control2011/setup.ml
index e69e702..4f9b978 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: 0b1f4d2eae96725b18adca89d9662a5e) *)
+(* DO NOT EDIT (digest: 2fa81b2af9e6c805314db247864ab4e1) *)
(*
Regenerated by OASIS v0.2.1~alpha1
Visit http://oasis.forge.ocamlcore.org for more information and
@@ -5007,7 +5007,8 @@ let setup_t =
"Krobot_solve";
"Krobot_pathfinding";
"Krobot_config";
- "Krobot_path"
+ "Krobot_path";
+ "Krobot_action"
];
lib_internal_modules = [];
lib_findlib_parent = None;
@@ -5420,6 +5421,33 @@ let setup_t =
{exec_custom = false; exec_main_is = "send_can.ml"; });
Executable
({
+ cs_name = "krobot-vm";
+ cs_data = PropList.Data.create ();
+ cs_plugin_data = [];
+ },
+ {
+ bs_build = [(OASISExpr.EBool true, true)];
+ bs_install = [(OASISExpr.EBool true, true)];
+ bs_path = "src/tools";
+ bs_compiled_object = Best;
+ bs_build_depends =
+ [
+ InternalLibrary "krobot";
+ FindlibPackage ("lwt.syntax", None)
+ ];
+ bs_build_tools = [ExternalTool "ocamlbuild"];
+ bs_c_sources = [];
+ bs_data_files = [];
+ 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_vm.ml"; });
+ Executable
+ ({
cs_name = "krobot-webcam";
cs_data = PropList.Data.create ();
cs_plugin_data = [];
@@ -5475,6 +5503,33 @@ let setup_t =
});
Executable
({
+ cs_name = "krobot-ia";
+ cs_data = PropList.Data.create ();
+ cs_plugin_data = [];
+ },
+ {
+ bs_build = [(OASISExpr.EBool true, true)];
+ bs_install = [(OASISExpr.EBool true, true)];
+ bs_path = "src/tools";
+ bs_compiled_object = Best;
+ bs_build_depends =
+ [
+ InternalLibrary "krobot";
+ FindlibPackage ("lwt.syntax", None)
+ ];
+ bs_build_tools = [ExternalTool "ocamlbuild"];
+ bs_c_sources = [];
+ bs_data_files = [];
+ 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_ia.ml"; });
+ Executable
+ ({
cs_name = "krobot-replay";
cs_data = PropList.Data.create ();
cs_plugin_data = [];
@@ -5663,33 +5718,6 @@ let setup_t =
});
Executable
({
- cs_name = "krobot-homo";
- cs_data = PropList.Data.create ();
- cs_plugin_data = [];
- },
- {
- bs_build = [(OASISExpr.EBool true, true)];
- bs_install = [(OASISExpr.EBool true, true)];
- bs_path = "src/tools";
- bs_compiled_object = Best;
- bs_build_depends =
- [
- InternalLibrary "krobot";
- FindlibPackage ("lwt.syntax", None)
- ];
- bs_build_tools = [ExternalTool "ocamlbuild"];
- bs_c_sources = [];
- bs_data_files = [];
- 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_homo.ml"; });
- Executable
- ({
cs_name = "krobot-driver";
cs_data = PropList.Data.create ();
cs_plugin_data = [];
diff --git a/info/control2011/src/lib/krobot.mllib b/info/control2011/src/lib/krobot.mllib
index 9dc39d3..44dbb2b 100644
--- a/info/control2011/src/lib/krobot.mllib
+++ b/info/control2011/src/lib/krobot.mllib
@@ -1,5 +1,5 @@
# OASIS_START
-# DO NOT EDIT (digest: 88a10a3a673ab385eafde0a601e2a247)
+# DO NOT EDIT (digest: 39a92ff71a82fc1bed00795c4d3b6319)
Krobot_can
Krobot_bus
Krobot_message
@@ -8,4 +8,5 @@ Krobot_solve
Krobot_pathfinding
Krobot_config
Krobot_path
+Krobot_action
# OASIS_STOP
diff --git a/info/control2011/src/lib/krobot_action.ml b/info/control2011/src/lib/krobot_action.ml
new file mode 100644
index 0000000..6255e48
--- /dev/null
+++ b/info/control2011/src/lib/krobot_action.ml
@@ -0,0 +1,71 @@
+(*
+ * krobot_action.ml
+ * ----------------
+ * Copyright : (c) 2011, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+open Printf
+open Krobot_geom
+
+type t =
+ | Node of t list
+ | Stop
+ | Think
+ | Goto of vertice
+ | Follow_path of vertice list
+ | Bezier of float * vertice * vertice * vertice * vertice * float
+ | Set_curve of Bezier.curve option
+ | Wait_for_jack of bool
+ | Wait_for_moving of bool
+ | Reset_odometry of [ `Red | `Blue | `Auto ]
+ | Wait_for_odometry of [ `Eq | `Gt | `Ge | `Lt | `Le ] * int
+
+let string_of_vertice { x; y } = sprintf "{ x = %f; y = %f }" x y
+
+let rec to_string = function
+ | Node l ->
+ sprintf "Node [%s]" (String.concat "; " (List.map to_string l))
+ | Stop ->
+ "Stop"
+ | Think ->
+ "Think"
+ | Goto v ->
+ sprintf "Goto %s" (string_of_vertice v)
+ | Follow_path l ->
+ sprintf "Follow_path [%s]" (String.concat "; " (List.map string_of_vertice l))
+ | Bezier(sign, p, q, r, s, end_velocity) ->
+ sprintf
+ "Bezier(%f, %s, %s, %s, %s, %f)"
+ sign
+ (string_of_vertice p)
+ (string_of_vertice q)
+ (string_of_vertice r)
+ (string_of_vertice s)
+ end_velocity
+ | Set_curve(Some c) ->
+ sprintf "Set_curve(Some %s)" (Bezier.string_of_curve c)
+ | Set_curve None ->
+ "Set_curve None"
+ | Wait_for_jack st ->
+ sprintf "Wait_for_jack %B" st
+ | Wait_for_moving st ->
+ sprintf "Wait_for_moving %B" st
+ | Reset_odometry `Red ->
+ "Reset_odometry `Red"
+ | Reset_odometry `Blue ->
+ "Reset_odometry `Blue"
+ | Reset_odometry `Auto ->
+ "Reset_odometry `Auto"
+ | Wait_for_odometry(test, value) ->
+ sprintf
+ "Wait_for_odometry(`%s, %d)"
+ (match test with
+ | `Eq -> "Eq"
+ | `Gt -> "Gt"
+ | `Ge -> "Ge"
+ | `Lt -> "Lt"
+ | `Le -> "Le")
+ value
diff --git a/info/control2011/src/lib/krobot_action.mli b/info/control2011/src/lib/krobot_action.mli
new file mode 100644
index 0000000..72e953a
--- /dev/null
+++ b/info/control2011/src/lib/krobot_action.mli
@@ -0,0 +1,46 @@
+(*
+ * krobot_action.mli
+ * -----------------
+ * Copyright : (c) 2011, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(** Robot's actions *)
+
+open Krobot_geom
+
+(** Type of actions. *)
+type t =
+ | Node of t list
+ (** A sequence of action to execute in order. *)
+ | Stop
+ (** Stop all actions. *)
+ | Think
+ (** This is the highest-level action. It instruct the VM to
+ think about a new strategy. *)
+ | Goto of vertice
+ (** Go to the given point. *)
+ | Follow_path of vertice list
+ (** Follow the given path. It does not check for obstacles. *)
+ | Bezier of float * vertice * vertice * vertice * vertice * float
+ (** Follow the bezier curve determined by the given four
+ vertices. The first float is the sign, the last one is the
+ end velocity. *)
+ | Set_curve of Bezier.curve option
+ (** Set the curve currently being followed. *)
+ | Wait_for_jack of bool
+ (** Wait for the jack to be in the given state. *)
+ | Wait_for_moving of bool
+ (** Wait for the robot to move or not. *)
+ | Reset_odometry of [ `Red | `Blue | `Auto ]
+ (** Set the odometry to the initial position according to the
+ team selector or the given color. *)
+ | Wait_for_odometry of [ `Eq | `Gt | `Ge | `Lt | `Le ] * int
+ (** Wait for the curve parameter of the odometry to reach the
+ given state. *)
+
+val to_string : t -> string
+ (** [to_string action] returns the string representation of the
+ given string. *)
diff --git a/info/control2011/src/lib/krobot_bus.ml b/info/control2011/src/lib/krobot_bus.ml
index b55ef14..bb2d08a 100644
--- a/info/control2011/src/lib/krobot_bus.ml
+++ b/info/control2011/src/lib/krobot_bus.ml
@@ -25,17 +25,18 @@ type message =
| Log of string
| Send
| Kill of string
- | Trajectory_vertices of vertice list * (vertice * vertice * vertice * vertice) list
+ | Trajectory_path of Bezier.curve list
| Trajectory_set_vertices of vertice list
| Trajectory_add_vertice of vertice
| Trajectory_simplify of float
- | Trajectory_go of float * float * float * float
- | Trajectory_goto of vertice
- | Trajectory_stop
- | Trajectory_moving of bool
+ | Trajectory_go
| Trajectory_find_path
| Objects of vertice list
| Sharps of float array
+ | Strategy_append of Krobot_action.t list
+ | Strategy_stop
+ | Strategy_set of Krobot_action.t list
+ | Strategy_path of Bezier.curve list option
type t = {
oc : Lwt_io.output_channel;
@@ -70,19 +71,10 @@ let string_of_message = function
sprintf
"Kill %S"
name
- | Trajectory_vertices(vertices, curves) ->
+ | Trajectory_path curves ->
sprintf
- "Trajectory_vertices([%s], [%s])"
- (String.concat "; " (List.map string_of_vertice vertices))
- (String.concat "; " (List.map
- (fun (p, q, r, s) ->
- Printf.sprintf
- "(%s, %s, %s, %s)"
- (string_of_vertice p)
- (string_of_vertice q)
- (string_of_vertice r)
- (string_of_vertice s))
- curves))
+ "Trajectory_path [%s]"
+ (String.concat "; " (List.map Bezier.string_of_curve curves))
| Trajectory_set_vertices l ->
sprintf
"Trajectory_set_vertices [%s]"
@@ -95,20 +87,8 @@ let string_of_message = function
sprintf
"Trajectory_simplify %f"
tolerance
- | Trajectory_go(a, b, c, d) ->
- sprintf
- "Trajectory_go(%f, %f, %f, %f)"
- a b c d
- | Trajectory_goto v ->
- sprintf
- "Trajectory_goto %s"
- (string_of_vertice v)
- | Trajectory_stop ->
- "Trajectory_stop"
- | Trajectory_moving b ->
- sprintf
- "Trajectory_moving %B"
- b
+ | Trajectory_go ->
+ "Trajectory_go"
| Trajectory_find_path ->
"Trajectory_find_path"
| Objects objects ->
@@ -119,6 +99,18 @@ let string_of_message = function
sprintf
"Sharps [|%s|]"
(String.concat "; " (List.map string_of_float (Array.to_list a)))
+ | Strategy_append l ->
+ sprintf "Strategy_append [%s]" (String.concat "; " (List.map Krobot_action.to_string l))
+ | Strategy_stop ->
+ "Strategy_stop"
+ | Strategy_set l ->
+ sprintf "Strategy_set [%s]" (String.concat "; " (List.map Krobot_action.to_string l))
+ | Strategy_path None ->
+ "Strategy_path None"
+ | Strategy_path(Some curves) ->
+ sprintf
+ "Strategy_path(Some [%s])"
+ (String.concat "; " (List.map Bezier.string_of_curve curves))
(* +-----------------------------------------------------------------+
| Sending/receiving messages |
diff --git a/info/control2011/src/lib/krobot_bus.mli b/info/control2011/src/lib/krobot_bus.mli
index 17e8697..bef8110 100644
--- a/info/control2011/src/lib/krobot_bus.mli
+++ b/info/control2011/src/lib/krobot_bus.mli
@@ -34,24 +34,16 @@ type message =
(** Trajectory messages. *)
- | Trajectory_vertices of vertice list * (vertice * vertice * vertice * vertice) list
- (** The list of vertices for the planned trajectory, along with
- the bezier curves. *)
+ | Trajectory_path of Bezier.curve list
+ (** The planned trajectory. *)
| Trajectory_set_vertices of vertice list
(** Sets the trajectory. *)
| Trajectory_add_vertice of vertice
(** Add a vertice to the trajectory. *)
| Trajectory_simplify of float
(** Simplify the trajectory with the given tolerance. *)
- | Trajectory_go of float * float * float * float
- (** [Trajectory_go(rotation_speed, rotation_acceleration,
- moving_speed, moving_acceleration)]. *)
- | Trajectory_goto of vertice
- (** [Trajectory_goto dest] go to the given position. *)
- | Trajectory_stop
- (** Stop the current trajectory. *)
- | Trajectory_moving of bool
- (** Is the robot following a trajectory ? *)
+ | Trajectory_go
+ (** Follow currently registered trajectory. *)
| Trajectory_find_path
(** Find a path avoiding objects. *)
@@ -65,6 +57,18 @@ type message =
| Sharps of float array
(** Distances measured by the sharps. *)
+ (** Strategy *)
+
+ | Strategy_append of Krobot_action.t list
+ (** Append the given list of action to the current strategy. *)
+ | Strategy_stop
+ (** Stop and empty the current strategy. *)
+ | Strategy_set of Krobot_action.t list
+ (** Stop and replace the current stategy by the given one. *)
+ | Strategy_path of (Bezier.curve list) option
+ (** Message emitted when the robot starts or stops a
+ trajectory. *)
+
val string_of_message : message -> string
(** Returns a string representation of the given message. *)
diff --git a/info/control2011/src/lib/krobot_geom.ml b/info/control2011/src/lib/krobot_geom.ml
index 3d110fa..a7a1dea 100644
--- a/info/control2011/src/lib/krobot_geom.ml
+++ b/info/control2011/src/lib/krobot_geom.ml
@@ -98,6 +98,8 @@ let tangents a b c =
module Bezier = struct
type curve = {
+ src : vertice;
+ dst : vertice;
p : vector;
a : vector;
b : vector;
@@ -105,6 +107,7 @@ module Bezier = struct
}
let of_vertices p q r s =
+ let src = p and dst = s in
let p = vector origin p
and q = vector origin q
and r = vector origin r
@@ -112,7 +115,13 @@ module Bezier = struct
let c = (q -| p) *| 3. in
let b = (r -| q) *| 3. -| c in
let a = s -| p -| c -| b in
- { p; a; b; c }
+ { src; dst; p; a; b; c }
+
+ let src c = c.src
+ let dst c = c.dst
+
+ let string_of_curve c =
+ Printf.sprintf "<bezier { x = %f; y = %f } -> { x = %f; y = %f }>" c.src.x c.src.y c.dst.x c.dst.y
let make ~p ~s ~vp ~vs ~a ~error_max =
let sp = norm vp and ss = norm vs in
diff --git a/info/control2011/src/lib/krobot_geom.mli b/info/control2011/src/lib/krobot_geom.mli
index df40b0d..a2fbbd5 100644
--- a/info/control2011/src/lib/krobot_geom.mli
+++ b/info/control2011/src/lib/krobot_geom.mli
@@ -52,6 +52,16 @@ module Bezier : sig
type curve
(** Type of cubic Bezier curves. *)
+ val string_of_curve : curve -> string
+ (** Returns the string representation of the given bezier
+ curve. *)
+
+ val src : curve -> vertice
+ (** Return the source vertice of the given bezier curve. *)
+
+ val dst : curve -> vertice
+ (** Return the destination vertice of the given bezier curve. *)
+
val of_vertices : vertice -> vertice -> vertice -> vertice -> curve
(** [of_vertices p q r s] creates a bezier curve from the given
four control points. [p] and [s] are the first and end point
diff --git a/info/control2011/src/lib/krobot_path.ml b/info/control2011/src/lib/krobot_path.ml
index a092156..35ebb74 100644
--- a/info/control2011/src/lib/krobot_path.ml
+++ b/info/control2011/src/lib/krobot_path.ml
@@ -18,8 +18,11 @@ let rec prev_last = function
| _ :: l ->
prev_last l
-let goto_object ~src ~dst ~objects ~beacon =
+let find ~src ~dst ~objects ~beacon =
+ (* Remove objects that are near the destination. *)
let objects = List.filter (fun obj -> distance dst obj >= object_safety_distance) objects in
+ (* Remove objects that are near the curent position. *)
+ let objects = List.filter (fun obj -> distance src obj >= object_safety_distance +. 0.01) objects in
let l = List.map (fun v -> (v, object_safety_distance +. 0.01)) objects in
let l =
match beacon with
@@ -28,14 +31,15 @@ let goto_object ~src ~dst ~objects ~beacon =
| None ->
l
in
- match
- Krobot_pathfinding.find_path ~src ~dst
- ({ x = border_safety_distance;
- y = border_safety_distance},
- { x = world_width -. border_safety_distance;
- y = world_height -. border_safety_distance})
- l
- with
+ Krobot_pathfinding.find_path ~src ~dst
+ ({ x = border_safety_distance;
+ y = border_safety_distance},
+ { x = world_width -. border_safety_distance;
+ y = world_height -. border_safety_distance})
+ l
+
+let goto_object ~src ~dst ~objects ~beacon =
+ match find ~src ~dst ~objects ~beacon with
| Some p ->
let v = vector dst (prev_last (src :: p)) in
let v = v /| norm v in
diff --git a/info/control2011/src/lib/krobot_path.mli b/info/control2011/src/lib/krobot_path.mli
index 8720856..7fa8359 100644
--- a/info/control2011/src/lib/krobot_path.mli
+++ b/info/control2011/src/lib/krobot_path.mli
@@ -11,6 +11,10 @@
open Krobot_geom
+val find : src : vertice -> dst : vertice -> objects : vertice list -> beacon : vertice option -> vertice list option
+ (** [goto ~src ~dst ~objects ~beacon] find a path from [src] to
+ [dst]. *)
+
val goto_object : src : vertice -> dst : vertice -> objects : vertice list -> beacon : vertice option -> vertice option
(** [goto_object ~src ~dst ~objects ~beacon] returns the goal
position to move to to reach an object in a position to take
diff --git a/info/control2011/src/tools/krobot_homo.ml b/info/control2011/src/tools/krobot_homo.ml
deleted file mode 100644
index c6cca44..0000000
--- a/info/control2011/src/tools/krobot_homo.ml
+++ /dev/null
@@ -1,128 +0,0 @@
-(*
- * krobot_homo.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_bus
-open Krobot_message
-open Krobot_geom
-
-(* +-----------------------------------------------------------------+
- | Types |
- +-----------------------------------------------------------------+ *)
-
-type robot = {
- bus : Krobot_bus.t;
- (* The message bus used to communicate with the robot. *)
-
- mutable moving : bool;
- (* Is the robot moving ? *)
-
- mutable position : vertice;
- (* The position of the robot on the board. *)
-
- mutable orientation : float;
- (* The orientation of the robot. *)
-
- mutable objects : vertice list;
- (* The list of objects on the board. *)
-
- mutable beacon : vertice option;
- (* Position of the beacon. *)
-
- mutable jack : bool;
- (* Is the jack present or not ? *)
-}
-
-(* +-----------------------------------------------------------------+
- | Message handling |
- +-----------------------------------------------------------------+ *)
-
-let handle_message robot (timestamp, message) =
- match message with
- | CAN(_, frame) -> begin
- match decode frame with
- | Odometry(x, y, theta) ->
- robot.position <- { x; y };
- robot.orientation <- math_mod_float theta (2. *. pi)
-
- | Beacon_position(angle, distance, period) ->
- if distance <> 0. then
- let angle = math_mod_float (robot.orientation +. Krobot_config.rotary_beacon_index_pos +. angle) (2. *. pi) in
- robot.beacon <- Some{
- x = robot.position.x +. distance *. cos angle;
- y = robot.position.y +. distance *. sin angle;
- }
- else
- robot.beacon <- None
-
- | Switch1_status(b, _, _, _, _, _, _, _) ->
- robot.jack <- not b
-
- | _ ->
- ()
- end
-
- | Trajectory_moving moving ->
- robot.moving <- moving
-
- | Objects l ->
- robot.objects <- l
-
- | _ ->
- ()
-
-(* +-----------------------------------------------------------------+
- | Entry point |
- +-----------------------------------------------------------------+ *)
-
-lwt () =
- lwt bus = Krobot_bus.get () in
-
- let robot = {
- bus;
- moving = false;
- position = origin;
- orientation = 0.;
- objects = [];
- beacon = None;
- jack = true;
- } in
-
- E.keep (E.map (handle_message robot) (Krobot_bus.recv bus));
-
- lwt () =
- Krobot_message.send bus
- (Unix.gettimeofday (),
- Set_odometry(0.215 -. Krobot_config.robot_size /. 2. +. Krobot_config.wheels_position, 1.885, 0.))
- in
-
- lwt () = Lwt_unix.sleep 1.0 in
-
- lwt () =
- while_lwt robot.jack do
- Lwt_unix.sleep 0.1
- done
- in
-
- let v = { x = 1.675; y = 1.225 } in
- lwt () = Krobot_bus.send bus (Unix.gettimeofday (), Trajectory_goto v) in
- lwt () = Lwt_unix.sleep 1.0 in
- lwt () =
- while_lwt robot.moving do
- Lwt_unix.sleep 0.01
- done
- in
-
- lwt () = Krobot_message.send bus (Unix.gettimeofday (), Motor_turn(-3. *. pi /. 4. -. robot.orientation, 1., 2.)) in
- lwt () = Lwt_unix.sleep 4.0 in
- lwt () = Krobot_message.send bus (Unix.gettimeofday (), Motor_move(0.3, 0.2, 0.4)) in
- lwt () = Lwt_unix.sleep 2.0 in
-
- return ()
diff --git a/info/control2011/src/tools/krobot_ia.ml b/info/control2011/src/tools/krobot_ia.ml
new file mode 100644
index 0000000..37af235
--- /dev/null
+++ b/info/control2011/src/tools/krobot_ia.ml
@@ -0,0 +1,23 @@
+(*
+ * krobot_ia.ml
+ * ------------
+ * Copyright : (c) 2011, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(* The IA of the robot. *)
+
+open Krobot_bus
+open Krobot_action
+
+lwt () =
+ lwt bus = Krobot_bus.get () in
+ Krobot_bus.send bus
+ (Unix.gettimeofday (),
+ Strategy_set [
+ Wait_for_jack false;
+ Reset_odometry `Auto;
+ Think;
+ ])
diff --git a/info/control2011/src/tools/krobot_planner.ml b/info/control2011/src/tools/krobot_planner.ml
index dd154d3..e9b2142 100644
--- a/info/control2011/src/tools/krobot_planner.ml
+++ b/info/control2011/src/tools/krobot_planner.ml
@@ -27,23 +27,13 @@ type planner = {
(* The message bus used to communicate with the robot. *)
mutable vertices : vertice list;
- (* The list of vertices for the trajectory. *)
+ (* The list of vertices of the current trajectory. *)
- mutable curves : (float * vertice * vertice * vertice * vertice) list;
- (* The parameters of bezier curves. *)
+ mutable curves : Bezier.curve list;
+ (* The list of bezier curves of the current trajectory. *)
- mutable moving : bool;
- (* Is the robot moving ? *)
-
- mutable motors_moving : bool;
- (* Are the motor currently active ? *)
-
- mutable curve_status : int;
- (* The status of the trajectory controller on the curve (between 0
- and 255). *)
-
- mutable mover : unit Lwt.t;
- (* The thread moving the robot. *)
+ mutable following_path : bool;
+ (* Is the VM following a path ? *)
mutable position : vertice;
(* The position of the robot on the board. *)
@@ -56,9 +46,6 @@ type planner = {
mutable beacon : vertice option;
(* Position of the beacon. *)
-
- mutable event : unit event;
- (* Event kept in the planner. *)
}
(* +-----------------------------------------------------------------+
@@ -66,54 +53,18 @@ type planner = {
+-----------------------------------------------------------------+ *)
let find_path planner src dst =
- (* Remove objects that are near the destination. *)
- let objects = List.filter (fun obj -> distance dst obj >= object_safety_distance) planner.objects in
- (* Remove objects that are near the curent position. *)
- let objects = List.filter (fun obj -> distance src obj >= object_safety_distance +. 0.01) objects in
- let l = List.map (fun v -> (v, object_safety_distance +. 0.01)) objects in
- let l =
- match planner.beacon with
- | Some v ->
- (v, beacon_safety_distance +. 0.01) :: l
- | None ->
- l
- in
- Krobot_pathfinding.find_path ~src ~dst
- ({ x = border_safety_distance;
- y = border_safety_distance},
- { x = world_width -. border_safety_distance;
- y = world_height -. border_safety_distance})
- l
+ Krobot_path.find ~src ~dst ~objects:planner.objects ~beacon:planner.beacon
(* +-----------------------------------------------------------------+
| Primitives |
+-----------------------------------------------------------------+ *)
-let set_vertices planner ?curves vertices =
+let set_vertices planner vertices =
+ let v = { vx = cos planner.orientation; vy = sin planner.orientation } in
+ let curves = List.rev (Bezier.fold_curves (fun curve acc -> curve :: acc) v (planner.position :: vertices) []) in
planner.vertices <- vertices;
- (* If the robot is not moving, add its current position to the
- trajectory. *)
- let vertices =
- match planner.moving with
- | true -> vertices
- | false -> planner.position :: vertices
- in
- (* Compute bezier curves if not provided. *)
- let curves =
- match curves with
- | Some l ->
- l
- | None ->
- let v = { vx = cos planner.orientation; vy = sin planner.orientation } in
- List.rev (Bezier.fold_vertices (fun sign p q r s acc -> (sign, p, q, r, s) :: acc) v vertices [])
- in
planner.curves <- curves;
- ignore (Krobot_bus.send planner.bus (Unix.gettimeofday (), Trajectory_vertices(vertices,
- List.map (fun (sign, p, q, r, s) -> (p, q, r, s)) curves)))
-
-let set_moving planner moving =
- planner.moving <- moving;
- ignore (Krobot_bus.send planner.bus (Unix.gettimeofday (), Trajectory_moving moving))
+ ignore (Krobot_bus.send planner.bus (Unix.gettimeofday (), Trajectory_path curves))
let simplify planner tolerance =
match planner.vertices with
@@ -154,215 +105,6 @@ let simplify planner tolerance =
let result = loop [0; Array.length points - 1] in
set_vertices planner (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.1
- done
- in
- Lwt_log.info "trajectory done"
-
-let wait_middle planner =
- lwt () = Lwt_log.info "waiting for the robot to be in the middle of the trajectory" in
- lwt () = Lwt_unix.sleep 0.3 in
- lwt () =
- while_lwt planner.curve_status < 128 do
- Lwt_unix.sleep 0.001
- done
- in
- Lwt_log.info "robot in the middle of the trajectory"
-
-let wait_start planner =
- lwt () = Lwt_log.info "waiting for the robot to start the new trajectory" in
- lwt () =
- while_lwt planner.curve_status >= 128 do
- Lwt_unix.sleep 0.001
- done
- in
- Lwt_log.info "robot started the new trajectory"
-
-(* Send a bezier curve to the robot. *)
-let send_curve planner sign p q r s v_end =
- (* Compute parameters. *)
- let d1 = sign *. distance p q and d2 = distance r s in
- let v = vector r s in
- let theta_end = atan2 v.vy v.vx in
-
- ignore (
- Lwt_log.info_f
- "sending bezier curve, x = %f, y = %f, d1 = %f, d2 = %f, theta_end = %f, v_end = %f"
- s.x s.y d1 d2 theta_end v_end
- );
-
- (* Send the curve. *)
- Krobot_message.send planner.bus (Unix.gettimeofday (), Motor_bezier(s.x, s.y, d1, d2, theta_end, v_end))
-
-(* Remove the first vertice of the trajecotry. *)
-let drop_vertice planner =
- match planner.vertices with
- | [] ->
- ()
- | _ :: vertices ->
- set_vertices planner ~curves:(List.tl planner.curves) vertices
-
-(* Follow the given path, updating [current_curve] each time we follow
- a new curve. *)
-let follow_path planner current_curve path =
- let rec loop = function
- | [] ->
- lwt () = wait_done planner in
- set_vertices planner [];
- return ()
-
- | [(sign, p, q, r, s)] ->
- lwt () = wait_middle planner in
- lwt () = send_curve planner sign p q r s 0.01 in
- lwt () = wait_start planner in
- current_curve := Bezier.of_vertices p q r s;
- drop_vertice planner;
- lwt () = wait_done planner in
- set_vertices planner [];
- return ()
-
- | (sign, p, q, r, s) :: rest ->
- lwt () = wait_middle planner in
- lwt () = send_curve planner sign p q r s 0.5 in
- lwt () = wait_start planner in
- current_curve := Bezier.of_vertices p q r s;
- drop_vertice planner;
- loop rest
- in
-
- (* Add the origin of the trajectory to keep displaying it. *)
- planner.vertices <- planner.position :: path;
- match planner.curves with
- | [] ->
- set_vertices planner [];
- return ()
-
- | [(sign, p, q, r, s)] ->
- current_curve := Bezier.of_vertices p q r s;
- lwt () = send_curve planner sign p q r s 0.01 in
- lwt () = wait_done planner in
- set_vertices planner [];
- return ()
-
- | (sign, p, q, r, s) :: rest ->
- current_curve := Bezier.of_vertices p q r s;
- lwt () = send_curve planner sign p q r s 0.5 in
- loop rest
-
-let go planner rotation_speed rotation_acceleration moving_speed moving_acceleration =
- if planner.moving then
- return ()
- else begin
- set_moving planner true;
- planner.mover <- (
- try_lwt
- follow_path planner (ref (Bezier.of_vertices origin origin origin origin)) planner.vertices
-
- with exn ->
- Lwt_log.error_f ~section ~exn "failed to move"
-
- finally
- set_moving planner false;
- return ()
- );
- return ()
- end
-
-let abort planner =
- Krobot_message.send planner.bus (Unix.gettimeofday (), Motor_stop(1.0, 0.0))
-
-let seen_beacon = ref None
-
-let check planner curve =
- let rec loop i =
- if i = 256 then
- true
- else
- let v = Bezier.vertice curve (float i /. 255.) in
- if (List.for_all (fun obj -> distance v obj >= object_safety_distance) planner.objects
- && (match planner.beacon , !seen_beacon with
- | Some v',_ ->
- seen_beacon := Some (Unix.gettimeofday ());
- false
- (* distance v v' >= beacon_safety_distance *)
- | None, Some x when (Unix.gettimeofday ()) < 4. ->
- false
- | None,_ -> true)) then
- loop (i + 1)
- else
- false
- in
- loop planner.curve_status
-
-(* Go to the given destination. *)
-let goto planner dst =
- if planner.moving then
- return ()
- else begin
- set_moving planner true;
- planner.mover <- (
- try_lwt
- let rec loop () =
- if (match planner.beacon , !seen_beacon with
- | Some v',_ ->
- seen_beacon := Some (Unix.gettimeofday ());
- false
- (* distance v v' >= beacon_safety_distance *)
- | None, Some x when (Unix.gettimeofday ()) < 4. ->
- false
- | None,_ -> true)
- then
- (match find_path planner planner.position dst with
- | None ->
- ignore (Lwt_log.info ~section "cannot find a path to the destination");
- return ()
- | Some path ->
- set_vertices planner (planner.position :: path);
- let dummy = Bezier.of_vertices origin origin origin origin in
- let current_curve = ref dummy in
- match_lwt
- pick [
- (lwt () = follow_path planner current_curve path in return true);
- (lwt () =
- while_lwt !current_curve = dummy do
- Lwt_unix.sleep 0.01
- done
- in
- lwt () =
- while_lwt check planner !current_curve do
- Lwt_unix.sleep 0.01
- done
- in
- return false);
- ]
- with
- | true ->
- return ()
- | false ->
- ignore (Lwt_log.info ~section "aborting current trajectory");
- lwt () = abort planner in
- lwt () = Lwt_unix.sleep 1.0 in
- loop ())
- else
- lwt () = Lwt_unix.sleep 1.0 in
- loop ()
- in
- loop ()
- with exn ->
- Lwt_log.error_f ~section ~exn "failed to move"
-
- finally
- set_moving planner false;
- return ()
- );
- return ()
- end
-
(* +-----------------------------------------------------------------+
| Message handling |
+-----------------------------------------------------------------+ *)
@@ -375,12 +117,8 @@ let handle_message planner (timestamp, message) =
planner.position <- { x; y };
planner.orientation <- math_mod_float theta (2. *. pi)
- | Odometry_ghost(x, y, theta, u, following) ->
- planner.curve_status <- u;
- planner.motors_moving <- following
-
| Beacon_position(angle, distance, period) ->
- if distance <> 0. then
+ if distance <> 0. then
let angle = math_mod_float (planner.orientation +. Krobot_config.rotary_beacon_index_pos +. angle) (2. *. pi) in
planner.beacon <- Some{
x = planner.position.x +. distance *. cos angle;
@@ -397,49 +135,29 @@ let handle_message planner (timestamp, message) =
exit 0
| Send ->
- ignore (
- let ts = Unix.gettimeofday () in
- let vertices = if planner.moving then planner.vertices else planner.position :: planner.vertices in
- join [
- Krobot_bus.send planner.bus (ts, Trajectory_vertices(vertices, List.map (fun (sign, p, q, r, s) -> (p, q, r, s)) planner.curves));
- Krobot_bus.send planner.bus (ts, Trajectory_moving planner.moving);
- ]
- )
+ ignore (Krobot_bus.send planner.bus (Unix.gettimeofday (), Trajectory_path planner.curves))
| Trajectory_set_vertices l ->
- if not planner.moving then
- set_vertices planner l
+ set_vertices planner l
| Trajectory_add_vertice vertice ->
- if not planner.moving then
- set_vertices planner (planner.vertices @ [vertice])
+ set_vertices planner (planner.vertices @ [vertice])
| Trajectory_simplify tolerance ->
- if not planner.moving then
- simplify planner tolerance
-
- | Trajectory_go(rotation_speed, rotation_acceleration, moving_speed, moving_acceleration) ->
- if not planner.moving then
- ignore (go planner rotation_speed rotation_acceleration moving_speed moving_acceleration)
-
- | Trajectory_goto v ->
- if not planner.moving then
- ignore (goto planner v)
-
- | Trajectory_stop ->
- cancel planner.mover;
- set_moving planner false;
- set_vertices planner [];
- ignore (Krobot_message.send planner.bus (Unix.gettimeofday (), Motor_stop(1.0, 0.0)))
-
- | Trajectory_find_path ->
- if not planner.moving then begin
- match planner.vertices with
- | v :: _ ->
- set_vertices planner (match find_path planner planner.position v with Some p -> p | None -> [])
- | _ ->
- ()
- end
+ simplify planner tolerance
+
+ | Trajectory_go ->
+ let path = planner.vertices in
+ ignore (Krobot_bus.send planner.bus (Unix.gettimeofday (), Strategy_set [Krobot_action.Follow_path path]));
+ set_vertices planner []
+
+ | Trajectory_find_path -> begin
+ match planner.vertices with
+ | v :: _ ->
+ set_vertices planner (match find_path planner planner.position v with Some p -> p | None -> [])
+ | _ ->
+ ()
+ end
| Objects l ->
planner.objects <- l
@@ -485,15 +203,11 @@ lwt () =
bus;
vertices = [];
curves = [];
- moving = false;
- motors_moving = false;
- curve_status = 0;
- mover = return ();
+ following_path = false;
position = { x = 0.; y = 0. };
orientation = 0.;
objects = [];
beacon = None;
- event = E.never;
} in
(* Handle krobot message. *)
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index de695e0..743f320 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -25,15 +25,9 @@ type state = {
theta : float;
}
-type beacon = {
- xbeacon : float;
- ybeacon : float;
- valid : bool;
-}
-
type viewer = {
bus : Krobot_bus.t;
- (* The D-Bus message bus used by this viwer. *)
+ (* The bus used by this viwer. *)
ui : Krobot_viewer_ui.window;
(* The UI of the viewer. *)
@@ -47,14 +41,14 @@ type viewer = {
mutable ghost : state;
(* The state of the ghost. *)
- mutable beacon : beacon;
- (* The state of the beacon. *)
+ mutable beacon : vertice option;
+ (* The position of the beacon, if any. *)
- mutable vertices : vertice list;
- (* The current trajectory. *)
+ mutable planner_path : Bezier.curve list;
+ (* The path of the planner. *)
- mutable curves : (vertice * vertice * vertice * vertice) list;
- (* The current bezier curves of the trajectory. *)
+ mutable vm_path : Bezier.curve list option;
+ (* The path of the VM. *)
mutable motor_status : bool * bool * bool *bool;
(* Status of the four motor controller. *)
@@ -275,30 +269,43 @@ let draw viewer =
(viewer.state, 1.0)];
(* Draw the beacon *)
- if viewer.beacon.valid then begin
- Cairo.arc ctx viewer.beacon.xbeacon viewer.beacon.ybeacon 0.04 0. (2. *. pi);
- set_color ctx Purple;
- Cairo.fill ctx;
- Cairo.arc ctx viewer.beacon.xbeacon viewer.beacon.ybeacon 0.04 0. (2. *. pi);
- set_color ctx Black;
- Cairo.stroke ctx
+ begin
+ match viewer.beacon with
+ | Some v ->
+ Cairo.arc ctx v.x v.y 0.04 0. (2. *. pi);
+ set_color ctx Purple;
+ Cairo.fill ctx;
+ Cairo.arc ctx v.x v.y 0.04 0. (2. *. pi);
+ set_color ctx Black;
+ Cairo.stroke ctx
+ | None ->
+ ()
end;
+ (* Draw the path of the VM if any or the path of the planner if the
+ VM is not following a trajectory. *)
+ let path =
+ match viewer.vm_path with
+ | Some path -> path
+ | None -> viewer.planner_path
+ in
+
(* Draw points. *)
Cairo.set_source_rgb ctx 255. 255. 0.;
- (match viewer.vertices with
+ (match path with
| [] ->
()
- | o :: l ->
- Cairo.move_to ctx o.x o.y;
- List.iter (fun { x; y } -> Cairo.line_to ctx x y) l;
+ | curve :: curves ->
+ let src = Bezier.src curve and dst = Bezier.dst curve in
+ Cairo.move_to ctx src.x src.y;
+ Cairo.line_to ctx dst.x dst.y;
+ List.iter (fun curve -> let v = Bezier.dst curve in Cairo.line_to ctx v.x v.y) curves;
Cairo.stroke ctx);
(* Draw bezier curves. *)
Cairo.set_source_rgb ctx 255. 0. 255.;
List.iter
- (fun (p, q, r, s) ->
- let curve = Bezier.of_vertices p q r s in
+ (fun curve ->
let { x; y } = Bezier.vertice curve 0. in
Cairo.move_to ctx x y;
for i = 1 to 100 do
@@ -306,7 +313,7 @@ let draw viewer =
Cairo.line_to ctx x y
done;
Cairo.stroke ctx)
- viewer.curves;
+ path;
let ctx = Cairo_lablgtk.create viewer.ui#scene#misc#window in
Cairo.set_source_surface ctx surface 0. 0.;
@@ -389,14 +396,19 @@ let handle_message viewer (timestamp, message) =
end
| Beacon_position(angle, distance, period) ->
- let newangle = math_mod_float (viewer.state.theta +. Krobot_config.rotary_beacon_index_pos +. angle) (2. *. pi) in
- let x = viewer.state.pos.x +. distance *. cos (newangle) in
- let y = viewer.state.pos.y +. distance *. sin (newangle) in
- let valid = distance <> 0. in
- let beacon = { xbeacon = x; ybeacon = y; valid; } in
+ let beacon =
+ if distance <> 0. then begin
+ let angle = math_mod_float (viewer.state.theta +. rotary_beacon_index_pos +. angle) (2. *. pi) in
+ Some {
+ x = viewer.state.pos.x +. distance *. cos angle;
+ y = viewer.state.pos.y +. distance *. sin angle;
+ }
+ end else
+ None
+ in
if beacon <> viewer.beacon then begin
viewer.beacon <- beacon;
- viewer.ui#beacon_status#set_text (if valid then "valid" else "-");
+ viewer.ui#beacon_status#set_text (if beacon = None then "-" else "valid");
viewer.ui#beacon_distance#set_text (string_of_float distance);
viewer.ui#beacon_angle#set_text (string_of_float angle);
viewer.ui#beacon_period#set_text (string_of_float period);
@@ -416,13 +428,19 @@ let handle_message viewer (timestamp, message) =
| Kill "viewer" ->
exit 0
- | Trajectory_vertices(vertices, curves) ->
- viewer.vertices <- vertices;
- viewer.curves <- curves;
+ | Trajectory_path curves ->
+ viewer.planner_path <- curves;
queue_draw viewer
- | Trajectory_moving moving ->
- viewer.ui#button_go#misc#set_sensitive (not moving)
+ | Strategy_path None ->
+ viewer.vm_path <- None;
+ viewer.ui#button_go#misc#set_sensitive true;
+ queue_draw viewer
+
+ | Strategy_path(Some curves) ->
+ viewer.vm_path <- Some curves;
+ viewer.ui#button_go#misc#set_sensitive false;
+ queue_draw viewer
| Log line ->
viewer.ui#logs#buffer#insert (line ^ "\n");
@@ -487,9 +505,9 @@ lwt () =
ui;
state = init;
ghost = init;
- beacon = { xbeacon = 1.; ybeacon = 1.; valid = false };
- vertices = [];
- curves = [];
+ beacon = None;
+ planner_path = [];
+ vm_path = None;
statusbar_context = ui#statusbar#new_context "";
motor_status = (false, false, false, false);
objects = [];
@@ -541,23 +559,16 @@ lwt () =
(ui#button_go#event#connect#button_release
(fun ev ->
if GdkEvent.Button.button ev = 1 then
- ignore (
- Krobot_bus.send bus
- (Unix.gettimeofday (),
- Trajectory_go(ui#rotation_speed#adjustment#value,
- ui#rotation_acceleration#adjustment#value,
- ui#moving_speed#adjustment#value,
- ui#moving_acceleration#adjustment#value))
- );
+ ignore (Krobot_bus.send bus (Unix.gettimeofday (), Trajectory_go));
false));
ignore
(ui#button_goto#event#connect#button_release
(fun ev ->
if GdkEvent.Button.button ev = 1 then begin
- match viewer.vertices with
- | _ :: v :: _ ->
- ignore (Krobot_bus.send bus (Unix.gettimeofday (), Trajectory_goto v))
+ match viewer.planner_path with
+ | curve :: _ ->
+ ignore (Krobot_bus.send bus (Unix.gettimeofday (), Strategy_set [Krobot_action.Goto(Bezier.dst curve)]))
| _ ->
()
end;
@@ -567,29 +578,21 @@ lwt () =
(ui#button_start_red#event#connect#button_release
(fun ev ->
if GdkEvent.Button.button ev = 1 then
- ignore (
- Krobot_message.send bus
- (Unix.gettimeofday (),
- Set_odometry(0.215 -. Krobot_config.robot_size /. 2. +. Krobot_config.wheels_position, 1.885, 0.))
- );
+ ignore (Krobot_bus.send bus (Unix.gettimeofday (), Strategy_set [Krobot_action.Reset_odometry `Red]));
false));
ignore
(ui#button_start_blue#event#connect#button_release
(fun ev ->
if GdkEvent.Button.button ev = 1 then
- ignore_result (
- Krobot_message.send bus
- (Unix.gettimeofday (),
- Set_odometry(2.77, 1.915, pi))
- );
+ ignore (Krobot_bus.send bus (Unix.gettimeofday (), Strategy_set [Krobot_action.Reset_odometry `Blue]));
false));
ignore
(ui#button_stop#event#connect#button_release
(fun ev ->
if GdkEvent.Button.button ev = 1 then
- ignore (Krobot_bus.send bus (Unix.gettimeofday (), Trajectory_stop));
+ ignore (Krobot_bus.send bus (Unix.gettimeofday (), Strategy_stop));
false));
ignore
diff --git a/info/control2011/src/tools/krobot_vm.ml b/info/control2011/src/tools/krobot_vm.ml
new file mode 100644
index 0000000..9d67145
--- /dev/null
+++ b/info/control2011/src/tools/krobot_vm.ml
@@ -0,0 +1,402 @@
+(*
+ * krobot_vm.ml
+ * ------------
+ * Copyright : (c) 2011, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(* The krobot virtual machine *)
+
+open Lwt
+open Lwt_react
+open Krobot_config
+open Krobot_geom
+open Krobot_bus
+open Krobot_message
+open Krobot_action
+
+(* +-----------------------------------------------------------------+
+ | Types |
+ +-----------------------------------------------------------------+ *)
+
+(* Type of robots. *)
+type robot = {
+ bus : Krobot_bus.t;
+ (* The bus used to communicate with the robot. *)
+
+ mutable strategy : Krobot_action.t list;
+ (* The current strategy. *)
+
+ mutable change_strategy : Krobot_action.t list option;
+ (* This is used the change the current strategy. *)
+
+ mutable append_strategy : Krobot_action.t list option;
+ (* This is used to append actions to the current strategy. *)
+
+ mutable path : Bezier.curve list option;
+ (* The path currently followed by the robot. *)
+
+ mutable position : vertice;
+ (* The position of the robot on the table. *)
+
+ mutable orientation : float;
+ (* The orientation of the robot. *)
+
+ mutable objects : vertice list;
+ (* Position of objects on the table. *)
+
+ mutable moving : bool;
+ (* Is the robot moving ? *)
+
+ mutable curve : Bezier.curve option;
+ (* The bezier curve currently being followed by the robot. *)
+
+ mutable curve_parameter : int;
+ (* The parameter of the bezier curve currently followed by the
+ robot. *)
+
+ mutable jack : bool;
+ (* Status of the jack. *)
+
+ mutable beacon : vertice option;
+ (* The detected position of the beacon, if any. *)
+
+ mutable date_seen_beacon : float;
+ (* Date at which the beacon has been seen. *)
+
+ mutable team : [ `Red | `Blue ];
+ (* The state of the team selector. *)
+}
+
+(* +-----------------------------------------------------------------+
+ | Message handling |
+ +-----------------------------------------------------------------+ *)
+
+let handle_message robot (timestamp, message) =
+ match message with
+ | CAN(_, frame) -> begin
+ match decode frame with
+ | Odometry(x, y, theta) ->
+ robot.position <- { x; y };
+ robot.orientation <- math_mod_float theta (2. *. pi)
+
+ | Odometry_ghost(x, y, theta, u, following) ->
+ robot.curve_parameter <- u;
+ robot.moving <- following
+
+ | Beacon_position(angle, distance, period) ->
+ if distance <> 0. then begin
+ robot.date_seen_beacon <- Unix.gettimeofday ();
+ let angle = math_mod_float (robot.orientation +. rotary_beacon_index_pos +. angle) (2. *. pi) in
+ robot.beacon <- Some{
+ x = robot.position.x +. distance *. cos angle;
+ y = robot.position.y +. distance *. sin angle;
+ }
+ end else
+ robot.beacon <- None
+
+ | Switch1_status(b, _, _, _, _, _, _, _) ->
+ robot.jack <- not b
+
+ | _ ->
+ ()
+ end
+
+ | Kill "vm" ->
+ exit 0
+
+ | Send ->
+ ignore (
+ let timestamp = Unix.gettimeofday () in
+ Krobot_bus.send robot.bus (timestamp, Strategy_path robot.path)
+ )
+
+ | Objects l ->
+ robot.objects <- l
+
+ | Strategy_append l -> begin
+ match robot.append_strategy with
+ | Some l' ->
+ robot.append_strategy <- Some(l' @ l)
+ | None ->
+ robot.append_strategy <- Some l
+ end
+
+ | Strategy_stop ->
+ robot.change_strategy <- Some [Stop]
+
+ | Strategy_set l ->
+ robot.append_strategy <- None;
+ robot.change_strategy <- Some(Stop :: l)
+
+ | _ ->
+ ()
+
+(* +-----------------------------------------------------------------+
+ | Helpers |
+ +-----------------------------------------------------------------+ *)
+
+let set_path robot path =
+ robot.path <- path;
+ ignore (Krobot_bus.send robot.bus (Unix.gettimeofday (), Strategy_path path))
+
+let reset robot =
+ robot.curve <- None;
+ set_path robot None
+
+(* +-----------------------------------------------------------------+
+ | Execution |
+ +-----------------------------------------------------------------+ *)
+
+(* Remove the leftest node from a tree of actions. *)
+let rec remove_leftest_node = function
+ | Node(Node _ :: _ as l) :: rest ->
+ Node(remove_leftest_node l) :: rest
+ | Node _ :: rest ->
+ rest
+ | l ->
+ l
+
+(* The effect triggered by the execution of the first action of a tree
+ of action. *)
+type effect =
+ | Wait
+ (* Wait a bit. *)
+ | Send of Krobot_message.t
+ (* Send a message. *)
+
+(* [exec robot actions] searches for the first action to execute in a
+ tree of actions and returns a new tree of actions and an effect. *)
+let rec exec robot actions =
+ match actions with
+ | [] ->
+ ([], Wait)
+ | Node [] :: rest ->
+ exec robot rest
+ | Node actions :: rest ->
+ let actions, effect = exec robot actions in
+ (Node actions :: rest, effect)
+ | Wait_for_jack state :: rest ->
+ if robot.jack = state then
+ exec robot rest
+ else
+ (actions, Wait)
+ | Wait_for_moving state :: rest ->
+ if robot.moving = state then
+ exec robot rest
+ else
+ (actions, Wait)
+ | Wait_for_odometry(test, value) :: rest ->
+ if (match test with
+ | `Eq -> robot.curve_parameter = value
+ | `Gt -> robot.curve_parameter > value
+ | `Ge -> robot.curve_parameter >= value
+ | `Lt -> robot.curve_parameter < value
+ | `Le -> robot.curve_parameter <= value) then
+ exec robot rest
+ else
+ (actions, Wait)
+ | Set_curve None :: rest ->
+ reset robot;
+ exec robot rest
+ | Set_curve(Some curve) :: rest ->
+ robot.curve <- Some curve;
+ exec robot rest
+ | Goto v :: rest -> begin
+ (* Try to find a path to the destination. *)
+ match Krobot_path.find ~src:robot.position ~dst:v ~objects:robot.objects ~beacon:robot.beacon with
+ | Some vertices ->
+ exec robot (Follow_path vertices :: rest)
+ | None ->
+ (* If not found, skip the command. *)
+ exec robot rest
+ end
+ | Follow_path vertices :: rest -> begin
+ (* Compute bezier curves. *)
+ let vector = { vx = cos robot.orientation; vy = sin robot.orientation } in
+ let curves = List.rev (Bezier.fold_vertices (fun sign p q r s acc -> (sign, p, q, r, s) :: acc) vector (robot.position :: vertices) []) in
+ (* Set the path. *)
+ set_path robot (Some (List.map (fun (sign, p, q, r, s) -> Bezier.of_vertices p q r s) curves));
+ (* Compute orders. *)
+ let rec loop = function
+ | [] ->
+ [Wait_for_moving false]
+ | [(sign, p, q, r, s)] ->
+ [
+ (* Wait for the odometry to reach the middle of the
+ current trajectory. *)
+ Wait_for_odometry(`Ge, 128);
+ (* Send the next bezier curve. *)
+ Bezier(sign, p, q, r, s, 0.01);
+ (* Wait for the odometry to start the new curve. *)
+ Wait_for_odometry(`Lt, 128);
+ (* Set the new curve. *)
+ Set_curve(Some(Bezier.of_vertices p q r s));
+ (* Wait for the end of the new curve. *)
+ Wait_for_moving false;
+ (* Remove the current curve. *)
+ Set_curve None;
+ ]
+ | (sign, p, q, r, s) :: rest ->
+ Wait_for_odometry(`Ge, 128)
+ :: Bezier(sign, p, q, r, s, 0.5)
+ :: Wait_for_odometry(`Lt, 128)
+ :: Set_curve(Some(Bezier.of_vertices p q r s))
+ :: loop rest
+ in
+ match curves with
+ | [] ->
+ exec robot rest
+ | [(sign, p, q, r, s)] ->
+ exec robot (Node [
+ Set_curve(Some(Bezier.of_vertices p q r s));
+ Bezier(sign, p, q, r, s, 0.01);
+ Wait_for_moving fals...
[truncated message content] |
