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

[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  14f3f3fb92621a0e816bad8022ff38ff5cf66e88 (commit)
       via  f983539a6786f1b5817a9736a4a202beea9af550 (commit)
       via  636151c8b2ab6aaa0f1fd75fde0f166e5f68b3be (commit)
       via  f1ad9c81359debc2656d5286760df5764bde5e8e (commit)
      from  f775e431796cbf2718c9c9128347b21f172ea159 (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 14f3f3fb92621a0e816bad8022ff38ff5cf66e88
Author: Jérémie Dimino <je...@di...>
Date:   Sat May 22 15:56:24 2010 +0200

    small fixes in the ia

commit f983539a6786f1b5817a9736a4a202beea9af550
Author: Jérémie Dimino <je...@di...>
Date:   Sat May 22 15:27:23 2010 +0200

    better scanning with the infrared

commit 636151c8b2ab6aaa0f1fd75fde0f166e5f68b3be
Author: Jérémie Dimino <je...@di...>
Date:   Sat May 22 15:26:20 2010 +0200

    more precise calibration

commit f1ad9c81359debc2656d5286760df5764bde5e8e
Author: Jérémie Dimino <je...@di...>
Date:   Sat May 22 13:24:14 2010 +0200

    add Infrared.Find

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

Changes:
diff --git a/info/control/clients/ia.ml b/info/control/clients/ia.ml
index 0da42ae..4941964 100644
--- a/info/control/clients/ia.ml
+++ b/info/control/clients/ia.ml
@@ -12,7 +12,7 @@ open Krobot_move
 
 let section = Lwt_log.Section.make "ia"
 
-let velocity = 100
+let velocity = 400
 let acceleration = 800
 
 (* +-----------------------------------------------------------------+
@@ -205,59 +205,21 @@ let take_ear krobot state ear =
           return { state with orientation = 180 }
   in
   lwt _ = Krobot.Motors.move krobot ~distance:100 ~velocity ~acceleration in
-  let pi = 4.0 *. atan 1.0 in
-  let sqr x = x *. x in
-  let scan () =
-    lwt pts = Krobot.Infrared.scan krobot in
-    let dist_min = List.fold_left (fun dist_min (angle, dist) ->
-                                     if dist >= 0 && dist <= 130 && dist < dist_min then
-                                       dist
-                                     else
-                                       dist_min) max_int pts in
-    let pts = List.filter (fun (a, d) -> d >= 0 && d <= 130 && d < dist_min + 10) pts in
-    lwt () =
-      Lwt_list.iter_s
-        (fun (angle, dist) ->
-           Lwt_io.printlf "angle = %d, distance = %d" angle dist)
-        pts
-    in
-    if dist_min = max_int then
-      lwt () = Lwt_io.printl "no ears found" in
-      exit 2
-    else begin
-      let mean = float_of_int (List.fold_left (fun acc (a, d) -> acc + a) 0 pts) /. float_of_int (List.length pts) in
-      lwt () = Lwt_io.printlf "ears found at angle = %f, distance = %d" mean dist_min in
-      let alpha = mean /. 180.0 *. pi and d = float_of_int dist_min +. 60.0 +. 25.0 in
-      let a = d *. cos alpha +. 270.0 and b = d *. sin alpha in
-      let b =
-        if b < -25.0 then
-          b -. 25.0
-        else if b <= 0.0 && b >= -25.0 then
-          25.0 -. b
-        else
-          b +. 25.0
-      in
-      let beta = atan(b /. a) in
-      let c = sqrt (sqr a +. sqr b) in
-      let e = c -. 270.0 -. 110.0 in
-      let beta = beta /. pi *. 180.0 in
-      lwt () = Lwt_io.printlf "a = %f, b = %f, c = %f" a b c in
-      lwt () = Lwt_io.printlf "turn: %f, move: %f" beta e in
-      return (beta, e)
-    end
-  in
-  lwt a, d = scan () in
-  lwt _ = Krobot.Motors.turn krobot ~angle:(-(truncate a)) ~velocity ~acceleration in
-  lwt a, d = scan () in
-  lwt _ = Krobot.Motors.turn krobot ~angle:(truncate a) ~velocity ~acceleration in
-  lwt _ = Krobot.Motors.move krobot ~distance:(truncate d) ~velocity ~acceleration in
-  lwt _ = Krobot.Motors.move krobot ~distance:(-100) ~velocity ~acceleration in
+  lwt a, d = Krobot.Infrared.find krobot in
+  let alpha = a in
+  lwt _ = Krobot.Motors.turn krobot ~angle:a ~velocity ~acceleration in
+  lwt a, d = Krobot.Infrared.find krobot in
+  let alpha = alpha + a in
+  lwt _ = Krobot.Motors.turn krobot ~angle:a ~velocity ~acceleration in
+  lwt _ = Krobot.Motors.move krobot ~distance:(d - 100) ~velocity ~acceleration in
   lwt () = Krobot.Grip.down krobot in
   lwt () = Krobot.Grip.open_ krobot in
-  lwt _ = Krobot.Motors.move krobot ~distance:80 ~velocity ~acceleration in
+  lwt _ = Krobot.Motors.move krobot ~distance:100 ~velocity ~acceleration in
   lwt () = Krobot.Grip.close krobot in
   lwt () = try_lwt Krobot.Grip.up krobot >> Krobot.Grip.release krobot with _ -> return ()
-  and _ = Krobot.Motors.move krobot ~distance:(-85) ~velocity ~acceleration in
+  and _ = Krobot.Motors.move krobot ~distance:(-(d + 100)) ~velocity ~acceleration in
+  lwt _ = Krobot.Motors.turn krobot ~angle:(-alpha) ~velocity ~acceleration in
+  ears := List.filter ((<>) ear) !ears;
   return state
 
 (* +-----------------------------------------------------------------+
@@ -291,4 +253,4 @@ lwt () =
     orientation = -90;
     position = if team = `Blue then a else r;
   } in
-  loop init 1
+  loop init 2
diff --git a/info/control/clients/record_infrared.ml b/info/control/clients/record_infrared.ml
index 0ae4da1..76ec6b0 100644
--- a/info/control/clients/record_infrared.ml
+++ b/info/control/clients/record_infrared.ml
@@ -12,7 +12,7 @@
 open Lwt
 open Lwt_io
 
-let step = 10
+let step = 1
 
 let measure_infrareds krobot infrareds =
   let rec loop acc1 acc2 = function
diff --git a/info/control/common/error.ml b/info/control/common/error.ml
index 5bcb78c..a37dd43 100644
--- a/info/control/common/error.ml
+++ b/info/control/common/error.ml
@@ -8,8 +8,10 @@
  *)
 
 exception Card_unavailable
+exception No_ear_found
 
 let () =
   List.iter (fun (name, exn) -> OBus_error.register name exn) [
-    "fr.krobot.CardUnavailable", Card_unavailable;
+    "fr.krobot.Error.CardUnavailable", Card_unavailable;
+    "fr.krobot.Error.NoEarFound", Card_unavailable;
   ]
diff --git a/info/control/common/error.mli b/info/control/common/error.mli
index b04e2b1..a741d30 100644
--- a/info/control/common/error.mli
+++ b/info/control/common/error.mli
@@ -11,3 +11,6 @@
 
 exception Card_unavailable
   (** The card is currently not available *)
+
+exception No_ear_found
+  (** The infrared scanner found no ear in front of the robot *)
diff --git a/info/control/driver/driver.ml b/info/control/driver/driver.ml
index 09fcb87..cb940ca 100644
--- a/info/control/driver/driver.ml
+++ b/info/control/driver/driver.ml
@@ -608,6 +608,33 @@ struct
     in
     loop []
 
+  let pi = 4.0 *. atan 1.0
+  let sqr x = x *. x
+
+  let find () =
+    lwt points = scan () in
+    let dist_min = List.fold_left (fun dist_min (angle, dist) ->
+                                     if dist >= 0 && dist <= 130 && dist < dist_min then
+                                       dist
+                                     else
+                                       dist_min) max_int points in
+    let points = List.filter (fun (a, d) -> d >= 0 && d <= 130 && d < dist_min + 10) points in
+    if dist_min = max_int then
+      OBus_error.fail Error.No_ear_found "no ear found in front of the robot"
+    else begin
+      let mean = float_of_int (List.fold_left (fun acc (a, d) -> acc + a) 0 points) /. float_of_int (List.length points) in
+      let alpha = mean /. 180.0 *. pi and c = float_of_int dist_min +. 60.0 +. 25.0 in
+      lwt () = Lwt_io.printlf "alpha = %f, c = %f" alpha c in
+      let x = -25.0 +. c *. sin alpha and y = 270.0 +. c *. cos alpha in
+      lwt () = Lwt_io.printlf "x = %f,  y = %f" x y in
+      let dist = sqrt (sqr x +. sqr y) in
+      let beta = asin (x /. dist) *. 180. /. 3.14159265 in
+      lwt () = Lwt_io.printlf "beta = %f, dist = %f" beta dist in
+      let beta = -(truncate beta) and dist = truncate dist - 25 - 270 - 110 in
+      lwt () = Lwt_io.printlf "beta = %d, dist = %d" beta dist in
+      return (beta, dist)
+    end
+
   let () =
     OBus_object.add_interfaces obus
       [make
@@ -634,6 +661,11 @@ struct
                lwt result = scan () in
                OBus_method.return ctx result
            );
+           m_Find = (
+             fun ctx () () ->
+               lwt angle, distance = find () in
+               OBus_method.return ctx (Int32.of_int angle, Int32.of_int distance)
+           );
            m_ReloadConfig = (
              fun ctx () () ->
                lwt () = reload_config () in
diff --git a/info/control/lib-krobot/krobot.ml b/info/control/lib-krobot/krobot.ml
index d0543c5..40f6c32 100644
--- a/info/control/lib-krobot/krobot.ml
+++ b/info/control/lib-krobot/krobot.ml
@@ -56,7 +56,6 @@ let services_status krobot =
        srv_power = List.assoc "Power" l;
      })
     (OBus_property.make
-      
        Krobot_interfaces.Fr_krobot_Manager.p_ServicesStatus
        (OBus_proxy.make krobot ["fr"; "krobot"; "Manager"]))
 
@@ -89,6 +88,10 @@ struct
     lwt l = OBus_method.call m_Scan (service krobot "Infrared") () in
     return (List.rev_map (fun (x, y) -> (Int32.to_int x, Int32.to_int y)) l)
 
+  let find krobot =
+    lwt a, d = OBus_method.call m_Find (service krobot "Infrared") () in
+    return (Int32.to_int a, Int32.to_int d)
+
   let reload_config krobot =
     OBus_method.call m_ReloadConfig (service krobot "Infrared") ()
 end
diff --git a/info/control/lib-krobot/krobot.mli b/info/control/lib-krobot/krobot.mli
index aa0b023..baa2e94 100644
--- a/info/control/lib-krobot/krobot.mli
+++ b/info/control/lib-krobot/krobot.mli
@@ -57,6 +57,11 @@ module Infrared : sig
     (** Scans the front of the robot and returns a list of [(angle,
         distance)] *)
 
+  val find : t -> (int * int) Lwt.t
+    (** Scans the front of the robot and returns the angle to turn and
+        the distance to move in order to take an ear in front of the
+        robot *)
+
   val reload_config : t -> unit Lwt.t
     (** Reload calirbatino data *)
 end
diff --git a/info/control/protocol/krobot.obus b/info/control/protocol/krobot.obus
index 2446015..bd82223 100644
--- a/info/control/protocol/krobot.obus
+++ b/info/control/protocol/krobot.obus
@@ -24,13 +24,25 @@ interface fr.krobot.Service.Infrared {
   method GoRight : () -> ()
   method GoCenter : () -> ()
 
-  method Scan : () -> (values : (int32 * int32) structure array)
+  method Scan : () -> (values : (int32, int32) dict)
+    (** Scan the front of the robot and returns the distances measured
+        for each angle. *)
+
+  method Find : () -> (angle : int32, distance : int32)
+    (** Scan the front the robot, and returns:
+
+        - the angle the robot must turn to take the ear
+        - the distance the robot must move to take the ear
+
+        It raises "fr.krobot.Error.NoEartFound" if no ear
+        is found at the front of the robot. *)
+
   method ReloadConfig : () -> ()
 }
 
-interface fr.krobot.Service.Gate {
-  method Enable : () -> ()
-    (** Enable the motor of the gate *)
+  interface fr.krobot.Service.Gate {
+    method Enable : () -> ()
+      (** Enable the motor of the gate *)
 
   method Disable : () -> ()
     (** Disable the motor of the gate *)
diff --git a/info/control/tests/take_ear_with_infrared.ml b/info/control/tests/take_ear_with_infrared.ml
index 6a48e1f..c048e83 100644
--- a/info/control/tests/take_ear_with_infrared.ml
+++ b/info/control/tests/take_ear_with_infrared.ml
@@ -10,6 +10,7 @@
 (* Scan the front of the robot to find an ear, then take it *)
 
 open Lwt
+open Krobot_move
 
 let pi = 4.0 *. atan 1.0
 let sqr x = x *. x
@@ -17,6 +18,13 @@ let sqr x = x *. x
 let pos_90_degree = 515
 let pos_0_degree = 205
 
+let pause () =
+  Lwt_term.read_key () >>= function
+    | Lwt_term.Key "q" ->
+        exit 0
+    | _ ->
+        return ()
+
 lwt () =
   lwt krobot = Krobot.create () in
   let prout () =
@@ -40,36 +48,37 @@ lwt () =
       let mean = float_of_int (List.fold_left (fun acc (a, d) -> acc + a) 0 pts) /. float_of_int (List.length pts) in
       lwt () = Lwt_io.printlf "ears found at angle = %f, distance = %d" mean dist_min in
       lwt () = Krobot.unsafe_call Commands.AX12.goto krobot (1, pos_90_degree - (truncate mean * (pos_90_degree - pos_0_degree) / 90), 0, `Now) in
-      let alpha = mean /. 180.0 *. pi and d = float_of_int dist_min +. 60.0 +. 25.0 in
-      let a = d *. cos alpha +. 270.0 and b = d *. sin alpha in
-      let b =
-        if b < -25.0 then
-          b -. 25.0
-        else if b <= 0.0 && b >= -25.0 then
-          25.0 -. b
-        else
-          b +. 25.0
-      in
-      let beta = atan(b /. a) in
-      let c = sqrt (sqr a +. sqr b) in
-      let e = c -. 270.0 -. 110.0 in
-      let beta = beta /. pi *. 180.0 in
-      lwt () = Lwt_io.printlf "a = %f, b = %f, c = %f" a b c in
-      lwt () = Lwt_io.printlf "turn: %f, move: %f" beta e in
-      return (beta, e)
+      let alpha = mean /. 180.0 *. pi and c = float_of_int dist_min +. 60.0 +. 25.0 in
+      lwt () = Lwt_io.printlf "alpha = %f, c = %f" alpha c in
+      let x = -25.0 +. c *. sin alpha and y = 270.0 +. c *. cos alpha in
+      lwt () = Lwt_io.printlf "x = %f,  y = %f" x y in
+      let dist = sqrt (sqr x +. sqr y) in
+      let beta = asin (x /. dist) *. 180. /. 3.14159265 in
+      lwt () = Lwt_io.printlf "beta = %f, dist = %f" beta dist in
+      let beta = -(truncate beta) and dist = truncate dist - 25 - 270 - 110 in
+      lwt () = Lwt_io.printlf "beta = %d, dist = %d" beta dist in
+      return (beta, dist)
     end
   in
   lwt a, d = prout () in
-  let velocity = 400 and acceleration = 800 in
-  lwt _ = Krobot.Motors.turn krobot ~angle:(-(truncate a)) ~velocity ~acceleration in
+  lwt () = pause () in
+  let velocity = 100 and acceleration = 800 in
+  lwt _ = Krobot.Motors.turn krobot ~angle:a ~velocity ~acceleration in
+  lwt () = pause () in
   lwt a, d = prout () in
-  lwt _ = Krobot.Motors.turn krobot ~angle:(truncate a) ~velocity ~acceleration in
-  lwt _ = Krobot.Motors.move krobot ~distance:(truncate d) ~velocity ~acceleration in
-  lwt _ = Krobot.Motors.move krobot ~distance:(-100) ~velocity ~acceleration in
+  lwt () = pause () in
+  lwt _ = Krobot.Motors.turn krobot ~angle:a ~velocity ~acceleration in
+  lwt () = pause () in
+  lwt _ = Krobot.Motors.move krobot ~distance:(d - 100) ~velocity ~acceleration in
+  lwt () = pause () in
   lwt () = Krobot.Grip.down krobot in
+  lwt () = pause () in
   lwt () = Krobot.Grip.open_ krobot in
-  lwt _ = Krobot.Motors.move krobot ~distance:80 ~velocity ~acceleration in
+  lwt () = pause () in
+  lwt _ = Krobot.Motors.move krobot ~distance:100 ~velocity ~acceleration in
+  lwt () = pause () in
   lwt () = Krobot.Grip.close krobot in
+  lwt () = pause () in
   lwt _ = Krobot.Grip.up krobot in
   return ()
 


hooks/post-receive
-- 
krobot