Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 7a45eb86b5cf68d266cb2ab686ee2ed49dbe37cd
|
From: Jérémie D. <Ba...@us...> - 2010-05-10 20:41:49
|
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 7a45eb86b5cf68d266cb2ab686ee2ed49dbe37cd (commit)
via 28c49ef23e7f4b890db2d50a8b310079aef45f45 (commit)
via d3386845388dbe6d50bdcc78efec44ea187b7545 (commit)
via f8f492cc46bd30a9ce6b36091e55198966e9ba7d (commit)
via b0f1b93429eb0bf34f55e20b334cfdd291ebea5f (commit)
via d302f549052f3783340d82cb0fa6dd62e54de15e (commit)
via 3d014ebb3153ec28975bcd03eaa7be81522c0f40 (commit)
from cb59d80d8b07ca593e1a2faeee5944c984369b1a (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 7a45eb86b5cf68d266cb2ab686ee2ed49dbe37cd
Author: Jérémie Dimino <je...@di...>
Date: Mon May 10 22:39:32 2010 +0200
add command recording + replaying
commit 28c49ef23e7f4b890db2d50a8b310079aef45f45
Author: Jérémie Dimino <je...@di...>
Date: Mon May 10 21:40:20 2010 +0200
add clients/scan.ml
commit d3386845388dbe6d50bdcc78efec44ea187b7545
Author: Jérémie Dimino <je...@di...>
Date: Mon May 10 21:35:19 2010 +0200
add a program which scan the front of the robot
commit f8f492cc46bd30a9ce6b36091e55198966e9ba7d
Author: Jérémie Dimino <je...@di...>
Date: Mon May 10 20:59:14 2010 +0200
scans all the front of the robot
commit b0f1b93429eb0bf34f55e20b334cfdd291ebea5f
Author: Jérémie Dimino <je...@di...>
Date: Mon May 10 16:15:23 2010 +0200
fix center position
commit d302f549052f3783340d82cb0fa6dd62e54de15e
Author: Jérémie Dimino <je...@di...>
Date: Mon May 10 16:06:23 2010 +0200
fix paths
commit 3d014ebb3153ec28975bcd03eaa7be81522c0f40
Author: Jérémie Dimino <je...@di...>
Date: Mon May 10 15:28:16 2010 +0200
add a command to scan the front of the robot
-----------------------------------------------------------------------
Changes:
diff --git a/info/control/Makefile b/info/control/Makefile
index d0e971f..b13886a 100644
--- a/info/control/Makefile
+++ b/info/control/Makefile
@@ -44,6 +44,8 @@ install:
install -m 0755 _build/clients/check.best $(PREFIX)/bin/krobot-check
install -m 0755 _build/clients/check.best $(PREFIX)/bin/krobot-check
install -m 0755 _build/clients/record_infrared.best $(PREFIX)/bin/krobot-record-infrared
+ install -m 0755 _build/clients/scan.best $(PREFIX)/bin/krobot-scan
+ install -m 0755 _build/clients/replay.best $(PREFIX)/bin/krobot-replay
.PHONY: uninstall
uninstall:
diff --git a/info/control/_tags b/info/control/_tags
index 303b53b..608802d 100644
--- a/info/control/_tags
+++ b/info/control/_tags
@@ -31,6 +31,8 @@
<clients/script{.*,_lexer.*}>: pkg_text
<clients/ax12_control.*>: pkg_lwt.text
<clients/check.*>: pkg_lwt.text
+<clients/scan.*>: pkg_lwt.text
+<clients/replay.*>: pkg_lwt.text
# +------------------------------------------------------------------+
# | Services |
diff --git a/info/control/clients/controller.ml b/info/control/clients/controller.ml
index dc4cda0..0cf0146 100644
--- a/info/control/clients/controller.ml
+++ b/info/control/clients/controller.ml
@@ -171,7 +171,7 @@ let draw krobot =
Krobot.srv_range_finders = false;
Krobot.srv_logic_sensors = false;
Krobot.srv_motor = false;
- Krobot.srv_ax12 = false;
+ Krobot.srv_grip = false;
Krobot.srv_lcd = false;
Krobot.srv_infrared = false;
Krobot.srv_power = false;
diff --git a/info/control/clients/joy_control.ml b/info/control/clients/joy_control.ml
index 0bf7de7..260726b 100644
--- a/info/control/clients/joy_control.ml
+++ b/info/control/clients/joy_control.ml
@@ -136,11 +136,38 @@ let try_call action f =
lwt () = Lwt_log.error_f "action %s failed with: %s" action msg in
return ()
+let record =
+ if Array.length Sys.argv >= 2 && Sys.argv.(1) = "record" then begin
+ let oc = Lwt_io.open_file ~mode:Lwt_io.output "krobot.record" in
+ let prev = ref (Unix.gettimeofday ()) in
+ fun fmt ->
+ Printf.ksprintf
+ (fun cmd ->
+ let date = Unix.gettimeofday () in
+ let delta = date -. !prev in
+ prev := date;
+ lwt () =
+ if delta > 0. then
+ Lwt_io.fprintlf oc "sleep duration=%f" delta
+ else
+ return ()
+ in
+ Lwt_io.write_line oc cmd)
+ fmt
+ end else begin
+ fun fmt -> Printf.ksprintf (fun cmd -> return ()) fmt
+ end
+
let rec set_velocities krobot velocities =
lwt () = Lwt_log.info_f "set-velocities: left=%d right=%d" (fst velocities) (snd velocities) in
lwt () =
try_call "set-velocities"
(fun () ->
+ lwt () =
+ record
+ "set-velocities acceleration-left=%d acceleration-right=%d velocity-left=%d velocity-right=%d duration=%f"
+ (fst accelerations) (snd accelerations) (fst velocities) (snd velocities) duration
+ in
Krobot.Motors.set_velocities krobot ~velocities ~accelerations ~duration)
in
if velocities = (0, 0) then
diff --git a/info/control/clients/replay.ml b/info/control/clients/replay.ml
new file mode 100644
index 0000000..d73f219
--- /dev/null
+++ b/info/control/clients/replay.ml
@@ -0,0 +1,28 @@
+(*
+ * replay.ml
+ * ---------
+ * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+open Lwt
+
+lwt () =
+ if Array.length Sys.argv <> 2 then begin
+ Printf.printf "usage: %s <script>\n" Sys.argv.(0);
+ exit 2
+ end;
+
+ let ic = Lwt_io.open_file ~mode:Lwt_io.input Sys.argv.(1) in
+ lwt krobot = Krobot.create () in
+
+ let rec loop () =
+ Lwt_io.read_line_opt ic >>= function
+ | None ->
+ return ()
+ | Some line ->
+ Script.exec ~krobot ~logger:Lwt_term.printlc ~command:line
+ in
+ loop ()
diff --git a/info/control/clients/scan.ml b/info/control/clients/scan.ml
new file mode 100644
index 0000000..2f5d045
--- /dev/null
+++ b/info/control/clients/scan.ml
@@ -0,0 +1,31 @@
+(*
+ * scan.ml
+ * -------
+ * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(* Scan and display the front of the robot *)
+
+open Lwt
+open Lwt_io
+open Lwt_term
+
+lwt () =
+ lwt krobot = Krobot.create () in
+ lwt () = printl "scanning..." in
+ lwt values = Krobot.Infrared.scan krobot in
+ let columns = React.S.value Lwt_term.columns and lines = React.S.value Lwt_term.lines in
+ let size = min lines columns in
+ let screen = Zone.make ~width:columns ~height:lines in
+ Draw.set screen (size / 2) 0 { char = "*"; style = { blank.style with foreground = lblue } };
+ List.iter
+ (fun (angle, dist) ->
+ let dist = dist * size / 200 in
+ let y = int_of_float (cos (float_of_int angle) *. (float_of_int dist)) in
+ let x = size / 2 + int_of_float (sin (float_of_int angle) *. (float_of_int dist)) in
+ Draw.set ~zone:screen ~x ~y ~point:{ char = "+"; style = { blank.style with foreground = white } })
+ values;
+ render (Zone.points screen)
diff --git a/info/control/clients/script.ml b/info/control/clients/script.ml
index e244b80..ea09a74 100644
--- a/info/control/clients/script.ml
+++ b/info/control/clients/script.ml
@@ -550,7 +550,24 @@ let () =
Krobot.Infrared.go_right krobot);
register ~path:["infrared"] "center" f0
(fun logger krobot ->
- Krobot.Infrared.go_center krobot)
+ Krobot.Infrared.go_center krobot);
+ register ~path:["infrared"] "scan" f0
+ (fun logger krobot ->
+ lwt l = Krobot.Infrared.scan krobot in
+ logger [text "infrared scan: [";
+ text (String.concat "; " (List.map (fun (angle, dist) -> Printf.sprintf "(%d, %d)" angle dist) l));
+ text "]"]);
+ register ~path:["infrared"] "reload-config" f0
+ (fun logger krobot ->
+ Krobot.Infrared.reload_config krobot);
+
+ (* +---------------------------------------------------------------+
+ | Misc |
+ +---------------------------------------------------------------+ *)
+
+ register "sleep" (f1 (float "duration"))
+ (fun logger krobot duration ->
+ Lwt_unix.sleep duration)
(* +-----------------------------------------------------------------+
| Unsafe commands |
diff --git a/info/control/driver/driver.ml b/info/control/driver/driver.ml
index c1631f1..c844330 100644
--- a/info/control/driver/driver.ml
+++ b/info/control/driver/driver.ml
@@ -422,21 +422,121 @@ struct
lwt ar = USBCard.call Commands.Infrared.get_state card () in
return (ar.(0), ar.(1)))
+ (* +---------------------------------------------------------------+
+ | Simple movement |
+ +---------------------------------------------------------------+ *)
+
+ let pos_90_degree = 515
+ let pos_0_degree = 205
+
+ let pos_left = 750
+ let pos_right = 250
+
let go_left () =
set_ax12 [{ aa_id = 1;
- aa_position = 750;
+ aa_position = pos_left;
aa_velocity = 200 }]
let go_right () =
set_ax12 [{ aa_id = 1;
- aa_position = 250;
+ aa_position = pos_right;
aa_velocity = 200 }]
let go_center () =
set_ax12 [{ aa_id = 1;
- aa_position = 514;
+ aa_position = pos_90_degree;
aa_velocity = 200 }]
+ (* +---------------------------------------------------------------+
+ | Calibration data |
+ +---------------------------------------------------------------+ *)
+
+ let remove_junk values =
+ let m = List.fold_left (fun m (p, n) -> max m n) 0 values in
+ let rec loop = function
+ | [] ->
+ None
+ | (p, n) :: l ->
+ match loop l with
+ | Some _ as result ->
+ result
+ | None ->
+ if n = m then
+ Some l
+ else
+ None
+ in
+ match loop values with
+ | Some l ->
+ l
+ | None ->
+ values
+
+ let distance data measured_value =
+ let delta, pos =
+ List.fold_left
+ (fun (min_delta, pos_at_min) (pos, value) ->
+ let delta = abs (measured_value - value) in
+ if delta < min_delta then
+ (delta, pos)
+ else
+ (min_delta, pos_at_min))
+ (max_int, 0)
+ data
+ in
+ if delta > 100 then
+ -1
+ else
+ pos
+
+ let data1 = ref []
+ let data2 = ref []
+
+ let data_file = ref (Printf.sprintf "%s/var/infrareds-record" (Sys.getenv "HOME"))
+
+ let reload_config () =
+ let ic = Lwt_io.open_file ~mode:Lwt_io.input !data_file in
+ let rec read_values () =
+ Lwt_io.read_line_opt ic >>= function
+ | Some line ->
+ let pos, i1, i2 = Scanf.sscanf line "%d %d %d" (fun x y z -> (x, y, z)) in
+ lwt l1, l2 = read_values () in
+ return ((pos, i1) :: l1, (pos, i2) :: l2)
+ | None ->
+ return ([], [])
+ in
+ lwt d1, d2 = read_values () in
+ lwt () = Lwt_io.close ic in
+ data1 := remove_junk d1;
+ data2 := remove_junk d2;
+ return ()
+
+ (* +---------------------------------------------------------------+
+ | Scanning |
+ +---------------------------------------------------------------+ *)
+
+ let step = 10
+
+ 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
+ else begin
+ let (i1, i2) = React.S.value states 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)
+ end
+ in
+ loop pos_left []
+
let () =
OBus_object.add_interfaces obus
[make ~notify_mode
@@ -457,6 +557,17 @@ struct
lwt () = go_center () in
OBus_method.return ctx ()
);
+ m_Scan = (
+ fun ctx () () ->
+ let ctx = OBus_context.map (List.map (fun (x, y) -> (Int32.of_int x, Int32.of_int y))) ctx in
+ lwt result = scan () in
+ OBus_method.return ctx result
+ );
+ m_ReloadConfig = (
+ fun ctx () () ->
+ lwt () = reload_config () in
+ OBus_method.return ctx ()
+ );
}]
end
@@ -1184,10 +1295,14 @@ lwt () =
let args = [
"-n", Arg.Set foreground, "do not daemonize";
"-k", Arg.Set kill_and_exit, "kill any running driver and exit";
+ "-c", Arg.Set_string Infrared.data_file, "file containing calibration data for the infrared sensors";
] in
let usage_msg = Printf.sprintf "Usage: %s [-n]\n\noptions are:" (Filename.basename (Sys.argv.(0))) in
Arg.parse args ignore usage_msg;
+ lwt () = Lwt_log.notice_f "loading infrareds calibration data from %S" !Infrared.data_file in
+ lwt () = Infrared.reload_config () in
+
(* Open the krobot message bus *)
lwt bus = OBus_bus.of_addresses (OBus_address.of_string Config.bus_address) in
diff --git a/info/control/lib-krobot/krobot.ml b/info/control/lib-krobot/krobot.ml
index 9ab5915..9b85c17 100644
--- a/info/control/lib-krobot/krobot.ml
+++ b/info/control/lib-krobot/krobot.ml
@@ -43,7 +43,7 @@ type services_status = {
srv_range_finders : bool;
srv_logic_sensors : bool;
srv_motor : bool;
- srv_ax12 : bool;
+ srv_grip : bool;
srv_lcd : bool;
srv_infrared : bool;
srv_power : bool;
@@ -56,7 +56,7 @@ let services_status krobot =
srv_range_finders = List.assoc "RangeFinders" l;
srv_logic_sensors = List.assoc "LogicSensors" l;
srv_motor = List.assoc "Motors" l;
- srv_ax12 = List.assoc "Grip" l;
+ srv_grip = List.assoc "Grip" l;
srv_lcd = List.assoc "LCD" l;
srv_infrared = List.assoc "Infrared" l;
srv_power = List.assoc "Power" l;
@@ -87,6 +87,13 @@ struct
let go_center krobot =
OBus_method.call m_GoCenter (service krobot "Infrared") ()
+
+ let scan krobot =
+ 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 reload_config krobot =
+ OBus_method.call m_ReloadConfig (service krobot "Infrared") ()
end
(* +-----------------------------------------------------------------+
@@ -235,19 +242,19 @@ struct
open Krobot_interfaces.Fr_krobot_Service_Claws
let enable krobot =
- OBus_method.call m_Enable (service krobot "Servo") ()
+ OBus_method.call m_Enable (service krobot "Claws") ()
let disable krobot =
- OBus_method.call m_Disable (service krobot "Servo") ()
+ OBus_method.call m_Disable (service krobot "Claws") ()
let open_ krobot =
- OBus_method.call m_Open (service krobot "Servo") ()
+ OBus_method.call m_Open (service krobot "Claws") ()
let close krobot =
- OBus_method.call m_Close (service krobot "Servo") ()
+ OBus_method.call m_Close (service krobot "Claws") ()
let take krobot =
- OBus_method.call m_Take (service krobot "Servo") ()
+ OBus_method.call m_Take (service krobot "Claws") ()
end
(* +-----------------------------------------------------------------+
diff --git a/info/control/lib-krobot/krobot.mli b/info/control/lib-krobot/krobot.mli
index 160973e..3047a8a 100644
--- a/info/control/lib-krobot/krobot.mli
+++ b/info/control/lib-krobot/krobot.mli
@@ -31,7 +31,7 @@ type services_status = {
srv_range_finders : bool;
srv_logic_sensors : bool;
srv_motor : bool;
- srv_ax12 : bool;
+ srv_grip : bool;
srv_lcd : bool;
srv_infrared : bool;
srv_power : bool;
@@ -47,6 +47,13 @@ module Infrared : sig
val go_right : t -> unit Lwt.t
val go_left : t -> unit Lwt.t
val go_center : t -> unit Lwt.t
+
+ val scan : t -> (int * int) list Lwt.t
+ (** Scans the front of the robot and returns a list of [(angle,
+ distance)] *)
+
+ val reload_config : t -> unit Lwt.t
+ (** Reload calirbatino data *)
end
module Power : sig
diff --git a/info/control/myocamlbuild.ml b/info/control/myocamlbuild.ml
index 1478695..cb7518d 100644
--- a/info/control/myocamlbuild.ml
+++ b/info/control/myocamlbuild.ml
@@ -76,6 +76,8 @@ let targets = List.filter_opt (function (true, target) -> Some target | (false,
(have_lwt_unix && have_obus && have_lwt_text, "clients/ax12_control.best");
(have_lwt_unix && have_obus && have_lwt_text, "clients/check.best");
(have_lwt_unix && have_obus, "clients/record_infrared.best");
+ (have_lwt_unix && have_obus && have_lwt_text, "clients/scan.best");
+ (have_lwt_unix && have_obus && have_lwt_text, "clients/replay.best");
(* Services *)
(have_lwt_unix && have_obus, "services/hard_stop.best");
diff --git a/info/control/protocol/krobot.obus b/info/control/protocol/krobot.obus
index 4405cd9..68c1357 100644
--- a/info/control/protocol/krobot.obus
+++ b/info/control/protocol/krobot.obus
@@ -27,6 +27,9 @@ interface "fr.krobot.Service.Infrared" {
method GoLeft : () -> ()
method GoRight : () -> ()
method GoCenter : () -> ()
+
+ method Scan : () -> (values : (int32 * int32) structure array)
+ method ReloadConfig : () -> ()
}
interface "fr.krobot.Service.Gate" {
hooks/post-receive
--
krobot
|
