Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. aa5da0a85fba2bb27d79da5be728f739743257f6
|
From: Jérémie D. <Ba...@us...> - 2011-05-20 19:45:20
|
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 aa5da0a85fba2bb27d79da5be728f739743257f6 (commit)
via c922608e44b2ea9e56a1dd6dce0b2efb778a0597 (commit)
from 9240ffa0043fb08e18cb6b9c308cc382fed0bc4a (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 aa5da0a85fba2bb27d79da5be728f739743257f6
Author: Jérémie Dimino <je...@di...>
Date: Fri May 20 21:44:06 2011 +0200
[info] display objects
commit c922608e44b2ea9e56a1dd6dce0b2efb778a0597
Author: Jérémie Dimino <je...@di...>
Date: Tue May 17 20:16:04 2011 +0200
[info] add messages for the coders 1 and 2
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/_oasis b/info/control2011/_oasis
index 81dd9ef..eea9438 100644
--- a/info/control2011/_oasis
+++ b/info/control2011/_oasis
@@ -155,6 +155,13 @@ Executable "krobot-planner"
MainIs: krobot_planner.ml
BuildDepends: krobot, lwt.syntax
+Executable "krobot-objects"
+ Path: src/tools
+ Install: true
+ CompiledObject: best
+ MainIs: krobot_objects.ml
+ BuildDepends: krobot, lwt.syntax
+
Executable "krobot-kill"
Path: src/tools
Install: true
diff --git a/info/control2011/_tags b/info/control2011/_tags
index 42135ad..8d7cff0 100644
--- a/info/control2011/_tags
+++ b/info/control2011/_tags
@@ -2,7 +2,7 @@
<src/interfaces/*.ml>: -syntax_camlp4o
# OASIS_START
-# DO NOT EDIT (digest: e0fdca06da598f490f540bfd1de2f7ad)
+# DO NOT EDIT (digest: 727ec33b4060953b9ebbde97c6c14363)
# Library krobot
"src/lib": include
"src/lib/krobot.cmxs": use_krobot
@@ -74,6 +74,11 @@
<examples/send_can.{native,byte}>: pkg_lwt.unix
<examples/send_can.{native,byte}>: pkg_lwt.syntax
<examples/send_can.{native,byte}>: pkg_lwt.react
+# Executable krobot-objects
+<src/tools/krobot_objects.{native,byte}>: use_krobot
+<src/tools/krobot_objects.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_objects.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_objects.{native,byte}>: pkg_lwt.react
# Executable krobot-kill
<src/tools/krobot_kill.{native,byte}>: use_krobot
<src/tools/krobot_kill.{native,byte}>: pkg_lwt.unix
diff --git a/info/control2011/setup.ml b/info/control2011/setup.ml
index 9065940..c202ad2 100644
--- a/info/control2011/setup.ml
+++ b/info/control2011/setup.ml
@@ -1,7 +1,7 @@
(* setup.ml generated for the first time by OASIS v0.2.0~alpha1 *)
(* OASIS_START *)
-(* DO NOT EDIT (digest: f74f01087e7cdeda8c8d0d200fd9f844) *)
+(* DO NOT EDIT (digest: 79727b7548ae5f100d31519956433b0b) *)
(*
Regenerated by OASIS v0.2.0
Visit http://oasis.forge.ocamlcore.org for more information and
@@ -5404,6 +5404,34 @@ let setup_t =
{exec_custom = false; exec_main_is = "send_can.ml"; });
Executable
({
+ cs_name = "krobot-objects";
+ cs_data = PropList.Data.create ();
+ cs_plugin_data = [];
+ },
+ {
+ bs_build = [(OASISExpr.EBool true, true)];
+ bs_install = [(OASISExpr.EBool true, true)];
+ bs_path = "src/tools";
+ bs_compiled_object = Best;
+ bs_build_depends =
+ [
+ InternalLibrary "krobot";
+ FindlibPackage ("lwt.syntax", None)
+ ];
+ bs_build_tools = [ExternalTool "ocamlbuild"];
+ bs_c_sources = [];
+ bs_data_files = [];
+ bs_ccopt = [(OASISExpr.EBool true, [])];
+ bs_cclib = [(OASISExpr.EBool true, [])];
+ bs_dlllib = [(OASISExpr.EBool true, [])];
+ bs_dllpath = [(OASISExpr.EBool true, [])];
+ bs_byteopt = [(OASISExpr.EBool true, [])];
+ bs_nativeopt = [(OASISExpr.EBool true, [])];
+ },
+ {exec_custom = false; exec_main_is = "krobot_objects.ml";
+ });
+ Executable
+ ({
cs_name = "krobot-kill";
cs_data = PropList.Data.create ();
cs_plugin_data = [];
diff --git a/info/control2011/src/lib/krobot_bus.ml b/info/control2011/src/lib/krobot_bus.ml
index cf22f9d..101151b 100644
--- a/info/control2011/src/lib/krobot_bus.ml
+++ b/info/control2011/src/lib/krobot_bus.ml
@@ -32,6 +32,7 @@ type message =
| Trajectory_go of float * float * float * float
| Trajectory_stop
| Trajectory_moving of bool
+ | Objects of vertice list
type t = {
oc : Lwt_io.output_channel;
@@ -101,6 +102,10 @@ let string_of_message = function
sprintf
"Trajectory_moving %B"
b
+ | Objects objects ->
+ sprintf
+ "Objects [%s]"
+ (String.concat "; " (List.map string_of_vertice objects))
(* +-----------------------------------------------------------------+
| Sending/receiving messages |
diff --git a/info/control2011/src/lib/krobot_bus.mli b/info/control2011/src/lib/krobot_bus.mli
index c453e26..28a37a2 100644
--- a/info/control2011/src/lib/krobot_bus.mli
+++ b/info/control2011/src/lib/krobot_bus.mli
@@ -51,6 +51,11 @@ type message =
| Trajectory_moving of bool
(** Is the robot following a trajectory ? *)
+ (** Objects *)
+
+ | Objects of vertice list
+ (** The list of objects on the board. *)
+
val string_of_message : message -> string
(** Returns a string representation of the given message. *)
diff --git a/info/control2011/src/lib/krobot_message.ml b/info/control2011/src/lib/krobot_message.ml
index 3a77845..d2ee967 100644
--- a/info/control2011/src/lib/krobot_message.ml
+++ b/info/control2011/src/lib/krobot_message.ml
@@ -23,6 +23,7 @@ type t =
| Battery2_voltages of float * float * float * float
| Beacon_position of float * float * float
| Beacon_lowlevel_position of float * float * int
+ | Encoder_position_direction_1_2 of int * direction * int * direction
| Encoder_position_direction_3_4 of int * direction * int * direction
| Encoder_position_speed_3 of float * float
| Encoder_position_speed_4 of float * float
@@ -73,6 +74,11 @@ let to_string = function
angle
width
period
+ | Encoder_position_direction_1_2(pos1, dir1, pos2, dir2) ->
+ sprintf
+ "Encoder_position_direction_1_2(%d, %s, %d, %s)"
+ pos1 (string_of_direction dir1)
+ pos2 (string_of_direction dir2)
| Encoder_position_direction_3_4(pos3, dir3, pos4, dir4) ->
sprintf
"Encoder_position_direction_3_4(%d, %s, %d, %s)"
@@ -136,6 +142,18 @@ let pi = 4. *. atan 1.
external encode_bezier : int * int * int * int * int * int -> string = "krobot_message_encode_bezier"
let encode = function
+ | Encoder_position_direction_1_2(pos1, dir1, pos2, dir2) ->
+ let data = String.create 6 in
+ put_uint16 data 0 pos1;
+ put_uint16 data 2 pos2;
+ put_uint8 data 4 (match dir1 with Forward -> 0 | Backward -> 1);
+ put_uint8 data 5 (match dir2 with Forward -> 0 | Backward -> 1);
+ frame
+ ~identifier:99
+ ~kind:Data
+ ~remote:false
+ ~format:F29bits
+ ~data
| Encoder_position_direction_3_4(pos3, dir3, pos4, dir4) ->
let data = String.create 6 in
put_uint16 data 0 pos3;
@@ -351,6 +369,12 @@ let decode frame =
Unknown frame
else
match frame.identifier with
+ | 99 ->
+ Encoder_position_direction_1_2
+ (get_uint16 frame.data 0,
+ (if get_uint8 frame.data 4 = 0 then Forward else Backward),
+ get_uint16 frame.data 2,
+ (if get_uint8 frame.data 5 = 0 then Forward else Backward))
| 100 ->
Encoder_position_direction_3_4
(get_uint16 frame.data 0,
diff --git a/info/control2011/src/lib/krobot_message.mli b/info/control2011/src/lib/krobot_message.mli
index 05f9e49..90ff8f7 100644
--- a/info/control2011/src/lib/krobot_message.mli
+++ b/info/control2011/src/lib/krobot_message.mli
@@ -22,6 +22,8 @@ type t =
(** The position of the beacon relative to the robot *)
| Beacon_lowlevel_position of float * float * int
(** The position of the beacon as internally stored (for calibration) *)
+ | Encoder_position_direction_1_2 of int * direction * int * direction
+ (** The position and direction of encoders 1 and 2. *)
| Encoder_position_direction_3_4 of int * direction * int * direction
(** The position and direction of encoders 3 and 4. *)
| Encoder_position_speed_3 of float * float
diff --git a/info/control2011/src/tools/krobot_objects.ml b/info/control2011/src/tools/krobot_objects.ml
new file mode 100644
index 0000000..0c2d681
--- /dev/null
+++ b/info/control2011/src/tools/krobot_objects.ml
@@ -0,0 +1,115 @@
+(*
+ * krobot_objects.ml
+ * -----------------
+ * Copyright : (c) 2011, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(* Service handling the position of the objects on the board. *)
+
+open Lwt
+open Lwt_react
+open Krobot_bus
+open Krobot_geom
+
+let section = Lwt_log.Section.make "krobot(objects)"
+
+(* +-----------------------------------------------------------------+
+ | Types |
+ +-----------------------------------------------------------------+ *)
+
+type objects = {
+ bus : Krobot_bus.t;
+ (* The message bus used to communicate with the robot. *)
+
+ mutable objects : vertice list;
+ (* The list of objects on the board. *)
+}
+
+(* +-----------------------------------------------------------------+
+ | Message handling |
+ +-----------------------------------------------------------------+ *)
+
+let handle_message objects (timestamp, message) =
+ match message with
+ | Kill "objects" ->
+ exit 0
+
+ | Send ->
+ ignore (
+ let ts = Unix.gettimeofday () in
+ Krobot_bus.send objects.bus (ts, Objects objects.objects)
+ )
+
+ | _ ->
+ ()
+
+(* +-----------------------------------------------------------------+
+ | Command-line arguments |
+ +-----------------------------------------------------------------+ *)
+
+let fork = ref true
+
+let options = Arg.align [
+ "-no-fork", Arg.Clear fork, " Run in foreground";
+]
+
+let usage = "\
+Usage: krobot-planner [options]
+options are:"
+
+(* +-----------------------------------------------------------------+
+ | Entry point |
+ +-----------------------------------------------------------------+ *)
+
+lwt () =
+ Arg.parse options ignore usage;
+
+ (* Display all informative messages. *)
+ Lwt_log.append_rule "*" Lwt_log.Info;
+
+ (* Open the krobot bus. *)
+ lwt bus = Krobot_bus.get () in
+
+ (* Fork if not prevented. *)
+ if !fork then Lwt_daemon.daemonize ();
+
+ (* Kill any running planner. *)
+ lwt () = Krobot_bus.send bus (Unix.gettimeofday (), Krobot_bus.Kill "objects") in
+
+ (* Create a new planner. *)
+ let objects = {
+ bus;
+ objects = [
+ { x = 0.800; y = 0.350 };
+ { x = 1.150; y = 0.350 };
+ { x = 1.850; y = 0.350 };
+ { x = 2.200; y = 0.350 };
+ { x = 1.500; y = 1.050 };
+ { x = 0.800; y = 1.400 };
+ { x = 2.200; y = 1.400 };
+ { x = 1.150; y = 1.750 };
+ { x = 1.850; y = 1.750 };
+ { x = 0.200; y = 0.290 };
+ { x = 0.200; y = 0.570 };
+ { x = 0.200; y = 0.850 };
+ { x = 0.200; y = 1.130 };
+ { x = 0.200; y = 1.410 };
+ { x = 2.800; y = 0.290 };
+ { x = 2.800; y = 0.570 };
+ { x = 2.800; y = 0.850 };
+ { x = 2.800; y = 1.130 };
+ { x = 2.800; y = 1.410 };
+ ];
+ } in
+
+ (* Handle krobot message. *)
+ E.keep (E.map (handle_message objects) (Krobot_bus.recv bus));
+
+ (* Sends initial objects. *)
+ lwt () = Krobot_bus.send objects.bus (Unix.gettimeofday (), Objects objects.objects) in
+
+ (* Wait forever. *)
+ fst (wait ())
diff --git a/info/control2011/src/tools/krobot_planner.ml b/info/control2011/src/tools/krobot_planner.ml
index e1e3596..b15a336 100644
--- a/info/control2011/src/tools/krobot_planner.ml
+++ b/info/control2011/src/tools/krobot_planner.ml
@@ -18,18 +18,6 @@ open Krobot_message
let section = Lwt_log.Section.make "krobot(planner)"
-let pi = 4. *. atan 1.
-
-let math_mod_float a b =
- let b2 = b /. 2. in
- let modf = mod_float a b in
- if modf > b2 then
- modf -. b
- else if modf < -. b2 then
- modf +. b
- else
- modf;;
-
(* +-----------------------------------------------------------------+
| Types |
+-----------------------------------------------------------------+ *)
@@ -63,6 +51,9 @@ type planner = {
mutable orientation : float;
(* The orientation of the robot. *)
+ mutable objects : vertice list;
+ (* The list of objects on the board. *)
+
mutable event : unit event;
(* Event kept in the planner. *)
}
@@ -169,44 +160,6 @@ let go planner rotation_speed rotation_acceleration moving_speed moving_accelera
if planner.moving then
return ()
else begin
- (* let rec loop () =
- match S.value planner.vertices with
- | { x; y } :: rest ->
- let sqr x = x *. x in
- let radius = sqrt (sqr (max wheels_position (robot_size -. wheels_position)) +. sqr (robot_size /. 2.)) in
- if x >= radius && x <= world_width -. radius && y >= radius && y <= world_height -. radius then begin
- (* Turn the robot. *)
- let alpha = math_mod_float (atan2 (y -. planner.position.y) (x -. planner.position.x) -. planner.orientation) (2. *. pi) in
- lwt () = Lwt_log.info_f "turning by %f radians" alpha in
- lwt () = Krobot_message.send planner.bus (Unix.gettimeofday (),
- Motor_turn(alpha,
- rotation_speed,
- rotation_acceleration)) in
- lwt () = wait_done planner in
-
- (* Move the robot. *)
- let dist = sqrt (sqr (x -. planner.position.x) +. sqr (y -. planner.position.y)) in
- lwt () = Lwt_log.info_f "moving by %f meters" dist in
- lwt () = Krobot_message.send planner.bus (Unix.gettimeofday (),
- Motor_move(dist,
- moving_speed,
- moving_acceleration)) in
- lwt () = wait_done planner in
-
- (* Remove the point. *)
- (match S.value planner.vertices with
- | _ :: l -> planner.set_vertices l
- | [] -> ());
- planner.set_origin (planner.position,
- { vx = cos planner.orientation;
- vy = sin planner.orientation });
-
- loop ()
- end else
- Lwt_log.warning_f "can not move to (%f, %f)" x y
- | [] ->
- return ()
- in*)
set_moving planner true;
planner.mover <- (
try_lwt
@@ -339,6 +292,9 @@ let handle_message planner (timestamp, message) =
set_vertices planner [];
ignore (Krobot_message.send planner.bus (Unix.gettimeofday (), Motor_stop(1.0,0.0)))
+ | Objects l ->
+ planner.objects <- l
+
| _ ->
()
@@ -386,11 +342,15 @@ lwt () =
mover = return ();
position = { x = 0.; y = 0. };
orientation = 0.;
+ objects = [];
event = E.never;
} in
(* Handle krobot message. *)
E.keep (E.map (handle_message planner) (Krobot_bus.recv bus));
+ (* Ask for objects. *)
+ lwt () = Krobot_bus.send bus (Unix.gettimeofday (), Send) in
+
(* Wait forever. *)
fst (wait ())
diff --git a/info/control2011/src/tools/krobot_plot.ml b/info/control2011/src/tools/krobot_plot.ml
index e5e4cab..1b84ecc 100644
--- a/info/control2011/src/tools/krobot_plot.ml
+++ b/info/control2011/src/tools/krobot_plot.ml
@@ -29,16 +29,22 @@ lwt () =
window#show ();
(* Create the graph. *)
- let graph = { points = Array.init 2 (fun _ -> Queue.create ()); max = 1. } in
+ let graph = { points = Array.init 4 (fun _ -> Queue.create ()); max = 1. } in
E.keep
(E.map
(fun (timestamp, msg) ->
match msg with
+ | Encoder_position_direction_1_2(pos1, dir1, pos2, dir2) ->
+ let pos1 = float pos1 and pos2 = float pos2 in
+ graph.max <- max graph.max (max pos1 pos2);
+ Queue.push (timestamp, pos1) graph.points.(0);
+ Queue.push (timestamp, pos2) graph.points.(1);
+ update_graph graph timestamp
| Encoder_position_direction_3_4(pos3, dir3, pos4, dir4) ->
let pos3 = float pos3 and pos4 = float pos4 in
graph.max <- max graph.max (max pos3 pos4);
- Queue.push (timestamp, pos3) graph.points.(0);
- Queue.push (timestamp, pos4) graph.points.(1);
+ Queue.push (timestamp, pos3) graph.points.(2);
+ Queue.push (timestamp, pos4) graph.points.(3);
update_graph graph timestamp
| _ ->
())
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index 7ddee83..98a36fc 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -58,6 +58,9 @@ type viewer = {
mutable motor_status : bool * bool * bool *bool;
(* Status of the four motor controller. *)
+
+ mutable objects : vertice list;
+ (* The objects on the table. *)
}
(* +-----------------------------------------------------------------+
@@ -233,6 +236,19 @@ let draw viewer =
Cairo.arc ctx 1.675 0.175 0.05 0. (2. *. pi);
Cairo.fill ctx;
+ (* Draw objects *)
+ List.iter
+ (fun { x; y } ->
+ set_color ctx Yellow;
+ Cairo.arc ctx x y 0.1 0. (2. *. pi);
+ Cairo.fill ctx;
+
+ set_color ctx Black;
+ Cairo.arc ctx x y 0.1 0. (2. *. pi);
+ Cairo.stroke ctx)
+ viewer.objects;
+
+ (* Draw the robot and the ghost *)
List.iter
(fun (state, alpha) ->
Cairo.save ctx;
@@ -412,6 +428,10 @@ let handle_message viewer (timestamp, message) =
viewer.ui#logs#buffer#insert (line ^ "\n");
viewer.ui#scrolled_logs#vadjustment#set_value viewer.ui#scrolled_logs#vadjustment#upper
+ | Objects l ->
+ viewer.objects <- l;
+ queue_draw viewer
+
| _ ->
()
@@ -472,6 +492,7 @@ lwt () =
curves = [];
statusbar_context = ui#statusbar#new_context "";
motor_status = (false, false, false, false);
+ objects = [];
} in
(* Handle messages. *)
hooks/post-receive
--
krobot
|
