[Krobot-commits] [SCM] krobot branch, master, updated. 7558393126f5f1a81a625ac777ee1875348d37e0

[Jérémie D. <Ba...@us...>]

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  7558393126f5f1a81a625ac777ee1875348d37e0 (commit)
      from  412d97271818ea4c3dd581b190b7ef306decf027 (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 7558393126f5f1a81a625ac777ee1875348d37e0
Author: Jérémie Dimino <je...@di...>
Date:   Sat Mar 27 16:55:46 2010 +0100

    Trajectory along a graph

-----------------------------------------------------------------------

Changes:
diff --git a/PC_Mainboard/krobot.mllib b/PC_Mainboard/krobot.mllib
index 6116255..03fb8f1 100644
--- a/PC_Mainboard/krobot.mllib
+++ b/PC_Mainboard/krobot.mllib
@@ -1,4 +1,5 @@
 lib_krobot/Krobot
 lib_krobot/Krobot_unsafe
+lib_krobot/Krobot_move
 common/Krobot_types
 common/Krobot_config
diff --git a/PC_Mainboard/lib_krobot/krobot_move.ml b/PC_Mainboard/lib_krobot/krobot_move.ml
new file mode 100644
index 0000000..69840f3
--- /dev/null
+++ b/PC_Mainboard/lib_krobot/krobot_move.ml
@@ -0,0 +1,92 @@
+(*
+ * krobot_move.ml
+ * --------------
+ * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
+ * Licence   : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+type vertex = {
+  vx : int;
+  vy : int;
+}
+
+module Vertex =
+struct
+  type t = vertex
+  let compare = compare
+end
+
+module Edge =
+struct
+  type t = vertex * vertex
+  let compare = compare
+end
+
+module Vertex_set = Set.Make(Vertex)
+module Edge_set = Set.Make(Edge)
+module Vertex_map = Map.Make(Vertex)
+
+type graph = Edge_set.t
+
+let empty =
+  Edge_set.empty
+
+let add_edge (a, b) graph =
+  Edge_set.add (a, b) (Edge_set.add (b, a) graph)
+
+let square x = x * x
+
+let distance a b = truncate (sqrt (float_of_int (square (a.vx - b.vy) + square (a.vy - b.vy))))
+let angle a b = truncate (atan2 (float_of_int (b.vy - a.vy)) (float_of_int (b.vx - a.vx)) *. 180. /. 3.14159265) - 90
+
+type vertex_search_info = {
+  vsi_distance : int;
+  (* The lemgth of the smallest path reaching this vertex *)
+
+  vsi_initial : vertex;
+  (* The vertex that was initially choosen for this path *)
+}
+
+let move ~graph ~position ~destination =
+  (* [visited] is a mapping from already visited vertexs to the length
+     of the smallest path to reach them, and the initial edge that was
+     choosen. *)
+  let rec search visited lastly_added =
+    if Vertex_set.is_empty lastly_added then
+      Vertex_map.find destination visited
+    else begin
+      let visited, newly_added =
+        Edge_set.fold
+          (fun (a, b) (visited, newly_added) ->
+             if Vertex_set.mem a lastly_added then
+               let vsi_a = Vertex_map.find a visited in
+               let dist = vsi_a.vsi_distance + distance a b in
+               try
+                 if (Vertex_map.find b visited).vsi_distance < dist then
+                   (visited, newly_added)
+                 else
+                   (Vertex_map.add b { vsi_a with vsi_distance = dist } visited, newly_added)
+               with Not_found ->
+                 (Vertex_map.add b { vsi_a with vsi_distance = dist } visited, Vertex_set.add b newly_added)
+             else
+               (visited, newly_added))
+          graph (visited, Vertex_set.empty)
+      in
+      search visited newly_added
+    end
+  in
+  let visited, newly_added =
+    Edge_set.fold
+      (fun (a, b) (visited, newly_added) ->
+         if a = position then
+           (Vertex_map.add b { vsi_distance = distance a b; vsi_initial = b } visited,
+            Vertex_set.add b newly_added)
+         else
+           (visited, newly_added))
+      graph
+      (Vertex_map.add position { vsi_initial = position; vsi_distance = 0 } Vertex_map.empty,
+       Vertex_set.empty)
+  in
+  (search visited newly_added).vsi_initial
diff --git a/PC_Mainboard/lib_krobot/krobot_move.mli b/PC_Mainboard/lib_krobot/krobot_move.mli
new file mode 100644
index 0000000..415ce1e
--- /dev/null
+++ b/PC_Mainboard/lib_krobot/krobot_move.mli
@@ -0,0 +1,27 @@
+(*
+ * krobot_move.mli
+ * ---------------
+ * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
+ * Licence   : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(** Make the robot to move along a graph *)
+
+type graph
+
+type vertex = {
+  vx : int;
+  vy : int;
+}
+
+val empty : graph
+val add_edge : vertex * vertex -> graph -> graph
+
+val move : graph : graph -> position : vertex -> destination : vertex -> vertex
+  (** [move ~graph ~position ~destination] returns the next step to
+      reach [destination] *)
+
+val distance : vertex -> vertex -> int
+val angle : vertex -> vertex -> int
diff --git a/PC_Mainboard/myocamlbuild.ml b/PC_Mainboard/myocamlbuild.ml
index 9d3c31b..a182489 100644
--- a/PC_Mainboard/myocamlbuild.ml
+++ b/PC_Mainboard/myocamlbuild.ml
@@ -81,6 +81,7 @@ let targets = List.filter_opt (function (true, target) -> Some target | (false,
   (have_lwt_unix && have_obus, "tests/double_move.best");
   (have_lwt_unix && have_obus, "tests/stop_and_restart.best");
   (have_lwt_unix && have_obus, "tests/trajectories.best");
+  (have_lwt_unix && have_obus, "tests/move.best");
 ]
 
 (* +-----------------------------------------------------------------+
diff --git a/PC_Mainboard/tests/move.ml b/PC_Mainboard/tests/move.ml
new file mode 100644
index 0000000..489acd9
--- /dev/null
+++ b/PC_Mainboard/tests/move.ml
@@ -0,0 +1,58 @@
+(*
+ * move.ml
+ * -------
+ * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
+ * Licence   : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+open Lwt
+open Printf
+open Krobot_move
+
+let graph = List.fold_left (fun graph edge -> add_edge edge graph) empty [
+  ({ vx = 0; vy = 0 }, { vx = 0; vy = 10 });
+  ({ vx = 0; vy = 10 }, { vx = 10; vy = 10 });
+  ({ vx = 10; vy = 10 }, { vx = 10; vy = 20 });
+]
+
+let init = { vx = 0; vy = 0 }
+let dest = { vx = 10; vy = 20 }
+
+let rec loop krobot position angle =
+  if position = dest then
+    return ()
+  else begin
+    let next = Krobot_move.move graph position dest in
+    printf "(%d, %d) -> (%d, %d)\n" position.vx position.vy next.vx next.vy;
+    let diff = Krobot_move.angle position next - angle in
+    printf "  current; %d, diff: %d\n" angle diff;
+    lwt () =
+      match krobot with
+        | Some krobot ->
+            lwt () =
+              if diff <> 0 then
+                lwt _ = Krobot.turn krobot ~angle:diff ~velocity:400 ~acceleration:800 in
+                return ()
+              else
+                return ()
+            in
+            lwt _ = Krobot.move krobot ~distance:(Krobot_move.distance position next) ~velocity:400 ~acceleration:800 in
+            return ()
+        | None ->
+            return ()
+    in
+    loop krobot next (angle + diff)
+  end
+
+lwt () =
+  lwt krobot =
+    match Array.to_list Sys.argv with
+      | _ :: "real" :: _ ->
+          lwt krobot = Krobot.create () in
+          return (Some krobot)
+      | _ ->
+          return None
+  in
+  loop krobot init 0


hooks/post-receive
-- 
krobot