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

[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  bafd157aea6b314a6972bcf90b29e33e946c2909 (commit)
      from  e81a37c02c8309be6b43cf092ba3c1d1d4dc5c11 (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 bafd157aea6b314a6972bcf90b29e33e946c2909
Author: Jérémie Dimino <je...@di...>
Date:   Mon Mar 28 23:34:00 2011 +0200

    [info] add messages for odometry + use odometry in the viewer

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

Changes:
diff --git a/info/control2011/src/lib/krobot_message.ml b/info/control2011/src/lib/krobot_message.ml
index 346a706..38b3d50 100644
--- a/info/control2011/src/lib/krobot_message.ml
+++ b/info/control2011/src/lib/krobot_message.ml
@@ -24,6 +24,7 @@ type t =
   | Motor_status of bool
   | Motor_move of float * float * float
   | Motor_turn of float * float * float
+  | Odometry of float * float * float
   | Req_motor_status
   | Unknown of frame
 
@@ -61,6 +62,10 @@ let to_string = function
       sprintf
         "Motor_turn(%f, %f, %f)"
         angle speed acc
+  | Odometry(x, y, theta) ->
+      sprintf
+        "Odometry(%f, %f, %f)"
+        x y theta
   | Req_motor_status ->
       "Req_motor_status"
   | Unknown frame ->
@@ -114,6 +119,17 @@ let encode = function
         ~remote:false
         ~format:F11bits
         ~data
+  | Odometry(x, y, theta) ->
+      let data = String.create 6 in
+      put_sint16 data 0 (truncate (x *. 1000.));
+      put_sint16 data 2 (truncate (y *. 1000.));
+      put_sint16 data 4 (truncate (theta /. pi *. 18000.));
+      frame
+        ~identifier:104
+        ~kind:Data
+        ~remote:false
+        ~format:F11bits
+        ~data
   | Motor_move(dist, speed, acc) ->
       let data = String.create 8 in
       put_sint32 data 0 (truncate (dist *. 1000.));
@@ -175,6 +191,11 @@ let decode frame =
              get_float32 frame.data 4)
       | 103 ->
           Motor_status(get_uint8 frame.data 0 <> 0)
+      | 104 ->
+          Odometry
+            (float (get_sint16 frame.data 0) /. 1000.,
+             float (get_sint16 frame.data 2) /. 1000.,
+             float (get_sint16 frame.data 4) *. pi /. 18000.)
       | 201 ->
           Motor_move
             (float (get_sint32 frame.data 0) /. 1000.,
diff --git a/info/control2011/src/lib/krobot_message.mli b/info/control2011/src/lib/krobot_message.mli
index a5c840d..e48cc4c 100644
--- a/info/control2011/src/lib/krobot_message.mli
+++ b/info/control2011/src/lib/krobot_message.mli
@@ -36,6 +36,9 @@ type t =
           - [speed] is in rad/s
           - [acceleration] is in rad/s^2
       *)
+  | Odometry of float * float * float
+      (** [Odometry(x, y, theta)] the position of the robot on the
+          table. *)
 
   | Req_motor_status
       (** Request the status of the motors. *)
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index 263e917..c5561c2 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -178,14 +178,7 @@ module Board = struct
     tolerance : GRange.scale;
     mutable state : state;
     mutable points : (float * float) list;
-  }
-
-  let create bus widget tolerance = {
-    bus;
-    widget;
-    tolerance;
-    state = { x = 0.2; y = 1.9; theta = 2. *. atan (-1.) };
-    points = [];
+    mutable event : unit event;
   }
 
   let world_height = 2.1
@@ -477,13 +470,10 @@ module Board = struct
             lwt () = Krobot_message.send board.bus (Unix.gettimeofday (), Motor_move(dist, 0.1, 0.2)) in
             lwt () = wait_done board in
 
-            (* Virtually move the robot. *)
-            board.state <- { x; y; theta = board.state.theta +. alpha };
-
             (* Remove the point. *)
             board.points <- rest;
 
-            (* Redraw everything. *)
+            (* Redraw everything without the last point. *)
             queue_draw board;
 
             loop ()
@@ -491,6 +481,30 @@ module Board = struct
             return ()
     in
     loop ()
+
+  let create bus widget tolerance =
+    let board ={
+      bus;
+      widget;
+      tolerance;
+      state = { x = 0.2; y = 1.9; theta = 2. *. atan (-1.) };
+      points = [];
+      event = E.never;
+    } in
+    (* Move the robot on the board when we receive odometry
+       informations. *)
+    board.event <- (
+      E.map
+        (fun (ts, frame) ->
+           match frame with
+             | Odometry(x, y, theta) ->
+                 board.state <- { x; y; theta };
+                 queue_draw board
+             | _ ->
+                 ())
+        (Krobot_message.recv bus)
+    );
+    board
 end
 
 (* +-----------------------------------------------------------------+


hooks/post-receive
-- 
krobot