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

[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  54d1ed6481b23cc83fc0168205a60a50e3f1e3e3 (commit)
       via  388c7859e24d2a860b24b94f1fc2b072fd6b1c40 (commit)
       via  a47e6014bed09e50501f3e7e40678bf261d4837b (commit)
       via  27f74d0ec09a3eb55ddc145e8ea3443d4f4b4ef3 (commit)
      from  e4676289e7990e6c6a086cf1597d4faf40df304f (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 54d1ed6481b23cc83fc0168205a60a50e3f1e3e3
Author: Jérémie Dimino <je...@di...>
Date:   Fri May 21 22:29:40 2010 +0200

    use the scanner for taking ears in the ia

commit 388c7859e24d2a860b24b94f1fc2b072fd6b1c40
Author: Jérémie Dimino <je...@di...>
Date:   Fri May 21 22:28:09 2010 +0200

    more tests for the infrared scanner

commit a47e6014bed09e50501f3e7e40678bf261d4837b
Author: [Kro]bot <kr...@wa...>
Date:   Fri May 21 22:13:35 2010 +0200

    enhancement of the infrared scanner

commit 27f74d0ec09a3eb55ddc145e8ea3443d4f4b4ef3
Author: Jérémie Dimino <je...@di...>
Date:   Thu May 20 23:16:09 2010 +0200

    fix infrared number for the scan

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

Changes:
diff --git a/info/control/clients/ia.ml b/info/control/clients/ia.ml
index dee979e..0da42ae 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 = 400
+let velocity = 100
 let acceleration = 800
 
 (* +-----------------------------------------------------------------+
@@ -162,7 +162,9 @@ let rec goto krobot state dest =
     return state
   else
     let next = Krobot_move.next graph state.position dest in
+    lwt () = Lwt_log.info_f "next position: (%d, %d)" next.vx next.vy in
     let angle, distance, state' = Krobot_move.move state next in
+    lwt () = Lwt_log.info_f "angle: %d, distance: %d, new state: position: (%d, %d), oritentation: %d" angle distance state'.position.vx state'.position.vy state'.orientation in
     lwt () =
       if angle <> 0 then
         lwt _ = Krobot.Motors.turn krobot ~angle ~velocity ~acceleration in
@@ -202,10 +204,58 @@ let take_ear krobot state ear =
           in
           return { state with orientation = 180 }
   in
-  lwt () = try_lwt Krobot.Grip.down krobot with _ -> return () in
-  lwt () = try_lwt Krobot.Grip.open_ krobot with _ -> return () in
-  lwt _ = Krobot.Motors.move krobot ~distance:85 ~velocity ~acceleration in
-  lwt () = try_lwt Krobot.Grip.close krobot with _ -> return () 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 () = Krobot.Grip.down krobot in
+  lwt () = Krobot.Grip.open_ krobot in
+  lwt _ = Krobot.Motors.move krobot ~distance:80 ~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
   return state
diff --git a/info/control/driver/driver.ml b/info/control/driver/driver.ml
index 08a8f59..09fcb87 100644
--- a/info/control/driver/driver.ml
+++ b/info/control/driver/driver.ml
@@ -318,6 +318,9 @@ type ax12_action = {
   aa_velocity : int;
 }
 
+let get_ax12 id =
+  USBCard.call Commands.AX12.get_position (get_card card_interface) (id, 1000)
+
 let set_ax12 actions =
   lwt () =
     Lwt_list.iter_p
@@ -335,7 +338,7 @@ let set_ax12 actions =
     lwt delta =
       Lwt_list.fold_left_s
         (fun delta action ->
-           lwt pos = USBCard.call Commands.AX12.get_position (get_card card_interface) (action.aa_id, 32000) in
+           lwt pos = get_ax12 action.aa_id in
            return (max delta (abs (pos - action.aa_position))))
         0 actions
     in
@@ -498,17 +501,17 @@ struct
   let go_left () =
     set_ax12 [{ aa_id = 1;
                 aa_position = pos_left;
-                aa_velocity = 200 }]
+                aa_velocity = 0 }]
 
   let go_right () =
     set_ax12 [{ aa_id = 1;
                 aa_position = pos_right;
-                aa_velocity = 200 }]
+                aa_velocity = 0 }]
 
   let go_center () =
     set_ax12 [{ aa_id = 1;
                 aa_position = pos_90_degree;
-                aa_velocity = 200 }]
+                aa_velocity = 0 }]
 
   (* +---------------------------------------------------------------+
      | Calibration data                                              |
@@ -578,28 +581,32 @@ struct
      | Scanning                                                      |
      +---------------------------------------------------------------+ *)
 
