Menu
▾
▴
krobot-commits — code commits
You can subscribe to this list here.
| 2009 |
Jan
|
Feb
|
Mar
|
Apr
|
May
|
Jun
|
Jul
|
Aug
|
Sep
|
Oct
|
Nov
|
Dec
(13) |
|---|---|---|---|---|---|---|---|---|---|---|---|---|
| 2010 |
Jan
(50) |
Feb
(137) |
Mar
(84) |
Apr
(36) |
May
(100) |
Jun
(5) |
Jul
|
Aug
(4) |
Sep
(13) |
Oct
(1) |
Nov
(4) |
Dec
(22) |
| 2011 |
Jan
(4) |
Feb
(9) |
Mar
(113) |
Apr
(76) |
May
(31) |
Jun
(19) |
Jul
|
Aug
|
Sep
|
Oct
|
Nov
|
Dec
|
| 2012 |
Jan
(4) |
Feb
|
Mar
(2) |
Apr
(6) |
May
(19) |
Jun
|
Jul
|
Aug
|
Sep
|
Oct
(4) |
Nov
|
Dec
|
| 2013 |
Jan
|
Feb
|
Mar
(2) |
Apr
(22) |
May
(6) |
Jun
|
Jul
|
Aug
|
Sep
|
Oct
(1) |
Nov
|
Dec
|
| 2014 |
Jan
|
Feb
|
Mar
|
Apr
|
May
(1) |
Jun
|
Jul
|
Aug
|
Sep
|
Oct
|
Nov
|
Dec
|
|
From: Pierre C. <Ba...@us...> - 2012-05-09 22:26:12
|
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 74be0d4bea5261dc4196f4d9e3c6a9850ea9aa7e (commit)
from ca16117fcd01ec5ee344604771ff2afa9c90fe65 (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 74be0d4bea5261dc4196f4d9e3c6a9850ea9aa7e
Author: chicco <cha...@cr...>
Date: Thu May 10 00:24:06 2012 +0200
[krobot_dump_ax12] add option to ax12 recording
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/tools/krobot_ax12_control.ml b/info/control2011/src/tools/krobot_ax12_control.ml
index 853ba44..d50c324 100644
--- a/info/control2011/src/tools/krobot_ax12_control.ml
+++ b/info/control2011/src/tools/krobot_ax12_control.ml
@@ -41,6 +41,7 @@ let run = function
let rec go = function
| [] -> Lwt.return ()
| t::q ->
+ Printf.printf "go\n%!";
lwt () = run t in
go q
@@ -76,5 +77,7 @@ lwt () =
match !run_file with
| None -> go (List.rev !commands)
| Some f ->
+ Printf.printf "run file\n%!";
let actions = to_actions (read (open_in f)) in
+ Printf.printf "truc %i\n%!" (List.length actions);
go actions
diff --git a/info/control2011/src/tools/krobot_dump_ax12.ml b/info/control2011/src/tools/krobot_dump_ax12.ml
index 80044c9..6bb9873 100644
--- a/info/control2011/src/tools/krobot_dump_ax12.ml
+++ b/info/control2011/src/tools/krobot_dump_ax12.ml
@@ -42,9 +42,22 @@ let log () =
| _ -> ())
(Krobot_bus.recv bus))
+let ax12_id = ref []
+let ax12_delay = ref 0.02
+let spec =
+ [ "-id", Arg.Int (fun i -> ax12_id := i::(!ax12_id)), "id of the recorded ax12";
+ "-delay", Arg.Set_float ax12_delay, "delay between to points"; ]
+
+let msg = "record ax12 movement ax12"
+let message _ = Arg.usage spec msg; flush stdout; exit 0
+let () = Arg.parse spec message msg
+
let rec loop_request () =
- lwt () = Lwt_unix.sleep 0.01 in
- lwt () = Lwt_list.iter_s (fun i -> Krobot_bus.send bus (Unix.gettimeofday (), CAN (Info, Krobot_message.encode (Ax12_Request_State i)))) [1;2;3;4] in
+ (*lwt () = Lwt_unix.sleep 0.01 in*)
+ lwt () = Lwt_list.iter_s (fun i ->
+ lwt () = Krobot_bus.send bus (Unix.gettimeofday (), CAN (Info, Krobot_message.encode (Ax12_Request_State i))) in
+ Lwt_unix.sleep !ax12_delay)
+ !ax12_id in
loop_request ()
let t = loop_request ()
hooks/post-receive
--
krobot
|
|
From: Pierre C. <Ba...@us...> - 2012-05-09 21:55: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 ca16117fcd01ec5ee344604771ff2afa9c90fe65 (commit)
from d6e150cc4e13e4f5265536c0de7d1e097fd8c926 (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 ca16117fcd01ec5ee344604771ff2afa9c90fe65
Author: chicco <cha...@cr...>
Date: Wed May 9 23:54:42 2012 +0200
[krobot_dump_ax12] record all ax12
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/tools/krobot_dump_ax12.ml b/info/control2011/src/tools/krobot_dump_ax12.ml
index 8b95a5e..80044c9 100644
--- a/info/control2011/src/tools/krobot_dump_ax12.ml
+++ b/info/control2011/src/tools/krobot_dump_ax12.ml
@@ -44,7 +44,7 @@ let log () =
let rec loop_request () =
lwt () = Lwt_unix.sleep 0.01 in
- lwt () = Lwt_list.iter_s (fun i -> Krobot_bus.send bus (Unix.gettimeofday (), CAN (Info, Krobot_message.encode (Ax12_Request_State i)))) [2;3] in
+ lwt () = Lwt_list.iter_s (fun i -> Krobot_bus.send bus (Unix.gettimeofday (), CAN (Info, Krobot_message.encode (Ax12_Request_State i)))) [1;2;3;4] in
loop_request ()
let t = loop_request ()
hooks/post-receive
--
krobot
|
|
From: Pierre C. <Ba...@us...> - 2012-05-09 21:17:33
|
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 d6e150cc4e13e4f5265536c0de7d1e097fd8c926 (commit)
from 5734668f606f94fd06962fae0aeb046bf4e7cf98 (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 d6e150cc4e13e4f5265536c0de7d1e097fd8c926
Author: chicco <cha...@cr...>
Date: Wed May 9 23:17:01 2012 +0200
[krobot_config] update robot size
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/lib/krobot_config.ml b/info/control2011/src/lib/krobot_config.ml
index b4d3946..749977a 100644
--- a/info/control2011/src/lib/krobot_config.ml
+++ b/info/control2011/src/lib/krobot_config.ml
@@ -13,8 +13,8 @@ let world_height = 2.1
let world_width = 3.
let robot_size = 0.26
let wheels_diameter = 0.098
-let wheels_distance = 0.150
-let wheels_position = robot_size /. 2.
+let wheels_distance = 0.224
+let wheels_position = robot_size *. 0.2
let rotary_beacon_index_pos = (4. *. atan 1.) /. 2. (* left side *)
let object_radius = 0.1
let border_safety_distance = sqrt (sqr robot_size /. 2.) +. 0.05
hooks/post-receive
--
krobot
|
|
From: Pierre C. <Ba...@us...> - 2012-05-09 20:59:06
|
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 5734668f606f94fd06962fae0aeb046bf4e7cf98 (commit)
from 2f4db51988fbdeabce5d24892650c2db962d73bb (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 5734668f606f94fd06962fae0aeb046bf4e7cf98
Author: chicco <cha...@cr...>
Date: Wed May 9 22:46:31 2012 +0200
[krobot_vm] stop the robot if it is too far from the ghost
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/lib/krobot_message.mli b/info/control2011/src/lib/krobot_message.mli
index f111d23..a071772 100644
--- a/info/control2011/src/lib/krobot_message.mli
+++ b/info/control2011/src/lib/krobot_message.mli
@@ -79,7 +79,9 @@ type t =
- [a_rad_max] in m/s^2 *)
| Odometry of float * float * float
(** [Odometry(x, y, theta)] the position of the robot on the
- table. *)
+ table.
+ - [x, y] in m
+ - [theta] in rad *)
| Odometry_ghost of float * float * float * int * bool
(** [Odometry_ghost(x, y, theta, following)]. [following] is
[true] iff the robot is following the ghost. *)
diff --git a/info/control2011/src/tools/krobot_vm.ml b/info/control2011/src/tools/krobot_vm.ml
index a613692..9a67034 100644
--- a/info/control2011/src/tools/krobot_vm.ml
+++ b/info/control2011/src/tools/krobot_vm.ml
@@ -48,6 +48,9 @@ type robot = {
mutable position : vertice;
(* The position of the robot on the table. *)
+ mutable ghost_position : vertice;
+ (* The position of the ghost on the table. *)
+
mutable orientation : float;
(* The orientation of the robot. *)
@@ -100,6 +103,7 @@ let handle_message robot (timestamp, message) =
robot.orientation <- math_mod_float theta (2. *. pi)
| Odometry_ghost(x, y, theta, u, following) ->
+ robot.ghost_position <- { x; y };
robot.curve_parameter <- u;
robot.moving <- following
@@ -389,6 +393,15 @@ let run robot =
()
| Some curve ->
try
+ (* Check that the robot is not too far from the ghost.
+ if it is then stop brutaly:
+ TODO do something interesting after the stop: retry what we were doing *)
+ if distance robot.ghost_position robot.position > 0.1 then begin
+ ignore (Lwt_log.info_f "Robot too far from the ghost");
+ robot.strategy <- [Stop];
+ reset robot;
+ raise Exit
+ end;
(* Check that there is no colision between the current
position and the end of the current curve. *)
for i = robot.curve_parameter to 255 do
@@ -457,6 +470,7 @@ lwt () =
change_strategy = None;
append_strategy = None;
position = { x = 0.; y = 0. };
+ ghost_position = { x = 0.; y = 0. };
orientation = 0.;
objects = [];
moving = false;
diff --git a/info/vision/camlcv/src/cv_caml.c b/info/vision/camlcv/src/cv_caml.c
index 7b70980..42cb9cd 100644
--- a/info/vision/camlcv/src/cv_caml.c
+++ b/info/vision/camlcv/src/cv_caml.c
@@ -963,21 +963,20 @@ CAMLprim value ocaml_cvInitUndistortMap( value vcameraMatrix, value vdistCoeffs,
{
CAMLparam4( vcameraMatrix, vdistCoeffs, vmap1, vmap2 );
- /*
CvMat cameraMatrix = CvMat_val(vcameraMatrix);
CvMat distCoeffs = CvMat_val(vdistCoeffs);
- */
- /*
+
cvInitUndistortMap(&cameraMatrix,
&distCoeffs,
Image_val(vmap1)->image,
Image_val(vmap2)->image);
- */
+
+ /*
cvInitUndistortMap(NULL,
NULL,
NULL,
NULL);
-
+ */
CAMLreturn(Val_unit);
}
hooks/post-receive
--
krobot
|
|
From: Pierre C. <Ba...@us...> - 2012-05-08 21:15:59
|
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 2f4db51988fbdeabce5d24892650c2db962d73bb (commit)
via e6ba7806e1a53d4bde4ea704f6b6aa20cd35859c (commit)
via a1bf36f03c3dc667452b5e8ccc2155b2a6b3ab26 (commit)
from b64fde9d0632479d027d3ebba220d7de086b645a (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 2f4db51988fbdeabce5d24892650c2db962d73bb
Author: chicco <cha...@cr...>
Date: Tue May 8 23:14:31 2012 +0200
[camlcv] some more
commit e6ba7806e1a53d4bde4ea704f6b6aa20cd35859c
Author: chicco <cha...@cr...>
Date: Sat May 5 18:25:00 2012 +0200
[camlcv] Update camlcv
commit a1bf36f03c3dc667452b5e8ccc2155b2a6b3ab26
Author: chicco <cha...@cr...>
Date: Sat May 5 15:55:27 2012 +0200
[krobot_ax12_clean] tool to clean ax12 sequences, krobot_ax12_control can play sequances
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/_oasis b/info/control2011/_oasis
index 7409387..587f26e 100644
--- a/info/control2011/_oasis
+++ b/info/control2011/_oasis
@@ -234,6 +234,12 @@ Executable "krobot-dump-ax12"
MainIs: krobot_dump_ax12.ml
BuildDepends: krobot.can, lwt.syntax
+Executable "krobot-ax12-clean"
+ Path: src/tools
+ Install: true
+ CompiledObject: best
+ MainIs: krobot_ax12_clean.ml
+
# +-------------------------------------------------------------------+
# | Examples |
# +-------------------------------------------------------------------+
diff --git a/info/control2011/_tags b/info/control2011/_tags
index 3049ac4..8e8eb2b 100644
--- a/info/control2011/_tags
+++ b/info/control2011/_tags
@@ -2,7 +2,7 @@
<src/interfaces/*.ml>: -syntax_camlp4o
# OASIS_START
-# DO NOT EDIT (digest: 77bb0a0cf22171b21400c7924cce86fe)
+# DO NOT EDIT (digest: c7b4424d285a5d14617154af42598aa7)
# Ignore VCS directories, you can use the same kind of rule outside
# OASIS_START/STOP if you want to exclude directories that contains
# useless stuff for the build process
@@ -110,6 +110,7 @@
<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_lwt.react
+# Executable krobot-ax12-clean
# Executable krobot-hub
<src/tools/krobot_hub.{native,byte}>: pkg_lwt.unix
<src/tools/krobot_hub.{native,byte}>: pkg_lwt.syntax
diff --git a/info/control2011/setup.ml b/info/control2011/setup.ml
index ef14fdf..a2b32ae 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: 3f1f69c4008e4511460ad8227132b513) *)
+(* DO NOT EDIT (digest: 8dd92996e901c6afd5668c8d5fed679a) *)
(*
Regenerated by OASIS v0.3.0~rc3
Visit http://oasis.forge.ocamlcore.org for more information and
@@ -5724,6 +5724,32 @@ let setup_t =
});
Executable
({
+ cs_name = "krobot-ax12-clean";
+ 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 = [];
+ 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_ax12_clean.ml";
+ });
+ Executable
+ ({
cs_name = "krobot-hub";
cs_data = PropList.Data.create ();
cs_plugin_data = [];
@@ -6165,13 +6191,13 @@ let setup_t =
};
oasis_fn = Some "_oasis";
oasis_version = "0.3.0~rc3";
- oasis_digest = Some "q\1674\179\017\0115F\190\024gsXI=$";
+ oasis_digest = Some "[\188\237\193\021xw\196\204\204<Y\157o\194F";
oasis_exec = None;
oasis_setup_args = [];
};;
let setup () = BaseSetup.setup setup_t;;
-# 6176 "setup.ml"
+# 6202 "setup.ml"
(* OASIS_STOP *)
let () = setup ();;
diff --git a/info/control2011/src/tools/krobot_ax12_clean.ml b/info/control2011/src/tools/krobot_ax12_clean.ml
new file mode 100644
index 0000000..62b5030
--- /dev/null
+++ b/info/control2011/src/tools/krobot_ax12_clean.ml
@@ -0,0 +1,105 @@
+open Arg
+
+type ax12_info =
+ { id : int;
+ pos : float;
+ speed : float;
+ torque : float;
+ time : float }
+
+let fscan_info ic =
+ Scanf.fscanf ic "id %i,pos %f, speed %f, torque %f, time %f\n"
+ (fun id pos speed torque time -> { id; pos; speed; torque; time; })
+
+let read ic =
+ let rec aux acc =
+ try
+ let v = fscan_info ic in
+ aux (v::acc)
+ with
+ | End_of_file
+ | Scanf.Scan_failure _ -> List.rev acc
+ in
+ aux []
+
+module IntMap = Map.Make(struct type t = int let compare = compare end)
+
+let separate l =
+ let f map v =
+ if IntMap.mem v.id map
+ then let r = IntMap.find v.id map in
+ r := v :: !r;
+ map
+ else IntMap.add v.id (ref [v]) map in
+ IntMap.map (fun r -> List.rev !r) (List.fold_left f IntMap.empty l)
+
+let soft window l =
+ let rec aux (l,v_win) v =
+ let v_win = List.filter (fun t -> v.time -. t.time <= window) l in
+ let v_win = v::v_win in
+ let mean = (List.fold_left (fun s v -> s +. v.pos) 0. v_win) /. float (List.length v_win) in
+ { v with pos = mean }::l, v_win in
+ List.rev (fst (List.fold_left aux ([],[]) l))
+
+let remove_useless remove_below l =
+ let rec aux prev acc = function
+ | [] -> List.rev acc
+ | t::q ->
+ if abs_float (t.pos -. prev.pos) <= remove_below
+ then aux prev acc q
+ else aux t (t::acc) q
+ in
+ aux (List.hd l) [List.hd l] (List.tl l)
+
+let add_speed speed_coef l =
+ let rec aux prev acc = function
+ | [] -> List.rev acc
+ | t::q ->
+ let dt = t.time -. prev.time in
+ let dpos = t.pos -. prev.pos in
+ let speed = (dpos /. dt) *. speed_coef in
+ if abs_float speed > 200.
+ then aux prev acc q
+ else aux t ({ t with speed }::acc) q
+ in
+ aux (List.hd l) [List.hd l] (List.tl l)
+
+let window = 0.1
+let remove_below = 1.
+let speed_coef = 1. (* ratio ax12 speed -> rad /. second *)
+
+let prepare l =
+ let l = List.map (fun v -> { v with speed = 0.; torque = 0. }) l in
+ let l = List.sort (fun v1 v2 -> compare v1.time v2.time) l in
+ let first_time = (List.hd l).time in
+ let l = List.map (fun v -> { v with time = v.time -. first_time }) l in
+ let map = separate l in
+ let map = IntMap.map (soft window) map in
+ let map = IntMap.map (remove_useless remove_below) map in
+ let map = IntMap.map (add_speed speed_coef) map in
+ let l = List.flatten (IntMap.fold (fun _ l acc -> l::acc) map []) in
+ let l = List.sort (fun v1 v2 -> compare v1.time v2.time) l in
+ l
+
+let map_speed l c =
+ let f v = { v with speed = v.speed *. c; time = v.time /. c } in
+ List.map f l
+
+let print_info { id; pos; speed; torque; time } =
+ Printf.printf "id %i,pos %f, speed %f, torque %f, time %f\n%!" id pos speed torque time
+
+let file = ref None
+let speed = ref 1.
+
+let spec = ["-s", Set_float speed, "speed";]
+let msg = "clean ax12 command sequence"
+let message _ = usage spec msg; flush stdout; exit 0
+let () = Arg.parse spec (fun s -> file := Some s) msg
+let () =
+ match !file with
+ | None -> message ()
+ | Some f ->
+ let l = read (open_in f) in
+ let l2 = prepare l in
+ let l3 = map_speed l2 !speed in
+ List.iter print_info l3
diff --git a/info/control2011/src/tools/krobot_ax12_control.ml b/info/control2011/src/tools/krobot_ax12_control.ml
index b281c36..853ba44 100644
--- a/info/control2011/src/tools/krobot_ax12_control.ml
+++ b/info/control2011/src/tools/krobot_ax12_control.ml
@@ -6,6 +6,8 @@ type action =
| Sleep of float
| Do of Krobot_message.t
+let run_file = ref None
+
let ax12_id = ref 0
let ax12_speed = ref 1
let commands = ref []
@@ -24,11 +26,13 @@ let spec =
"-t", Bool add_torque, "-torque";
"-state", Unit add_request, "request the ax12 state";
"-sleep", Float add_sleep, "sleep";
+ "-file", String (fun s -> run_file := Some s), "run sequence from file";
]
+
let msg = "do things with ax12"
let message _ = usage spec msg; flush stdout; exit 0
let () = Arg.parse spec message msg
-let () = if !commands = [] then message ()
+let () = if !commands = [] && !run_file = None then message ()
lwt bus = Krobot_bus.get ()
let run = function
| Sleep t -> Lwt_unix.sleep t
@@ -40,4 +44,37 @@ let rec go = function
lwt () = run t in
go q
-lwt () = go (List.rev !commands)
+type ax12_info =
+ { id : int;
+ pos : float;
+ speed : float;
+ torque : float;
+ time : float }
+
+let fscan_info ic =
+ Scanf.fscanf ic "id %i,pos %f, speed %f, torque %f, time %f\n"
+ (fun id pos speed torque time -> { id; pos; speed; torque; time; })
+
+let read ic =
+ let rec aux acc =
+ try
+ let v = fscan_info ic in
+ aux (v::acc)
+ with
+ | End_of_file
+ | Scanf.Scan_failure _ -> List.rev acc
+ in
+ aux []
+
+let to_actions l =
+ let f (t,l) v =
+ v.time, ( Do (Ax12_Goto (v.id, int_of_float v.pos, int_of_float v.speed)) ) :: (Sleep (v.time -. t)) :: l in
+ let _, l = List.fold_left f (0.,[]) l in
+ List.rev l
+
+lwt () =
+ match !run_file with
+ | None -> go (List.rev !commands)
+ | Some f ->
+ let actions = to_actions (read (open_in f)) in
+ go actions
diff --git a/info/control2011/src/tools/krobot_vm.ml b/info/control2011/src/tools/krobot_vm.ml
index d4dc507..a613692 100644
--- a/info/control2011/src/tools/krobot_vm.ml
+++ b/info/control2011/src/tools/krobot_vm.ml
@@ -198,6 +198,13 @@ type effect =
| Send of Krobot_message.t
(* Send a message. *)
+let string_of_test = function
+ | `Eq -> "Eq"
+ | `Gt -> "Gt"
+ | `Ge -> "Ge"
+ | `Lt -> "Lt"
+ | `Le -> "Le"
+
(* [exec robot actions] searches for the first action to execute in a
tree of actions and returns a new tree of actions and an effect. *)
let rec exec robot actions =
@@ -216,7 +223,8 @@ let rec exec robot actions =
(actions, Wait)
| Wait_for_moving state :: rest ->
if robot.moving = state then
- exec robot rest
+ (ignore (Lwt_log.info_f "Wait_for_moving %b done" state);
+ exec robot rest)
else
(actions, Wait)
| Wait_for_odometry(test, value) :: rest ->
@@ -226,7 +234,9 @@ let rec exec robot actions =
| `Ge -> robot.curve_parameter >= value
| `Lt -> robot.curve_parameter < value
| `Le -> robot.curve_parameter <= value) then
- exec robot rest
+ (ignore (Lwt_log.info_f "Wait_for_odometry %i %s %i done"
+ robot.curve_parameter (string_of_test test) value);
+ exec robot rest)
else
(actions, Wait)
| Wait_for t :: rest ->
@@ -252,6 +262,7 @@ let rec exec robot actions =
exec robot rest
end
| Follow_path vertices :: rest -> begin
+ ignore (Lwt_log.info_f "Follow_path %i vertices" (List.length vertices));
(* Compute bezier curves. *)
let vector = { vx = cos robot.orientation; vy = sin robot.orientation } in
let curves = List.rev (Bezier.fold_vertices (fun sign p q r s acc -> (sign, p, q, r, s) :: acc) vector (robot.position :: vertices) []) in
@@ -262,6 +273,7 @@ let rec exec robot actions =
| [] ->
[Wait_for_moving false]
| [(sign, p, q, r, s)] ->
+ ignore (Lwt_log.info_f "add last %f %f" p.x p.y);
[
(* Wait for the odometry to reach the middle of the
current trajectory. *)
@@ -278,6 +290,7 @@ let rec exec robot actions =
Set_curve None;
]
| (sign, p, q, r, s) :: rest ->
+ ignore (Lwt_log.info_f "add middle %f %f" p.x p.y);
Wait_for_odometry(`Ge, 128)
:: Bezier(sign, p, q, r, s, 0.5)
:: Wait_for_odometry(`Lt, 128)
@@ -291,15 +304,19 @@ let rec exec robot actions =
exec robot (Node [
Set_curve(Some(Bezier.of_vertices p q r s));
Bezier(sign, p, q, r, s, 0.01);
+ Wait_for_odometry(`Le, 128);
+ Wait_for_odometry(`Ge, 128);
Wait_for_moving false;
Set_curve None;
] :: rest)
| (sign, p, q, r, s) :: curves ->
exec robot (Node(Set_curve(Some(Bezier.of_vertices p q r s))
:: Bezier(sign, p, q, r, s, 0.5)
+ :: Wait_for_odometry(`Le, 128)
:: loop curves) :: rest)
end
| Bezier(sign, p, q, r, s, v_end) :: rest ->
+ ignore (Lwt_log.info "Bezier");
(* Compute parameters. *)
let d1 = sign *. distance p q and d2 = distance r s in
let v = vector r s in
@@ -420,6 +437,10 @@ lwt () =
(* Display all informative messages. *)
Lwt_log.append_rule "*" Lwt_log.Info;
+ Lwt_log.default :=
+ Lwt_log.channel ~template:"$(name): $(section): $(message) $(date) $(milliseconds)"
+ ~close_mode:`Keep ~channel:Lwt_io.stderr ();
+
(* Open the krobot bus. *)
lwt bus = Krobot_bus.get () in
diff --git a/info/vision/camlcv/_oasis b/info/vision/camlcv/_oasis
index edf16a3..e586f57 100644
--- a/info/vision/camlcv/_oasis
+++ b/info/vision/camlcv/_oasis
@@ -25,8 +25,9 @@ Description:
Library "cv"
Path: src/
Modules:
- OpenCv
+ CvCore,
+ CvHighGui
CSources:
cv_caml.c
XMETADescription: Core Opencv
- BuildDepends:
+ BuildDepends: bigarray
diff --git a/info/vision/camlcv/_tags b/info/vision/camlcv/_tags
index d6cef63..b018bdb 100644
--- a/info/vision/camlcv/_tags
+++ b/info/vision/camlcv/_tags
@@ -1,3 +1,22 @@
# OASIS_START
+# DO NOT EDIT (digest: b8f60d120ef9d7618b2ed02d64b8ef03)
+# Ignore VCS directories, you can use the same kind of rule outside
+# OASIS_START/STOP if you want to exclude directories that contains
+# useless stuff for the build process
+<**/.svn>: -traverse
+<**/.svn>: not_hygienic
+".bzr": -traverse
+".bzr": not_hygienic
+".hg": -traverse
+".hg": not_hygienic
+".git": -traverse
+".git": not_hygienic
+"_darcs": -traverse
+"_darcs": not_hygienic
+# Library cv
+"src/cv.cmxs": use_cv
+<src/cv.{cma,cmxa}>: use_libcv_stubs
+<src/*.ml{,i}>: pkg_bigarray
+"src/cv_caml.c": pkg_bigarray
# OASIS_STOP
<src/*>: use_C_opencv
diff --git a/info/vision/camlcv/myocamlbuild.ml b/info/vision/camlcv/myocamlbuild.ml
index 1b02616..ed0acd9 100644
--- a/info/vision/camlcv/myocamlbuild.ml
+++ b/info/vision/camlcv/myocamlbuild.ml
@@ -1,4 +1,561 @@
(* OASIS_START *)
+(* DO NOT EDIT (digest: 4691146961f957a78ce95a45557aba80) *)
+module OASISGettext = struct
+# 21 "/media/data/ocaml/oasis/src/oasis/OASISGettext.ml"
+
+ let ns_ str =
+ str
+
+ let s_ str =
+ str
+
+ let f_ (str : ('a, 'b, 'c, 'd) format4) =
+ str
+
+ let fn_ fmt1 fmt2 n =
+ if n = 1 then
+ fmt1^^""
+ else
+ fmt2^^""
+
+ let init =
+ []
+
+end
+
+module OASISExpr = struct
+# 21 "/media/data/ocaml/oasis/src/oasis/OASISExpr.ml"
+
+
+
+ open OASISGettext
+
+ type test = string
+
+ type flag = string
+
+ type t =
+ | EBool of bool
+ | ENot of t
+ | EAnd of t * t
+ | EOr of t * t
+ | EFlag of flag
+ | ETest of test * string
+
+
+ type 'a choices = (t * 'a) list
+
+ let eval var_get t =
+ let rec eval' =
+ function
+ | EBool b ->
+ b
+
+ | ENot e ->
+ not (eval' e)
+
+ | EAnd (e1, e2) ->
+ (eval' e1) && (eval' e2)
+
+ | EOr (e1, e2) ->
+ (eval' e1) || (eval' e2)
+
+ | EFlag nm ->
+ let v =
+ var_get nm
+ in
+ assert(v = "true" || v = "false");
+ (v = "true")
+
+ | ETest (nm, vl) ->
+ let v =
+ var_get nm
+ in
+ (v = vl)
+ in
+ eval' t
+
+ let choose ?printer ?name var_get lst =
+ let rec choose_aux =
+ function
+ | (cond, vl) :: tl ->
+ if eval var_get cond then
+ vl
+ else
+ choose_aux tl
+ | [] ->
+ let str_lst =
+ if lst = [] then
+ s_ "<empty>"
+ else
+ String.concat
+ (s_ ", ")
+ (List.map
+ (fun (cond, vl) ->
+ match printer with
+ | Some p -> p vl
+ | None -> s_ "<no printer>")
+ lst)
+ in
+ match name with
+ | Some nm ->
+ failwith
+ (Printf.sprintf
+ (f_ "No result for the choice list '%s': %s")
+ nm str_lst)
+ | None ->
+ failwith
+ (Printf.sprintf
+ (f_ "No result for a choice list: %s")
+ str_lst)
+ in
+ choose_aux (List.rev lst)
+
+end
+
+
+module BaseEnvLight = struct
+# 21 "/media/data/ocaml/oasis/src/base/BaseEnvLight.ml"
+
+ module MapString = Map.Make(String)
+
+ type t = string MapString.t
+
+ let default_filename =
+ Filename.concat
+ (Sys.getcwd ())
+ "setup.data"
+
+ let load ?(allow_empty=false) ?(filename=default_filename) () =
+ if Sys.file_exists filename then
+ begin
+ let chn =
+ open_in_bin filename
+ in
+ let st =
+ Stream.of_channel chn
+ in
+ let line =
+ ref 1
+ in
+ let st_line =
+ Stream.from
+ (fun _ ->
+ try
+ match Stream.next st with
+ | '\n' -> incr line; Some '\n'
+ | c -> Some c
+ with Stream.Failure -> None)
+ in
+ let lexer =
+ Genlex.make_lexer ["="] st_line
+ in
+ let rec read_file mp =
+ match Stream.npeek 3 lexer with
+ | [Genlex.Ident nm; Genlex.Kwd "="; Genlex.String value] ->
+ Stream.junk lexer;
+ Stream.junk lexer;
+ Stream.junk lexer;
+ read_file (MapString.add nm value mp)
+ | [] ->
+ mp
+ | _ ->
+ failwith
+ (Printf.sprintf
+ "Malformed data file '%s' line %d"
+ filename !line)
+ in
+ let mp =
+ read_file MapString.empty
+ in
+ close_in chn;
+ mp
+ end
+ else if allow_empty then
+ begin
+ MapString.empty
+ end
+ else
+ begin
+ failwith
+ (Printf.sprintf
+ "Unable to load environment, the file '%s' doesn't exist."
+ filename)
+ end
+
+ let var_get name env =
+ let rec var_expand str =
+ let buff =
+ Buffer.create ((String.length str) * 2)
+ in
+ Buffer.add_substitute
+ buff
+ (fun var ->
+ try
+ var_expand (MapString.find var env)
+ with Not_found ->
+ failwith
+ (Printf.sprintf
+ "No variable %s defined when trying to expand %S."
+ var
+ str))
+ str;
+ Buffer.contents buff
+ in
+ var_expand (MapString.find name env)
+
+ let var_choose lst env =
+ OASISExpr.choose
+ (fun nm -> var_get nm env)
+ lst
+end
+
+
+module MyOCamlbuildFindlib = struct
+# 21 "/media/data/ocaml/oasis/src/plugins/ocamlbuild/MyOCamlbuildFindlib.ml"
+
+ (** OCamlbuild extension, copied from
+ * http://brion.inria.fr/gallium/index.php/Using_ocamlfind_with_ocamlbuild
+ * by N. Pouillard and others
+ *
+ * Updated on 2009/02/28
+ *
+ * Modified by Sylvain Le Gall
+ *)
+ open Ocamlbuild_plugin
+
+ (* these functions are not really officially exported *)
+ let run_and_read =
+ Ocamlbuild_pack.My_unix.run_and_read
+
+ let blank_sep_strings =
+ Ocamlbuild_pack.Lexers.blank_sep_strings
+
+ let split s ch =
+ let x =
+ ref []
+ in
+ let rec go s =
+ let pos =
+ String.index s ch
+ in
+ x := (String.before s pos)::!x;
+ go (String.after s (pos + 1))
+ in
+ try
+ go s
+ with Not_found -> !x
+
+ let split_nl s = split s '\n'
+
+ let before_space s =
+ try
+ String.before s (String.index s ' ')
+ with Not_found -> s
+
+ (* this lists all supported packages *)
+ let find_packages () =
+ List.map before_space (split_nl & run_and_read "ocamlfind list")
+
+ (* this is supposed to list available syntaxes, but I don't know how to do it. *)
+ let find_syntaxes () = ["camlp4o"; "camlp4r"]
+
+ (* ocamlfind command *)
+ let ocamlfind x = S[A"ocamlfind"; x]
+
+ let dispatch =
+ function
+ | Before_options ->
+ (* by using Before_options one let command line options have an higher priority *)
+ (* on the contrary using After_options will guarantee to have the higher priority *)
+ (* override default commands by ocamlfind ones *)
+ Options.ocamlc := ocamlfind & A"ocamlc";
+ Options.ocamlopt := ocamlfind & A"ocamlopt";
+ Options.ocamldep := ocamlfind & A"ocamldep";
+ Options.ocamldoc := ocamlfind & A"ocamldoc";
+ Options.ocamlmktop := ocamlfind & A"ocamlmktop"
+
+ | After_rules ->
+
+ (* When one link an OCaml library/binary/package, one should use -linkpkg *)
+ flag ["ocaml"; "link"; "program"] & A"-linkpkg";
+
+ (* For each ocamlfind package one inject the -package option when
+ * compiling, computing dependencies, generating documentation and
+ * linking. *)
+ List.iter
+ begin fun pkg ->
+ flag ["ocaml"; "compile"; "pkg_"^pkg] & S[A"-package"; A pkg];
+ flag ["ocaml"; "ocamldep"; "pkg_"^pkg] & S[A"-package"; A pkg];
+ flag ["ocaml"; "doc"; "pkg_"^pkg] & S[A"-package"; A pkg];
+ flag ["ocaml"; "link"; "pkg_"^pkg] & S[A"-package"; A pkg];
+ flag ["ocaml"; "infer_interface"; "pkg_"^pkg] & S[A"-package"; A pkg];
+ end
+ (find_packages ());
+
+ (* Like -package but for extensions syntax. Morover -syntax is useless
+ * when linking. *)
+ List.iter begin fun syntax ->
+ flag ["ocaml"; "compile"; "syntax_"^syntax] & S[A"-syntax"; A syntax];
+ flag ["ocaml"; "ocamldep"; "syntax_"^syntax] & S[A"-syntax"; A syntax];
+ flag ["ocaml"; "doc"; "syntax_"^syntax] & S[A"-syntax"; A syntax];
+ flag ["ocaml"; "infer_interface"; "syntax_"^syntax] & S[A"-syntax"; A syntax];
+ end (find_syntaxes ());
+
+ (* The default "thread" tag is not compatible with ocamlfind.
+ * Indeed, the default rules add the "threads.cma" or "threads.cmxa"
+ * options when using this tag. When using the "-linkpkg" option with
+ * ocamlfind, this module will then be added twice on the command line.
+ *
+ * To solve this, one approach is to add the "-thread" option when using
+ * the "threads" package using the previous plugin.
+ *)
+ flag ["ocaml"; "pkg_threads"; "compile"] (S[A "-thread"]);
+ flag ["ocaml"; "pkg_threads"; "doc"] (S[A "-I"; A "+threads"]);
+ flag ["ocaml"; "pkg_threads"; "link"] (S[A "-thread"]);
+ flag ["ocaml"; "pkg_threads"; "infer_interface"] (S[A "-thread"])
+
+ | _ ->
+ ()
+
+end
+
+module MyOCamlbuildBase = struct
+# 21 "/media/data/ocaml/oasis/src/plugins/ocamlbuild/MyOCamlbuildBase.ml"
+
+ (** Base functions for writing myocamlbuild.ml
+ @author Sylvain Le Gall
+ *)
+
+
+
+ open Ocamlbuild_plugin
+ module OC = Ocamlbuild_pack.Ocaml_compiler
+
+ type dir = string
+ type file = string
+ type name = string
+ type tag = string
+
+# 56 "/media/data/ocaml/oasis/src/plugins/ocamlbuild/MyOCamlbuildBase.ml"
+
+ type t =
+ {
+ lib_ocaml: (name * dir list) list;
+ lib_c: (name * dir * file list) list;
+ flags: (tag list * (spec OASISExpr.choices)) list;
+ (* Replace the 'dir: include' from _tags by a precise interdepends in
+ * directory.
+ *)
+ includes: (dir * dir list) list;
+ }
+
+ let env_filename =
+ Pathname.basename
+ BaseEnvLight.default_filename
+
+ let dispatch_combine lst =
+ fun e ->
+ List.iter
+ (fun dispatch -> dispatch e)
+ lst
+
+ let tag_libstubs nm =
+ "use_lib"^nm^"_stubs"
+
+ let nm_libstubs nm =
+ nm^"_stubs"
+
+ let dispatch t e =
+ let env =
+ BaseEnvLight.load
+ ~filename:env_filename
+ ~allow_empty:true
+ ()
+ in
+ match e with
+ | Before_options ->
+ let no_trailing_dot s =
+ if String.length s >= 1 && s.[0] = '.' then
+ String.sub s 1 ((String.length s) - 1)
+ else
+ s
+ in
+ List.iter
+ (fun (opt, var) ->
+ try
+ opt := no_trailing_dot (BaseEnvLight.var_get var env)
+ with Not_found ->
+ Printf.eprintf "W: Cannot get variable %s" var)
+ [
+ Options.ext_obj, "ext_obj";
+ Options.ext_lib, "ext_lib";
+ Options.ext_dll, "ext_dll";
+ ]
+
+ | Before_rules ->
+ (* TODO: move this into its own file and conditionnaly include it, if
+ * needed.
+ *)
+ (* OCaml cmxs rules: cmxs available in ocamlopt but not ocamlbuild.
+ Copied from ocaml_specific.ml in ocamlbuild sources. *)
+ let has_native_dynlink =
+ try
+ bool_of_string (BaseEnvLight.var_get "native_dynlink" env)
+ with Not_found ->
+ false
+ in
+ if has_native_dynlink && String.sub Sys.ocaml_version 0 4 = "3.11" then
+ begin
+ let ext_lib = !Options.ext_lib in
+ let ext_obj = !Options.ext_obj in
+ let ext_dll = !Options.ext_dll in
+ let x_o = "%"-.-ext_obj in
+ let x_a = "%"-.-ext_lib in
+ let x_dll = "%"-.-ext_dll in
+ let x_p_o = "%.p"-.-ext_obj in
+ let x_p_a = "%.p"-.-ext_lib in
+ let x_p_dll = "%.p"-.-ext_dll in
+
+ rule "ocaml: mldylib & p.cmx* & p.o* -> p.cmxs & p.so"
+ ~tags:["ocaml"; "native"; "profile"; "shared"; "library"]
+ ~prods:["%.p.cmxs"; x_p_dll]
+ ~dep:"%.mldylib"
+ (OC.native_profile_shared_library_link_mldylib
+ "%.mldylib" "%.p.cmxs");
+
+ rule "ocaml: mldylib & cmx* & o* -> cmxs & so"
+ ~tags:["ocaml"; "native"; "shared"; "library"]
+ ~prods:["%.cmxs"; x_dll]
+ ~dep:"%.mldylib"
+ (OC.native_shared_library_link_mldylib
+ "%.mldylib" "%.cmxs");
+
+ rule "ocaml: p.cmx & p.o -> p.cmxs & p.so"
+ ~tags:["ocaml"; "native"; "profile"; "shared"; "library"]
+ ~prods:["%.p.cmxs"; x_p_dll]
+ ~deps:["%.p.cmx"; x_p_o]
+ (OC.native_shared_library_link ~tags:["profile"]
+ "%.p.cmx" "%.p.cmxs");
+
+ rule "ocaml: p.cmxa & p.a -> p.cmxs & p.so"
+ ~tags:["ocaml"; "native"; "profile"; "shared"; "library"]
+ ~prods:["%.p.cmxs"; x_p_dll]
+ ~deps:["%.p.cmxa"; x_p_a]
+ (OC.native_shared_library_link ~tags:["profile"; "linkall"]
+ "%.p.cmxa" "%.p.cmxs");
+
+ rule "ocaml: cmx & o -> cmxs"
+ ~tags:["ocaml"; "native"; "shared"; "library"]
+ ~prods:["%.cmxs"]
+ ~deps:["%.cmx"; x_o]
+ (OC.native_shared_library_link "%.cmx" "%.cmxs");
+
+ rule "ocaml: cmx & o -> cmxs & so"
+ ~tags:["ocaml"; "native"; "shared"; "library"]
+ ~prods:["%.cmxs"; x_dll]
+ ~deps:["%.cmx"; x_o]
+ (OC.native_shared_library_link "%.cmx" "%.cmxs");
+
+ rule "ocaml: cmxa & a -> cmxs & so"
+ ~tags:["ocaml"; "native"; "shared"; "library"]
+ ~prods:["%.cmxs"; x_dll]
+ ~deps:["%.cmxa"; x_a]
+ (OC.native_shared_library_link ~tags:["linkall"]
+ "%.cmxa" "%.cmxs");
+ end
+
+ | After_rules ->
+ (* Declare OCaml libraries *)
+ List.iter
+ (function
+ | nm, [] ->
+ ocaml_lib nm
+ | nm, dir :: tl ->
+ ocaml_lib ~dir:dir (dir^"/"^nm);
+ List.iter
+ (fun dir ->
+ List.iter
+ (fun str ->
+ flag ["ocaml"; "use_"^nm; str] (S[A"-I"; P dir]))
+ ["compile"; "infer_interface"; "doc"])
+ tl)
+ t.lib_ocaml;
+
+ (* Declare directories dependencies, replace "include" in _tags. *)
+ List.iter
+ (fun (dir, include_dirs) ->
+ Pathname.define_context dir include_dirs)
+ t.includes;
+
+ (* Declare C libraries *)
+ List.iter
+ (fun (lib, dir, headers) ->
+ (* Handle C part of library *)
+ flag ["link"; "library"; "ocaml"; "byte"; tag_libstubs lib]
+ (S[A"-dllib"; A("-l"^(nm_libstubs lib)); A"-cclib";
+ A("-l"^(nm_libstubs lib))]);
+
+ flag ["link"; "library"; "ocaml"; "native"; tag_libstubs lib]
+ (S[A"-cclib"; A("-l"^(nm_libstubs lib))]);
+
+ flag ["link"; "program"; "ocaml"; "byte"; tag_libstubs lib]
+ (S[A"-dllib"; A("dll"^(nm_libstubs lib))]);
+
+ (* When ocaml link something that use the C library, then one
+ need that file to be up to date.
+ *)
+ dep ["link"; "ocaml"; "program"; tag_libstubs lib]
+ [dir/"lib"^(nm_libstubs lib)^"."^(!Options.ext_lib)];
+
+ dep ["compile"; "ocaml"; "program"; tag_libstubs lib]
+ [dir/"lib"^(nm_libstubs lib)^"."^(!Options.ext_lib)];
+
+ (* TODO: be more specific about what depends on headers *)
+ (* Depends on .h files *)
+ dep ["compile"; "c"]
+ headers;
+
+ (* Setup search path for lib *)
+ flag ["link"; "ocaml"; "use_"^lib]
+ (S[A"-I"; P(dir)]);
+ )
+ t.lib_c;
+
+ (* Add flags *)
+ List.iter
+ (fun (tags, cond_specs) ->
+ let spec =
+ BaseEnvLight.var_choose cond_specs env
+ in
+ flag tags & spec)
+ t.flags
+ | _ ->
+ ()
+
+ let dispatch_default t =
+ dispatch_combine
+ [
+ dispatch t;
+ MyOCamlbuildFindlib.dispatch;
+ ]
+
+end
+
+
+open Ocamlbuild_plugin;;
+let package_default =
+ {
+ MyOCamlbuildBase.lib_ocaml = [("cv", ["src"])];
+ lib_c = [("cv", "src/", [])];
+ flags = [];
+ includes = [];
+ }
+ ;;
+
+let dispatch_default = MyOCamlbuildBase.dispatch_default package_default;;
+
+# 559 "myocamlbuild.ml"
(* OASIS_STOP *)
open Ocamlbuild_plugin
diff --git a/info/vision/camlcv/src/META b/info/vision/camlcv/src/META
new file mode 100644
index 0000000..26e4895
--- /dev/null
+++ b/info/vision/camlcv/src/META
@@ -0,0 +1,10 @@
+# OASIS_START
+# DO NOT EDIT (digest: 3f508b1b1a49417ae0d748f2f8bc10e1)
+version = "0.1"
+description = "Core Opencv"
+requires = "bigarray"
+archive(byte) = "cv.cma"
+archive(native) = "cv.cmxa"
+exists_if = "cv.cma"
+# OASIS_STOP
+
diff --git a/info/vision/camlcv/src/cv.mllib b/info/vision/camlcv/src/cv.mllib
new file mode 100644
index 0000000..7e8b7f4
--- /dev/null
+++ b/info/vision/camlcv/src/cv.mllib
@@ -0,0 +1,5 @@
+# OASIS_START
+# DO NOT EDIT (digest: 789af667e1aace6d4731799b5a02a2b0)
+CvCore
+CvHighGui
+# OASIS_STOP
diff --git a/info/vision/camlcv/src/cvCore.ml b/info/vision/camlcv/src/cvCore.ml
new file mode 100644
index 0000000..155690a
--- /dev/null
+++ b/info/vision/camlcv/src/cvCore.ml
@@ -0,0 +1,462 @@
+
+type ('a,'b) iplImage
+type cvType_ (* int *)
+type color_conversion_ (* int *)
+
+type channel_num_ = [ `Channel_1 | `Channel_2 | `Channel_3 | `Channel_4 ]
+type 'a channel_num = int
+
+let channel_1 = 1
+let channel_2 = 2
+let channel_3 = 3
+let channel_4 = 4
+
+type cvType =
+ | IPL_DEPTH_1U
+ | IPL_DEPTH_8U
+ | IPL_DEPTH_16U
+ | IPL_DEPTH_32F
+ | IPL_DEPTH_8S
+ | IPL_DEPTH_16S
+ | IPL_DEPTH_32S
+
+type depth_ = [ `U1 | `U8 | `U16 | `F32 | `S8 | `S16 | `S32 ]
+type 'a depth = cvType
+
+let depth_u1 = IPL_DEPTH_1U
+let depth_u8 = IPL_DEPTH_8U
+let depth_u16 = IPL_DEPTH_16U
+let depth_f32 = IPL_DEPTH_32F
+
+type 'a image_depth = 'a depth
+
+type color_conversion =
+ | CV_BGR2BGRA
+ | CV_RGB2RGBA
+ | CV_BGRA2BGR
+ | CV_RGBA2RGB
+ | CV_BGR2RGBA
+ | CV_RGB2BGRA
+ | CV_RGBA2BGR
+ | CV_BGRA2RGB
+ | CV_BGR2RGB
+ | CV_RGB2BGR
+ | CV_BGRA2RGBA
+ | CV_RGBA2BGRA
+ | CV_BGR2GRAY
+ | CV_RGB2GRAY
+ | CV_GRAY2BGR
+ | CV_GRAY2RGB
+ | CV_GRAY2BGRA
+ | CV_GRAY2RGBA
+ | CV_BGRA2GRAY
+ | CV_RGBA2GRAY
+ | CV_BGR2BGR565
+ | CV_RGB2BGR565
+ | CV_BGR5652BGR
+ | CV_BGR5652RGB
+ | CV_BGRA2BGR565
+ | CV_RGBA2BGR565
+ | CV_BGR5652BGRA
+ | CV_BGR5652RGBA
+ | CV_GRAY2BGR565
+ | CV_BGR5652GRAY
+ | CV_BGR2BGR555
+ | CV_RGB2BGR555
+ | CV_BGR5552BGR
+ | CV_BGR5552RGB
+ | CV_BGRA2BGR555
+ | CV_RGBA2BGR555
+ | CV_BGR5552BGRA
+ | CV_BGR5552RGBA
+ | CV_GRAY2BGR555
+ | CV_BGR5552GRAY
+ | CV_BGR2XYZ
+ | CV_RGB2XYZ
+ | CV_XYZ2BGR
+ | CV_XYZ2RGB
+ | CV_BGR2YCrCb
+ | CV_RGB2YCrCb
+ | CV_YCrCb2BGR
+ | CV_YCrCb2RGB
+ | CV_BGR2HSV
+ | CV_RGB2HSV
+ | CV_BGR2Lab
+ | CV_RGB2Lab
+ | CV_BayerBG2BGR
+ | CV_BayerGB2BGR
+ | CV_BayerRG2BGR
+ | CV_BayerGR2BGR
+ | CV_BayerBG2RGB
+ | CV_BayerGB2RGB
+ | CV_BayerRG2RGB
+ | CV_BayerGR2RGB
+ | CV_BGR2Luv
+ | CV_RGB2Luv
+ | CV_BGR2HLS
+ | CV_RGB2HLS
+ | CV_HSV2BGR
+ | CV_HSV2RGB
+ | CV_Lab2BGR
+ | CV_Lab2RGB
+ | CV_Luv2BGR
+ | CV_Luv2RGB
+ | CV_HLS2BGR
+ | CV_HLS2RGB
+
+type threshold_type =
+ | CV_THRESH_BINARY
+ | CV_THRESH_BINARY_INV
+ | CV_THRESH_TRUNC
+ | CV_THRESH_TOZERO
+ | CV_THRESH_TOZERO_INV
+ | CV_THRESH_MASK
+ | CV_THRESH_OTSU
+
+type adaptive_method =
+ | CV_ADAPTIVE_THRESH_MEAN_C
+ | CV_ADAPTIVE_THRESH_GAUSSIAN_C
+
+type ('a,'b) conversion = color_conversion
+
+let bgr2rgb = CV_BGR2RGB
+let rgb2bgr = CV_RGB2BGR
+let bgr2hsv = CV_BGR2HSV
+let hsv2bgr = CV_HSV2BGR
+let rgb2hsv = CV_RGB2HSV
+let hsv2rgb = CV_HSV2RGB
+let bgr2gray = CV_BGR2GRAY
+let gray2bgr = CV_GRAY2BGR
+let bgr2bgra = CV_BGR2BGRA
+let rgb2gray = CV_RGB2GRAY
+let gray2rgb = CV_GRAY2RGB
+let rgb2rgba = CV_RGB2RGBA
+
+external cvCreateImage : (int*int) -> cvType_ -> int -> ('a,'b) iplImage = "ocaml_cvCreateImage"
+external cvCvtColor : ('a,'b) iplImage -> ('d,'e) iplImage -> color_conversion_ -> unit = "ocaml_cvCvtColor"
+external threshold : ('a,'b) iplImage -> ('d,'e) iplImage -> float -> float -> threshold_type -> unit = "ocaml_cvThreshold"
+external adaptive_threshold : ('a,'b) iplImage -> ('d,'e) iplImage -> float -> adaptive_method -> threshold_type -> int -> float -> unit = "ocaml_cvAdaptiveThreshold_bytecode" "ocaml_cvAdaptiveThreshold"
+
+external cvSplit : ('a,'b) iplImage -> ([`Channel_1],'b) iplImage option ->
+ ([`Channel_1],'b) iplImage option -> ([`Channel_1],'b) iplImage option ->
+ ([`Channel_1],'b) iplImage option -> unit = "ocaml_cvSplit"
+
+external cvMerge : ([`Channel_1],'b) iplImage option -> ([`Channel_1],'b) iplImage option ->
+ ([`Channel_1],'b) iplImage option -> ([`Channel_1],'b) iplImage option ->
+ ('a,'b) iplImage -> unit = "ocaml_cvMerge"
+
+let split_3 src c1 c2 c3 =
+ cvSplit src (Some c1) (Some c2) (Some c3) None
+
+let merge_3 c1 c2 c3 dst =
+ cvMerge (Some c1) (Some c2) (Some c3) None dst
+
+external get_cvType : cvType -> cvType_ = "ocaml_get_cvType"
+external get_color_conversion : color_conversion -> color_conversion_
+ = "ocaml_get_color_conversion"
+external image_size : ('a,'b) iplImage -> (int*int) = "ocaml_image_size"
+external image_channels : ('a,'b) iplImage -> int = "ocaml_image_channels"
+external image_depth : ('a,'b) iplImage -> int = "ocaml_image_depth"
+external image_data_order : ('a,'b) iplImage -> int = "ocaml_image_data_order"
+
+(* type float_3point = *)
+(* { mutable f0 : float; *)
+(* mutable f1 : float; *)
+(* mutable f2 : float; } *)
+
+(* type int_3point = *)
+(* { mutable i0 : int; *)
+(* mutable i1 : int; *)
+(* mutable i2 : int; } *)
+
+(* external get_float_3point : *)
+(* ([`One],[`F32],[`BGR | `RGB | `HSV ]) iplImage -> *)
+(* float_point -> unit *)
+(* = "ocaml_get_float_3point" "noalloc" *)
+
+(* external set_float_3point : *)
+(* ([`One],[`F32],[`BGR | `RGB | `HSV ]) iplImage -> *)
+(* float_point -> unit *)
+(* = "ocaml_set_float_3point" "noalloc" *)
+
+(* external unsafe_get_int_3point : *)
+(* ([`One],[`U8],[<`BGR | `RGB | `HSV ]) iplImage -> *)
+(* int -> int_3point -> unit *)
+(* = "ocaml_get_int_3point" "noalloc" *)
+
+(* external unsafe_set_int_3point : *)
+(* ([`One],[`U8],[<`BGR | `RGB | `HSV ]) iplImage -> *)
+(* int -> int_3point -> unit *)
+(* = "ocaml_set_int_3point" "noalloc" *)
+
+type image_data = (int, Bigarray.int8_unsigned_elt, Bigarray.c_layout) Bigarray.Array2.t
+
+external image_data : ('a,[`U8]) iplImage -> image_data = "ocaml_image_to_bigarray"
+
+external zero_image : ('a,'b) iplImage -> unit = "ocaml_cvZero"
+
+external clone_image : ('a,'b) iplImage -> ('a,'b) iplImage = "ocaml_cvCloneImage"
+
+let create_image ~x ~y _type i = cvCreateImage (x,y) (get_cvType _type) i
+let convert_color ~src ~dst color_conversion =
+ cvCvtColor src dst (get_color_conversion color_conversion)
+
+type cvScalar = float * float * float * float
+type cvPoint = int * int
+type cvPoint2D32f = float * float
+type cvPoint3D32f = float * float * float
+type cvPoint2D64f = float * float
+type cvPoint3D64f = float * float * float
+type cvSize = int * int
+type cvTermCriteria =
+ { termcrit_iter : bool;
+ termcrit_epsilon : bool;
+ max_iter : int;
+ epsilon : float; }
+
+external cvCircle : ('a,[`U8]) iplImage -> cvPoint -> int -> cvScalar -> int -> unit = "ocaml_cvCircle"
+external cvEllipse : ('a,[`U8]) iplImage -> cvPoint -> cvSize ->
+ float -> float -> float -> cvScalar -> int -> unit =
+ "ocaml_cvEllipse_bytecode" "ocaml_cvEllipse"
+external cvRectangle : ('a,[`U8]) iplImage -> cvPoint -> cvPoint -> cvScalar -> int -> unit = "ocaml_cvRectangle"
+external cvLine : ('a,[`U8]) iplImage -> cvPoint -> cvPoint -> cvScalar -> int -> unit = "ocaml_cvLine"
+
+(* memory handling *)
+type cvMemStorage
+
+external create_CvMemStorage : unit -> cvMemStorage = "ocaml_create_CvMemStorage"
+external free_CvMemStorage : cvMemStorage -> unit = "ocaml_free_CvMemStorage"
+
+type contour_retrieval_mode =
+ | CV_RETR_EXTERNAL
+ | CV_RETR_LIST
+ | CV_RETR_CCOMP
+ | CV_RETR_TREE
+
+type contour_approximation_method =
+ | CV_CHAIN_CODE
+ | CV_CHAIN_APPROX_NONE
+ | CV_CHAIN_APPROX_SIMPLE
+ | CV_CHAIN_APPROX_TC89_L1
+ | CV_CHAIN_APPROX_TC89_KCOS
+ | CV_LINK_RUNS
+
+type cvSeq
+
+external cvFindContours : ([`Channel_1],[`U8]) iplImage -> cvMemStorage ->
+ contour_retrieval_mode -> contour_approximation_method -> cvPoint -> cvSeq =
+ "ocaml_cvFindContours"
+
+type cvSeq_info = {
+ cv_h_prev : cvSeq option;
+ cv_h_next : cvSeq option;
+ cv_v_prev : cvSeq option;
+ cv_v_next : cvSeq option;
+}
+
+external cvSeq_info : cvSeq -> cvSeq_info = "ocaml_CvSeq_info"
+
+type seq = {
+ seq_stor : cvMemStorage;
+ seq : cvSeq;
+}
+
+type seq_info = {
+ h_prev : seq option;
+ h_next : seq option;
+ v_prev : seq option;
+ v_next : seq option;
+}
+
+let add_stor s = function
+ | None -> None
+ | Some v -> Some { seq_stor = s; seq = v }
+
+let seq_info seq =
+ let info = cvSeq_info seq.seq in
+ { h_prev = add_stor seq.seq_stor info.cv_h_prev;
+ h_next = add_stor seq.seq_stor info.cv_h_next;
+ v_prev = add_stor seq.seq_stor info.cv_v_prev;
+ v_next = add_stor seq.seq_stor info.cv_v_next; }
+
+let find_contours ?(mode=CV_RETR_LIST) ?(meth=CV_CHAIN_APPROX_SIMPLE) image =
+ let stor = create_CvMemStorage () in
+ let cvSeq = cvFindContours image stor mode meth (0,0) in
+ { seq_stor = stor;
+ seq = cvSeq }
+
+external cvDrawContours : ('a,[`U8]) iplImage -> cvSeq -> cvScalar -> cvScalar ->
+ int -> int -> cvPoint -> unit = "ocaml_cvDrawContours_bytecode" "ocaml_cvDrawContours"
+
+let draw_contours image seq in_color out_color level thickness offset =
+ cvDrawContours image seq.seq in_color out_color level thickness offset
+
+type ellipse = {
+ ellipse_center : float * float;
+ ellipse_size : float * float;
+ ellipse_angle : float;
+}
+
+external cvFitEllipse2 : cvSeq -> (float * float * float * float * float) option = "ocaml_cvFitEllipse2"
+
+let fit_ellipse seq =
+ match cvFitEllipse2 seq.seq with
+ | None -> None
+ | Some (x,y,width,height,angle) ->
+ Some { ellipse_center = x,y;
+ ellipse_size = width,height;
+ ellipse_angle = angle }
+
+type calib_cb =
+ | CV_CALIB_CB_ADAPTIVE_THRESH
+ | CV_CALIB_CB_NORMALIZE_IMAGE
+ | CV_CALIB_CB_FILTER_QUADS
+ | CV_CALIB_CB_FAST_CHECK
+
+type chessboard_corners = cvSize * bool * int *
+ (float, Bigarray.float32_elt, Bigarray.c_layout) Bigarray.Array3.t
+
+external cvFindChessboardCorners : ('a,[`U8]) iplImage -> cvSize -> calib_cb list ->
+ bool * int * (float, Bigarray.float32_elt, Bigarray.c_layout) Bigarray.Array3.t
+ = "ocaml_cvFindChessboardCorners"
+
+let find_chessboard_corners ?(flags=[CV_CALIB_CB_ADAPTIVE_THRESH]) image size =
+ let (found,count,ba) = cvFindChessboardCorners image size flags in
+ (size,found,count,ba)
+
+let found_chessboard_corners (_,found,_,_) = found
+
+let array_of_chessboard_corners (size,found,count,ba) =
+ if found
+ then
+ Some (Array.init (Bigarray.Array3.dim1 ba)
+ (fun i -> Array.init (Bigarray.Array3.dim2 ba)
+ (fun j -> ba.{i,j,0}, ba.{i,j,1})))
+ else
+ None
+
+external cvDrawChessboardCorners : ('a,[`U8]) iplImage -> cvSize ->
+ (float, Bigarray.float32_elt, Bigarray.c_layout) Bigarray.Array3.t ->
+ int -> bool -> unit
+ = "ocaml_cvDrawChessboardCorners"
+
+let draw_chessboard_corners i (size,found,count,ba) =
+ cvDrawChessboardCorners i size ba count found
+
+external cvFindCornerSubPix : ('a,[`U8]) iplImage ->
+ (float, Bigarray.float32_elt, Bigarray.c_layout) Bigarray.Array3.t ->
+ int -> cvSize -> cvSize -> cvTermCriteria -> unit
+ = "ocaml_cvFindCornerSubPix_bytecode" "ocaml_cvFindCornerSubPix"
+
+let default_criteria =
+ { termcrit_iter = true;
+ termcrit_epsilon = true;
+ max_iter = 30;
+ epsilon = 0.1; }
+
+let find_corner_subpix ?(criteria=default_criteria) ?(winSize=11,11) ?(zeroZone=(-1,-1))
+ i (size,found,count,ba) =
+ if found
+ then
+ begin
+ cvFindCornerSubPix i ba count (11,11) (-1,-1) criteria;
+ end
+
+
+(** camera calibration *)
+
+type ('channel) cvMat_float32 =
+ (float, Bigarray.float32_elt, Bigarray.c_layout) Bigarray.Array3.t
+
+type ('channel) cvMat_int =
+ (int, Bigarray.int_elt, Bigarray.c_layout) Bigarray.Array3.t
+
+external cvCalibrateCamera2 : [`Channel_3] cvMat_float32 -> [`Channel_2] cvMat_float32 ->
+ [`Channel_1] cvMat_int -> cvSize -> [`Channel_1] cvMat_float32 ->
+ [`Channel_1] cvMat_float32 -> float
+ = "ocaml_cvCalibrateCamera2_bytecode" "ocaml_cvCalibrateCamera2"
+
+type calibration = cvSize * float * cvSize option * ((float*float) array array*(float*float*float) array array) list
+
+let init_calibration cb_size square_size : calibration = (cb_size, square_size, None, [])
+
+let corner_positions (x,y) square_size =
+ Array.init x
+ (fun i -> Array.init y
+ (fun j -> (float j) *. square_size, (float i) *. square_size, 0.))
+
+let add_calibration_image ((cb_size, square_size, i_size, state):calibration) image : calibration =
+ let i_size = match i_size with
+ | None -> image_size image
+ | Some s ->
+ if s <> image_size image
+ then raise (Invalid_argument "not same size");
+ s in
+ let r = find_chessboard_corners image cb_size in
+ let a = corner_positions cb_size square_size in
+ find_corner_subpix image r;
+ let state =
+ match array_of_chessboard_corners r with
+ | None -> state
+ | Some v -> (v,a)::state in
+ (cb_size, square_size, Some i_size, state)
+
+let map_a2 a = Array.map (fun e -> Array.map (fun (x,y) -> [|x;y|]) e) a
+let map_a3 a = Array.map (fun e -> Array.map (fun (x,y,z) -> [|x;y;z|]) e) a
+
+let calibrate (cal:calibration) =
+ let (cb_x,cb_y), square_size, i_size, state = cal in
+ let i_size = match i_size with
+ | None -> raise (Invalid_argument "no calibration data")
+ | Some v -> v in
+ let points = cb_x * cb_y in
+ let obj_pos = Array.concat (List.map (fun (_,a) -> map_a3 a) state) in
+ let obj_pos = Bigarray.Array3.of_array Bigarray.float32 Bigarray.c_layout obj_pos in
+ let obj_pos = Bigarray.reshape_3
+ (Bigarray.genarray_of_array3 obj_pos)
+ ((List.length state) * points) 1 3 in
+ let img_pos = Array.concat (List.map (fun (a,_) -> map_a2 a) state) in
+ let img_pos = Bigarray.Array3.of_array Bigarray.float32 Bigarray.c_layout img_pos in
+ let img_pos = Bigarray.reshape_3
+ (Bigarray.genarray_of_array3 img_pos)
+ ((List.length state) * points) 1 2 in
+ let point_count = Array.create (List.length state) points in
+ let point_count = Bigarray.Array1.of_array Bigarray.int Bigarray.c_layout point_count in
+ let point_count = Bigarray.reshape_3
+ (Bigarray.genarray_of_array1 point_count)
+ (List.length state) 1 1 in
+ let out_mat = Bigarray.Array3.create Bigarray.float32 Bigarray.c_layout 3 3 1 in
+ Bigarray.Array3.fill out_mat 0.;
+ out_mat.{0,0,0} <- 1.;
+ out_mat.{1,1,0} <- 1.;
+ let out_vect = Bigarray.Array3.create Bigarray.float32 Bigarray.c_layout 5 1 1 in
+ let r = cvCalibrateCamera2
+ obj_pos img_pos point_count i_size out_mat out_vect in
+ let r_mat = Array.init 3 (fun i -> Array.init 3 (fun j -> out_mat.{i,j,0})) in
+ let r_vect = Array.init 5 (fun i -> out_vect.{i,0,0}) in
+ r, (r_mat, r_vect)
+
+external cvInitUndistortMap :
+ [`Channel_1] cvMat_float32 ->
+ [`Channel_1] cvMat_float32 ->
+ ([`Channel_1],[`F32]) iplImage ->
+ ([`Channel_1],[`F32]) iplImage ->
+ unit
+ = "cvInitUndistortMap"
+
+let init_undistort_map (x,y) (mat,v) =
+ let mat = Bigarray.Array3.of_array Bigarray.float32 Bigarray.c_layout
+ ((Array.map (Array.map (fun v -> [|v|]))) mat) in
+ let v = Bigarray.Array3.of_array Bigarray.float32 Bigarray.c_layout
+ (Array.map (fun v -> [|[|v|]|]) v) in
+ let i1 = create_image ~x ~y depth_f32 channel_1 in
+ let i2 = create_image ~x ~y depth_f32 channel_1 in
+ cvInitUndistortMap mat v i1 i2;
+ i1, i2
+
+external cvRemap :
+ ('a,'b) iplImage -> ('a,'b) iplImage ->
+ ([`Channel_1],[`F32]) iplImage ->
+ ([`Channel_1],[`F32]) iplImage ->
+ unit
+ = "cvRemap"
diff --git a/info/vision/camlcv/src/cvCore.mli b/info/vision/camlcv/src/cvCore.mli
new file mode 100644
index 0000000..658edc5
--- /dev/null
+++ b/info/vision/camlcv/src/cvCore.mli
@@ -0,0 +1,176 @@
+
+type cvScalar = float * float * float * float
+type cvPoint = int * int
+type cvPoint2D32f = float * float
+type cvPoint3D32f = float * float * float
+type cvPoint2D64f = float * float
+type cvPoint3D64f = float * float * float
+type cvSize = int * int
+type cvTermCriteria =
+ { termcrit_iter : bool;
+ termcrit_epsilon : bool;
+ max_iter : int;
+ epsilon : float; }
+
+type ('chan_num,'depth) iplImage
+
+type channel_num_ = [ `Channel_1 | `Channel_2 | `Channel_3 | `Channel_4 ]
+type 'num channel_num
+
+val channel_1 : [ `Channel_1 ] channel_num
+val channel_2 : [ `Channel_2 ] channel_num
+val channel_3 : [ `Channel_3 ] channel_num
+val channel_4 : [ `Channel_4 ] channel_num
+
+type 'depth image_depth
+
+val depth_u1 : [`U1] image_depth
+val depth_u8 : [`U8] image_depth
+val depth_u16 : [`U16] image_depth
+val depth_f32 : [`F32] image_depth
+
+val create_image : x:int -> y:int -> 'depth image_depth -> 'chan_num channel_num ->
+ ('chan_num,'depth) iplImage
+val clone_image : ('a,'b) iplImage -> ('a,'b) iplImage
+val zero_image : ('a,'b) iplImage -> unit
+
+val image_size : ('a,'b) iplImage -> int * int
+val image_channels : ('a,'b) iplImage -> int
+val image_depth : ('a,'b) iplImage -> int
+
+(** color conversions *)
+type ('c_src,'c_dst) conversion
+
+val bgr2rgb : ([`Channel_3],[`Channel_3]) conversion
+val rgb2bgr : ([`Channel_3],[`Channel_3]) conversion
+val bgr2hsv : ([`Channel_3],[`Channel_3]) conversion
+val hsv2bgr : ([`Channel_3],[`Channel_3]) conversion
+val rgb2hsv : ([`Channel_3],[`Channel_3]) conversion
+val hsv2rgb : ([`Channel_3],[`Channel_3]) conversion
+val bgr2gray : ([`Channel_3],[`Channel_1]) conversion
+val gray2bgr : ([`Channel_1],[`Channel_3]) conversion
+val bgr2bgra : ([`Channel_3],[`Channel_4]) conversion
+val rgb2gray : ([`Channel_3],[`Channel_1]) conversion
+val gray2rgb : ([`Channel_1],[`Channel_3]) conversion
+val rgb2rgba : ([`Channel_3],[`Channel_4]) conversion
+
+val convert_color :
+ src:('c_src,'b) iplImage ->
+ dst:('c_dst,'b) iplImage ->
+ ('c_src,'c_dst) conversion ->
+ unit
+
+val split_3 : ([`Channel_3],'b) iplImage -> ([`Channel_1],'b) iplImage ->
+ ([`Channel_1],'b) iplImage -> ([`Channel_1],'b) iplImage -> unit
+
+val merge_3 : ([`Channel_1],'b) iplImage -> ([`Channel_1],'b) iplImage ->
+ ([`Channel_1],'b) iplImage -> ([`Channel_3],'b) iplImage -> unit
+
+(** threshold *)
+
+type threshold_type =
+ | CV_THRESH_BINARY
+ | CV_THRESH_BINARY_INV
+ | CV_THRESH_TRUNC
+ | CV_THRESH_TOZERO
+ | CV_THRESH_TOZERO_INV
+ | CV_THRESH_MASK
+ | CV_THRESH_OTSU
+
+type adaptive_method =
+ | CV_ADAPTIVE_THRESH_MEAN_C
+ | CV_ADAPTIVE_THRESH_GAUSSIAN_C
+
+val threshold : ('a,'b) iplImage -> ('d,'e) iplImage -> float -> float -> threshold_type -> unit
+(** [threshold src dst threshold maxValue thresholdType] *)
+val adaptive_threshold : ('a,'b) iplImage -> ('d,'e) iplImage -> float -> adaptive_method -> threshold_type -> int -> float -> unit
+(** [adaptive_threshold src dst maxValue adaptiveMethod thresholdType blockSize param1] *)
+
+(** drawing *)
+
+val cvCircle : ('a,[`U8]) iplImage -> cvPoint -> int -> cvScalar -> int -> unit
+val cvEllipse : ('a,[`U8]) iplImage -> cvPoint -> cvSize -> float -> float -> float -> cvScalar -> int -> unit
+val cvRectangle : ('a,[`U8]) iplImage -> cvPoint -> cvPoint -> cvScalar -> int -> unit
+val cvLine : ('a,[`U8]) iplImage -> cvPoint -> cvPoint -> cvScalar -> int -> unit
+
+(**/**)
+val image_data_order : ('a,'b) iplImage -> int
+type image_data = (int, Bigarray.int8_unsigned_elt, Bigarray.c_layout) Bigarray.Array2.t
+external image_data : ('a,[`U8]) iplImage -> image_data = "ocaml_image_to_bigarray"
+
+(* contour finding *)
+
+type contour_retrieval_mode =
+ | CV_RETR_EXTERNAL
+ | CV_RETR_LIST
+ | CV_RETR_CCOMP
+ | CV_RETR_TREE
+
+type contour_approximation_method =
+ | CV_CHAIN_CODE
+ | CV_CHAIN_APPROX_NONE
+ | CV_CHAIN_APPROX_SIMPLE
+ | CV_CHAIN_APPROX_TC89_L1
+ | CV_CHAIN_APPROX_TC89_KCOS
+ | CV_LINK_RUNS
+
+type seq
+
+type seq_info = {
+ h_prev : seq option;
+ h_next : seq option;
+ v_prev : seq option;
+ v_next : seq option;
+}
+
+val seq_info : seq -> seq_info
+val find_contours : ?mode:contour_retrieval_mode -> ?meth:contour_approximation_method ->
+ ([`Channel_1],[`U8]) iplImage -> seq
+
+val draw_contours : ('a,[`U8]) iplImage -> seq -> cvScalar -> cvScalar -> int -> int -> cvPoint -> unit
+
+type ellipse = {
+ ellipse_center : float * float;
+ ellipse_size : float * float;
+ ellipse_angle : float;
+}
+
+val fit_ellipse : seq -> ellipse option
+
+type calib_cb =
+ | CV_CALIB_CB_ADAPTIVE_THRESH
+ | CV_CALIB_CB_NORMALIZE_IMAGE
+ | CV_CALIB_CB_FILTER_QUADS
+ | CV_CALIB_CB_FAST_CHECK
+
+type chessboard_corners
+
+val find_chessboard_corners : ?flags:calib_cb list -> ('a,[`U8]) iplImage -> cvSize ->
+ chessboard_corners
+
+val found_chessboard_corners : chessboard_corners -> bool
+
+val array_of_chessboard_corners : chessboard_corners -> (float*float) array array option
+
+val draw_chessboard_corners : ([`Channel_3],[`U8]) iplImage ->
+ chessboard_corners -> unit
+
+val find_corner_subpix : ?criteria:cvTermCriteria -> ?winSize:cvSize ->
+ ?zeroZone:cvSize -> ([`Channel_1],[`U8]) iplImage -> chessboard_corners -> unit
+
+type calibration
+
+val init_calibration : cvSize -> float -> calibration
+val add_calibration_image :
+ calibration -> ([ `Channel_1 ], [ `U8 ]) iplImage -> calibration
+val calibrate : calibration -> float * ( float array array * float array )
+
+val init_undistort_map : cvSize -> ( float array array * float array ) ->
+ ([ `Channel_1 ], [ `F32 ]) iplImage * ([ `Channel_1 ], [ `F32 ]) iplImage
+
+external cvRemap :
+ ('a,'b) iplImage -> ('a,'b) iplImage ->
+ ([`Channel_1],[`F32]) iplImage ->
+ ([`Channel_1],[`F32]) iplImage ->
+ unit
+ = "cvRemap"
diff --git a/info/vision/camlcv/src/cvHighGui.ml b/info/vision/camlcv/src/cvHighGui.ml
new file mode 100644
index 0000000..bc5ada3
--- /dev/null
+++ b/info/vision/camlcv/src/cvHighGui.ml
@@ -0,0 +1,53 @@
+open CvCore
+
+(* image loading from file *)
+
+type iscolor_ (* int *)
+type iscolor =
+ | CV_LOAD_IMAGE_UNCHANGED
+ | CV_LOAD_IMAGE_GRAYSCALE
+ | CV_LOAD_IMAGE_COLOR
+ | CV_LOAD_IMAGE_ANYDEPTH
+ | CV_LOAD_IMAGE_ANYCOLOR
+
+type 'a load_color = iscolor
+
+let load_color = CV_LOAD_IMAGE_COLOR
+let load_grayscale = CV_LOAD_IMAGE_GRAYSCALE
+
+external get_iscolor : iscolor -> iscolor_ = "ocaml_get_iscolor"
+external cvLoadImage : string -> iscolor_ -> ('a,'b) iplImage = "ocaml_cvLoadImage"
+
+let load_image file iscolor = cvLoadImage file (get_iscolor iscolor)
+
+(* capture *)
+
+type cvCapture
+external capture_from_cam : int -> cvCapture = "ocaml_cvCaptureFromCAM"
+external query_frame : cvCapture -> ('a,'b) iplImage = "ocaml_cvQueryFrame"
+
+(* interface *)
+
+type window_option =
+ | CV_WINDOW_DEFAULT
+ | CV_WINDOW_AUTOSIZE
+
+external cvNamedWindow : string -> int -> unit = "ocaml_cvNamedWindow"
+let named_window ?(option=CV_WINDOW_DEFAULT) name =
+ let opt =
+ match option with
+ | CV_WINDOW_DEFAULT -> 0
+ | CV_WINDOW_AUTOSIZE -> 1 in
+ cvNamedWindow name opt
+
+external wait_key : int -> char = "ocaml_cvWaitKey"
+external show_image : string -> ('a,'b) iplImage -> unit = "ocaml_cvShowImage"
+
+type int_var
+
+external create_int_var : int -> int_var = "ocaml_create_int_var"
+external get_int_var : int_var -> int = "ocaml_get_int_var"
+external set_int_var : int_var -> int -> unit = "ocaml_set_int_var"
+external cvCreateTrackbar : string -> string -> int_var -> int -> int = "ocaml_cvCreateTrackbar"
+
+let create_trackbar ~name ~window var max = ignore (cvCreateTrackbar name window var max)
diff --git a/info/vision/camlcv/src/cvHighGui.mli b/info/vision/camlcv/src/cvHighGui.mli
new file mode 100644
index 0000000..8e93588
--- /dev/null
+++ b/info/vision/camlcv/src/cvHighGui.mli
@@ -0,0 +1,41 @@
+open CvCore
+
+(** image loading from file *)
+
+type 'depth load_color
+
+val load_color : [`Channel_3] load_color
+val load_grayscale : [`Channel_1] load_color
+
+val load_image : string -> 'depth load_color -> ('depth,[`U8]) iplImage
+
+(** camera capture *)
+
+type cvCapture
+
+val capture_from_cam : int -> cvCapture
+val query_frame : cvCapture -> ([`Channel_3],[`U8]) iplImage
+
+(** Gui interface *)
+
+type window_option =
+ | CV_WINDOW_DEFAULT
+ | CV_WINDOW_AUTOSIZE
+
+val named_window : ?option:window_option -> string -> unit
+(** [named_window name] create a window named [name] *)
+
+val show_image : string -> ('a,'b) iplImage -> unit
+(** [show_image window image] display the image [image] in the window
+ [window] *)
+val wait_key : int -> char
+(** [wait_key time] wait for [time] milliseconds for an event to
+ occur. If [time < 0] it waits indefinitely. Nothing happens in
+ the gui until this function is called. *)
+
+type int_var
+
+val create_int_var : int -> int_var
+val get_int_var : int_var -> int
+val set_int_var : int_var -> int -> unit
+val create_trackbar : name:string -> window:string -> int_var -> int -> unit
diff --git a/info/vision/camlcv/src/cv_caml.c b/info/vision/camlcv/src/cv_caml.c
index a0fa283..7b70980 100644
--- a/info/vision/camlcv/src/cv_caml.c
+++ b/info/vision/camlcv/src/cv_caml.c
@@ -49,6 +49,43 @@ int ocaml_get_iscolor(int iscolor)
return (Val_int(iscolor_table[Int_val(iscolor)]));
}
+#define CvMemStorage_val(v) (*(CvMemStorage**) (Data_custom_val(v)))
+
+void caml_finalize_CvMemStorage(value vstor)
+{
+ CvMemStorage* stor = CvMemStorage_val(vstor);
+ if (stor) {
+ cvReleaseMemStorage(&stor);
+ CvMemStorage_val(vstor) = 0;
+ }
+}
+
+static struct custom_operations CvMemStorage_operations = {
+ "ocaml_opencv_CvMemStorage",
+ caml_finalize_CvMemStorage,
+ custom_compare_default,
+ custom_hash_default,
+ custom_serialize_default,
+ custom_deserialize_default
+};
+
+CAMLexport value ocaml_create_CvMemStorage(value vunit)
+{
+ CAMLparam1(vunit);
+ CAMLlocal1 (res);
+ CvMemStorage *stor = cvCreateMemStorage(0);
+ res = caml_alloc_custom(&CvMemStorage_operations, sizeof(CvMemStorage*), 1, 10);
+ CvMemStorage_val(res) = stor;
+ CAMLreturn (res);
+}
+
+CAMLexport value ocaml_free_CvMemStorage(value vstor)
+{
+ CAMLparam1(vstor);
+ caml_finalize_CvMemStorage(vstor);
+ CAMLreturn(Val_unit);
+}
+
struct image
{
IplImage* image;
@@ -75,8 +112,8 @@ static struct custom_operations IplImage_operations = {
CAMLexport value caml_alloc_IplImage(IplImage *image)
{
CAMLparam0();
- CAMLlocal1 (res);
- res = caml_alloc_custom(&IplImage_operations, sizeof(struct image), 1, 1000);
+ CAMLlocal1(res);
+ res = caml_alloc_custom(&IplImage_operations, sizeof(struct image), 1, 10);
Image_val(res)->image = image;
Imag...
[truncated message content] |
|
From: Pierre C. <Ba...@us...> - 2012-04-25 23:07:49
|
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 b64fde9d0632479d027d3ebba220d7de086b645a (commit)
from f089f8fedd909a0c41a4fe75fc52e2584b31f7f1 (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 b64fde9d0632479d027d3ebba220d7de086b645a
Author: chicco <cha...@cr...>
Date: Thu Apr 26 01:02:25 2012 +0200
[krobot_dump_ax12] small tool to record ax12 positions
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/_oasis b/info/control2011/_oasis
index 08eb04c..7409387 100644
--- a/info/control2011/_oasis
+++ b/info/control2011/_oasis
@@ -222,12 +222,18 @@ Executable "krobot-can-display"
Executable "krobot-ax12-control"
Path: src/tools
- Build$: flag(gtk)
- Install$: flag(gtk)
+ Install: true
CompiledObject: best
MainIs: krobot_ax12_control.ml
BuildDepends: krobot.can, lwt.syntax
+Executable "krobot-dump-ax12"
+ Path: src/tools
+ Install: true
+ CompiledObject: best
+ MainIs: krobot_dump_ax12.ml
+ BuildDepends: krobot.can, lwt.syntax
+
# +-------------------------------------------------------------------+
# | Examples |
# +-------------------------------------------------------------------+
diff --git a/info/control2011/_tags b/info/control2011/_tags
index b45f0f4..3049ac4 100644
--- a/info/control2011/_tags
+++ b/info/control2011/_tags
@@ -2,7 +2,7 @@
<src/interfaces/*.ml>: -syntax_camlp4o
# OASIS_START
-# DO NOT EDIT (digest: 3852a61af000802215e40b6ad6830d71)
+# DO NOT EDIT (digest: 77bb0a0cf22171b21400c7924cce86fe)
# Ignore VCS directories, you can use the same kind of rule outside
# OASIS_START/STOP if you want to exclude directories that contains
# useless stuff for the build process
@@ -91,10 +91,6 @@
<src/tools/krobot_ax12_control.{native,byte}>: pkg_lwt.syntax
<src/tools/krobot_ax12_control.{native,byte}>: pkg_lwt.react
<src/tools/krobot_ax12_control.{native,byte}>: pkg_bitstring
-<src/tools/*.ml{,i}>: use_krobot-can
-<src/tools/*.ml{,i}>: pkg_sexplib.syntax
-<src/tools/*.ml{,i}>: pkg_sexplib
-<src/tools/*.ml{,i}>: pkg_bitstring
# Executable krobot-viewer
<src/tools/krobot_viewer.{native,byte}>: use_krobot
<src/tools/krobot_viewer.{native,byte}>: pkg_lwt.unix
@@ -185,6 +181,19 @@
<src/tools/krobot_plot.{native,byte}>: pkg_cairo.lablgtk2
<src/tools/*.ml{,i}>: pkg_lwt.glib
<src/tools/*.ml{,i}>: pkg_cairo.lablgtk2
+# Executable krobot-dump-ax12
+<src/tools/krobot_dump_ax12.{native,byte}>: use_krobot-can
+<src/tools/krobot_dump_ax12.{native,byte}>: use_krobot
+<src/tools/krobot_dump_ax12.{native,byte}>: pkg_sexplib.syntax
+<src/tools/krobot_dump_ax12.{native,byte}>: pkg_sexplib
+<src/tools/krobot_dump_ax12.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_dump_ax12.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_dump_ax12.{native,byte}>: pkg_lwt.react
+<src/tools/krobot_dump_ax12.{native,byte}>: pkg_bitstring
+<src/tools/*.ml{,i}>: use_krobot-can
+<src/tools/*.ml{,i}>: pkg_sexplib.syntax
+<src/tools/*.ml{,i}>: pkg_sexplib
+<src/tools/*.ml{,i}>: pkg_bitstring
# Executable krobot-dump
<src/tools/krobot_dump.{native,byte}>: use_krobot
<src/tools/krobot_dump.{native,byte}>: pkg_lwt.unix
diff --git a/info/control2011/setup.ml b/info/control2011/setup.ml
index e2a975d..ef14fdf 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: ebf28739fe9d63028b7611d27a835a73) *)
+(* DO NOT EDIT (digest: 3f1f69c4008e4511460ad8227132b513) *)
(*
Regenerated by OASIS v0.3.0~rc3
Visit http://oasis.forge.ocamlcore.org for more information and
@@ -5575,16 +5575,8 @@ let setup_t =
cs_plugin_data = [];
},
{
- bs_build =
- [
- (OASISExpr.EBool true, false);
- (OASISExpr.EFlag "gtk", true)
- ];
- bs_install =
- [
- (OASISExpr.EBool true, false);
- (OASISExpr.EFlag "gtk", true)
- ];
+ bs_build = [(OASISExpr.EBool true, true)];
+ bs_install = [(OASISExpr.EBool true, true)];
bs_path = "src/tools";
bs_compiled_object = Best;
bs_build_depends =
@@ -6053,6 +6045,36 @@ let setup_t =
{exec_custom = false; exec_main_is = "krobot_plot.ml"; });
Executable
({
+ cs_name = "krobot-dump-ax12";
+ 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-can";
+ 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_dump_ax12.ml";
+ });
+ Executable
+ ({
cs_name = "krobot-dump";
cs_data = PropList.Data.create ();
cs_plugin_data = [];
@@ -6143,13 +6165,13 @@ let setup_t =
};
oasis_fn = Some "_oasis";
oasis_version = "0.3.0~rc3";
- oasis_digest = Some "\252#\185\030(\181\232\r\160\214B\172\240o\156_";
+ oasis_digest = Some "q\1674\179\017\0115F\190\024gsXI=$";
oasis_exec = None;
oasis_setup_args = [];
};;
let setup () = BaseSetup.setup setup_t;;
-# 6154 "setup.ml"
+# 6176 "setup.ml"
(* OASIS_STOP *)
let () = setup ();;
diff --git a/info/control2011/src/tools/krobot_dump_ax12.ml b/info/control2011/src/tools/krobot_dump_ax12.ml
new file mode 100644
index 0000000..8b95a5e
--- /dev/null
+++ b/info/control2011/src/tools/krobot_dump_ax12.ml
@@ -0,0 +1,52 @@
+open Krobot_bus
+open Krobot_message
+open Lwt_react
+
+lwt bus = Krobot_bus.get ()
+
+type ax12_info =
+ { id : int;
+ pos : int;
+ speed : int;
+ torque : int;
+ time : float }
+
+module IntMap = Map.Make(struct type t = int let compare = compare end)
+
+let previous = ref IntMap.empty
+let is_previous info =
+ try
+ let v = IntMap.find info.id !previous in
+ (v.pos,v.speed) = (info.pos,info.speed)
+ with
+ | Not_found -> false
+
+let print_info ({ id; pos; speed; torque; time } as info) =
+ previous := IntMap.add id info !previous;
+ Printf.printf "id %i,pos %i, speed %i, torque %i, time %f\n%!" id pos speed torque time
+
+let log () =
+ E.keep
+ (E.map
+ (fun (time, message) ->
+ match message with
+ | CAN(_, frame) ->
+ begin
+ match Krobot_message.decode frame with
+ | Ax12_State (id,pos,speed,torque) ->
+ let info = { id;pos;speed;torque;time } in
+ if not (is_previous info)
+ then print_info info
+ | _ -> ()
+ end
+ | _ -> ())
+ (Krobot_bus.recv bus))
+
+let rec loop_request () =
+ lwt () = Lwt_unix.sleep 0.01 in
+ lwt () = Lwt_list.iter_s (fun i -> Krobot_bus.send bus (Unix.gettimeofday (), CAN (Info, Krobot_message.encode (Ax12_Request_State i)))) [2;3] in
+ loop_request ()
+
+let t = loop_request ()
+let () = log ()
+lwt () = fst (Lwt.wait ())
hooks/post-receive
--
krobot
|
|
From: Pierre C. <Ba...@us...> - 2012-04-25 21:21:58
|
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 f089f8fedd909a0c41a4fe75fc52e2584b31f7f1 (commit)
from 52ee072e134d531d77ee2de21fc7824ce8bde751 (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 f089f8fedd909a0c41a4fe75fc52e2584b31f7f1
Author: chicco <cha...@cr...>
Date: Wed Apr 25 23:21:04 2012 +0200
[krobot_ax12_control] small tool to control ax12
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/_oasis b/info/control2011/_oasis
index 9472d20..08eb04c 100644
--- a/info/control2011/_oasis
+++ b/info/control2011/_oasis
@@ -220,6 +220,14 @@ Executable "krobot-can-display"
MainIs: krobot_can_display.ml
BuildDepends: krobot.can, lwt.syntax, sexplib, sexplib.syntax, cairo.lablgtk2, lwt.glib
+Executable "krobot-ax12-control"
+ Path: src/tools
+ Build$: flag(gtk)
+ Install$: flag(gtk)
+ CompiledObject: best
+ MainIs: krobot_ax12_control.ml
+ BuildDepends: krobot.can, lwt.syntax
+
# +-------------------------------------------------------------------+
# | Examples |
# +-------------------------------------------------------------------+
diff --git a/info/control2011/_tags b/info/control2011/_tags
index 7a89fb3..b45f0f4 100644
--- a/info/control2011/_tags
+++ b/info/control2011/_tags
@@ -2,7 +2,7 @@
<src/interfaces/*.ml>: -syntax_camlp4o
# OASIS_START
-# DO NOT EDIT (digest: fb711b43f944541635b3a5ea56a5681e)
+# DO NOT EDIT (digest: 3852a61af000802215e40b6ad6830d71)
# Ignore VCS directories, you can use the same kind of rule outside
# OASIS_START/STOP if you want to exclude directories that contains
# useless stuff for the build process
@@ -17,8 +17,8 @@
"_darcs": -traverse
"_darcs": not_hygienic
# Library krobot
-"src/lib": include
-<src/lib/krobot.{cma,cmxa}>: use_libkrobot
+"src/lib/krobot.cmxs": use_krobot
+<src/lib/krobot.{cma,cmxa}>: use_libkrobot_stubs
<src/lib/*.ml{,i}>: pkg_lwt.unix
<src/lib/*.ml{,i}>: pkg_lwt.syntax
<src/lib/*.ml{,i}>: pkg_lwt.react
@@ -26,8 +26,8 @@
"src/lib/krobot_message_stubs.c": pkg_lwt.syntax
"src/lib/krobot_message_stubs.c": pkg_lwt.react
# Library krobot-can
-"src/can": include
-<src/can/krobot-can.{cma,cmxa}>: use_libkrobot-can
+"src/can/krobot-can.cmxs": use_krobot-can
+<src/can/krobot-can.{cma,cmxa}>: use_libkrobot-can_stubs
<src/can/*.ml{,i}>: use_krobot
<src/can/*.ml{,i}>: pkg_sexplib.syntax
<src/can/*.ml{,i}>: pkg_sexplib
@@ -62,10 +62,6 @@
<src/tools/krobot_can_display.{native,byte}>: pkg_lwt.glib
<src/tools/krobot_can_display.{native,byte}>: pkg_cairo.lablgtk2
<src/tools/krobot_can_display.{native,byte}>: pkg_bitstring
-<src/tools/*.ml{,i}>: use_krobot-can
-<src/tools/*.ml{,i}>: pkg_sexplib.syntax
-<src/tools/*.ml{,i}>: pkg_sexplib
-<src/tools/*.ml{,i}>: pkg_bitstring
# Executable krobot-calibrate-sharps
<src/tools/krobot_calibrate_sharps.{native,byte}>: use_krobot
<src/tools/krobot_calibrate_sharps.{native,byte}>: pkg_lwt.unix
@@ -86,6 +82,19 @@
<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_lwt.react
+# Executable krobot-ax12-control
+<src/tools/krobot_ax12_control.{native,byte}>: use_krobot-can
+<src/tools/krobot_ax12_control.{native,byte}>: use_krobot
+<src/tools/krobot_ax12_control.{native,byte}>: pkg_sexplib.syntax
+<src/tools/krobot_ax12_control.{native,byte}>: pkg_sexplib
+<src/tools/krobot_ax12_control.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_ax12_control.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_ax12_control.{native,byte}>: pkg_lwt.react
+<src/tools/krobot_ax12_control.{native,byte}>: pkg_bitstring
+<src/tools/*.ml{,i}>: use_krobot-can
+<src/tools/*.ml{,i}>: pkg_sexplib.syntax
+<src/tools/*.ml{,i}>: pkg_sexplib
+<src/tools/*.ml{,i}>: pkg_bitstring
# Executable krobot-viewer
<src/tools/krobot_viewer.{native,byte}>: use_krobot
<src/tools/krobot_viewer.{native,byte}>: pkg_lwt.unix
diff --git a/info/control2011/myocamlbuild.ml b/info/control2011/myocamlbuild.ml
index 2cfed59..e871f21 100644
--- a/info/control2011/myocamlbuild.ml
+++ b/info/control2011/myocamlbuild.ml
@@ -1,5 +1,5 @@
(* OASIS_START *)
-(* DO NOT EDIT (digest: 4d333a06e10a6d542b31a757ed455f80) *)
+(* DO NOT EDIT (digest: 5747542e19b7f9cfea0dfefe92aa4aee) *)
module OASISGettext = struct
# 21 "/media/data/ocaml/oasis/src/oasis/OASISGettext.ml"
@@ -330,19 +330,24 @@ module MyOCamlbuildBase = struct
open Ocamlbuild_plugin
+ module OC = Ocamlbuild_pack.Ocaml_compiler
type dir = string
type file = string
type name = string
type tag = string
-# 55 "/media/data/ocaml/oasis/src/plugins/ocamlbuild/MyOCamlbuildBase.ml"
+# 56 "/media/data/ocaml/oasis/src/plugins/ocamlbuild/MyOCamlbuildBase.ml"
type t =
{
lib_ocaml: (name * dir list) list;
lib_c: (name * dir * file list) list;
flags: (tag list * (spec OASISExpr.choices)) list;
+ (* Replace the 'dir: include' from _tags by a precise interdepends in
+ * directory.
+ *)
+ includes: (dir * dir list) list;
}
let env_filename =
@@ -355,6 +360,12 @@ module MyOCamlbuildBase = struct
(fun dispatch -> dispatch e)
lst
+ let tag_libstubs nm =
+ "use_lib"^nm^"_stubs"
+
+ let nm_libstubs nm =
+ nm^"_stubs"
+
let dispatch t e =
let env =
BaseEnvLight.load
@@ -382,40 +393,123 @@ module MyOCamlbuildBase = struct
Options.ext_dll, "ext_dll";
]
+ | Before_rules ->
+ (* TODO: move this into its own file and conditionnaly include it, if
+ * needed.
+ *)
+ (* OCaml cmxs rules: cmxs available in ocamlopt but not ocamlbuild.
+ Copied from ocaml_specific.ml in ocamlbuild sources. *)
+ let has_native_dynlink =
+ try
+ bool_of_string (BaseEnvLight.var_get "native_dynlink" env)
+ with Not_found ->
+ false
+ in
+ if has_native_dynlink && String.sub Sys.ocaml_version 0 4 = "3.11" then
+ begin
+ let ext_lib = !Options.ext_lib in
+ let ext_obj = !Options.ext_obj in
+ let ext_dll = !Options.ext_dll in
+ let x_o = "%"-.-ext_obj in
+ let x_a = "%"-.-ext_lib in
+ let x_dll = "%"-.-ext_dll in
+ let x_p_o = "%.p"-.-ext_obj in
+ let x_p_a = "%.p"-.-ext_lib in
+ let x_p_dll = "%.p"-.-ext_dll in
+
+ rule "ocaml: mldylib & p.cmx* & p.o* -> p.cmxs & p.so"
+ ~tags:["ocaml"; "native"; "profile"; "shared"; "library"]
+ ~prods:["%.p.cmxs"; x_p_dll]
+ ~dep:"%.mldylib"
+ (OC.native_profile_shared_library_link_mldylib
+ "%.mldylib" "%.p.cmxs");
+
+ rule "ocaml: mldylib & cmx* & o* -> cmxs & so"
+ ~tags:["ocaml"; "native"; "shared"; "library"]
+ ~prods:["%.cmxs"; x_dll]
+ ~dep:"%.mldylib"
+ (OC.native_shared_library_link_mldylib
+ "%.mldylib" "%.cmxs");
+
+ rule "ocaml: p.cmx & p.o -> p.cmxs & p.so"
+ ~tags:["ocaml"; "native"; "profile"; "shared"; "library"]
+ ~prods:["%.p.cmxs"; x_p_dll]
+ ~deps:["%.p.cmx"; x_p_o]
+ (OC.native_shared_library_link ~tags:["profile"]
+ "%.p.cmx" "%.p.cmxs");
+
+ rule "ocaml: p.cmxa & p.a -> p.cmxs & p.so"
+ ~tags:["ocaml"; "native"; "profile"; "shared"; "library"]
+ ~prods:["%.p.cmxs"; x_p_dll]
+ ~deps:["%.p.cmxa"; x_p_a]
+ (OC.native_shared_library_link ~tags:["profile"; "linkall"]
+ "%.p.cmxa" "%.p.cmxs");
+
+ rule "ocaml: cmx & o -> cmxs"
+ ~tags:["ocaml"; "native"; "shared"; "library"]
+ ~prods:["%.cmxs"]
+ ~deps:["%.cmx"; x_o]
+ (OC.native_shared_library_link "%.cmx" "%.cmxs");
+
+ rule "ocaml: cmx & o -> cmxs & so"
+ ~tags:["ocaml"; "native"; "shared"; "library"]
+ ~prods:["%.cmxs"; x_dll]
+ ~deps:["%.cmx"; x_o]
+ (OC.native_shared_library_link "%.cmx" "%.cmxs");
+
+ rule "ocaml: cmxa & a -> cmxs & so"
+ ~tags:["ocaml"; "native"; "shared"; "library"]
+ ~prods:["%.cmxs"; x_dll]
+ ~deps:["%.cmxa"; x_a]
+ (OC.native_shared_library_link ~tags:["linkall"]
+ "%.cmxa" "%.cmxs");
+ end
+
| After_rules ->
(* Declare OCaml libraries *)
List.iter
(function
- | lib, [] ->
- ocaml_lib lib;
- | lib, dir :: tl ->
- ocaml_lib ~dir:dir lib;
+ | nm, [] ->
+ ocaml_lib nm
+ | nm, dir :: tl ->
+ ocaml_lib ~dir:dir (dir^"/"^nm);
List.iter
(fun dir ->
- flag
- ["ocaml"; "use_"^lib; "compile"]
- (S[A"-I"; P dir]))
+ List.iter
+ (fun str ->
+ flag ["ocaml"; "use_"^nm; str] (S[A"-I"; P dir]))
+ ["compile"; "infer_interface"; "doc"])
tl)
t.lib_ocaml;
+ (* Declare directories dependencies, replace "include" in _tags. *)
+ List.iter
+ (fun (dir, include_dirs) ->
+ Pathname.define_context dir include_dirs)
+ t.includes;
+
(* Declare C libraries *)
List.iter
(fun (lib, dir, headers) ->
(* Handle C part of library *)
- flag ["link"; "library"; "ocaml"; "byte"; "use_lib"^lib]
- (S[A"-dllib"; A("-l"^lib); A"-cclib"; A("-l"^lib)]);
+ flag ["link"; "library"; "ocaml"; "byte"; tag_libstubs lib]
+ (S[A"-dllib"; A("-l"^(nm_libstubs lib)); A"-cclib";
+ A("-l"^(nm_libstubs lib))]);
- flag ["link"; "library"; "ocaml"; "native"; "use_lib"^lib]
- (S[A"-cclib"; A("-l"^lib)]);
+ flag ["link"; "library"; "ocaml"; "native"; tag_libstubs lib]
+ (S[A"-cclib"; A("-l"^(nm_libstubs lib))]);
- flag ["link"; "program"; "ocaml"; "byte"; "use_lib"^lib]
- (S[A"-dllib"; A("dll"^lib)]);
+ flag ["link"; "program"; "ocaml"; "byte"; tag_libstubs lib]
+ (S[A"-dllib"; A("dll"^(nm_libstubs lib))]);
(* When ocaml link something that use the C library, then one
need that file to be up to date.
*)
- dep ["link"; "ocaml"; "use_lib"^lib]
- [dir/"lib"^lib^"."^(!Options.ext_lib)];
+ dep ["link"; "ocaml"; "program"; tag_libstubs lib]
+ [dir/"lib"^(nm_libstubs lib)^"."^(!Options.ext_lib)];
+
+ dep ["compile"; "ocaml"; "program"; tag_libstubs lib]
+ [dir/"lib"^(nm_libstubs lib)^"."^(!Options.ext_lib)];
(* TODO: be more specific about what depends on headers *)
(* Depends on .h files *)
@@ -453,15 +547,22 @@ open Ocamlbuild_plugin;;
let package_default =
{
MyOCamlbuildBase.lib_ocaml =
- [("src/lib/krobot", ["src/lib"]); ("src/can/krobot-can", ["src/can"])];
+ [("krobot", ["src/lib"]); ("krobot-can", ["src/can"])];
lib_c = [("krobot", "src/lib", []); ("krobot-can", "src/can", [])];
flags = [];
+ includes =
+ [
+ ("src/tools", ["src/can"; "src/lib"]);
+ ("src/driver", ["src/can"]);
+ ("src/can", ["src/lib"]);
+ ("examples", ["src/can"])
+ ];
}
;;
let dispatch_default = MyOCamlbuildBase.dispatch_default package_default;;
-# 465 "myocamlbuild.ml"
+# 566 "myocamlbuild.ml"
(* OASIS_STOP *)
open Ocamlbuild_plugin
diff --git a/info/control2011/setup.ml b/info/control2011/setup.ml
index 8a23328..e2a975d 100644
--- a/info/control2011/setup.ml
+++ b/info/control2011/setup.ml
@@ -1,9 +1,9 @@
(* setup.ml generated for the first time by OASIS v0.2.0~alpha1 *)
(* OASIS_START *)
-(* DO NOT EDIT (digest: 300912e61b966bc3109c698735347ca1) *)
+(* DO NOT EDIT (digest: ebf28739fe9d63028b7611d27a835a73) *)
(*
- Regenerated by OASIS v0.2.1~alpha1
+ Regenerated by OASIS v0.3.0~rc3
Visit http://oasis.forge.ocamlcore.org for more information and
documentation about functions used in this file.
*)
@@ -90,6 +90,8 @@ end
module OASISUtils = struct
# 21 "/media/data/ocaml/oasis/src/oasis/OASISUtils.ml"
+ open OASISGettext
+
module MapString = Map.Make(String)
let map_string_of_assoc assoc =
@@ -224,6 +226,19 @@ module OASISUtils = struct
let failwithf fmt = Printf.ksprintf failwith fmt
+ let file_exists fn =
+ let dirname = Filename.dirname fn in
+ let basename = Filename.basename fn in
+ if Sys.file_exists dirname then
+ if basename = Filename.current_dir_name then
+ true
+ else
+ List.mem
+ basename
+ (Array.to_list (Sys.readdir dirname))
+ else
+ false
+
end
module PropList = struct
@@ -237,18 +252,23 @@ module PropList = struct
exception No_printer of name
exception Unknown_field of name * name
- let string_of_exception =
- function
- | Not_set (nm, Some rsn) ->
- Printf.sprintf (f_ "Field '%s' is not set: %s") nm rsn
- | Not_set (nm, None) ->
- Printf.sprintf (f_ "Field '%s' is not set") nm
- | No_printer nm ->
- Printf.sprintf (f_ "No default printer for value %s") nm
- | Unknown_field (nm, schm) ->
- Printf.sprintf (f_ "Field %s is not defined in schema %s") nm schm
- | e ->
- raise e
+ let () =
+ Printexc.register_printer
+ (function
+ | Not_set (nm, Some rsn) ->
+ Some
+ (Printf.sprintf (f_ "Field '%s' is not set: %s") nm rsn)
+ | Not_set (nm, None) ->
+ Some
+ (Printf.sprintf (f_ "Field '%s' is not set") nm)
+ | No_printer nm ->
+ Some
+ (Printf.sprintf (f_ "No default printer for value %s") nm)
+ | Unknown_field (nm, schm) ->
+ Some
+ (Printf.sprintf (f_ "Field %s is not defined in schema %s") nm schm)
+ | _ ->
+ None)
module Data =
struct
@@ -262,7 +282,7 @@ module PropList = struct
let clear t =
Hashtbl.clear t
-# 66 "/media/data/ocaml/oasis/src/oasis/PropList.ml"
+# 71 "/media/data/ocaml/oasis/src/oasis/PropList.ml"
end
module Schema =
@@ -535,20 +555,6 @@ module OASISMessage = struct
let error ~ctxt fmt =
generic_message ~ctxt `Error fmt
-
- let string_of_exception e =
- try
- PropList.string_of_exception e
- with
- | Failure s ->
- s
- | e ->
- Printexc.to_string e
-
- (* TODO
- let register_exn_printer f =
- *)
-
end
module OASISVersion = struct
@@ -981,6 +987,7 @@ module OASISTypes = struct
type library =
{
lib_modules: string list;
+ lib_pack: bool;
lib_internal_modules: string list;
lib_findlib_parent: findlib_name option;
lib_findlib_name: findlib_name option;
@@ -1176,7 +1183,8 @@ module OASISUnixPath = struct
end
module OASISSection = struct
-# 1 "/media/data/ocaml/oasis/src/oasis/OASISSection.ml"
+# 21 "/media/data/ocaml/oasis/src/oasis/OASISSection.ml"
+
open OASISTypes
let section_kind_common =
@@ -1259,7 +1267,7 @@ module OASISExecutable = struct
if not is_native_exec &&
not exec.exec_custom &&
bs.bs_c_sources <> [] then
- Some (dir^"/dll"^cs.cs_name^(ext_dll ()))
+ Some (dir^"/dll"^cs.cs_name^"_stubs"^(ext_dll ()))
else
None
@@ -1274,40 +1282,116 @@ module OASISLibrary = struct
type library_name = name
- let generated_unix_files ~ctxt (cs, bs, lib)
- source_file_exists is_native ext_lib ext_dll =
+ (* Look for a module file, considering capitalization or not. *)
+ let find_module source_file_exists (cs, bs, lib) modul =
+ let possible_base_fn =
+ List.map
+ (OASISUnixPath.concat bs.bs_path)
+ [modul;
+ OASISUnixPath.uncapitalize_file modul;
+ OASISUnixPath.capitalize_file modul]
+ in
+ (* TODO: we should be able to be able to determine the source for every
+ * files. Hence we should introduce a Module(source: fn) for the fields
+ * Modules and InternalModules
+ *)
+ List.fold_left
+ (fun acc base_fn ->
+ match acc with
+ | `No_sources _ ->
+ begin
+ let file_found =
+ List.fold_left
+ (fun acc ext ->
+ if source_file_exists (base_fn^ext) then
+ (base_fn^ext) :: acc
+ else
+ acc)
+ []
+ [".ml"; ".mli"; ".mll"; ".mly"]
+ in
+ match file_found with
+ | [] ->
+ acc
+ | lst ->
+ `Sources (base_fn, lst)
+ end
+ | `Sources _ ->
+ acc)
+ (`No_sources possible_base_fn)
+ possible_base_fn
+
+ let source_unix_files ~ctxt (cs, bs, lib) source_file_exists =
+ List.fold_left
+ (fun acc modul ->
+ match find_module source_file_exists (cs, bs, lib) modul with
+ | `Sources (base_fn, lst) ->
+ (base_fn, lst) :: acc
+ | `No_sources _ ->
+ OASISMessage.warning
+ ~ctxt
+ (f_ "Cannot find source file matching \
+ module '%s' in library %s")
+ modul cs.cs_name;
+ acc)
+ []
+ (lib.lib_modules @ lib.lib_internal_modules)
+
+ let generated_unix_files
+ ~ctxt
+ ~is_native
+ ~has_native_dynlink
+ ~ext_lib
+ ~ext_dll
+ ~source_file_exists
+ (cs, bs, lib) =
+
+ let find_modules lst ext =
+ let find_module modul =
+ match find_module source_file_exists (cs, bs, lib) modul with
+ | `Sources (base_fn, _) ->
+ [base_fn]
+ | `No_sources lst ->
+ OASISMessage.warning
+ ~ctxt
+ (f_ "Cannot find source file matching \
+ module '%s' in library %s")
+ modul cs.cs_name;
+ lst
+ in
+ List.map
+ (fun nm ->
+ List.map
+ (fun base_fn -> base_fn ^"."^ext)
+ (find_module nm))
+ lst
+ in
+
(* The headers that should be compiled along *)
let headers =
- List.fold_left
- (fun hdrs modul ->
- try
- let base_fn =
- List.find
- (fun fn ->
- source_file_exists (fn^".ml") ||
- source_file_exists (fn^".mli") ||
- source_file_exists (fn^".mll") ||
- source_file_exists (fn^".mly"))
- (List.map
- (OASISUnixPath.concat bs.bs_path)
- [modul;
- OASISUnixPath.uncapitalize_file modul;
- OASISUnixPath.capitalize_file modul])
- in
- [base_fn^".cmi"] :: hdrs
- with Not_found ->
- OASISMessage.warning
- ~ctxt
- (f_ "Cannot find source file matching \
- module '%s' in library %s")
- modul cs.cs_name;
- (List.map (OASISUnixPath.concat bs.bs_path)
- [modul^".cmi";
- OASISUnixPath.uncapitalize_file modul ^ ".cmi";
- OASISUnixPath.capitalize_file modul ^ ".cmi"])
- :: hdrs)
+ if lib.lib_pack then
[]
- lib.lib_modules
+ else
+ find_modules
+ lib.lib_modules
+ "cmi"
+ in
+
+ (* The .cmx that be compiled along *)
+ let cmxs =
+ let should_be_built =
+ (not lib.lib_pack) && (* Do not install .cmx packed submodules *)
+ match bs.bs_compiled_object with
+ | Native -> true
+ | Best -> is_native
+ | Byte -> false
+ in
+ if should_be_built then
+ find_modules
+ (lib.lib_modules @ lib.lib_internal_modules)
+ "cmx"
+ else
+ []
in
let acc_nopath =
@@ -1316,16 +1400,27 @@ module OASISLibrary = struct
(* Compute what libraries should be built *)
let acc_nopath =
+ (* Add the packed header file if required *)
+ let add_pack_header acc =
+ if lib.lib_pack then
+ [cs.cs_name^".cmi"] :: acc
+ else
+ acc
+ in
let byte acc =
- [cs.cs_name^".cma"] :: acc
+ add_pack_header ([cs.cs_name^".cma"] :: acc)
in
let native acc =
- [cs.cs_name^".cmxa"] :: [cs.cs_name^(ext_lib ())] :: acc
+ let acc = [cs.cs_name^".cmxa"] :: [cs.cs_name^ext_lib] :: acc in
+ add_pack_header
+ (if has_native_dynlink then
+ [cs.cs_name^".cmxs"] :: acc
+ else acc)
in
match bs.bs_compiled_object with
| Native ->
byte (native acc_nopath)
- | Best when is_native () ->
+ | Best when is_native ->
byte (native acc_nopath)
| Byte | Best ->
byte acc_nopath
@@ -1335,9 +1430,9 @@ module OASISLibrary = struct
let acc_nopath =
if bs.bs_c_sources <> [] then
begin
- ["lib"^cs.cs_name^(ext_lib ())]
+ ["lib"^cs.cs_name^"_stubs"^ext_lib]
::
- ["dll"^cs.cs_name^(ext_dll ())]
+ ["dll"^cs.cs_name^"_stubs"^ext_dll]
::
acc_nopath
end
@@ -1351,7 +1446,7 @@ module OASISLibrary = struct
(List.rev_map
(OASISUnixPath.concat bs.bs_path))
acc_nopath)
- headers
+ (headers @ cmxs)
type group_t =
@@ -1707,8 +1802,6 @@ module BaseMessage = struct
let error fmt = error ~ctxt:!default fmt
- let string_of_exception = string_of_exception
-
end
module BaseFilePath = struct
@@ -1884,9 +1977,9 @@ module BaseEnv = struct
let default =
[
- OFileLoad, lazy (MapString.find name !env_from_file);
+ OFileLoad, (fun () -> MapString.find name !env_from_file);
ODefault, dflt;
- OGetEnv, lazy (Sys.getenv name);
+ OGetEnv, (fun () -> Sys.getenv name);
]
in
@@ -1905,11 +1998,11 @@ module BaseEnv = struct
let var_get_low lst =
let errors, res =
List.fold_left
- (fun (errors, res) (_, v) ->
+ (fun (errors, res) (o, v) ->
if res = None then
begin
try
- errors, Some (Lazy.force v)
+ errors, Some (v ())
with
| Not_found ->
errors, res
@@ -1923,12 +2016,7 @@ module BaseEnv = struct
([], None)
(List.sort
(fun (o1, _) (o2, _) ->
- if o1 < o2 then
- 1
- else if o1 = o2 then
- 0
- else
- -1)
+ Pervasives.compare o2 o1)
lst)
in
match res, errors with
@@ -1950,7 +2038,7 @@ module BaseEnv = struct
FieldRO.create
~schema
~name
- ~parse:(fun ?(context=ODefault) s -> [context, lazy s])
+ ~parse:(fun ?(context=ODefault) s -> [context, fun () -> s])
~print:var_get_low
~default
~update:(fun ?context x old_x -> x @ old_x)
@@ -1972,7 +2060,8 @@ module BaseEnv = struct
dflt =
if Schema.mem schema name then
begin
- Schema.set schema env ~context:ODefault name (Lazy.force dflt);
+ (* TODO: look suspsicious, we want to memorize dflt not dflt () *)
+ Schema.set schema env ~context:ODefault name (dflt ());
fun () -> var_get name
end
else
@@ -1998,7 +2087,7 @@ module BaseEnv = struct
~cli:CLIAuto
~arg_help:"Print even non-printable variable. (debug)"
"print_hidden"
- (lazy "false")
+ (fun () -> "false")
let var_all () =
List.rev
@@ -2018,7 +2107,6 @@ module BaseEnv = struct
env_from_file := BaseEnvLight.load ?allow_empty ?filename ()
let unload () =
- (* TODO: reset lazy values *)
env_from_file := MapString.empty;
Data.clear env
@@ -2026,8 +2114,13 @@ module BaseEnv = struct
let chn =
open_out_bin filename
in
- Schema.iter
- (fun nm def _ ->
+ let output nm value =
+ Printf.fprintf chn "%s=%S\n" nm value
+ in
+ let mp_todo =
+ (* Dump data from schema *)
+ Schema.fold
+ (fun mp_todo nm def _ ->
if def.dump then
begin
try
@@ -2037,11 +2130,18 @@ module BaseEnv = struct
env
nm
in
- Printf.fprintf chn "%s = %S\n" nm value
+ output nm value
with Not_set _ ->
()
- end)
- schema;
+ end;
+ MapString.remove nm mp_todo)
+ !env_from_file
+ schema
+ in
+ (* Dump data defined outside of schema *)
+ MapString.iter output mp_todo;
+
+ (* End of the dump *)
close_out chn
let print () =
@@ -2173,23 +2273,21 @@ module BaseEnv = struct
Printf.sprintf (f_ "%s %s%s") arg_hlp hlp default_value
]
| CLIEnable ->
- [
- arg_concat "--enable-" arg_name,
- Arg.Unit (fun () -> var_set "true"),
- Printf.sprintf (f_ " %s%s") hlp
- (if default_value = " [true]" then
- (s_ " [default]")
- else
- "");
-
- arg_concat "--disable-" arg_name,
- Arg.Unit (fun () -> var_set "false"),
- Printf.sprintf (f_ " %s%s") hlp
- (if default_value = " [false]" then
- (s_ " [default]")
- else
- "");
- ]
+ let dflt =
+ if default_value = " [true]" then
+ s_ " [default: enabled]"
+ else
+ s_ " [default: disabled]"
+ in
+ [
+ arg_concat "--enable-" arg_name,
+ Arg.Unit (fun () -> var_set "true"),
+ Printf.sprintf (f_ " %s%s") hlp dflt;
+
+ arg_concat "--disable-" arg_name,
+ Arg.Unit (fun () -> var_set "false"),
+ Printf.sprintf (f_ " %s%s") hlp dflt
+ ]
| CLIUser lst ->
lst
in
@@ -2301,7 +2399,7 @@ module BaseFileUtil = struct
((combined_paths paths) * exts)
in
List.find
- Sys.file_exists
+ OASISUtils.file_exists
alternatives
let which prg =
@@ -2364,7 +2462,7 @@ module BaseFileUtil = struct
let tgt =
fix_dir tgt
in
- if Sys.file_exists tgt then
+ if OASISUtils.file_exists tgt then
begin
if not (Sys.is_directory tgt) then
OASISUtils.failwithf
@@ -2375,7 +2473,7 @@ module BaseFileUtil = struct
else
begin
mkdir_parent f (Filename.dirname tgt);
- if not (Sys.file_exists tgt) then
+ if not (OASISUtils.file_exists tgt) then
begin
f tgt;
mkdir tgt
@@ -2429,7 +2527,7 @@ module BaseFileUtil = struct
end
else
begin
- if Sys.file_exists fn then
+ if OASISUtils.file_exists fn then
[fn]
else
[]
@@ -2475,24 +2573,24 @@ module BaseCheck = struct
let prog_best prg prg_lst =
var_redefine
prg
- (lazy
- (let alternate =
- List.fold_left
- (fun res e ->
- match res with
- | Some _ ->
- res
- | None ->
- try
- Some (BaseFileUtil.which e)
- with Not_found ->
- None)
- None
- prg_lst
- in
- match alternate with
- | Some prg -> prg
- | None -> raise Not_found))
+ (fun () ->
+ let alternate =
+ List.fold_left
+ (fun res e ->
+ match res with
+ | Some _ ->
+ res
+ | None ->
+ try
+ Some (BaseFileUtil.which e)
+ with Not_found ->
+ None)
+ None
+ prg_lst
+ in
+ match alternate with
+ | Some prg -> prg
+ | None -> raise Not_found)
let prog prg =
prog_best prg [prg]
@@ -2515,33 +2613,33 @@ module BaseCheck = struct
var_redefine
~hide:true
var
- (lazy
- (let version_str =
- match fversion () with
- | "[Distributed with OCaml]" ->
- begin
- try
- (var_get "ocaml_version")
- with Not_found ->
- warning
- (f_ "Variable ocaml_version not defined, fallback \
- to default");
- Sys.ocaml_version
- end
- | res ->
- res
- in
- let version =
- OASISVersion.version_of_string version_str
- in
- if OASISVersion.comparator_apply version cmp then
- version_str
- else
- failwithf
- (f_ "Cannot satisfy version constraint on %s: %s (version: %s)")
- var_prefix
- (OASISVersion.string_of_comparator cmp)
- version_str))
+ (fun () ->
+ let version_str =
+ match fversion () with
+ | "[Distributed with OCaml]" ->
+ begin
+ try
+ (var_get "ocaml_version")
+ with Not_found ->
+ warning
+ (f_ "Variable ocaml_version not defined, fallback \
+ to default");
+ Sys.ocaml_version
+ end
+ | res ->
+ res
+ in
+ let version =
+ OASISVersion.version_of_string version_str
+ in
+ if OASISVersion.comparator_apply version cmp then
+ version_str
+ else
+ failwithf
+ (f_ "Cannot satisfy version constraint on %s: %s (version: %s)")
+ var_prefix
+ (OASISVersion.string_of_comparator cmp)
+ version_str)
()
let package_version pkg =
@@ -2572,7 +2670,7 @@ module BaseCheck = struct
let vl =
var_redefine
var
- (lazy (findlib_dir pkg))
+ (fun () -> findlib_dir pkg)
()
in
(
@@ -2648,18 +2746,23 @@ module BaseOCamlcConfig = struct
mp
in
+ let cache =
+ lazy
+ (var_protect
+ (Marshal.to_string
+ (split_field
+ SMap.empty
+ (BaseExec.run_read_output
+ (ocamlc ()) ["-config"]))
+ []))
+ in
var_redefine
"ocamlc_config_map"
~hide:true
~dump:false
- (lazy
- (var_protect
- (Marshal.to_string
- (split_field
- SMap.empty
- (BaseExec.run_read_output
- (ocamlc ()) ["-config"]))
- [])))
+ (fun () ->
+ (* TODO: update if ocamlc change !!! *)
+ Lazy.force cache)
let var_define nm =
(* Extract data from ocamlc -config *)
@@ -2683,20 +2786,20 @@ module BaseOCamlcConfig = struct
in
var_redefine
nm
- (lazy
- (try
- let map =
- avlbl_config_get ()
- in
- let value =
- SMap.find nm_config map
- in
- value_config value
- with Not_found ->
- failwithf
- (f_ "Cannot find field '%s' in '%s -config' output")
- nm
- (ocamlc ())))
+ (fun () ->
+ try
+ let map =
+ avlbl_config_get ()
+ in
+ let value =
+ SMap.find nm_config map
+ in
+ value_config value
+ with Not_found ->
+ failwithf
+ (f_ "Cannot find field '%s' in '%s -config' output")
+ nm
+ (ocamlc ()))
end
@@ -2730,13 +2833,13 @@ module BaseStandardVar = struct
var_define
~short_desc:(fun () -> s_ "Package name")
"pkg_name"
- (lazy (pkg_get ()).name)
+ (fun () -> (pkg_get ()).name)
let pkg_version =
var_define
~short_desc:(fun () -> s_ "Package version")
"pkg_version"
- (lazy
+ (fun () ->
(OASISVersion.string_of_version (pkg_get ()).version))
let c = BaseOCamlcConfig.var_define
@@ -2785,174 +2888,216 @@ module BaseStandardVar = struct
let prefix =
p "prefix"
(fun () -> s_ "Install architecture-independent files dir")
- (lazy
- (match os_type () with
- | "Win32" ->
- let program_files =
- Sys.getenv "PROGRAMFILES"
- in
- program_files/(pkg_name ())
- | _ ->
- "/usr/local"))
+ (fun () ->
+ match os_type () with
+ | "Win32" ->
+ let program_files =
+ Sys.getenv "PROGRAMFILES"
+ in
+ program_files/(pkg_name ())
+ | _ ->
+ "/usr/local")
let exec_prefix =
p "exec_prefix"
(fun () -> s_ "Install architecture-dependent files in dir")
- (lazy "$prefix")
+ (fun () -> "$prefix")
let bindir =
p "bindir"
(fun () -> s_ "User executables")
- (lazy ("$exec_prefix"/"bin"))
+ (fun () -> "$exec_prefix"/"bin")
let sbindir =
p "sbindir"
(fun () -> s_ "System admin executables")
- (lazy ("$exec_prefix"/"sbin"))
+ (fun () -> "$exec_prefix"/"sbin")
let libexecdir =
p "libexecdir"
(fun () -> s_ "Program executables")
- (lazy ("$exec_prefix"/"libexec"))
+ (fun () -> "$exec_prefix"/"libexec")
let sysconfdir =
p "sysconfdir"
(fun () -> s_ "Read-only single-machine data")
- (lazy ("$prefix"/"etc"))
+ (fun () -> "$prefix"/"etc")
let sharedstatedir =
p "sharedstatedir"
(fun () -> s_ "Modifiable architecture-independent data")
- (lazy ("$prefix"/"com"))
+ (fun () -> "$prefix"/"com")
let localstatedir =
p "localstatedir"
(fun () -> s_ "Modifiable single-machine data")
- (lazy ("$prefix"/"var"))
+ (fun () -> "$prefix"/"var")
let libdir =
p "libdir"
(fun () -> s_ "Object code libraries")
- (lazy ("$exec_prefix"/"lib"))
+ (fun () -> "$exec_prefix"/"lib")
let datarootdir =
p "datarootdir"
(fun () -> s_ "Read-only arch-independent data root")
- (lazy ("$prefix"/"share"))
+ (fun () -> "$prefix"/"share")
let datadir =
p "datadir"
(fun () -> s_ "Read-only architecture-independent data")
- (lazy ("$datarootdir"))
+ (fun () -> "$datarootdir")
let infodir =
p "infodir"
(fun () -> s_ "Info documentation")
- (lazy ("$datarootdir"/"info"))
+ (fun () -> "$datarootdir"/"info")
let localedir =
p "localedir"
(fun () -> s_ "Locale-dependent data")
- (lazy ("$datarootdir"/"locale"))
+ (fun () -> "$datarootdir"/"locale")
let mandir =
p "mandir"
(fun () -> s_ "Man documentation")
- (lazy ("$datarootdir"/"man"))
+ (fun () -> "$datarootdir"/"man")
let docdir =
p "docdir"
(fun () -> s_ "Documentation root")
- (lazy ("$datarootdir"/"doc"/"$pkg_name"))
+ (fun () -> "$datarootdir"/"doc"/"$pkg_name")
let htmldir =
p "htmldir"
(fun () -> s_ "HTML documentation")
- (lazy ("$docdir"))
+ (fun () -> "$docdir")
let dvidir =
p "dvidir"
(fun () -> s_ "DVI documentation")
- (lazy ("$docdir"))
+ (fun () -> "$docdir")
let pdfdir =
p "pdfdir"
(fun () -> s_ "PDF documentation")
- (lazy ("$docdir"))
+ (fun () -> "$docdir")
let psdir =
p "psdir"
(fun () -> s_ "PS documentation")
- (lazy ("$docdir"))
+ (fun () -> "$docdir")
let destdir =
p "destdir"
(fun () -> s_ "Prepend a path when installing package")
- (lazy
- (raise
- (PropList.Not_set
- ("destdir",
- Some (s_ "undefined by construct")))))
+ (fun () ->
+ raise
+ (PropList.Not_set
+ ("destdir",
+ Some (s_ "undefined by construct"))))
let findlib_version =
var_define
"findlib_version"
- (lazy
- (BaseCheck.package_version "findlib"))
+ (fun () ->
+ BaseCheck.package_version "findlib")
let is_native =
var_define
"is_native"
- (lazy
- (try
- let _s : string =
- ocamlopt ()
- in
- "true"
- with PropList.Not_set _ ->
- let _s : string =
- ocamlc ()
- in
- "false"))
+ (fun () ->
+ try
+ let _s : string =
+ ocamlopt ()
+ in
+ "true"
+ with PropList.Not_set _ ->
+ let _s : string =
+ ocamlc ()
+ in
+ "false")
let ext_program =
var_define
"suffix_program"
- (lazy
- (match os_type () with
- | "Win32" -> ".exe"
- | _ -> ""
- ))
+ (fun () ->
+ match os_type () with
+ | "Win32" -> ".exe"
+ | _ -> "")
let rm =
var_define
~short_desc:(fun () -> s_ "Remove a file.")
"rm"
- (lazy
- (match os_type () with
- | "Win32" -> "del"
- | _ -> "rm -f"))
+ (fun () ->
+ match os_type () with
+ | "Win32" -> "del"
+ | _ -> "rm -f")
let rmdir =
var_define
~short_desc:(fun () -> s_ "Remove a directory.")
"rmdir"
- (lazy
- (match os_type () with
- | "Win32" -> "rd"
- | _ -> "rm -rf"))
+ (fun () ->
+ match os_type () with
+ | "Win32" -> "rd"
+ | _ -> "rm -rf")
let debug =
var_define
- ~short_desc:(fun () -> s_ "Compile with ocaml debug flag on.")
+ ~short_desc:(fun () -> s_ "Turn ocaml debug flag on")
+ ~cli:CLIEnable
"debug"
- (lazy "true")
+ (fun () -> "true")
let profile =
var_define
- ~short_desc:(fun () -> s_ "Compile with ocaml profile flag on.")
+ ~short_desc:(fun () -> s_ "Turn ocaml profile flag on")
+ ~cli:CLIEnable
"profile"
- (lazy "false")
+ (fun () -> "false")
+
+ let tests =
+ var_define
+ ~short_desc:(fun () ->
+ s_ "Compile tests executable and library and run them")
+ ~cli:CLIEnable
+ "tests"
+ (fun () -> "false")
+
+ let docs =
+ var_define
+ ~short_desc:(fun () -> s_ "Create documentations")
+ ~cli:CLIEnable
+ "docs"
+ (fun () -> "true")
+
+ let native_dynlink =
+ var_define
+ ~short_desc:(fun () -> s_ "Compiler support generation of .cmxs.")
+ ~cli:CLINone
+ "native_dynlink"
+ (fun () ->
+ let res =
+ if bool_of_string (is_native ()) then
+ begin
+ let ocamlfind = ocamlfind () in
+ try
+ let fn =
+ BaseExec.run_read_one_line
+ ocamlfind
+ ["query"; "-predicates"; "native"; "dynlink";
+ "-format"; "%d/%a"]
+ in
+ Sys.file_exists fn
+ with _ ->
+ false
+ end
+ else
+ false
+ in
+ string_of_bool res)
let init pkg =
rpkg := Some pkg
@@ -3041,7 +3186,7 @@ module BaseLog = struct
begin
let acc =
try
- Scanf.bscanf scbuf "%S %S@\n"
+ Scanf.bscanf scbuf "%S %S\n"
(fun e d ->
let t =
e, d
@@ -3161,7 +3306,7 @@ module BaseBuilt = struct
let registered =
List.fold_left
(fun registered fn ->
- if Sys.file_exists fn then
+ if OASISUtils.file_exists fn then
begin
BaseLog.register
(to_log_event_file t nm)
@@ -3193,7 +3338,7 @@ module BaseBuilt = struct
let fold t nm f acc =
List.fold_left
(fun acc (_, fn) ->
- if Sys.file_exists fn then
+ if OASISUtils.file_exists fn then
begin
f acc fn
end
@@ -3256,13 +3401,13 @@ module BaseBuilt = struct
let unix_lst =
OASISLibrary.generated_unix_files
~ctxt:!BaseContext.default
+ ~source_file_exists:(fun fn ->
+ OASISUtils.file_exists (BaseFilePath.of_unix fn))
+ ~is_native:(bool_of_string (is_native ()))
+ ~has_native_dynlink:(bool_of_string (native_dynlink ()))
+ ~ext_lib:(ext_lib ())
+ ~ext_dll:(ext_dll ())
(cs, bs, lib)
- (fun fn ->
- Sys.file_exists (BaseFilePath.of_unix fn))
- (fun () ->
- bool_of_string (is_native ()))
- ext_lib
- ext_dll
in
let evs =
[BLib,
@@ -3336,31 +3481,32 @@ module BaseDynVar = struct
List.iter
(function
| Executable (cs, bs, exec) ->
- var_ignore
- (var_redefine
- (* We don't save this variable *)
- ~dump:false
- ~short_desc:(fun () ->
- Printf.sprintf
- (f_ "Filename of executable '%s'")
- cs.cs_name)
- cs.cs_name
- (lazy
- (let fn_opt =
- fold
- BExec cs.cs_name
- (fun _ fn -> Some fn)
- None
- in
- match fn_opt with
- | Some fn -> fn
- | None ->
- raise
- (PropList.Not_set
- (cs.cs_name,
- Some (Printf.sprintf
- (f_ "Executable '%s' not yet built.")
- cs.cs_name))))))
+ if var_choose bs.bs_build then
+ var_ignore
+ (var_redefine
+ (* We don't save this variable *)
+ ~dump:false
+ ~short_desc:(fun () ->
+ Printf.sprintf
+ (f_ "Filename of executable '%s'")
+ cs.cs_name)
+ cs.cs_name
+ (fun () ->
+ let fn_opt =
+ fold
+ BExec cs.cs_name
+ (fun _ fn -> Some fn)
+ None
+ in
+ match fn_opt with
+ | Some fn -> fn
+ | None ->
+ raise
+ (PropList.Not_set
+ (cs.cs_name,
+ Some (Printf.sprintf
+ (f_ "Executable '%s' not yet built.")
+ cs.cs_name)))))
| Library _ | Flag _ | Test _ | SrcRepo _ | Doc _ ->
())
@@ -3497,20 +3643,24 @@ module BaseSetup = struct
type t =
{
- configure: std_args_fun;
- build: std_args_fun;
- doc: ((doc, unit) section_args_fun) list;
- test: ((test, float) section_args_fun) list;
- install: std_args_fun;
- uninstall: std_args_fun;
- clean: std_args_fun list;
- clean_doc: (doc, unit) section_args_fun list;
- clean_test: (test, unit) section_args_fun list;
- distclean: std_args_fun list;
- distclean_doc: (doc, unit) section_args_fun list;
- distclean_test: (test, unit) section_args_fun list;
- package: package;
- version: string;
+ configure: std_args_fun;
+ build: std_args_fun;
+ doc: ((doc, unit) section_args_fun) list;
+ test: ((test, float) section_args_fun) list;
+ install: std_args_fun;
+ uninstall: std_args_fun;
+ clean: std_args_fun list;
+ clean_doc: (doc, unit) section_args_fun list;
+ clean_test: (test, unit) section_args_fun list;
+ distclean: std_args_fun list;
+ distclean_doc: (doc, unit) section_args_fun list;
+ distclean_test: (test, unit) section_args_fun list;
+ package: package;
+ oasis_fn: string option;
+ oasis_version: string;
+ oasis_digest: Digest.t option;
+ oasis_exec: string option;
+ oasis_setup_args: string list;
}
(* Associate a plugin function with data from package *)
@@ -3541,13 +3691,30 @@ module BaseSetup = struct
(* Run configure *)
BaseCustom.hook
t.package.conf_custom
- (t.configure t.package)
- args;
+ (fun () ->
+ (* Reload if preconf has changed it *)
+ begin
+ try
+ unload ();
+ load ();
+ with _ ->
+ ()
+ end;
+
+ (* Run plugin's configure *)
+ t.configure t.package args;
+
+ (* Dump to allow postconf to change it *)
+ dump ())
+ ();
(* Reload environment *)
unload ();
load ();
+ (* Save environment *)
+ print ();
+
(* Replace data in file *)
BaseFileAB.replace t.package.files_ab
@@ -3558,42 +3725,52 @@ module BaseSetup = struct
args
let doc t args =
- BaseDoc.doc
- (join_plugin_sections
- (function
- | Doc (cs, e) ->
- Some
- (lookup_plugin_section
- "documentation"
- (s_ "build")
- cs.cs_name
- t.doc,
- cs,
- e)
- | _ ->
- None)
- t.package.sections)
- t.package
- args
+ if bool_of_string (BaseStandardVar.docs ()) then
+ BaseDoc.doc
+ (join_plugin_sections
+ (function
+ | Doc (cs, e) ->
+ Some
+ (lookup_plugin_section
+ "documentation"
+ (s_ "build")
+ cs.cs_name
+ t.doc,
+ cs,
+ e)
+ | _ ->
+ None)
+ t.package.sections)
+ t.package
+ args
+ else
+ BaseMessage.warning
+ "Docs are turned off, consider enabling with \
+ 'ocaml setup.ml -configure --enable-docs'"
let test t args =
- BaseTest.test
- (join_plugin_sections
- (function
- | Test (cs, e) ->
- Some
- (lookup_plugin_section
- "test"
- (s_ "run")
- cs.cs_name
- t.test,
- cs,
- e)
- | _ ->
- None)
- t.package.sections)
- t.package
- args
+ if bool_of_string (BaseStandardVar.tests ()) then
+ BaseTest.test
+ (join_plugin_sections
+ (function
+ | Test (cs, e) ->
+ Some
+ (lookup_plugin_section
+ "test"
+ (s_ "run")
+ cs.cs_name
+ t.test,
+ cs,
+ e)
+ | _ ->
+ None)
+ t.package.sections)
+ t.package
+ args
+ else
+ BaseMessage.warning
+ "Tests are turned off, consider enabling with \
+ 'ocaml setup.ml -configure --enable-tests'"
let all t args =
let rno_doc =
@@ -3761,7 +3938,134 @@ module BaseSetup = struct
clean, distclean
let version t _ =
- print_endline t.version
+ print_endline t.oasis_version
+
+ let update_setup_ml, no_update_setup_ml_cli =
+ let b = ref true in
+ b,
+ ("-no-update-setup-ml",
+ Arg.Clear b,
+ s_ " Don't try to update setup.ml, even if _oasis has changed.")
+
+ let update_setup_ml t =
+ let oasis_fn =
+ match t.oasis_fn with
+ | Some fn -> fn
+ | None -> "_oasis"
+ in
+ let oasis_exec =
+ match t.oasis_exec with
+ | Some fn -> fn
+ | None -> "oasis"
+ in
+ let ocaml =
+ Sys.executable_name
+ in
+ let setup_ml, args =
+ match Array.to_list Sys.argv with
+ | setup_ml :: args ->
+ setup_ml, args
+ | [] ->
+ failwith
+ (s_ "Expecting non-empty command line arguments.")
+ in
+ let ocaml, setup_ml =
+ if Sys.executable_name = Sys.argv.(0) then
+ (* We are not running in standard mode, probably the script
+ * is precompiled.
+ *)
+ "ocaml", "setup.ml"
+ else
+ ocaml, setup_ml
+ in
+ let no_update_setup_ml_cli, _, _ = no_update_setup_ml_cli in
+ let do_update () =
+ let oasis_exec_version =
+ BaseExec.run_read_one_line
+ ~f_exit_code:
+ (function
+ | 0 ->
+ ()
+ | 1 ->
+ failwithf
+ (f_ "Executable '%s' is probably an old version \
+ of oasis (< 0.3.0), please update to version \
+ v%s.")
+ oasis_exec t.oasis_version
+ | 127 ->
+ failwithf
+ (f_ "Cannot find executable '%s', please install \
+ oasis v%s.")
+ oasis_exec t.oasis_version
+ | n ->
+ failwithf
+ (f_ "Command '%s version' exited with code %d.")
+ oasis_exec n)
+ oasis_exec ["version"]
+ in
+ if OASISVersion.comparator_apply
+ (OASISVersion.version_of_string oasis_exec_version)
+ (OASISVersion.VGreaterEqual
+ (OASISVersion.version_of_string t.oasis_version)) then
+ begin
+ (* We have a version >= for the executable oasis, proceed with
+ * update.
+ *)
+ (* TODO: delegate this check to 'oasis setup'. *)
+ if Sys.os_type = "Win32" then
+ failwithf
+ (f_ "It is not possible to update the running script \
+ setup.ml on Windows. Please update setup.ml by \
+ running '%s'.")
+ (String.concat " " (oasis_exec :: "setup" :: t.oasis_setup_args))
+ else
+ begin
+ BaseExec.run
+ ~f_exit_code:
+ (function
+ | 0 ->
+ ()
+ | n ->
+ failwithf
+ (f_ "Unable to update setup.ml using '%s', \
+ please fix the problem and retry.")
+ oasis_exec)
+ oasis_exec ("setup" :: t.oasis_setup_args);
+ BaseExec.run ocaml (setup_ml :: args)
+ end
+ end
+ else
+ failwithf
+ (f_ "The version of '%s' (v%s) doesn't match the version of \
+ oasis used to generate the %s file. Please install at \
+ least oasis v%s.")
+ oasis_exec oasis_exec_version setup_ml t.oasis_version
+ in
+
+ if !update_setup_ml then
+ begin
+ try
+ match t.oasis_digest with
+ | Some dgst ->
+ if Sys.file_exists oasis_fn && dgst <> Digest.file "_oasis" then
+ begin
+ do_update ();
+ true
+ end
+ else
+ false
+ | None ->
+ false
+ with e ->
+ error
+ (f_ "Error when updating setup.ml. If you want to avoid this error, \
+ you can bypass the update of %s by running '%s %s %s %s'")
+ setup_ml ocaml setup_ml no_update_setup_ml_cli
+ (String.concat " " args);
+ raise e
+ end
+ else
+ false
let setup t =
let catch_exn =
@@ -3847,6 +4151,8 @@ module BaseSetup = struct
"-no-catch-exn",
Arg.Clear catch_exn,
s_ " Don't catch exception, useful for debugging.";
+
+ no_update_setup_ml_cli;
]
@ (BaseContext.args ()))
(failwithf (f_ "Don't know what to do with '%s'"))
@@ -3867,13 +4173,14 @@ module BaseSetup = struct
~cli:CLIEnable
?short_desc
(OASISUtils.varname_of_string cs.cs_name)
- (lazy (string_of_bool
- (var_choose
- ~name:(Printf.sprintf
- (f_ "default value of flag %s")
- cs.cs_name)
- ~printer:string_of_bool
- choices))))
+ (fun () ->
+ string_of_bool
+ (var_choose
+ ~name:(Printf.sprintf
+ (f_ "default value of flag %s")
+ cs.cs_name)
+ ~printer:string_of_bool
+ choices)))
in
match hlp with
| Some hlp ->
@@ -3889,71 +4196,15 @@ module BaseSetup = struct
BaseDynVar.init t.package;
- !act_ref t (Array.of_list (List.rev !extra_args_ref))
+ if not (update_setup_ml t) then
+ !act_ref t (Array.of_list (List.rev !extra_args_ref))
with e when !catch_exn ->
- error "%s" (string_of_exception e);
+ error "%s" (Printexc.to_string e);
exit 1
end
-module BaseDev = struct
-# 21 "/media/data/ocaml/oasis/src/base/BaseDev.ml"
-
-
-
- open OASISGettext
- open BaseMessage
-
- type t =
- {
- oasis_cmd: string;
- }
-
- let update_and_run t =
- (* Command line to run setup-dev *)
- let oasis_args =
- "setup-dev" :: "-run" ::
- Sys.executable_name ::
- (Array.to_list Sys.argv)
- in
-
- let exit_on_child_error =
- function
- | 0 -> ()
- | 2 ->
- (* Bad CLI arguments *)
- error
- (f_ "The command '%s %s' exit with code 2. It often means that we \
- don't use the right command-line arguments, rerun \
- 'oasis setup-dev'.")
- t.oasis_cmd
- (String.concat " " oasis_args)
-
- | 127 ->
- (* Cannot find OASIS *)
- error
- (f_ "Cannot find executable '%s', check where 'oasis' is located \
- and rerun 'oasis setup-dev'")
- t.oasis_cmd
-
- | i ->
- exit i
- in
-
- let () =
- (* Run OASIS to generate a temporary setup.ml
- *)
- BaseExec.run
- ~f_exit_code:exit_on_child_error
- t.oasis_cmd
- oasis_args
- in
-
- ()
-
-end
-
module InternalConfigurePlugin = struct
# 21 "/media/data/ocaml/oasis/src/plugins/internal/InternalConfigurePlugin.ml"
@@ -3997,7 +4248,7 @@ module InternalConfigurePlugin = struct
in
let warn_exception e =
- warning "%s" (string_of_exception e)
+ warning "%s" (Printexc.to_string e)
in
(* Check tools *)
@@ -4150,13 +4401,28 @@ module InternalConfigurePlugin = struct
())
pkg.sections;
- (* Save and print environment *)
- if SetString.empty = !errors then
- begin
- dump ();
- print ()
- end
- else
+ (* Check if we need native dynlink (presence of libraries that compile to
+ * native)
+ *)
+ begin
+ let has_cmxa =
+ List.exists
+ (function
+ | Library (_, bs, _) ->
+ var_choose bs.bs_build &&
+ (bs.bs_compiled_object = Native ||
+ (bs.bs_compiled_object = Best &&
+ bool_of_string (BaseStandardVar.is_native ())))
+ | _ ->
...
[truncated message content] |
|
From: Pierre C. <Ba...@us...> - 2012-04-23 22:41:06
|
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 52ee072e134d531d77ee2de21fc7824ce8bde751 (commit)
from 9ddc98c3ea90aee3feff58d375a76516b7553b02 (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 52ee072e134d531d77ee2de21fc7824ce8bde751
Author: chicco <cha...@cr...>
Date: Tue Apr 24 00:40:53 2012 +0200
[krobot_viewer] change vbox to hbox for bezier limits
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/tools/krobot_viewer_ui.glade b/info/control2011/src/tools/krobot_viewer_ui.glade
index 4abeca2..d8af3cf 100644
--- a/info/control2011/src/tools/krobot_viewer_ui.glade
+++ b/info/control2011/src/tools/krobot_viewer_ui.glade
@@ -767,7 +767,7 @@
</packing>
</child>
<child>
- <widget class="GtkVBox" id="bezier_limits_vbox">
+ <widget class="GtkHBox" id="bezier_limits_vbox">
<property name="visible">True</property>
<child>
<widget class="GtkVBox" id="v_max_box">
hooks/post-receive
--
krobot
|
|
From: Pierre C. <Ba...@us...> - 2012-04-23 22:25:34
|
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 9ddc98c3ea90aee3feff58d375a76516b7553b02 (commit)
from 87263167c3172b765e0232d7e424ded68d057c95 (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 9ddc98c3ea90aee3feff58d375a76516b7553b02
Author: chicco <cha...@cr...>
Date: Tue Apr 24 00:25:23 2012 +0200
[krobot_viewer] commit glade file
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/tools/krobot_viewer_ui.glade b/info/control2011/src/tools/krobot_viewer_ui.glade
index 238adf1..4abeca2 100644
--- a/info/control2011/src/tools/krobot_viewer_ui.glade
+++ b/info/control2011/src/tools/krobot_viewer_ui.glade
@@ -592,83 +592,173 @@
<property name="can_focus">True</property>
<property name="expanded">True</property>
<child>
- <widget class="GtkHBox" id="settings_subbox">
+ <widget class="GtkHBox" id="settings_overbox">
<property name="visible">True</property>
- <property name="homogeneous">True</property>
+ <property name="orientation">vertical</property>
<child>
- <widget class="GtkVBox" id="line_settings_box">
+ <widget class="GtkHBox" id="settings_subbox">
<property name="visible">True</property>
- <property name="orientation">vertical</property>
- <child>
- <widget class="GtkLabel" id="line_settings_label">
- <property name="visible">True</property>
- <property name="label" translatable="yes">Lines</property>
- </widget>
- <packing>
- <property name="position">0</property>
- </packing>
- </child>
+ <property name="homogeneous">True</property>
<child>
- <widget class="GtkHBox" id="line_settings_label_box">
+ <widget class="GtkVBox" id="line_settings_box">
<property name="visible">True</property>
- <property name="homogeneous">True</property>
+ <property name="orientation">vertical</property>
<child>
- <widget class="GtkLabel" id="lines_speed_label">
+ <widget class="GtkLabel" id="line_settings_label">
<property name="visible">True</property>
- <property name="label" translatable="yes">speed</property>
+ <property name="label" translatable="yes">Lines</property>
</widget>
<packing>
<property name="position">0</property>
</packing>
</child>
<child>
- <widget class="GtkLabel" id="lines_acceleration_label">
+ <widget class="GtkHBox" id="line_settings_label_box">
<property name="visible">True</property>
- <property name="yalign">0.47999998927116394</property>
- <property name="label" translatable="yes">accel</property>
+ <property name="homogeneous">True</property>
+ <child>
+ <widget class="GtkLabel" id="lines_speed_label">
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">speed</property>
+ </widget>
+ <packing>
+ <property name="position">0</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkLabel" id="lines_acceleration_label">
+ <property name="visible">True</property>
+ <property name="yalign">0.47999998927116394</property>
+ <property name="label" translatable="yes">accel</property>
+ </widget>
+ <packing>
+ <property name="position">1</property>
+ </packing>
+ </child>
</widget>
<packing>
<property name="position">1</property>
</packing>
</child>
+ <child>
+ <widget class="GtkHBox" id="line_settings_value_box">
+ <property name="visible">True</property>
+ <property name="homogeneous">True</property>
+ <child>
+ <widget class="GtkSpinButton" id="moving_speed">
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="invisible_char">●</property>
+ <property name="adjustment">0.99999999999999989 0 2 0.10000000000000001 0 0</property>
+ <property name="digits">2</property>
+ <property name="numeric">True</property>
+ </widget>
+ <packing>
+ <property name="position">0</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkSpinButton" id="moving_acceleration">
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="invisible_char">●</property>
+ <property name="adjustment">1 0 10 0.10000000000000001 0 0</property>
+ <property name="digits">2</property>
+ <property name="numeric">True</property>
+ </widget>
+ <packing>
+ <property name="position">1</property>
+ </packing>
+ </child>
+ </widget>
+ <packing>
+ <property name="position">2</property>
+ </packing>
+ </child>
</widget>
<packing>
- <property name="position">1</property>
+ <property name="position">0</property>
</packing>
</child>
<child>
- <widget class="GtkHBox" id="line_settings_value_box">
+ <widget class="GtkVBox" id="rotation_settings_box">
<property name="visible">True</property>
- <property name="homogeneous">True</property>
+ <property name="orientation">vertical</property>
<child>
- <widget class="GtkSpinButton" id="moving_speed">
+ <widget class="GtkLabel" id="rotation_settings_label">
<property name="visible">True</property>
- <property name="can_focus">True</property>
- <property name="invisible_char">•</property>
- <property name="adjustment">0.99999999999999989 0 2 0.10000000000000001 0 0</property>
- <property name="digits">2</property>
- <property name="numeric">True</property>
+ <property name="label" translatable="yes">Rotation</property>
+ <property name="selectable">True</property>
</widget>
<packing>
<property name="position">0</property>
</packing>
</child>
<child>
- <widget class="GtkSpinButton" id="moving_acceleration">
+ <widget class="GtkHBox" id="rotation_settings_label_box">
<property name="visible">True</property>
- <property name="can_focus">True</property>
- <property name="invisible_char">•</property>
- <property name="adjustment">1 0 10 0.10000000000000001 0 0</property>
- <property name="digits">2</property>
- <property name="numeric">True</property>
+ <property name="homogeneous">True</property>
+ <child>
+ <widget class="GtkLabel" id="rotation_speed_label">
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">speed</property>
+ </widget>
+ <packing>
+ <property name="position">0</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkLabel" id="rotation_acceleration_label">
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">accel</property>
+ </widget>
+ <packing>
+ <property name="position">1</property>
+ </packing>
+ </child>
</widget>
<packing>
<property name="position">1</property>
</packing>
</child>
+ <child>
+ <widget class="GtkHBox" id="rotation_settings_value_box">
+ <property name="visible">True</property>
+ <property name="homogeneous">True</property>
+ <child>
+ <widget class="GtkSpinButton" id="rotation_speed">
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="invisible_char">●</property>
+ <property name="adjustment">2.0000000000000004 0 4 0.10000000000000001 0 0</property>
+ <property name="digits">2</property>
+ <property name="numeric">True</property>
+ </widget>
+ <packing>
+ <property name="position">0</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkSpinButton" id="rotation_acceleration">
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="invisible_char">●</property>
+ <property name="adjustment">2.0000000000000009 0 10 0.10000000000000001 0 0</property>
+ <property name="digits">2</property>
+ <property name="numeric">True</property>
+ </widget>
+ <packing>
+ <property name="position">1</property>
+ </packing>
+ </child>
+ </widget>
+ <packing>
+ <property name="position">2</property>
+ </packing>
+ </child>
</widget>
<packing>
- <property name="position">2</property>
+ <property name="position">1</property>
</packing>
</child>
</widget>
@@ -677,36 +767,60 @@
</packing>
</child>
<child>
- <widget class="GtkVBox" id="rotation_settings_box">
+ <widget class="GtkVBox" id="bezier_limits_vbox">
<property name="visible">True</property>
- <property name="orientation">vertical</property>
<child>
- <widget class="GtkLabel" id="rotation_settings_label">
+ <widget class="GtkVBox" id="v_max_box">
<property name="visible">True</property>
- <property name="label" translatable="yes">Rotation</property>
- <property name="selectable">True</property>
+ <property name="orientation">vertical</property>
+ <child>
+ <widget class="GtkLabel" id="v_max_label">
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">max speed</property>
+ </widget>
+ <packing>
+ <property name="position">0</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkSpinButton" id="v_max">
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="invisible_char">●</property>
+ <property name="adjustment">0.5 0.01 2 0.10000000000000001 1 1</property>
+ <property name="digits">2</property>
+ <property name="numeric">True</property>
+ </widget>
+ <packing>
+ <property name="position">1</property>
+ </packing>
+ </child>
</widget>
<packing>
<property name="position">0</property>
</packing>
</child>
<child>
- <widget class="GtkHBox" id="rotation_settings_label_box">
+ <widget class="GtkVBox" id="a_tan_max_box">
<property name="visible">True</property>
- <property name="homogeneous">True</property>
+ <property name="orientation">vertical</property>
<child>
- <widget class="GtkLabel" id="rotation_speed_label">
+ <widget class="GtkLabel" id="a_tan_max_label">
<property name="visible">True</property>
- <property name="label" translatable="yes">speed</property>
+ <property name="label" translatable="yes">max tangential accel</property>
</widget>
<packing>
<property name="position">0</property>
</packing>
</child>
<child>
- <widget class="GtkLabel" id="rotation_acceleration_label">
+ <widget class="GtkSpinButton" id="a_tan_max">
<property name="visible">True</property>
- <property name="label" translatable="yes">accel</property>
+ <property name="can_focus">True</property>
+ <property name="invisible_char">●</property>
+ <property name="adjustment">1 0.01 4 0.10000000000000001 1 1</property>
+ <property name="digits">2</property>
+ <property name="numeric">True</property>
</widget>
<packing>
<property name="position">1</property>
@@ -718,28 +832,24 @@
</packing>
</child>
<child>
- <widget class="GtkHBox" id="rotation_settings_value_box">
+ <widget class="GtkVBox" id="a_rand_max_box">
<property name="visible">True</property>
- <property name="homogeneous">True</property>
+ <property name="orientation">vertical</property>
<child>
- <widget class="GtkSpinButton" id="rotation_speed">
+ <widget class="GtkLabel" id="a_rad_max_label">
<property name="visible">True</property>
- <property name="can_focus">True</property>
- <property name="invisible_char">•</property>
- <property name="adjustment">2.0000000000000004 0 4 0.10000000000000001 0 0</property>
- <property name="digits">2</property>
- <property name="numeric">True</property>
+ <property name="label" translatable="yes">max radial accel</property>
</widget>
<packing>
<property name="position">0</property>
</packing>
</child>
<child>
- <widget class="GtkSpinButton" id="rotation_acceleration">
+ <widget class="GtkSpinButton" id="a_rad_max">
<property name="visible">True</property>
<property name="can_focus">True</property>
- <property name="invisible_char">•</property>
- <property name="adjustment">2.0000000000000009 0 10 0.10000000000000001 0 0</property>
+ <property name="invisible_char">●</property>
+ <property name="adjustment">1 0.01 4 0.10000000000000001 1 1</property>
<property name="digits">2</property>
<property name="numeric">True</property>
</widget>
hooks/post-receive
--
krobot
|
|
From: Pierre C. <Ba...@us...> - 2012-04-21 22:11:22
|
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 87263167c3172b765e0232d7e424ded68d057c95 (commit)
via cacf9f3521bc38807280c350216b64dcabdbe595 (commit)
from 2fc328b74df14a0b11be0c3964e5afa7b8ae0a06 (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 87263167c3172b765e0232d7e424ded68d057c95
Author: chicco <cha...@cr...>
Date: Sun Apr 22 00:10:55 2012 +0200
[info] add protocol definition
commit cacf9f3521bc38807280c350216b64dcabdbe595
Author: chicco <cha...@cr...>
Date: Sun Apr 22 00:04:36 2012 +0200
[info]
* can_viewer: stuffs
* can: add packet motor_bezier_limits
* viewer: add box to control the speed
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/_tags b/info/control2011/_tags
index 79de459..7a89fb3 100644
--- a/info/control2011/_tags
+++ b/info/control2011/_tags
@@ -1,8 +1,8 @@
-<**/*.ml>: syntax_camlp4o
+<**/*.ml{,i}>: syntax_camlp4o
<src/interfaces/*.ml>: -syntax_camlp4o
# OASIS_START
-# DO NOT EDIT (digest: 4e49eb45e4c273e67ec75220ea03b734)
+# DO NOT EDIT (digest: fb711b43f944541635b3a5ea56a5681e)
# Ignore VCS directories, you can use the same kind of rule outside
# OASIS_START/STOP if you want to exclude directories that contains
# useless stuff for the build process
@@ -29,19 +29,43 @@
"src/can": include
<src/can/krobot-can.{cma,cmxa}>: use_libkrobot-can
<src/can/*.ml{,i}>: use_krobot
+<src/can/*.ml{,i}>: pkg_sexplib.syntax
+<src/can/*.ml{,i}>: pkg_sexplib
<src/can/*.ml{,i}>: pkg_lwt.unix
<src/can/*.ml{,i}>: pkg_lwt.syntax
<src/can/*.ml{,i}>: pkg_lwt.react
+<src/can/*.ml{,i}>: pkg_bitstring
"src/can/can_stubs.c": use_krobot
+"src/can/can_stubs.c": pkg_sexplib.syntax
+"src/can/can_stubs.c": pkg_sexplib
"src/can/can_stubs.c": pkg_lwt.unix
"src/can/can_stubs.c": pkg_lwt.syntax
"src/can/can_stubs.c": pkg_lwt.react
+"src/can/can_stubs.c": pkg_bitstring
# Executable dump-can
<examples/dump_can.{native,byte}>: use_krobot-can
<examples/dump_can.{native,byte}>: use_krobot
+<examples/dump_can.{native,byte}>: pkg_sexplib.syntax
+<examples/dump_can.{native,byte}>: pkg_sexplib
<examples/dump_can.{native,byte}>: pkg_lwt.unix
<examples/dump_can.{native,byte}>: pkg_lwt.syntax
<examples/dump_can.{native,byte}>: pkg_lwt.react
+<examples/dump_can.{native,byte}>: pkg_bitstring
+# Executable krobot-can-display
+<src/tools/krobot_can_display.{native,byte}>: use_krobot-can
+<src/tools/krobot_can_display.{native,byte}>: use_krobot
+<src/tools/krobot_can_display.{native,byte}>: pkg_sexplib.syntax
+<src/tools/krobot_can_display.{native,byte}>: pkg_sexplib
+<src/tools/krobot_can_display.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_can_display.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_can_display.{native,byte}>: pkg_lwt.react
+<src/tools/krobot_can_display.{native,byte}>: pkg_lwt.glib
+<src/tools/krobot_can_display.{native,byte}>: pkg_cairo.lablgtk2
+<src/tools/krobot_can_display.{native,byte}>: pkg_bitstring
+<src/tools/*.ml{,i}>: use_krobot-can
+<src/tools/*.ml{,i}>: pkg_sexplib.syntax
+<src/tools/*.ml{,i}>: pkg_sexplib
+<src/tools/*.ml{,i}>: pkg_bitstring
# Executable krobot-calibrate-sharps
<src/tools/krobot_calibrate_sharps.{native,byte}>: use_krobot
<src/tools/krobot_calibrate_sharps.{native,byte}>: pkg_lwt.unix
@@ -92,14 +116,20 @@
# Executable send-can
<examples/send_can.{native,byte}>: use_krobot-can
<examples/send_can.{native,byte}>: use_krobot
+<examples/send_can.{native,byte}>: pkg_sexplib.syntax
+<examples/send_can.{native,byte}>: pkg_sexplib
<examples/send_can.{native,byte}>: pkg_lwt.unix
<examples/send_can.{native,byte}>: pkg_lwt.syntax
<examples/send_can.{native,byte}>: pkg_lwt.react
+<examples/send_can.{native,byte}>: pkg_bitstring
<examples/*.ml{,i}>: use_krobot-can
<examples/*.ml{,i}>: use_krobot
+<examples/*.ml{,i}>: pkg_sexplib.syntax
+<examples/*.ml{,i}>: pkg_sexplib
<examples/*.ml{,i}>: pkg_lwt.unix
<examples/*.ml{,i}>: pkg_lwt.syntax
<examples/*.ml{,i}>: pkg_lwt.react
+<examples/*.ml{,i}>: pkg_bitstring
# Executable krobot-vm
<src/tools/krobot_vm.{native,byte}>: use_krobot
<src/tools/krobot_vm.{native,byte}>: pkg_lwt.unix
@@ -163,12 +193,18 @@
# Executable krobot-driver
<src/driver/krobot_driver.{native,byte}>: use_krobot-can
<src/driver/krobot_driver.{native,byte}>: use_krobot
+<src/driver/krobot_driver.{native,byte}>: pkg_sexplib.syntax
+<src/driver/krobot_driver.{native,byte}>: pkg_sexplib
<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_lwt.react
+<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}>: pkg_sexplib.syntax
+<src/driver/*.ml{,i}>: pkg_sexplib
<src/driver/*.ml{,i}>: pkg_lwt.unix
<src/driver/*.ml{,i}>: pkg_lwt.syntax
<src/driver/*.ml{,i}>: pkg_lwt.react
+<src/driver/*.ml{,i}>: pkg_bitstring
# OASIS_STOP
diff --git a/info/control2011/krobot-api.odocl b/info/control2011/krobot-api.odocl
index e19a38b..09b1b3d 100644
--- a/info/control2011/krobot-api.odocl
+++ b/info/control2011/krobot-api.odocl
@@ -1,5 +1,5 @@
# OASIS_START
-# DO NOT EDIT (digest: 7edbd90b2355975a6b906edaa0a69b3e)
+# DO NOT EDIT (digest: d7dcb726ba6a88520ef6021e2c4dec89)
src/lib/Krobot_can
src/lib/Krobot_bus
src/lib/Krobot_message
@@ -10,4 +10,7 @@ src/lib/Krobot_config
src/lib/Krobot_path
src/lib/Krobot_action
src/can/Krobot_can_bus
+src/can/Krobot_can_decoder
+src/can/Krobot_can_desc_lexer
+src/can/Krobot_can_desc_parser
# OASIS_STOP
diff --git a/info/control2011/myocamlbuild.ml b/info/control2011/myocamlbuild.ml
index 5fd4717..2cfed59 100644
--- a/info/control2011/myocamlbuild.ml
+++ b/info/control2011/myocamlbuild.ml
@@ -1,7 +1,7 @@
(* OASIS_START *)
-(* DO NOT EDIT (digest: d2e269dc4718223067ec6884e511c44b) *)
+(* DO NOT EDIT (digest: 4d333a06e10a6d542b31a757ed455f80) *)
module OASISGettext = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISGettext.ml"
+# 21 "/media/data/ocaml/oasis/src/oasis/OASISGettext.ml"
let ns_ str =
str
@@ -24,7 +24,7 @@ module OASISGettext = struct
end
module OASISExpr = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISExpr.ml"
+# 21 "/media/data/ocaml/oasis/src/oasis/OASISExpr.ml"
@@ -115,7 +115,7 @@ end
module BaseEnvLight = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseEnvLight.ml"
+# 21 "/media/data/ocaml/oasis/src/base/BaseEnvLight.ml"
module MapString = Map.Make(String)
@@ -212,7 +212,7 @@ end
module MyOCamlbuildFindlib = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/plugins/ocamlbuild/MyOCamlbuildFindlib.ml"
+# 21 "/media/data/ocaml/oasis/src/plugins/ocamlbuild/MyOCamlbuildFindlib.ml"
(** OCamlbuild extension, copied from
* http://brion.inria.fr/gallium/index.php/Using_ocamlfind_with_ocamlbuild
@@ -321,7 +321,7 @@ module MyOCamlbuildFindlib = struct
end
module MyOCamlbuildBase = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/plugins/ocamlbuild/MyOCamlbuildBase.ml"
+# 21 "/media/data/ocaml/oasis/src/plugins/ocamlbuild/MyOCamlbuildBase.ml"
(** Base functions for writing myocamlbuild.ml
@author Sylvain Le Gall
@@ -336,7 +336,7 @@ module MyOCamlbuildBase = struct
type name = string
type tag = string
-# 55 "/home/dim/sources/oasis-0.2.1~alpha1/src/plugins/ocamlbuild/MyOCamlbuildBase.ml"
+# 55 "/media/data/ocaml/oasis/src/plugins/ocamlbuild/MyOCamlbuildBase.ml"
type t =
{
@@ -461,6 +461,7 @@ let package_default =
let dispatch_default = MyOCamlbuildBase.dispatch_default package_default;;
+# 465 "myocamlbuild.ml"
(* OASIS_STOP *)
open Ocamlbuild_plugin
diff --git a/info/control2011/protocol/krobot_can_config.cfg b/info/control2011/protocol/krobot_can_config.cfg
new file mode 100644
index 0000000..bc2908c
--- /dev/null
+++ b/info/control2011/protocol/krobot_can_config.cfg
@@ -0,0 +1,35 @@
+odometry { x "x" "min" min value "max" max ; y "y" value; thetha "thetha" value }
+ghost {
+ x value;
+ y value;
+ theta value;
+ u value;
+ state value;
+}
+adc_values_1 { val1 value; val2 value; val3 value; val4 value; }
+adc_values_2 { val1 value; val2 value; val3 value; val4 value; }
+status { is_moving value }
+switch_status_1 {
+ sw1 value; sw2 value; sw3 value; sw4 value;
+ sw5 value; sw6 value; sw7 value; sw8 value;
+}
+switch_status_2 {
+ sw1 value; sw2 value; sw3 value; sw4 value;
+ sw5 value; sw6 value; sw7 value; sw8 value;
+}
+
+motor3 {
+ position value;
+ speed value;
+}
+
+motor4 {
+ position value;
+ speed value;
+}
+
+bezier_limits {
+ v_max value;
+ a_tan_max value;
+ a_rad_max value;
+}
diff --git a/info/control2011/protocol/krobot_can_messages.kmd b/info/control2011/protocol/krobot_can_messages.kmd
new file mode 100644
index 0000000..fba51cb
--- /dev/null
+++ b/info/control2011/protocol/krobot_can_messages.kmd
@@ -0,0 +1,170 @@
+encoders34 100 {
+ encoder1_pos int unsigned 16;
+ encoder2_pos int unsigned 16;
+ encoder1_dir int unsigned 8;
+ encoder2_dir int unsigned 8;
+}
+
+motor3 101 {
+ position float;
+ speed float;
+}
+
+motor4 102 {
+ position float;
+ speed float;
+}
+
+status 103 {
+ is_moving int unsigned 8;
+}
+
+odometry 104 {
+ x int signed 16 "in mm";
+ y int signed 16 "in mm";
+ theta int signed 16 "in 1/10000 radians";
+}
+
+ghost 105 {
+ x int signed 16 "in mm";
+ y int signed 16 "in mm";
+ theta int signed 16 "in 1/10000 radians";
+ u int unsigned 8 "proportion of the spline between 0 and 255";
+ state int unsigned 8 "1 if trajectory in progress, else 0";
+}
+
+move 201 {
+ distance int signed 32 "in mm";
+ speed int unsigned 16 "in mm/s";
+ acceleration int unsigned 16 "in mm/s^2";
+}
+
+turn 202 {
+ angle int signed 32 "in 1/10000 radians";
+ speed int unsigned 16 "in 1/1000 rad/s";
+ acceleration int unsigned 16 "in 1/1000 rad/s^2"
+}
+
+odometry_set 203 {
+ x int signed 16 "in mm";
+ y int signed 16 "in mm";
+ theta int signed 16 "in 1/10000 radians";
+}
+
+stop 204 {
+ lin_acc float;
+ rot_acc float;
+}
+
+controller_mode 205 {
+ mode int unsigned 8;
+}
+
+bezier_add 206 {
+ x_end int unsigned 12 "in mm";
+ y_end int unsigned 12 "in mm";
+ d1 int signed 9 "in cm";
+ d2 int unsigned 10 "in cm";
+ theta_end int signed 12 "in 1/100 rad";
+ v_end int unsigned 1;
+}
+
+bezier_limits 207 {
+ v_max int unsigned 16 "in mm";
+ a_tan_max int unsigned 16 "in mm/s^2";
+ a_rad_max int unsigned 16 "in mm/s^2";
+}
+
+beacon_position 301 {
+ angle int unsigned 16 "in 1/10000 radians";
+ distance int unsigned 16 "in mm";
+ period int unsigned 16 "in 1/10000 s";
+}
+
+beacon_lowlevel_position 302 {
+ angle int unsigned 16 "in 1/10000 radians";
+ width int unsigned 16 "in 1/100000 radians";
+ period int unsigned 32 "in timer ticks";
+}
+
+breacon_calibration 303 {
+ width int unsigned 16 "in 1/100000 rad";
+ distance int unsigned 16 "in mm";
+}
+
+switch_status_1 311 {
+ sw1 int unsigned 8;
+ sw2 int unsigned 8;
+ sw3 int unsigned 8;
+ sw4 int unsigned 8;
+ sw5 int unsigned 8;
+ sw6 int unsigned 8;
+ sw7 int unsigned 8;
+ sw8 int unsigned 8;
+}
+
+switch_status_2 312 {
+ sw1 int unsigned 8;
+ sw2 int unsigned 8;
+ sw3 int unsigned 8;
+ sw4 int unsigned 8;
+ sw5 int unsigned 8;
+ sw6 int unsigned 8;
+ sw7 int unsigned 8;
+ sw8 int unsigned 8;
+}
+
+switch_set 313 {
+ num int unsigned 8;
+ state int unsigned 8;
+}
+
+adc_values_1 321 {
+ val1 int unsigned 16;
+ val2 int unsigned 16;
+ val3 int unsigned 16;
+ val4 int unsigned 16;
+}
+
+adc_values_2 322 {
+ val1 int unsigned 16;
+ val2 int unsigned 16;
+ val3 int unsigned 16;
+ val4 int unsigned 16;
+}
+
+battery_status_1 331 {
+ elem1 int unsigned 16 "in 1/10000 V";
+ elem2 int unsigned 16 "in 1/10000 V";
+ elem3 int unsigned 16 "in 1/10000 V";
+ elem4 int unsigned 16 "in 1/10000 V";
+}
+
+battery_status_2 332 {
+ elem1 int unsigned 16 "in 1/10000 V";
+ elem2 int unsigned 16 "in 1/10000 V";
+ elem3 int unsigned 16 "in 1/10000 V";
+ elem4 int unsigned 16 "in 1/10000 V";
+}
+
+ax12_state 341 {
+ address int unsigned 8;
+ position int unsigned 16;
+ speed int unsigned 16;
+ torque int unsigned 16;
+}
+
+ax12_request_state 342 {
+ address int unsigned 8;
+}
+
+ax12_goto 343 {
+ address int unsigned 8;
+ position int unsigned 16;
+ speed int unsigned 16;
+}
+
+ax12_set_torque_enable 345 {
+ address int unsigned 8;
+ enable int unsigned 8;
+}
diff --git a/info/control2011/setup.ml b/info/control2011/setup.ml
index 4f9b978..8a23328 100644
--- a/info/control2011/setup.ml
+++ b/info/control2011/setup.ml
@@ -1,14 +1,14 @@
(* setup.ml generated for the first time by OASIS v0.2.0~alpha1 *)
(* OASIS_START *)
-(* DO NOT EDIT (digest: 2fa81b2af9e6c805314db247864ab4e1) *)
+(* DO NOT EDIT (digest: 300912e61b966bc3109c698735347ca1) *)
(*
Regenerated by OASIS v0.2.1~alpha1
Visit http://oasis.forge.ocamlcore.org for more information and
documentation about functions used in this file.
*)
module OASISGettext = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISGettext.ml"
+# 21 "/media/data/ocaml/oasis/src/oasis/OASISGettext.ml"
let ns_ str =
str
@@ -31,7 +31,7 @@ module OASISGettext = struct
end
module OASISContext = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISContext.ml"
+# 21 "/media/data/ocaml/oasis/src/oasis/OASISContext.ml"
open OASISGettext
@@ -43,10 +43,11 @@ module OASISContext = struct
type t =
{
- verbose: bool;
- debug: bool;
- ignore_plugins: bool;
- printf: level -> string -> unit;
+ verbose: bool;
+ debug: bool;
+ ignore_plugins: bool;
+ ignore_unknown_fields: bool;
+ printf: level -> string -> unit;
}
let printf lvl str =
@@ -62,10 +63,11 @@ module OASISContext = struct
let default =
ref
{
- verbose = true;
- debug = false;
- ignore_plugins = false;
- printf = printf;
+ verbose = true;
+ debug = false;
+ ignore_plugins = false;
+ ignore_unknown_fields = false;
+ printf = printf;
}
let quiet =
@@ -86,7 +88,7 @@ module OASISContext = struct
end
module OASISUtils = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISUtils.ml"
+# 21 "/media/data/ocaml/oasis/src/oasis/OASISUtils.ml"
module MapString = Map.Make(String)
@@ -225,7 +227,7 @@ module OASISUtils = struct
end
module PropList = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/PropList.ml"
+# 21 "/media/data/ocaml/oasis/src/oasis/PropList.ml"
open OASISGettext
@@ -260,7 +262,7 @@ module PropList = struct
let clear t =
Hashtbl.clear t
-# 66 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/PropList.ml"
+# 66 "/media/data/ocaml/oasis/src/oasis/PropList.ml"
end
module Schema =
@@ -501,7 +503,7 @@ module PropList = struct
end
module OASISMessage = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISMessage.ml"
+# 21 "/media/data/ocaml/oasis/src/oasis/OASISMessage.ml"
open OASISGettext
@@ -550,7 +552,7 @@ module OASISMessage = struct
end
module OASISVersion = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISVersion.ml"
+# 21 "/media/data/ocaml/oasis/src/oasis/OASISVersion.ml"
open OASISGettext
@@ -738,7 +740,7 @@ module OASISVersion = struct
end
module OASISLicense = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISLicense.ml"
+# 21 "/media/data/ocaml/oasis/src/oasis/OASISLicense.ml"
(** License for _oasis fields
@author Sylvain Le Gall
@@ -771,7 +773,7 @@ module OASISLicense = struct
end
module OASISExpr = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISExpr.ml"
+# 21 "/media/data/ocaml/oasis/src/oasis/OASISExpr.ml"
@@ -861,7 +863,7 @@ module OASISExpr = struct
end
module OASISTypes = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISTypes.ml"
+# 21 "/media/data/ocaml/oasis/src/oasis/OASISTypes.ml"
@@ -938,7 +940,7 @@ module OASISTypes = struct
type plugin_data = (all_plugin * plugin_data_purpose * (unit -> unit)) list
-# 102 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISTypes.ml"
+# 102 "/media/data/ocaml/oasis/src/oasis/OASISTypes.ml"
type 'a conditional = 'a OASISExpr.choices
@@ -1095,7 +1097,7 @@ module OASISTypes = struct
end
module OASISUnixPath = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISUnixPath.ml"
+# 21 "/media/data/ocaml/oasis/src/oasis/OASISUnixPath.ml"
type unix_filename = string
type unix_dirname = string
@@ -1174,7 +1176,7 @@ module OASISUnixPath = struct
end
module OASISSection = struct
-# 1 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISSection.ml"
+# 1 "/media/data/ocaml/oasis/src/oasis/OASISSection.ml"
open OASISTypes
let section_kind_common =
@@ -1228,12 +1230,12 @@ module OASISSection = struct
end
module OASISBuildSection = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISBuildSection.ml"
+# 21 "/media/data/ocaml/oasis/src/oasis/OASISBuildSection.ml"
end
module OASISExecutable = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISExecutable.ml"
+# 21 "/media/data/ocaml/oasis/src/oasis/OASISExecutable.ml"
open OASISTypes
@@ -1264,7 +1266,7 @@ module OASISExecutable = struct
end
module OASISLibrary = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISLibrary.ml"
+# 21 "/media/data/ocaml/oasis/src/oasis/OASISLibrary.ml"
open OASISTypes
open OASISUtils
@@ -1555,33 +1557,33 @@ module OASISLibrary = struct
end
module OASISFlag = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISFlag.ml"
+# 21 "/media/data/ocaml/oasis/src/oasis/OASISFlag.ml"
end
module OASISPackage = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISPackage.ml"
+# 21 "/media/data/ocaml/oasis/src/oasis/OASISPackage.ml"
end
module OASISSourceRepository = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISSourceRepository.ml"
+# 21 "/media/data/ocaml/oasis/src/oasis/OASISSourceRepository.ml"
end
module OASISTest = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISTest.ml"
+# 21 "/media/data/ocaml/oasis/src/oasis/OASISTest.ml"
end
module OASISDocument = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/oasis/OASISDocument.ml"
+# 21 "/media/data/ocaml/oasis/src/oasis/OASISDocument.ml"
end
module BaseEnvLight = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseEnvLight.ml"
+# 21 "/media/data/ocaml/oasis/src/base/BaseEnvLight.ml"
module MapString = Map.Make(String)
@@ -1678,7 +1680,7 @@ end
module BaseContext = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseContext.ml"
+# 21 "/media/data/ocaml/oasis/src/base/BaseContext.ml"
open OASISContext
@@ -1689,7 +1691,7 @@ module BaseContext = struct
end
module BaseMessage = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseMessage.ml"
+# 21 "/media/data/ocaml/oasis/src/base/BaseMessage.ml"
(** Message to user, overrid for Base
@author Sylvain Le Gall
@@ -1710,7 +1712,7 @@ module BaseMessage = struct
end
module BaseFilePath = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseFilePath.ml"
+# 21 "/media/data/ocaml/oasis/src/base/BaseFilePath.ml"
open Filename
@@ -1742,7 +1744,7 @@ module BaseFilePath = struct
end
module BaseEnv = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseEnv.ml"
+# 21 "/media/data/ocaml/oasis/src/base/BaseEnv.ml"
open OASISGettext
open OASISUtils
@@ -2197,7 +2199,7 @@ module BaseEnv = struct
end
module BaseExec = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseExec.ml"
+# 21 "/media/data/ocaml/oasis/src/base/BaseExec.ml"
open OASISGettext
open OASISUtils
@@ -2257,7 +2259,7 @@ module BaseExec = struct
end
module BaseFileUtil = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseFileUtil.ml"
+# 21 "/media/data/ocaml/oasis/src/base/BaseFileUtil.ml"
open OASISGettext
@@ -2435,7 +2437,7 @@ module BaseFileUtil = struct
end
module BaseArgExt = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseArgExt.ml"
+# 21 "/media/data/ocaml/oasis/src/base/BaseArgExt.ml"
open OASISUtils
open OASISGettext
@@ -2463,7 +2465,7 @@ module BaseArgExt = struct
end
module BaseCheck = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseCheck.ml"
+# 21 "/media/data/ocaml/oasis/src/base/BaseCheck.ml"
open BaseEnv
open BaseMessage
@@ -2589,7 +2591,7 @@ module BaseCheck = struct
end
module BaseOCamlcConfig = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseOCamlcConfig.ml"
+# 21 "/media/data/ocaml/oasis/src/base/BaseOCamlcConfig.ml"
open BaseEnv
@@ -2699,7 +2701,7 @@ module BaseOCamlcConfig = struct
end
module BaseStandardVar = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseStandardVar.ml"
+# 21 "/media/data/ocaml/oasis/src/base/BaseStandardVar.ml"
open OASISGettext
@@ -2958,7 +2960,7 @@ module BaseStandardVar = struct
end
module BaseFileAB = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseFileAB.ml"
+# 21 "/media/data/ocaml/oasis/src/base/BaseFileAB.ml"
open BaseEnv
open OASISGettext
@@ -3006,7 +3008,7 @@ module BaseFileAB = struct
end
module BaseLog = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseLog.ml"
+# 21 "/media/data/ocaml/oasis/src/base/BaseLog.ml"
open OASISUtils
@@ -3125,7 +3127,7 @@ module BaseLog = struct
end
module BaseBuilt = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseBuilt.ml"
+# 21 "/media/data/ocaml/oasis/src/base/BaseBuilt.ml"
open OASISTypes
open OASISGettext
@@ -3272,7 +3274,7 @@ module BaseBuilt = struct
end
module BaseCustom = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseCustom.ml"
+# 21 "/media/data/ocaml/oasis/src/base/BaseCustom.ml"
open BaseEnv
open BaseMessage
@@ -3322,7 +3324,7 @@ module BaseCustom = struct
end
module BaseDynVar = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseDynVar.ml"
+# 21 "/media/data/ocaml/oasis/src/base/BaseDynVar.ml"
open OASISTypes
@@ -3366,7 +3368,7 @@ module BaseDynVar = struct
end
module BaseTest = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseTest.ml"
+# 21 "/media/data/ocaml/oasis/src/base/BaseTest.ml"
open BaseEnv
open BaseMessage
@@ -3448,7 +3450,7 @@ module BaseTest = struct
end
module BaseDoc = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseDoc.ml"
+# 21 "/media/data/ocaml/oasis/src/base/BaseDoc.ml"
open BaseEnv
open BaseMessage
@@ -3478,7 +3480,7 @@ module BaseDoc = struct
end
module BaseSetup = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseSetup.ml"
+# 21 "/media/data/ocaml/oasis/src/base/BaseSetup.ml"
open BaseEnv
open BaseMessage
@@ -3896,7 +3898,7 @@ module BaseSetup = struct
end
module BaseDev = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/base/BaseDev.ml"
+# 21 "/media/data/ocaml/oasis/src/base/BaseDev.ml"
@@ -3954,7 +3956,7 @@ end
module InternalConfigurePlugin = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/plugins/internal/InternalConfigurePlugin.ml"
+# 21 "/media/data/ocaml/oasis/src/plugins/internal/InternalConfigurePlugin.ml"
(** Configure using internal scheme
@author Sylvain Le Gall
@@ -4170,7 +4172,7 @@ module InternalConfigurePlugin = struct
end
module InternalInstallPlugin = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/plugins/internal/InternalInstallPlugin.ml"
+# 21 "/media/data/ocaml/oasis/src/plugins/internal/InternalInstallPlugin.ml"
(** Install using internal scheme
@author Sylvain Le Gall
@@ -4574,7 +4576,7 @@ end
module OCamlbuildCommon = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/plugins/ocamlbuild/OCamlbuildCommon.ml"
+# 21 "/media/data/ocaml/oasis/src/plugins/ocamlbuild/OCamlbuildCommon.ml"
(** Functions common to OCamlbuild build and doc plugin
*)
@@ -4674,7 +4676,7 @@ module OCamlbuildCommon = struct
end
module OCamlbuildPlugin = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/plugins/ocamlbuild/OCamlbuildPlugin.ml"
+# 21 "/media/data/ocaml/oasis/src/plugins/ocamlbuild/OCamlbuildPlugin.ml"
(** Build using ocamlbuild
@author Sylvain Le Gall
@@ -4846,7 +4848,7 @@ module OCamlbuildPlugin = struct
end
module OCamlbuildDocPlugin = struct
-# 21 "/home/dim/sources/oasis-0.2.1~alpha1/src/plugins/ocamlbuild/OCamlbuildDocPlugin.ml"
+# 21 "/media/data/ocaml/oasis/src/plugins/ocamlbuild/OCamlbuildDocPlugin.ml"
(* Create documentation using ocamlbuild .odocl files
@author Sylvain Le Gall
@@ -5029,7 +5031,10 @@ let setup_t =
bs_build_depends =
[
InternalLibrary "krobot";
- FindlibPackage ("lwt.syntax", None)
+ FindlibPackage ("lwt.syntax", None);
+ FindlibPackage ("sexplib", None);
+ FindlibPackage ("sexplib.syntax", None);
+ FindlibPackage ("bitstring", None)
];
bs_build_tools = [ExternalTool "ocamlbuild"];
bs_c_sources = ["can_stubs.c"];
@@ -5042,7 +5047,13 @@ let setup_t =
bs_nativeopt = [(OASISExpr.EBool true, [])];
},
{
- lib_modules = ["Krobot_can_bus"];
+ lib_modules =
+ [
+ "Krobot_can_bus";
+ "Krobot_can_decoder";
+ "Krobot_can_desc_lexer";
+ "Krobot_can_desc_parser"
+ ];
lib_internal_modules = [];
lib_findlib_parent = Some "krobot";
lib_findlib_name = Some "can";
@@ -5105,6 +5116,48 @@ let setup_t =
});
Executable
({
+ cs_name = "krobot-can-display";
+ 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-can";
+ FindlibPackage ("lwt.syntax", None);
+ FindlibPackage ("sexplib", None);
+ FindlibPackage ("sexplib.syntax", None);
+ FindlibPackage ("cairo.lablgtk2", None);
+ FindlibPackage ("lwt.glib", 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_can_display.ml";
+ });
+ Executable
+ ({
cs_name = "krobot-calibrate-sharps";
cs_data = PropList.Data.create ();
cs_plugin_data = [];
@@ -5754,5 +5807,6 @@ let setup_t =
let setup () = BaseSetup.setup setup_t;;
+# 5811 "setup.ml"
(* OASIS_STOP *)
let () = setup ();;
diff --git a/info/control2011/src/can/krobot-can.mllib b/info/control2011/src/can/krobot-can.mllib
index 198bccd..e2d02bd 100644
--- a/info/control2011/src/can/krobot-can.mllib
+++ b/info/control2011/src/can/krobot-can.mllib
@@ -1,4 +1,7 @@
# OASIS_START
-# DO NOT EDIT (digest: 05e2413cff140c2f298af6bff05bd866)
+# DO NOT EDIT (digest: fe3b1f11910d1f7667123fd3f2470bb1)
Krobot_can_bus
+Krobot_can_decoder
+Krobot_can_desc_lexer
+Krobot_can_desc_parser
# OASIS_STOP
diff --git a/info/control2011/src/can/krobot_can_decoder.ml b/info/control2011/src/can/krobot_can_decoder.ml
index bf98edc..60e234b 100644
--- a/info/control2011/src/can/krobot_can_decoder.ml
+++ b/info/control2011/src/can/krobot_can_decoder.ml
@@ -149,12 +149,19 @@ let result_to_string = function
| R_float f -> string_of_float f
| R_char c -> Printf.sprintf "%c" c
+let result_to_float = function
+ | R_bit b -> None
+ | R_hex i -> Some (float i)
+ | R_int i -> Some (float i)
+ | R_float f -> Some f
+ | R_char c -> None
(* configuration *)
type cap =
| Value
| Min
| Max
+ | C_text of string
let cap_of_string = function
| "min" -> Min
diff --git a/info/control2011/src/can/krobot_can_decoder.mli b/info/control2011/src/can/krobot_can_decoder.mli
index b2cfe9e..a440982 100644
--- a/info/control2011/src/can/krobot_can_decoder.mli
+++ b/info/control2011/src/can/krobot_can_decoder.mli
@@ -53,12 +53,15 @@ val set_description : decode_table -> description -> unit
val result_to_string : result_field -> string
+val result_to_float : result_field -> float option
+
(* configuration file *)
type cap =
| Value
| Min
| Max
+ | C_text of string
val cap_of_string : string -> cap
diff --git a/info/control2011/src/can/krobot_can_desc_parser.mly b/info/control2011/src/can/krobot_can_desc_parser.mly
index d2b2e51..fbcbacd 100644
--- a/info/control2011/src/can/krobot_can_desc_parser.mly
+++ b/info/control2011/src/can/krobot_can_desc_parser.mly
@@ -64,6 +64,8 @@ config :
config_field :
| IDENT LCBRACKET options RCBRACKET
{ { frame = $1; options = $3 } }
+ | IDENT LCBRACKET RCBRACKET
+ { { frame = $1; options = [] } }
options :
| option { [$1] }
@@ -80,5 +82,6 @@ caps :
cap :
| IDENT { cap_of_string $1 }
+ | DESCRIPTION { C_text $1 }
%%
diff --git a/info/control2011/src/lib/META b/info/control2011/src/lib/META
index dfccb8d..1b2ac8d 100644
--- a/info/control2011/src/lib/META
+++ b/info/control2011/src/lib/META
@@ -1,5 +1,5 @@
# OASIS_START
-# DO NOT EDIT (digest: 3dcfd084b393896010b17f3aac3a6423)
+# DO NOT EDIT (digest: ae78269c0645eb8dc894e1c1955a6e56)
version = "1.0"
description = "The [Kro]bot library"
requires = "lwt.unix lwt.react"
@@ -9,7 +9,7 @@ exists_if = "krobot.cma"
package "can" (
version = "1.0"
description = "CAN interface using SocketCAN"
- requires = "krobot"
+ requires = "krobot sexplib bitstring"
archive(byte) = "krobot-can.cma"
archive(native) = "krobot-can.cmxa"
exists_if = "krobot-can.cma"
diff --git a/info/control2011/src/lib/krobot_message.ml b/info/control2011/src/lib/krobot_message.ml
index a300948..4addf15 100644
--- a/info/control2011/src/lib/krobot_message.ml
+++ b/info/control2011/src/lib/krobot_message.ml
@@ -41,6 +41,7 @@ type t =
| Motor_turn of float * float * float
| Motor_bezier of float * float * float * float * float * float
| Motor_stop of float * float
+ | Motor_bezier_limits of float * float * float
| Odometry of float * float * float
| Odometry_ghost of float * float * float * int * bool
| Set_odometry of float * float * float
@@ -159,6 +160,10 @@ let to_string = function
sprintf
"Motor_stop(%f, %f)"
lin_acc rot_acc
+ | Motor_bezier_limits(v_max, a_tan_max, a_rad_max) ->
+ sprintf
+ "Motor_bezier_limits(%f, %f, %f)"
+ v_max a_tan_max a_rad_max
| Odometry(x, y, theta) ->
sprintf
"Odometry(%f, %f, %f)"
@@ -485,6 +490,20 @@ let encode = function
~remote:false
~format:F29bits
~data
+ | Motor_bezier_limits(v_max, a_tan_max, a_rad_max) ->
+ let v_max = v_max *. 1000. in
+ let a_tan_max = a_tan_max *. 1000. in
+ let a_rad_max = a_rad_max *. 1000. in
+ let data = String.create 6 in
+ put_uint16 data 0 (int_of_float v_max);
+ put_uint16 data 2 (int_of_float a_tan_max);
+ put_uint16 data 4 (int_of_float a_rad_max);
+ frame
+ ~identifier:207
+ ~kind:Data
+ ~remote:false
+ ~format:F29bits
+ ~data
| Set_controller_mode b ->
frame
~identifier:205
@@ -606,6 +625,11 @@ let decode frame =
float d2 /. 100.,
float theta /. 100.,
float v /. 1000.)
+ | 207 ->
+ Motor_bezier_limits
+ (float (get_uint16 frame.data 0) /. 1000.,
+ float (get_uint16 frame.data 2) /. 1000.,
+ float (get_uint16 frame.data 4) /. 1000.)
| 231 ->
Elevator(get_float32 frame.data 0,
get_float32 frame.data 4)
diff --git a/info/control2011/src/lib/krobot_message.mli b/info/control2011/src/lib/krobot_message.mli
index 7a46699..f111d23 100644
--- a/info/control2011/src/lib/krobot_message.mli
+++ b/info/control2011/src/lib/krobot_message.mli
@@ -72,6 +72,11 @@ type t =
- [lin_acc] in m/s^2
- [rot_acc] in rad/s^2
*)
+ | Motor_bezier_limits of float * float * float
+ (** [Motor_bezier_limits(v_max, a_tan_max, a_rad_max)]
+ - [v_max] in m/s
+ - [a_tan_max] in m/s^2
+ - [a_rad_max] in m/s^2 *)
| Odometry of float * float * float
(** [Odometry(x, y, theta)] the position of the robot on the
table. *)
diff --git a/info/control2011/src/tools/krobot_can_display.ml b/info/control2011/src/tools/krobot_can_display.ml
index 3372904..e3532f6 100644
--- a/info/control2011/src/tools/krobot_can_display.ml
+++ b/info/control2011/src/tools/krobot_can_display.ml
@@ -88,17 +88,57 @@ end
module StringMap = Map.Make(String)
+class basic_field ~packing =
+object
+ val widget = GMisc.label ~packing ()
+ method clear () = widget#set_label ""
+end
+
+class value_field ~packing =
+object
+ inherit basic_field ~packing
+ method set_result result =
+ widget#set_label (result_to_string result)
+end
+
+class text_field ~packing text =
+object
+ inherit basic_field ~packing
+ method set_result (result:result_field) =
+ widget#set_label text
+ method! clear () = ()
+end
+
+class filter_field ~packing filter =
+ let old_v = ref None in
+object
+ inherit basic_field ~packing
+ method set_result result =
+ match result_to_float result with
+ | None -> ()
+ | Some f ->
+ let new_v =
+ match !old_v with
+ | None -> f
+ | Some old -> filter old f in
+ old_v := Some new_v;
+ widget#set_label (string_of_float new_v)
+end
+
+let cap_field ~packing = function
+ | Value -> new value_field ~packing
+ | C_text t -> new text_field ~packing t
+ | Min -> new filter_field ~packing min
+ | Max -> new filter_field ~packing max
+
class field_info ~packing field_name caps =
let box = GPack.hbox ~packing () in
let _ = GMisc.label ~packing:box#add ~text:field_name () in
- let result_widgets = List.map (fun cap -> cap, GMisc.label ~packing:box#add ()) caps in
+ let result_widgets = List.map (cap_field ~packing:box#add) caps in
object
method set_result result =
- List.iter (fun (cap, widget) ->
- match cap with
- | Value -> widget#set_label (result_to_string result)
- | Min | Max -> failwith "TODO min/max display") result_widgets
- method clear () = List.iter (fun (_,widget) -> widget#set_label "") result_widgets
+ List.iter (fun widget -> widget#set_result result) result_widgets
+ method clear () = List.iter (fun widget -> widget#clear ()) result_widgets
end
class kind_info ~packing type_ (options:Krobot_can_decoder.opt list) =
@@ -194,12 +234,14 @@ end
let decode_table = ref None
let iface = ref None
let config_file = ref None
+let use_krobot_bus = ref false
let parse_arg () =
let desc =
[ "-p", Arg.String (fun s -> decode_table := Some s), "file containing the protocol";
"-c", Arg.String (fun s -> config_file := Some s), "file containing the configuration";
- "-i", Arg.String (fun s -> iface := Some s), "can interface";] in
+ "-i", Arg.String (fun s -> iface := Some s), "can interface";
+ "-b", Arg.Set use_krobot_bus, "use krobot bus"; ] in
Arg.parse desc (function "" -> () | _ -> Arg.usage desc ""; exit 2) ""
let report_error f s e lexbuf =
@@ -243,7 +285,7 @@ let load_conf f =
with
| e -> report_error f "while parsing configuration" e lexbuf
-let loop ui bus =
+let loop_can bus ui =
let rec aux () =
lwt (timestamp, frame) = Krobot_can_bus.recv bus in
lwt () = ui#add_packet timestamp frame in
@@ -251,8 +293,18 @@ let loop ui bus =
in
aux ()
-let init iface =
- lwt bus = Krobot_can_bus.open_can iface in
+let loop_krobot_bus bus ui =
+ let ev = Krobot_bus.recv bus in
+ Lwt_react.E.keep
+ (Lwt_react.E.map_s
+ (function
+ | timestamp, Krobot_bus.CAN (_, frame) ->
+ ui#add_packet timestamp frame
+ | _ -> return ()) ev);
+ let s,_ = Lwt.wait () in
+ s
+
+let init loop =
ignore (GMain.init ~setlocale:false ());
Lwt_glib.install ();
let ui = new ui () in
@@ -270,16 +322,23 @@ let init iface =
ui#window#show ();
pick
[waiter;
- loop ui bus]
+ loop ui]
lwt () =
parse_arg ();
- let iface =
- match !iface with
- | None -> "slcan0"
- | Some s -> s in
try_lwt
- init iface
- with Unix.Unix_error(error, func, arg) ->
- Lwt_log.error_f "'%s' failed with: %s" func (Unix.error_message error)
-
+ if !use_krobot_bus
+ then
+ lwt bus = Krobot_bus.get () in
+ init (loop_krobot_bus bus)
+ else
+ let iface =
+ match !iface with
+ | None -> "slcan0"
+ | Some s -> s in
+ lwt bus = Krobot_can_bus.open_can iface in
+ init (loop_can bus)
+ with
+ | Unix.Unix_error(error, func, arg) ->
+ Lwt_log.error_f "'%s' failed with: %s" func (Unix.error_message error)
+ | exn -> Lwt_log.error_f ~exn "Uncaught exception"
diff --git a/info/control2011/src/tools/krobot_dump.ml b/info/control2011/src/tools/krobot_dump.ml
index 4b75d21..a734159 100644
--- a/info/control2011/src/tools/krobot_dump.ml
+++ b/info/control2011/src/tools/krobot_dump.ml
@@ -77,8 +77,11 @@ lwt () =
return ()
in
Lwt_io.printl ""
+ | Trajectory_go ->
+ Lwt_io.printf "trajectory_go"
| _ ->
- return ())
+ Lwt_io.printf "truc"
+ (*return ()*))
(Krobot_bus.recv bus));
fst (wait ())
diff --git a/info/control2011/src/tools/krobot_run.ml b/info/control2011/src/tools/krobot_run.ml
index 910d1b8..1ecfb55 100644
--- a/info/control2011/src/tools/krobot_run.ml
+++ b/info/control2011/src/tools/krobot_run.ml
@@ -1,5 +1,6 @@
#require "krobot"
+open Lwt
open Lwt_react
open Krobot_message
open Krobot_bus
@@ -42,8 +43,8 @@ let handle_message state_ref (timestamp, message) =
let state = !state_ref in
let state =
(match message with
- | Trajectory_moving b ->
- { state with moving = b }
+(* | Trajectory_moving b ->
+ { state with moving = b } *)
| Objects objects ->
{ state with objects = objects }
| CAN (_,f) ->
@@ -118,8 +119,8 @@ let rewrite_action bus = function
[ Can [Elevator (0.,-.1.)] ]
| Lift_up ->
[ Can [Elevator (1.,-.1.)] ]
- | Goto v ->
- [ Bus [Trajectory_goto v]]
+(* | Goto v ->
+ [ Bus [Trajectory_goto v]] *)
| Move_to_pawn ->
[ ]
| Close_grip_low ->
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index 743f320..7d4b629 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -617,4 +617,25 @@ lwt () =
);
false));
+
+ let send_motor_limit () =
+ let v_max = ui#v_max#adjustment#value in
+ let a_tan_max = ui#a_tan_max#adjustment#value in
+ let a_rad_max = ui#a_rad_max#adjustment#value in
+ ignore (Krobot_bus.send viewer.bus
+ (Unix.gettimeofday (),
+ CAN (Info,
+ Krobot_message.encode
+ (Motor_bezier_limits (v_max, a_tan_max, a_rad_max))))) in
+
+ ignore
+ (ui#v_max#connect#value_changed
+ (fun () -> send_motor_limit ()));
+ ignore
+ (ui#a_tan_max#connect#value_changed
+ (fun () -> send_motor_limit ()));
+ ignore
+ (ui#a_rad_max#connect#value_changed
+ (fun () -> send_motor_limit ()));
+
waiter
hooks/post-receive
--
krobot
|
|
From: Pierre C. <Ba...@us...> - 2012-04-15 05:11:28
|
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 2fc328b74df14a0b11be0c3964e5afa7b8ae0a06 (commit)
from 75f833848ac5b267cfa2a81304f038a28c26b37a (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 2fc328b74df14a0b11be0c3964e5afa7b8ae0a06
Author: chicco <cha...@cr...>
Date: Sun Apr 15 07:10:58 2012 +0200
improve can debugger
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/can/krobot_can_decoder.ml b/info/control2011/src/can/krobot_can_decoder.ml
index 4c82c9e..bf98edc 100644
--- a/info/control2011/src/can/krobot_can_decoder.ml
+++ b/info/control2011/src/can/krobot_can_decoder.ml
@@ -24,14 +24,13 @@ type field =
endian : endian;
field_description : string option } with sexp
-type desc = field list with sexp
-type description = field list with sexp
-
type frame_desc =
{ frame_name : string;
frame_id : int;
frame_data : field list;
- frame_description : string option }
+ frame_description : string option } with sexp
+
+type description = frame_desc with sexp
type result_field =
| R_bit of bool
@@ -42,8 +41,7 @@ type result_field =
type result = ( string * result_field ) list with sexp
-type decode_table =
- (int * kind,desc * string option) Hashtbl.t
+type decode_table = (int,frame_desc) Hashtbl.t
let is_description_correct desc =
( List.for_all (fun { display; size } ->
@@ -58,7 +56,7 @@ let is_description_correct desc =
else true
let check_description desc =
- if is_description_correct desc
+ if is_description_correct desc.frame_data
then Some desc
else None
@@ -70,6 +68,13 @@ let split_bitstring (acc,bitstring) field =
let rest = Bitstring.dropbits field.size bitstring in
((value,field)::acc,rest)
+let bit n i = ((1 lsl i) land n) lsr i
+let ones n = max_int mod (1 lsl n)
+let resize_signed ~n ~size =
+ if bit n (size - 1) = 1
+ then n lor ((lnot 0) lsl size)
+ else n
+
let read_field ((str,start,end_),field) =
let i = Bitstring.extract_int_ee_unsigned field.endian str start end_ field.size in
match field.display with
@@ -82,7 +87,9 @@ let read_field ((str,start,end_),field) =
Some (field.name,R_bit b)
end
| Int sign ->
- failwith "handle sign here...";
+ let i = match sign with
+ | Unsigned -> i
+ | Signed -> resize_signed ~n:i ~size:field.size in
Some (field.name,R_int i)
| Hex -> Some (field.name,R_hex i)
| Char -> Some (field.name,R_char (Char.chr i))
@@ -119,15 +126,21 @@ let default_desc =
field; field; field; field ]
let decode_frame frame table =
- let desc,name = try Hashtbl.find table (frame.identifier, frame.kind) with
- | Not_found -> default_desc,None in
+ let desc,name =
+ try
+ let desc = Hashtbl.find table frame.identifier in
+ desc.frame_data, Some desc.frame_name
+ with
+ | Not_found -> default_desc,None in
decode_frame' frame desc,name
-let init_decode_table () =
- Hashtbl.create 0
+let set_description t desc =
+ Hashtbl.replace t desc.frame_id desc
-let set_description t (i,kind) ?name desc =
- Hashtbl.replace t (i,kind) (desc,name)
+let init_decode_table l : decode_table =
+ let t = Hashtbl.create 0 in
+ List.iter (set_description t) l;
+ t
let result_to_string = function
| R_bit b -> string_of_bool b
@@ -136,3 +149,22 @@ let result_to_string = function
| R_float f -> string_of_float f
| R_char c -> Printf.sprintf "%c" c
+
+(* configuration *)
+type cap =
+ | Value
+ | Min
+ | Max
+
+let cap_of_string = function
+ | "min" -> Min
+ | "max" -> Max
+ | "value" -> Value
+ | s -> failwith (Printf.sprintf "unknown option: %s" s)
+
+type opt =
+ | Field of (string * cap list)
+
+type config =
+ { frame : string;
+ options : opt list; }
diff --git a/info/control2011/src/can/krobot_can_decoder.mli b/info/control2011/src/can/krobot_can_decoder.mli
index 426f8f6..b2cfe9e 100644
--- a/info/control2011/src/can/krobot_can_decoder.mli
+++ b/info/control2011/src/can/krobot_can_decoder.mli
@@ -28,9 +28,8 @@ type frame_desc =
{ frame_name : string;
frame_id : int;
frame_data : field list;
- frame_description : string option }
+ frame_description : string option } with sexp
-type desc = field list with sexp
type description with sexp
type result_field =
@@ -44,12 +43,28 @@ type result = ( string * result_field ) list with sexp
type decode_table
-val check_description : desc -> description option
+val check_description : frame_desc -> description option
val decode_frame : Krobot_can.frame -> decode_table -> result * string option
-val init_decode_table : unit -> decode_table
+val init_decode_table : frame_desc list -> decode_table
-val set_description : decode_table -> (int * Krobot_can.kind) -> ?name:string -> description -> unit
+val set_description : decode_table -> description -> unit
val result_to_string : result_field -> string
+
+(* configuration file *)
+
+type cap =
+ | Value
+ | Min
+ | Max
+
+val cap_of_string : string -> cap
+
+type opt =
+ | Field of (string * cap list)
+
+type config =
+ { frame : string;
+ options : opt list; }
diff --git a/info/control2011/src/can/krobot_can_desc_parser.mly b/info/control2011/src/can/krobot_can_desc_parser.mly
index 322e88f..d2b2e51 100644
--- a/info/control2011/src/can/krobot_can_desc_parser.mly
+++ b/info/control2011/src/can/krobot_can_desc_parser.mly
@@ -9,8 +9,9 @@
%token LCOLON, LCBRACKET, RCBRACKET, SEMICOLON, DBLQUOTE
%token EOF
-%start file
+%start file config
%type <Krobot_can_decoder.frame_desc list> file
+%type <Krobot_can_decoder.config list> config
%%
file :
@@ -55,4 +56,29 @@ endianness :
| LITTLEENDIAN { LittleEndian }
*/
+config :
+ | config_field EOF { [$1] }
+ | config_field config { $1 :: $2 }
+;
+
+config_field :
+ | IDENT LCBRACKET options RCBRACKET
+ { { frame = $1; options = $3 } }
+
+options :
+ | option { [$1] }
+ | option SEMICOLON { [$1] }
+ | option SEMICOLON options { $1 :: $3 }
+
+option :
+ | IDENT { Field ($1,[]) }
+ | IDENT caps { Field ($1,$2) }
+
+caps :
+ | cap { [$1] }
+ | cap caps { $1 :: $2 }
+
+cap :
+ | IDENT { cap_of_string $1 }
+
%%
diff --git a/info/control2011/src/tools/krobot_can_display.ml b/info/control2011/src/tools/krobot_can_display.ml
index 5984832..3372904 100644
--- a/info/control2011/src/tools/krobot_can_display.ml
+++ b/info/control2011/src/tools/krobot_can_display.ml
@@ -1,7 +1,7 @@
open Lwt
open Krobot_can_decoder
-type frame_identifie =
+type frame_identifier =
| Text of string
| Num of int
@@ -31,6 +31,10 @@ object (self)
end
+let type_name type_ = match type_ with
+ | Text t -> t
+ | Num i -> string_of_int i
+
class packet_list ~packing =
let box = GPack.vbox ~packing () in
let hbox = GPack.hbox ~packing:box#add () in
@@ -65,10 +69,8 @@ class packet_list ~packing =
object
- method add type_ id timestamp (content:result) =
- let type_name = match type_ with
- | Text t -> t
- | Num i -> string_of_int i in
+ method add_packet (type_:frame_identifier) id timestamp (content:result) =
+ let type_name = type_name type_ in
let iter = packet_store#append () in
packet_store#set ~row:iter ~column:packet_time timestamp;
packet_store#set ~row:iter ~column:packet_id id;
@@ -84,19 +86,77 @@ object
end
+module StringMap = Map.Make(String)
+
+class field_info ~packing field_name caps =
+ let box = GPack.hbox ~packing () in
+ let _ = GMisc.label ~packing:box#add ~text:field_name () in
+ let result_widgets = List.map (fun cap -> cap, GMisc.label ~packing:box#add ()) caps in
+object
+ method set_result result =
+ List.iter (fun (cap, widget) ->
+ match cap with
+ | Value -> widget#set_label (result_to_string result)
+ | Min | Max -> failwith "TODO min/max display") result_widgets
+ method clear () = List.iter (fun (_,widget) -> widget#set_label "") result_widgets
+end
+
+class kind_info ~packing type_ (options:Krobot_can_decoder.opt list) =
+ let box = GPack.hbox ~packing () in
+ let _ = GMisc.label ~packing:box#add ~text:(type_name type_) () in
+ let count_widget = GMisc.label ~packing:box#add ~text:"0" () in
+ let count = ref 0 in
+ let id = ref 0 in
+ let timestamp = ref 0. in
+ let field_widgets = List.fold_left
+ (fun map -> function
+ | Field (name,cap) ->
+ let field_info = new field_info ~packing:box#add name cap in
+ StringMap.add name field_info map)
+ StringMap.empty options in
+object (self)
+ method add_packet p_id p_timestamp (content:result) =
+ id := p_id;
+ timestamp := p_timestamp;
+ count := !count + 1;
+ count_widget#set_label (string_of_int !count);
+ List.iter (fun (name,result) ->
+ try (StringMap.find name field_widgets)#set_result result
+ with
+ | Not_found -> ()) content
+end
+
+class display_box ~packing =
+ let box = GPack.vbox ~packing () in
+ let displayed = Hashtbl.create 0 in
+object
+ method add_packet (type_:frame_identifier) id timestamp (content:result) =
+ try
+ let kind_info = Hashtbl.find displayed type_ in
+ kind_info#add_packet id timestamp content;
+ true
+ with
+ | Not_found -> false
+ method add_kind type_ options =
+ Hashtbl.add displayed type_ (new kind_info ~packing:box#add type_ options)
+ method clear () = Hashtbl.clear displayed
+end
+
class ui () =
(* The toplevel window. *)
let window = GWindow.window ~title:"can debug" ~width:800 ~height:600 () in
let main_vbox = GPack.vbox ~packing:window#add () in
let packet_list = new packet_list ~packing:main_vbox#add in
let packet_store = ref [] in
- let decode_table = init_decode_table () in
+ let decode_table = init_decode_table [] in
let packet_count = ref 0 in
+ let display_box = new display_box ~packing:main_vbox#add in
object (self)
method window = window
method main_vbox = main_vbox
method packet_list = packet_list
+ method display_box = display_box
method display_frame id timestamp frame =
let result,name = decode_frame frame decode_table in
@@ -104,7 +164,8 @@ object (self)
| None -> Num frame.Krobot_can.identifier
| Some n -> Text n
in
- self#packet_list#add ident id timestamp result
+ if not (display_box#add_packet ident id timestamp result)
+ then self#packet_list#add_packet ident id timestamp result
method add_packet (timestamp:float) frame =
let id = !packet_count in
@@ -119,9 +180,12 @@ object (self)
method refresh () =
packet_list#clear ();
+ display_box#clear ();
List.iter (fun (id,timestamp,frame) -> self#display_frame id timestamp frame)
(List.rev !packet_store)
+ method decode_table = decode_table
+
initializer
ignore (window#connect#destroy quit);
@@ -129,13 +193,56 @@ end
let decode_table = ref None
let iface = ref None
+let config_file = ref None
let parse_arg () =
let desc =
- [ "-c", Arg.String (fun s -> decode_table := Some s), "file containing the configuration";
+ [ "-p", Arg.String (fun s -> decode_table := Some s), "file containing the protocol";
+ "-c", Arg.String (fun s -> config_file := Some s), "file containing the configuration";
"-i", Arg.String (fun s -> iface := Some s), "can interface";] in
Arg.parse desc (function "" -> () | _ -> Arg.usage desc ""; exit 2) ""
+let report_error f s e lexbuf =
+ let open Lexing in
+ let p1 = lexeme_start_p lexbuf in
+ let p2 = lexeme_end_p lexbuf in
+ let exn = Printexc.to_string e in
+ let off1 = p1.pos_cnum - p1.pos_bol in
+ let off2 = p2.pos_cnum - p1.pos_bol in
+ Printf.eprintf "File \"%s\", line %i, characters %i-%i:\nError: %s %s\n"
+ f p1.pos_lnum off1 off2
+ s exn;
+ exit 1
+
+let load_frames_desc f =
+ let channel = open_in f in
+ let lexbuf = Lexing.from_channel channel in
+ try
+ let r =
+ Krobot_can_desc_parser.file
+ Krobot_can_desc_lexer.token
+ lexbuf in
+ close_in channel;
+ r
+ with
+ | e -> report_error f "while parsing frame descriptions" e lexbuf
+
+let load_conf f =
+ match f with
+ | None -> []
+ | Some f ->
+ let channel = open_in f in
+ let lexbuf = Lexing.from_channel channel in
+ try
+ let r =
+ Krobot_can_desc_parser.config
+ Krobot_can_desc_lexer.token
+ lexbuf in
+ close_in channel;
+ r
+ with
+ | e -> report_error f "while parsing configuration" e lexbuf
+
let loop ui bus =
let rec aux () =
lwt (timestamp, frame) = Krobot_can_bus.recv bus in
@@ -149,6 +256,17 @@ let init iface =
ignore (GMain.init ~setlocale:false ());
Lwt_glib.install ();
let ui = new ui () in
+ begin match !decode_table with
+ | None -> ()
+ | Some f ->
+ let l = load_frames_desc f in
+ List.iter (fun d ->
+ match Krobot_can_decoder.check_description d with
+ | None -> ()
+ | Some d -> Krobot_can_decoder.set_description ui#decode_table d) l
+ end;
+ let config = load_conf !config_file in
+ List.iter (fun c -> ui#display_box#add_kind (Text c.frame) c.options) config;
ui#window#show ();
pick
[waiter;
hooks/post-receive
--
krobot
|
|
From: Pierre C. <Ba...@us...> - 2012-03-21 01:10:17
|
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 75f833848ac5b267cfa2a81304f038a28c26b37a (commit)
from ed621a1fd71df2caabf923996c609f366b36c211 (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 75f833848ac5b267cfa2a81304f038a28c26b37a
Author: chicco <cha...@cr...>
Date: Wed Mar 21 02:08:10 2012 +0100
[info] parser for can frame description
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/_oasis b/info/control2011/_oasis
index 1f4cca0..9472d20 100644
--- a/info/control2011/_oasis
+++ b/info/control2011/_oasis
@@ -49,12 +49,16 @@ Library krobot
Library "krobot-can"
FindlibName: can
FindlibParent: krobot
- BuildDepends: krobot, lwt.syntax
+ BuildDepends: krobot, lwt.syntax, sexplib, sexplib.syntax, bitstring
XMETADescription: CAN interface using SocketCAN
- XMETARequires: krobot
+ XMETARequires: krobot, sexplib, bitstring
Path: src/can
Install: true
- Modules: Krobot_can_bus
+ Modules:
+ Krobot_can_bus,
+ Krobot_can_decoder,
+ Krobot_can_desc_lexer,
+ Krobot_can_desc_parser
CSources: can_stubs.c
# +-------------------------------------------------------------------+
@@ -208,6 +212,14 @@ Executable "krobot-ia"
MainIs: krobot_ia.ml
BuildDepends: krobot, lwt.syntax
+Executable "krobot-can-display"
+ Path: src/tools
+ Build$: flag(gtk)
+ Install$: flag(gtk)
+ CompiledObject: best
+ MainIs: krobot_can_display.ml
+ BuildDepends: krobot.can, lwt.syntax, sexplib, sexplib.syntax, cairo.lablgtk2, lwt.glib
+
# +-------------------------------------------------------------------+
# | Examples |
# +-------------------------------------------------------------------+
diff --git a/info/control2011/src/can/krobot_can_decoder.ml b/info/control2011/src/can/krobot_can_decoder.ml
new file mode 100644
index 0000000..4c82c9e
--- /dev/null
+++ b/info/control2011/src/can/krobot_can_decoder.ml
@@ -0,0 +1,138 @@
+open Sexplib.Conv
+open Krobot_can
+
+type signedness =
+ | Signed
+ | Unsigned with sexp
+
+type display =
+ | Bit
+ | Hex
+ | Int of signedness
+ | Float of float option
+ | Char
+ | No with sexp
+
+type size = int with sexp
+
+type endian = Bitstring.endian = BigEndian | LittleEndian | NativeEndian with sexp
+
+type field =
+ { name : string;
+ display : display;
+ size : size;
+ endian : endian;
+ field_description : string option } with sexp
+
+type desc = field list with sexp
+type description = field list with sexp
+
+type frame_desc =
+ { frame_name : string;
+ frame_id : int;
+ frame_data : field list;
+ frame_description : string option }
+
+type result_field =
+ | R_bit of bool
+ | R_hex of int
+ | R_int of int
+ | R_float of float
+ | R_char of char with sexp
+
+type result = ( string * result_field ) list with sexp
+
+type decode_table =
+ (int * kind,desc * string option) Hashtbl.t
+
+let is_description_correct desc =
+ ( List.for_all (fun { display; size } ->
+ size > 0 &&
+ ( match display with
+ | Bit -> size = 1
+ | Char -> size <= 8
+ | _ -> true )) desc ) &&
+ let size = List.fold_left (fun acc t -> acc + t.size) 0 desc in
+ if size > 64
+ then false
+ else true
+
+let check_description desc =
+ if is_description_correct desc
+ then Some desc
+ else None
+
+let split_bitstring (acc,bitstring) field =
+ let (s,start,len) = bitstring in
+ if len < field.size
+ then (acc,(s,start + len,0))
+ else let value = Bitstring.takebits field.size bitstring in
+ let rest = Bitstring.dropbits field.size bitstring in
+ ((value,field)::acc,rest)
+
+let read_field ((str,start,end_),field) =
+ let i = Bitstring.extract_int_ee_unsigned field.endian str start end_ field.size in
+ match field.display with
+ | Bit ->
+ begin
+ let b = match i with
+ | 0 -> false
+ | 1 -> true
+ | _ -> failwith "incorrect description" in
+ Some (field.name,R_bit b)
+ end
+ | Int sign ->
+ failwith "handle sign here...";
+ Some (field.name,R_int i)
+ | Hex -> Some (field.name,R_hex i)
+ | Char -> Some (field.name,R_char (Char.chr i))
+ | Float coef ->
+ Some (field.name,R_float
+ (match coef with
+ | None -> float i
+ | Some c -> c *. (float i)))
+ | No -> None
+
+let filter_map f l =
+ let rec aux = function
+ | [] -> []
+ | t::q -> match f t with
+ | None -> aux q
+ | Some v -> v :: (aux q) in
+ aux l
+
+let read_fields bitstring description =
+ let fields,rest = List.fold_left split_bitstring ([],bitstring) description in
+ filter_map read_field (List.rev fields)
+
+let decode_frame' frame descriptions =
+ read_fields (Bitstring.bitstring_of_string frame.Krobot_can.data) descriptions
+
+let default_desc =
+ let field =
+ { name = "";
+ display = Hex;
+ size = 8;
+ endian = BigEndian;
+ field_description = None } in
+ [ field; field; field; field;
+ field; field; field; field ]
+
+let decode_frame frame table =
+ let desc,name = try Hashtbl.find table (frame.identifier, frame.kind) with
+ | Not_found -> default_desc,None in
+ decode_frame' frame desc,name
+
+let init_decode_table () =
+ Hashtbl.create 0
+
+let set_description t (i,kind) ?name desc =
+ Hashtbl.replace t (i,kind) (desc,name)
+
+let result_to_string = function
+ | R_bit b -> string_of_bool b
+ | R_hex i -> Printf.sprintf "%X" i
+ | R_int i -> string_of_int i
+ | R_float f -> string_of_float f
+ | R_char c -> Printf.sprintf "%c" c
+
diff --git a/info/control2011/src/can/krobot_can_decoder.mli b/info/control2011/src/can/krobot_can_decoder.mli
new file mode 100644
index 0000000..426f8f6
--- /dev/null
+++ b/info/control2011/src/can/krobot_can_decoder.mli
@@ -0,0 +1,55 @@
+open Sexplib.Conv
+
+type signedness =
+ | Signed
+ | Unsigned with sexp
+
+type display =
+ | Bit
+ | Hex
+ | Int of signedness
+ | Float of float option
+ | Char
+ | No
+ with sexp
+
+type size = int with sexp
+
+type endian = Bitstring.endian = BigEndian | LittleEndian | NativeEndian with sexp
+
+type field =
+ { name : string;
+ display : display;
+ size : size;
+ endian : endian;
+ field_description : string option } with sexp
+
+type frame_desc =
+ { frame_name : string;
+ frame_id : int;
+ frame_data : field list;
+ frame_description : string option }
+
+type desc = field list with sexp
+type description with sexp
+
+type result_field =
+ | R_bit of bool
+ | R_hex of int
+ | R_int of int
+ | R_float of float
+ | R_char of char with sexp
+
+type result = ( string * result_field ) list with sexp
+
+type decode_table
+
+val check_description : desc -> description option
+
+val decode_frame : Krobot_can.frame -> decode_table -> result * string option
+
+val init_decode_table : unit -> decode_table
+
+val set_description : decode_table -> (int * Krobot_can.kind) -> ?name:string -> description -> unit
+
+val result_to_string : result_field -> string
diff --git a/info/control2011/src/can/krobot_can_desc_lexer.mll b/info/control2011/src/can/krobot_can_desc_lexer.mll
new file mode 100644
index 0000000..b313ff0
--- /dev/null
+++ b/info/control2011/src/can/krobot_can_desc_lexer.mll
@@ -0,0 +1,55 @@
+{
+ open Lexing
+ open Krobot_can_decoder
+ open Krobot_can_desc_parser
+ type pos = Lexing.position * Lexing.position
+ exception Unexpected_character of char * pos
+
+ let string_buffer = Buffer.create 0
+}
+
+let number =
+ ['0'-'9']+
+ | "0x" ['0'-'9' 'a'-'f' 'A'-'F']+
+ | "0b" ['0' '1']+
+
+let alpha = ['a'-'z' 'A'-'Z' '_']
+let alphanum = ['a'-'z' 'A'-'Z' '0'-'9' '_' ]
+let ident = alpha alphanum*
+
+rule token = parse
+ | [' ' '\t' ] { token lexbuf } (* skip blanks *)
+ | ['\n'] { Lexing.new_line lexbuf; token lexbuf }
+ | number as num { NUM (int_of_string num) }
+ | "bit" { BIT }
+ | "int" { INT }
+ | "float" { FLOAT }
+ | "double" { DOUBLE }
+ | "signed" { SIGNED }
+ | "unsigned" { UNSIGNED }
+ | "bigendian" { BIGENDIAN }
+ | "littleendian" { LITTLEENDIAN }
+ | '{' { LCBRACKET }
+ | '}' { RCBRACKET }
+ | ';' { SEMICOLON }
+ | '"' { let s = text lexbuf in DESCRIPTION s }
+ | ident { IDENT (lexeme lexbuf) }
+ | eof { EOF }
+ | "(*" { comments 0 lexbuf }
+ | _ as c { raise (Unexpected_character
+ (c,(Lexing.lexeme_start_p lexbuf,
+ Lexing.lexeme_end_p lexbuf))) }
+
+and comments level = parse
+ | "*)" { if level = 0 then token lexbuf
+ else comments (level-1) lexbuf }
+ | "(*" { comments (level+1) lexbuf }
+ | eof { raise End_of_file }
+ | _ { comments level lexbuf }
+
+and text = parse
+ | '"' { let r = Buffer.contents string_buffer in
+ Buffer.clear string_buffer;
+ r }
+ | eof { raise End_of_file }
+ | _ as c { Buffer.add_char string_buffer c; text lexbuf }
diff --git a/info/control2011/src/can/krobot_can_desc_parser.mly b/info/control2011/src/can/krobot_can_desc_parser.mly
new file mode 100644
index 0000000..322e88f
--- /dev/null
+++ b/info/control2011/src/can/krobot_can_desc_parser.mly
@@ -0,0 +1,58 @@
+%{
+ open Krobot_can_decoder
+%}
+
+%token <int> NUM
+%token <string> IDENT
+%token <string> DESCRIPTION
+%token INT, BIT, FLOAT, DOUBLE, SIGNED, UNSIGNED, LITTLEENDIAN, BIGENDIAN
+%token LCOLON, LCBRACKET, RCBRACKET, SEMICOLON, DBLQUOTE
+%token EOF
+
+%start file
+%type <Krobot_can_decoder.frame_desc list> file
+%%
+
+file :
+ | frame EOF { [$1] }
+ | frame file { $1 :: $2 }
+;
+
+frame :
+ | IDENT NUM LCBRACKET fields RCBRACKET
+ { { frame_name = $1; frame_id = $2; frame_data = $4; frame_description = None } }
+ | IDENT NUM DESCRIPTION LCBRACKET fields RCBRACKET
+ { { frame_name = $1; frame_id = $2; frame_data = $5; frame_description = Some $3 } }
+
+fields :
+ | field { [$1] }
+ | field SEMICOLON { [$1] }
+ | field SEMICOLON fields { $1 :: $3 }
+
+field :
+ | IDENT field_type
+ { let type_, size = $2 in
+ { name = $1; display = type_; size = size; endian = LittleEndian;
+ field_description = None } }
+ | IDENT field_type DESCRIPTION
+ { let type_, size = $2 in
+ { name = $1; display = type_; size = size; endian = LittleEndian;
+ field_description = Some $3 } }
+
+field_type :
+ | BIT { Bit, 1 }
+ | INT signedness NUM { Int $2, $3 }
+ | FLOAT { Float None, 32 }
+ | DOUBLE { Float None, 64 }
+
+signedness :
+ | SIGNED { Signed }
+ | UNSIGNED { Unsigned }
+
+/*
+endianness :
+ | BIGENDIAN { BigEndian }
+ | LITTLEENDIAN { LittleEndian }
+*/
+
+%%
diff --git a/info/control2011/src/tools/krobot_can_display.ml b/info/control2011/src/tools/krobot_can_display.ml
new file mode 100644
index 0000000..5984832
--- /dev/null
+++ b/info/control2011/src/tools/krobot_can_display.ml
@@ -0,0 +1,167 @@
+open Lwt
+open Krobot_can_decoder
+
+type frame_identifie =
+ | Text of string
+ | Num of int
+
+let waiter, wakener = wait ()
+let quit () = Lwt.wakeup wakener ()
+
+class packet_display ~packing =
+ let box = GPack.vbox ~packing () in
+ let current_child = ref (GPack.vbox ~packing:box#add ()) in
+object (self)
+
+ method new_box () =
+ let old_child = !current_child in
+ let tmp_box = GPack.vbox ~packing:box#add () in
+ current_child := tmp_box;
+ old_child#destroy ();
+ tmp_box
+
+ method set_packet result =
+ let tmp_box = self#new_box () in
+ let _ = List.map (fun (name, result) ->
+ GMisc.label ~text:(Printf.sprintf "%s : %s" name (result_to_string result))
+ ~packing:tmp_box#add ()) result in
+ ()
+
+ method clear () = ignore (self#new_box ())
+
+end
+
+class packet_list ~packing =
+ let box = GPack.vbox ~packing () in
+ let hbox = GPack.hbox ~packing:box#add () in
+ let packet_display = new packet_display ~packing:(box#pack ~expand:false) in
+ let scroll = GBin.scrolled_window ~vpolicy:`ALWAYS ~hpolicy:`NEVER ~packing:hbox#add () in
+
+ let model = new GTree.column_list in
+ let packet_time = model#add Gobject.Data.double in
+ let packet_type = model#add Gobject.Data.string in
+ let packet_id = model#add Gobject.Data.int in
+ let packet_content = model#add Gobject.Data.caml in
+
+ let packet_store = GTree.list_store model in
+
+ let column_packet_time = GTree.view_column ~title:"time" ~renderer:(GTree.cell_renderer_text [], ["text", packet_time]) () in
+ let column_packet_type = GTree.view_column ~title:"type" ~renderer:(GTree.cell_renderer_text [], ["text", packet_type]) () in
+ let column_packet_id = GTree.view_column ~title:"id" ~renderer:(GTree.cell_renderer_text [], ["text", packet_id]) () in
+
+ let view = GTree.view ~packing:scroll#add_with_viewport ~model:packet_store () in
+
+ let _ = view#append_column column_packet_time in
+ let _ = view#append_column column_packet_type in
+ let _ = view#append_column column_packet_id in
+
+ let selection_changed selection () =
+ let update path =
+ let row = packet_store#get_iter path in
+ let result = packet_store#get ~row ~column:packet_content in
+ packet_display#set_packet result
+ in
+ List.iter update selection#get_selected_rows in
+
+object
+
+ method add type_ id timestamp (content:result) =
+ let type_name = match type_ with
+ | Text t -> t
+ | Num i -> string_of_int i in
+ let iter = packet_store#append () in
+ packet_store#set ~row:iter ~column:packet_time timestamp;
+ packet_store#set ~row:iter ~column:packet_id id;
+ packet_store#set ~row:iter ~column:packet_type type_name;
+ packet_store#set ~row:iter ~column:packet_content content
+
+ method clear () =
+ packet_store#clear ();
+ packet_display#clear ()
+
+ initializer
+ ignore (view#selection#connect#changed ~callback:(selection_changed view#selection));
+
+end
+
+class ui () =
+ (* The toplevel window. *)
+ let window = GWindow.window ~title:"can debug" ~width:800 ~height:600 () in
+ let main_vbox = GPack.vbox ~packing:window#add () in
+ let packet_list = new packet_list ~packing:main_vbox#add in
+ let packet_store = ref [] in
+ let decode_table = init_decode_table () in
+ let packet_count = ref 0 in
+
+object (self)
+ method window = window
+ method main_vbox = main_vbox
+ method packet_list = packet_list
+
+ method display_frame id timestamp frame =
+ let result,name = decode_frame frame decode_table in
+ let ident = match name with
+ | None -> Num frame.Krobot_can.identifier
+ | Some n -> Text n
+ in
+ self#packet_list#add ident id timestamp result
+
+ method add_packet (timestamp:float) frame =
+ let id = !packet_count in
+ incr packet_count;
+ packet_store := (id,timestamp,frame) :: !packet_store;
+ try
+ self#display_frame id timestamp frame;
+ return ()
+ with
+ | exn ->
+ Lwt_log.warning_f ~exn "display_packet failed"
+
+ method refresh () =
+ packet_list#clear ();
+ List.iter (fun (id,timestamp,frame) -> self#display_frame id timestamp frame)
+ (List.rev !packet_store)
+
+ initializer
+ ignore (window#connect#destroy quit);
+
+end
+
+let decode_table = ref None
+let iface = ref None
+
+let parse_arg () =
+ let desc =
+ [ "-c", Arg.String (fun s -> decode_table := Some s), "file containing the configuration";
+ "-i", Arg.String (fun s -> iface := Some s), "can interface";] in
+ Arg.parse desc (function "" -> () | _ -> Arg.usage desc ""; exit 2) ""
+
+let loop ui bus =
+ let rec aux () =
+ lwt (timestamp, frame) = Krobot_can_bus.recv bus in
+ lwt () = ui#add_packet timestamp frame in
+ aux ()
+ in
+ aux ()
+
+let init iface =
+ lwt bus = Krobot_can_bus.open_can iface in
+ ignore (GMain.init ~setlocale:false ());
+ Lwt_glib.install ();
+ let ui = new ui () in
+ ui#window#show ();
+ pick
+ [waiter;
+ loop ui bus]
+
+lwt () =
+ parse_arg ();
+ let iface =
+ match !iface with
+ | None -> "slcan0"
+ | Some s -> s in
+ try_lwt
+ init iface
+ with Unix.Unix_error(error, func, arg) ->
+ Lwt_log.error_f "'%s' failed with: %s" func (Unix.error_message error)
+
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2012-03-03 17:15:01
|
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 ed621a1fd71df2caabf923996c609f366b36c211 (commit)
via 188b210e987633a8dc3c31bd9d898005ab9dac50 (commit)
from fbc16a5651e122e72848efe649c9666aed0a78a5 (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 ed621a1fd71df2caabf923996c609f366b36c211
Author: Xavier Lagorce <Xav...@cr...>
Date: Sat Mar 3 18:18:38 2012 +0100
[Controller_Motor_STM32/lib] Bug fixes in library
commit 188b210e987633a8dc3c31bd9d898005ab9dac50
Author: Xavier Lagorce <Xav...@cr...>
Date: Tue Jan 31 16:27:29 2012 +0100
[Controller_Motor_STM32/lib] Added a helper to the arm2R_controller to know
if an arm is moving.
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.c b/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.c
index 5c64043..1b4d770 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.c
@@ -58,44 +58,51 @@ void a2Rc_start(uint8_t arm,
tc_set_position(tc_x, x);
tc_set_position(tc_y, y);
// Create the command generators
- new_a2r_generator(&ac_state[arm].theta1_s,
+ new_a2r_generator(&(ac_state[arm].theta1_s),
tc_get_position_generator(tc_x),
tc_get_speed_generator(tc_x),
tc_get_position_generator(tc_y),
tc_get_speed_generator(tc_y),
l1, l2, enc_theta1, enc_theta2, 1);
- new_a2r_generator(&ac_state[arm].theta2_s,
+ new_a2r_generator(&(ac_state[arm].theta2_s),
tc_get_position_generator(tc_x),
tc_get_speed_generator(tc_x),
tc_get_position_generator(tc_y),
tc_get_speed_generator(tc_y),
l1, l2, enc_theta1, enc_theta2, 2);
// Create the reference generators
- new_ramp2_generator(&ac_state[arm].theta1, theta1, &ac_state[arm].theta1_s);
- new_ramp2_generator(&ac_state[arm].theta2, theta2, &ac_state[arm].theta2_s);
+ new_ramp2_generator(&(ac_state[arm].theta1), theta1, &(ac_state[arm].theta1_s));
+ new_ramp2_generator(&(ac_state[arm].theta2), theta2, &(ac_state[arm].theta2_s));
// Start the different references generators
- start_generator(&ac_state[arm].theta1_s);
- start_generator(&ac_state[arm].theta2_s);
- start_generator(&ac_state[arm].theta1);
- start_generator(&ac_state[arm].theta2);
+ start_generator(&(ac_state[arm].theta1_s));
+ start_generator(&(ac_state[arm].theta2_s));
+ start_generator(&(ac_state[arm].theta1));
+ start_generator(&(ac_state[arm].theta2));
// Activate the 'enabled' flag
ac_state[arm].enabled = 1;
}
void a2Rc_goto_position_x(uint8_t arm, float position, float speed, float acc) {
- tc_goto(arm+A2R_TC_X, position, speed, acc);
+ tc_goto(ac_state[arm].tc_ind[0], position, speed, acc);
}
void a2Rc_goto_position_y(uint8_t arm, float position, float speed, float acc) {
- tc_goto(arm+A2R_TC_Y, position, speed, acc);
+ tc_goto(ac_state[arm].tc_ind[1], position, speed, acc);
}
+uint8_t a2Rc_is_moving(uint8_t arm) {
+ return tc_is_working(
+ TC_MASK(ac_state[arm].tc_ind[0])
+ | TC_MASK(ac_state[arm].tc_ind[1]));
+}
+
+
command_generator_t* a2Rc_get_first_articulation_gen(uint8_t arm) {
- return &ac_state[arm].theta1;
+ return &(ac_state[arm].theta1);
}
command_generator_t* a2Rc_get_second_articulation_gen(uint8_t arm) {
- return &ac_state[arm].theta2;
+ return &(ac_state[arm].theta2);
}
void a2Rc_get_angles(uint8_t arm, float *theta1, float *theta2) {
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.h b/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.h
index fc884f5..3b88f90 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.h
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.h
@@ -55,6 +55,12 @@ void a2Rc_goto_position_x(uint8_t arm, float position, float speed, float acc);
void a2Rc_goto_position_y(uint8_t arm, float position, float speed, float acc);
/*
+ * Returns a non-zero value if the arm is moving
+ * - arm : ID of the arm to consider
+ */
+uint8_t a2Rc_is_moving(uint8_t arm);
+
+/*
* Get the command generator outputing the reference position for
* the first articulation
* - arm : ID of the arm of which to get the generator
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/command_generator.c b/elec/boards/Controller_Motor_STM32/Firmwares/lib/command_generator.c
index 3b36e70..baa58a6 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/command_generator.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/command_generator.c
@@ -50,6 +50,7 @@ command_generator_t* new_dd_generator(command_generator_t *generator,
uint8_t type) {
generator->type.t = (type == 1) ? GEN_DD_RIGHT : GEN_DD_LEFT;
generator->type.callback.type = GEN_CALLBACK_NONE;
+ generator->type.state = GEN_STATE_PAUSE;
generator->dd.linear_pos = linear_pos;
generator->dd.linear_speed = linear_speed;
generator->dd.rotational_pos = rotational_pos;
@@ -85,6 +86,7 @@ command_generator_t* new_hd_generator(command_generator_t *generator,
break;
}
generator->type.callback.type = GEN_CALLBACK_NONE;
+ generator->type.state = GEN_STATE_PAUSE;
generator->hd.linear_pos_x = linear_pos_x;
generator->hd.linear_speed_x = linear_speed_x;
generator->hd.linear_pos_x = linear_pos_y;
@@ -107,10 +109,10 @@ command_generator_t* new_a2r_generator(command_generator_t *generator,
uint8_t enc_theta1, uint8_t enc_theta2,
uint8_t type) {
switch (type) {
- case 1: // Back wheel
+ case 1: // First articulation
generator->type.t = GEN_AC_T1;
break;
- case 2: // Right-front wheel
+ case 2: // Second articulation
generator->type.t = GEN_AC_T2;
break;
default:
@@ -118,10 +120,11 @@ command_generator_t* new_a2r_generator(command_generator_t *generator,
break;
}
generator->type.callback.type = GEN_CALLBACK_NONE;
+ generator->type.state = GEN_STATE_PAUSE;
generator->a2r.linear_pos_x = linear_pos_x;
generator->a2r.linear_speed_x = linear_speed_x;
- generator->a2r.linear_pos_x = linear_pos_y;
- generator->a2r.linear_speed_x = linear_speed_y;
+ generator->a2r.linear_pos_y = linear_pos_y;
+ generator->a2r.linear_speed_y = linear_speed_y;
generator->a2r.l1 = l1;
generator->a2r.l2 = l2;
generator->a2r.enc_theta1 = enc_theta1;
@@ -286,6 +289,12 @@ float get_output_value(command_generator_t *generator) {
s1 = sin(theta1);
s2 = sin(theta2);
w = -l1*l2*s1*c2+l1*l2*s2*c1;
+ if (w < 0.01 && w > -0.01) {
+ if (w > 0)
+ w = 0.01;
+ else
+ w = -0.01;
+ }
switch (generator->type.t) {
case GEN_AC_T1:
generator->type.last_output = (-l1*s1*u_x+l2*s2*u_y)/w;
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/encoder.c b/elec/boards/Controller_Motor_STM32/Firmwares/lib/encoder.c
index 10587f0..b30eb52 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/encoder.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/encoder.c
@@ -109,10 +109,10 @@ void encodersInit(void) {
TIM_Cmd(TIM4,ENABLE);
// Initialize scaling factors
- for (int i=0; i < 4; i++) {
- enc_params[i].origin = 0;
- enc_params[i].scale= 1.0;
- }
+ //for (int i=0; i < 4; i++) {
+ // enc_params[i].origin = 0;
+ // enc_params[i].scale= 1.0;
+ //}
}
/*
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2012-01-31 15:03: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 fbc16a5651e122e72848efe649c9666aed0a78a5 (commit)
via 4526c9089a4329c2a0d2697c86750a707a3e9b7e (commit)
from 194a925f13cf0074d8da123604f1c5740efddd73 (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 fbc16a5651e122e72848efe649c9666aed0a78a5
Author: Xavier Lagorce <Xav...@cr...>
Date: Tue Jan 31 16:06:04 2012 +0100
[Controller_Motor_STM32/lib] Added some functionnalities and better initialization
of the arm2R_controller module.
commit 4526c9089a4329c2a0d2697c86750a707a3e9b7e
Author: Xavier Lagorce <Xav...@cr...>
Date: Tue Jan 31 16:05:18 2012 +0100
[Controller_Motor_STM32/lib] Added the possibility to force the position of a trajectory controller
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.c b/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.c
index f2d8064..5c64043 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.c
@@ -10,8 +10,9 @@
#include "arm2R_controller.h"
typedef struct {
- uint8_t enabled, tc_ind[2];
+ uint8_t enabled, tc_ind[2], enc_theta1, enc_theta2;
command_generator_t theta1_s, theta2_s, theta1, theta2;
+ float l1, l2;
} a2Rc_state_t;
a2Rc_state_t ac_state[2];
@@ -34,6 +35,18 @@ void a2Rc_start(uint8_t arm,
uint8_t enc_theta1, uint8_t enc_theta2) {
uint8_t tc_x, tc_y;
+ float theta1, theta2, x, y;
+
+ // Record the parameters of the arm
+ ac_state[arm].l1 = l1;
+ ac_state[arm].l2 = l2;
+ ac_state[arm].enc_theta1 = enc_theta1;
+ ac_state[arm].enc_theta2 = enc_theta2;
+
+ // Get the current position of the arm
+ a2Rc_get_angles(arm, &theta1, &theta2);
+ x = l1*cos(theta1) + l2*cos(theta2);
+ y = l1*sin(theta1) + l2*sin(theta2);
// Read trajectory controllers identifiers
tc_x = ac_state[arm].tc_ind[0];
@@ -42,6 +55,9 @@ void a2Rc_start(uint8_t arm,
// Init Trajectory controllers
tc_new_controller(tc_x);
tc_new_controller(tc_y);
+ tc_set_position(tc_x, x);
+ tc_set_position(tc_y, y);
+ // Create the command generators
new_a2r_generator(&ac_state[arm].theta1_s,
tc_get_position_generator(tc_x),
tc_get_speed_generator(tc_x),
@@ -54,13 +70,18 @@ void a2Rc_start(uint8_t arm,
tc_get_position_generator(tc_y),
tc_get_speed_generator(tc_y),
l1, l2, enc_theta1, enc_theta2, 2);
- new_ramp2_generator(&ac_state[arm].theta1, 0.0, &ac_state[arm].theta1_s);
- new_ramp2_generator(&ac_state[arm].theta2, 0.0, &ac_state[arm].theta2_s);
+ // Create the reference generators
+ new_ramp2_generator(&ac_state[arm].theta1, theta1, &ac_state[arm].theta1_s);
+ new_ramp2_generator(&ac_state[arm].theta2, theta2, &ac_state[arm].theta2_s);
+ // Start the different references generators
start_generator(&ac_state[arm].theta1_s);
start_generator(&ac_state[arm].theta2_s);
start_generator(&ac_state[arm].theta1);
start_generator(&ac_state[arm].theta2);
+
+ // Activate the 'enabled' flag
+ ac_state[arm].enabled = 1;
}
void a2Rc_goto_position_x(uint8_t arm, float position, float speed, float acc) {
@@ -76,3 +97,16 @@ command_generator_t* a2Rc_get_first_articulation_gen(uint8_t arm) {
command_generator_t* a2Rc_get_second_articulation_gen(uint8_t arm) {
return &ac_state[arm].theta2;
}
+
+void a2Rc_get_angles(uint8_t arm, float *theta1, float *theta2) {
+ *theta1 = getEncoderPosition_f(ac_state[arm].enc_theta1);
+ *theta2 = getEncoderPosition_f(ac_state[arm].enc_theta2);
+}
+
+void a2Rc_get_position(uint8_t arm, float *x, float *y) {
+ float theta1, theta2;
+
+ a2Rc_get_angles(arm, &theta1, &theta2);
+ *x = ac_state[arm].l1 * cos(theta1) + ac_state[arm].l2 * cos(theta2);
+ *y = ac_state[arm].l1 * sin(theta1) + ac_state[arm].l2 * sin(theta2);
+}
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.h b/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.h
index 4620a47..fc884f5 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.h
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.h
@@ -67,4 +67,20 @@ command_generator_t* a2Rc_get_first_articulation_gen(uint8_t arm);
*/
command_generator_t* a2Rc_get_second_articulation_gen(uint8_t arm);
+/*
+ * Get the current angles of the articulations.
+ * - arm : ID of the arm of which to get the angles
+ * - theta1 : address of the variable to write the first angle into
+ * - theta2 : address of the variable to write the second angle into
+ */
+void a2Rc_get_angles(uint8_t arm, float *theta1, float *theta2);
+
+/*
+ * Computes the current position of the end of the arm
+ * - arm : ID of the arm of which to compute the end's position
+ * - x : address of the variable to wirte the X coordinate into
+ * - y : address of the variable to wirte the Y coordinate into
+ */
+void a2Rc_get_position(uint8_t arm, float *x, float *y);
+
#endif /* __ARM2R_CONTROLLER_H */
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/trajectory_controller.c b/elec/boards/Controller_Motor_STM32/Firmwares/lib/trajectory_controller.c
index ef59d93..d843f4e 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/trajectory_controller.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/trajectory_controller.c
@@ -316,6 +316,29 @@ void tc_goto_speed(uint8_t cntr_index, float speed, float acceleration) {
cont->working = 1;
}
+void tc_set_position(uint8_t cntr_index, float position) {
+ trajectory_controller_t *cont;
+
+ // Get the controller and verifies it is enabled
+ if (cntr_index >= NUM_TC_MAX)
+ return;
+ cont = &controllers[cntr_index];
+ if (!cont->enabled)
+ return;
+
+ // Disable a possibly running profile
+ cont->aut.state = TRAPEZOID_STATE_STOP;
+ remove_callback(&cont->position);
+ remove_callback(&cont->speed);
+
+ // Set speed and acceleration
+ adjust_value(&cont->position, position);
+ adjust_value(&cont->speed, 0.0);
+ adjust_speed(&cont->speed, 0.0);
+
+ cont->working = 0;
+}
+
void tc_set_speed(uint8_t cntr_index, float speed) {
trajectory_controller_t *cont;
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/trajectory_controller.h b/elec/boards/Controller_Motor_STM32/Firmwares/lib/trajectory_controller.h
index ba4df32..d0959e7 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/trajectory_controller.h
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/trajectory_controller.h
@@ -84,6 +84,11 @@ void tc_move(uint8_t cntr_index, float angle, float speed, float acceleration);
void tc_goto_speed(uint8_t cntr_index, float speed, float acceleration);
/*
+ * Change the position of a controller without any concern about continuity
+ */
+void tc_set_position(uint8_t cntr_index, float position);
+
+/*
* Change the speed command of a controller without any concern about acceleration
*/
void tc_set_speed(uint8_t cntr_index, float speed);
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2012-01-28 16:09: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 194a925f13cf0074d8da123604f1c5740efddd73 (commit)
from 356ea449946b76d194a653473fe99df01c6b6b1a (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 194a925f13cf0074d8da123604f1c5740efddd73
Author: Xavier Lagorce <Xav...@cr...>
Date: Sat Jan 28 17:11:10 2012 +0100
[Controller_Motor_STM32/lib] Added some support for some 2R arms.
The associated functions and generators enable the control of a 2R arm
in effector space (X,Y of the end of the arm) instead of controlling its
two articulations directly.
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/controller_motor_stm32_user.mk b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/controller_motor_stm32_user.mk
index 523bdbb..6092393 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/controller_motor_stm32_user.mk
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/controller_motor_stm32_user.mk
@@ -24,6 +24,7 @@ controller_motor_stm32_USER_CSRC = \
$(controller_motor_stm32_LIB_PATH)/bezier_utils.c \
$(controller_motor_stm32_LIB_PATH)/differential_drive.c \
$(controller_motor_stm32_LIB_PATH)/lift_controller.c \
+ $(controller_motor_stm32_LIB_PATH)/arm2R_controller.c \
$(controller_motor_stm32_SRC_PATH)/can_monitor.c \
$(controller_motor_stm32_SRC_PATH)/main.c \
#
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/controller_motor_stm32_user.mk b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/controller_motor_stm32_user.mk
index 523bdbb..6092393 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/controller_motor_stm32_user.mk
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/controller_motor_stm32_user.mk
@@ -24,6 +24,7 @@ controller_motor_stm32_USER_CSRC = \
$(controller_motor_stm32_LIB_PATH)/bezier_utils.c \
$(controller_motor_stm32_LIB_PATH)/differential_drive.c \
$(controller_motor_stm32_LIB_PATH)/lift_controller.c \
+ $(controller_motor_stm32_LIB_PATH)/arm2R_controller.c \
$(controller_motor_stm32_SRC_PATH)/can_monitor.c \
$(controller_motor_stm32_SRC_PATH)/main.c \
#
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.c b/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.c
new file mode 100644
index 0000000..f2d8064
--- /dev/null
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.c
@@ -0,0 +1,78 @@
+/*
+ * arm2R_controller.c
+ * -----------------
+ * Copyright : (c) 2012, Xavier Lagorce <Xav...@cr...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ */
+
+#include "arm2R_controller.h"
+
+typedef struct {
+ uint8_t enabled, tc_ind[2];
+ command_generator_t theta1_s, theta2_s, theta1, theta2;
+} a2Rc_state_t;
+
+a2Rc_state_t ac_state[2];
+
+void a2Rc_init(void) {
+ int i;
+
+ // Init state
+ for (i=0; i<2; i++) {
+ ac_state[i].enabled = 0;
+ }
+ ac_state[ARM1].tc_ind[0] = A2R_ARM1_TC + A2R_TC_X;
+ ac_state[ARM1].tc_ind[1] = A2R_ARM1_TC + A2R_TC_Y;
+ ac_state[ARM2].tc_ind[0] = A2R_ARM2_TC + A2R_TC_X;
+ ac_state[ARM2].tc_ind[1] = A2R_ARM2_TC + A2R_TC_Y;
+}
+
+void a2Rc_start(uint8_t arm,
+ float l1, float l2,
+ uint8_t enc_theta1, uint8_t enc_theta2) {
+
+ uint8_t tc_x, tc_y;
+
+ // Read trajectory controllers identifiers
+ tc_x = ac_state[arm].tc_ind[0];
+ tc_y = ac_state[arm].tc_ind[1];
+
+ // Init Trajectory controllers
+ tc_new_controller(tc_x);
+ tc_new_controller(tc_y);
+ new_a2r_generator(&ac_state[arm].theta1_s,
+ tc_get_position_generator(tc_x),
+ tc_get_speed_generator(tc_x),
+ tc_get_position_generator(tc_y),
+ tc_get_speed_generator(tc_y),
+ l1, l2, enc_theta1, enc_theta2, 1);
+ new_a2r_generator(&ac_state[arm].theta2_s,
+ tc_get_position_generator(tc_x),
+ tc_get_speed_generator(tc_x),
+ tc_get_position_generator(tc_y),
+ tc_get_speed_generator(tc_y),
+ l1, l2, enc_theta1, enc_theta2, 2);
+ new_ramp2_generator(&ac_state[arm].theta1, 0.0, &ac_state[arm].theta1_s);
+ new_ramp2_generator(&ac_state[arm].theta2, 0.0, &ac_state[arm].theta2_s);
+
+ start_generator(&ac_state[arm].theta1_s);
+ start_generator(&ac_state[arm].theta2_s);
+ start_generator(&ac_state[arm].theta1);
+ start_generator(&ac_state[arm].theta2);
+}
+
+void a2Rc_goto_position_x(uint8_t arm, float position, float speed, float acc) {
+ tc_goto(arm+A2R_TC_X, position, speed, acc);
+}
+void a2Rc_goto_position_y(uint8_t arm, float position, float speed, float acc) {
+ tc_goto(arm+A2R_TC_Y, position, speed, acc);
+}
+
+command_generator_t* a2Rc_get_first_articulation_gen(uint8_t arm) {
+ return &ac_state[arm].theta1;
+}
+command_generator_t* a2Rc_get_second_articulation_gen(uint8_t arm) {
+ return &ac_state[arm].theta2;
+}
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.h b/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.h
new file mode 100644
index 0000000..4620a47
--- /dev/null
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.h
@@ -0,0 +1,70 @@
+/*
+ * arm2R_controller.h
+ * -----------------
+ * Copyright : (c) 2012, Xavier Lagorce <Xav...@cr...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ */
+
+#ifndef __ARM2R_CONTROLLER_H
+#define __ARM2R_CONTROLLER_H
+
+#include "motor_controller.h"
+#include "trajectory_controller.h"
+#include "command_generator.h"
+
+#define ARM1 0
+#define ARM2 1
+
+#ifndef A2R_ARM1_TC
+ #define A2R_ARM1_TC 0
+#endif
+#ifndef A2R_ARM2_TC
+ #define A2R_ARM2_TC 2
+#endif
+
+#define A2R_TC_X 0
+#define A2R_TC_Y 1
+
+/*
+ * Initializes data structures associated with the arm 2R controller
+ */
+void a2Rc_init(void);
+
+/*
+ * Initializes a particular arm controller and configure the associated
+ * generators
+ * - arm : ID of the arm controller to start
+ * - l1, l2 : length of the two segments of the arm
+ * - enc_theta1, enc_theta2 : IDs of the encoders measuring the position
+ * of the two articulations
+ */
+void a2Rc_start(uint8_t arm,
+ float l1, float l2,
+ uint8_t enc_theta1, uint8_t enc_theta2);
+
+/*
+ * Moves an arm2R controller along an axis.
+ * - arm : ID of the arm to move
+ * - position : position on the axis to go to
+ * - speed : movement speed along the axis
+ * - acc : movement acceleration along the axis
+ */
+void a2Rc_goto_position_x(uint8_t arm, float position, float speed, float acc);
+void a2Rc_goto_position_y(uint8_t arm, float position, float speed, float acc);
+
+/*
+ * Get the command generator outputing the reference position for
+ * the first articulation
+ * - arm : ID of the arm of which to get the generator
+ */
+command_generator_t* a2Rc_get_first_articulation_gen(uint8_t arm);
+/*
+ * Get the command generator outputing the reference position for
+ * the second articulation
+ * - arm : ID of the arm of which to get the generator
+ */
+command_generator_t* a2Rc_get_second_articulation_gen(uint8_t arm);
+
+#endif /* __ARM2R_CONTROLLER_H */
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/command_generator.c b/elec/boards/Controller_Motor_STM32/Firmwares/lib/command_generator.c
index 0d1145a..3b36e70 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/command_generator.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/command_generator.c
@@ -98,12 +98,45 @@ command_generator_t* new_hd_generator(command_generator_t *generator,
return generator;
}
+command_generator_t* new_a2r_generator(command_generator_t *generator,
+ command_generator_t *linear_pos_x,
+ command_generator_t *linear_speed_x,
+ command_generator_t *linear_pos_y,
+ command_generator_t *linear_speed_y,
+ float l1, float l2,
+ uint8_t enc_theta1, uint8_t enc_theta2,
+ uint8_t type) {
+ switch (type) {
+ case 1: // Back wheel
+ generator->type.t = GEN_AC_T1;
+ break;
+ case 2: // Right-front wheel
+ generator->type.t = GEN_AC_T2;
+ break;
+ default:
+ return NULL;
+ break;
+ }
+ generator->type.callback.type = GEN_CALLBACK_NONE;
+ generator->a2r.linear_pos_x = linear_pos_x;
+ generator->a2r.linear_speed_x = linear_speed_x;
+ generator->a2r.linear_pos_x = linear_pos_y;
+ generator->a2r.linear_speed_x = linear_speed_y;
+ generator->a2r.l1 = l1;
+ generator->a2r.l2 = l2;
+ generator->a2r.enc_theta1 = enc_theta1;
+ generator->a2r.enc_theta2 = enc_theta2;
+
+ return generator;
+}
+
command_generator_t* adjust_value(command_generator_t *generator, float value) {
uint8_t type = generator->type.t;
if (type != GEN_DD_RIGHT && type != GEN_DD_LEFT
- && type != GEN_HD_B && type != GEN_HD_RF && type != GEN_HD_LF) {
+ && type != GEN_HD_B && type != GEN_HD_RF && type != GEN_HD_LF
+ && type != GEN_AC_T1 && type != GEN_AC_T2) {
generator->type.last_output = value;
return generator;
} else {
@@ -160,7 +193,8 @@ command_generator_t* remove_callback(command_generator_t *generator) {
float get_output_value(command_generator_t *generator) {
int32_t cur_time;
- float speed, dt, u1, u2, u_x, u_y, w;
+ float speed, dt, u1, u2, u_x, u_y, w, theta1, theta2;
+ float l1, l2, s1, c1, s2, c2;
if (generator->type.state != GEN_STATE_RUNNING)
return generator->type.last_output;
@@ -235,6 +269,32 @@ float get_output_value(command_generator_t *generator) {
generator->type.last_output = MAX(generator->type.last_output, -generator->hd.max_speed);
}
break;
+ case GEN_AC_T1:
+ case GEN_AC_T2:
+ // Update position generators to allow callbacks
+ get_output_value(generator->a2r.linear_pos_x);
+ get_output_value(generator->a2r.linear_pos_y);
+ // Compute output
+ u_x = get_output_value(generator->a2r.linear_speed_x);
+ u_y = get_output_value(generator->a2r.linear_speed_y);
+ l1 = generator->a2r.l1;
+ l2 = generator->a2r.l2;
+ theta1 = getEncoderPosition_f(generator->a2r.enc_theta1);
+ theta2 = getEncoderPosition_f(generator->a2r.enc_theta2);
+ c1 = cos(theta1);
+ c2 = cos(theta2);
+ s1 = sin(theta1);
+ s2 = sin(theta2);
+ w = -l1*l2*s1*c2+l1*l2*s2*c1;
+ switch (generator->type.t) {
+ case GEN_AC_T1:
+ generator->type.last_output = (-l1*s1*u_x+l2*s2*u_y)/w;
+ break;
+ case GEN_AC_T2:
+ generator->type.last_output = (-l1*c1*u_x+l2*c2*u_y)/w;
+ break;
+ }
+ break;
}
switch (generator->type.callback.type) {
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/command_generator.h b/elec/boards/Controller_Motor_STM32/Firmwares/lib/command_generator.h
index 3fa2b6a..bd5ae8e 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/command_generator.h
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/command_generator.h
@@ -15,6 +15,7 @@
#include <drv/timer.h>
#include <math.h>
+#include "encoder.h"
// Generator types
#define GEN_NONE 0 // No type, the generator is not initialized.
@@ -26,9 +27,12 @@
// drive.
#define GEN_DD_RIGHT 5 // Outputs the right wheel speed for a differential
// drive.
-#define GEN_HD_B 6 // Outpus the back wheel speed for an holonomic drive.
-#define GEN_HD_RF 7 // Outpus the right-front wheel speed for an holonomic drive.
-#define GEN_HD_LF 8 // Outpus the left-front wheel speed for an holonomic drive.
+#define GEN_HD_B 6 // Outputs the back wheel speed for an holonomic drive.
+#define GEN_HD_RF 7 // Outputs the right-front wheel speed for an holonomic drive.
+#define GEN_HD_LF 8 // Outputs the left-front wheel speed for an holonomic drive.
+#define GEN_AC_T1 9 // Outputs the angle of the first articulation of a RR arm
+#define GEN_AC_T2 10 // Outputs the angle of the second articulation of a RR arm
+
// Generator states
#define GEN_STATE_PAUSE 0 // The output is freezed.
@@ -96,6 +100,18 @@ typedef struct {
float max_speed;
} hd_generator_t;
+typedef struct {
+ placeholder_generator_t gen;
+ command_generator_t *linear_pos_x;
+ command_generator_t *linear_speed_x;
+ command_generator_t *linear_pos_y;
+ command_generator_t *linear_speed_y;
+ float l1;
+ float l2;
+ uint8_t enc_theta1;
+ uint8_t enc_theta2;
+} a2r_generator_t;
+
// Usable generator meta-type
union _command_generator_t {
placeholder_generator_t type;
@@ -104,6 +120,7 @@ union _command_generator_t {
ramp2_generator_t ramp2;
dd_generator_t dd;
hd_generator_t hd;
+ a2r_generator_t a2r;
};
/* Initializes a new Constant Generator.
@@ -187,6 +204,35 @@ command_generator_t* new_hd_generator(command_generator_t *generator,
float wheel_radius, float struct_radius, float max_speed,
uint8_t type);
+/* Initializes a new 2R arm controller generator.
+ * - generator : pointer to the generator to initialize
+ * - linear_pos_x : pointer to the generator giving the integrates of linear_speed_x. This
+ * generator will be called at each computation to allow update in parallel
+ * with linear_speed.
+ * - linear_speed_x : pointer to the generator giving the linear speed along the x axis of
+ * the arm end.
+ * - linear_pos_y : pointer to the generator giving the integrates of linear_speed_y. This
+ * generator will be called at each computation to allow update in parallel
+ * with linear_speed.
+ * - linear_speed_y : pointer to the generator giving the linear speed along the y axis of
+ * the arm end.
+ * - l1 : length of the first segment of the arm.
+ * - l2 : length of the second segment of the arm.
+ * - enc_theta1 : ID of the encoder measuring the first articulation position
+ * - enc_theta2 : ID of the encoder measuring the second articulation position
+ * - type :
+ * o 1 is the first articulation
+ * o 2 is the second articulation
+ */
+command_generator_t* new_a2r_generator(command_generator_t *generator,
+ command_generator_t *linear_pos_x,
+ command_generator_t *linear_speed_x,
+ command_generator_t *linear_pos_y,
+ command_generator_t *linear_speed_y,
+ float l1, float l2,
+ uint8_t enc_theta1, uint8_t enc_theta2,
+ uint8_t type);
+
/*
* Adjusts the current output value of 'generator' to 'value'.
*/
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2012-01-28 16:00:55
|
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 356ea449946b76d194a653473fe99df01c6b6b1a (commit)
from 40a60d3a36a22d90795f4a64353fd34cedc4a79a (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 356ea449946b76d194a653473fe99df01c6b6b1a
Author: Xavier Lagorce <Xav...@cr...>
Date: Sat Jan 28 17:03:30 2012 +0100
[Controller_Motor_STM32/lib] Added helpers to rescale encoder output
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/encoder.c b/elec/boards/Controller_Motor_STM32/Firmwares/lib/encoder.c
index 6552dbb..10587f0 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/encoder.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/encoder.c
@@ -12,6 +12,12 @@
#include "stm32lib/stm32f10x_rcc.h"
#include "stm32lib/stm32f10x_tim.h"
+typedef struct {
+ uint16_t origin;
+ float scale;
+} enc_params_t;
+
+enc_params_t enc_params[4];
/*
* Function to initialise one encoder interface
@@ -101,6 +107,20 @@ void encodersInit(void) {
TIM_Cmd(TIM8,ENABLE);
TIM_Cmd(TIM1,ENABLE);
TIM_Cmd(TIM4,ENABLE);
+
+ // Initialize scaling factors
+ for (int i=0; i < 4; i++) {
+ enc_params[i].origin = 0;
+ enc_params[i].scale= 1.0;
+ }
+}
+
+/*
+ * Helper to set the sacling factor for the readout helper
+ */
+void setEncoderScaling(uint8_t encoder, uint16_t origin, float scale) {
+ enc_params[encoder].origin = origin;
+ enc_params[encoder].scale = scale;
}
/*
@@ -125,6 +145,14 @@ uint16_t getEncoderPosition(uint8_t encoder) {
return 0;
}
}
+/*
+ * Helper to get the current encoder position after the applying
+ * some scaling factors
+ */
+float getEncoderPosition_f(uint8_t encoder) {
+ return enc_params[encoder].scale *
+ (getEncoderPosition(encoder) - enc_params[encoder].origin);
+}
/*
* Helper to get the current direction of evolution of the counter
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/encoder.h b/elec/boards/Controller_Motor_STM32/Firmwares/lib/encoder.h
index e94ac88..729c10e 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/encoder.h
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/encoder.h
@@ -6,10 +6,10 @@
#ifndef HEADER__ENCODER
#define HEADER__ENCODER
-#define ENCODER1 1
-#define ENCODER2 2
-#define ENCODER3 3
-#define ENCODER4 4
+#define ENCODER1 0
+#define ENCODER2 1
+#define ENCODER3 2
+#define ENCODER4 3
#define ENCODER_DIR_UP 0
#define ENCODER_DIR_DOWN 1
@@ -17,9 +17,32 @@
#include <drv/gpio_stm32.h>
#include <drv/clock_stm32.h>
+/*
+ * Intializes the encoder interface
+ */
void encodersInit(void);
+/*
+ * Sets the scaling factors for readout helper
+ */
+void setEncoderScaling(uint8_t encoder, uint16_t origin, float scale);
+
+/*
+ * Returns the current position of a particular encoder
+ */
uint16_t getEncoderPosition(uint8_t encoder);
+/*
+ * Gets the corrent value of an encoder after scaling process
+ */
+float getEncoderPosition_f(uint8_t encoder);
+
+/*
+ * Resets the counter register associated to a given encoder
+ */
void resetEncoderPosition(uint8_t encoder);
+
+/*
+ * Gets the direction of the evolution of an encoder value
+ */
uint8_t getEncoderDirection(uint8_t encoder);
#endif
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2012-01-27 14:36:13
|
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 40a60d3a36a22d90795f4a64353fd34cedc4a79a (commit)
from 5403b5850fb9dd5aadbb3350764821280e44de0e (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 40a60d3a36a22d90795f4a64353fd34cedc4a79a
Author: Xavier Lagorce <Xav...@cr...>
Date: Fri Jan 27 15:38:17 2012 +0100
[Controller_Motor_STM32] Reorganized code into a common 'lib' directory and specific
code for Effectors board and Propulsion_Drive board
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/bezier_utils.c b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/bezier_utils.c
deleted file mode 100644
index 83fe983..0000000
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/bezier_utils.c
+++ /dev/null
@@ -1,134 +0,0 @@
-/*
- * bezier_utils.c
- * --------------
- * Copyright : (c) 2011, Xavier Lagorce <Xav...@cr...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- */
-
-#include "bezier_utils.h"
-
-#ifdef BEZIER_UTILS_USE_BERTOS
-#include <cpu/power.h>
-#endif
-
-float bezier_apply(float params[4], float u) {
- return (params[0] + params[1]*u + params[2]*u*u + params[3]*u*u*u);
-}
-
-void bezier_generate(float params[2][4],
- float p_x, float p_y,
- float p1_x, float p1_y,
- float p2_x, float p2_y,
- float s_x, float s_y) {
-
- params[0][0] = p_x;
- params[1][0] = p_y;
- params[0][1] = -3*p_x+3*p1_x;
- params[1][1] = -3*p_y+3*p1_y;
- params[0][2] = 3*p_x-6*p1_x+3*p2_x;
- params[1][2] = 3*p_y-6*p1_y+3*p2_y;
- params[0][3] = -p_x+3*p1_x-3*p2_x+s_x;
- params[1][3] = -p_y+3*p1_y-3*p2_y+s_y;
-}
-
-void bezier_velocity_profile(float dparams[2][4], float ddparams[2][4],
- float v_max, float at_max, float ar_max,
- float v_ini, float v_end,
- float du, float* v_tab) {
- float vmins[4], cr, cr_p, cr_pp, u, vm, dt, nv, dx, dy, dsu;
- int ind = 0, j, nb_pts, mins[4], im, i;
-
- for (i=0, u=0.0; u <= 1.0; i++, u+=du) {
- v_tab[i] = v_max;
- }
- nb_pts = i;
-
- // Looking for curvature radius minima to limit speed at this positions
- mins[ind] = 0;
- vmins[ind] = v_ini;
- ind++;
-
- cr_pp = fabsf(bezier_cr(0, dparams, ddparams));
- cr_p = fabsf(bezier_cr(du, dparams, ddparams));
- for (i=2, u=2*du; u <= 1.0; i++, u+=du) {
- cr = fabsf(bezier_cr(u, dparams, ddparams));
- if ((cr >= cr_p) && (cr_p < cr_pp)) {
- mins[ind] = i;
- vmins[ind] = sqrtf(ar_max*cr);
- ind++;
- }
- cr_pp = cr_p;
- cr_p = cr;
- }
- mins[ind] = nb_pts-1;
- vmins[ind] = v_end;
- ind++;
-
-#ifdef BEZIER_UTILS_USE_BERTOS
- cpu_relax();
-#endif
-
- // Compute speed limitations
- for (j=0; j < ind; j++) {
- im = mins[j];
- vm = vmins[j];
- if (vm < v_max) {
- v_tab[im] = vm;
-
- // Profile for preceding velocity
- for (i = im-1; i >= 0; i--) {
- dx = bezier_apply(dparams[0], du*(i+1));
- dy = bezier_apply(dparams[1], du*(i+1));
- dsu = sqrtf(dx*dx + dy*dy);
- dt = (-v_tab[i+1]+sqrtf(v_tab[i+1]*v_tab[i+1]+2*at_max*du*dsu))/at_max;
- nv = v_tab[i+1]+at_max*dt;
- if (nv < v_tab[i]) {
- v_tab[i] = nv;
- } else {
- break;
- }
- }
- // Profile for following sector
- for (i = im+1; i < nb_pts; i++) {
- dx = bezier_apply(dparams[0], du*(i-1));
- dy = bezier_apply(dparams[1], du*(i-1));
- dsu = sqrtf(dx*dx + dy*dy);
- dt = (-v_tab[i-1]+sqrtf(v_tab[i-1]*v_tab[i-1]+2*at_max*du*dsu))/at_max;
- nv = v_tab[i-1]+at_max*dt;
- if (nv < v_tab[i]) {
- v_tab[i] = nv;
- } else {
- break;
- }
- }
-
-#ifdef BEZIER_UTILS_USE_BERTOS
- cpu_relax();
-#endif
- } // end if (vm < v_max)
- } // for mins
-}
-
-float bezier_cr(float u, float dparams[2][4], float ddparams[2][4]) {
-
- float dx, dy, ddx, ddy;
-
- dx = bezier_apply(dparams[0], u);
- dy = bezier_apply(dparams[1], u);
- ddx = bezier_apply(ddparams[0], u);
- ddy = bezier_apply(ddparams[1], u);
-
- return powf(dx*dx+dy*dy, 1.5) / (dx*ddy - dy*ddx);
-}
-
-void bezier_diff(float params[2][4], float dparams[2][4]) {
- int i, j;
- for (i=0; i<=1; i++) {
- for (j=0; j<=2; j++) {
- dparams[i][j] = (j+1)*params[i][j+1];
- }
- dparams[i][3] = 0.;
- }
-}
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/bezier_utils.h b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/bezier_utils.h
deleted file mode 100644
index 2aa45fd..0000000
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/bezier_utils.h
+++ /dev/null
@@ -1,52 +0,0 @@
-/*
- * bezier_utils.h
- * --------------
- * Copyright : (c) 2011, Xavier Lagorce <Xav...@cr...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- */
-
-#ifndef __BEZIER_UTILS_H
-#define __BEZIER_UTILS_H
-
-#define BEZIER_UTILS_USE_BERTOS
-
-#include <math.h>
-#include <stdint.h>
-
-/*
- * Apply a polynomial form of a Bezier Spline to a value of the parameter
- */
-float bezier_apply(float params[4], float u);
-
-/*
- * Computes the parameters of the polynomial from of a Bezier Spline
- */
-void bezier_generate(float params[2][4],
- float p_x, float p_y,
- float p1_x, float p1_y,
- float p2_x, float p2_y,
- float s_x, float s_y);
-
-/*
- * Computes the velocity profile along a Bezier Spline and according to physical
- * contraints
- */
-void bezier_velocity_profile(float dparams[2][4], float ddparams[2][4],
- float v_max, float at_max, float ar_max,
- float v_ini, float v_end,
- float du, float* v_tab);
-
-/*
- * Computes the curvature radius of a Bezier Spline for a given value of the
- * parameter
- */
-float bezier_cr(float u, float dparams[2][4], float ddparams[2][4]);
-
-/*
- * Differentiates the polynomial form of a Bezier Spline
- */
-void bezier_diff(float params[2][4], float dparams[2][4]);
-
-#endif /* __BEZIER_UTILS_H */
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/command_generator.c b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/command_generator.c
deleted file mode 100644
index 0d1145a..0000000
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/command_generator.c
+++ /dev/null
@@ -1,254 +0,0 @@
-/*
- * command_generator.c
- * -------------------
- * Copyright : (c) 2011, Xavier Lagorce <Xav...@cr...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- */
-
-#include "command_generator.h"
-#include <stdlib.h>
-
-command_generator_t* new_constant_generator(command_generator_t *generator, float value) {
- generator->type.t = GEN_CONSTANT;
- generator->type.callback.type = GEN_CALLBACK_NONE;
- generator->constant.gen.last_output = value;
- generator->constant.gen.state = GEN_STATE_PAUSE;
-
- return generator;
-}
-
-command_generator_t* new_ramp_generator(command_generator_t *generator, float starting_value, float speed) {
- generator->type.t = GEN_RAMP;
- generator->type.callback.type = GEN_CALLBACK_NONE;
- generator->ramp.gen.state = GEN_STATE_PAUSE;
- generator->ramp.gen.last_output = starting_value;
- generator->ramp.last_time = 0;
- generator->ramp.speed = speed;
-
- return generator;
-}
-
-command_generator_t* new_ramp2_generator(command_generator_t *generator, float starting_value, command_generator_t *speed) {
- generator->type.t = GEN_RAMP2;
- generator->type.callback.type = GEN_CALLBACK_NONE;
- generator->ramp2.gen.state = GEN_STATE_PAUSE;
- generator->ramp2.gen.last_output = starting_value;
- generator->ramp2.last_time = 0;
- generator->ramp2.speed = speed;
-
- return generator;
-}
-
-command_generator_t* new_dd_generator(command_generator_t *generator,
- command_generator_t *linear_pos,
- command_generator_t *linear_speed,
- command_generator_t *rotational_pos,
- command_generator_t *rotational_speed,
- float wheel_radius, float shaft_width, float max_speed,
- uint8_t type) {
- generator->type.t = (type == 1) ? GEN_DD_RIGHT : GEN_DD_LEFT;
- generator->type.callback.type = GEN_CALLBACK_NONE;
- generator->dd.linear_pos = linear_pos;
- generator->dd.linear_speed = linear_speed;
- generator->dd.rotational_pos = rotational_pos;
- generator->dd.rotational_speed = rotational_speed;
- generator->dd.wheel_radius = wheel_radius;
- generator->dd.shaft_width = shaft_width;
- generator->dd.max_speed = max_speed;
-
- return generator;
-}
-
-command_generator_t* new_hd_generator(command_generator_t *generator,
- command_generator_t *linear_pos_x,
- command_generator_t *linear_speed_x,
- command_generator_t *linear_pos_y,
- command_generator_t *linear_speed_y,
- command_generator_t *rotational_pos,
- command_generator_t *rotational_speed,
- float wheel_radius, float struct_radius, float max_speed,
- uint8_t type) {
- switch (type) {
- case 1: // Back wheel
- generator->type.t = GEN_HD_B;
- break;
- case 2: // Right-front wheel
- generator->type.t = GEN_HD_RF;
- break;
- case 3: // Left-front wheel
- generator->type.t = GEN_HD_LF;
- break;
- default:
- return NULL;
- break;
- }
- generator->type.callback.type = GEN_CALLBACK_NONE;
- generator->hd.linear_pos_x = linear_pos_x;
- generator->hd.linear_speed_x = linear_speed_x;
- generator->hd.linear_pos_x = linear_pos_y;
- generator->hd.linear_speed_x = linear_speed_y;
- generator->hd.rotational_pos = rotational_pos;
- generator->hd.rotational_speed = rotational_speed;
- generator->hd.wheel_radius = wheel_radius;
- generator->hd.struct_radius = struct_radius;
- generator->hd.max_speed = max_speed;
-
- return generator;
-}
-
-
-command_generator_t* adjust_value(command_generator_t *generator, float value) {
- uint8_t type = generator->type.t;
-
- if (type != GEN_DD_RIGHT && type != GEN_DD_LEFT
- && type != GEN_HD_B && type != GEN_HD_RF && type != GEN_HD_LF) {
- generator->type.last_output = value;
- return generator;
- } else {
- return NULL;
- }
-}
-
-command_generator_t* adjust_speed(command_generator_t *generator, float speed) {
- switch (generator->type.t) {
- case GEN_RAMP:
- generator->ramp.speed = speed;
- return generator;
- default:
- return NULL;
- }
-}
-
-command_generator_t* start_generator(command_generator_t *generator) {
- switch (generator->type.t) {
- case GEN_RAMP:
- generator->ramp.last_time = ticks_to_us(timer_clock());
- break;
- case GEN_RAMP2:
- generator->ramp2.last_time = ticks_to_us(timer_clock());
- }
- generator->type.state = GEN_STATE_RUNNING;
-
- return generator;
-}
-
-command_generator_t* pause_generator(command_generator_t *generator) {
- // update generator->type.last_output
- get_output_value(generator);
- // pause the generator
- generator->type.state = GEN_STATE_PAUSE;
-
- return generator;
-}
-
-command_generator_t* add_callback(command_generator_t *generator, uint8_t type, float threshold, void (*callback_function)(command_generator_t*)) {
- generator->type.callback.callback_function = callback_function;
- generator->type.callback.threshold = threshold;
- generator->type.callback.type = type;
-
- return generator;
-}
-
-command_generator_t* remove_callback(command_generator_t *generator) {
- generator->type.callback.type = GEN_CALLBACK_NONE;
-
- return generator;
-}
-
-
-float get_output_value(command_generator_t *generator) {
- int32_t cur_time;
- float speed, dt, u1, u2, u_x, u_y, w;
-
- if (generator->type.state != GEN_STATE_RUNNING)
- return generator->type.last_output;
-
- switch (generator->type.t) {
- case GEN_CONSTANT:
- // Constant generator, no update needed
- break;
- case GEN_RAMP:
- cur_time = ticks_to_us(timer_clock());
- dt = (cur_time - generator->ramp.last_time)*1e-6;
- generator->type.last_output += dt*generator->ramp.speed;
- generator->ramp.last_time = cur_time;
- break;
- case GEN_RAMP2:
- cur_time = ticks_to_us(timer_clock());
- speed = get_output_value(generator->ramp2.speed);
- dt = (cur_time - generator->ramp2.last_time)*1e-6;
- generator->type.last_output += dt*speed;
- generator->ramp2.last_time = cur_time;
- break;
- case GEN_DD_RIGHT:
- case GEN_DD_LEFT:
- // Update position generators to allow callbacks
- get_output_value(generator->dd.linear_pos);
- get_output_value(generator->dd.rotational_pos);
- // Compute output
- u1 = get_output_value(generator->dd.linear_speed);
- u2 = get_output_value(generator->dd.rotational_speed);
- switch (generator->type.t) {
- case GEN_DD_RIGHT:
- generator->type.last_output = (2.0*u1+u2*generator->dd.shaft_width) / (2.0 * generator->dd.wheel_radius);
- break;
- case GEN_DD_LEFT:
- generator->type.last_output = (2.0*u1-u2*generator->dd.shaft_width) / (2.0 * generator->dd.wheel_radius);
- break;
- }
- if (generator->type.last_output >= 0) {
- generator->type.last_output = MIN(generator->type.last_output, generator->dd.max_speed);
- } else {
- generator->type.last_output = MAX(generator->type.last_output, -generator->dd.max_speed);
- }
- break;
- case GEN_HD_B:
- case GEN_HD_RF:
- case GEN_HD_LF:
- // Update position generators to allow callbacks
- get_output_value(generator->hd.linear_pos_x);
- get_output_value(generator->hd.linear_pos_y);
- get_output_value(generator->hd.rotational_pos);
- // Compute output
- u_x = get_output_value(generator->hd.linear_speed_x);
- u_y = get_output_value(generator->hd.linear_speed_y);
- w = get_output_value(generator->hd.rotational_speed);
- switch (generator->type.t) {
- case GEN_HD_B:
- generator->type.last_output =
- (u_x + w*generator->hd.struct_radius) / generator->hd.wheel_radius;
- break;
- case GEN_HD_RF:
- generator->type.last_output =
- (-u_x/2.0 + u_y*sqrt(3.0)/2.0 + w*generator->hd.struct_radius) / generator->hd.wheel_radius;
- break;
- case GEN_HD_LF:
- generator->type.last_output =
- (-u_x/2.0 - u_y*sqrt(3.0)/2.0 + w*generator->hd.struct_radius) / generator->hd.wheel_radius;
- break;
- }
- if (generator->type.last_output >= 0) {
- generator->type.last_output = MIN(generator->type.last_output, generator->hd.max_speed);
- } else {
- generator->type.last_output = MAX(generator->type.last_output, -generator->hd.max_speed);
- }
- break;
- }
-
- switch (generator->type.callback.type) {
- case GEN_CALLBACK_NONE :
- break;
- case GEN_CALLBACK_SUP :
- if (generator->type.last_output > generator->type.callback.threshold)
- generator->type.callback.callback_function(generator);
- break;
- case GEN_CALLBACK_INF :
- if (generator->type.last_output < generator->type.callback.threshold)
- generator->type.callback.callback_function(generator);
- break;
- }
-
- return generator->type.last_output;
-}
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/command_generator.h b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/command_generator.h
deleted file mode 100644
index 3fa2b6a..0000000
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/command_generator.h
+++ /dev/null
@@ -1,224 +0,0 @@
-/*
- * command_generator.h
- * -------------------
- * Copyright : (c) 2011, Xavier Lagorce <Xav...@cr...>
- * Licence : BSD3
- *
- * This module aims at providing a simple way to generate some command signal
- * for control processes (like steps, ramps, ...)
- *
- * This file is a part of [kro]bot.
- */
-
-#ifndef __COMMAND_GENERATOR_H
-#define __COMMAND_GENERATOR_H
-
-#include <drv/timer.h>
-#include <math.h>
-
-// Generator types
-#define GEN_NONE 0 // No type, the generator is not initialized.
-#define GEN_CONSTANT 1 // Outputs a constant.
-#define GEN_RAMP 2 // Outputs a ramp with the given slope.
-#define GEN_RAMP2 3 // Same as GEN_RAMP, but the slope is given by another
- // generator.
-#define GEN_DD_LEFT 4 // Outputs the left wheel speed for a differential
- // drive.
-#define GEN_DD_RIGHT 5 // Outputs the right wheel speed for a differential
- // drive.
-#define GEN_HD_B 6 // Outpus the back wheel speed for an holonomic drive.
-#define GEN_HD_RF 7 // Outpus the right-front wheel speed for an holonomic drive.
-#define GEN_HD_LF 8 // Outpus the left-front wheel speed for an holonomic drive.
-
-// Generator states
-#define GEN_STATE_PAUSE 0 // The output is freezed.
-#define GEN_STATE_RUNNING 1 // The generator is running.
-
-// Threshold comparison for callback trigger
-#define GEN_CALLBACK_NONE 0 // There is no callback.
-#define GEN_CALLBACK_SUP 1 // The callback is triggered if output > threshold.
-#define GEN_CALLBACK_INF 2 // The callback is triggered if output < threshold.
-
-typedef union _command_generator_t command_generator_t;
-
-// Callback description
-typedef struct {
- uint8_t type;
- float threshold;
- void (*callback_function)(command_generator_t*);
-} generator_callback_t;
-
-// Generator descriptions
-typedef struct {
- uint8_t t;
- float last_output;
- uint8_t state;
- generator_callback_t callback;
-} placeholder_generator_t;
-
-typedef struct {
- placeholder_generator_t gen;
-} constant_generator_t;
-
-typedef struct {
- placeholder_generator_t gen;
- int32_t last_time;
- float speed;
-} ramp_generator_t;
-
-typedef struct {
- placeholder_generator_t gen;
- int32_t last_time;
- command_generator_t *speed;
-} ramp2_generator_t;
-
-typedef struct {
- placeholder_generator_t gen;
- command_generator_t *linear_pos;
- command_generator_t *linear_speed;
- command_generator_t *rotational_pos;
- command_generator_t *rotational_speed;
- float wheel_radius;
- float shaft_width;
- float max_speed;
-} dd_generator_t;
-
-typedef struct {
- placeholder_generator_t gen;
- command_generator_t *linear_pos_x;
- command_generator_t *linear_speed_x;
- command_generator_t *linear_pos_y;
- command_generator_t *linear_speed_y;
- command_generator_t *rotational_pos;
- command_generator_t *rotational_speed;
- float wheel_radius;
- float struct_radius;
- float max_speed;
-} hd_generator_t;
-
-// Usable generator meta-type
-union _command_generator_t {
- placeholder_generator_t type;
- constant_generator_t constant;
- ramp_generator_t ramp;
- ramp2_generator_t ramp2;
- dd_generator_t dd;
- hd_generator_t hd;
-};
-
-/* Initializes a new Constant Generator.
- * - generator : pointer to the generator to initialize
- * - value : output value of the generator
- */
-command_generator_t* new_constant_generator(command_generator_t *generator,
- float value);
-
-/* Initializes a new Ramp Generator.
- * - generator : pointer to the generator to initialize
- * - starting_value : initial output value
- * - speed : slope
- */
-command_generator_t* new_ramp_generator(command_generator_t *generator,
- float starting_value, float speed);
-
-/* Initializes a new Ramp2 Generator.
- * - generator : pointer to the generator to initialize
- * - starting_value : initial output value
- * - speed : pointer to the generator which output is used as this generator's slope
- */
-command_generator_t* new_ramp2_generator(command_generator_t *generator,
- float starting_value,
- command_generator_t *speed);
-
-/* Initializes a new Differential Drive generator.
- * - generator : pointer to the generator to initialize
- * - linear_pos : pointer to the generator giving the integrates of linear_speed. This
- * generator will be called at each computation to allow update in parallel
- * with linear_speed.
- * - linear_speed : pointer to the generator giving the linear speed of the drive
- * - rotational_pos : pointer to the generator giving the integrates of rotational_speed. This
- * generator will be called at each computation to allow update in parallel
- * with rotational_speed.
- * - rotational_speed : pointer to the generator giving the rotational speed of the drive
- * - wheel_radius : radius of the wheels
- * - shaft_width : width of the propulsion shaft
- * - max_speed : maximum wheel speed (in rad/s)
- * - type : 1 for the right_wheel, -1 for the left_wheel
- */
-command_generator_t* new_dd_generator(command_generator_t *generator,
- command_generator_t *linear_pos,
- command_generator_t *linear_speed,
- command_generator_t *rotational_pos,
- command_generator_t *rotational_speed,
- float wheel_radius, float shaft_width, float max_speed,
- uint8_t type);
-
-/* Initializes a new Holonomic Drive generator.
- * - generator : pointer to the generator to initialize
- * - linear_pos_x : pointer to the generator giving the integrates of linear_speed_x. This
- * generator will be called at each computation to allow update in parallel
- * with linear_speed.
- * - linear_speed_x : pointer to the generator giving the linear speed along the x axis of
- * the drive.
- * - linear_pos_y : pointer to the generator giving the integrates of linear_speed_y. This
- * generator will be called at each computation to allow update in parallel
- * with linear_speed.
- * - linear_speed_y : pointer to the generator giving the linear speed along the y axis of
- * the drive.
- * - rotational_pos : pointer to the generator giving the integrates of rotational_speed. This
- * generator will be called at each computation to allow update in parallel
- * with rotational_speed.
- * - rotational_speed : pointer to the generator giving the rotational speed of the drive.
- * - wheel_radius : radius of the wheels.
- * - struct_radius : radius of the holonomic drive.
- * - max_speed : maximum wheel speed (in rad/s).
- * - type :
- * o 1 is the back wheel
- * o 2 is the right-front wheel
- * o 3 is the left-front wheel
- */
-command_generator_t* new_hd_generator(command_generator_t *generator,
- command_generator_t *linear_pos_x,
- command_generator_t *linear_speed_x,
- command_generator_t *linear_pos_y,
- command_generator_t *linear_speed_y,
- command_generator_t *rotational_pos,
- command_generator_t *rotational_speed,
- float wheel_radius, float struct_radius, float max_speed,
- uint8_t type);
-
-/*
- * Adjusts the current output value of 'generator' to 'value'.
- */
-command_generator_t* adjust_value(command_generator_t *generator, float value);
-
-/*
- * Adjusts the current slope of the ramp generator 'generator' to 'speed'.
- */
-command_generator_t* adjust_speed(command_generator_t *generator, float speed);
-
-/*
- * Starts or pauses 'generator'.
- */
-command_generator_t* start_generator(command_generator_t *generator);
-command_generator_t* pause_generator(command_generator_t *generator);
-
-/* Adds a callback to a generator
- * - generator : pointer to the generator to add a callback to
- * - type : threshold comparison type
- * - threshold : comparison threshold
- * - callback_function : callback to be called when the event is triggered
- */
-command_generator_t* add_callback(command_generator_t *generator, uint8_t type, float threshold, void (*callback_function)(command_generator_t*));
-
-/*
- * Removes the callback from 'generator'
- */
-command_generator_t* remove_callback(command_generator_t *generator);
-
-/*
- * Gets (and computes) the current output value of 'generator'
- */
-float get_output_value(command_generator_t *generator);
-
-#endif /* __COMMAND_GENERATOR_H */
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/controller_motor_stm32_user.mk b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/controller_motor_stm32_user.mk
index 56ea826..523bdbb 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/controller_motor_stm32_user.mk
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/controller_motor_stm32_user.mk
@@ -7,20 +7,24 @@
controller_motor_stm32_PROGRAMMER_TYPE = none
controller_motor_stm32_PROGRAMMER_PORT = none
+# Support board library
+controller_motor_stm32_LIB_PATH = $(controller_motor_stm32_SRC_PATH)/../../lib
+CFLAGS += -I$(controller_motor_stm32_LIB_PATH)
+
# Files included by the user.
controller_motor_stm32_USER_CSRC = \
- $(controller_motor_stm32_SRC_PATH)/stm32lib/stm32f10x_rcc.c \
- $(controller_motor_stm32_SRC_PATH)/stm32lib/stm32f10x_tim.c \
- $(controller_motor_stm32_SRC_PATH)/motor.c \
- $(controller_motor_stm32_SRC_PATH)/encoder.c \
- $(controller_motor_stm32_SRC_PATH)/odometry.c \
- $(controller_motor_stm32_SRC_PATH)/motor_controller.c \
- $(controller_motor_stm32_SRC_PATH)/command_generator.c \
- $(controller_motor_stm32_SRC_PATH)/trajectory_controller.c \
- $(controller_motor_stm32_SRC_PATH)/bezier_utils.c \
- $(controller_motor_stm32_SRC_PATH)/differential_drive.c \
+ $(controller_motor_stm32_LIB_PATH)/stm32lib/stm32f10x_rcc.c \
+ $(controller_motor_stm32_LIB_PATH)/stm32lib/stm32f10x_tim.c \
+ $(controller_motor_stm32_LIB_PATH)/motor.c \
+ $(controller_motor_stm32_LIB_PATH)/encoder.c \
+ $(controller_motor_stm32_LIB_PATH)/odometry.c \
+ $(controller_motor_stm32_LIB_PATH)/motor_controller.c \
+ $(controller_motor_stm32_LIB_PATH)/command_generator.c \
+ $(controller_motor_stm32_LIB_PATH)/trajectory_controller.c \
+ $(controller_motor_stm32_LIB_PATH)/bezier_utils.c \
+ $(controller_motor_stm32_LIB_PATH)/differential_drive.c \
+ $(controller_motor_stm32_LIB_PATH)/lift_controller.c \
$(controller_motor_stm32_SRC_PATH)/can_monitor.c \
- $(controller_motor_stm32_SRC_PATH)/lift_controller.c \
$(controller_motor_stm32_SRC_PATH)/main.c \
#
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/differential_drive.c b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/differential_drive.c
deleted file mode 100644
index b566a84..0000000
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/differential_drive.c
+++ /dev/null
@@ -1,373 +0,0 @@
-/*
- * differential_drive.c
- * --------------------
- * Copyright : (c) 2011, Xavier Lagorce <Xav...@cr...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- */
-
-#include "differential_drive.h"
-
-#ifndef MIN
-#define MIN(a,b) ((a) < (b) ? (a) : (b))
-#endif
-
-PROC_DEFINE_STACK(stack_traj_following, KERN_MINSTACKSIZE * 32);
-
-typedef struct {
- uint8_t initialized, enabled;
- float params[2][4], dparams[2][4], ddparams[2][4];
- float du, v_tab[101];
- float goal[2], v_end, theta_end, v_lin;
- float start[2], theta_ini;
-} dd_bezier_traj_t;
-
-typedef struct {
- uint8_t initialized, enabled, running, working;
- float wheel_radius, shaft_width;
- float last_lin_acceleration, last_rot_acceleration;
- float u, v_max, at_max, ar_max;
- command_generator_t left_wheel_speed, right_wheel_speed;
- command_generator_t left_wheel, right_wheel;
- uint8_t current_traj;
- dd_bezier_traj_t trajs[2];
- float Ts, k1, k2, k3;
- robot_state_t ghost_state;
-} dd_params_t;
-
-static dd_params_t params;
-
-static void NORETURN traj_following_process(void);
-
-static void NORETURN traj_following_process(void) {
- Timer timer;
- uint8_t next_traj, ui;
- robot_state_t rs;
- float cr, v_ratio, v_max, v_rot, dxu, dyu;
- float z1, z2, z3, w1, w2, u1, u2, dt;
- dd_bezier_traj_t *traj;
- int32_t last_time, cur_time;
-
- // configure timer
- timer_setDelay(&timer, ms_to_ticks(params.Ts * 1000));
- timer_setEvent(&timer);
-
- // Indicate we are running
- params.running = 1;
-
- // Init
- params.working = 0;
- next_traj = (params.current_traj + 1) % 2;
-
- while (1) {
- if (params.enabled == 0) {
- params.running = 0;
- proc_exit();
- } else {
- if (!params.working && params.trajs[next_traj].initialized) {
- params.working = 1;
- params.u = 0;
- ui = 0;
- params.current_traj = next_traj;
- traj = ¶ms.trajs[params.current_traj];
- next_traj = (params.current_traj + 1) % 2;
- params.ghost_state.x = traj->start[0];
- params.ghost_state.y = traj->start[1];
- params.ghost_state.theta = traj->theta_ini;
- traj->enabled = 1;
- last_time = ticks_to_us(timer_clock());
- }
- timer_add(&timer);
- if (params.working) {
- odo_getState(&rs);
-
- // Stop following the trajectory if we are close enough to our goal
- if (params.u >= 1.0 || ((rs.x-traj->goal[0]) * (rs.x-traj->goal[0]) + (rs.y-traj->goal[1]) * (rs.y-traj->goal[1])) <= (0.01*0.01)) {
- params.working = 0;
- traj->initialized = 0;
- traj->enabled = 0;
- if (!params.trajs[next_traj].initialized) {
- // We have no other trajectory to follow, let's brake
- dd_set_rotational_speed(0., params.at_max);
- dd_set_linear_speed(0., params.at_max);
- }
- } else {
- // We are following a trajectory, let's do it
- // Compute ghost vehicule parameters
- cr = bezier_cr(params.u, traj->dparams, traj->ddparams);
- for (; ui*traj->du <= params.u; ui++);
- if (ui*traj->du > 1.0)
- ui--;
- if (ui > 0) {
- traj->v_lin = traj->v_tab[ui-1] +
- (traj->v_tab[ui]-traj->v_tab[ui-1]) * (params.u - traj->du*(ui-1))/traj->du;
- } else {
- traj->v_lin = traj->v_tab[0];
- }
- if (isnan(cr) || isinf(cr)) {
- v_ratio = 1.0;
- } else {
- v_ratio = (cr + params.shaft_width/2.0) / (cr - params.shaft_width/2.0);
- }
- v_max = 2/(1+MIN(fabsf(v_ratio), fabsf(1.0/v_ratio)))*traj->v_lin;
- if (cr >= 0) {
- v_rot = v_max * (1 - 1/v_ratio) / params.shaft_width;
- } else {
- v_rot = v_max * (v_ratio - 1) / params.shaft_width;
- }
-
- // Evolution of the ghost vehicule state
- cur_time = ticks_to_us(timer_clock());
- dt = (cur_time - last_time) * 1e-6;
- dxu = bezier_apply(traj->dparams[0], params.u);
- dyu = bezier_apply(traj->dparams[1], params.u);
- params.u += traj->v_lin/sqrtf(dxu*dxu+dyu*dyu)*dt;
- params.ghost_state.x += traj->v_lin*cosf(params.ghost_state.theta)*dt;
- params.ghost_state.y += traj->v_lin*sinf(params.ghost_state.theta)*dt;
- params.ghost_state.theta = fmodf(params.ghost_state.theta + v_rot*dt, 2*M_PI);
- if (params.ghost_state.theta > M_PI) {
- params.ghost_state.theta -= 2*M_PI;
- } else if (params.ghost_state.theta <= M_PI) {
- params.ghost_state.theta += 2*M_PI;
- }
-
- // Compute command
- z1=(rs.x-params.ghost_state.x)*cosf(params.ghost_state.theta)+(rs.y-params.ghost_state.y)*sinf(params.ghost_state.theta);
- z2=-(rs.x-params.ghost_state.x)*sinf(params.ghost_state.theta)+(rs.y-params.ghost_state.y)*cosf(params.ghost_state.theta);
- z3=tanf(rs.theta-params.ghost_state.theta);
-
- w1=-params.k1*fabsf(traj->v_lin)*(z1+z2*z3);
- w2=-params.k2*traj->v_lin*z2-params.k3*fabsf(traj->v_lin)*z3;
-
- u1=(w1+traj->v_lin)/cosf(rs.theta-params.ghost_state.theta);
- u2=w2*powf(cosf(rs.theta-params.ghost_state.theta),2)+v_rot;
-
- // Apply command
- dd_set_linear_speed(u1, 0.);
- dd_set_rotational_speed(u2, 0.);
-
- // Keep current time
- last_time = cur_time;
- }
- }
- }
- timer_waitEvent(&timer); // Wait for the remaining of the sample period
- }
-}
-
-void dd_start(float wheel_radius, float shaft_width, float max_wheel_speed,
- float v_max, float at_max, float ar_max,
- float k1, float k2, float k3, float Ts) {
- params.wheel_radius = wheel_radius;
- params.shaft_width = shaft_width;
- params.last_lin_acceleration = 0.0;
- params.last_rot_acceleration = 0.0;
-
- params.ghost_state.x = 0;
- params.ghost_state.y = 0;
- params.ghost_state.theta = 0;
-
- params.running = 0;
- params.working = 0;
- params.current_traj = 0;
- params.trajs[0].initialized = 0;
- params.trajs[1].initialized = 0;
- params.trajs[0].enabled = 0;
- params.trajs[1].enabled = 0;
- params.v_max = v_max;
- params.at_max = at_max;
- params.ar_max = ar_max;
-
- params.k1 = k1;
- params.k2 = k2;
- params.k3 = k3;
- params.Ts = Ts;
-
- tc_new_controller(DD_LINEAR_SPEED_TC);
- tc_new_controller(DD_ROTATIONAL_SPEED_TC);
- new_dd_generator(¶ms.left_wheel_speed,
- tc_get_position_generator(DD_LINEAR_SPEED_TC),
- tc_get_speed_generator(DD_LINEAR_SPEED_TC),
- tc_get_position_generator(DD_ROTATIONAL_SPEED_TC),
- tc_get_speed_generator(DD_ROTATIONAL_SPEED_TC),
- wheel_radius, shaft_width, max_wheel_speed,
- -1);
- new_dd_generator(¶ms.right_wheel_speed,
- tc_get_position_generator(DD_LINEAR_SPEED_TC),
- tc_get_speed_generator(DD_LINEAR_SPEED_TC),
- tc_get_position_generator(DD_ROTATIONAL_SPEED_TC),
- tc_get_speed_generator(DD_ROTATIONAL_SPEED_TC),
- wheel_radius, shaft_width, max_wheel_speed,
- 1);
- new_ramp2_generator(¶ms.left_wheel, 0.0, ¶ms.left_wheel_speed);
- new_ramp2_generator(¶ms.right_wheel, 0.0, ¶ms.right_wheel_speed);
-
- start_generator(¶ms.left_wheel_speed);
- start_generator(¶ms.right_wheel_speed);
- start_generator(¶ms.left_wheel);
- start_generator(¶ms.right_wheel);
-
- params.initialized = 1;
- params.enabled = 1;
-
- proc_new(traj_following_process, NULL, sizeof(stack_traj_following), stack_traj_following);
-}
-
-void dd_pause(void) {
- if (params.initialized) {
- dd_set_linear_speed(0.0, params.last_lin_acceleration);
- dd_set_rotational_speed(0.0, params.last_rot_acceleration);
- params.enabled = 0;
- }
-}
-
-void dd_resume(void) {
- if (params.initialized && params.enabled) {
- params.enabled = 1;
- }
-}
-
-void dd_stop(void) {
- if (params.initialized) {
- pause_generator(¶ms.left_wheel);
- pause_generator(¶ms.right_wheel);
- pause_generator(¶ms.left_wheel_speed);
- pause_generator(¶ms.right_wheel_speed);
- tc_delete_controller(DD_LINEAR_SPEED_TC);
- tc_delete_controller(DD_ROTATIONAL_SPEED_TC);
- params.enabled = 0;
- params.initialized = 0;
- }
-}
-
-command_generator_t* dd_get_left_wheel_generator(void) {
- return ¶ms.left_wheel;
-}
-
-command_generator_t* dd_get_right_wheel_generator(void) {
- return ¶ms.right_wheel;
-}
-
-void dd_move(float distance, float speed, float acceleration) {
- if (params.enabled) {
- params.last_lin_acceleration = acceleration;
- tc_move(DD_LINEAR_SPEED_TC, distance, speed, params.last_lin_acceleration);
- }
-}
-
-void dd_turn(float angle, float speed, float acceleration) {
- if (params.enabled) {
- params.last_rot_acceleration = acceleration;
- tc_move(DD_ROTATIONAL_SPEED_TC, angle, speed, params.last_rot_acceleration);
- }
-}
-
-void dd_set_linear_speed(float speed, float acceleration) {
- if (params.enabled) {
- if (acceleration != 0.) {
- params.last_lin_acceleration = acceleration;
- tc_goto_speed(DD_LINEAR_SPEED_TC, speed, params.last_lin_acceleration);
- } else {
- tc_set_speed(DD_LINEAR_SPEED_TC, speed);
- }
- }
-}
-
-void dd_set_rotational_speed(float speed, float acceleration) {
- if (params.enabled) {
- if (acceleration != 0.) {
- params.last_rot_acceleration = acceleration;
- tc_goto_speed(DD_ROTATIONAL_SPEED_TC, speed, params.last_rot_acceleration);
- } else {
- tc_set_speed(DD_ROTATIONAL_SPEED_TC, speed);
- }
- }
-}
-
-uint8_t dd_add_bezier(float x_end, float y_end, float d1, float d2, float end_angle, float end_speed) {
- uint8_t t_ind;
- robot_state_t rs;
- dd_bezier_traj_t *traj;
- float v_ini, x_ini, y_ini, theta_ini;
-
- t_ind = (params.current_traj + 1) % 2;
-
- if (params.trajs[t_ind].initialized == 1) {
- return DD_TRAJECTORY_ALREADY_USED;
- } else {
- traj = ¶ms.trajs[t_ind];
- // New trajectory is not enabled
- traj->enabled = 0;
-
- // Get starting parameters
- if (params.trajs[params.current_traj].enabled) {
- v_ini = params.trajs[params.current_traj].v_end;
- x_ini = params.trajs[params.current_traj].goal[0];
- y_ini = params.trajs[params.current_traj].goal[1];
- theta_ini = params.trajs[params.current_traj].theta_end;
- } else {
- odo_getState(&rs);
- x_ini = rs.x;
- y_ini = rs.y;
- theta_ini = rs.theta;
- v_ini = 0.01; // non null low velocity to allow the system to start
- }
-
- // Compute Bezier Spline parameters
- bezier_generate(traj->params,
- x_ini, y_ini,
- x_ini + d1*cosf(theta_ini), y_ini + d1*sinf(theta_ini),
- x_end - d2*cosf(end_angle), y_end - d2*sinf(end_angle),
- x_end, y_end);
- traj->goal[0] = x_end;
- traj->goal[1] = y_end;
- traj->v_end = end_speed;
- traj->theta_end = end_angle;
- traj->start[0] = x_ini;
- traj->start[1] = y_ini;
- traj->theta_ini = (d1 >= 0.0) ? theta_ini : (theta_ini + M_PI);
- // Differentiate parameters
- bezier_diff(traj->params, traj->dparams);
- bezier_diff(traj->dparams, traj->ddparams);
- // Compute velocity profile
- bezier_velocity_profile(traj->dparams, traj->ddparams,
- params.v_max, params.at_max, params.ar_max,
- v_ini, end_speed,
- 0.01, traj->v_tab);
- traj->du = 0.01;
-
- params.trajs[t_ind].initialized = 1;
- return DD_NO_ERROR;
- }
-}
-
-void dd_interrupt_trajectory(float rot_acc, float lin_acc) {
-
- // Prevent the controller from following the current trajectory
- params.working = 0;
-
- // Disable all the trajectories
- params.trajs[0].initialized = 0;
- params.trajs[0].enabled = 0;
- params.trajs[1].initialized = 0;
- params.trajs[1].enabled = 0;
- // Brake !
- dd_set_rotational_speed(0., rot_acc);
- dd_set_linear_speed(0., lin_acc);
-}
-
-uint8_t dd_get_ghost_state(robot_state_t *state, float *u) {
- if (state != NULL) {
- state->x = params.ghost_state.x;
- state->y = params.ghost_state.y;
- state->theta = params.ghost_state.theta;
- }
- if (u != NULL)
- *u = params.u;
-
- if (params.working == 1)
- return DD_GHOST_MOVING;
- else
- return DD_GHOST_STOPPED;
-}
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/differential_drive.h b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/differential_drive.h
deleted file mode 100644
index 0a0a0f9..0000000
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/differential_drive.h
+++ /dev/null
@@ -1,120 +0,0 @@
-/*
- * differential_drive.h
- * --------------------
- * Copyright : (c) 2011, Xavier Lagorce <Xav...@cr...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- */
-
-#ifndef __DIFFERENTIAL_DRIVE_H
-#define __DIFFERENTIAL_DRIVE_H
-
-#include <math.h>
-#include <drv/timer.h>
-#include "trajectory_controller.h"
-#include "command_generator.h"
-#include "odometry.h"
-#include "bezier_utils.h"
-
-#ifndef DD_LINEAR_SPEED_TC
- #define DD_LINEAR_SPEED_TC 0
-#endif
-#ifndef DD_ROTATIONAL_SPEED_TC
- #define DD_ROTATIONAL_SPEED_TC 1
-#endif
-
-#define DD_NO_ERROR 0
-#define DD_TRAJECTORY_ALREADY_USED 1
-
-#define DD_GHOST_STOPPED 0
-#define DD_GHOST_MOVING 1
-
-/* Initializes the differential drive
- * - wheel_radius : radius of the wheels (in meters)
- * - shaft_width : propulsion shaft width (in meters)
- * - max_wheel_speed : maximum wheel speed (in rad/s)
- * - v_max : maximum linear speed (in m/s)
- * - at_max : maximum tangential acceleration (in m/s/s)
- * - ar_max : maximum radial acceleration (in m/s/s)
- * - k1, k2, k3 : control loop gain for trajectory following
- * - Ts : sample time for control loop in seconds
- *
- * Note : the differential drive system will use Trajectory controllers
- * DD_LINEAR_SPEED_TC and DD_ROTATIONAL_SPEED_TC
- */
-void dd_start(float wheel_radius, float shaft_width, float max_wheel_speed,
- float v_max, float at_max, float ar_max,
- float k1, float k2, float k3, float Ts);
-
-/* Pauses or Resumes the differential drive system.
- * In pause mode, the drive will accept no further command and actions will be
- * stopped.
- * If the robot is moving, wheels will be slowed down with the last used
- * acceleration (note that this won't necessarily give a straight line) when
- * pausing.
- */
-void dd_pause(void);
-void dd_resume(void);
-
-/*
- * Stops the differential drive system.
- */
-void dd_stop(void);
-
-/* Gets the generator correspondig to the wheels positions
- * to use them in motor_control for instance
- */
-command_generator_t* dd_get_left_wheel_generator(void);
-command_generator_t* dd_get_right_wheel_generator(void);
-
-/* Moves along a line
- * - distance : distance to move of (in meters), can be positive (forward)
- * or negative (backward).
- * - speed : moving speed (in meters per second) should be positive.
- * - acceleration : in meters per second per second, should be positive.
- */
-void dd_move(float distance, float speed, float acceleration);
-
-/* Turns around the propulsion shaft center
- * - angle : angle to turn of (in degrees), can be positive (CCW)
- * or negative (CW).
- * - speed : turning speed (in degrees per second) should be positive.
- * - acceleration : in degrees per second per second, should be positive.
- */
-void dd_turn(float angle, float speed, float acceleration);
-
-/*
- * Modify a given speed of the robot using the specified acceleration
- */
-void dd_set_linear_speed(float speed, float acceleration);
-void dd_set_rotational_speed(float speed, float acceleration);
-
-/*
- * Add a Bezier Spline to the trajectory follower
- */
-uint8_t dd_add_bezier(float x_end, float y_end, float d1, float d2, float end_angle, float end_speed);
-
-/*
- * Stops the current trajectory and removes the queued ones.
- * - rot_acc : acceleration to stop the rotational motion of the drive.
- * - lin_acc : acceleration to stop the linear motion of the drive.
- *
- * If one of the accelerations if zero, the corresponding movement will be stopped abruptly
- */
-void dd_interrupt_trajectory(float rot_acc, float lin_acc);
-
-/*
- * Return the current state of the followed ghost robot
- * - state : pointer to a robot_state_t structure where the ghost state will be written
- * - u : pointer to a float in which to write the current value of the parameter on the spline
- *
- * If one of these pointers is NULL, the associated value won't be written.
- *
- * return value :
- * - DD_GHOST_MOVING : if a trajectory is currently followed
- * - DD_GHOST_STOPPED : if the ghost robot is stopped
- */
-uint8_t dd_get_ghost_state(robot_state_t *state, float *u);
-
-#endif /* __DIFFERENTIAL_DRIVE_H */
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/encoder.c b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/encoder.c
deleted file mode 100644
index 6552dbb..0000000
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/encoder.c
+++ /dev/null
@@ -1,173 +0,0 @@
-/*
- * Wrapper to use the Encoder interface
- * Xavier Lagorce
- */
-
-#include "encoder.h"
-
-#include <cfg/macros.h>
-#include <drv/gpio_stm32.h>
-
-#include "stm32lib/stm32f10x.h"
-#include "stm32lib/stm32f10x_rcc.h"
-#include "stm32lib/stm32f10x_tim.h"
-
-
-/*
- * Function to initialise one encoder interface
- */
-void encodersInit(void) {
-
- TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
- TIM_ICInitTypeDef TIM_ICInitStructure;
-
- //Enable GPIOA and GPIOB clock
- RCC->APB2ENR |= RCC_APB2_GPIOA | RCC_APB2_GPIOB | RCC_APB2_GPIOC;
-
- //Enable timer clock
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
- RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE);
- RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
- RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
-
- //Setup timer for quadrature encoder interface
- //Encoder1 A channel at PA6 (Ch1)
- // B channel at PA7 (Ch2) -> TIM3
- //Encoder2 A channel at PC6 (Ch1)
- // B channel at PC7 (Ch2) -> TIM8
- //Encoder3 A channel at PA8 (Ch1)
- // B channel at PA9 (Ch2) -> TIM1
- //Encoder4 A channel at PB6 (Ch1)
- // B channel at PB7 (Ch2) -> TIM4
- stm32_gpioPinConfig(((struct stm32_gpio *)GPIOA_BASE),
- BV(6) | BV(7) | BV(8) | BV(9),
- GPIO_MODE_IN_FLOATING, GPIO_SPEED_50MHZ);
- stm32_gpioPinConfig(((struct stm32_gpio *)GPIOB_BASE),
- BV(6) | BV(7),
- GPIO_MODE_IN_FLOATING, GPIO_SPEED_50MHZ);
- stm32_gpioPinConfig(((struct stm32_gpio *)GPIOC_BASE),
- BV(6) | BV(7),
- GPIO_MODE_IN_FLOATING, GPIO_SPEED_50MHZ);
-
- // TimeBase configuration
- TIM_TimeBaseStructure.TIM_Prescaler = 0;
- TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
- TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
- TIM_TimeBaseStructure.TIM_ClockDivision = 0;
- TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
- TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
- TIM_TimeBaseInit(TIM8, &TIM_TimeBaseStructure);
- TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);
- TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
-
- //Initialize input capture structure: Ch1
- TIM_ICStructInit(&TIM_ICInitStructure);
- TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
- TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
- TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
- TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
- TIM_ICInitStructure.TIM_ICFilter = 0;
- TIM_ICInit(TIM3, &TIM_ICInitStructure);
- TIM_ICInit(TIM8, &TIM_ICInitStructure);
- TIM_ICInit(TIM1, &TIM_ICInitStructure);
- TIM_ICInit(TIM4, &TIM_ICInitStructure);
-
- //Initialize input capture structure: Ch2
- TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
- TIM_ICInit(TIM3, &TIM_ICInitStructure);
- TIM_ICInit(TIM8, &TIM_ICInitStructure);
- TIM_ICInit(TIM1, &TIM_ICInitStructure);
- TIM_ICInit(TIM4, &TIM_ICInitStructure);
-
- //Encoder Interface Configuration
- TIM_EncoderInterfaceConfig(TIM3,
- TIM_EncoderMode_TI12,
- TIM_ICPolarity_Rising,
- TIM_ICPolarity_Rising);
- TIM_EncoderInterfaceConfig(TIM8,
- TIM_EncoderMode_TI12,
- TIM_ICPolarity_Rising,
- TIM_ICPolarity_Rising);
- TIM_EncoderInterfaceConfig(TIM1,
- TIM_EncoderMode_TI12,
- TIM_ICPolarity_Rising,
- TIM_ICPolarity_Rising);
- TIM_EncoderInterfaceConfig(TIM4,
- TIM_EncoderMode_TI12,
- TIM_ICPolarity_Rising,
- TIM_ICPolarity_Rising);
- //Enable timer Peripherals
- TIM_Cmd(TIM3,ENABLE);
- TIM_Cmd(TIM8,ENABLE);
- TIM_Cmd(TIM1,ENABLE);
- TIM_Cmd(TIM4,ENABLE);
-}
-
-/*
- * Helper to get the current encoder position
- * encoder : get the value form this encoder
- */
-uint16_t getEncoderPosition(uint8_t encoder) {
- switch(encoder) {
- case ENCODER1:
- return TIM_GetCounter(TIM3);
- break;
- case ENCODER2:
- return TIM_GetCounter(TIM8);
- break;
- case ENCODER3:
- return TIM_GetCounter(TIM1);
- break;
- case ENCODER4:
- return TIM_GetCounter(TIM4);
- break;
- default:
- return 0;
- }
-}
-
-/*
- * Helper to get the current direction of evolution of the counter
- */
-uint8_t getEncoderDirection(uint8_t encoder) {
- switch(encoder) {
- case ENCODER1:
- return ((TIM3->CR1 & TIM_CR1_DIR) != 0 ? ENCODER_DIR_DOWN : ENCODER_DIR_UP);
- break;
- case ENCODER2:
- return ((TIM8->CR1 & TIM_CR1_DIR) != 0 ? ENCODER_DIR_DOWN : ENCODER_DIR_UP);
- break;
- case ENCODER3:
- return ((TIM1->CR1 & TIM_CR1_DIR) != 0 ? ENCODER_DIR_DOWN : ENCODER_DIR_UP);
- break;
- case ENCODER4:
- return ((TIM4->CR1 & TIM_CR1_DIR) != 0 ? ENCODER_DIR_DOWN : ENCODER_DIR_UP);
- break;
- default:
- return 0;
- }
-}
-
-/*
- * Helper to reset the current encoder position
- * encoder : reset the value from this encoder
- *
- */
-void resetEncoderPosition(uint8_t encoder) {
- switch(encoder) {
- case ENCODER1:
- TIM_SetCounter(TIM3, 0);
- break;
- case ENCODER2:
- TIM_SetCounter(TIM8, 0);
- break;
- case ENCODER3:
- TIM_SetCounter(TIM1, 0);
- break;
- case ENCODER4:
- TIM_SetCounter(TIM4, 0);
- break;
- default:
- break;
- }
-}
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/encoder.h b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/encoder.h
deleted file mode 100644
index e94ac88..0000000
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/encoder.h
+++ /dev/null
@@ -1,25 +0,0 @@
-/*
- * Wrapper to use the Encoder interface
- * Xavier Lagorce
- */
-
-#ifndef HEADER__ENCODER
-#define HEADER__ENCODER
-
-#define ENCODER1 1
-#define ENCODER2 2
-#define ENCODER3 3
-#define ENCODER4 4
-
-#define ENCODER_DIR_UP 0
-#define ENCODER_DIR_DOWN 1
-
-#include <drv/gpio_stm32.h>
-#include <drv/clock_stm32.h>
-
-void encodersInit(void);
-uint16_t getEncoderPosition(uint8_t encoder);
-void resetEncoderPosition(uint8_t encoder);
-uint8_t getEncoderDirection(uint8_t encoder);
-
-#endif
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/lift_controller.c b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/lift_controller.c
deleted file mode 100644
index 56cb686..0000000
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/lift_controller.c
+++ /dev/null
@@ -1,139 +0,0 @@
-/*
- * lift_controller.c
- * -----------------
- * Copyright : (c) 2011, Xavier Lagorce <Xav...@cr...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- */
-
-#include "lift_controller.h"
-
-#define DIR_UP 0
-#define DIR_DOWN 1
-
-typedef struct {
- uint8_t enabled, tc_ind;
- int8_t direction;
- float bottom, extend;
- uint8_t end_stop_ind;
-} lc_state_t;
-
-lc_state_t lc_state[2];
-
-float pos2angle(uint8_t lift, float position);
-
-float pos2angle(uint8_t lift, float position) {
-
- //return lc_state[lift].offset - position / 23.474;
- return lc_state[lift].bottom + lc_state[lift].extend*position;
-}
-
-void lc_init(void) {
- motor_controller_params_t params;
- int i;
-
- // Init state
- for (i=0; i<2; i++) {
- lc_state[i].enabled = 1;
- lc_state[i].direction = -1;
- lc_state[i].bottom = 0;
- lc_state[i].extend = 0;
- lc_state[i].end_stop_ind = 0;
- }
- lc_state[LC_FRONT_LIFT].tc_ind = LC_TC_FRONT;
- lc_state[LC_BACK_LIFT].tc_ind = LC_TC_BACK;
-
- // Init Trajectory controllers
- tc_new_controller(LC_TC_FRONT);
- tc_new_controller(LC_TC_BACK);
- // Limit PWM value
- motorSetMaxPWM(MOTOR3, 1600);
- motorSetMaxPWM(MOTOR4, 1600);
- // Common parameters
- params.encoder_gain = 2.0*M_PI/588.0;
- params.G0 = 0.0035;
- params.tau = 0.025;
- params.k[0] = -10216;
- params.k[1] = -255.39;
- params.l = -params.k[0];
- params.l0[0] = 0.0091;
- params.l0[1] = 1.6361;
- params.T = 0.005;
- // Initialize front lift
- params.motor = MOTOR4;
- params.encoder = ENCODER4;
- mc_new_controller(¶ms, tc_get_position_generator(LC_TC_FRONT), CONTROLLER_MODE_NORMAL);
- // Initialize back lift
- params.motor = MOTOR3;
- params.encoder = ENCODER3;
- mc_new_controller(¶ms, tc_get_position_generator(LC_TC_BACK), CONTROLLER_MODE_NORMAL);
-}
-
-void lc_end_stop_reached(uint8_t end_stops) {
-
- // Set the end stop indicators and stop the lift if needed
- if ((end_stops & LC_FRONT_UP) || (end_stops & LC_FRONT_BOTTOM)) {
- if ((end_stops & LC_FRONT_UP) && (lc_state[LC_FRONT_LIFT].direction == 1))
- tc_stop(LC_TC_FRONT);
- if ((end_stops & LC_FRONT_BOTTOM) && (lc_state[LC_FRONT_LIFT].direction == -1))
- tc_stop(LC_TC_FRONT);
- lc_state[LC_FRONT_LIFT].end_stop_ind = end_stops & (LC_FRONT_UP|LC_FRONT_BOTTOM);
- }
- if ((end_stops & LC_BACK_UP) || (end_stops & LC_BACK_BOTTOM)) {
- if ((end_stops & LC_BACK_UP) && (lc_state[LC_BACK_LIFT].direction == 1))
- tc_stop(LC_TC_BACK);
- if ((end_stops & LC_BACK_BOTTOM) && (lc_state[LC_BACK_LIFT].direction == -1))
- tc_stop(LC_TC_BACK);
- lc_state[LC_BACK_LIFT].end_stop_ind = (end_stops & (LC_BACK_UP|LC_BACK_BOTTOM)) >> 2;
- }
-}
-
-void lc_homing(uint8_t lift) {
- // Go down
- lc_state[lift].direction = -1;
- tc_goto_speed(lc_state[lift].tc_ind, M_PI/4, 1);
- // Wait for the end stop to trigger
- while (!(lc_state[lift].end_stop_ind & LC_BOTTOM))
- cpu_relax();
- // Stop the lift
- tc_stop(lc_state[lift].tc_ind);
- // Assert end stop
- lc_state[lift].end_stop_ind &= ~LC_BOTTOM;
- // Wait and get current generator position as offset
- timer_delay(50);
- lc_state[lift].bottom = get_output_value(tc_get_position_generator(lc_state[lift].tc_ind));
-
- // Go up
- lc_state[lift].direction = 1;
- tc_goto_speed(lc_state[lift].tc_ind, -M_PI/4, 1);
- // Wait for the end stop to trigger
- while (!(lc_state[lift].end_stop_ind & LC_UP))
- cpu_relax();
- // Stop the lift
- tc_stop(lc_state[lift].tc_ind);
- // Assert end stop
- lc_state[lift].end_stop_ind &= ~LC_UP;
- // Wait and get current generator position as offset
- timer_delay(50);
- lc_state[lift].extend = get_output_value(tc_get_position_generator(lc_state[lift].tc_ind)) - lc_state[lift].bottom;
-}
-
-void lc_goto_position(uint8_t lift, float position) {
- float goal = pos2angle(lift, position);
-
- if (goal > get_output_value(tc_get_position_generator(lc_state[lift].tc_ind))) {
- lc_state[lift].direction = -1;
- } else {
- lc_state[lift].direction = 1;
- }
- tc_goto(lift, goal, M_PI, 5);
-}
-
-void lc_release(void) {
- mc_delete_controller(MOTOR3);
- mc_delete_controller(MOTOR4);
-
- lc_state[LC_FRONT_LIFT].enabled = 0;
- lc_state[LC_BACK_LIFT].enabled = 0;
-}
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/lift_controller.h b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/lift_controller.h
deleted file mode 100644
index 717c8cc..0000000
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/lift_controller.h
+++ /dev/null
@@ -1,41 +0,0 @@
-/*
- * lift_controller.h
- * -----------------
- * Copyright : (c) 2011, Xavier Lagorce <Xav...@cr...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- */
-
-#ifndef __LIFT_CONTROLLER_H
-#define __LIFT_CONTROLLER_H
-
-#include "motor_controller.h"
-#include "trajectory_controller.h"
-#include "command_generator.h"
-
-#define LC_FRONT_LIFT 0
-#define LC_BACK_LIFT 1
-
-#define LC_FRONT_UP 1
-#define LC_FRONT_BOTTOM 2
-#define LC_BACK_UP 4
-#define LC_BACK_BOTTOM 8
-
-#define LC_UP 1
-#define LC_BOTTOM 2
-
-#define LC_POSITION_BOTTOM 1
-#define LC_POSITION_MIDDLE 2
-#define LC_POSITION_UP 4
-
-#define LC_TC_FRONT 0
-#define LC_TC_BACK 1
-
-void lc_init(void);
-void lc_end_stop_reached(uint8_t end_stops);
-void lc_homing(uint8_t lift);
-void lc_goto_position(uint8_t lift, float position);
-void lc_release(void);
-
-#endif /* __LIFT_CONTROLLER_H */
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/motor.c b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/motor.c
deleted file mode 100644
index f085184..0000000
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/motor.c
+++ /dev/null
@@ -1,381 +0,0 @@
-/*
- * Motor controller interface
- * This is a quick implementation based of the stm32lib from ST and from
- * previous implementations.
- *
- * This is supposed to be quick and dirty, waiting for BeRTOS proper
- * PWMs integration, to allow work on other systems using PWMs.
- *
- * Author : Xavier Lagorce
- */
-
-#include "motor.h"
-#include <cfg/macros.h>
-#include <drv/gpio_stm32.h>
-#include "stm32lib/stm32f10x_tim.h"
-
-uint8_t enabledMotors = 0, indMotors = 0;
-signed char currentSpeedSign[] = {0, 0, 0, 0};
-int32_t maxPWMs[] = {MAX_PWM, MAX_PWM, MAX_PWM, MAX_PWM};
-TIM_OCInitTypeDef TIM_OCInitStructure;
-
-static int32_t staSpeed(int32_t speed, int32_t maxSpeed, uint8_t *ind) {
- if (speed >= maxSpeed) {
- *ind = 1;
- return maxSpeed;
- } else if (speed <= -maxSpeed) {
- *ind = 1;
- return -maxSpeed;
- } else {
- *ind = 0;
- return speed;
- }
-}
-
-/*
- * Initialises TIM2 for PWM generation and associated GPIOs
- */
-void motorsInit(void) {
-
- TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
-
- //Enable GPIOA, GPIOB, GPIOC and GPIOD clock
- RCC->APB2ENR |= RCC_APB2_GPIOA | RCC_APB2_GPIOB | RCC_APB2_GPIOC | RCC_APB2_GPIOD;
-
- //Enable timer clock
- RCC->APB1ENR |= RCC_APB1_TIM2;
-
- //Setup timer for quadrature encoder interface
- //Motor1 : EN PA5
- // INA PC4
- // INB PC5
- // PWM PA0
- // IND LED1
- //Motor2 : EN PB15
- // INA PB1
- // INB PB14
- // PWM PA1
- // IND LED2
- //Motor3 : EN PA10
- // INA PC10
- // INB PC11
- // PWM PA2
- // IND LED3
- //Motor4 : EN PD2
- // INA PB5
- // INB PD9
- // PWM PA3
- // IND LED4
- stm32_gpioPinConfig(((struct stm32_gpio *)GPIOA_BASE),
- BV(5) | BV(10),
- GPIO_MODE_OUT_PP, GPIO_SPEED_50MHZ);
- stm32_gpioPinConfig(((struct stm32_gpio *)GPIOA_BASE),
- BV(0) | BV(1) | BV(2) | BV(3),
- GPIO_MODE_AF_...
[truncated message content] |
|
From: Jérémie D. <Ba...@us...> - 2011-06-16 17:41:26
|
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 5403b5850fb9dd5aadbb3350764821280e44de0e (commit)
from 3f6b2f56b27416cf35ffbc0d5fa6d67eb8b6b0ea (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 5403b5850fb9dd5aadbb3350764821280e44de0e
Author: Jérémie Dimino <je...@di...>
Date: Thu Jun 16 19:40:36 2011 +0200
[info] integrates the rest of krobot_run into krobot_vm/action
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/lib/krobot_action.ml b/info/control2011/src/lib/krobot_action.ml
index 6255e48..6e1dd3c 100644
--- a/info/control2011/src/lib/krobot_action.ml
+++ b/info/control2011/src/lib/krobot_action.ml
@@ -22,9 +22,24 @@ type t =
| Wait_for_moving of bool
| Reset_odometry of [ `Red | `Blue | `Auto ]
| Wait_for_odometry of [ `Eq | `Gt | `Ge | `Lt | `Le ] * int
+ | Load of [ `Front | `Back ]
+ | Lift_down of [ `Front | `Back ]
+ | Lift_up of [ `Front | `Back ]
+ | Open_grip_low of [ `Front | `Back ]
+ | Close_grip_low of [ `Front | `Back ]
+ | Open_grip_high of [ `Front | `Back ]
+ | Close_grip_high of [ `Front | `Back ]
+ | Wait_for of float
+ | Wait_until of float
+ | Wait_for_grip_open_low of [ `Front | `Back ]
+ | Wait_for_grip_close_low of [ `Front | `Back ]
let string_of_vertice { x; y } = sprintf "{ x = %f; y = %f }" x y
+let string_of_face = function
+ | `Front -> "`Front"
+ | `Back -> "`Back"
+
let rec to_string = function
| Node l ->
sprintf "Node [%s]" (String.concat "; " (List.map to_string l))
@@ -69,3 +84,25 @@ let rec to_string = function
| `Lt -> "Lt"
| `Le -> "Le")
value
+ | Load face ->
+ sprintf "Load %s" (string_of_face face)
+ | Lift_down face ->
+ sprintf "Lift_down %s" (string_of_face face)
+ | Lift_up face ->
+ sprintf "Lift_up %s" (string_of_face face)
+ | Open_grip_low face ->
+ sprintf "Open_grip_low %s" (string_of_face face)
+ | Close_grip_low face ->
+ sprintf "Close_grip_low %s" (string_of_face face)
+ | Open_grip_high face ->
+ sprintf "Open_grip_high %s" (string_of_face face)
+ | Close_grip_high face ->
+ sprintf "Close_grip_high %s" (string_of_face face)
+ | Wait_for t ->
+ sprintf "Wait_for %f" t
+ | Wait_until t ->
+ sprintf "Wait_until %f" t
+ | Wait_for_grip_open_low face ->
+ sprintf "Wait_for_grip_open_low %S" (string_of_face face)
+ | Wait_for_grip_close_low face ->
+ sprintf "Wait_for_grip_close_low %S" (string_of_face face)
diff --git a/info/control2011/src/lib/krobot_action.mli b/info/control2011/src/lib/krobot_action.mli
index 72e953a..f103559 100644
--- a/info/control2011/src/lib/krobot_action.mli
+++ b/info/control2011/src/lib/krobot_action.mli
@@ -40,6 +40,28 @@ type t =
| Wait_for_odometry of [ `Eq | `Gt | `Ge | `Lt | `Le ] * int
(** Wait for the curve parameter of the odometry to reach the
given state. *)
+ | Load of [ `Front | `Back ]
+ (** Load a pawn. *)
+ | Lift_down of [ `Front | `Back ]
+ (** Move the front or back lift down. *)
+ | Lift_up of [ `Front | `Back ]
+ (** Move the front or back lift up. *)
+ | Open_grip_low of [ `Front | `Back ]
+ (** Open the front or back low grip. *)
+ | Close_grip_low of [ `Front | `Back ]
+ (** Close the front or back low grip. *)
+ | Open_grip_high of [ `Front | `Back ]
+ (** Open the front or back low grip. *)
+ | Close_grip_high of [ `Front | `Back ]
+ (** Close the front or back low grip. *)
+ | Wait_for of float
+ (** Wait for the given number of seconds. *)
+ | Wait_until of float
+ (** Wait until the given date. *)
+ | Wait_for_grip_open_low of [ `Front | `Back ]
+ (** Wait for the given low grip to be opened. *)
+ | Wait_for_grip_close_low of [ `Front | `Back ]
+ (** Wait for the given low grip to be opened. *)
val to_string : t -> string
(** [to_string action] returns the string representation of the
diff --git a/info/control2011/src/tools/krobot_vm.ml b/info/control2011/src/tools/krobot_vm.ml
index 9d67145..d4dc507 100644
--- a/info/control2011/src/tools/krobot_vm.ml
+++ b/info/control2011/src/tools/krobot_vm.ml
@@ -21,6 +21,13 @@ open Krobot_action
| Types |
+-----------------------------------------------------------------+ *)
+(* State of an AX12. *)
+type ax12 = {
+ mutable ax12_position : int;
+ mutable ax12_speed : int;
+ mutable ax12_torque : int;
+}
+
(* Type of robots. *)
type robot = {
bus : Krobot_bus.t;
@@ -68,6 +75,16 @@ type robot = {
mutable team : [ `Red | `Blue ];
(* The state of the team selector. *)
+
+ ax12_front_low_left : ax12;
+ ax12_front_low_right : ax12;
+ ax12_front_high_left : ax12;
+ ax12_front_high_right : ax12;
+ ax12_back_low_left : ax12;
+ ax12_back_low_right : ax12;
+ ax12_back_high_left : ax12;
+ ax12_back_high_right : ax12;
+ (* State of AX12s. *)
}
(* +-----------------------------------------------------------------+
@@ -100,6 +117,20 @@ let handle_message robot (timestamp, message) =
| Switch1_status(b, _, _, _, _, _, _, _) ->
robot.jack <- not b
+ | Ax12_State(id, position, speed, torque) -> begin
+ let set ax12 =
+ ax12.ax12_position <- position;
+ ax12.ax12_speed <- speed;
+ ax12.ax12_torque <- torque
+ in
+ match id with
+ | 1 -> set robot.ax12_front_low_left
+ | 2 -> set robot.ax12_front_low_right
+ | 3 -> set robot.ax12_front_high_left
+ | 4 -> set robot.ax12_front_high_right
+ | _ -> ()
+ end
+
| _ ->
()
end
@@ -198,6 +229,13 @@ let rec exec robot actions =
exec robot rest
else
(actions, Wait)
+ | Wait_for t :: rest ->
+ exec robot (Wait_until (Unix.gettimeofday () +. t) :: rest)
+ | Wait_until t :: rest ->
+ if Unix.gettimeofday () >= t then
+ exec robot rest
+ else
+ (actions, Wait)
| Set_curve None :: rest ->
reset robot;
exec robot rest
@@ -278,8 +316,26 @@ let rec exec robot actions =
Set_odometry(0.215 -. robot_size /. 2. +. wheels_position, 1.885, 0.)
| `Blue, _ | `Auto, `Blue ->
Set_odometry(2.77, 1.915, pi)))
+ | Load face :: rest ->
+ exec robot (Node [
+ Lift_down face;
+ Open_grip_low face;
+ Wait_for_grip_open_low face;
+ Wait_for 0.5;
+ (* Move_to_pawn *)
+ Close_grip_low face;
+ Lift_up face;
+ Wait_for_grip_close_low face;
+ Wait_for 0.5;
+ ] :: rest)
+ | Lift_down `Front :: rest ->
+ (rest, Send(Elevator(0., -1.)))
+ | Lift_up `Front :: rest ->
+ (rest, Send(Elevator(1., -1.)))
| Think :: rest ->
exec robot rest
+ | _ :: rest ->
+ exec robot rest
(* +-----------------------------------------------------------------+
| Main loop |
@@ -390,6 +446,14 @@ lwt () =
beacon = None;
date_seen_beacon = 0.;
team = `Red;
+ ax12_front_low_left = { ax12_position = 0; ax12_speed = 0; ax12_torque = 0 };
+ ax12_front_low_right = { ax12_position = 0; ax12_speed = 0; ax12_torque = 0 };
+ ax12_front_high_left = { ax12_position = 0; ax12_speed = 0; ax12_torque = 0 };
+ ax12_front_high_right = { ax12_position = 0; ax12_speed = 0; ax12_torque = 0 };
+ ax12_back_low_left = { ax12_position = 0; ax12_speed = 0; ax12_torque = 0 };
+ ax12_back_low_right = { ax12_position = 0; ax12_speed = 0; ax12_torque = 0 };
+ ax12_back_high_left = { ax12_position = 0; ax12_speed = 0; ax12_torque = 0 };
+ ax12_back_high_right = { ax12_position = 0; ax12_speed = 0; ax12_torque = 0 };
} in
(* Handle krobot message. *)
hooks/post-receive
--
krobot
|
|
From: Jérémie D. <Ba...@us...> - 2011-06-12 15:23:42
|
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 3f6b2f56b27416cf35ffbc0d5fa6d67eb8b6b0ea (commit)
from 86cafa05a128e93f4e8630479c9669bc495fb006 (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 3f6b2f56b27416cf35ffbc0d5fa6d67eb8b6b0ea
Author: Jérémie Dimino <je...@di...>
Date: Sat Jun 11 22:29:20 2011 +0200
[info] rewrite path planning using a system of actions
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/_oasis b/info/control2011/_oasis
index acedcc8..1f4cca0 100644
--- a/info/control2011/_oasis
+++ b/info/control2011/_oasis
@@ -41,7 +41,8 @@ Library krobot
Krobot_solve,
Krobot_pathfinding,
Krobot_config,
- Krobot_path
+ Krobot_path,
+ Krobot_action
CSources:
krobot_message_stubs.c
@@ -193,11 +194,18 @@ Executable "krobot-sharps"
MainIs: krobot_sharps.ml
BuildDepends: krobot, lwt.syntax
-Executable "krobot-homo"
+Executable "krobot-vm"
Path: src/tools
Install: true
CompiledObject: best
- MainIs: krobot_homo.ml
+ MainIs: krobot_vm.ml
+ BuildDepends: krobot, lwt.syntax
+
+Executable "krobot-ia"
+ Path: src/tools
+ Install: true
+ CompiledObject: best
+ MainIs: krobot_ia.ml
BuildDepends: krobot, lwt.syntax
# +-------------------------------------------------------------------+
diff --git a/info/control2011/_tags b/info/control2011/_tags
index 2f33846..79de459 100644
--- a/info/control2011/_tags
+++ b/info/control2011/_tags
@@ -2,7 +2,7 @@
<src/interfaces/*.ml>: -syntax_camlp4o
# OASIS_START
-# DO NOT EDIT (digest: 5752a329a86d17ea144b965fdf9cbca2)
+# DO NOT EDIT (digest: 4e49eb45e4c273e67ec75220ea03b734)
# Ignore VCS directories, you can use the same kind of rule outside
# OASIS_START/STOP if you want to exclude directories that contains
# useless stuff for the build process
@@ -100,6 +100,11 @@
<examples/*.ml{,i}>: pkg_lwt.unix
<examples/*.ml{,i}>: pkg_lwt.syntax
<examples/*.ml{,i}>: pkg_lwt.react
+# Executable krobot-vm
+<src/tools/krobot_vm.{native,byte}>: use_krobot
+<src/tools/krobot_vm.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_vm.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_vm.{native,byte}>: pkg_lwt.react
# Executable krobot-webcam
<src/tools/krobot_webcam.{native,byte}>: use_krobot
<src/tools/krobot_webcam.{native,byte}>: pkg_lwt.unix
@@ -110,6 +115,11 @@
<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-ia
+<src/tools/krobot_ia.{native,byte}>: use_krobot
+<src/tools/krobot_ia.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_ia.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_ia.{native,byte}>: pkg_lwt.react
# Executable krobot-replay
<src/tools/krobot_replay.{native,byte}>: use_krobot
<src/tools/krobot_replay.{native,byte}>: pkg_lwt.unix
@@ -146,11 +156,6 @@
<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_lwt.react
-# Executable krobot-homo
-<src/tools/krobot_homo.{native,byte}>: use_krobot
-<src/tools/krobot_homo.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_homo.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_homo.{native,byte}>: pkg_lwt.react
<src/tools/*.ml{,i}>: use_krobot
<src/tools/*.ml{,i}>: pkg_lwt.unix
<src/tools/*.ml{,i}>: pkg_lwt.syntax
diff --git a/info/control2011/krobot-api.odocl b/info/control2011/krobot-api.odocl
index 21068ef..e19a38b 100644
--- a/info/control2011/krobot-api.odocl
+++ b/info/control2011/krobot-api.odocl
@@ -1,5 +1,5 @@
# OASIS_START
-# DO NOT EDIT (digest: 8446681ae962223329ec305b5442f818)
+# DO NOT EDIT (digest: 7edbd90b2355975a6b906edaa0a69b3e)
src/lib/Krobot_can
src/lib/Krobot_bus
src/lib/Krobot_message
@@ -8,5 +8,6 @@ src/lib/Krobot_solve
src/lib/Krobot_pathfinding
src/lib/Krobot_config
src/lib/Krobot_path
+src/lib/Krobot_action
src/can/Krobot_can_bus
# OASIS_STOP
diff --git a/info/control2011/setup.ml b/info/control2011/setup.ml
index e69e702..4f9b978 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: 0b1f4d2eae96725b18adca89d9662a5e) *)
+(* DO NOT EDIT (digest: 2fa81b2af9e6c805314db247864ab4e1) *)
(*
Regenerated by OASIS v0.2.1~alpha1
Visit http://oasis.forge.ocamlcore.org for more information and
@@ -5007,7 +5007,8 @@ let setup_t =
"Krobot_solve";
"Krobot_pathfinding";
"Krobot_config";
- "Krobot_path"
+ "Krobot_path";
+ "Krobot_action"
];
lib_internal_modules = [];
lib_findlib_parent = None;
@@ -5420,6 +5421,33 @@ let setup_t =
{exec_custom = false; exec_main_is = "send_can.ml"; });
Executable
({
+ cs_name = "krobot-vm";
+ 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_vm.ml"; });
+ Executable
+ ({
cs_name = "krobot-webcam";
cs_data = PropList.Data.create ();
cs_plugin_data = [];
@@ -5475,6 +5503,33 @@ let setup_t =
});
Executable
({
+ cs_name = "krobot-ia";
+ 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_ia.ml"; });
+ Executable
+ ({
cs_name = "krobot-replay";
cs_data = PropList.Data.create ();
cs_plugin_data = [];
@@ -5663,33 +5718,6 @@ let setup_t =
});
Executable
({
- cs_name = "krobot-homo";
- 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_homo.ml"; });
- Executable
- ({
cs_name = "krobot-driver";
cs_data = PropList.Data.create ();
cs_plugin_data = [];
diff --git a/info/control2011/src/lib/krobot.mllib b/info/control2011/src/lib/krobot.mllib
index 9dc39d3..44dbb2b 100644
--- a/info/control2011/src/lib/krobot.mllib
+++ b/info/control2011/src/lib/krobot.mllib
@@ -1,5 +1,5 @@
# OASIS_START
-# DO NOT EDIT (digest: 88a10a3a673ab385eafde0a601e2a247)
+# DO NOT EDIT (digest: 39a92ff71a82fc1bed00795c4d3b6319)
Krobot_can
Krobot_bus
Krobot_message
@@ -8,4 +8,5 @@ Krobot_solve
Krobot_pathfinding
Krobot_config
Krobot_path
+Krobot_action
# OASIS_STOP
diff --git a/info/control2011/src/lib/krobot_action.ml b/info/control2011/src/lib/krobot_action.ml
new file mode 100644
index 0000000..6255e48
--- /dev/null
+++ b/info/control2011/src/lib/krobot_action.ml
@@ -0,0 +1,71 @@
+(*
+ * krobot_action.ml
+ * ----------------
+ * Copyright : (c) 2011, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+open Printf
+open Krobot_geom
+
+type t =
+ | Node of t list
+ | Stop
+ | Think
+ | Goto of vertice
+ | Follow_path of vertice list
+ | Bezier of float * vertice * vertice * vertice * vertice * float
+ | Set_curve of Bezier.curve option
+ | Wait_for_jack of bool
+ | Wait_for_moving of bool
+ | Reset_odometry of [ `Red | `Blue | `Auto ]
+ | Wait_for_odometry of [ `Eq | `Gt | `Ge | `Lt | `Le ] * int
+
+let string_of_vertice { x; y } = sprintf "{ x = %f; y = %f }" x y
+
+let rec to_string = function
+ | Node l ->
+ sprintf "Node [%s]" (String.concat "; " (List.map to_string l))
+ | Stop ->
+ "Stop"
+ | Think ->
+ "Think"
+ | Goto v ->
+ sprintf "Goto %s" (string_of_vertice v)
+ | Follow_path l ->
+ sprintf "Follow_path [%s]" (String.concat "; " (List.map string_of_vertice l))
+ | Bezier(sign, p, q, r, s, end_velocity) ->
+ sprintf
+ "Bezier(%f, %s, %s, %s, %s, %f)"
+ sign
+ (string_of_vertice p)
+ (string_of_vertice q)
+ (string_of_vertice r)
+ (string_of_vertice s)
+ end_velocity
+ | Set_curve(Some c) ->
+ sprintf "Set_curve(Some %s)" (Bezier.string_of_curve c)
+ | Set_curve None ->
+ "Set_curve None"
+ | Wait_for_jack st ->
+ sprintf "Wait_for_jack %B" st
+ | Wait_for_moving st ->
+ sprintf "Wait_for_moving %B" st
+ | Reset_odometry `Red ->
+ "Reset_odometry `Red"
+ | Reset_odometry `Blue ->
+ "Reset_odometry `Blue"
+ | Reset_odometry `Auto ->
+ "Reset_odometry `Auto"
+ | Wait_for_odometry(test, value) ->
+ sprintf
+ "Wait_for_odometry(`%s, %d)"
+ (match test with
+ | `Eq -> "Eq"
+ | `Gt -> "Gt"
+ | `Ge -> "Ge"
+ | `Lt -> "Lt"
+ | `Le -> "Le")
+ value
diff --git a/info/control2011/src/lib/krobot_action.mli b/info/control2011/src/lib/krobot_action.mli
new file mode 100644
index 0000000..72e953a
--- /dev/null
+++ b/info/control2011/src/lib/krobot_action.mli
@@ -0,0 +1,46 @@
+(*
+ * krobot_action.mli
+ * -----------------
+ * Copyright : (c) 2011, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(** Robot's actions *)
+
+open Krobot_geom
+
+(** Type of actions. *)
+type t =
+ | Node of t list
+ (** A sequence of action to execute in order. *)
+ | Stop
+ (** Stop all actions. *)
+ | Think
+ (** This is the highest-level action. It instruct the VM to
+ think about a new strategy. *)
+ | Goto of vertice
+ (** Go to the given point. *)
+ | Follow_path of vertice list
+ (** Follow the given path. It does not check for obstacles. *)
+ | Bezier of float * vertice * vertice * vertice * vertice * float
+ (** Follow the bezier curve determined by the given four
+ vertices. The first float is the sign, the last one is the
+ end velocity. *)
+ | Set_curve of Bezier.curve option
+ (** Set the curve currently being followed. *)
+ | Wait_for_jack of bool
+ (** Wait for the jack to be in the given state. *)
+ | Wait_for_moving of bool
+ (** Wait for the robot to move or not. *)
+ | Reset_odometry of [ `Red | `Blue | `Auto ]
+ (** Set the odometry to the initial position according to the
+ team selector or the given color. *)
+ | Wait_for_odometry of [ `Eq | `Gt | `Ge | `Lt | `Le ] * int
+ (** Wait for the curve parameter of the odometry to reach the
+ given state. *)
+
+val to_string : t -> string
+ (** [to_string action] returns the string representation of the
+ given string. *)
diff --git a/info/control2011/src/lib/krobot_bus.ml b/info/control2011/src/lib/krobot_bus.ml
index b55ef14..bb2d08a 100644
--- a/info/control2011/src/lib/krobot_bus.ml
+++ b/info/control2011/src/lib/krobot_bus.ml
@@ -25,17 +25,18 @@ type message =
| Log of string
| Send
| Kill of string
- | Trajectory_vertices of vertice list * (vertice * vertice * vertice * vertice) list
+ | Trajectory_path of Bezier.curve list
| Trajectory_set_vertices of vertice list
| Trajectory_add_vertice of vertice
| Trajectory_simplify of float
- | Trajectory_go of float * float * float * float
- | Trajectory_goto of vertice
- | Trajectory_stop
- | Trajectory_moving of bool
+ | Trajectory_go
| Trajectory_find_path
| Objects of vertice list
| Sharps of float array
+ | Strategy_append of Krobot_action.t list
+ | Strategy_stop
+ | Strategy_set of Krobot_action.t list
+ | Strategy_path of Bezier.curve list option
type t = {
oc : Lwt_io.output_channel;
@@ -70,19 +71,10 @@ let string_of_message = function
sprintf
"Kill %S"
name
- | Trajectory_vertices(vertices, curves) ->
+ | Trajectory_path curves ->
sprintf
- "Trajectory_vertices([%s], [%s])"
- (String.concat "; " (List.map string_of_vertice vertices))
- (String.concat "; " (List.map
- (fun (p, q, r, s) ->
- Printf.sprintf
- "(%s, %s, %s, %s)"
- (string_of_vertice p)
- (string_of_vertice q)
- (string_of_vertice r)
- (string_of_vertice s))
- curves))
+ "Trajectory_path [%s]"
+ (String.concat "; " (List.map Bezier.string_of_curve curves))
| Trajectory_set_vertices l ->
sprintf
"Trajectory_set_vertices [%s]"
@@ -95,20 +87,8 @@ let string_of_message = function
sprintf
"Trajectory_simplify %f"
tolerance
- | Trajectory_go(a, b, c, d) ->
- sprintf
- "Trajectory_go(%f, %f, %f, %f)"
- a b c d
- | Trajectory_goto v ->
- sprintf
- "Trajectory_goto %s"
- (string_of_vertice v)
- | Trajectory_stop ->
- "Trajectory_stop"
- | Trajectory_moving b ->
- sprintf
- "Trajectory_moving %B"
- b
+ | Trajectory_go ->
+ "Trajectory_go"
| Trajectory_find_path ->
"Trajectory_find_path"
| Objects objects ->
@@ -119,6 +99,18 @@ let string_of_message = function
sprintf
"Sharps [|%s|]"
(String.concat "; " (List.map string_of_float (Array.to_list a)))
+ | Strategy_append l ->
+ sprintf "Strategy_append [%s]" (String.concat "; " (List.map Krobot_action.to_string l))
+ | Strategy_stop ->
+ "Strategy_stop"
+ | Strategy_set l ->
+ sprintf "Strategy_set [%s]" (String.concat "; " (List.map Krobot_action.to_string l))
+ | Strategy_path None ->
+ "Strategy_path None"
+ | Strategy_path(Some curves) ->
+ sprintf
+ "Strategy_path(Some [%s])"
+ (String.concat "; " (List.map Bezier.string_of_curve curves))
(* +-----------------------------------------------------------------+
| Sending/receiving messages |
diff --git a/info/control2011/src/lib/krobot_bus.mli b/info/control2011/src/lib/krobot_bus.mli
index 17e8697..bef8110 100644
--- a/info/control2011/src/lib/krobot_bus.mli
+++ b/info/control2011/src/lib/krobot_bus.mli
@@ -34,24 +34,16 @@ type message =
(** Trajectory messages. *)
- | Trajectory_vertices of vertice list * (vertice * vertice * vertice * vertice) list
- (** The list of vertices for the planned trajectory, along with
- the bezier curves. *)
+ | Trajectory_path of Bezier.curve list
+ (** The planned trajectory. *)
| Trajectory_set_vertices of vertice list
(** Sets the trajectory. *)
| Trajectory_add_vertice of vertice
(** Add a vertice to the trajectory. *)
| Trajectory_simplify of float
(** Simplify the trajectory with the given tolerance. *)
- | Trajectory_go of float * float * float * float
- (** [Trajectory_go(rotation_speed, rotation_acceleration,
- moving_speed, moving_acceleration)]. *)
- | Trajectory_goto of vertice
- (** [Trajectory_goto dest] go to the given position. *)
- | Trajectory_stop
- (** Stop the current trajectory. *)
- | Trajectory_moving of bool
- (** Is the robot following a trajectory ? *)
+ | Trajectory_go
+ (** Follow currently registered trajectory. *)
| Trajectory_find_path
(** Find a path avoiding objects. *)
@@ -65,6 +57,18 @@ type message =
| Sharps of float array
(** Distances measured by the sharps. *)
+ (** Strategy *)
+
+ | Strategy_append of Krobot_action.t list
+ (** Append the given list of action to the current strategy. *)
+ | Strategy_stop
+ (** Stop and empty the current strategy. *)
+ | Strategy_set of Krobot_action.t list
+ (** Stop and replace the current stategy by the given one. *)
+ | Strategy_path of (Bezier.curve list) option
+ (** Message emitted when the robot starts or stops a
+ trajectory. *)
+
val string_of_message : message -> string
(** Returns a string representation of the given message. *)
diff --git a/info/control2011/src/lib/krobot_geom.ml b/info/control2011/src/lib/krobot_geom.ml
index 3d110fa..a7a1dea 100644
--- a/info/control2011/src/lib/krobot_geom.ml
+++ b/info/control2011/src/lib/krobot_geom.ml
@@ -98,6 +98,8 @@ let tangents a b c =
module Bezier = struct
type curve = {
+ src : vertice;
+ dst : vertice;
p : vector;
a : vector;
b : vector;
@@ -105,6 +107,7 @@ module Bezier = struct
}
let of_vertices p q r s =
+ let src = p and dst = s in
let p = vector origin p
and q = vector origin q
and r = vector origin r
@@ -112,7 +115,13 @@ module Bezier = struct
let c = (q -| p) *| 3. in
let b = (r -| q) *| 3. -| c in
let a = s -| p -| c -| b in
- { p; a; b; c }
+ { src; dst; p; a; b; c }
+
+ let src c = c.src
+ let dst c = c.dst
+
+ let string_of_curve c =
+ Printf.sprintf "<bezier { x = %f; y = %f } -> { x = %f; y = %f }>" c.src.x c.src.y c.dst.x c.dst.y
let make ~p ~s ~vp ~vs ~a ~error_max =
let sp = norm vp and ss = norm vs in
diff --git a/info/control2011/src/lib/krobot_geom.mli b/info/control2011/src/lib/krobot_geom.mli
index df40b0d..a2fbbd5 100644
--- a/info/control2011/src/lib/krobot_geom.mli
+++ b/info/control2011/src/lib/krobot_geom.mli
@@ -52,6 +52,16 @@ module Bezier : sig
type curve
(** Type of cubic Bezier curves. *)
+ val string_of_curve : curve -> string
+ (** Returns the string representation of the given bezier
+ curve. *)
+
+ val src : curve -> vertice
+ (** Return the source vertice of the given bezier curve. *)
+
+ val dst : curve -> vertice
+ (** Return the destination vertice of the given bezier curve. *)
+
val of_vertices : vertice -> vertice -> vertice -> vertice -> curve
(** [of_vertices p q r s] creates a bezier curve from the given
four control points. [p] and [s] are the first and end point
diff --git a/info/control2011/src/lib/krobot_path.ml b/info/control2011/src/lib/krobot_path.ml
index a092156..35ebb74 100644
--- a/info/control2011/src/lib/krobot_path.ml
+++ b/info/control2011/src/lib/krobot_path.ml
@@ -18,8 +18,11 @@ let rec prev_last = function
| _ :: l ->
prev_last l
-let goto_object ~src ~dst ~objects ~beacon =
+let find ~src ~dst ~objects ~beacon =
+ (* Remove objects that are near the destination. *)
let objects = List.filter (fun obj -> distance dst obj >= object_safety_distance) objects in
+ (* Remove objects that are near the curent position. *)
+ let objects = List.filter (fun obj -> distance src obj >= object_safety_distance +. 0.01) objects in
let l = List.map (fun v -> (v, object_safety_distance +. 0.01)) objects in
let l =
match beacon with
@@ -28,14 +31,15 @@ let goto_object ~src ~dst ~objects ~beacon =
| None ->
l
in
- match
- Krobot_pathfinding.find_path ~src ~dst
- ({ x = border_safety_distance;
- y = border_safety_distance},
- { x = world_width -. border_safety_distance;
- y = world_height -. border_safety_distance})
- l
- with
+ Krobot_pathfinding.find_path ~src ~dst
+ ({ x = border_safety_distance;
+ y = border_safety_distance},
+ { x = world_width -. border_safety_distance;
+ y = world_height -. border_safety_distance})
+ l
+
+let goto_object ~src ~dst ~objects ~beacon =
+ match find ~src ~dst ~objects ~beacon with
| Some p ->
let v = vector dst (prev_last (src :: p)) in
let v = v /| norm v in
diff --git a/info/control2011/src/lib/krobot_path.mli b/info/control2011/src/lib/krobot_path.mli
index 8720856..7fa8359 100644
--- a/info/control2011/src/lib/krobot_path.mli
+++ b/info/control2011/src/lib/krobot_path.mli
@@ -11,6 +11,10 @@
open Krobot_geom
+val find : src : vertice -> dst : vertice -> objects : vertice list -> beacon : vertice option -> vertice list option
+ (** [goto ~src ~dst ~objects ~beacon] find a path from [src] to
+ [dst]. *)
+
val goto_object : src : vertice -> dst : vertice -> objects : vertice list -> beacon : vertice option -> vertice option
(** [goto_object ~src ~dst ~objects ~beacon] returns the goal
position to move to to reach an object in a position to take
diff --git a/info/control2011/src/tools/krobot_homo.ml b/info/control2011/src/tools/krobot_homo.ml
deleted file mode 100644
index c6cca44..0000000
--- a/info/control2011/src/tools/krobot_homo.ml
+++ /dev/null
@@ -1,128 +0,0 @@
-(*
- * krobot_homo.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_bus
-open Krobot_message
-open Krobot_geom
-
-(* +-----------------------------------------------------------------+
- | Types |
- +-----------------------------------------------------------------+ *)
-
-type robot = {
- bus : Krobot_bus.t;
- (* The message bus used to communicate with the robot. *)
-
- mutable moving : bool;
- (* Is the robot moving ? *)
-
- mutable position : vertice;
- (* The position of the robot on the board. *)
-
- mutable orientation : float;
- (* The orientation of the robot. *)
-
- mutable objects : vertice list;
- (* The list of objects on the board. *)
-
- mutable beacon : vertice option;
- (* Position of the beacon. *)
-
- mutable jack : bool;
- (* Is the jack present or not ? *)
-}
-
-(* +-----------------------------------------------------------------+
- | Message handling |
- +-----------------------------------------------------------------+ *)
-
-let handle_message robot (timestamp, message) =
- match message with
- | CAN(_, frame) -> begin
- match decode frame with
- | Odometry(x, y, theta) ->
- robot.position <- { x; y };
- robot.orientation <- math_mod_float theta (2. *. pi)
-
- | Beacon_position(angle, distance, period) ->
- if distance <> 0. then
- let angle = math_mod_float (robot.orientation +. Krobot_config.rotary_beacon_index_pos +. angle) (2. *. pi) in
- robot.beacon <- Some{
- x = robot.position.x +. distance *. cos angle;
- y = robot.position.y +. distance *. sin angle;
- }
- else
- robot.beacon <- None
-
- | Switch1_status(b, _, _, _, _, _, _, _) ->
- robot.jack <- not b
-
- | _ ->
- ()
- end
-
- | Trajectory_moving moving ->
- robot.moving <- moving
-
- | Objects l ->
- robot.objects <- l
-
- | _ ->
- ()
-
-(* +-----------------------------------------------------------------+
- | Entry point |
- +-----------------------------------------------------------------+ *)
-
-lwt () =
- lwt bus = Krobot_bus.get () in
-
- let robot = {
- bus;
- moving = false;
- position = origin;
- orientation = 0.;
- objects = [];
- beacon = None;
- jack = true;
- } in
-
- E.keep (E.map (handle_message robot) (Krobot_bus.recv bus));
-
- lwt () =
- Krobot_message.send bus
- (Unix.gettimeofday (),
- Set_odometry(0.215 -. Krobot_config.robot_size /. 2. +. Krobot_config.wheels_position, 1.885, 0.))
- in
-
- lwt () = Lwt_unix.sleep 1.0 in
-
- lwt () =
- while_lwt robot.jack do
- Lwt_unix.sleep 0.1
- done
- in
-
- let v = { x = 1.675; y = 1.225 } in
- lwt () = Krobot_bus.send bus (Unix.gettimeofday (), Trajectory_goto v) in
- lwt () = Lwt_unix.sleep 1.0 in
- lwt () =
- while_lwt robot.moving do
- Lwt_unix.sleep 0.01
- done
- in
-
- lwt () = Krobot_message.send bus (Unix.gettimeofday (), Motor_turn(-3. *. pi /. 4. -. robot.orientation, 1., 2.)) in
- lwt () = Lwt_unix.sleep 4.0 in
- lwt () = Krobot_message.send bus (Unix.gettimeofday (), Motor_move(0.3, 0.2, 0.4)) in
- lwt () = Lwt_unix.sleep 2.0 in
-
- return ()
diff --git a/info/control2011/src/tools/krobot_ia.ml b/info/control2011/src/tools/krobot_ia.ml
new file mode 100644
index 0000000..37af235
--- /dev/null
+++ b/info/control2011/src/tools/krobot_ia.ml
@@ -0,0 +1,23 @@
+(*
+ * krobot_ia.ml
+ * ------------
+ * Copyright : (c) 2011, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(* The IA of the robot. *)
+
+open Krobot_bus
+open Krobot_action
+
+lwt () =
+ lwt bus = Krobot_bus.get () in
+ Krobot_bus.send bus
+ (Unix.gettimeofday (),
+ Strategy_set [
+ Wait_for_jack false;
+ Reset_odometry `Auto;
+ Think;
+ ])
diff --git a/info/control2011/src/tools/krobot_planner.ml b/info/control2011/src/tools/krobot_planner.ml
index dd154d3..e9b2142 100644
--- a/info/control2011/src/tools/krobot_planner.ml
+++ b/info/control2011/src/tools/krobot_planner.ml
@@ -27,23 +27,13 @@ type planner = {
(* The message bus used to communicate with the robot. *)
mutable vertices : vertice list;
- (* The list of vertices for the trajectory. *)
+ (* The list of vertices of the current trajectory. *)
- mutable curves : (float * vertice * vertice * vertice * vertice) list;
- (* The parameters of bezier curves. *)
+ mutable curves : Bezier.curve list;
+ (* The list of bezier curves of the current trajectory. *)
- mutable moving : bool;
- (* Is the robot moving ? *)
-
- 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. *)
+ mutable following_path : bool;
+ (* Is the VM following a path ? *)
mutable position : vertice;
(* The position of the robot on the board. *)
@@ -56,9 +46,6 @@ type planner = {
mutable beacon : vertice option;
(* Position of the beacon. *)
-
- mutable event : unit event;
- (* Event kept in the planner. *)
}
(* +-----------------------------------------------------------------+
@@ -66,54 +53,18 @@ type planner = {
+-----------------------------------------------------------------+ *)
let find_path planner src dst =
- (* Remove objects that are near the destination. *)
- let objects = List.filter (fun obj -> distance dst obj >= object_safety_distance) planner.objects in
- (* Remove objects that are near the curent position. *)
- let objects = List.filter (fun obj -> distance src obj >= object_safety_distance +. 0.01) objects in
- let l = List.map (fun v -> (v, object_safety_distance +. 0.01)) objects in
- let l =
- match planner.beacon with
- | Some v ->
- (v, beacon_safety_distance +. 0.01) :: l
- | None ->
- l
- in
- Krobot_pathfinding.find_path ~src ~dst
- ({ x = border_safety_distance;
- y = border_safety_distance},
- { x = world_width -. border_safety_distance;
- y = world_height -. border_safety_distance})
- l
+ Krobot_path.find ~src ~dst ~objects:planner.objects ~beacon:planner.beacon
(* +-----------------------------------------------------------------+
| Primitives |
+-----------------------------------------------------------------+ *)
-let set_vertices planner ?curves vertices =
+let set_vertices planner vertices =
+ let v = { vx = cos planner.orientation; vy = sin planner.orientation } in
+ let curves = List.rev (Bezier.fold_curves (fun curve acc -> curve :: acc) v (planner.position :: vertices) []) in
planner.vertices <- vertices;
- (* If the robot is not moving, add its current position to the
- trajectory. *)
- let vertices =
- match planner.moving with
- | true -> vertices
- | false -> planner.position :: vertices
- in
- (* Compute bezier curves if not provided. *)
- let curves =
- match curves with
- | Some l ->
- l
- | None ->
- let v = { vx = cos planner.orientation; vy = sin planner.orientation } in
- List.rev (Bezier.fold_vertices (fun sign p q r s acc -> (sign, p, q, r, s) :: acc) v vertices [])
- in
planner.curves <- curves;
- ignore (Krobot_bus.send planner.bus (Unix.gettimeofday (), Trajectory_vertices(vertices,
- List.map (fun (sign, p, q, r, s) -> (p, q, r, s)) curves)))
-
-let set_moving planner moving =
- planner.moving <- moving;
- ignore (Krobot_bus.send planner.bus (Unix.gettimeofday (), Trajectory_moving moving))
+ ignore (Krobot_bus.send planner.bus (Unix.gettimeofday (), Trajectory_path curves))
let simplify planner tolerance =
match planner.vertices with
@@ -154,215 +105,6 @@ let simplify planner tolerance =
let result = loop [0; Array.length points - 1] in
set_vertices planner (List.map (fun i -> points.(i)) result)
-let wait_done planner =
- lwt () = Lwt_log.info "waiting for the robot to stop moving" in
- lwt () = Lwt_unix.sleep 0.3 in
- lwt () =
- while_lwt planner.motors_moving do
- 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"
-
-(* Send a bezier curve to the robot. *)
-let send_curve planner sign p q r s v_end =
- (* Compute parameters. *)
- let d1 = sign *. 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))
-
-(* Remove the first vertice of the trajecotry. *)
-let drop_vertice planner =
- match planner.vertices with
- | [] ->
- ()
- | _ :: vertices ->
- set_vertices planner ~curves:(List.tl planner.curves) vertices
-
-(* Follow the given path, updating [current_curve] each time we follow
- a new curve. *)
-let follow_path planner current_curve path =
- let rec loop = function
- | [] ->
- lwt () = wait_done planner in
- set_vertices planner [];
- return ()
-
- | [(sign, p, q, r, s)] ->
- lwt () = wait_middle planner in
- lwt () = send_curve planner sign p q r s 0.01 in
- lwt () = wait_start planner in
- current_curve := Bezier.of_vertices p q r s;
- drop_vertice planner;
- lwt () = wait_done planner in
- set_vertices planner [];
- return ()
-
- | (sign, p, q, r, s) :: rest ->
- lwt () = wait_middle planner in
- lwt () = send_curve planner sign p q r s 0.5 in
- lwt () = wait_start planner in
- current_curve := Bezier.of_vertices p q r s;
- drop_vertice planner;
- loop rest
- in
-
- (* Add the origin of the trajectory to keep displaying it. *)
- planner.vertices <- planner.position :: path;
- match planner.curves with
- | [] ->
- set_vertices planner [];
- return ()
-
- | [(sign, p, q, r, s)] ->
- current_curve := Bezier.of_vertices p q r s;
- lwt () = send_curve planner sign p q r s 0.01 in
- lwt () = wait_done planner in
- set_vertices planner [];
- return ()
-
- | (sign, p, q, r, s) :: rest ->
- current_curve := Bezier.of_vertices p q r s;
- lwt () = send_curve planner sign p q r s 0.5 in
- loop rest
-
-let go planner rotation_speed rotation_acceleration moving_speed moving_acceleration =
- if planner.moving then
- return ()
- else begin
- set_moving planner true;
- planner.mover <- (
- try_lwt
- follow_path planner (ref (Bezier.of_vertices origin origin origin origin)) planner.vertices
-
- with exn ->
- Lwt_log.error_f ~section ~exn "failed to move"
-
- finally
- set_moving planner false;
- return ()
- );
- return ()
- end
-
-let abort planner =
- Krobot_message.send planner.bus (Unix.gettimeofday (), Motor_stop(1.0, 0.0))
-
-let seen_beacon = ref None
-
-let check planner curve =
- let rec loop i =
- if i = 256 then
- true
- else
- let v = Bezier.vertice curve (float i /. 255.) in
- if (List.for_all (fun obj -> distance v obj >= object_safety_distance) planner.objects
- && (match planner.beacon , !seen_beacon with
- | Some v',_ ->
- seen_beacon := Some (Unix.gettimeofday ());
- false
- (* distance v v' >= beacon_safety_distance *)
- | None, Some x when (Unix.gettimeofday ()) < 4. ->
- false
- | None,_ -> true)) then
- loop (i + 1)
- else
- false
- in
- loop planner.curve_status
-
-(* Go to the given destination. *)
-let goto planner dst =
- if planner.moving then
- return ()
- else begin
- set_moving planner true;
- planner.mover <- (
- try_lwt
- let rec loop () =
- if (match planner.beacon , !seen_beacon with
- | Some v',_ ->
- seen_beacon := Some (Unix.gettimeofday ());
- false
- (* distance v v' >= beacon_safety_distance *)
- | None, Some x when (Unix.gettimeofday ()) < 4. ->
- false
- | None,_ -> true)
- then
- (match find_path planner planner.position dst with
- | None ->
- ignore (Lwt_log.info ~section "cannot find a path to the destination");
- return ()
- | Some path ->
- set_vertices planner (planner.position :: path);
- let dummy = Bezier.of_vertices origin origin origin origin in
- let current_curve = ref dummy in
- match_lwt
- pick [
- (lwt () = follow_path planner current_curve path in return true);
- (lwt () =
- while_lwt !current_curve = dummy do
- Lwt_unix.sleep 0.01
- done
- in
- lwt () =
- while_lwt check planner !current_curve do
- Lwt_unix.sleep 0.01
- done
- in
- return false);
- ]
- with
- | true ->
- return ()
- | false ->
- ignore (Lwt_log.info ~section "aborting current trajectory");
- lwt () = abort planner in
- lwt () = Lwt_unix.sleep 1.0 in
- loop ())
- else
- lwt () = Lwt_unix.sleep 1.0 in
- loop ()
- in
- loop ()
- with exn ->
- Lwt_log.error_f ~section ~exn "failed to move"
-
- finally
- set_moving planner false;
- return ()
- );
- return ()
- end
-
(* +-----------------------------------------------------------------+
| Message handling |
+-----------------------------------------------------------------+ *)
@@ -375,12 +117,8 @@ let handle_message planner (timestamp, message) =
planner.position <- { x; y };
planner.orientation <- math_mod_float theta (2. *. pi)
- | Odometry_ghost(x, y, theta, u, following) ->
- planner.curve_status <- u;
- planner.motors_moving <- following
-
| Beacon_position(angle, distance, period) ->
- if distance <> 0. then
+ if distance <> 0. then
let angle = math_mod_float (planner.orientation +. Krobot_config.rotary_beacon_index_pos +. angle) (2. *. pi) in
planner.beacon <- Some{
x = planner.position.x +. distance *. cos angle;
@@ -397,49 +135,29 @@ let handle_message planner (timestamp, message) =
exit 0
| Send ->
- ignore (
- let ts = Unix.gettimeofday () in
- let vertices = if planner.moving then planner.vertices else planner.position :: planner.vertices in
- join [
- Krobot_bus.send planner.bus (ts, Trajectory_vertices(vertices, List.map (fun (sign, p, q, r, s) -> (p, q, r, s)) planner.curves));
- Krobot_bus.send planner.bus (ts, Trajectory_moving planner.moving);
- ]
- )
+ ignore (Krobot_bus.send planner.bus (Unix.gettimeofday (), Trajectory_path planner.curves))
| Trajectory_set_vertices l ->
- if not planner.moving then
- set_vertices planner l
+ set_vertices planner l
| Trajectory_add_vertice vertice ->
- if not planner.moving then
- set_vertices planner (planner.vertices @ [vertice])
+ set_vertices planner (planner.vertices @ [vertice])
| Trajectory_simplify tolerance ->
- if not planner.moving then
- simplify planner tolerance
-
- | Trajectory_go(rotation_speed, rotation_acceleration, moving_speed, moving_acceleration) ->
- if not planner.moving then
- ignore (go planner rotation_speed rotation_acceleration moving_speed moving_acceleration)
-
- | Trajectory_goto v ->
- if not planner.moving then
- ignore (goto planner v)
-
- | Trajectory_stop ->
- cancel planner.mover;
- set_moving planner false;
- set_vertices planner [];
- ignore (Krobot_message.send planner.bus (Unix.gettimeofday (), Motor_stop(1.0, 0.0)))
-
- | Trajectory_find_path ->
- if not planner.moving then begin
- match planner.vertices with
- | v :: _ ->
- set_vertices planner (match find_path planner planner.position v with Some p -> p | None -> [])
- | _ ->
- ()
- end
+ simplify planner tolerance
+
+ | Trajectory_go ->
+ let path = planner.vertices in
+ ignore (Krobot_bus.send planner.bus (Unix.gettimeofday (), Strategy_set [Krobot_action.Follow_path path]));
+ set_vertices planner []
+
+ | Trajectory_find_path -> begin
+ match planner.vertices with
+ | v :: _ ->
+ set_vertices planner (match find_path planner planner.position v with Some p -> p | None -> [])
+ | _ ->
+ ()
+ end
| Objects l ->
planner.objects <- l
@@ -485,15 +203,11 @@ lwt () =
bus;
vertices = [];
curves = [];
- moving = false;
- motors_moving = false;
- curve_status = 0;
- mover = return ();
+ following_path = false;
position = { x = 0.; y = 0. };
orientation = 0.;
objects = [];
beacon = None;
- event = E.never;
} in
(* Handle krobot message. *)
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index de695e0..743f320 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -25,15 +25,9 @@ type state = {
theta : float;
}
-type beacon = {
- xbeacon : float;
- ybeacon : float;
- valid : bool;
-}
-
type viewer = {
bus : Krobot_bus.t;
- (* The D-Bus message bus used by this viwer. *)
+ (* The bus used by this viwer. *)
ui : Krobot_viewer_ui.window;
(* The UI of the viewer. *)
@@ -47,14 +41,14 @@ type viewer = {
mutable ghost : state;
(* The state of the ghost. *)
- mutable beacon : beacon;
- (* The state of the beacon. *)
+ mutable beacon : vertice option;
+ (* The position of the beacon, if any. *)
- mutable vertices : vertice list;
- (* The current trajectory. *)
+ mutable planner_path : Bezier.curve list;
+ (* The path of the planner. *)
- mutable curves : (vertice * vertice * vertice * vertice) list;
- (* The current bezier curves of the trajectory. *)
+ mutable vm_path : Bezier.curve list option;
+ (* The path of the VM. *)
mutable motor_status : bool * bool * bool *bool;
(* Status of the four motor controller. *)
@@ -275,30 +269,43 @@ let draw viewer =
(viewer.state, 1.0)];
(* Draw the beacon *)
- if viewer.beacon.valid then begin
- Cairo.arc ctx viewer.beacon.xbeacon viewer.beacon.ybeacon 0.04 0. (2. *. pi);
- set_color ctx Purple;
- Cairo.fill ctx;
- Cairo.arc ctx viewer.beacon.xbeacon viewer.beacon.ybeacon 0.04 0. (2. *. pi);
- set_color ctx Black;
- Cairo.stroke ctx
+ begin
+ match viewer.beacon with
+ | Some v ->
+ Cairo.arc ctx v.x v.y 0.04 0. (2. *. pi);
+ set_color ctx Purple;
+ Cairo.fill ctx;
+ Cairo.arc ctx v.x v.y 0.04 0. (2. *. pi);
+ set_color ctx Black;
+ Cairo.stroke ctx
+ | None ->
+ ()
end;
+ (* Draw the path of the VM if any or the path of the planner if the
+ VM is not following a trajectory. *)
+ let path =
+ match viewer.vm_path with
+ | Some path -> path
+ | None -> viewer.planner_path
+ in
+
(* Draw points. *)
Cairo.set_source_rgb ctx 255. 255. 0.;
- (match viewer.vertices with
+ (match path with
| [] ->
()
- | o :: l ->
- Cairo.move_to ctx o.x o.y;
- List.iter (fun { x; y } -> Cairo.line_to ctx x y) l;
+ | curve :: curves ->
+ let src = Bezier.src curve and dst = Bezier.dst curve in
+ Cairo.move_to ctx src.x src.y;
+ Cairo.line_to ctx dst.x dst.y;
+ List.iter (fun curve -> let v = Bezier.dst curve in Cairo.line_to ctx v.x v.y) curves;
Cairo.stroke ctx);
(* Draw bezier curves. *)
Cairo.set_source_rgb ctx 255. 0. 255.;
List.iter
- (fun (p, q, r, s) ->
- let curve = Bezier.of_vertices p q r s in
+ (fun curve ->
let { x; y } = Bezier.vertice curve 0. in
Cairo.move_to ctx x y;
for i = 1 to 100 do
@@ -306,7 +313,7 @@ let draw viewer =
Cairo.line_to ctx x y
done;
Cairo.stroke ctx)
- viewer.curves;
+ path;
let ctx = Cairo_lablgtk.create viewer.ui#scene#misc#window in
Cairo.set_source_surface ctx surface 0. 0.;
@@ -389,14 +396,19 @@ let handle_message viewer (timestamp, message) =
end
| 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
- let y = viewer.state.pos.y +. distance *. sin (newangle) in
- let valid = distance <> 0. in
- let beacon = { xbeacon = x; ybeacon = y; valid; } in
+ let beacon =
+ if distance <> 0. then begin
+ let angle = math_mod_float (viewer.state.theta +. rotary_beacon_index_pos +. angle) (2. *. pi) in
+ Some {
+ x = viewer.state.pos.x +. distance *. cos angle;
+ y = viewer.state.pos.y +. distance *. sin angle;
+ }
+ end else
+ None
+ in
if beacon <> viewer.beacon then begin
viewer.beacon <- beacon;
- viewer.ui#beacon_status#set_text (if valid then "valid" else "-");
+ viewer.ui#beacon_status#set_text (if beacon = None then "-" else "valid");
viewer.ui#beacon_distance#set_text (string_of_float distance);
viewer.ui#beacon_angle#set_text (string_of_float angle);
viewer.ui#beacon_period#set_text (string_of_float period);
@@ -416,13 +428,19 @@ let handle_message viewer (timestamp, message) =
| Kill "viewer" ->
exit 0
- | Trajectory_vertices(vertices, curves) ->
- viewer.vertices <- vertices;
- viewer.curves <- curves;
+ | Trajectory_path curves ->
+ viewer.planner_path <- curves;
queue_draw viewer
- | Trajectory_moving moving ->
- viewer.ui#button_go#misc#set_sensitive (not moving)
+ | Strategy_path None ->
+ viewer.vm_path <- None;
+ viewer.ui#button_go#misc#set_sensitive true;
+ queue_draw viewer
+
+ | Strategy_path(Some curves) ->
+ viewer.vm_path <- Some curves;
+ viewer.ui#button_go#misc#set_sensitive false;
+ queue_draw viewer
| Log line ->
viewer.ui#logs#buffer#insert (line ^ "\n");
@@ -487,9 +505,9 @@ lwt () =
ui;
state = init;
ghost = init;
- beacon = { xbeacon = 1.; ybeacon = 1.; valid = false };
- vertices = [];
- curves = [];
+ beacon = None;
+ planner_path = [];
+ vm_path = None;
statusbar_context = ui#statusbar#new_context "";
motor_status = (false, false, false, false);
objects = [];
@@ -541,23 +559,16 @@ lwt () =
(ui#button_go#event#connect#button_release
(fun ev ->
if GdkEvent.Button.button ev = 1 then
- ignore (
- Krobot_bus.send bus
- (Unix.gettimeofday (),
- Trajectory_go(ui#rotation_speed#adjustment#value,
- ui#rotation_acceleration#adjustment#value,
- ui#moving_speed#adjustment#value,
- ui#moving_acceleration#adjustment#value))
- );
+ ignore (Krobot_bus.send bus (Unix.gettimeofday (), Trajectory_go));
false));
ignore
(ui#button_goto#event#connect#button_release
(fun ev ->
if GdkEvent.Button.button ev = 1 then begin
- match viewer.vertices with
- | _ :: v :: _ ->
- ignore (Krobot_bus.send bus (Unix.gettimeofday (), Trajectory_goto v))
+ match viewer.planner_path with
+ | curve :: _ ->
+ ignore (Krobot_bus.send bus (Unix.gettimeofday (), Strategy_set [Krobot_action.Goto(Bezier.dst curve)]))
| _ ->
()
end;
@@ -567,29 +578,21 @@ lwt () =
(ui#button_start_red#event#connect#button_release
(fun ev ->
if GdkEvent.Button.button ev = 1 then
- ignore (
- Krobot_message.send bus
- (Unix.gettimeofday (),
- Set_odometry(0.215 -. Krobot_config.robot_size /. 2. +. Krobot_config.wheels_position, 1.885, 0.))
- );
+ ignore (Krobot_bus.send bus (Unix.gettimeofday (), Strategy_set [Krobot_action.Reset_odometry `Red]));
false));
ignore
(ui#button_start_blue#event#connect#button_release
(fun ev ->
if GdkEvent.Button.button ev = 1 then
- ignore_result (
- Krobot_message.send bus
- (Unix.gettimeofday (),
- Set_odometry(2.77, 1.915, pi))
- );
+ ignore (Krobot_bus.send bus (Unix.gettimeofday (), Strategy_set [Krobot_action.Reset_odometry `Blue]));
false));
ignore
(ui#button_stop#event#connect#button_release
(fun ev ->
if GdkEvent.Button.button ev = 1 then
- ignore (Krobot_bus.send bus (Unix.gettimeofday (), Trajectory_stop));
+ ignore (Krobot_bus.send bus (Unix.gettimeofday (), Strategy_stop));
false));
ignore
diff --git a/info/control2011/src/tools/krobot_vm.ml b/info/control2011/src/tools/krobot_vm.ml
new file mode 100644
index 0000000..9d67145
--- /dev/null
+++ b/info/control2011/src/tools/krobot_vm.ml
@@ -0,0 +1,402 @@
+(*
+ * krobot_vm.ml
+ * ------------
+ * Copyright : (c) 2011, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(* The krobot virtual machine *)
+
+open Lwt
+open Lwt_react
+open Krobot_config
+open Krobot_geom
+open Krobot_bus
+open Krobot_message
+open Krobot_action
+
+(* +-----------------------------------------------------------------+
+ | Types |
+ +-----------------------------------------------------------------+ *)
+
+(* Type of robots. *)
+type robot = {
+ bus : Krobot_bus.t;
+ (* The bus used to communicate with the robot. *)
+
+ mutable strategy : Krobot_action.t list;
+ (* The current strategy. *)
+
+ mutable change_strategy : Krobot_action.t list option;
+ (* This is used the change the current strategy. *)
+
+ mutable append_strategy : Krobot_action.t list option;
+ (* This is used to append actions to the current strategy. *)
+
+ mutable path : Bezier.curve list option;
+ (* The path currently followed by the robot. *)
+
+ mutable position : vertice;
+ (* The position of the robot on the table. *)
+
+ mutable orientation : float;
+ (* The orientation of the robot. *)
+
+ mutable objects : vertice list;
+ (* Position of objects on the table. *)
+
+ mutable moving : bool;
+ (* Is the robot moving ? *)
+
+ mutable curve : Bezier.curve option;
+ (* The bezier curve currently being followed by the robot. *)
+
+ mutable curve_parameter : int;
+ (* The parameter of the bezier curve currently followed by the
+ robot. *)
+
+ mutable jack : bool;
+ (* Status of the jack. *)
+
+ mutable beacon : vertice option;
+ (* The detected position of the beacon, if any. *)
+
+ mutable date_seen_beacon : float;
+ (* Date at which the beacon has been seen. *)
+
+ mutable team : [ `Red | `Blue ];
+ (* The state of the team selector. *)
+}
+
+(* +-----------------------------------------------------------------+
+ | Message handling |
+ +-----------------------------------------------------------------+ *)
+
+let handle_message robot (timestamp, message) =
+ match message with
+ | CAN(_, frame) -> begin
+ match decode frame with
+ | Odometry(x, y, theta) ->
+ robot.position <- { x; y };
+ robot.orientation <- math_mod_float theta (2. *. pi)
+
+ | Odometry_ghost(x, y, theta, u, following) ->
+ robot.curve_parameter <- u;
+ robot.moving <- following
+
+ | Beacon_position(angle, distance, period) ->
+ if distance <> 0. then begin
+ robot.date_seen_beacon <- Unix.gettimeofday ();
+ let angle = math_mod_float (robot.orientation +. rotary_beacon_index_pos +. angle) (2. *. pi) in
+ robot.beacon <- Some{
+ x = robot.position.x +. distance *. cos angle;
+ y = robot.position.y +. distance *. sin angle;
+ }
+ end else
+ robot.beacon <- None
+
+ | Switch1_status(b, _, _, _, _, _, _, _) ->
+ robot.jack <- not b
+
+ | _ ->
+ ()
+ end
+
+ | Kill "vm" ->
+ exit 0
+
+ | Send ->
+ ignore (
+ let timestamp = Unix.gettimeofday () in
+ Krobot_bus.send robot.bus (timestamp, Strategy_path robot.path)
+ )
+
+ | Objects l ->
+ robot.objects <- l
+
+ | Strategy_append l -> begin
+ match robot.append_strategy with
+ | Some l' ->
+ robot.append_strategy <- Some(l' @ l)
+ | None ->
+ robot.append_strategy <- Some l
+ end
+
+ | Strategy_stop ->
+ robot.change_strategy <- Some [Stop]
+
+ | Strategy_set l ->
+ robot.append_strategy <- None;
+ robot.change_strategy <- Some(Stop :: l)
+
+ | _ ->
+ ()
+
+(* +-----------------------------------------------------------------+
+ | Helpers |
+ +-----------------------------------------------------------------+ *)
+
+let set_path robot path =
+ robot.path <- path;
+ ignore (Krobot_bus.send robot.bus (Unix.gettimeofday (), Strategy_path path))
+
+let reset robot =
+ robot.curve <- None;
+ set_path robot None
+
+(* +-----------------------------------------------------------------+
+ | Execution |
+ +-----------------------------------------------------------------+ *)
+
+(* Remove the leftest node from a tree of actions. *)
+let rec remove_leftest_node = function
+ | Node(Node _ :: _ as l) :: rest ->
+ Node(remove_leftest_node l) :: rest
+ | Node _ :: rest ->
+ rest
+ | l ->
+ l
+
+(* The effect triggered by the execution of the first action of a tree
+ of action. *)
+type effect =
+ | Wait
+ (* Wait a bit. *)
+ | Send of Krobot_message.t
+ (* Send a message. *)
+
+(* [exec robot actions] searches for the first action to execute in a
+ tree of actions and returns a new tree of actions and an effect. *)
+let rec exec robot actions =
+ match actions with
+ | [] ->
+ ([], Wait)
+ | Node [] :: rest ->
+ exec robot rest
+ | Node actions :: rest ->
+ let actions, effect = exec robot actions in
+ (Node actions :: rest, effect)
+ | Wait_for_jack state :: rest ->
+ if robot.jack = state then
+ exec robot rest
+ else
+ (actions, Wait)
+ | Wait_for_moving state :: rest ->
+ if robot.moving = state then
+ exec robot rest
+ else
+ (actions, Wait)
+ | Wait_for_odometry(test, value) :: rest ->
+ if (match test with
+ | `Eq -> robot.curve_parameter = value
+ | `Gt -> robot.curve_parameter > value
+ | `Ge -> robot.curve_parameter >= value
+ | `Lt -> robot.curve_parameter < value
+ | `Le -> robot.curve_parameter <= value) then
+ exec robot rest
+ else
+ (actions, Wait)
+ | Set_curve None :: rest ->
+ reset robot;
+ exec robot rest
+ | Set_curve(Some curve) :: rest ->
+ robot.curve <- Some curve;
+ exec robot rest
+ | Goto v :: rest -> begin
+ (* Try to find a path to the destination. *)
+ match Krobot_path.find ~src:robot.position ~dst:v ~objects:robot.objects ~beacon:robot.beacon with
+ | Some vertices ->
+ exec robot (Follow_path vertices :: rest)
+ | None ->
+ (* If not found, skip the command. *)
+ exec robot rest
+ end
+ | Follow_path vertices :: rest -> begin
+ (* Compute bezier curves. *)
+ let vector = { vx = cos robot.orientation; vy = sin robot.orientation } in
+ let curves = List.rev (Bezier.fold_vertices (fun sign p q r s acc -> (sign, p, q, r, s) :: acc) vector (robot.position :: vertices) []) in
+ (* Set the path. *)
+ set_path robot (Some (List.map (fun (sign, p, q, r, s) -> Bezier.of_vertices p q r s) curves));
+ (* Compute orders. *)
+ let rec loop = function
+ | [] ->
+ [Wait_for_moving false]
+ | [(sign, p, q, r, s)] ->
+ [
+ (* Wait for the odometry to reach the middle of the
+ current trajectory. *)
+ Wait_for_odometry(`Ge, 128);
+ (* Send the next bezier curve. *)
+ Bezier(sign, p, q, r, s, 0.01);
+ (* Wait for the odometry to start the new curve. *)
+ Wait_for_odometry(`Lt, 128);
+ (* Set the new curve. *)
+ Set_curve(Some(Bezier.of_vertices p q r s));
+ (* Wait for the end of the new curve. *)
+ Wait_for_moving false;
+ (* Remove the current curve. *)
+ Set_curve None;
+ ]
+ | (sign, p, q, r, s) :: rest ->
+ Wait_for_odometry(`Ge, 128)
+ :: Bezier(sign, p, q, r, s, 0.5)
+ :: Wait_for_odometry(`Lt, 128)
+ :: Set_curve(Some(Bezier.of_vertices p q r s))
+ :: loop rest
+ in
+ match curves with
+ | [] ->
+ exec robot rest
+ | [(sign, p, q, r, s)] ->
+ exec robot (Node [
+ Set_curve(Some(Bezier.of_vertices p q r s));
+ Bezier(sign, p, q, r, s, 0.01);
+ Wait_for_moving fals...
[truncated message content] |
|
From: Xavier L. <Ba...@us...> - 2011-06-11 21:24:26
|
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 86cafa05a128e93f4e8630479c9669bc495fb006 (commit)
from 1cc7805cf65d3898cc2005d08e2e839c5bbed921 (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 86cafa05a128e93f4e8630479c9669bc495fb006
Author: Xavier Lagorce <Xav...@cr...>
Date: Sat Jun 11 23:23:31 2011 +0200
[Controller_Motor_STM32/Propulsion_Drive] Changed controller parameters.
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/main.c b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/main.c
index 5ec825e..b52b689 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/main.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/main.c
@@ -18,7 +18,10 @@
#include "differential_drive.h"
#define WHEEL_RADIUS 0.049245
-#define SHAFT_WIDTH 0.150
+#define SHAFT_WIDTH 0.14320
+
+//#define WHEEL_RADIUS 0.0325
+//#define SHAFT_WIDTH 0.255
PROC_DEFINE_STACK(stack_ind, KERN_MINSTACKSIZE * 2);
@@ -55,27 +58,34 @@ static void init(void)
0.4, 0.7, 1.0, // Controller gains
0.005); // Sample period
// Common parameters
- params.encoder_gain = -2.0*M_PI/2000.0/15.0;
- params.G0 = 0.0145;
- params.tau = 0.015;
- params.k[0] = -3898.0;
- params.k[1] = -58.4696;
- params.l = -params.k[0];
- params.l0[0] = 0.0236;
- params.l0[1] = 3.9715;
+ params.encoder_gain = -2.0*M_PI/2000.0/15;
params.T = 0.005;
// Initialize right motor
+ params.G0 = 0.0146;
+ params.tau = 0.120;
+ params.k[0] = -2139.6;
+ params.k[1] = -193.9178;
+ params.l = -params.k[0];
+ params.l0[0] = 0.0408;
+ params.l0[1] = 0;
params.motor = MOTOR4;
params.encoder = ENCODER4;
mc_new_controller(¶ms, dd_get_right_wheel_generator(), CONTROLLER_MODE_NORMAL);
// Initialize left motor
+ params.G0 = 0.0146;
+ params.tau = 0.266;
+ params.k[0] = -4689.1;
+ params.k[1] = -506.4099;
+ params.l = -params.k[0];
+ params.l0[0] = 0.0630;
+ params.l0[1] = 0.0994;
params.motor = MOTOR3;
params.encoder = ENCODER3;
- params.encoder_gain = 2.0*M_PI/2000.0/15.0; // Left motor is reversed
+ params.encoder_gain = 2.0*M_PI/2000.0/15; // Left motor is reversed
mc_new_controller(¶ms, dd_get_left_wheel_generator(), CONTROLLER_MODE_NORMAL);
// Start odometry
- odometryInit(1e-3, WHEEL_RADIUS, SHAFT_WIDTH, 2.0*M_PI/2000.0/15.0, -2.0*M_PI/2000.0/15.0);
+ odometryInit(1e-3, WHEEL_RADIUS, SHAFT_WIDTH, 2.0*M_PI/2000.0/15, -2.0*M_PI/2000.0/15);
// Blink to say we are ready
for (uint8_t i=0; i < 5; i++) {
hooks/post-receive
--
krobot
|
|
From: Nicolas D. <Ba...@us...> - 2011-06-11 21:22:35
|
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 1cc7805cf65d3898cc2005d08e2e839c5bbed921 (commit)
from f5b33ba75fc0c0e19719a5a7d087a6e3c6a47495 (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 1cc7805cf65d3898cc2005d08e2e839c5bbed921
Author: Nicolas Dandrimont <Nic...@cr...>
Date: Sat Jun 11 23:21:16 2011 +0200
[control2011] New Blue initial position (the red one is still to be measured)
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index 6132da2..de695e0 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -581,7 +581,7 @@ lwt () =
ignore_result (
Krobot_message.send bus
(Unix.gettimeofday (),
- Set_odometry(Krobot_config.world_width -. (0.215 -. Krobot_config.robot_size /. 2. +. Krobot_config.wheels_position), 1.885, pi))
+ Set_odometry(2.77, 1.915, pi))
);
false));
hooks/post-receive
--
krobot
|
|
From: Jérémie D. <Ba...@us...> - 2011-06-11 18:35:55
|
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 f5b33ba75fc0c0e19719a5a7d087a6e3c6a47495 (commit)
from 8720b869e55af8181ec8093f7adbb23bbf33bfea (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 f5b33ba75fc0c0e19719a5a7d087a6e3c6a47495
Author: Jérémie Dimino <je...@di...>
Date: Sat Jun 11 19:51:01 2011 +0200
[vision] remove an executable
-----------------------------------------------------------------------
Changes:
diff --git a/info/vision/coupe2011/findBallse_edge b/info/vision/coupe2011/findBallse_edge
deleted file mode 100755
index 2fdd11f..0000000
Binary files a/info/vision/coupe2011/findBallse_edge and /dev/null differ
hooks/post-receive
--
krobot
|
|
From: Nicolas D. <Ba...@us...> - 2011-06-11 18:27: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 8720b869e55af8181ec8093f7adbb23bbf33bfea (commit)
from efa746efba990043a76fe4dfa5726a11fe35dc9b (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 8720b869e55af8181ec8093f7adbb23bbf33bfea
Author: Nicolas Dandrimont <Nic...@cr...>
Date: Sat Jun 11 19:26:07 2011 +0200
[Sensor_Actuator] Less annoying beeps
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator/battery_monitoring/battery_monitoring.c b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator/battery_monitoring/battery_monitoring.c
index de6250a..f82f81f 100644
--- a/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator/battery_monitoring/battery_monitoring.c
+++ b/elec/boards/Sensor_Actuator/Firmware/sensor_actuator/sensor_actuator/battery_monitoring/battery_monitoring.c
@@ -52,7 +52,10 @@ int get_battery_monitoring(battery_status *pkt1, battery_status *pkt2) {
static void NORETURN battery_monitoring_process(void) {
uint8_t battery_charge;
- uint8_t buzzer_state = 0;
+
+ int dead_cells = 0;
+ int dead_cell_counter = 0;
+ int low_charge_counter = 0;
uint8_t ch;
uint16_t value = 42;
@@ -67,8 +70,10 @@ static void NORETURN battery_monitoring_process(void) {
if (battery_state[ch] < 10*LOW_CHARGE_THRES && battery_charge > 1)
battery_charge = 1;
- else if (battery_state[ch] < 10*ABSENT_CELL_THRES && battery_charge > 0)
+ else if (battery_state[ch] < 10*ABSENT_CELL_THRES && battery_charge > 0) {
+ dead_cells++;
battery_charge = 0;
+ }
}
// Battery 2
@@ -78,8 +83,10 @@ static void NORETURN battery_monitoring_process(void) {
if (battery_state[4+ch] < 10*LOW_CHARGE_THRES && battery_charge > 1)
battery_charge = 1;
- else if (battery_state[4+ch] < 10*ABSENT_CELL_THRES && battery_charge > 0)
+ else if (battery_state[4+ch] < 10*ABSENT_CELL_THRES && battery_charge > 0) {
+ dead_cells++;
battery_charge = 0;
+ }
}
measure_flag = 1;
@@ -90,12 +97,23 @@ static void NORETURN battery_monitoring_process(void) {
}
else if (battery_charge == 1) {
// Low charge state
- set_buzzer(1);
+ if (low_charge_counter < 100) {
+ for (int j = 0; j < 4; j++) {
+ set_buzzer((j+1) % 2);
+ timer_delay(50);
+ }
+ low_charge_counter++;
+ } else {
+ set_buzzer(1);
+ }
}
- else if (battery_charge == 0) {
+ else if (battery_charge == 0 && dead_cell_counter < 2) {
// No cell, or dead cell, or bad connection
- set_buzzer(buzzer_state);
- buzzer_state = (buzzer_state == 0) ? 1 : 0;
+ for (int j = 0; j < 6; j++) {
+ set_buzzer((j+1) % 2);
+ timer_delay(50);
+ }
+ dead_cell_counter++;
}
hooks/post-receive
--
krobot
|
|
From: Justin <Ba...@us...> - 2011-06-09 18:55:57
|
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 efa746efba990043a76fe4dfa5726a11fe35dc9b (commit)
from 9a0595184057aabb2471e9817b87acd721d54393 (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 efa746efba990043a76fe4dfa5726a11fe35dc9b
Author: Justin <jus...@en...>
Date: Thu Jun 9 20:55:05 2011 +0200
Simplification of control
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/asservissement.c b/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/asservissement.c
index 7f6081a..b6703ed 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/asservissement.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/asservissement.c
@@ -52,7 +52,6 @@ void NORETURN control_process(void)
timer_setEvent(&timer_control_process);
enableMotors( MOTEUR4|MOTEUR2|MOTEUR3);
- //convertRadInImpulsions(desiredPositions, desiredImpulsions);
while(1) {
@@ -64,9 +63,9 @@ void NORETURN control_process(void)
if (desiredPositions[i]>(2*M_PI)) desiredPositions[i]=2*M_PI;
}
convertRadInImpulsions(desiredPositions, desiredImpulsions);*/
- //generateRampeMotor(MOTEUR4|MOTEUR2|MOTEUR3,-20*2*M_PI,10,5);
+ //generateRampeMotor(MOTEUR1,2*M_PI,10,5);
generateRampeRobotPlan();
- controlMotors(MOTEUR4|MOTEUR2|MOTEUR3);
+ controlMotors(MOTEUR4|MOTEUR2|MOTEUR3|MOTEUR2);
timer_waitEvent(&timer_control_process); // Wait until the end of counting
}
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/avoid.c b/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/avoid.c
index bc3efa8..70f79ca 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/avoid.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/avoid.c
@@ -63,8 +63,8 @@ void NORETURN avoid_process(void)
timer_add(&timer_avoid_process);// Start process timer
refreshSensors();
- if(sharp_sensors.state_trigger[7]==1) LED2_ON();
- else LED2_OFF();
+ //if(sharp_sensors.state_trigger[7]==1) LED2_ON();
+ //else LED2_OFF();
make_choice();
timer_waitEvent(&timer_avoid_process); // Wait until the end of counting
}
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/intelligence.c b/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/intelligence.c
index c63bdad..ffab3c2 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/intelligence.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/intelligence.c
@@ -99,7 +99,8 @@ void makePath(void) {
holonome.ymax = signe*0;
holonome.vitesse = 0.5;
holonome.acceleration=0.5;
- //ouvrir_pinces();
+ holonome.Tmax=2*M_PI;
+ ouvrir_pinces();
etape+=1;
break;
case 1://recul
@@ -107,11 +108,11 @@ void makePath(void) {
holonome.asserPosition=0;
holonome.xmax = 0;
holonome.ymax = signe*0;
- holonome.vitesse = 0.25;
+ holonome.vitesse = 0.5;
holonome.acceleration=0.5;
holonome.Tmax = 0;
- //ouvrir_pinces();
- etape+=1;
+ fermer_pinces();
+ etape=0;
break;
/*case 2://drift sur la droite
holonome.busy=1;
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/main.c b/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/main.c
index a2d7035..96e92e2 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/main.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/main.c
@@ -88,13 +88,12 @@ int main(void)
* utilization of all the processes (1 probe per second).
*/
-
-
+ //enableMotors(MOTEUR1);
while (1)
{
//monitor_report();
- /* if( getEncoderDirection(ENCODER3) == FORWARD_DIRECTION )
+ /*if( getEncoderDirection(ENCODER1) == FORWARD_DIRECTION )
{
if(status==0)
{
@@ -110,12 +109,11 @@ int main(void)
status=0;
}
LED4_OFF();
- }
- coder = getEncoderCount(ENCODER2);
- timer_delay(10);
- if ( getEncoderCount(ENCODER2)==65535) LED3_ON();
- //else LED3_OFF();
- timer_delay(10);*/
+ }*/
+ //coder = getEncoderCount(ENCODER2);
+ //timer_delay(10);
+ //if (abs(encoders_status[0].globalCounter)>10000) LED2_ON();
+ //else LED2_OFF();
timer_delay(1000);
}
hooks/post-receive
--
krobot
|
|
From: Justin <Ba...@us...> - 2011-06-09 16:01:35
|
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 9a0595184057aabb2471e9817b87acd721d54393 (commit)
via 459cf5121ff7e558ea17db5ffed41606e8283974 (commit)
from 0c87730d46b7f3cb3d288d7b1da1cb6cbefae993 (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 9a0595184057aabb2471e9817b87acd721d54393
Author: Justin <jus...@en...>
Date: Thu Jun 9 18:00:41 2011 +0200
Add modification to the control program
commit 459cf5121ff7e558ea17db5ffed41606e8283974
Author: Justin <jus...@en...>
Date: Thu Jun 9 15:50:44 2011 +0200
Delete futil lines
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/AX12.c b/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/AX12.c
index b94ee67..a4d3bd8 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/AX12.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/AX12.c
@@ -2,6 +2,7 @@
void AX12Init() {
+ timer_delay(1000);
ax1.address=(uint8_t) 1;
ax1.position=(uint16_t) DEFAULT_AX1_POSITION;
ax1.speed=(uint16_t) DEFAULT_SPEED;
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/CAN.c b/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/CAN.c
deleted file mode 100644
index e69de29..0000000
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/CAN.h b/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/CAN.h
deleted file mode 100644
index e69de29..0000000
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/asservissement.c b/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/asservissement.c
index f960c90..7f6081a 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/asservissement.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/asservissement.c
@@ -47,9 +47,7 @@ void controlInit(void) {
void NORETURN control_process(void)
{
Timer timer_control_process;
- int64_t desiredImpulsions[NUM_MOTORS];
- int i;
- float desiredPositions[NUM_MOTORS]={0,2*M_PI,2*M_PI,2*M_PI};
+
timer_setDelay(&timer_control_process, us_to_ticks((utime_t)(control_refresh)));
timer_setEvent(&timer_control_process);
@@ -438,10 +436,10 @@ void generateRampeRobotPlan() {
float W, thetaMax, vthetaMax,aW;
float xVirtuel, yVirtuel;
float Vx, Vy, theta;
- float Pvmax,tvmax,tmax;
+ float Pvmax,tvmax,tmax, Tvmax;
float T = control_refresh/1000000.0; //Défintion de la période d'échantillonnage
float t; //Défini le temps discret
- int N=0; //Nombre de points dans le vecteur de rampe
+ int N=0, NP, NW; //Nombre de points dans le vecteur de rampe
float t1, t2; //Temps pour les trapèzes
float desiredP, desiredT;
int signeT=1; //suivant la valeur de DTheta = variation de l'angle du robot
@@ -488,11 +486,12 @@ void generateRampeRobotPlan() {
xmax = holonome.xmax;
ymax = holonome.ymax;
vmax = holonome.vitesse;
-
+ a = holonome.acceleration;
+
thetaMax = holonome.Tmax;
vthetaMax = holonome.vTmax;
-
- a = holonome.acceleration;
+ aW = holonome.aT;
+
Xstart = holonome.Xstart;
Ystart = holonome.Ystart;
Tstart = holonome.Tstart;
@@ -500,13 +499,51 @@ void generateRampeRobotPlan() {
thetaRobot = holonome_odometry.T; //theta du robot dans le repère de la table
desiredP=sqrt((xmax-Xstart)*(xmax-Xstart)+(ymax-Ystart)*(ymax-Ystart));
- desiredT=thetaMax - Tstart;
+ desiredT=thetaMax-Tstart;
if(desiredT<0) {
signeT=-1;
desiredT = -desiredT;
}
+ //What do we do ? Follow the position or the angular position ?
+ //Evaluation du nombre de points dans chacun des cas
+ //In the case of position
+ tvmax = vmax/a;
+ Pvmax = a*tvmax*tvmax;
+ if(Pvmax>desiredP) {
+ //On génère un profil triangulaire
+ tmax = 2*sqrt(desiredP/a);
+ NP=(int)(floorf(tmax/T))+1;
+ if(NP*T < tmax) NP+=1; //On oublie forcément le point final
+ }
+ else {
+ //On génère un profil trapézoïdal
+ t1=vmax/a;
+ tmax = desiredP/vmax+t1;
+ t2 = tmax - t1;
+ NP=(int)(floorf(tmax/T))+1;
+ if(NP*T < tmax) NP+=1;
+ }
+ //In the case of angular position
+ tvmax = vthetaMax/aW;
+ Tvmax = aW*tvmax*tvmax;
+ if(Tvmax>desiredT) {
+ //On génère un profil triangulaire
+ tmax = 2*sqrt(desiredT/aW);
+ NW=(int)(floorf(tmax/T))+1;
+ if(NW*T < tmax) NW+=1; //On oublie forcément le point final
+ }
+ else {
+ //On génère un profil trapézoïdal
+ t1=vthetaMax/aW;
+ tmax = desiredT/vthetaMax+t1;
+ t2 = tmax - t1;
+ NW=(int)(floorf(tmax/T))+1;
+ if(NW*T < tmax) NW+=1;
+ }
+
+ if(NP>NW) {
//Calcul de la vitesse linéaire
//Choix du profil : triangulaire ou trapézoïdal
tvmax = vmax/a;
@@ -578,9 +615,7 @@ void generateRampeRobotPlan() {
}
}
- Vx = V*cos(theta-thetaRobot);
- Vy = V*sin(theta-thetaRobot);
-
+
//Calcul de la composante de rotation
//Choix du profil : triangulaire ou trapézoïdal
@@ -604,7 +639,7 @@ void generateRampeRobotPlan() {
}
else {
//On génère un profil trapézoïdal
- t1=tmax-desiredT/vthetaMax;
+ t1 = tmax-desiredT/vthetaMax;
t2 = tmax - t1;
t=k*T;
@@ -630,10 +665,127 @@ void generateRampeRobotPlan() {
}
}
+ }
+ else {
+ //Calcul de la vitesse angulaire
+ //Choix du profil : triangulaire ou trapézoïdal
+ tvmax = vthetaMax/aW;
+ Tvmax = aW*tvmax*tvmax;
+ if(Tvmax>desiredT) {
+ //On génère un profil triangulaire
+ tmax = 2*sqrt(desiredT/aW);
+ N=(int)(floorf(tmax/T))+1;
+ if(N*T < tmax) N+=1; //On oublie forcément le point final
+ //Définition des CI et CF
+ t=(float)(k)*T;
+
+ if(t<=tmax/2) {
+ //On est dans la première partie de la courbe
+ W=aW*t;
+ }
+ else {
+ if(t<=tmax) {
+ //On est dans la deuxième partie de la coube
+ W=-aW*t+aW*tmax;
+ }
+ else {
+ W=0;
+ if(obstacle==0) { // No obstacles around us
+ holonome.busy=0;
+ }
+ }
+ }
+ }
+ else {
+ //On génère un profil trapézoïdal
+ t1=vthetaMax/a;
+ tmax = desiredT/vthetaMax+t1;
+ t2 = tmax - t1;
+ N =(int)(floorf(tmax/T))+1;
+ if(N*T < tmax) N+=1;
+
+ t=k*T;
+
+ if(t<=t1) {
+ //On est dans la première partie de la courbe
+ W=aW*t;
+ }
+ else {
+ if(t<=t2) {
+ // On est dans la deuxième partie de la courbe
+ W=vthetaMax;
+ }
+ else {
+ if(t<=tmax) {
+ //On est dans la troisième partie de la coube
+ W=-aW*t+aW*tmax;
+ }
+ else {
+ W=0;
+ if(obstacle==0) {
+ holonome.busy=0;
+ }
+ }
+ }
+
+ }
+ }
+
+
+ //Calcul de la composante de translation
+ //Choix du profil : triangulaire ou trapézoïdal
+
+ if(vmax*tmax/2>desiredP) {
+ //On génère un profil triangulaire
+ a = desiredP*4/(tmax*tmax);
+ if(t<=tmax/2) {
+ //On est dans la première partie de la courbe
+ //Calcul de aW : accélération sur theta
+ V=a*t;
+ }
+ else {
+ if(t<=tmax) {
+ //On est dans la deuxième partie de la coube
+ V=-a*t+a*tmax;
+ }
+ else {
+ V=0;
+ }
+ }
+ }
+ else {
+ //On génère un profil trapézoïdal
+ t1 = tmax-desiredP/vmax;
+ t2 = tmax - t1;
+
+ t=k*T;
+ a = vmax/t1;
+ if(t<=t1) {
+ //On est dans la première partie de la courbe
+ V=a*t;
+ }
+ else {
+ if(t<=t2) {
+ // On est dans la deuxième partie de la courbe
+ V=vmax;
+ }
+ else {
+ if(t<=tmax) {
+ //On est dans la troisième partie de la coube
+ V=-a*t+a*tmax;
+ }
+ else {
+ V=0;
+ }
+ }
+
+ }
+ }
+ }
+ Vx = V*cos(theta-thetaRobot);
+ Vy = V*sin(theta-thetaRobot);
- // In the case of negative desiredT
- W = signeT*W;
-
+ W = signeT*W; // signe use the signe of desiredT
// If we want to correct our path
// Ajout des composantes de déviations
@@ -643,7 +795,9 @@ void generateRampeRobotPlan() {
//holonome.Pprevious = P;
//Vx += ((xVirtuel+Xstart-holonome_odometry.X)*cos(thetaRobot)-(yVirtuel+Ystart-holonome_odometry.Y)*sin(thetaRobot))/(5*T);
//Vy += ((yVirtuel+Ystart-holonome_odometry.Y)*cos(thetaRobot)+(xVirtuel+Xstart-holonome_odometry.X)*sin(thetaRobot))/(5*T);
-
+
+
+ //Evaluate motors speed
w[0] = (-Vy)/R-W*L1/R;
w[1] = (Vx*sqrt(3)/2+Vy/2)/R-W*L2/R;
w[2] = (-Vx*sqrt(3)/2+Vy/2)/R-W*L3/R;
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/can_monitor.c b/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/can_monitor.c
index 218dbb6..d1a0665 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/can_monitor.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/can_monitor.c
@@ -162,6 +162,7 @@ static void NORETURN canMonitorListen_process(void) {
case CAN_ADC_VALUES_2:
adc2_msg.d[0] = frame.data32[0];
adc2_msg.d[1] = frame.data32[1];
+ //if you want to test SHARP sensors
//if(adc2_msg.p.val1>2000) LED2_ON();
//else LED2_OFF();
break;
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/intelligence.c b/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/intelligence.c
index 359561d..c63bdad 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/intelligence.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Junior_Drive/controller_motor_stm32/intelligence.c
@@ -29,8 +29,8 @@ void intelligenceInit(void) {
holonome.ymax=-0;
holonome.vYmax=0.3;
holonome.ay=0.15;
- holonome.Tmax=M_PI;
- holonome.vTmax=5;
+ holonome.Tmax=2*M_PI;
+ holonome.vTmax=2;
holonome.aT=0.5;
holonome.asserPosition=0;
holonome.X=0;
@@ -97,7 +97,7 @@ void makePath(void) {
holonome.asserPosition=0;
holonome.xmax = -1.5;
holonome.ymax = signe*0;
- holonome.vitesse = 0.25;
+ holonome.vitesse = 0.5;
holonome.acceleration=0.5;
//ouvrir_pinces();
etape+=1;
hooks/post-receive
--
krobot
|
