Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 89e804e0141a8c908fb78fa549e50f1dc08864dd
|
From: Jérémie D. <Ba...@us...> - 2011-04-17 21:26:37
|
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 89e804e0141a8c908fb78fa549e50f1dc08864dd (commit)
via af742329207ea5d42ea085c7c26b7d3e7c086bd9 (commit)
from 5e6c03e008aa81f186f26c61430308b5084fcffc (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 89e804e0141a8c908fb78fa549e50f1dc08864dd
Author: Jérémie Dimino <je...@di...>
Date: Sun Apr 17 23:25:37 2011 +0200
[info] send bezier curves
commit af742329207ea5d42ea085c7c26b7d3e7c086bd9
Author: Jérémie Dimino <je...@di...>
Date: Sun Apr 17 19:49:51 2011 +0200
[info] add messages for bezier curves
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/_oasis b/info/control2011/_oasis
index 5b931a1..85e61ae 100644
--- a/info/control2011/_oasis
+++ b/info/control2011/_oasis
@@ -31,9 +31,9 @@ Flag gtk
Library krobot
FindlibName: krobot
- BuildDepends: lwt.unix, lwt.syntax, obus, krobot.interfaces
+ BuildDepends: lwt.unix, lwt.syntax, obus, krobot.interfaces, bitstring, bitstring.syntax
XMETADescription: The [Kro]bot library
- XMETARequires: lwt.unix, obus, krobot.interfaces
+ XMETARequires: lwt.unix, obus, krobot.interfaces, bitstring
Path: src/lib
Install: true
Modules:
diff --git a/info/control2011/_tags b/info/control2011/_tags
index f190b59..f8002fe 100644
--- a/info/control2011/_tags
+++ b/info/control2011/_tags
@@ -2,7 +2,7 @@
<src/interfaces/*.ml>: -syntax_camlp4o
# OASIS_START
-# DO NOT EDIT (digest: 19f32386260765eb3050ee674e3a3e18)
+# DO NOT EDIT (digest: bc62a620a945818c8391362124b031c3)
# Library krobot-interfaces
"src/interfaces": include
"src/interfaces/krobot-interfaces.cmxs": use_krobot-interfaces
@@ -14,6 +14,8 @@
<src/lib/*.ml{,i}>: pkg_obus
<src/lib/*.ml{,i}>: pkg_lwt.unix
<src/lib/*.ml{,i}>: pkg_lwt.syntax
+<src/lib/*.ml{,i}>: pkg_bitstring.syntax
+<src/lib/*.ml{,i}>: pkg_bitstring
# Library krobot-can
"src/can": include
"src/can/krobot-can.cmxs": use_krobot-can
@@ -23,23 +25,31 @@
<src/can/*.ml{,i}>: pkg_obus
<src/can/*.ml{,i}>: pkg_lwt.unix
<src/can/*.ml{,i}>: pkg_lwt.syntax
+<src/can/*.ml{,i}>: pkg_bitstring.syntax
+<src/can/*.ml{,i}>: pkg_bitstring
"src/can/can_stubs.c": use_krobot
"src/can/can_stubs.c": use_krobot-interfaces
"src/can/can_stubs.c": pkg_obus
"src/can/can_stubs.c": pkg_lwt.unix
"src/can/can_stubs.c": pkg_lwt.syntax
+"src/can/can_stubs.c": pkg_bitstring.syntax
+"src/can/can_stubs.c": pkg_bitstring
# Executable krobot-dump
<src/tools/krobot_dump.{native,byte}>: use_krobot
<src/tools/krobot_dump.{native,byte}>: use_krobot-interfaces
<src/tools/krobot_dump.{native,byte}>: pkg_obus
<src/tools/krobot_dump.{native,byte}>: pkg_lwt.unix
<src/tools/krobot_dump.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_dump.{native,byte}>: pkg_bitstring.syntax
+<src/tools/krobot_dump.{native,byte}>: pkg_bitstring
# Executable krobot-dump-encoders
<src/tools/krobot_dump_encoders.{native,byte}>: use_krobot
<src/tools/krobot_dump_encoders.{native,byte}>: use_krobot-interfaces
<src/tools/krobot_dump_encoders.{native,byte}>: pkg_obus
<src/tools/krobot_dump_encoders.{native,byte}>: pkg_lwt.unix
<src/tools/krobot_dump_encoders.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_dump_encoders.{native,byte}>: pkg_bitstring.syntax
+<src/tools/krobot_dump_encoders.{native,byte}>: pkg_bitstring
# Executable krobot-driver
<src/driver/krobot_driver.{native,byte}>: use_krobot-can
<src/driver/krobot_driver.{native,byte}>: use_krobot
@@ -47,12 +57,16 @@
<src/driver/krobot_driver.{native,byte}>: pkg_obus
<src/driver/krobot_driver.{native,byte}>: pkg_lwt.unix
<src/driver/krobot_driver.{native,byte}>: pkg_lwt.syntax
+<src/driver/krobot_driver.{native,byte}>: pkg_bitstring.syntax
+<src/driver/krobot_driver.{native,byte}>: pkg_bitstring
<src/driver/*.ml{,i}>: use_krobot-can
<src/driver/*.ml{,i}>: use_krobot
<src/driver/*.ml{,i}>: use_krobot-interfaces
<src/driver/*.ml{,i}>: pkg_obus
<src/driver/*.ml{,i}>: pkg_lwt.unix
<src/driver/*.ml{,i}>: pkg_lwt.syntax
+<src/driver/*.ml{,i}>: pkg_bitstring.syntax
+<src/driver/*.ml{,i}>: pkg_bitstring
# Executable krobot-viewer
<src/tools/krobot_viewer.{native,byte}>: use_krobot
<src/tools/krobot_viewer.{native,byte}>: use_krobot-interfaces
@@ -62,6 +76,8 @@
<src/tools/krobot_viewer.{native,byte}>: pkg_lwt.glib
<src/tools/krobot_viewer.{native,byte}>: pkg_lablgtk2.glade
<src/tools/krobot_viewer.{native,byte}>: pkg_cairo.lablgtk2
+<src/tools/krobot_viewer.{native,byte}>: pkg_bitstring.syntax
+<src/tools/krobot_viewer.{native,byte}>: pkg_bitstring
<src/tools/*.ml{,i}>: pkg_lablgtk2.glade
# Executable krobot-hil-simulator
<src/tools/krobot_hil_simulator.{native,byte}>: use_krobot
@@ -69,6 +85,8 @@
<src/tools/krobot_hil_simulator.{native,byte}>: pkg_obus
<src/tools/krobot_hil_simulator.{native,byte}>: pkg_lwt.unix
<src/tools/krobot_hil_simulator.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_hil_simulator.{native,byte}>: pkg_bitstring.syntax
+<src/tools/krobot_hil_simulator.{native,byte}>: pkg_bitstring
# Executable send-can
<examples/send_can.{native,byte}>: use_krobot-can
<examples/send_can.{native,byte}>: use_krobot
@@ -76,6 +94,8 @@
<examples/send_can.{native,byte}>: pkg_obus
<examples/send_can.{native,byte}>: pkg_lwt.unix
<examples/send_can.{native,byte}>: pkg_lwt.syntax
+<examples/send_can.{native,byte}>: pkg_bitstring.syntax
+<examples/send_can.{native,byte}>: pkg_bitstring
# Executable krobot-plot-battery
<src/tools/krobot_plot_battery.{native,byte}>: use_krobot
<src/tools/krobot_plot_battery.{native,byte}>: use_krobot-interfaces
@@ -84,29 +104,39 @@
<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
+<src/tools/krobot_plot_battery.{native,byte}>: pkg_bitstring.syntax
+<src/tools/krobot_plot_battery.{native,byte}>: pkg_bitstring
# Executable krobot-service-planner
<src/services/krobot_service_planner.{native,byte}>: use_krobot
<src/services/krobot_service_planner.{native,byte}>: use_krobot-interfaces
<src/services/krobot_service_planner.{native,byte}>: pkg_obus
<src/services/krobot_service_planner.{native,byte}>: pkg_lwt.unix
<src/services/krobot_service_planner.{native,byte}>: pkg_lwt.syntax
+<src/services/krobot_service_planner.{native,byte}>: pkg_bitstring.syntax
+<src/services/krobot_service_planner.{native,byte}>: pkg_bitstring
<src/services/*.ml{,i}>: use_krobot
<src/services/*.ml{,i}>: use_krobot-interfaces
<src/services/*.ml{,i}>: pkg_obus
<src/services/*.ml{,i}>: pkg_lwt.unix
<src/services/*.ml{,i}>: pkg_lwt.syntax
+<src/services/*.ml{,i}>: pkg_bitstring.syntax
+<src/services/*.ml{,i}>: pkg_bitstring
# Executable krobot-remote
<src/tools/krobot_remote.{native,byte}>: use_krobot
<src/tools/krobot_remote.{native,byte}>: use_krobot-interfaces
<src/tools/krobot_remote.{native,byte}>: pkg_obus
<src/tools/krobot_remote.{native,byte}>: pkg_lwt.unix
<src/tools/krobot_remote.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_remote.{native,byte}>: pkg_bitstring.syntax
+<src/tools/krobot_remote.{native,byte}>: pkg_bitstring
# Executable krobot-replay
<src/tools/krobot_replay.{native,byte}>: use_krobot
<src/tools/krobot_replay.{native,byte}>: use_krobot-interfaces
<src/tools/krobot_replay.{native,byte}>: pkg_obus
<src/tools/krobot_replay.{native,byte}>: pkg_lwt.unix
<src/tools/krobot_replay.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_replay.{native,byte}>: pkg_bitstring.syntax
+<src/tools/krobot_replay.{native,byte}>: pkg_bitstring
# Executable dump-can
<examples/dump_can.{native,byte}>: use_krobot-can
<examples/dump_can.{native,byte}>: use_krobot
@@ -114,24 +144,32 @@
<examples/dump_can.{native,byte}>: pkg_obus
<examples/dump_can.{native,byte}>: pkg_lwt.unix
<examples/dump_can.{native,byte}>: pkg_lwt.syntax
+<examples/dump_can.{native,byte}>: pkg_bitstring.syntax
+<examples/dump_can.{native,byte}>: pkg_bitstring
<examples/*.ml{,i}>: use_krobot-can
<examples/*.ml{,i}>: use_krobot
<examples/*.ml{,i}>: use_krobot-interfaces
<examples/*.ml{,i}>: pkg_obus
<examples/*.ml{,i}>: pkg_lwt.unix
<examples/*.ml{,i}>: pkg_lwt.syntax
+<examples/*.ml{,i}>: pkg_bitstring.syntax
+<examples/*.ml{,i}>: pkg_bitstring
# Executable krobot-local
<src/tools/krobot_local.{native,byte}>: use_krobot
<src/tools/krobot_local.{native,byte}>: use_krobot-interfaces
<src/tools/krobot_local.{native,byte}>: pkg_obus
<src/tools/krobot_local.{native,byte}>: pkg_lwt.unix
<src/tools/krobot_local.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_local.{native,byte}>: pkg_bitstring.syntax
+<src/tools/krobot_local.{native,byte}>: pkg_bitstring
# Executable krobot-record
<src/tools/krobot_record.{native,byte}>: use_krobot
<src/tools/krobot_record.{native,byte}>: use_krobot-interfaces
<src/tools/krobot_record.{native,byte}>: pkg_obus
<src/tools/krobot_record.{native,byte}>: pkg_lwt.unix
<src/tools/krobot_record.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_record.{native,byte}>: pkg_bitstring.syntax
+<src/tools/krobot_record.{native,byte}>: pkg_bitstring
# Executable krobot-plot
<src/tools/krobot_plot.{native,byte}>: use_krobot
<src/tools/krobot_plot.{native,byte}>: use_krobot-interfaces
@@ -140,6 +178,8 @@
<src/tools/krobot_plot.{native,byte}>: pkg_lwt.syntax
<src/tools/krobot_plot.{native,byte}>: pkg_lwt.glib
<src/tools/krobot_plot.{native,byte}>: pkg_cairo.lablgtk2
+<src/tools/krobot_plot.{native,byte}>: pkg_bitstring.syntax
+<src/tools/krobot_plot.{native,byte}>: pkg_bitstring
<src/tools/*.ml{,i}>: pkg_lwt.glib
<src/tools/*.ml{,i}>: pkg_cairo.lablgtk2
# Executable krobot-simulator
@@ -148,9 +188,13 @@
<src/tools/krobot_simulator.{native,byte}>: pkg_obus
<src/tools/krobot_simulator.{native,byte}>: pkg_lwt.unix
<src/tools/krobot_simulator.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_simulator.{native,byte}>: pkg_bitstring.syntax
+<src/tools/krobot_simulator.{native,byte}>: pkg_bitstring
<src/tools/*.ml{,i}>: use_krobot
<src/tools/*.ml{,i}>: use_krobot-interfaces
<src/tools/*.ml{,i}>: pkg_obus
<src/tools/*.ml{,i}>: pkg_lwt.unix
<src/tools/*.ml{,i}>: pkg_lwt.syntax
+<src/tools/*.ml{,i}>: pkg_bitstring.syntax
+<src/tools/*.ml{,i}>: pkg_bitstring
# OASIS_STOP
diff --git a/info/control2011/krobot-api.odocl b/info/control2011/krobot-api.odocl
index a6dd103..c450da7 100644
--- a/info/control2011/krobot-api.odocl
+++ b/info/control2011/krobot-api.odocl
@@ -1,9 +1,10 @@
# OASIS_START
-# DO NOT EDIT (digest: e58fdc99e81067f38e47e3f89ed4db14)
+# DO NOT EDIT (digest: 38c17322394dfbb39f5f25c95efe932e)
src/lib/Krobot_can
src/lib/Krobot_bus
src/lib/Krobot_message
src/lib/Krobot_service
src/lib/Krobot_geom
+src/lib/Krobot_planner
src/can/Krobot_can_bus
# OASIS_STOP
diff --git a/info/control2011/setup.ml b/info/control2011/setup.ml
index d6cccba..8d91fa9 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: b36c743e22b6b1a7b932599bea11f68b) *)
+(* DO NOT EDIT (digest: 0d24f30592bfc03631b0c362d4d27577) *)
(*
Regenerated by OASIS v0.2.0
Visit http://oasis.forge.ocamlcore.org for more information and
@@ -5106,7 +5106,9 @@ let setup_t =
FindlibPackage ("lwt.unix", None);
FindlibPackage ("lwt.syntax", None);
FindlibPackage ("obus", None);
- InternalLibrary "krobot-interfaces"
+ InternalLibrary "krobot-interfaces";
+ FindlibPackage ("bitstring", None);
+ FindlibPackage ("bitstring.syntax", None)
];
bs_build_tools = [ExternalTool "ocamlbuild"];
bs_c_sources = [];
@@ -5125,7 +5127,8 @@ let setup_t =
"Krobot_bus";
"Krobot_message";
"Krobot_service";
- "Krobot_geom"
+ "Krobot_geom";
+ "Krobot_planner"
];
lib_internal_modules = [];
lib_findlib_parent = None;
diff --git a/info/control2011/src/lib/META b/info/control2011/src/lib/META
index 8cab18d..47df438 100644
--- a/info/control2011/src/lib/META
+++ b/info/control2011/src/lib/META
@@ -1,8 +1,8 @@
# OASIS_START
-# DO NOT EDIT (digest: 6d4fe02a501933ad8464e70af7fcf3b1)
+# DO NOT EDIT (digest: 060bacace64817181263e75a71185b59)
version = "1.0"
description = "The [Kro]bot library"
-requires = "lwt.unix obus krobot.interfaces"
+requires = "lwt.unix obus krobot.interfaces bitstring"
archive(byte) = "krobot.cma"
archive(native) = "krobot.cmxa"
exists_if = "krobot.cma"
diff --git a/info/control2011/src/lib/krobot.mllib b/info/control2011/src/lib/krobot.mllib
index b2daa79..2d6a922 100644
--- a/info/control2011/src/lib/krobot.mllib
+++ b/info/control2011/src/lib/krobot.mllib
@@ -1,8 +1,9 @@
# OASIS_START
-# DO NOT EDIT (digest: d28f502a765cf0ae56f93e800363c356)
+# DO NOT EDIT (digest: a2debf72e72f6ae38ea62c8b35c3784d)
Krobot_can
Krobot_bus
Krobot_message
Krobot_service
Krobot_geom
+Krobot_planner
# OASIS_STOP
diff --git a/info/control2011/src/lib/krobot_geom.ml b/info/control2011/src/lib/krobot_geom.ml
index 34bf8a6..1f8a016 100644
--- a/info/control2011/src/lib/krobot_geom.ml
+++ b/info/control2011/src/lib/krobot_geom.ml
@@ -133,13 +133,8 @@ module Bezier = struct
let t3 = t2 *. t in
translate origin ((b.a *| t3) +| (b.b *| t2) +| (b.c *| t1) +| b.p)
- let fold_curves f initial vertices acc =
- let add_curve q r v1 v2 acc =
- (* Create the bezier curve. *)
- let curve = of_vertices q (translate q v1) (translate r v2) r in
-
- f curve acc
- in
+ let fold_vertices f initial vertices acc =
+ let add_vertices q r v1 v2 acc = f q (translate q v1) (translate r v2) r acc in
(* Compute cubic bezier curves. *)
let rec loop acc = function
@@ -149,12 +144,12 @@ module Bezier = struct
let _, v1 = tangents p q r and v2, _ = tangents q r s in
let v1 = v1 *| (min (distance p q) (distance q r) /. 2.)
and v2 = v2 *| (min (distance q r) (distance r s) /. 2.) in
- loop (add_curve q r v1 v2 acc) rest
+ loop (add_vertices q r v1 v2 acc) rest
| [p; q; r] ->
let _, v1 = tangents p q r and v2 = vector r q /| distance q r in
let v1 = v1 *| (min (distance p q) (distance q r) /. 2.)
and v2 = v2 *| (distance q r /. 2.) in
- add_curve q r v1 v2 acc
+ add_vertices q r v1 v2 acc
| _ ->
acc
in
@@ -164,13 +159,16 @@ module Bezier = struct
and v2, _ = tangents q r s in
let v1 = v1 *| (distance q r /. 2.)
and v2 = v2 *| (distance q r /. 2.) in
- loop (add_curve q r v1 v2 acc) vertices
+ loop (add_vertices q r v1 v2 acc) vertices
| [q; r] ->
let v1 = initial
and v2 = vector r q /| distance q r in
let v1 = v1 *| (distance q r /. 2.)
and v2 = v2 *| (distance q r /. 2.) in
- add_curve q r v1 v2 acc
+ add_vertices q r v1 v2 acc
| [_] | [] ->
acc
+
+ let fold_curves f initial vertices acc =
+ fold_vertices (fun p q r s acc -> f (of_vertices p q r s) acc) initial vertices acc
end
diff --git a/info/control2011/src/lib/krobot_geom.mli b/info/control2011/src/lib/krobot_geom.mli
index 2226508..c611dd6 100644
--- a/info/control2011/src/lib/krobot_geom.mli
+++ b/info/control2011/src/lib/krobot_geom.mli
@@ -68,4 +68,8 @@ module Bezier : sig
(** [fold_curves f vector vertices acc] folds [f] over the curve
passing through the given list of vertices. [vector] is the
initial direction vector. *)
+
+ val fold_vertices : (vertice -> vertice -> vertice -> vertice -> 'a -> 'a) -> vector -> vertice list -> 'a -> 'a
+ (** [fold_vertices f vector vertices acc] same as {!fold_curves}
+ but pass parameters instead of curves to [f]. *)
end
diff --git a/info/control2011/src/lib/krobot_message.ml b/info/control2011/src/lib/krobot_message.ml
index 9792c9d..7640a41 100644
--- a/info/control2011/src/lib/krobot_message.ml
+++ b/info/control2011/src/lib/krobot_message.ml
@@ -29,6 +29,7 @@ type t =
| Motor_status of bool * bool * bool * bool
| Motor_move of float * float * float
| Motor_turn of float * float * float
+ | Motor_bezier of float * float * float * float * float * float
| Motor_stop
| Odometry of float * float * float
| Set_odometry of float * float * float
@@ -96,6 +97,10 @@ let to_string = function
sprintf
"Motor_turn(%f, %f, %f)"
angle speed acc
+ | Motor_bezier(x, y, d1, d2, theta, v) ->
+ sprintf
+ "Motor_bezier(%f, %f, %f, %f, %f, %f)"
+ x y d1 d2 theta v
| Motor_stop ->
"Motor_stop"
| Odometry(x, y, theta) ->
@@ -201,6 +206,29 @@ let encode = function
~remote:false
~format:F29bits
~data
+ | Motor_bezier(x, y, d1, d2, theta, v) ->
+ let x = int_of_float (x *. 1000.)
+ and y = int_of_float (y *. 1000.)
+ and d1 = int_of_float (d1 *. 100.)
+ and d2 = int_of_float (d2 *. 100.)
+ and theta = int_of_float (theta *. 1000.) land 0b1111111111111;
+ and v = int_of_float (v *. 1000.) in
+ let data = Bitstring.string_of_bitstring
+ (BITSTRING{
+ x : 12 : littleendian, unsigned;
+ y : 12 : littleendian, unsigned;
+ d1 : 8 : littleendian, unsigned;
+ d2 : 8 : littleendian, unsigned;
+ theta : 13 : littleendian, unsigned;
+ v : 11 : littleendian, unsigned
+ })
+ in
+ frame
+ ~identifier:206
+ ~kind:Data
+ ~remote:false
+ ~format:F29bits
+ ~data
| Set_odometry(x, y, theta) ->
let data = String.create 6 in
put_sint16 data 0 (truncate (x *. 1000.));
@@ -351,6 +379,23 @@ let decode frame =
| 205 ->
Set_controller_mode
(get_uint8 frame.data 0 <> 0)
+ | 206 ->
+ (bitmatch Bitstring.bitstring_of_string frame.data with
+ | { x : 12 : littleendian;
+ y : 12 : littleendian;
+ d1 : 8 : littleendian;
+ d2 : 8 : littleendian;
+ theta : 13 : littleendian;
+ v : 11 : littleendian } ->
+ Motor_bezier(float x /. 1000.,
+ float y /. 1000.,
+ float d1 /. 100.,
+ float d2 /. 100.,
+ (if theta >= 4096 then
+ float (theta - 8192) /. 1000.
+ else
+ float theta /. 1000.),
+ float v /. 1000.))
| 301 ->
Beacon_position
(float (get_uint16 frame.data 0) /. 10000.,
diff --git a/info/control2011/src/lib/krobot_message.mli b/info/control2011/src/lib/krobot_message.mli
index fb116bf..991d2b4 100644
--- a/info/control2011/src/lib/krobot_message.mli
+++ b/info/control2011/src/lib/krobot_message.mli
@@ -44,6 +44,8 @@ type t =
- [speed] is in rad/s
- [acceleration] is in rad/s^2
*)
+ | Motor_bezier of float * float * float * float * float * float
+ (** [Motor_bezier(x_end, y_end, d1, d2, theta_end, v_end)] *)
| Motor_stop
(** Stops the motors. *)
| Odometry of float * float * float
diff --git a/info/control2011/src/services/krobot_service_planner.ml b/info/control2011/src/services/krobot_service_planner.ml
index cdda7e5..431a08d 100644
--- a/info/control2011/src/services/krobot_service_planner.ml
+++ b/info/control2011/src/services/krobot_service_planner.ml
@@ -16,6 +16,8 @@ open Krobot_config
open Krobot_message
open Krobot_interface_planner.Fr_krobot_Planner
+let section = Lwt_log.Section.make "krobot(planner)"
+
let pi = 4. *. atan 1.
let math_mod_float a b =
@@ -135,7 +137,7 @@ let go planner rotation_speed rotation_acceleration moving_speed moving_accelera
if S.value planner.moving then
return ()
else begin
- let rec loop () =
+(* let rec loop () =
match S.value planner.vertices with
| { x; y } :: rest ->
let sqr x = x *. x in
@@ -172,11 +174,42 @@ let go planner rotation_speed rotation_acceleration moving_speed moving_accelera
Lwt_log.warning_f "can not move to (%f, %f)" x y
| [] ->
return ()
- in
+ in*)
planner.set_moving true;
planner.mover <- (
try_lwt
- loop ()
+ let o, v = S.value planner.origin in
+ let params = List.rev (Bezier.fold_vertices (fun p q r s acc -> (p, q, r, s) :: acc) v (o :: S.value 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 () = Lwt_unix.sleep 2.0 in
+
+ (match S.value planner.vertices with
+ | _ :: l -> planner.set_vertices l
+ | [] -> ());
+ planner.set_origin (planner.position,
+ { vx = cos planner.orientation;
+ vy = sin planner.orientation });
+
+ return ()
+ )
+ params
+ with exn ->
+ Lwt_log.error_f ~section ~exn "failed to move"
finally
planner.set_moving false;
return ()
hooks/post-receive
--
krobot
|
