Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. ce758846dbc2dfaf0e1d50ae203a0e955dc33aaf
|
From: Xavier L. <Ba...@us...> - 2013-05-22 19:40:00
|
This is an automated email from the git hooks/post-receive script. It was
generated because a ref change was pushed to the repository containing
the project "krobot".
The branch, master has been updated
via ce758846dbc2dfaf0e1d50ae203a0e955dc33aaf (commit)
via 295eeed1694fcd66c866a47adf80d185a5e006ed (commit)
via 1469f324204351a9fed364e7cd255536c1192bc2 (commit)
via 151f042d69b02a6f65a4ece42a5fb3bdba392546 (commit)
via 63aabe085353534abc4dba4d0ef0ffb5d04d1801 (commit)
via 3e283fa22033418ebf6c4d103c7e776301193f62 (commit)
via 4d0245d3a58ad206462f821ec5c3aeacc4b83ecb (commit)
via 31786065d863cf2d4aa07eb5cd7de558db72b4f0 (commit)
via c2c799d2a17fb401b88bc2849cc1f1841759486e (commit)
via c08a114cea56f74ab6b2f05c801ccc51298db979 (commit)
via 3bf0f7ea516321571c2aed4e4e7183643452575b (commit)
via 7dbe121c133f23ab4a6138e3dc96b930bac06d57 (commit)
via eb7dd7a042219ec1367ab1e4c70638abfb967aba (commit)
via 533f1301506c201c4eee01e880188aef30e46795 (commit)
via 290770ae636671de184457766135eac3185507bd (commit)
via d9d6be1512ead188854051362a1c02d3e98b430f (commit)
via 2b85f9f2fb495bdc813093536d4dfb118b317a93 (commit)
via 41de715dce9c59ddd5e01ad2e46cf9ca78cc2199 (commit)
via 4a99bb6f314d8fd380c838aca948c14fa74b3644 (commit)
via 928be7aa366f0f3dc8bddfa795fe8d770dd66d93 (commit)
via e9af8ac8951ba2f07d3c42b2cca4760635c49ca9 (commit)
via 8b3c3a760cfe86119055ab8a4efe0ef32708ba39 (commit)
via 6b10692ae39b4877fcbb3aac0ea13a4f705983e2 (commit)
via 21f4c7294724d3dd47cda68d1449f0c8ac0cc1f7 (commit)
via 7b0b7c21e1651c41b78feefad032dc9a7979ff3a (commit)
via 04b81baf4f9b35178c3ab8be28a188039df38447 (commit)
via 0c2fdf06b1b22b3fad45aed19e5340503d5e7c6b (commit)
via b6e98afc67a9ef75084b799937b41098aefbc941 (commit)
via e585caecbceac260f2445e29cc8a0b749e9e6f29 (commit)
via 5e1b821afc9a263e6d6ca62f147be3bc5f0f9a5c (commit)
via 5969c55d6a482da3c5ca64b297534ddc169d2f6b (commit)
via 4b9de7fd9542e1146be0c2acdf8f25531bb63ffe (commit)
via f6536242e8ccfa1a1be748f4d39f676ffdef4043 (commit)
via 6e85c22cba2f58f6fc3ca3e79531eba7c65c58a6 (commit)
via c03ebbe9394f6e3d6a2c6f259af7784201946857 (commit)
via a664d667f121bc3b28987193ace0c70e7e71b386 (commit)
via a4574aae138c61257232e728da0f70998ca57afa (commit)
via 06174d40f9dde8634a703d6d9e27826c9e91269a (commit)
via 53c2b745afb88b6702b451ca23e40610622fa961 (commit)
via c59ad622b32fe84e054198a44cccbeaabaee29b8 (commit)
via bb9d6760a512f97c12324052b9328668ccffeb07 (commit)
via 98ff61ee2de1ebd6c4a90a4d4d25428b054aa72e (commit)
via 01f7336a3ed8c3eab82b646dfc7d8dc4d723545f (commit)
via d3cf53369367cb157e87a197f8e417ac8f7c6569 (commit)
from bc96bdd7d9d8342fe018fe76ff3bf9b5a6fdc3c7 (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 ce758846dbc2dfaf0e1d50ae203a0e955dc33aaf
Author: Pierre Chambart <pie...@oc...>
Date: Sat May 11 10:05:24 2013 +0200
filter urg
commit 295eeed1694fcd66c866a47adf80d185a5e006ed
Author: Pierre Chambart <pie...@oc...>
Date: Sat May 11 09:34:45 2013 +0200
plop
commit 1469f324204351a9fed364e7cd255536c1192bc2
Author: Pierre Chambart <pie...@oc...>
Date: Sat May 11 09:20:33 2013 +0200
test
commit 151f042d69b02a6f65a4ece42a5fb3bdba392546
Author: Pierre Chambart <pie...@oc...>
Date: Fri May 10 17:17:06 2013 +0200
Move back action, more things in homologation
commit 63aabe085353534abc4dba4d0ef0ffb5d04d1801
Author: Pierre Chambart <pie...@oc...>
Date: Fri May 10 15:51:19 2013 +0200
more on xbee
commit 3e283fa22033418ebf6c4d103c7e776301193f62
Author: Pierre Chambart <pie...@oc...>
Date: Fri May 10 15:51:08 2013 +0200
generate trajectories that does not go backward
commit 4d0245d3a58ad206462f821ec5c3aeacc4b83ecb
Author: Pierre Chambart <pie...@oc...>
Date: Fri May 10 07:42:38 2013 +0200
Homologation: infinite loop
commit 31786065d863cf2d4aa07eb5cd7de558db72b4f0
Author: Pierre Chambart <pie...@oc...>
Date: Fri May 10 07:16:10 2013 +0200
More flexible node, allow n retries/skip/loop
commit c2c799d2a17fb401b88bc2849cc1f1841759486e
Author: Pierre Chambart <pie...@oc...>
Date: Fri May 10 06:22:13 2013 +0200
Finish krobot-xbee
commit c08a114cea56f74ab6b2f05c801ccc51298db979
Author: Pierre Chambart <pie...@oc...>
Date: Fri May 10 02:31:21 2013 +0200
Stuff start
commit 3bf0f7ea516321571c2aed4e4e7183643452575b
Author: Pierre Chambart <pie...@oc...>
Date: Fri May 10 02:30:52 2013 +0200
Start xbee
commit 7dbe121c133f23ab4a6138e3dc96b930bac06d57
Author: Pierre Chambart <pie...@oc...>
Date: Fri May 10 01:23:07 2013 +0200
Little bit smarter
commit eb7dd7a042219ec1367ab1e4c70638abfb967aba
Author: Pierre Chambart <pie...@oc...>
Date: Fri May 10 00:52:17 2013 +0200
Clean some dead code (since 2011)
commit 533f1301506c201c4eee01e880188aef30e46795
Author: Pierre Chambart <pie...@oc...>
Date: Fri May 10 00:45:27 2013 +0200
Many fixes, better startegy
commit 290770ae636671de184457766135eac3185507bd
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 20:04:28 2013 +0200
Add cake to the table extraction
commit d9d6be1512ead188854051362a1c02d3e98b430f
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 20:04:06 2013 +0200
Urg replay
commit 2b85f9f2fb495bdc813093536d4dfb118b317a93
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 17:38:37 2013 +0200
Multiple fix
commit 41de715dce9c59ddd5e01ad2e46cf9ca78cc2199
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 13:38:26 2013 +0200
krobot_urg_extract: Filter when only one point
commit 4a99bb6f314d8fd380c838aca948c14fa74b3644
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 13:34:17 2013 +0200
Return to nice homologation
commit 928be7aa366f0f3dc8bddfa795fe8d770dd66d93
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 13:28:36 2013 +0200
Fix vm explosions
commit e9af8ac8951ba2f07d3c42b2cca4760635c49ca9
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 11:45:12 2013 +0200
homo
commit 8b3c3a760cfe86119055ab8a4efe0ef32708ba39
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 10:34:29 2013 +0200
Hack homologation
commit 6b10692ae39b4877fcbb3aac0ea13a4f705983e2
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 10:09:36 2013 +0200
Fix homologation: changing switch reset timer
commit 21f4c7294724d3dd47cda68d1449f0c8ac0cc1f7
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu May 9 13:26:40 2013 +0200
[krobot-vm] Blink all LEDs if emergency stop button if pressed
commit 7b0b7c21e1651c41b78feefad032dc9a7979ff3a
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu May 9 08:53:49 2013 +0200
[krobot-viewer] Change robot's color for better visibility
commit 04b81baf4f9b35178c3ab8be28a188039df38447
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 08:48:30 2013 +0200
Change team led: red on red
commit 0c2fdf06b1b22b3fad45aed19e5340503d5e7c6b
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 08:44:57 2013 +0200
Remove safety margin: we start too close from the wall to have one...
commit b6e98afc67a9ef75084b799937b41098aefbc941
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu May 9 08:27:34 2013 +0200
[krobot-vm] Fixed "typo"
commit e585caecbceac260f2445e29cc8a0b749e9e6f29
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 08:09:59 2013 +0200
Optimise Icp_utils.far_enougth_filter
commit 5e1b821afc9a263e6d6ca62f147be3bc5f0f9a5c
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu May 9 07:47:41 2013 +0200
[Krobot_vm] Don't start the pump at the beggining of the match
commit 5969c55d6a482da3c5ca64b297534ddc169d2f6b
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 00:56:02 2013 +0200
try something is now an action when goto fail
commit 4b9de7fd9542e1146be0c2acdf8f25531bb63ffe
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 00:55:39 2013 +0200
Change homologation
commit f6536242e8ccfa1a1be748f4d39f676ffdef4043
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 00:55:20 2013 +0200
Fix initial position
commit 6e85c22cba2f58f6fc3ca3e79531eba7c65c58a6
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 00:10:01 2013 +0200
Fix ax12 position in homologation
commit c03ebbe9394f6e3d6a2c6f259af7784201946857
Author: Pierre Chambart <pie...@oc...>
Date: Thu May 9 00:09:27 2013 +0200
Update config
commit a664d667f121bc3b28987193ace0c70e7e71b386
Author: Pierre Chambart <pie...@oc...>
Date: Wed May 8 23:30:57 2013 +0200
fix homologation
commit a4574aae138c61257232e728da0f70998ca57afa
Author: Pierre Chambart <pie...@oc...>
Date: Wed May 8 23:28:20 2013 +0200
Fix end of match
commit 06174d40f9dde8634a703d6d9e27826c9e91269a
Author: Pierre Chambart <pie...@oc...>
Date: Wed May 8 23:28:00 2013 +0200
Homologation with baloon
commit 53c2b745afb88b6702b451ca23e40610622fa961
Author: Pierre Chambart <pie...@oc...>
Date: Wed May 8 23:27:25 2013 +0200
filter urg data to avoid spurious points
commit c59ad622b32fe84e054198a44cccbeaabaee29b8
Author: Xavier Lagorce <Xav...@cr...>
Date: Wed May 8 23:24:53 2013 +0200
[Krobot_viewer] Draw 2013 Table (better late than never...)
commit bb9d6760a512f97c12324052b9328668ccffeb07
Author: Pierre Chambart <pie...@oc...>
Date: Wed May 8 21:56:46 2013 +0200
Trivial homologation program
commit 98ff61ee2de1ebd6c4a90a4d4d25428b054aa72e
Author: Pierre Chambart <pie...@oc...>
Date: Wed May 8 19:58:27 2013 +0200
See and avoid objects
commit 01f7336a3ed8c3eab82b646dfc7d8dc4d723545f
Author: Pierre Chambart <pie...@oc...>
Date: Wed May 8 18:17:14 2013 +0200
Fix build
commit d3cf53369367cb157e87a197f8e417ac8f7c6569
Author: Pierre Chambart <pie...@oc...>
Date: Wed May 8 18:10:48 2013 +0200
krobot_urg_extract: extract objects from urg captures
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/_oasis b/info/control2011/_oasis
index 85d5e9e..adcbdf0 100644
--- a/info/control2011/_oasis
+++ b/info/control2011/_oasis
@@ -34,6 +34,10 @@ Flag sdl
Description: enable sdl
Default: true
+Flag lacaml
+ Description: compile icp depending on lacaml
+ Default: true
+
# +-------------------------------------------------------------------+
# | Libraries |
# +-------------------------------------------------------------------+
@@ -77,6 +81,8 @@ Library "krobot-can"
Library "krobot-icp"
FindlibName: icp
FindlibParent: krobot
+ Build$: flag(lacaml)
+ Install$: flag(lacaml)
BuildDepends: lacaml
XMETADescription: 2d Point cloud registration
XMETARequires: lacaml
@@ -317,6 +323,22 @@ Executable "krobot-urg"
MainIs: krobot_urg.ml
BuildDepends: krobot, urg, lwt.syntax, lwt.preemptive, threads
+Executable "krobot-urg-extract"
+ Path: src/tools
+ Build$: flag(lacaml)
+ Install$: flag(lacaml)
+ CompiledObject: best
+ MainIs: krobot_urg_extract.ml
+ BuildDepends: krobot, krobot.icp, lwt.syntax
+
+Executable "krobot-urg-replay"
+ Path: src/tools
+ Build$: flag(lacaml)
+ Install$: flag(lacaml)
+ CompiledObject: best
+ MainIs: krobot_urg_replay.ml
+ BuildDepends: krobot, lwt.syntax, krobot.icp
+
Executable "krobot-beacon-reader"
Path: src/tools
Install: true
@@ -324,6 +346,13 @@ Executable "krobot-beacon-reader"
MainIs: krobot_beacon_reader.ml
BuildDepends: krobot, lwt.syntax, lwt.unix
+Executable "krobot-xbee"
+ Path: src/tools
+ Install: true
+ CompiledObject: best
+ MainIs: krobot_xbee.ml
+ BuildDepends: krobot, lwt.syntax, lwt.unix, str
+
# +-------------------------------------------------------------------+
# | Examples |
# +-------------------------------------------------------------------+
diff --git a/info/control2011/_tags b/info/control2011/_tags
index 4b773d9..7aa1784 100644
--- a/info/control2011/_tags
+++ b/info/control2011/_tags
@@ -1,6 +1,6 @@
<**/*.ml{,i}>: syntax_camlp4o
<src/interfaces/*.ml>: -syntax_camlp4o
-<src/icp/*.ml>: -syntax_camlp4o
+<src/icp/*.ml{,i}>: -syntax_camlp4o
# OASIS_START
# DO NOT EDIT (digest: 9a283fb86531fb8de178da3ad5f872b1)
diff --git a/info/control2011/src/icp/icp_utils.ml b/info/control2011/src/icp/icp_utils.ml
index a961e5e..b85b81b 100644
--- a/info/control2011/src/icp/icp_utils.ml
+++ b/info/control2011/src/icp/icp_utils.ml
@@ -21,6 +21,12 @@ let shuffle l =
let l1,l2 = split ([],[]) l in
merge [] l1 (List.rev l2)
+let circle (dx,dy) r a0 an n =
+ let angle_ramp = linear a0 an n in
+ let x = Array.map (fun theta -> r *. cos theta +. dx) angle_ramp in
+ let y = Array.map (fun theta -> r *. sin theta +. dy) angle_ramp in
+ x,y
+
let table ~width ~length n =
let x1,y1 = line (0.,0.) (length,0.) n in
let x2,y2 = line (length,0.) (length,width) n in
@@ -31,6 +37,24 @@ let table ~width ~length n =
let f a = Array.of_list (shuffle (shuffle (Array.to_list a))) in
{ dx = f x; dy = f y }
+let pi = 3.14159265358979323
+
+let real_table n =
+ let length = 3. in
+ let width = 2. in
+ let x1,y1 = line (0.,0.) (length,0.) n in
+ let x2,y2 = line (length,0.) (length,width) n in
+ let x3,y3 = line (0.,width) (0.,0.) n in
+ let x4,y4 = line (0.,width) (1.,width) (n/2) in
+ let x5,y5 = line (2.,width) (length,width) (n/2) in
+ let x6,y6 = circle (1.5,2.0) 0.5 (-.pi) 0. n in
+ let x7,y7 = line (0.,0.1) (0.4,0.1) (n/4) in
+ let x8,y8 = line (3.,0.1) (2.6,0.1) (n/4) in
+ let x = Array.concat [x1;x2;x3;x4;x5;x6;x7;x8] in
+ let y = Array.concat [y1;y2;y3;y4;y5;y6;y7;y8] in
+ let f a = Array.of_list (shuffle (shuffle (Array.to_list a))) in
+ { dx = f x; dy = f y }
+
(**** dump loading ****)
@@ -76,18 +100,39 @@ let load_file ?(min_dist=0.15) ?(max_dist=6.) f =
ts, { dx = Array.of_list x; dy = Array.of_list y }) l in
Array.of_list l'
+let load_raw_file f =
+ let l = load_file' f in
+ let l' =
+ List.map (fun (ts,v) ->
+ let x,y = List.split v in
+ ts, { dx = Array.of_list x; dy = Array.of_list y }) l in
+ Array.of_list l'
(**** filtering ****)
-let far_enougth_filter kd a min_dist data =
- let dist = distance_transform (fun i -> i) kd a data in
- let dist = Array.mapi (fun i d -> i,d) dist in
- let distl = Array.to_list dist in
- let min_dist2 = min_dist *. min_dist in
- let far_enougth (i,d) = d >= min_dist2 in
- let ai = Array.of_list (List.filter far_enougth distl) in
- { dx = Array.map (fun (i,_) -> data.dx.(i)) ai;
- dy = Array.map (fun (i,_) -> data.dy.(i)) ai }
+(* TODO: can be done more efficiently using Kd_tree.closer *)
+(* let far_enougth_filter kd a min_dist data = *)
+(* let dist = distance_transform (fun i -> i) kd a data in *)
+(* let dist = Array.mapi (fun i d -> i,d) dist in *)
+(* let distl = Array.to_list dist in *)
+(* let min_dist2 = min_dist *. min_dist in *)
+(* let far_enougth (i,d) = d >= min_dist2 in *)
+(* let ai = Array.of_list (List.filter far_enougth distl) in *)
+(* { dx = Array.map (fun (i,_) -> data.dx.(i)) ai; *)
+(* dy = Array.map (fun (i,_) -> data.dy.(i)) ai } *)
+
+let far_enougth_filter kd min_dist data =
+ let dx = data.dx in
+ let dy = data.dy in
+ let l = ref [] in
+ for i = 0 to Array.length dx - 1 do
+ if not (Kd_tree.closer min_dist { Kd_tree.x = dx.(i); y = dy.(i) } kd)
+ then l := i :: !l
+ done;
+ let l = Array.of_list !l in
+ let x = Array.map (fun i -> dx.(i)) l in
+ let y = Array.map (fun i -> dy.(i)) l in
+ { dx = x; dy = y }
let invert_transform a =
let co = cos (-. a.ath) in
diff --git a/info/control2011/src/icp/icp_utils.mli b/info/control2011/src/icp/icp_utils.mli
index 1e23bba..55c30e3 100644
--- a/info/control2011/src/icp/icp_utils.mli
+++ b/info/control2011/src/icp/icp_utils.mli
@@ -2,15 +2,19 @@ open Icp_minimisation
val table : width:float -> length:float -> int -> data
+val real_table : int -> data
+
val load_file : ?min_dist:float -> ?max_dist:float -> string ->
(float * data) array
(** load a dump file in the format dumpped by [krobot_urg -listen] *)
+val load_raw_file : string -> (float * data) array
+(** load a dump file in the format dumpped by [krobot_urg -listen] *)
(* filtering *)
-val far_enougth_filter : 'a Kd_tree.t -> a -> float -> data -> data
-(** [far_enougth_filter kd a min_dist data] filter out values of
- [data] that are closer than [min_dist] to a vertex of kd *)
+val far_enougth_filter : 'a Kd_tree.t -> float -> data -> data
+(** [far_enougth_filter kd min_dist data] filter out values of [data]
+ that are closer than [min_dist] to a vertex of kd *)
val invert_transform : a -> a
diff --git a/info/control2011/src/icp/kd_tree.ml b/info/control2011/src/icp/kd_tree.ml
index 45601d4..96631a7 100644
--- a/info/control2011/src/icp/kd_tree.ml
+++ b/info/control2011/src/icp/kd_tree.ml
@@ -98,6 +98,58 @@ let nearest_neighbor v = function
let curr = (med_val, median_elt, sq_dist median_elt v) in
nearest_neighbor' v curr t
+let rec closer' dist_sq v t = match t with
+ | Empty -> false
+ | Leaf (va,p) -> sq_dist v p <= dist_sq
+ | Split (dir, med_val, median_elt, left, right) ->
+ let split_distance = match dir with
+ | Hori -> sq (v.x -. median_elt.x)
+ | Vert -> sq (v.y -. median_elt.y) in
+ if split_distance <= dist_sq
+ then begin
+ if sq_dist v median_elt <= dist_sq
+ then true
+ else closer' dist_sq v left || closer' dist_sq v right
+ end
+ else
+ let cmp = match dir with
+ | Hori -> compare v.x median_elt.x
+ | Vert -> compare v.y median_elt.y in
+ if cmp > 0
+ then closer' dist_sq v right
+ else closer' dist_sq v left
+
+let closer dist v t = closer' (sq dist) v t
+
+let add_closer dist_sq v p va acc =
+ if sq_dist v p <= dist_sq
+ then va::acc
+ else acc
+
+let rec closer_points' dist_sq v t acc = match t with
+ | Empty -> acc
+ | Leaf (va,p) ->
+ add_closer dist_sq v p va acc
+ | Split (dir, med_val, median_elt, left, right) ->
+ let split_distance = match dir with
+ | Hori -> sq (v.x -. median_elt.x)
+ | Vert -> sq (v.y -. median_elt.y) in
+ if split_distance <= dist_sq
+ then begin
+ let acc = add_closer dist_sq v median_elt med_val acc in
+ let acc = closer_points' dist_sq v left acc in
+ closer_points' dist_sq v right acc
+ end
+ else
+ let cmp = match dir with
+ | Hori -> compare v.x median_elt.x
+ | Vert -> compare v.y median_elt.y in
+ if cmp > 0
+ then closer_points' dist_sq v right acc
+ else closer_points' dist_sq v left acc
+
+let closer_points dist v t = closer_points' (sq dist) v t []
+
let rec depth = function
| Split (_, _, _, t1,t2 ) ->
1 + max (depth t1) (depth t2)
diff --git a/info/control2011/src/icp/kd_tree.mli b/info/control2011/src/icp/kd_tree.mli
index 947b1ad..9fd0f7c 100644
--- a/info/control2011/src/icp/kd_tree.mli
+++ b/info/control2011/src/icp/kd_tree.mli
@@ -12,3 +12,11 @@ val nearest_neighbor : vertice -> 'a t -> 'a * vertice * float
[vert] *)
val depth : 'a t -> int
+
+val closer : float -> vertice -> 'a t -> bool
+(** [closer dist vert t] returns true if there is a point in [t]
+ closer than [dist] of [vert] *)
+
+val closer_points : float -> vertice -> 'a t -> 'a list
+(** [closer_points dist vert t] returns the lis tof all points in [t]
+ closer than [dist] of [vert] *)
diff --git a/info/control2011/src/lib/krobot_action.ml b/info/control2011/src/lib/krobot_action.ml
index 8b86239..290f661 100644
--- a/info/control2011/src/lib/krobot_action.ml
+++ b/info/control2011/src/lib/krobot_action.ml
@@ -10,15 +10,24 @@
open Printf
open Krobot_geom
-type t =
- | Node of t option * t list
+type node_kind =
+ | Simple
+ | Retry of int * t
+ | Loop of t
+ | Next
+
+and t =
+ | Node of node_kind * t list
| Stop
| Think
| Goto of vertice * vector option
+ | Simple_goto of vertice * vector option
+ | Random_move of ( vertice * vertice )
| Set_limits of float * float * float * float
| Follow_path of vertice list * vector option * bool
| Bezier of float * vertice * vertice * vertice * vertice * float
- | Set_curve of Bezier.curve option
+ | Move_back of float
+ | Set_curve of (bool * Bezier.curve) option
| Wait_for_jack of bool
| Wait_for_bezier_moving of bool * float option
| Wait_for_motors_moving of bool * float option
@@ -27,18 +36,11 @@ type t =
| Try_something of vertice
| Fail
| Wait_for_odometry_reset of [ `Red | `Blue | `Auto ]
- | Load of [ `Front | `Back ]
- | Lift_down of [ `Front | `Back ]
- | Lift_up of [ `Front | `Back ]
- | Open_grip_low of [ `Front | `Back ]
- | Close_grip_low of [ `Front | `Back ]
- | Open_grip_high of [ `Front | `Back ]
- | Close_grip_high of [ `Front | `Back ]
| Wait_for of float
| Wait_until of float
- | Wait_for_grip_open_low of [ `Front | `Back ]
- | Wait_for_grip_close_low of [ `Front | `Back ]
| Start_timer of float * t list
+ | Stop_timer
+ | Start_match
| Can of Krobot_can.frame
| Set_led of [ `Red | `Yellow | `Green ] * bool
| Set_orientation of float
@@ -58,14 +60,27 @@ let string_of_face = function
| `Back -> "`Back"
let rec to_string = function
- | Node (t,l) ->
- sprintf "Node [%s, %s]" (string_of_option to_string t) (list_to_string l)
+ | Node (Simple,l) ->
+ sprintf "Node [%s]" (list_to_string l)
+ | Node (Next,l) ->
+ sprintf "Node [Next,%s]" (list_to_string l)
+ | Node (Retry(n,l'),l) ->
+ sprintf "Node [%i, %s, %s]"
+ n (to_string l') (list_to_string l)
+ | Node (Loop t,l) ->
+ sprintf "Node [loop, %s, %s]" (to_string t) (list_to_string l)
| Stop ->
"Stop"
| Think ->
"Think"
+ | Move_back f ->
+ sprintf "Move_back %f" f
+ | Random_move (v1,v2) ->
+ sprintf "Random move %s %s" (string_of_vertice v1) (string_of_vertice v2)
| Goto (v,vect) ->
sprintf "Goto %s %s" (string_of_vertice v) (string_of_option string_of_vector vect)
+ | Simple_goto (v,vect) ->
+ sprintf "Simple_goto %s %s" (string_of_vertice v) (string_of_option string_of_vector vect)
| Set_limits (vmax,omega_max,atan_max, arad_max) ->
sprintf "Set_limits(%f, %f, %f, %f)" vmax omega_max atan_max arad_max
| Set_led (_,_) -> "Set_led"
@@ -83,8 +98,8 @@ let rec to_string = function
(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(Some (dir,c)) ->
+ sprintf "Set_curve(Some (%b, %s))" dir (Bezier.string_of_curve c)
| Set_curve None ->
"Set_curve None"
| Wait_for_jack st ->
@@ -115,30 +130,14 @@ let rec to_string = function
"Wait_for_odometry_reset `Blue"
| Wait_for_odometry_reset `Auto ->
"Wait_for_odometry_reset `Auto"
- | Load face ->
- sprintf "Load %s" (string_of_face face)
- | Lift_down face ->
- sprintf "Lift_down %s" (string_of_face face)
- | Lift_up face ->
- sprintf "Lift_up %s" (string_of_face face)
- | Open_grip_low face ->
- sprintf "Open_grip_low %s" (string_of_face face)
- | Close_grip_low face ->
- sprintf "Close_grip_low %s" (string_of_face face)
- | Open_grip_high face ->
- sprintf "Open_grip_high %s" (string_of_face face)
- | Close_grip_high face ->
- sprintf "Close_grip_high %s" (string_of_face face)
| Wait_for t ->
sprintf "Wait_for %f" t
| Wait_until t ->
sprintf "Wait_until %f" t
- | Wait_for_grip_open_low face ->
- sprintf "Wait_for_grip_open_low %S" (string_of_face face)
- | Wait_for_grip_close_low face ->
- sprintf "Wait_for_grip_close_low %S" (string_of_face face)
| Start_timer (delay,t) ->
sprintf "Start_timer(%f,%s)" delay (list_to_string t)
+ | Stop_timer ->
+ sprintf "Stop_timer"
| Can c -> "Can"
| Try_something v ->
sprintf "Try_something %s" (string_of_vertice v)
@@ -152,5 +151,6 @@ let rec to_string = function
(string_of_option string_of_float o)
| Calibrate (_,_,_,_,_,_) -> "Calibrate"
| End -> "End"
+ | Start_match -> "Start_match"
and list_to_string l = String.concat "; " (List.map to_string l)
diff --git a/info/control2011/src/lib/krobot_action.mli b/info/control2011/src/lib/krobot_action.mli
index a2ebb91..0ec9114 100644
--- a/info/control2011/src/lib/krobot_action.mli
+++ b/info/control2011/src/lib/krobot_action.mli
@@ -11,9 +11,16 @@
open Krobot_geom
+type node_kind =
+ | Simple
+ | Retry of int * t
+ | Loop of t
+ | Next
+
(** Type of actions. *)
-type t =
- | Node of t option * t list
+and t =
+ | Node of node_kind * t list
+
(** A sequence of action to execute in order. *)
| Stop
(** Stop all actions. *)
@@ -24,6 +31,10 @@ type t =
(** Go to the given point.
if the bool parameter is true, the path is inverted according
to the robot team *)
+ | Simple_goto of vertice * vector option
+ (** Same as Goto but can fail when obstacles block the trajectory *)
+
+ | Random_move of ( vertice * vertice )
| Set_limits of float * float * float * float
(** limit the speed *)
@@ -40,7 +51,8 @@ type t =
(** 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
+ | Move_back of float
+ | Set_curve of (bool * Bezier.curve) option
(** Set the curve currently being followed. *)
| Wait_for_jack of bool
(** Wait for the jack to be in the given state. *)
@@ -65,30 +77,14 @@ type t =
| Wait_for_odometry_reset of [ `Red | `Blue | `Auto ]
(** Wait for the odometry to say it is in red or blue initial position. *)
- | Load of [ `Front | `Back ]
- (** Load a pawn. *)
- | Lift_down of [ `Front | `Back ]
- (** Move the front or back lift down. *)
- | Lift_up of [ `Front | `Back ]
- (** Move the front or back lift up. *)
- | Open_grip_low of [ `Front | `Back ]
- (** Open the front or back low grip. *)
- | Close_grip_low of [ `Front | `Back ]
- (** Close the front or back low grip. *)
- | Open_grip_high of [ `Front | `Back ]
- (** Open the front or back low grip. *)
- | Close_grip_high of [ `Front | `Back ]
- (** Close the front or back low grip. *)
| Wait_for of float
(** Wait for the given number of seconds. *)
| Wait_until of float
(** Wait until the given date. *)
- | Wait_for_grip_open_low of [ `Front | `Back ]
- (** Wait for the given low grip to be opened. *)
- | Wait_for_grip_close_low of [ `Front | `Back ]
- (** Wait for the given low grip to be opened. *)
| Start_timer of float * t list
+ | Stop_timer
+ | Start_match
| Can of Krobot_can.frame
| Set_led of [ `Red | `Yellow | `Green ] * bool
@@ -110,3 +106,5 @@ type t =
val to_string : t -> string
(** [to_string action] returns the string representation of the
given string. *)
+
+val list_to_string : t list -> string
diff --git a/info/control2011/src/lib/krobot_bus.ml b/info/control2011/src/lib/krobot_bus.ml
index 387f972..25b85b7 100644
--- a/info/control2011/src/lib/krobot_bus.ml
+++ b/info/control2011/src/lib/krobot_bus.ml
@@ -31,7 +31,7 @@ type message =
| Trajectory_simplify of float
| Trajectory_go
| Trajectory_find_path
- | Objects of vertice list
+ | Objects of (vertice*float) list
| Sharps of float array
| Strategy_append of Krobot_action.t list
| Strategy_stop
@@ -45,6 +45,7 @@ type message =
| Urg_lines of (vertice*vertice) array
| Beacon_raw of (int * int * int * int * int * int
* int * int * int * int * int)
+ | Match_start
type t = {
oc : Lwt_io.output_channel;
@@ -60,6 +61,9 @@ open Printf
let string_of_vertice v =
sprintf "{ x = %f; y = %f }" v.x v.y
+let string_of_object (v,d) =
+ sprintf "{ x = %f; y = %f; d = %f }" v.x v.y d
+
let string_of_vector v =
sprintf "{ vx = %f; vy = %f }" v.vx v.vy
@@ -109,7 +113,7 @@ let string_of_message = function
| Objects objects ->
sprintf
"Objects [%s]"
- (String.concat "; " (List.map string_of_vertice objects))
+ (String.concat "; " (List.map string_of_object objects))
| Sharps a ->
sprintf
"Sharps [|%s|]"
@@ -156,6 +160,8 @@ let string_of_message = function
sprintf "Urg_lines (many_lines...)"
| Beacon_raw _ ->
sprintf "Raw beacon packet"
+ | Match_start ->
+ sprintf "Match start"
(* +-----------------------------------------------------------------+
| Sending/receiving messages |
diff --git a/info/control2011/src/lib/krobot_bus.mli b/info/control2011/src/lib/krobot_bus.mli
index b04aac3..28a6c1d 100644
--- a/info/control2011/src/lib/krobot_bus.mli
+++ b/info/control2011/src/lib/krobot_bus.mli
@@ -52,7 +52,7 @@ type message =
(** Objects *)
- | Objects of vertice list
+ | Objects of (vertice*float) list
(** The list of objects on the board. *)
(** Sharps *)
@@ -93,6 +93,8 @@ type message =
| Beacon_raw of (int * int * int * int * int * int
* int * int * int * int * int)
+ | Match_start
+
val string_of_message : message -> string
(** Returns a string representation of the given message. *)
diff --git a/info/control2011/src/lib/krobot_config.ml b/info/control2011/src/lib/krobot_config.ml
index e7975da..8eb06b0 100644
--- a/info/control2011/src/lib/krobot_config.ml
+++ b/info/control2011/src/lib/krobot_config.ml
@@ -11,7 +11,7 @@ let sqr x = x *. x
let world_height = 2.
let world_width = 3.
-let robot_length = 0.225
+let robot_length = 0.23
let robot_width = 0.30
let wheels_diameter = 0.098
let wheels_distance = 0.224
@@ -22,7 +22,7 @@ let robot_radius =
sqrt (l1 *. l1 +. l2 *. l2)
let rotary_beacon_index_pos = 0.
-let safety_margin = 0.01
+let safety_margin = 0.0
let beacon_radius = 0.2
@@ -33,14 +33,14 @@ open Krobot_geom
let pi = 4. *. atan 1.
let red_initial_position =
- { x = 0.10;
- y = world_height -. 0.25; },
- 0.
+ { x = world_width -. wheels_position -. 0.001;
+ y = (0.10 +. robot_width/.2.); },
+ pi
let blue_initial_position =
- { x = world_width -. 0.10;
- y = world_height -. 0.25; },
- pi
+ { x = wheels_position +. 0.001;
+ y = (0.10 +. robot_width/.2.); },
+ 0.
let symetrical p =
@@ -65,42 +65,33 @@ let line_obs p1 p2 =
let left_obstacles =
[
- (* the trees *)
- (*{ pos =
- { x = 0.64 +. 0.477;
- y = 1. };
- size = sqrt (2. *. 0.125 *. 0.125) +. 0.12; };*)
-
- (* entry position *)
- (*{ pos =
- { x = 0.;
- y = 1.5 };
- size = 0.02; };
- { pos =
- { x = 0.1;
- y = 1.5 };
- size = 0.02; };
- { pos =
- { x = 0.2;
- y = 1.5 };
- size = 0.02; };
- { pos =
- { x = 0.3;
- y = 1.5 };
- size = 0.02; };
- { pos =
- { x = 0.4;
- y = 1.5 };
- size = 0.02; };*)
(* coins at the bottom *)
-(*
- { pos =
- { x = 1.5;
- y = 0.3; };
- size = 0.15 };
-*)
-
+ (* we cheat a bit on the size to avoid problems... *)
+ (* { pos = *)
+ (* { x = 0.0; *)
+ (* y = 0.0; }; *)
+ (* size = 0.05 }; *)
+ (* { pos = *)
+ (* { x = 0.1; *)
+ (* y = 0.0; }; *)
+ (* size = 0.05 }; *)
+ (* { pos = *)
+ (* { x = 0.2; *)
+ (* y = 0.0; }; *)
+ (* size = 0.05 }; *)
+ (* { pos = *)
+ (* { x = 0.3; *)
+ (* y = 0.0; }; *)
+ (* size = 0.05 }; *)
+ (* { pos = *)
+ (* { x = 0.4; *)
+ (* y = 0.0; }; *)
+ (* size = 0.05 }; *)
+ (* { pos = *)
+ (* { x = 0.35; *)
+ (* y = 0.05; }; *)
+ (* size = 0.05 }; *)
(*] @ (line_obs { x = 0.325; y = 0. } { x = 0.325 +. (0.075 /. 2.); y = 0.75 } )*)
]
@@ -113,6 +104,11 @@ let fixed_obstacles =
size = 0.5; };
] @ (List.map symetrical left_obstacles) @ left_obstacles
+let test_obstacles =
+ [ { pos =
+ { x = 1.5;
+ y = 1.0 };
+ size = 0.1; }; ]
let initial_coins = []
(*
@@ -123,7 +119,7 @@ let initial_coins = []
(* 2.55, 0.3; *) ]
*)
-let urg_position = { x = 0.135; y = 0. }
+let urg_position = { x = 0.095; y = 0. }
let urg_angles =
[|-2.356194490192344836998472601408; -2.350058567040802071090865865699;
diff --git a/info/control2011/src/lib/krobot_config.mli b/info/control2011/src/lib/krobot_config.mli
index 1f88fa2..313c3b1 100644
--- a/info/control2011/src/lib/krobot_config.mli
+++ b/info/control2011/src/lib/krobot_config.mli
@@ -54,6 +54,8 @@ val blue_initial_position : Krobot_geom.vertice * float
val fixed_obstacles : Krobot_geom.obj list
+val test_obstacles : Krobot_geom.obj list
+
val initial_coins : Krobot_geom.vertice list
val urg_position : Krobot_geom.vertice
diff --git a/info/control2011/src/lib/krobot_geom.ml b/info/control2011/src/lib/krobot_geom.ml
index ddcd72e..8169410 100644
--- a/info/control2011/src/lib/krobot_geom.ml
+++ b/info/control2011/src/lib/krobot_geom.ml
@@ -236,7 +236,6 @@ module Bezier = struct
exception Exit_for_vel
let velocity_profile b v_max omega_max at_max ar_max v_ini v_end du =
- Printf.printf "inside velocity_profile\n%!";
let n_pts = int_of_float ( 1. /. du +. 1.) in
let us = Array.init n_pts (fun i -> (float_of_int i) *. du) in
let v_tab = Array.map (fun _ -> v_max) us in
diff --git a/info/control2011/src/lib/krobot_path.ml b/info/control2011/src/lib/krobot_path.ml
index 49b2311..84818cf 100644
--- a/info/control2011/src/lib/krobot_path.ml
+++ b/info/control2011/src/lib/krobot_path.ml
@@ -18,18 +18,22 @@ let rec prev_last = function
| _ :: l ->
prev_last l
-let find ~src ~dst ~beacon =
+let find ?src_orient ?dst_orient ~src ~dst ~beacon objects =
let fixed_objects = List.map (fun { pos; size } -> pos,
size +. Krobot_config.robot_radius +. 0.01)
Krobot_config.fixed_obstacles in
+ let objects = List.map (fun { pos; size } -> pos,
+ size +. Krobot_config.robot_radius +. 0.01)
+ objects in
+
(* do that in a better way when we have time... *)
let init_coins = List.map (fun pos -> pos,
Krobot_config.coin_radius +. Krobot_config.robot_radius +. 0.01)
Krobot_config.initial_coins in
- let l = fixed_objects @ init_coins in
+ let l = objects @ init_coins @ fixed_objects in
let l =
match beacon with
| (Some v, None)
@@ -47,7 +51,7 @@ let find ~src ~dst ~beacon =
in
let l = List.map (fun (v,s) -> (v, min s (distance v src -. 0.1))) l in
let min_distance = Krobot_config.robot_radius +. safety_margin in
- Krobot_pathfinding.find_path ~src ~dst
+ Krobot_pathfinding.find_path ?src_orient ?dst_orient ~src ~dst
({ x = min_distance;
y = min_distance},
{ x = world_width -. min_distance;
diff --git a/info/control2011/src/lib/krobot_path.mli b/info/control2011/src/lib/krobot_path.mli
index b7d3637..f3294c7 100644
--- a/info/control2011/src/lib/krobot_path.mli
+++ b/info/control2011/src/lib/krobot_path.mli
@@ -11,7 +11,13 @@
open Krobot_geom
-val find : src : vertice -> dst : vertice -> beacon : vertice option * vertice option -> vertice list option
+val find :
+ ?src_orient:float * vector ->
+ ?dst_orient:float * vector ->
+ src : vertice -> dst : vertice ->
+ beacon : vertice option * vertice option ->
+ obj list ->
+ vertice list option
(** [goto ~src ~dst ~beacon] find a path from [src] to [dst]. *)
(*
val goto_object : src : vertice -> dst : vertice -> beacon : vertice option * vertice option -> vertice option
diff --git a/info/control2011/src/lib/krobot_pathfinding.ml b/info/control2011/src/lib/krobot_pathfinding.ml
index ebfbd64..9710d68 100644
--- a/info/control2011/src/lib/krobot_pathfinding.ml
+++ b/info/control2011/src/lib/krobot_pathfinding.ml
@@ -358,11 +358,38 @@ let v_of_vertice { Krobot_geom.x; y } =
let vertice_of_seg { p1 = {px;py} } =
{ Krobot_geom.x = px; y = py }
-let find_path ~src ~dst (box_min,box_max) objects =
+let orientation_circles radius vect p =
+ let unit = unitaire vect in
+ let back_left = unitaire ((turn_trigo unit) -| (0.5 *@ unit)) in
+ let back_right = unitaire ((turn_antitrigo unit) -| (0.5 *@ unit)) in
+ let c1 = p +! (radius +. epsilon_float) *@ back_left in
+ let c2 = p +! (radius +. epsilon_float) *@ back_right in
+ [{ c = c1; r = radius };
+ { c = c2; r = radius }]
+
+let v_of_geom_v { Krobot_geom.vx; Krobot_geom.vy} = { vx; vy }
+
+let add_start_orientation b radius vect =
+ let crcls = orientation_circles radius vect b.src in
+ { b with obstacles = crcls @ b.obstacles }
+
+let add_end_orientation b radius vect =
+ let crcls = orientation_circles radius (-. 1. *@ vect) b.dst in
+ { b with obstacles = crcls @ b.obstacles }
+
+let find_path ?src_orient ?dst_orient ~src ~dst (box_min,box_max) objects =
let b = { src = v_of_vertice src;
dst = v_of_vertice dst;
obstacles = List.map (fun (v,r) -> { c = v_of_vertice v; r }) objects;
box = (v_of_vertice box_min, v_of_vertice box_max)} in
+
+ let b = match src_orient with
+ | None -> b
+ | Some (radius,vect) -> add_start_orientation b radius (v_of_geom_v vect) in
+ let b = match dst_orient with
+ | None -> b
+ | Some (radius,vect) -> add_end_orientation b radius (v_of_geom_v vect) in
+
(* let b = filter_far_objects b b.src in *)
if not ( (in_box b.box b.dst) && (check_point b b.dst) && (check_point b b.src) )
then
diff --git a/info/control2011/src/lib/krobot_pathfinding.mli b/info/control2011/src/lib/krobot_pathfinding.mli
index 1b6644d..dcef822 100644
--- a/info/control2011/src/lib/krobot_pathfinding.mli
+++ b/info/control2011/src/lib/krobot_pathfinding.mli
@@ -2,6 +2,8 @@
open Krobot_geom
val find_path :
+ ?src_orient:float * vector ->
+ ?dst_orient:float * vector ->
src:vertice ->
dst:vertice ->
vertice * vertice ->
diff --git a/info/control2011/src/tools/krobot_homologation.ml b/info/control2011/src/tools/krobot_homologation.ml
index 7823746..52051b0 100644
--- a/info/control2011/src/tools/krobot_homologation.ml
+++ b/info/control2011/src/tools/krobot_homologation.ml
@@ -16,65 +16,160 @@ open Krobot_bus
open Krobot_action
open Krobot_geom
-let init_pos, init_angle = Krobot_config.red_initial_position
-
-let path =
- [
- { x = 0.7; y = init_pos.y -. 0.1};
- { x = 0.85; y = 1.5 };
- { x = 0.75; y = 1.20 };
- { x = 0.55; y = 1.15 };
- { x = 0.4; y = 1.15 };
- ]
-
-let strat_loop =
- [
- Calibrate ( { x = 0.64 +. 0.477; y = 1. +. 0.125 +. 0.1; },
- pi /. 2.,
- 0.20,
- None,
- Some (1. +. 0.125),
- Some (pi /. 2.) );
-
- Calibrate ( { x = 0.9; y = 1.; },
- pi,
- 0.20,
- Some (0.64 +. 0.477 -. 0.125),
- None,
- Some (pi) );
-
- Goto ({ x = 1.5; y = 0.5 }, None);
- Goto ({ x = 1.5; y = 1.5 }, None);
- Goto ({ x = 1.5; y = 0.5 }, None);
- Goto ({ x = 1.5; y = 1.5 }, None);
- Wait_for 2.;
- End;
- ]
-
-let strat_base =
- [
- Can (Krobot_message.encode (Drive_activation true));
- Wait_for_jack true;
- Wait_for 1.;
- Wait_for_jack false;
- Start_timer (10.,[End]);
- Set_led(`Red,false);
- Set_led(`Green,false);
- Reset_odometry `Auto;
- Wait_for_odometry_reset `Auto;
- Set_limits (0.3,3.14,1.0,1.0);
- End;
- ]
-
-let launch bus =
- Krobot_bus.send bus
- (Unix.gettimeofday (),
- Strategy_set strat_base)
-
-let loop bus =
- Krobot_bus.send bus
- (Unix.gettimeofday (),
- Strategy_set strat_loop)
+type reset = [ `Auto | `Blue | `Red ]
+
+let gift_width = 0.15
+
+let init_y = 0.244
+
+let ax12_2_base_position = 519
+let ax12_2_high_position = 210
+
+let ax12_2_mid_position = 400
+let ax12_1_mid_position = 600
+
+let ax12_1_base_position = 518
+let ax12_1_high_position = 823
+
+let init_blue_pos =
+ Set_odometry (
+ Some (Krobot_config.wheels_position +. 0.001),
+ Some 24.4,
+ Some 0.)
+
+let gifts_positions =
+ [ { x = 0.60 ; y = 0. };
+ { x = 1.2 ; y = 0. };
+ { x = 1.8 ; y = 0. };
+ { x = 2.4 ; y = 0. }; ]
+
+let gift_shift = -. (gift_width /. 2.) -. 0.09
+let gift_position1 = { x = 0.60 ; y = 0. }
+let gift_position2 = { x = 1.2 ; y = 0. }
+let gift_position3 = { x = 1.8 ; y = 0. }
+let gift_position4 = { x = 2.4 ; y = 0. }
+
+let gift_go pos = { x = pos.x +. gift_shift ; y = init_y }
+let gift_go_red pos = { x = pos.x -. gift_shift ; y = init_y }
+
+let gitf_go1 = { x = gift_position1.x +. gift_shift ; y = init_y }
+let gift_dir1_blue = { vx = -.1.; vy = 0. }
+let gift_dir1_red = { vx = 1.; vy = 0. }
+
+let retry_n n t = Node(Retry(n,Node(Simple,[Stop;Wait_for 0.2;t])),[t])
+
+let node t = Node(Simple,t)
+
+let back_and_retry t =
+ Node(Retry(3,
+ node ((Node(Next,[Stop;Move_back 0.1])) :: t)),
+ t)
+
+let retry_follow n dst dir =
+ let follow =
+ Follow_path ([dst], Some dir, false) in
+ let tmp =
+ Node(Retry(n,node [Stop;Wait_for 0.2;follow]),[follow]) in
+ Node(Next,[back_and_retry [tmp]])
+
+let act1_blue = retry_follow 15 (gift_go gift_position1) gift_dir1_blue
+let act2_blue = retry_follow 15 (gift_go gift_position2) gift_dir1_blue
+let act3_blue = retry_follow 15 (gift_go gift_position3) gift_dir1_blue
+let act4_blue = retry_follow 15 (gift_go gift_position4) gift_dir1_blue
+
+let act1_red = retry_follow 15 (gift_go_red gift_position4) gift_dir1_red
+let act2_red = retry_follow 15 (gift_go_red gift_position3) gift_dir1_red
+let act3_red = retry_follow 15 (gift_go_red gift_position2) gift_dir1_red
+let act4_red = retry_follow 15 (gift_go_red gift_position1) gift_dir1_red
+
+
+let hit_ax12_id id =
+ let base, high =
+ if id = 1
+ then ax12_1_base_position, ax12_1_high_position
+ else ax12_2_base_position, ax12_2_high_position
+ in
+ Node (Simple,
+ [Can (Krobot_message.encode (Ax12_Goto (id, high, 0)));
+ Wait_for 0.3;
+ Can (Krobot_message.encode (Ax12_Goto (id, base, 0)));
+ Wait_for 0.3])
+
+let ramene p1 p2 dir =
+ [ Goto (p1, None);
+ Move_back 0.4;
+ Goto (p2, Some dir); ]
+
+let pos1 = { x = 0.4 ; y = 0.7 }
+let pos2 = { x = 2.6 ; y = 0.7 }
+let pos3 = { x = 2.6 ; y = 1.5 }
+let pos4 = { x = 2.6 ; y = 1.0 }
+
+let haut = { vx = 0. ; vy = 1. }
+let bas = { vx = 0. ; vy = -.1. }
+
+let inner_loop_blue =
+ let l =
+ ramene pos1 pos2 haut
+ @ ramene pos1 pos3 bas
+ @ ramene pos1 pos4 haut in
+ Node(Loop(node l),l)
+
+let sym_pos p = { p with x = 3. -. p.x }
+
+let pos1_red = sym_pos { x = 0.4 ; y = 0.7 }
+let pos2_red = sym_pos { x = 2.6 ; y = 0.7 }
+let pos3_red = sym_pos { x = 2.6 ; y = 1.5 }
+let pos4_red = sym_pos { x = 2.6 ; y = 1.0 }
+
+let inner_loop_red =
+ let l =
+ ramene pos1_red pos2_red haut
+ @ ramene pos1_red pos3_red bas
+ @ ramene pos1_red pos4_red haut in
+ Node(Loop(node l),l)
+
+
+let run_strategy_blue =
+ [ act1_blue;
+ Stop;
+ hit_ax12_id 2;
+ act2_blue;
+ Stop;
+ hit_ax12_id 2;
+ act3_blue;
+ Stop;
+ hit_ax12_id 2;
+ act4_blue;
+ Stop;
+ hit_ax12_id 2;
+ inner_loop_blue;
+ ]
+
+let run_strategy_red =
+ [ act1_red;
+ Stop;
+ hit_ax12_id 1;
+ act2_red;
+ Stop;
+ hit_ax12_id 1;
+ act3_red;
+ Stop;
+ hit_ax12_id 1;
+ act4_red;
+ Stop;
+ hit_ax12_id 1;
+ inner_loop_red;
+ ]
+
+let team_gift_position team p =
+ match team with
+ | `Red -> { x = p.x +. (gift_width /. 2. +. 0.09); y = p.y }
+ | `Blue -> { x= p.x -. (gift_width /. 2. +. 0.09); y = p.y }
+
+(* let gift_destination team p = *)
+(* let p = team_gift_position team p in *)
+(* { x = p.x; y = p.y +. gift_distance } *)
type status = {
bus : Krobot_bus.t;
@@ -83,12 +178,213 @@ type status = {
(* The state of the team selector. *)
}
+let vmax = 0.5
+let omega_max = 3.14 /. 2.
+let accel_tan_max = 1.0
+let accel_rad_max = 1.0
+
+let gonfle_baloon =
+ [Can (Krobot_message.encode (Motor_command (2,3600)));
+ Wait_for 10.;
+ Can (Krobot_message.encode (Motor_command (2,0)));
+ Wait_for 0.1]
+
+let start team =
+ (* assert(team = `Blue); *)
+ [ Stop_timer;
+ Wait_for 0.1;
+ (* init_blue_pos; *)
+ Reset_odometry (team:>reset);
+ Can (Krobot_message.encode (Drive_activation true));
+ Can (Krobot_message.encode (Ax12_Set_Torque_Enable (2,true)));
+ Wait_for 0.1;
+ Can (Krobot_message.encode (Ax12_Set_Torque_Enable (1,true)));
+ Wait_for 0.1;
+ Can (Krobot_message.encode (Ax12_Goto (2, ax12_2_mid_position, 0)));
+ Wait_for 0.1;
+ Can (Krobot_message.encode (Ax12_Goto (1, ax12_1_mid_position, 0)));
+ Wait_for 0.1;
+ Can (Krobot_message.encode (Ax12_Goto (2, ax12_2_base_position, 0)));
+ Wait_for 0.1;
+ Can (Krobot_message.encode (Ax12_Goto (1, ax12_1_base_position, 0)));
+ Wait_for_jack true;
+ Wait_for 0.4;
+ Wait_for_jack false;
+ Start_timer (90.,[Stop] @ gonfle_baloon @ [End]);
+ Start_match;
+ Set_led(`Red,false);
+ Set_led(`Green,false);
+ Reset_odometry (team:>reset);
+ Set_led(`Red,true);
+ (* init_blue_pos; *)
+ (* Wait_for 0.05; *)
+ Wait_for_odometry_reset (team:>reset);
+ Set_led(`Red,true);
+ Set_limits (vmax,omega_max,accel_tan_max,accel_rad_max) ]
+
+let end_ =
+ [ End; ]
+
+let hit_ax12_id id =
+ let base, high =
+ if id = 1
+ then ax12_1_base_position, ax12_1_high_position
+ else ax12_2_base_position, ax12_2_high_position
+ in
+ [Can (Krobot_message.encode (Ax12_Set_Torque_Enable (2,true)));
+ Wait_for 0.1;
+ Can (Krobot_message.encode (Ax12_Goto (id, high, 100)));
+ Wait_for 1.;
+ Can (Krobot_message.encode (Ax12_Goto (id, base, 100)));
+ Wait_for 1.]
+
+let hit_ax12_dir dir =
+ (* TODO: test ! *)
+ let ax12id = if dir.vx < 0. then 1 else 2 in
+ hit_ax12_id ax12id
+
+let retry_follow_node dest dir =
+ let rec follow_node =
+ Node(Retry(1, Node(Simple,[Wait_for 0.2; follow_node])),
+ [Follow_path ([dest], Some dir, false)]) in
+ follow_node
+
+let direction pos = function
+ | `Blue ->
+ (* if pos.x >= 2.2 *)
+ (* then { vx = -.1.; vy = 0. } *)
+ (* else *)
+ { vx = 1.; vy = 0. }
+ | `Red ->
+ (* if pos.x <= 0.8 *)
+ (* then { vx = 1.; vy = 0. } *)
+ (* else *)
+ { vx = -.1.; vy = 0. }
+
+let retry_n n t = Node(Retry(n,Node(Simple,[Stop;Wait_for 0.05;t])),[t])
+
+let go_first_gift pos dir =
+ let shift_len = 0.2 in
+ let shift =
+ if dir.vx >= 0.
+ then shift_len
+ else -. shift_len in
+ let back_shifted_position =
+ { x = pos.x +. shift; y = Krobot_config.robot_radius +. 0.01 } in
+ Node(Simple,
+ [ retry_n 2 (Simple_goto (back_shifted_position, Some (minus dir)));
+ retry_n 2 (Follow_path ([pos], Some dir, false)); ])
+
+let approach_lower_border retries pos dir =
+ let shift_len = 0.2 in
+ let shift =
+ if dir.vx >= 0.
+ then shift_len
+ else -. shift_len in
+ let shifted_position =
+ { x = pos.x -. shift; y = Krobot_config.robot_radius +. 0.01 } in
+ let back_shifted_position =
+ { x = pos.x +. shift; y = Krobot_config.robot_radius +. 0.01 } in
+ let goto_normal =
+ Node(Simple,
+ [ retry_n retries (Simple_goto (shifted_position, Some (minus dir)));
+ retry_n retries (Follow_path ([pos], Some (minus dir), false)); ]) in
+ let goto_back =
+ Node(Simple,
+ [ retry_n retries (Simple_goto (back_shifted_position, Some (minus dir)));
+ retry_n retries (Follow_path ([pos], Some dir, false)); ]) in
+ [Node(Retry(1,goto_back),[goto_normal])]
+
+ (* let goto_path = *)
+ (* Node(Simple, *)
+ (* [ retry_n retries (Simple_goto (shifted_position, Some (minus dir))); *)
+ (* retry_n retries (Follow_path ([pos], Some dir, false)); ]) in *)
+ (* let simple_path = *)
+ (* retry_n 2 (Follow_path ([pos], Some (minus dir), false)) in *)
+ (* [Node (Retry(1,goto_path), [simple_path])] *)
+
+(* let approach_lower_border pos dir = *)
+(* let shift_len = 0.1 in *)
+(* let shift = *)
+(* if dir.vx >= 0. *)
+(* then shift_len *)
+(* else -. shift_len in *)
+(* let shifted_position = *)
+(* { x = pos.x +. shift; y = Krobot_config.robot_radius +. 0.01 } in *)
+(* [ Goto (shifted_position, Some (minus dir)); *)
+(* (\* Follow_path ([pos], Some dir, false); *\) *)
+(* retry_follow_node pos dir; ] *)
+
+(* let goto_gift retries team gift = *)
+(* let destination = gift_destination team gift in *)
+(* approach_lower_border retries destination (direction destination team) *)
+
+(* let do_gift retries team gift = *)
+(* let destination = gift_destination team gift in *)
+(* let dir = direction destination team in *)
+(* let goto = approach_lower_border retries destination dir in *)
+(* let hit = hit_ax12_dir dir in *)
+(* Node(Next,goto @ hit) *)
+
+(* let do_first_gift team gift = *)
+(* let destination = gift_destination team gift in *)
+(* let dir = direction destination team in *)
+(* let goto = go_first_gift destination dir in *)
+(* let hit = hit_ax12_dir dir in *)
+(* Node(Next,goto :: hit) *)
+
+(* let gifts_actions retries team = *)
+(* let gifts = match team with *)
+(* | `Red -> List.rev gifts_positions *)
+(* | `Blue -> gifts_positions *)
+(* in *)
+(* let first = List.hd gifts in *)
+(* let rest = List.tl gifts in *)
+(* do_first_gift team first *)
+(* ,(List.map (do_gift retries team) rest) *)
+
+(* let random team = *)
+(* let zone = match team with *)
+(* | `Red -> ({x = 0.3; y = 0.4},{x = 0.6; y = 1.5}) *)
+(* | `Blue -> ({x = 2.2; y = 0.4},{x = 2.5; y = 1.5}) in *)
+(* let home = match team with *)
+(* | `Red -> ({x = 2.5; y = 0.4},{x = 2.4; y = 1.}) *)
+(* | `Blue -> ({x = 0.4; y = 0.4},{x = 0.5; y = 1.}) in *)
+(* let go = Random_move zone in *)
+(* let back' = Random_move home in *)
+(* let back = Node(Retry(3,Node(Simple,[Stop;back'])),[back']) in *)
+(* let r = Node(Simple,[Stop;go;back;Move_back 0.1]) in *)
+(* [Node(Loop(Node(Simple,[Stop;Wait_for 0.1;r])),[r])] *)
+
+(* let loop l = Node(Loop(Node(Simple,l)),l) *)
+
+(* let strategy gift_retries team = *)
+(* let first_gift, rest_gift = gifts_actions gift_retries team in *)
+(* start team @ [first_gift] @ rest_gift @ random team @ end_ *)
+
+
+(* let launch bus team = *)
+(* Krobot_bus.send bus *)
+(* (Unix.gettimeofday (), *)
+(* Strategy_set (strategy 20 team)) *)
+
+
+let launch bus team =
+ let s = match team with
+ | `Red -> run_strategy_red
+ | `Blue -> run_strategy_blue
+ in
+ let strat = start team @ s in
+ Krobot_bus.send bus (Unix.gettimeofday (),
+ Strategy_set strat)
+
+
let update_team_led status =
let m1,m2 =
if status.team = `Red then
- Switch_request(7,true), Switch_request(6,false)
- else
Switch_request(7,false), Switch_request(6,true)
+ else
+ Switch_request(7,true), Switch_request(6,false)
in
lwt () = Krobot_message.send status.bus (Unix.gettimeofday (),m1) in
Krobot_message.send status.bus (Unix.gettimeofday (), m2)
@@ -106,14 +402,14 @@ let handle_message status (timestamp, message) =
begin
status.team <- team;
ignore (update_team_led status);
- ignore (launch bus)
+ ignore (launch bus status.team)
end
| _ ->
()
end
- | Strategy_finished ->
- ignore (loop bus)
+ (* | Strategy_finished -> *)
+ (* ignore (loop bus) *)
| Kill "homologation" ->
exit 0
diff --git a/info/control2011/src/tools/krobot_objects.ml b/info/control2011/src/tools/krobot_objects.ml
index 23a855a..460a925 100644
--- a/info/control2011/src/tools/krobot_objects.ml
+++ b/info/control2011/src/tools/krobot_objects.ml
@@ -32,6 +32,8 @@ type objects = {
| Message handling |
+-----------------------------------------------------------------+ *)
+let add_diameter = List.map (fun v -> v,0.1)
+
let handle_message objects (timestamp, message) =
match message with
| Kill "objects" ->
@@ -40,7 +42,7 @@ let handle_message objects (timestamp, message) =
| Send ->
ignore (
let ts = Unix.gettimeofday () in
- Krobot_bus.send objects.bus (ts, Objects objects.objects)
+ Krobot_bus.send objects.bus (ts, Objects (add_diameter objects.objects))
)
| _ ->
@@ -109,7 +111,7 @@ lwt () =
E.keep (E.map (handle_message objects) (Krobot_bus.recv bus));
(* Sends initial objects. *)
- lwt () = Krobot_bus.send objects.bus (Unix.gettimeofday (), Objects objects.objects) in
+ lwt () = Krobot_bus.send objects.bus (Unix.gettimeofday (), Objects (add_diameter objects.objects)) in
(* Wait forever. *)
fst (wait ())
diff --git a/info/control2011/src/tools/krobot_planner.ml b/info/control2011/src/tools/krobot_planner.ml
index 6b8e8d5..209a9c6 100644
--- a/info/control2011/src/tools/krobot_planner.ml
+++ b/info/control2011/src/tools/krobot_planner.ml
@@ -50,7 +50,7 @@ type planner = {
+-----------------------------------------------------------------+ *)
let find_path planner src dst =
- Krobot_path.find ~src ~dst ~beacon:planner.beacon
+ Krobot_path.find ~src ~dst ~beacon:planner.beacon []
(* +-----------------------------------------------------------------+
| Primitives |
diff --git a/info/control2011/src/tools/krobot_simulator.ml b/info/control2011/src/tools/krobot_simulator.ml
index 7d5df77..d27c3ff 100644
--- a/info/control2011/src/tools/krobot_simulator.ml
+++ b/info/control2011/src/tools/krobot_simulator.ml
@@ -148,7 +148,8 @@ let velocities sim dt =
in
match sim.bezier_next with
| None ->
- sim.command <- Idle
+ sim.command <- Idle;
+ sim.bezier_curve <- None
| Some (curve,v_end,dir) ->
let v_max, omega_max, at_max, ar_max = sim.bezier_limits in
sim.command <-
@@ -417,10 +418,10 @@ let handle_message bus (timestamp, message) =
else begin
match decode frame with
| Motor_move(dist, speed, acc) ->
- Lwt_unix.run (Lwt_log.info_f "received: move(%f, %f, %f)" dist speed acc);
+ ignore (Lwt_log.info_f "received: move(%f, %f, %f)" dist speed acc);
move sim dist speed acc
| Motor_turn(angle, speed, acc) ->
- Lwt_unix.run (Lwt_log.info_f "received: turn(%f, %f, %f)" angle speed acc);
+ ignore (Lwt_log.info_f "received: turn(%f, %f, %f)" angle speed acc);
turn sim angle speed acc
| Motor_stop(lin_acc, rot_acc) ->
sim.command <- Idle;
@@ -433,7 +434,7 @@ let handle_message bus (timestamp, message) =
| Motor_bezier_limits(v_max, omega_max, a_tan_max, a_rad_max) ->
sim.bezier_limits <- (v_max, omega_max, a_tan_max, a_rad_max)
| Motor_bezier(x_end, y_end, d1, d2, theta_end, v_end) ->
- Lwt_unix.run (Lwt_log.info_f "received: bezier(%f, %f, %f, %f, %f, %f)" x_end y_end d1 d2 theta_end v_end);
+ ignore (Lwt_log.info_f "received: bezier(%f, %f, %f, %f, %f, %f)" x_end y_end d1 d2 theta_end v_end);
bezier sim (x_end, y_end, d1, d2, theta_end, v_end)
| _ ->
() end);
@@ -539,12 +540,22 @@ let closest_obstacle x y theta objs =
done;
Array.of_list !l*)
+let circle_obstacle r { pos = { Krobot_geom.x; y }; size } =
+ let t = Unix.gettimeofday () in
+ let t = 0.3 *. t in
+ let x' = x +. r *. cos t in
+ let y' = y +. r *. sin t in
+ { pos = { Krobot_geom.x = x'; y = y' }; size }
+
let gen_data robot =
let dim = Array.length Krobot_config.urg_angles in
let l = ref [] in
for i = 0 to dim - 1 do
let angle = Krobot_config.urg_angles.(i) in
- match closest_obstacle robot.x robot.y (robot.theta +. angle) Krobot_config.fixed_obstacles with
+ let test_obstacles = List.map (circle_obstacle 0.2)
+ Krobot_config.test_obstacles in
+ let obstacles = test_obstacles @ Krobot_config.fixed_obstacles in
+ match closest_obstacle robot.x robot.y (robot.theta +. angle) obstacles with
| Some dist ->
let x = dist *. cos angle in
let y = dist *. sin angle in
diff --git a/info/control2011/src/tools/krobot_urg.ml b/info/control2011/src/tools/krobot_urg.ml
index aecb465..f01903a 100644
--- a/info/control2011/src/tools/krobot_urg.ml
+++ b/info/control2011/src/tools/krobot_urg.ml
@@ -22,6 +22,11 @@ let section = Lwt_log.Section.make "krobot(urg)"
| read/send loop |
+-----------------------------------------------------------------+ *)
+let min_index = 90
+let max_index = 690
+
+let min_distance = 60
+
let scale = 0.001
let convert_pos dist angle =
@@ -32,9 +37,9 @@ let convert_pos dist angle =
let convert (b:Urg.point_data) =
let dim = Bigarray.Array1.dim b in
let l = ref [] in
- for i = 0 to dim - 1 do
+ for i = max 0 min_index to min (dim - 1) max_index do
let data = Nativeint.to_int b.{i} in
- if data > 0
+ if data > min_distance
then
let angle = Krobot_config.urg_angles.(i) in
let (x,y) = convert_pos data angle in
diff --git a/info/control2011/src/tools/krobot_urg_extract.ml b/info/control2011/src/tools/krobot_urg_extract.ml
new file mode 100644
index 0000000..75a88dc
--- /dev/null
+++ b/info/control2011/src/tools/krobot_urg_extract.ml
@@ -0,0 +1,182 @@
+open Lwt
+open Lwt_react
+open Lwt_preemptive
+open Krobot_bus
+open Krobot_message
+open Krobot_geom
+open Icp_minimisation
+
+let section = Lwt_log.Section.make "krobot(urg_extract)"
+
+type info = {
+ bus : Krobot_bus.t;
+ (* The message bus used to communicate with the robot. *)
+
+ mutable position : vertice;
+ (* The position of the robot on the table. *)
+
+ mutable orientation : float;
+ (* The orientation of the robot. *)
+
+ mutable urg : Krobot_geom.vertice array;
+}
+
+
+(**********************)
+
+let default_obstacle_diameter = 0.04
+let keep_above_dist = 0.1
+
+(* let table = Icp_utils.table 2. 3. 200 *)
+let table = Icp_utils.real_table 100
+let table_kd = make_kd_tree table
+
+let a0 = { ath = 0.; ax = 0.; ay = 0. }
+
+let filter_data keep_above_dist robot_transform data =
+ let back_transform = Icp_utils.invert_transform robot_transform in
+ let data = transform back_transform data in
+ Icp_utils.far_enougth_filter table_kd keep_above_dist data
+
+let mark_close kd marking dist v marking_val =
+ let close_points = Kd_tree.closer_points dist v kd in
+ List.iter (fun i -> marking.(i) <- Some marking_val) close_points
+
+let mark_circles diameter data =
+ let kd = make_kd_tree data in
+ let marking = Array.map (fun _ -> None) data.dx in
+ let count = ref 0 in
+ let a = Array.mapi (fun i -> function
+ | Some c -> c
+ | None ->
+ let v = { Kd_tree.x = data.dx.(i); y = data.dy.(i) } in
+ mark_close kd marking diameter v !count;
+ let c = !count in
+ incr count;
+ c) marking in
+ !count, a
+
+let baricenter { dx; dy } l =
+ let len = List.length l in
+ let accx = List.fold_left (fun acc i -> acc +. dx.(i)) 0. l in
+ let accy = List.fold_left (fun acc i -> acc +. dy.(i)) 0. l in
+ { Krobot_geom.x = accx /. (float len)...
[truncated message content] |
