Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. d68c3713a184beeec683548139d7af472a545b70
|
From: Jérémie D. <Ba...@us...> - 2011-04-19 17:10:40
|
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 d68c3713a184beeec683548139d7af472a545b70 (commit)
from 37baee2571d2c9ab630f4840cb3591934598d5fa (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 d68c3713a184beeec683548139d7af472a545b70
Author: Jérémie Dimino <je...@di...>
Date: Tue Apr 19 19:09:57 2011 +0200
[krobot_viewer] show the ghost
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/lib/krobot_message.ml b/info/control2011/src/lib/krobot_message.ml
index 62aff7a..94db4fc 100644
--- a/info/control2011/src/lib/krobot_message.ml
+++ b/info/control2011/src/lib/krobot_message.ml
@@ -32,6 +32,7 @@ type t =
| Motor_bezier of float * float * float * float * float * float
| Motor_stop
| Odometry of float * float * float
+ | Odometry_ghost of float * float * float * bool
| Set_odometry of float * float * float
| Set_controller_mode of bool
| Req_motor_status
@@ -107,6 +108,10 @@ let to_string = function
sprintf
"Odometry(%f, %f, %f)"
x y theta
+ | Odometry_ghost(x, y, theta, following) ->
+ sprintf
+ "Odometry_ghost(%f, %f, %f, %B)"
+ x y theta following
| Set_odometry(x, y, theta) ->
sprintf
"Set_odometry(%f, %f, %f)"
@@ -186,6 +191,18 @@ let encode = function
~remote:false
~format:F29bits
~data
+ | Odometry_ghost(x, y, theta, following) ->
+ let data = String.create 7 in
+ put_sint16 data 0 (truncate (x *. 1000.));
+ put_sint16 data 2 (truncate (y *. 1000.));
+ put_sint16 data 4 (truncate (theta *. 10000.));
+ put_uint8 data 6 (if following then 1 else 0);
+ frame
+ ~identifier:105
+ ~kind:Data
+ ~remote:false
+ ~format:F29bits
+ ~data
| Motor_move(dist, speed, acc) ->
let data = String.create 8 in
put_sint32 data 0 (truncate (dist *. 1000.));
@@ -353,6 +370,12 @@ let decode frame =
(float (get_sint16 frame.data 0) /. 1000.,
float (get_sint16 frame.data 2) /. 1000.,
float (get_sint16 frame.data 4) /. 10000.)
+ | 105 ->
+ Odometry_ghost
+ (float (get_sint16 frame.data 0) /. 1000.,
+ float (get_sint16 frame.data 2) /. 1000.,
+ float (get_sint16 frame.data 4) /. 10000.,
+ get_uint8 frame.data 6 <> 0)
| 201 ->
Motor_move
(float (get_sint32 frame.data 0) /. 1000.,
diff --git a/info/control2011/src/lib/krobot_message.mli b/info/control2011/src/lib/krobot_message.mli
index 557994c..53df1d6 100644
--- a/info/control2011/src/lib/krobot_message.mli
+++ b/info/control2011/src/lib/krobot_message.mli
@@ -51,6 +51,9 @@ type t =
| Odometry of float * float * float
(** [Odometry(x, y, theta)] the position of the robot on the
table. *)
+ | Odometry_ghost of float * float * float * bool
+ (** [Odometry_ghost(x, y, theta, following)]. [following] is
+ [true] iff the robot is following the ghost. *)
| Set_odometry of float * float * float
(** [set_odometry(x, y, theta)] sets the parameters of the
odometry to the given ones. *)
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index 12926f4..43e01d8 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -44,6 +44,9 @@ type viewer = {
mutable state : state;
(* The state of the robot. *)
+ mutable ghost : state;
+ (* The state of the ghost. *)
+
mutable beacon : beacon;
(* The state of the beacon. *)
@@ -229,22 +232,26 @@ let draw viewer =
Cairo.save ctx;
- (* Draw the robot *)
- Cairo.translate ctx viewer.state.pos.x viewer.state.pos.y;
- Cairo.rotate ctx viewer.state.theta;
- Cairo.rectangle ctx (-. wheels_position) (-. robot_size /. 2.) robot_size robot_size;
- set_color ctx White;
- Cairo.fill ctx;
-
- (* Draw an arrow on the robot *)
- let d = robot_size /. 2. -. wheels_position in
- Cairo.move_to ctx 0. 0.;
- Cairo.line_to ctx (d +. robot_size /. 4.) 0.;
- Cairo.line_to ctx d (-. robot_size /. 4.);
- Cairo.line_to ctx d (robot_size /. 4.);
- Cairo.line_to ctx (d +. robot_size /. 4.) 0.;
- set_color ctx Black;
- Cairo.stroke ctx;
+ List.iter
+ (fun (state, alpha) ->
+ (* Draw the robot *)
+ Cairo.translate ctx state.pos.x state.pos.y;
+ Cairo.rotate ctx state.theta;
+ Cairo.rectangle ctx (-. wheels_position) (-. robot_size /. 2.) robot_size robot_size;
+ Cairo.set_source_rgba ctx 1. 1. 1. alpha;
+ Cairo.fill ctx;
+
+ (* Draw an arrow on the robot *)
+ let d = robot_size /. 2. -. wheels_position in
+ Cairo.move_to ctx 0. 0.;
+ Cairo.line_to ctx (d +. robot_size /. 4.) 0.;
+ Cairo.line_to ctx d (-. robot_size /. 4.);
+ Cairo.line_to ctx d (robot_size /. 4.);
+ Cairo.line_to ctx (d +. robot_size /. 4.) 0.;
+ Cairo.set_source_rgba ctx 0. 0. 0. 0.5;
+ Cairo.stroke ctx)
+ [(viewer.ghost, 0.5);
+ (viewer.state, 1.0)];
Cairo.restore ctx;
@@ -328,6 +335,15 @@ let handle_message viewer (timestamp, message) =
viewer.ui#entry_theta#set_text (string_of_float theta);
queue_draw viewer
end
+
+ | Odometry_ghost(x, y, theta, following) ->
+ let angle = math_mod_float (theta) (2. *. pi) in
+ let ghost = { pos = { x; y }; theta = angle } in
+ if ghost <> viewer.ghost then begin
+ viewer.ghost <- ghost;
+ queue_draw viewer
+ end
+
| Motor_status(m1, m2, m3, m4) ->
let moving = m1 || m2 in
if moving then begin
@@ -347,6 +363,7 @@ let handle_message viewer (timestamp, message) =
viewer.ui#entry_moving2#set_text (if m2 then "yes" else "no");
viewer.ui#entry_moving3#set_text (if m3 then "yes" else "no");
viewer.ui#entry_moving4#set_text (if m4 then "yes" else "no")
+
| Beacon_position(angle, distance, period) ->
let newangle = math_mod_float (viewer.state.theta +. Krobot_config.rotary_beacon_index_pos +. angle) (2. *. pi) in
let x = viewer.state.pos.x +. distance *. cos (newangle) in
@@ -361,11 +378,13 @@ let handle_message viewer (timestamp, message) =
viewer.ui#beacon_period#set_text (string_of_float period);
queue_draw viewer
end
+
| Set_controller_mode hil ->
if hil then
viewer.ui#menu_mode_hil#set_active true
else
viewer.ui#menu_mode_normal#set_active true
+
| _ ->
()
end
@@ -433,14 +452,16 @@ lwt () =
];
(* Create the viewer. *)
+ let init = {
+ pos = { x = 0.2;
+ y = 1.9 +. Krobot_config.robot_size /. 2. -. Krobot_config.wheels_position };
+ theta = -0.5 *. pi
+ } in
let viewer ={
bus;
ui;
- state = {
- pos = { x = 0.2;
- y = 1.9 +. Krobot_config.robot_size /. 2. -. Krobot_config.wheels_position };
- theta = -0.5 *. pi
- };
+ state = init;
+ ghost = init;
beacon = { xbeacon = 1.; ybeacon = 1.; valid = false };
origin = ({ x = 0.; y = 0. }, { vx = 0.; vy = 0. });
vertices = [];
hooks/post-receive
--
krobot
|