-  let step = 10
+  let step = 5
 
   let scan () =
-    lwt () = go_left () in
-    lwt () = Lwt_unix.sleep 1.5 in
-    let rec loop pos acc =
-      if pos <= pos_right then
-	return acc
+    lwt pos = get_ax12 1 in
+    lwt t =
+      if pos >= pos_90_degree then
+        lwt () = go_left () in
+        return (set_ax12 [{ aa_id = 1; aa_position = pos_right; aa_velocity = 100 }])
+      else
+        lwt () = go_right () in
+        return (set_ax12 [{ aa_id = 1; aa_position = pos_left; aa_velocity = 100 }])
+    in
+    let rec loop acc =
+      if state t <> Sleep then
+        return acc
       else begin
+        lwt pos = get_ax12 1 in
+        lwt () = Lwt_unix.sleep 0.1 in
         let infrareds = React.S.value states in
-        let i1 = infrareds.(2) in
+        let i1 = infrareds.(0) in
         let dist1 = distance !data1 i1
         and angle = (pos_90_degree - pos) * 90 / (pos_90_degree - pos_0_degree) in
-        let pos = pos - step in
-        lwt () = set_ax12 [{ aa_id = 1;
-                             aa_position = pos;
-                             aa_velocity = 200 }] in
-        lwt () = Lwt_unix.sleep 0.05 in
-        loop pos ((angle, dist1) :: acc)
+        loop ((angle, dist1) :: acc)
       end
     in
-    loop pos_left []
+    loop []
 
   let () =
     OBus_object.add_interfaces obus
diff --git a/info/control/tests/scan_infrared.ml b/info/control/tests/scan_infrared.ml
index bade5cf..59de9db 100644
--- a/info/control/tests/scan_infrared.ml
+++ b/info/control/tests/scan_infrared.ml
@@ -10,7 +10,7 @@
 open Lwt
 
 let rec print krobot infrared =
-  lwt pos = Krobot.unsafe_call Commands.AX12.get_position krobot (1, 100) in
+  lwt pos = Krobot.unsafe_call Commands.AX12.get_position krobot (1, 1000) in
   let is = React.S.value infrared in
   lwt () = Lwt_io.printlf "position: %3d, %4d %4d %4d %4d" pos is.(0) is.(1) is.(2) is.(3) in
   lwt () = Lwt_unix.sleep 0.2 in
@@ -18,15 +18,25 @@ let rec print krobot infrared =
 
 let delay = 1.5
 
+let goto krobot pos =
+  let rec loop () =
+    lwt pos' = Krobot.unsafe_call Commands.AX12.get_position krobot (1, 1000) in
+    lwt () = Lwt_unix.sleep 0.1 in
+    if abs (pos - pos') < 10 then
+      return ()
+    else
+      loop ()
+  in
+  lwt () = Krobot.unsafe_call Commands.AX12.goto krobot (1, pos, 100, `Now) in
+  loop ()
+
 lwt () =
   lwt krobot = Krobot.create () in
   let rec loop1 () =
-    lwt () = Krobot.Infrared.go_left krobot in
-    lwt () = Lwt_unix.sleep delay in
+    lwt () = goto krobot 750 in
     loop2 ()
   and loop2 () =
-    lwt () = Krobot.Infrared.go_right krobot in
-    lwt () = Lwt_unix.sleep delay in
+    lwt () = goto krobot 250 in
     loop1 ()
   in
   lwt infrared = OBus_property.monitor (Krobot.Infrared.states krobot) in
diff --git a/info/control/tests/take_ear_with_infrared.ml b/info/control/tests/take_ear_with_infrared.ml
new file mode 100644
index 0000000..6a48e1f
--- /dev/null
+++ b/info/control/tests/take_ear_with_infrared.ml
@@ -0,0 +1,75 @@
+(*
+ * take_ear_with_infrared.ml
+ * -------------------------
+ * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
+ * Licence   : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(* Scan the front of the robot to find an ear, then take it *)
+
+open Lwt
+
+let pi = 4.0 *. atan 1.0
+let sqr x = x *. x
+
+let pos_90_degree = 515
+let pos_0_degree = 205
+
+lwt () =
+  lwt krobot = Krobot.create () in
+  let prout () =
+    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
+      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)
+    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 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 () = Krobot.Grip.down krobot in
+  lwt () = Krobot.Grip.open_ krobot in
+  lwt _ = Krobot.Motors.move krobot ~distance:80 ~velocity ~acceleration in
+  lwt () = Krobot.Grip.close krobot in
+  lwt _ = Krobot.Grip.up krobot in
+  return ()
+


hooks/post-receive
-- 
krobot