Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. df19a6189b6e7dcc5f1f0b9aad90d51201605e4a
|
From: Jérémie D. <Ba...@us...> - 2011-04-04 22:23:50
|
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 df19a6189b6e7dcc5f1f0b9aad90d51201605e4a (commit)
via 4f76274d2c7fc17b0265d59f47e7bd7c16b1e6d9 (commit)
via 2fc3f6a82ab36af5da8237093ded26ca547afa0e (commit)
via 2954ce9c54bcec2b8712e9a5a8842e8d19125809 (commit)
from dd7c5daac4fc87ee81874bfca82f20efde0a8f92 (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 df19a6189b6e7dcc5f1f0b9aad90d51201605e4a
Author: Jérémie Dimino <je...@di...>
Date: Tue Apr 5 00:13:52 2011 +0200
[info] add a plotter for batteries
commit 4f76274d2c7fc17b0265d59f47e7bd7c16b1e6d9
Author: Jérémie Dimino <je...@di...>
Date: Tue Apr 5 00:00:42 2011 +0200
[krobot_viewer] add bezier curve generation
It is currently commented since it does not work.
commit 2fc3f6a82ab36af5da8237093ded26ca547afa0e
Author: Jérémie Dimino <je...@di...>
Date: Mon Apr 4 16:35:36 2011 +0200
[krobot_viewer] disable antialias for drawing the board
commit 2954ce9c54bcec2b8712e9a5a8842e8d19125809
Author: Jérémie Dimino <je...@di...>
Date: Mon Apr 4 16:11:49 2011 +0200
[krobot_viewer] draw a black circle around the beacon
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/_oasis b/info/control2011/_oasis
index e74d55b..8eb1060 100644
--- a/info/control2011/_oasis
+++ b/info/control2011/_oasis
@@ -130,6 +130,14 @@ Executable "krobot-plot"
MainIs: krobot_plot.ml
BuildDepends: krobot, lwt.syntax, cairo.lablgtk2, lwt.glib, threads
+Executable "krobot-plot-battery"
+ Path: src/tools
+ Build$: flag(gtk)
+ Install$: flag(gtk)
+ CompiledObject: best
+ MainIs: krobot_plot_battery.ml
+ BuildDepends: krobot, lwt.syntax, cairo.lablgtk2, lwt.glib, threads
+
Executable "krobot-viewer"
Path: src/tools
Build$: flag(gtk)
diff --git a/info/control2011/_tags b/info/control2011/_tags
index 2f57243..7c5ae73 100644
--- a/info/control2011/_tags
+++ b/info/control2011/_tags
@@ -2,7 +2,7 @@
<src/interfaces/*.ml>: -syntax_camlp4o
# OASIS_START
-# DO NOT EDIT (digest: 9d40de9cfde679815e27aa44dbe6939f)
+# DO NOT EDIT (digest: 5cd0fff2e5e7303cfc985c8215a456e0)
# Library krobot-interfaces
"src/interfaces": include
"src/interfaces/krobot-interfaces.cmxs": use_krobot-interfaces
@@ -70,6 +70,15 @@
<examples/send_can.{native,byte}>: pkg_obus
<examples/send_can.{native,byte}>: pkg_lwt.unix
<examples/send_can.{native,byte}>: pkg_lwt.syntax
+# Executable krobot-plot-battery
+<src/tools/krobot_plot_battery.{native,byte}>: use_krobot
+<src/tools/krobot_plot_battery.{native,byte}>: use_krobot-interfaces
+<src/tools/krobot_plot_battery.{native,byte}>: pkg_threads
+<src/tools/krobot_plot_battery.{native,byte}>: pkg_obus
+<src/tools/krobot_plot_battery.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_plot_battery.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_plot_battery.{native,byte}>: pkg_lwt.glib
+<src/tools/krobot_plot_battery.{native,byte}>: pkg_cairo.lablgtk2
# Executable krobot-remote
<src/tools/krobot_remote.{native,byte}>: use_krobot
<src/tools/krobot_remote.{native,byte}>: use_krobot-interfaces
diff --git a/info/control2011/setup.ml b/info/control2011/setup.ml
index d3ed546..05b8ab2 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: 698f29e4a2f614788e3b1ec0344c74ff) *)
+(* DO NOT EDIT (digest: ab615aa1d52e40116d1afa35b031733b) *)
(*
Regenerated by OASIS v0.2.0
Visit http://oasis.forge.ocamlcore.org for more information and
@@ -5360,6 +5360,47 @@ let setup_t =
});
Executable
({
+ cs_name = "krobot-plot-battery";
+ cs_data = PropList.Data.create ();
+ cs_plugin_data = [];
+ },
+ {
+ bs_build =
+ [
+ (OASISExpr.EBool true, false);
+ (OASISExpr.EFlag "gtk", true)
+ ];
+ bs_install =
+ [
+ (OASISExpr.EBool true, false);
+ (OASISExpr.EFlag "gtk", true)
+ ];
+ bs_path = "src/tools";
+ bs_compiled_object = Best;
+ bs_build_depends =
+ [
+ InternalLibrary "krobot";
+ FindlibPackage ("lwt.syntax", None);
+ FindlibPackage ("cairo.lablgtk2", None);
+ FindlibPackage ("lwt.glib", None);
+ FindlibPackage ("threads", 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_plot_battery.ml";
+ });
+ Executable
+ ({
cs_name = "krobot-remote";
cs_data = PropList.Data.create ();
cs_plugin_data = [];
diff --git a/info/control2011/src/lib/krobot_geom.ml b/info/control2011/src/lib/krobot_geom.ml
index c5a6941..7f04395 100644
--- a/info/control2011/src/lib/krobot_geom.ml
+++ b/info/control2011/src/lib/krobot_geom.ml
@@ -71,6 +71,13 @@ let vector a b = {
let distance a b =
sqrt (sqr (a.x -. b.x) +. sqr (a.y -. b.y))
+let tangent a b c =
+ let a = vector origin a
+ and b = vector origin b
+ and c = vector origin c in
+ let v = (b -| a) +| (b -| c) in
+ v /| norm v
+
(* +-----------------------------------------------------------------+
| Cubic bezier curves |
+-----------------------------------------------------------------+ *)
@@ -108,8 +115,8 @@ module Bezier = struct
let h2 = 2. *. (h0 *. vs.vx -. g0 *. vs.vy) in
(* The loop for finding d1 and d2. *)
let rec loop d1 d2 =
- let rho_p = 3. *. d1 *. d1 /. (h1 +. d2 *. g1)
- and rho_s = 3. *. d2 *. d2 /. (h2 +. d1 *. g2) in
+ let rho_p = 3. *. sqr d1 /. (h1 +. d2 *. g1)
+ and rho_s = 3. *. sqr d2 /. (h2 +. d1 *. g2) in
let err_1 = r_p -. rho_p and err_2 = r_s -. rho_s in
let error = max (abs_float err_1) (abs_float err_2) in
if error < error_max then
@@ -117,7 +124,7 @@ module Bezier = struct
and r = translate s (vs *| d2) in
of_vertices p q r s
else
- loop (d1 +. err_1 /. r_p) (d2 +. err_2 +. r_s)
+ loop (d1 +. err_1 /. r_p) (d2 +. err_2 /. r_s)
in
loop 1. 1.
diff --git a/info/control2011/src/lib/krobot_geom.mli b/info/control2011/src/lib/krobot_geom.mli
index 748b85e..b1a9936 100644
--- a/info/control2011/src/lib/krobot_geom.mli
+++ b/info/control2011/src/lib/krobot_geom.mli
@@ -35,6 +35,9 @@ val vector : vertice -> vertice -> vector
val norm : vector -> float
val distance : vertice -> vertice -> float
+val tangent : vertice -> vertice -> vertice -> vector
+ (** [tangent a b c] returns the tangent to the triangle abc in b. *)
+
(** {6 Cubic Bezier curves} *)
module Bezier : sig
diff --git a/info/control2011/src/lib/krobot_message.mli b/info/control2011/src/lib/krobot_message.mli
index 059b16a..9324094 100644
--- a/info/control2011/src/lib/krobot_message.mli
+++ b/info/control2011/src/lib/krobot_message.mli
@@ -17,7 +17,7 @@ type t =
| Battery1_voltages of float * float * float * float
(** The voltages of the elements of the first battery *)
| Battery2_voltages of float * float * float * float
- (** The voltages of the elements of the first battery *)
+ (** The voltages of the elements of the second battery *)
| Beacon_position of float * float * float
(** The position of the beacon relative to the robot *)
| Beacon_lowlevel_position of float * float * int
diff --git a/info/control2011/src/tools/krobot_graph.ml b/info/control2011/src/tools/krobot_graph.ml
new file mode 100644
index 0000000..327ae72
--- /dev/null
+++ b/info/control2011/src/tools/krobot_graph.ml
@@ -0,0 +1,80 @@
+(*
+ * krobot_graph.ml
+ * ---------------
+ * Copyright : (c) 2011, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(* +-----------------------------------------------------------------+
+ | Graphs |
+ +-----------------------------------------------------------------+ *)
+
+let graph_duration = 10.0
+ (* Which amount of data to keep in graphs. *)
+
+(* Type of graphs. *)
+type graph = {
+ points : (float * int) Queue.t array;
+ (* Queue of points with their time. They are ordered by increasing
+ date. *)
+ mutable max : int;
+ (* The maximum reached. *)
+}
+
+(* Remove old points. *)
+let update_graph graph time =
+ Array.iter
+ (fun q ->
+ while not (Queue.is_empty q) && fst (Queue.top q) +. graph_duration < time do
+ ignore (Queue.take q)
+ done)
+ graph.points
+
+(* +-----------------------------------------------------------------+
+ | Plotting |
+ +-----------------------------------------------------------------+ *)
+
+let rec colors = (1., 0., 0.) :: (0., 1., 0.) :: (0., 0., 1.) :: (1., 1., 0.) :: colors
+
+let plot ctx width height graph time =
+ Cairo.set_source_rgb ctx 1. 1. 1.;
+ Cairo.rectangle ctx 0. 0. width height;
+ Cairo.fill ctx;
+ let colors = ref colors in
+ Array.iter
+ (fun q ->
+ let r, g, b = List.hd !colors in
+ colors := List.tl !colors;
+ Cairo.set_source_rgb ctx r g b;
+ let prev = ref None in
+ Queue.iter
+ (fun (date, position) ->
+ let x = (date -. (time -. graph_duration)) /. graph_duration *. width
+ and y = height -. height *. (float position /. float graph.max) in
+ match !prev with
+ | None ->
+ prev := Some(x, y)
+ | Some(x', y') ->
+ prev := Some(x, y);
+ Cairo.move_to ctx x' y';
+ Cairo.line_to ctx x y;
+ Cairo.stroke ctx)
+ q)
+ graph.points
+
+(* +-----------------------------------------------------------------+
+ | Drawing |
+ +-----------------------------------------------------------------+ *)
+
+let draw window graph =
+ let { Gtk.width; Gtk.height } = window#misc#allocation in
+ let surface = Cairo.image_surface_create Cairo.FORMAT_ARGB32 width height in
+ let ctx = Cairo.create surface in
+ plot ctx (float width) (float height) graph (Unix.gettimeofday ());
+ let ctx = Cairo_lablgtk.create window#misc#window in
+ Cairo.set_source_surface ctx surface 0. 0.;
+ Cairo.rectangle ctx 0. 0. (float width) (float height);
+ Cairo.fill ctx;
+ Cairo.surface_finish surface
diff --git a/info/control2011/src/tools/krobot_plot_battery.ml b/info/control2011/src/tools/krobot_plot_battery.ml
new file mode 100644
index 0000000..fa79f9b
--- /dev/null
+++ b/info/control2011/src/tools/krobot_plot_battery.ml
@@ -0,0 +1,54 @@
+(*
+ * krobot_plot_battery.ml
+ * ----------------------
+ * Copyright : (c) 2011, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+open Lwt
+open Lwt_react
+open Krobot_message
+
+lwt () =
+ lwt bus = Krobot_bus.get () in
+ ignore (GMain.init ());
+ Lwt_glib.install ();
+
+ let waiter, wakener = wait () in
+
+ (* GTK stuff. *)
+ let window = GWindow.window ~title:"Krobot batteries" () in
+ ignore (window#connect#destroy ~callback:(wakeup wakener));
+ window#show ();
+
+ (* Create the graph. *)
+ let graph = { Krobot_graph.points = Array.init 2 (fun _ -> Queue.create ());
+ Krobot_graph.max = 1 } in
+
+ E.keep
+ (E.map
+ (fun (timestamp, msg) ->
+ match msg with
+ | Battery1_voltages(x1, x2, x3, x4) ->
+ let s = int_of_float ((x1 +. x2 +. x3 +. x4) *. 1000.) in
+ graph.Krobot_graph.max <- max graph.Krobot_graph.max s;
+ Queue.push (timestamp, s) graph.Krobot_graph.points.(0);
+ Krobot_graph.update_graph graph timestamp
+ | Battery2_voltages(x1, x2, x3, x4) ->
+ let s = int_of_float ((x1 +. x2 +. x3 +. x4) *. 1000.) in
+ graph.Krobot_graph.max <- max graph.Krobot_graph.max s;
+ Queue.push (timestamp, s) graph.Krobot_graph.points.(1);
+ Krobot_graph.update_graph graph timestamp
+ | _ ->
+ ())
+ (Krobot_message.recv bus));
+
+ pick [
+ waiter;
+ while_lwt true do
+ Krobot_graph.draw window graph;
+ Lwt_unix.sleep (1. /. 25.)
+ done;
+ ]
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index 6941356..08b3d16 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -10,6 +10,7 @@
open Lwt
open Lwt_react
open Krobot_message
+open Krobot_geom
let utf8 code =
let set_byte s o x = String.unsafe_set s o (Char.unsafe_chr x) in
@@ -181,8 +182,7 @@ module Board = struct
open Krobot_config
type state = {
- x : float;
- y : float;
+ pos : vertice;
theta : float;
}
@@ -197,7 +197,8 @@ module Board = struct
ui : Krobot_viewer_ui.window;
mutable state : state;
mutable beacon : beacon;
- mutable points : (float * float) list;
+ mutable points : vertice list;
+ mutable bezier : vertice array list;
mutable event : unit event;
mutable moving : bool;
}
@@ -235,6 +236,8 @@ module Board = struct
let ctx = Cairo.create surface in
let width = float width and height = float height in
+ Cairo.set_antialias ctx Cairo.ANTIALIAS_NONE;
+
(* Draw the background *)
Cairo.rectangle ctx 0. 0. width height;
set_color ctx White;
@@ -348,6 +351,8 @@ module Board = struct
Cairo.rel_line_to ctx 0.4 0.;
Cairo.stroke ctx;
+ Cairo.set_antialias ctx Cairo.ANTIALIAS_DEFAULT;
+
(* Draw circles on bonus cases *)
Cairo.arc ctx 0.975 0.875 0.05 0. (2. *. pi);
Cairo.fill ctx;
@@ -370,7 +375,7 @@ module Board = struct
Cairo.save ctx;
(* Draw the robot *)
- Cairo.translate ctx board.state.x board.state.y;
+ Cairo.translate ctx board.state.pos.x board.state.pos.y;
Cairo.rotate ctx board.state.theta;
Cairo.rectangle ctx (-. wheels_position) (-. robot_size /. 2.) robot_size robot_size;
set_color ctx White;
@@ -393,12 +398,18 @@ module Board = struct
Cairo.arc ctx board.beacon.xbeacon board.beacon.ybeacon 0.04 0. (2. *. pi);
set_color ctx Purple;
Cairo.fill ctx;
+ Cairo.arc ctx board.beacon.xbeacon board.beacon.ybeacon 0.04 0. (2. *. pi);
+ set_color ctx Black;
+ Cairo.stroke ctx
end;
(* Draw points. *)
Cairo.set_source_rgb ctx 255. 255. 0.;
- Cairo.move_to ctx board.state.x board.state.y;
- List.iter (fun (x, y) -> Cairo.line_to ctx x y) board.points;
+ Cairo.move_to ctx board.state.pos.x board.state.pos.y;
+ List.iter (fun { x; y } -> Cairo.line_to ctx x y) board.points;
+ Cairo.stroke ctx;
+ Cairo.set_source_rgb ctx 255. 0. 255.;
+ List.iter (Array.iter (fun { x; y } -> Cairo.line_to ctx x y)) board.bezier;
Cairo.stroke ctx;
let ctx = Cairo_lablgtk.create board.ui#scene#misc#window in
@@ -418,7 +429,7 @@ module Board = struct
let x0 = (width -. dw) /. 2. and y0 = (height -. dh) /. 2. in
let x = (x -. x0) /. scale -. 0.102 and y = world_height -. ((y -. y0) /. scale -. 0.102) in
if x >= 0. && x < world_width && y >= 0. && y < world_height then begin
- board.points <- board.points @ [(x, y)];
+ board.points <- board.points @ [{ x; y }];
queue_draw board
end
@@ -432,11 +443,11 @@ module Board = struct
| _ :: l -> last l
let smooth board =
- let points = Array.of_list ((board.state.x, board.state.y) :: board.points) in
+ let points = Array.of_list ({ x = board.state.pos.x; y = board.state.pos.y } :: board.points) in
let tolerance = board.ui#tolerance#adjustment#value in
let rec loop = function
| i1 :: i2 :: rest ->
- let (x1, y1) = points.(i1) and (x2, y2) = points.(i2) in
+ let { x = x1; y = y1 } = points.(i1) and { x = x2; y = y2 } = points.(i2) in
let a = y2 -. y1 and b = x1 -. x2 and c = x2 *. y1 -. x1 *. y2 in
let r = sqrt (a *. a +. b *. b) in
if r <> 0. then begin
@@ -444,7 +455,7 @@ module Board = struct
y1) and (x2, y2) *)
let max_dist = ref 0. and at_max = ref i1 in
for i = i1 + 1 to i2 - 1 do
- let (x, y) = points.(i) in
+ let { x; y } = points.(i) in
let d = abs_float (a *. x +. b *. y +. c) /. r in
if d > !max_dist then begin
max_dist := d;
@@ -466,6 +477,33 @@ module Board = struct
in
let result = List.tl (loop [0; Array.length points - 1]); in
board.points <- List.map (fun i -> points.(i)) result;
+(*
+ let speed = board.ui#moving_speed#adjustment#value
+ and acceleration = board.ui#moving_acceleration#adjustment#value in
+
+ (* Compute cubic bezier curves. *)
+ let rec loop = function
+ | p :: (q :: r :: s :: _ as rest) ->
+ (* Compute the speed vectors. *)
+ let v1 = tangent p q r and v2 = tangent q r s in
+ let v1 = { vx = -. v1.vy; vy = v1.vx } *| speed
+ and v2 = { vx = v2.vy; vy = -. v2.vx } *| speed in
+
+ (* Create the bezier curve. *)
+ let curve = Bezier.make ~p:q ~s:r ~vp:v1 ~vs:v2 ~a:acceleration ~error_max:0.1 in
+
+ (* Create vertices. *)
+ let vertices = Array.create 101 origin in
+ for i = 0 to 100 do
+ vertices.(i) <- Bezier.vertice curve (float i /. 100.)
+ done;
+
+ vertices :: loop rest
+ | _ ->
+ []
+ in
+ board.bezier <- [||] :: loop board.points;
+*)
queue_draw board
let wait_done board =
@@ -481,12 +519,12 @@ module Board = struct
let go board =
let rec loop () =
match board.points with
- | (x, y) :: rest ->
+ | { 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 -. board.state.y) (x -. board.state.x) -. board.state.theta) (2. *. pi) in
+ let alpha = math_mod_float (atan2 (y -. board.state.pos.y) (x -. board.state.pos.x) -. board.state.theta) (2. *. pi) in
lwt () = Lwt_log.info_f "turning by %f radiants" alpha in
lwt () = Krobot_message.send board.bus (Unix.gettimeofday (),
Motor_turn(alpha,
@@ -495,7 +533,7 @@ module Board = struct
lwt () = wait_done board in
(* Move the robot. *)
- let dist = sqrt (sqr (x -. board.state.x) +. sqr (y -. board.state.y)) in
+ let dist = sqrt (sqr (x -. board.state.pos.x) +. sqr (y -. board.state.pos.y)) in
lwt () = Lwt_log.info_f "moving by %f meters" dist in
lwt () = Krobot_message.send board.bus (Unix.gettimeofday (),
Motor_move(dist,
@@ -507,6 +545,9 @@ module Board = struct
(match board.points with
| _ :: l -> board.points <- l
| [] -> ());
+ (match board.bezier with
+ | _ :: l -> board.bezier <- l
+ | [] -> ());
(* Redraw everything without the last point. *)
queue_draw board;
@@ -523,13 +564,14 @@ module Board = struct
let board ={
bus;
ui;
- state = {
- x = 0.2;
- y = 1.9 +. Krobot_config.robot_size /. 2. -. Krobot_config.wheels_position;
+ state = {
+ pos = { x = 0.2;
+ y = 1.9 +. Krobot_config.robot_size /. 2. -. Krobot_config.wheels_position };
theta = -0.5 *. pi
};
beacon = { xbeacon = 1.; ybeacon = 1.; valid = true };
points = [];
+ bezier = [];
event = E.never;
moving = false;
} in
@@ -541,7 +583,7 @@ module Board = struct
match frame with
| Odometry(x, y, theta) ->
let angle = math_mod_float (theta) (2. *. pi) in
- let state = { x; y; theta = angle; } in
+ let state = { pos = { x; y }; theta = angle } in
if state <> board.state then begin
board.state <- state;
board.ui#entry_x#set_text (string_of_float x);
@@ -557,8 +599,8 @@ module Board = struct
board.ui#entry_moving4#set_text (if m4 then "yes" else "no")
| Beacon_position(angle, distance, period) ->
let newangle = math_mod_float (board.state.theta +. Krobot_config.rotary_beacon_index_pos +. angle) (2. *. pi) in
- let x = board.state.x +. distance *. cos (newangle) in
- let y = board.state.y +. distance *. sin (newangle) in
+ let x = board.state.pos.x +. distance *. cos (newangle) in
+ let y = board.state.pos.y +. distance *. sin (newangle) in
let valid = distance <> 0. in
let beacon = { xbeacon = x; ybeacon = y; valid; } in
if beacon <> board.beacon then begin
hooks/post-receive
--
krobot
|
