Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 5f3e029aaab36e94eb061e83d3a810f3c6c3adce
|
From: Jérémie D. <Ba...@us...> - 2011-04-20 09:16:31
|
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 5f3e029aaab36e94eb061e83d3a810f3c6c3adce (commit)
via 02e03abf2ff214556d7ec225b4837e0c42845bb8 (commit)
via cb1cc4e6da65d34ec8d6b2cf7f045279265dc726 (commit)
from d75c624ea6637e76d1acdc2f25ca59c8c93fc4c7 (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 5f3e029aaab36e94eb061e83d3a810f3c6c3adce
Author: Jérémie Dimino <je...@di...>
Date: Wed Apr 20 11:14:47 2011 +0200
[info] add a flag to CAN frames to tell whether they come from a card or from a program
commit 02e03abf2ff214556d7ec225b4837e0c42845bb8
Author: Jérémie Dimino <je...@di...>
Date: Wed Apr 20 10:10:30 2011 +0200
[krobot_monitor] show decoded CAN frames
commit cb1cc4e6da65d34ec8d6b2cf7f045279265dc726
Author: Jérémie Dimino <je...@di...>
Date: Wed Apr 20 10:00:47 2011 +0200
[info] make bezier trajectories continous
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/_oasis b/info/control2011/_oasis
index 3b5b668..81dd9ef 100644
--- a/info/control2011/_oasis
+++ b/info/control2011/_oasis
@@ -98,8 +98,7 @@ Executable "krobot-dump-encoders"
Executable "krobot-record"
Path: src/tools
- Install: false
- Build: false
+ Install: true
CompiledObject: best
MainIs: krobot_record.ml
BuildDepends: krobot, lwt.syntax
diff --git a/info/control2011/setup.ml b/info/control2011/setup.ml
index a90aa6e..9065940 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: e5aa81e40566663130f898cbaee3b839) *)
+(* DO NOT EDIT (digest: f74f01087e7cdeda8c8d0d200fd9f844) *)
(*
Regenerated by OASIS v0.2.0
Visit http://oasis.forge.ocamlcore.org for more information and
@@ -5568,8 +5568,8 @@ let setup_t =
cs_plugin_data = [];
},
{
- bs_build = [(OASISExpr.EBool true, false)];
- bs_install = [(OASISExpr.EBool true, false)];
+ bs_build = [(OASISExpr.EBool true, true)];
+ bs_install = [(OASISExpr.EBool true, true)];
bs_path = "src/tools";
bs_compiled_object = Best;
bs_build_depends =
diff --git a/info/control2011/src/driver/krobot_driver.ml b/info/control2011/src/driver/krobot_driver.ml
index 9e5b578..184dfe9 100644
--- a/info/control2011/src/driver/krobot_driver.ml
+++ b/info/control2011/src/driver/krobot_driver.ml
@@ -65,7 +65,7 @@ lwt () =
| Kill "driver" ->
exit 0
- | CAN frame -> begin
+ | CAN(Info, frame) -> begin
match !can_opt with
| Some can ->
Krobot_can_bus.send can (ts, frame)
@@ -86,7 +86,7 @@ lwt () =
try_lwt
while_lwt true do
lwt (ts, frame) = Krobot_can_bus.recv can in
- Krobot_bus.send bus (ts, CAN frame)
+ Krobot_bus.send bus (ts, CAN(Elec, frame))
done
with exn ->
(* Make sure no more messages are sent on the CAN bus. *)
diff --git a/info/control2011/src/lib/krobot_bus.ml b/info/control2011/src/lib/krobot_bus.ml
index a77f60e..8633395 100644
--- a/info/control2011/src/lib/krobot_bus.ml
+++ b/info/control2011/src/lib/krobot_bus.ml
@@ -18,8 +18,10 @@ let port = 50000
| Types |
+-----------------------------------------------------------------+ *)
+type frame_source = Elec | Info
+
type message =
- | CAN of Krobot_can.frame
+ | CAN of frame_source * Krobot_can.frame
| Log of string
| Send
| Kill of string
@@ -50,9 +52,10 @@ let string_of_vector v =
sprintf "{ vx = %f; vy = %f }" v.vx v.vy
let string_of_message = function
- | CAN frame ->
+ | CAN(source, frame) ->
sprintf
- "CAN %s"
+ "CAN(%s, %s)"
+ (match source with Elec -> "Elec" | Info -> "Info")
(Krobot_can.string_of_frame frame)
| Log str ->
sprintf
diff --git a/info/control2011/src/lib/krobot_bus.mli b/info/control2011/src/lib/krobot_bus.mli
index dccfb9f..fb1333a 100644
--- a/info/control2011/src/lib/krobot_bus.mli
+++ b/info/control2011/src/lib/krobot_bus.mli
@@ -18,10 +18,13 @@ val get : unit -> t Lwt.t
(** [get ()] returns the krobot bus. It exits the program on
error. *)
+type frame_source = Elec | Info
+ (** The source of CAN frames. *)
+
(** Type of message exchanged over the bus. *)
type message =
- | CAN of Krobot_can.frame
- (** A CAN frame. *)
+ | CAN of frame_source * Krobot_can.frame
+ (** [CAN(source, frmae)] a CAN frame. *)
| Log of string
(** A log message. *)
| Send
diff --git a/info/control2011/src/lib/krobot_message.ml b/info/control2011/src/lib/krobot_message.ml
index 94db4fc..93ee3f4 100644
--- a/info/control2011/src/lib/krobot_message.ml
+++ b/info/control2011/src/lib/krobot_message.ml
@@ -32,7 +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
+ | Odometry_ghost of float * float * float * int * bool
| Set_odometry of float * float * float
| Set_controller_mode of bool
| Req_motor_status
@@ -108,10 +108,10 @@ let to_string = function
sprintf
"Odometry(%f, %f, %f)"
x y theta
- | Odometry_ghost(x, y, theta, following) ->
+ | Odometry_ghost(x, y, theta, u, following) ->
sprintf
- "Odometry_ghost(%f, %f, %f, %B)"
- x y theta following
+ "Odometry_ghost(%f, %f, %f, %d, %B)"
+ x y theta u following
| Set_odometry(x, y, theta) ->
sprintf
"Set_odometry(%f, %f, %f)"
@@ -191,12 +191,13 @@ let encode = function
~remote:false
~format:F29bits
~data
- | Odometry_ghost(x, y, theta, following) ->
- let data = String.create 7 in
+ | Odometry_ghost(x, y, theta, u, following) ->
+ let data = String.create 8 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);
+ put_uint8 data 6 u;
+ put_uint8 data 7 (if following then 1 else 0);
frame
~identifier:105
~kind:Data
@@ -375,7 +376,8 @@ let decode frame =
(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)
+ get_uint8 frame.data 6,
+ get_uint8 frame.data 7 <> 0)
| 201 ->
Motor_move
(float (get_sint32 frame.data 0) /. 1000.,
@@ -439,13 +441,12 @@ let decode frame =
+-----------------------------------------------------------------+ *)
let send bus (timestamp, msg) =
- Krobot_bus.send bus (timestamp, Krobot_bus.CAN(encode msg))
-
+ Krobot_bus.send bus (timestamp, Krobot_bus.CAN(Krobot_bus.Info, encode msg))
let recv bus =
E.fmap
(fun (timestamp, message) ->
match message with
- | Krobot_bus.CAN frame ->
+ | Krobot_bus.CAN(_, frame) ->
Some(timestamp, decode frame)
| _ ->
None)
diff --git a/info/control2011/src/lib/krobot_message.mli b/info/control2011/src/lib/krobot_message.mli
index 53df1d6..c17cb0b 100644
--- a/info/control2011/src/lib/krobot_message.mli
+++ b/info/control2011/src/lib/krobot_message.mli
@@ -51,7 +51,7 @@ 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 of float * float * float * int * bool
(** [Odometry_ghost(x, y, theta, following)]. [following] is
[true] iff the robot is following the ghost. *)
| Set_odometry of float * float * float
diff --git a/info/control2011/src/tools/krobot_dump.ml b/info/control2011/src/tools/krobot_dump.ml
index 1715ca5..4b75d21 100644
--- a/info/control2011/src/tools/krobot_dump.ml
+++ b/info/control2011/src/tools/krobot_dump.ml
@@ -11,6 +11,7 @@
open Lwt
open Lwt_react
+open Krobot_bus
let raw = ref false
let decoded = ref true
@@ -60,9 +61,9 @@ lwt () =
(E.map_s
(fun (timestamp, message) ->
match message with
- | Krobot_bus.CAN frame ->
+ | CAN(source, frame) ->
let msg = Krobot_message.decode frame in
- lwt () = Lwt_io.print (date_string timestamp)in
+ lwt () = Lwt_io.printf "%s| %s" (match source with Elec -> "elec" | Info -> "info") (date_string timestamp)in
lwt () =
if !decoded then
Lwt_io.printf ": %s" (Krobot_message.to_string msg)
diff --git a/info/control2011/src/tools/krobot_monitor.ml b/info/control2011/src/tools/krobot_monitor.ml
index 0fae3c7..81f529e 100644
--- a/info/control2011/src/tools/krobot_monitor.ml
+++ b/info/control2011/src/tools/krobot_monitor.ml
@@ -11,6 +11,7 @@
open Lwt
open Lwt_react
+open Krobot_bus
let date_string time =
let tm = Unix.localtime time in
@@ -45,7 +46,16 @@ lwt () =
E.keep
(E.map_s
(fun (timestamp, message) ->
- Lwt_io.printlf "%s: %s" (date_string timestamp) (Krobot_bus.string_of_message message))
+ match message with
+ | CAN(_, frame) ->
+ Lwt_io.printlf "%s: %s -> %s"
+ (date_string timestamp)
+ (Krobot_bus.string_of_message message)
+ (Krobot_message.to_string (Krobot_message.decode frame))
+ | _ ->
+ Lwt_io.printlf "%s: %s"
+ (date_string timestamp)
+ (Krobot_bus.string_of_message message))
(Krobot_bus.recv bus));
fst (wait ())
diff --git a/info/control2011/src/tools/krobot_planner.ml b/info/control2011/src/tools/krobot_planner.ml
index 0dbe9f7..dec418e 100644
--- a/info/control2011/src/tools/krobot_planner.ml
+++ b/info/control2011/src/tools/krobot_planner.ml
@@ -52,6 +52,10 @@ type planner = {
mutable motors_moving : bool;
(* Are the motor currently active ? *)
+ mutable curve_status : int;
+ (* The status of the trajectory controller on the curve (between 0
+ and 255). *)
+
mutable mover : unit Lwt.t;
(* The thread moving the robot. *)
@@ -125,90 +129,147 @@ let wait_done planner =
lwt () = Lwt_unix.sleep 0.3 in
lwt () =
while_lwt planner.motors_moving do
- Lwt_unix.sleep 0.2
+ Lwt_unix.sleep 0.1
done
in
Lwt_log.info "trajectory done"
+let wait_middle planner =
+ lwt () = Lwt_log.info "waiting for the robot to be in the middle of the trajectory" in
+ lwt () = Lwt_unix.sleep 0.3 in
+ lwt () =
+ while_lwt planner.curve_status < 128 do
+ Lwt_unix.sleep 0.001
+ done
+ in
+ Lwt_log.info "robot in the middle of the trajectory"
+
+let wait_start planner =
+ lwt () = Lwt_log.info "waiting for the robot to start the new trajectory" in
+ lwt () =
+ while_lwt planner.curve_status >= 128 do
+ Lwt_unix.sleep 0.001
+ done
+ in
+ Lwt_log.info "robot started the new trajectory"
+
let go planner rotation_speed rotation_acceleration moving_speed moving_acceleration =
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*)
+ (* 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
let o, v = planner.origin in
+
+ (* Compute all cubic bezier curves. *)
let params = List.rev (Bezier.fold_vertices (fun p q r s acc -> (p, q, r, s) :: acc) v (o :: planner.vertices) []) in
- Lwt_list.iter_s
- (fun (p, q, r, s) ->
-
- let v = vector r s in
- lwt () =
- Krobot_message.send
- planner.bus
- (Unix.gettimeofday (),
- Motor_bezier(s.x,
- s.y,
- distance p q,
- distance r s,
- atan2 v.vy v.vx,
- 0.))
- in
-
- lwt () = wait_done planner in
-
- (match planner.vertices with
- | _ :: l ->
- set_vertices planner l
- | [] ->
- ());
- set_origin planner (planner.position,
- { vx = cos planner.orientation;
- vy = sin planner.orientation });
-
- return ()
- )
- params
+
+ (* Send a bezier curve to the robot. *)
+ let send_curve (p, q, r, s) v_end =
+ (* Compute parameters. *)
+ let d1 = distance p q and d2 = distance r s in
+ let v = vector r s in
+ let theta_end = atan2 v.vy v.vx in
+
+ ignore (
+ Lwt_log.info_f
+ "sending bezier curve, x = %f, y = %f, d1 = %f, d2 = %f, theta_end = %f, v_end = %f"
+ s.x s.y d1 d2 theta_end v_end
+ );
+
+ (* Send the curve. *)
+ Krobot_message.send planner.bus (Unix.gettimeofday (), Motor_bezier(s.x, s.y, d1, d2, theta_end, v_end))
+ in
+
+ (* Remove the first vertice of the trajecotry. *)
+ let drop_vertice () =
+ (match planner.vertices with
+ | _ :: l ->
+ set_vertices planner l
+ | [] ->
+ ());
+ set_origin planner (planner.position,
+ { vx = cos planner.orientation;
+ vy = sin planner.orientation })
+ in
+
+ let rec loop = function
+ | [] ->
+ lwt () = wait_done planner in
+ drop_vertice ();
+ return ()
+
+ | [points] ->
+ lwt () = wait_middle planner in
+ lwt () = send_curve points 0.01 in
+ lwt () = wait_done planner in
+ drop_vertice ();
+ return ()
+
+ | points :: rest ->
+ lwt () = wait_middle planner in
+ lwt () = send_curve points 0.5 in
+ lwt () = wait_start planner in
+ drop_vertice ();
+ loop rest
+ in
+
+ match params with
+ | [] ->
+ return ()
+
+ | [points] ->
+ lwt () = send_curve points 0.01 in
+ lwt () = wait_done planner in
+ drop_vertice ();
+ return ()
+
+ | points :: rest ->
+ lwt () = send_curve points 0.5 in
+ loop rest
+
with exn ->
Lwt_log.error_f ~section ~exn "failed to move"
+
finally
set_moving planner false;
return ()
@@ -222,7 +283,7 @@ let go planner rotation_speed rotation_acceleration moving_speed moving_accelera
let handle_message planner (timestamp, message) =
match message with
- | CAN frame -> begin
+ | CAN(_, frame) -> begin
match decode frame with
| Odometry(x, y, theta) ->
planner.position <- { x; y };
@@ -232,7 +293,8 @@ let handle_message planner (timestamp, message) =
{ vx = cos planner.orientation;
vy = sin planner.orientation })
- | Odometry_ghost(x, y, theta, following) ->
+ | Odometry_ghost(x, y, theta, u, following) ->
+ planner.curve_status <- u;
planner.motors_moving <- following
| _ ->
@@ -313,6 +375,7 @@ lwt () =
vertices = [];
moving = false;
motors_moving = false;
+ curve_status = 0;
mover = return ();
position = { x = 0.; y = 0. };
orientation = 0.;
diff --git a/info/control2011/src/tools/krobot_record.ml b/info/control2011/src/tools/krobot_record.ml
index f0a7d72..29bc65a 100644
--- a/info/control2011/src/tools/krobot_record.ml
+++ b/info/control2011/src/tools/krobot_record.ml
@@ -11,6 +11,7 @@
open Lwt
open Lwt_react
+open Krobot_bus
lwt () =
if Array.length Sys.argv <> 2 then begin
@@ -21,13 +22,16 @@ lwt () =
lwt bus = Krobot_bus.get () in
lwt oc = Lwt_io.open_file ~mode:Lwt_io.output Sys.argv.(1) in
- (* The proxy for the driver. *)
- let driver = OBus_proxy.make (OBus_peer.make (Krobot_bus.to_bus bus) "fr.krobot.Service.Driver") ["fr"; "krobot"; "CAN"] in
-
- (* Receive frames comming from the driver. *)
- lwt ev = OBus_signal.connect (OBus_signal.make Krobot_interface_can.Fr_krobot_CAN.s_message driver) in
-
- (* Write all frames to the output file. *)
- E.keep (E.map_s (fun v -> Lwt_io.write_value oc (Krobot_can.frame_of_tuple v)) ev);
+ (* Write all frames comming from the electronic to the output
+ file. *)
+ E.keep
+ (E.map_s
+ (fun (timestamp, message) ->
+ match message with
+ | CAN(Elec, frame) ->
+ Lwt_io.write_value oc (timestamp, frame)
+ | _ ->
+ return ())
+ (Krobot_bus.recv bus));
fst (wait ())
diff --git a/info/control2011/src/tools/krobot_replay.ml b/info/control2011/src/tools/krobot_replay.ml
index 73284bb..f1c96e4 100644
--- a/info/control2011/src/tools/krobot_replay.ml
+++ b/info/control2011/src/tools/krobot_replay.ml
@@ -16,7 +16,7 @@ open Krobot_bus
let rec loop bus ic prev_timestamp =
lwt timestamp, frame = Lwt_io.read_value ic in
lwt () = Lwt_unix.sleep (timestamp -. prev_timestamp) in
- lwt () = Krobot_bus.send bus (Unix.gettimeofday (), CAN frame) in
+ lwt () = Krobot_bus.send bus (Unix.gettimeofday (), CAN(Elec, frame)) in
loop bus ic timestamp
lwt () =
@@ -30,7 +30,7 @@ lwt () =
try_lwt
lwt timestamp, frame = Lwt_io.read_value ic in
- lwt () = Krobot_bus.send bus (Unix.gettimeofday (), CAN frame) in
+ lwt () = Krobot_bus.send bus (Unix.gettimeofday (), CAN(Elec, frame)) in
loop bus ic timestamp
with End_of_file ->
Lwt_io.close ic
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index a79ddea..9e22445 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -326,7 +326,7 @@ let simplify viewer =
let handle_message viewer (timestamp, message) =
match message with
- | CAN frame -> begin
+ | CAN(_, frame) -> begin
match decode frame with
| Odometry(x, y, theta) ->
let angle = math_mod_float (theta) (2. *. pi) in
@@ -339,7 +339,7 @@ let handle_message viewer (timestamp, message) =
queue_draw viewer
end
- | Odometry_ghost(x, y, theta, following) ->
+ | Odometry_ghost(x, y, theta, u, following) ->
let angle = math_mod_float (theta) (2. *. pi) in
let ghost = { pos = { x; y }; theta = angle } in
if ghost <> viewer.ghost then begin
hooks/post-receive
--
krobot
|
