Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 0140e38a6bc4281a82ece8fac326d852bcc9377c
|
From: Xavier L. <Ba...@us...> - 2013-04-18 12:13: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 0140e38a6bc4281a82ece8fac326d852bcc9377c (commit)
from e7d6c3175ae17c43e72df1fc2e7b56c1b9bfdff9 (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 0140e38a6bc4281a82ece8fac326d852bcc9377c
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu Apr 18 14:16:49 2013 +0200
[info/krobot-simulator] Only one application regrouping all simulators from now on.
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/_oasis b/info/control2011/_oasis
index be457fb..94c60bf 100644
--- a/info/control2011/_oasis
+++ b/info/control2011/_oasis
@@ -169,27 +169,6 @@ Executable "krobot-simulator"
MainIs: krobot_simulator.ml
BuildDepends: krobot, lwt.syntax
-Executable "krobot-hil-simulator"
- Path: src/tools
- Install: true
- CompiledObject: best
- MainIs: krobot_hil_simulator.ml
- BuildDepends: krobot, lwt.syntax
-
-Executable "krobot-mc-simulator"
- Path: src/tools
- Install: true
- CompiledObject: best
- MainIs: krobot_mc_simulator.ml
- BuildDepends: krobot, lwt.syntax
-
-Executable "krobot-io-simulator"
- Path: src/tools
- Install: true
- CompiledObject: best
- MainIs: krobot_io_simulator.ml
- BuildDepends: krobot, lwt.syntax
-
Executable "krobot-planner"
Path: src/tools
Install: true
diff --git a/info/control2011/_tags b/info/control2011/_tags
index 9af45e4..8609920 100644
--- a/info/control2011/_tags
+++ b/info/control2011/_tags
@@ -2,7 +2,7 @@
<src/interfaces/*.ml>: -syntax_camlp4o
# OASIS_START
-# DO NOT EDIT (digest: 44c1536155bea156ff4e324744cffda8)
+# DO NOT EDIT (digest: 9a283fb86531fb8de178da3ad5f872b1)
# Library krobot
"src/lib": include
<src/lib/krobot.{cma,cmxa}>: use_libkrobot
@@ -84,11 +84,6 @@
<src/tools/krobot_monitor.{native,byte}>: pkg_lwt.unix
<src/tools/krobot_monitor.{native,byte}>: pkg_lwt.syntax
<src/tools/krobot_monitor.{native,byte}>: pkg_lwt.react
-# Executable krobot-hil-simulator
-<src/tools/krobot_hil_simulator.{native,byte}>: use_krobot
-<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-hub
<src/tools/krobot_hub.{native,byte}>: pkg_lwt.unix
<src/tools/krobot_hub.{native,byte}>: pkg_lwt.syntax
@@ -192,11 +187,6 @@
<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
-# Executable krobot-mc-simulator
-<src/tools/krobot_mc_simulator.{native,byte}>: use_krobot
-<src/tools/krobot_mc_simulator.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_mc_simulator.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_mc_simulator.{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
@@ -260,11 +250,6 @@
<src/tools/krobot_homologation.{native,byte}>: pkg_lwt.unix
<src/tools/krobot_homologation.{native,byte}>: pkg_lwt.syntax
<src/tools/krobot_homologation.{native,byte}>: pkg_lwt.react
-# Executable krobot-io-simulator
-<src/tools/krobot_io_simulator.{native,byte}>: use_krobot
-<src/tools/krobot_io_simulator.{native,byte}>: pkg_lwt.unix
-<src/tools/krobot_io_simulator.{native,byte}>: pkg_lwt.syntax
-<src/tools/krobot_io_simulator.{native,byte}>: pkg_lwt.react
# Executable krobot-pet
<src/tools/krobot_pet.{native,byte}>: use_krobot
<src/tools/krobot_pet.{native,byte}>: pkg_lwt.unix
diff --git a/info/control2011/setup.ml b/info/control2011/setup.ml
index 2b5c6f0..e0cc01c 100644
--- a/info/control2011/setup.ml
+++ b/info/control2011/setup.ml
@@ -8,7 +8,7 @@
*)
(* OASIS_START *)
-(* DO NOT EDIT (digest: bf059af587069cdcfbcb6e820bb05831) *)
+(* DO NOT EDIT (digest: a04a83a7ce5a2eba466a7d0057270c5b) *)
(*
Regenerated by OASIS v0.2.0
Visit http://oasis.forge.ocamlcore.org for more information and
@@ -5375,36 +5375,6 @@ let setup_t =
});
Executable
({
- cs_name = "krobot-hil-simulator";
- 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_hil_simulator.ml";
- });
- Executable
- ({
cs_name = "krobot-hub";
cs_data = PropList.Data.create ();
cs_plugin_data = [];
@@ -5977,36 +5947,6 @@ let setup_t =
});
Executable
({
- cs_name = "krobot-mc-simulator";
- 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_mc_simulator.ml";
- });
- Executable
- ({
cs_name = "krobot-replay";
cs_data = PropList.Data.create ();
cs_plugin_data = [];
@@ -6226,36 +6166,6 @@ let setup_t =
});
Executable
({
- cs_name = "krobot-io-simulator";
- 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_io_simulator.ml";
- });
- Executable
- ({
cs_name = "krobot-pet";
cs_data = PropList.Data.create ();
cs_plugin_data = [];
diff --git a/info/control2011/src/tools/krobot_hil_simulator.ml b/info/control2011/src/tools/krobot_hil_simulator.ml
deleted file mode 100644
index 9fcf5cd..0000000
--- a/info/control2011/src/tools/krobot_hil_simulator.ml
+++ /dev/null
@@ -1,142 +0,0 @@
-(*
- * krobot_hil_simulator.ml
- * -----------------------
- * Copyright : (c) 2011, Jeremie Dimino <je...@di...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- *)
-
-(* Simulate the robot. *)
-
-open Lwt
-open Lwt_react
-open Krobot_config
-open Krobot_message
-open Krobot_geom
-
-(* +-----------------------------------------------------------------+
- | Types |
- +-----------------------------------------------------------------+ *)
-
-(* State of the robot. *)
-type state = {
- x : float;
- y : float;
- theta : float;
-}
-
-type internal_state = {
- theta_l : float;
- theta_r : float;
-}
-
-type command =
- | Idle
- (* The robot is doint nothing. *)
- | Speed of float * float
- (* [Speed(left_velocity, right_velocity)] *)
- | Turn of float * float
- (* [Turn(t_acc, velocity)] *)
- | Move of float * float
- (* [Move(t_acc, velocity)] *)
-
-(* Type of simulators. *)
-type simulator = {
- mutable state : state;
- (* The state of the robot. *)
- mutable internal_state : internal_state;
- (* The state of the wheels. *)
- mutable velocity_l : float;
- (* Velocity of the left motor (read from the CAN). *)
- mutable velocity_r : float;
- (* Velocity of the right motor (read from the CAN). *)
- mutable time : float;
- (* The current time. *)
-}
-
-(* +-----------------------------------------------------------------+
- | Entry point |
- +-----------------------------------------------------------------+ *)
-
-let print sim =
- Lwt_log.debug_f "
-time = %f
-state:
- x = %f
- y = %f
- theta = %f
-internal_state:
- theta_l = %f
- theta_r = %f
-velocities:
- left = %f
- right = %f
-"
- sim.time
- sim.state.x
- sim.state.y
- sim.state.theta
- sim.internal_state.theta_l
- sim.internal_state.theta_r
- sim.velocity_l
- sim.velocity_r
-
-lwt () =
- lwt bus = Krobot_bus.get () in
-
- let sim = {
- state = { x = 0.; y = 0.; theta = 0. };
- internal_state = { theta_l = 0.; theta_r = 0. };
- velocity_l = 0.;
- velocity_r = 0.;
- time = Unix.gettimeofday ();
- } in
-
- (* Handle commands. *)
- E.keep
- (E.map_s
- (fun (ts, msg) ->
- match msg with
- | Encoder_position_speed_3(pos, speed) ->
- sim.velocity_r <- speed;
- return ()
- | Encoder_position_speed_4(pos, speed) ->
- sim.velocity_l <- speed;
- return ()
- | Set_odometry(x, y, theta) ->
- sim.state <- { x; y; theta };
- return ()
- | _ ->
- return ())
- (Krobot_message.recv bus));
-
- lwt () = Krobot_message.send bus (Unix.gettimeofday (), Set_controller_mode true) in
-
- while_lwt true do
- let time = Unix.gettimeofday () in
- let delta = time -. sim.time in
- sim.time <- time;
-
- (* Sends the state of the robot. *)
- lwt () = Krobot_message.send bus (sim.time, Odometry(sim.state.x, sim.state.y, sim.state.theta)) in
-
- lwt () = print sim in
-
- let u1 = (sim.velocity_l +. sim.velocity_r) *. wheels_diameter /. 4.
- and u2 = (sim.velocity_l -. sim.velocity_r) *. wheels_diameter /. wheels_distance /. 2. in
- let dx = u1 *. (cos sim.state.theta)
- and dy = u1 *. (sin sim.state.theta)
- and dtheta = u2 in
- sim.state <- {
- x = sim.state.x +. dx *. delta;
- y = sim.state.y +. dy *. delta;
- theta = math_mod_float (sim.state.theta +. dtheta *. delta) (2. *. pi);
- };
- sim.internal_state <- {
- theta_l = sim.internal_state.theta_l +. delta *. (u1 *. 4. +. u2 *. wheels_distance) /. (2. *. wheels_diameter);
- theta_r = sim.internal_state.theta_r +. delta *. (u1 *. 4. -. u2 *. wheels_distance) /. (2. *. wheels_diameter);
- };
-
- Lwt_unix.sleep 0.01
- done
diff --git a/info/control2011/src/tools/krobot_io_simulator.ml b/info/control2011/src/tools/krobot_io_simulator.ml
deleted file mode 100644
index dbaf6e6..0000000
--- a/info/control2011/src/tools/krobot_io_simulator.ml
+++ /dev/null
@@ -1,149 +0,0 @@
-(*
- * krobot_io_simulator.ml
- * ----------------
- * Copyright : (c) 2013, Xavier Lagorce <Xav...@cr...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- *)
-
-(* Service simulating all inputs/outputs from the robot *)
-
-open Lwt
-open Lwt_react
-open Lwt_preemptive
-open Krobot_bus
-open Krobot_message
-open Krobot_geom
-
-let section = Lwt_log.Section.make "krobot(io_simulator)"
-
-(* +-----------------------------------------------------------------+
- | Types |
- +-----------------------------------------------------------------+ *)
-
-(* State of the robot. *)
-type state = {
- s_x : float;
- s_y : float;
- theta : float;
-}
-
-let robot = ref { s_x = 0.; s_y = 0.; theta = 0.}
-
-(* +-----------------------------------------------------------------+
- | read/send loop |
- +-----------------------------------------------------------------+ *)
-
-let dist_x x y theta obstacle =
- let y_int = y +. (obstacle -. x) *. (tan theta) in
- if (y_int > 0. && y_int < Krobot_config.world_height) then
- let d = (obstacle -. x) /. (cos theta) in
- if d >= 0. then
- Some d
- else
- None
- else
- None
-
-let dist_y x y theta obstacle =
- let x_int = x +. (obstacle -. y) /. (tan theta) in
- if (x_int > 0. && x_int < Krobot_config.world_width) then
- let d = (obstacle -. y) /. (sin theta) in
- if d >= 0. then
- Some d
- else
- None
- else
- None
-
-let min_border_distance x y theta =
- let l = match dist_x x y theta 0. with Some v -> [v] | None -> [] in
- let l = match dist_x x y theta Krobot_config.world_width with Some v -> v::l | None -> l in
- let l = match dist_y x y theta 0. with Some v -> v::l | None -> l in
- let l = match dist_y x y theta Krobot_config.world_height with Some v -> v::l | None -> l in
- match l with
- | [] -> assert false
- | l -> List.fold_left min max_float l
- | _ -> assert false
-
-let gen_data robot =
- let dim = Array.length Krobot_config.urg_angles in
- let l = ref [] in
- for i = 0 to dim - 1 do
- let angle = Krobot_config.urg_angles.(i) in
- let dist = min_border_distance robot.s_x robot.s_y (robot.theta +. angle) in
- let x = dist *. cos angle in
- let y = dist *. sin angle in
- l := {x;y} :: !l
- done;
- Array.of_list !l
-
-let loop bus =
- let rec aux () =
- let time = Unix.gettimeofday () in
- let msg = Urg (gen_data !robot) in
- lwt () = Krobot_bus.send bus (time, msg) in
- lwt () = Lwt_unix.sleep 0.01 in
- aux () in
- aux ()
-
-(* +-----------------------------------------------------------------+
- | Message handling |
- +-----------------------------------------------------------------+ *)
-
-let handle_message (timestamp, message) =
- match message with
- | Kill "io_simulator" ->
- exit 0
- | CAN(_, frame) -> begin
- match decode frame with
- | Odometry(x, y, theta) ->
- robot := { s_x = x; s_y = y; theta }
- | _ ->
- ()
- end
- | _ ->
- ()
-
-(* +-----------------------------------------------------------------+
- | Command-line arguments |
- +-----------------------------------------------------------------+ *)
-
-let fork = ref true
-
-let options = Arg.align [
- "-no-fork", Arg.Clear fork, " Run in foreground";
-]
-
-let usage = "\
-Usage: krobot-io-simulator [options]
-options are:"
-
-(* +-----------------------------------------------------------------+
- | Entry point |
- +-----------------------------------------------------------------+ *)
-
-lwt () =
- Arg.parse options ignore usage;
-
- (* Display all informative messages. *)
- Lwt_log.append_rule "*" Lwt_log.Info;
-
- (* Open the krobot bus. *)
- lwt bus = Krobot_bus.get () in
-
- (* Fork if not prevented. *)
- if !fork then Krobot_daemon.daemonize bus;
-
- (* Handle krobot message. *)
- E.keep (E.map (handle_message) (Krobot_bus.recv bus));
-
- (* Kill any running io_simulator. *)
- lwt () = Krobot_bus.send bus (Unix.gettimeofday (), Krobot_bus.Kill "io_simulator") in
-
- (* Wait a bit to let the other handler release the connection *)
- lwt () = Lwt_unix.sleep 0.4 in
-
- (* Loop forever. *)
- Lwt_unix.run (loop bus)
diff --git a/info/control2011/src/tools/krobot_mc_simulator.ml b/info/control2011/src/tools/krobot_mc_simulator.ml
deleted file mode 100644
index c307a52..0000000
--- a/info/control2011/src/tools/krobot_mc_simulator.ml
+++ /dev/null
@@ -1,538 +0,0 @@
-(*
- * krobot_simulator.ml
- * -----------------------
- * Copyright : (c) 2013, Xavier Lagorce <Xav...@cr...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- *)
-
-(* Simulate the robot. *)
-
-open Lwt
-open Lwt_react
-open Krobot_config
-open Krobot_bus
-open Krobot_message
-open Krobot_geom
-
-let section = Lwt_log.Section.make "krobot(simulator)"
-let time_step = 0.001
-
-(* +-----------------------------------------------------------------+
- | Command-line arguments |
- +-----------------------------------------------------------------+ *)
-
-let fork = ref true
-let hil = ref false
-
-let options = Arg.align [
- "-no-fork", Arg.Clear fork, " Run in foreground";
- "-hil", Arg.Set hil, " Run in hardware in the loop mode";
-]
-
-let usage = "\
-Usage: krobot-mc-simulator [options]
-options are:"
-
-(* +-----------------------------------------------------------------+
- | Types |
- +-----------------------------------------------------------------+ *)
-
-(* State of the robot. *)
-type state = {
- x : float;
- y : float;
- theta : float;
-}
-
-type internal_state = {
- theta_l : float;
- theta_r : float;
-}
-
-type command =
- | Idle
- (* The robot is doing nothing. *)
- | Speed of float * float
- (* [Speed(left_velocity, right_velocity)] *)
- | Turn of float * float
- (* [Turn(t_acc, velocity)] *)
- | Move of float * float
- (* [Move(t_acc, velocity)] *)
- | Bezier_cmd of (float * float * float) * bool
- (** [Motor_bezier_limits(v_max, a_tan_max, a_rad_max)] *)
-
-(* Type of simulators. *)
-type simulator = {
- mutable state : state;
- (* The state of the robot. *)
- mutable state_indep : state;
- (* The state of the robot for second set of encoders. *)
- mutable internal_state : internal_state;
- (* The state of the wheels. *)
- mutable velocity_l : float;
- (* Velocity of the left motor. *)
- mutable velocity_r : float;
- (* Velocity of the right motor. *)
- mutable ghost : state;
- (* The state of the ghost. *)
- mutable bezier_u : float;
- (* position on the Bezier's curve*)
- mutable bezier_curve : Bezier.curve option;
- (* Bezier's curve currently being followed if existing *)
- mutable bezier_next : (Bezier.curve * bool) option;
- (* Next Bezier's curve to follow *)
- mutable time : float;
- (* The current time. *)
- mutable command : command;
- (* The current command. *)
- mutable command_start : float;
- (* The start time of the current command. *)
- mutable command_end : float;
- (* The end time of the current command. *)
- mutable bezier_limits : float * float * float;
-}
-
-(* +-----------------------------------------------------------------+
- | Utility functions |
- +-----------------------------------------------------------------+ *)
-
-let print sim =
- Lwt_log.debug_f "
-time = %f
-state:
- x = %f
- y = %f
- theta = %f
-internal_state:
- theta_l = %f
- theta_r = %f
-velocities:
- left = %f
- right = %f
-"
- sim.time
- sim.state.x
- sim.state.y
- sim.state.theta
- sim.internal_state.theta_l
- sim.internal_state.theta_r
- sim.velocity_l
- sim.velocity_r
-
-
-(* +-----------------------------------------------------------------+
- | Trajectory generation |
- +-----------------------------------------------------------------+ *)
-
-let velocities sim dt =
- (* Put the robot into idle if the last command is terminated. *)
- (match sim.command with
- | Bezier_cmd (_,cur_dir) ->
- if sim.bezier_u >= 1. then begin
- match sim.bezier_next with
- | None ->
- sim.command <- Idle
- | Some (curve,dir) ->
- sim.command <- Bezier_cmd (sim.bezier_limits, cur_dir);
- sim.bezier_curve <- Some curve;
- sim.bezier_next <- None;
- sim.bezier_u <- 0.
- end
- | _ -> if sim.time > sim.command_end then sim.command <- Idle);
- match sim.command with
- | Idle ->
- (0., 0.)
- | Speed(l_v, r_v) ->
- ((l_v +. r_v) *. wheels_diameter /. 4., (l_v -. r_v) *. wheels_diameter /. wheels_distance)
- | Turn(t_acc, vel) ->
- if sim.time < (sim.command_start +. t_acc) then
- (0., vel *. (sim.time -. sim.command_start) /. t_acc)
- else if sim.time < (sim.command_end -. t_acc) then
- (0., vel)
- else
- (0., vel *. (sim.command_end -. sim.time) /. t_acc)
- | Move(t_acc, vel) ->
- if sim.time < (sim.command_start +. t_acc) then
- (vel *. (sim.time -. sim.command_start) /. t_acc, 0.)
- else if sim.time < (sim.command_end -. t_acc) then
- (vel, 0.)
- else
- (vel *. (sim.command_end -. sim.time) /. t_acc, 0.)
- | Bezier_cmd (limits, dir) ->
- (match sim.bezier_curve with
- | None ->
- sim.command <- Idle;
- (0., 0.)
- | Some curve ->
- let (v_max,_,a_r_max) = limits in
- let s' = norm (Bezier.dt curve sim.bezier_u) in
- let { vx = x'; vy = y' } = Bezier.dt curve sim.bezier_u in
- let { vx = x'';vy = y''} = Bezier.ddt curve sim.bezier_u in
- let theta' = ( y'' *. x' -. x'' *. y' ) /. ( x' *. x' +. y' *. y' ) in
- let cr = (x'*.x'+.y'*.y') ** 1.5 /. (x'*.y''-.y'*.x'') in
- let vel = min v_max (sqrt (a_r_max *. (abs_float cr))) in
- sim.bezier_u <- sim.bezier_u +. vel /. s' *. dt;
- if dir then
- (vel, theta' *. vel /. s')
- else
- (-.vel, theta' *. vel /. s'))
-
-let bezier sim (x_end, y_end, d1, d2, theta_end, v_end) =
- let p,theta_start = match sim.bezier_curve with
- | None ->
- {Krobot_geom.x = sim.state.x; Krobot_geom.y = sim.state.y},
- sim.state.theta
- | Some curve ->
- let _,_,r,s = Bezier.pqrs curve in
- s,
- (angle (vector r s))
- in
- let s = {Krobot_geom.x = x_end; Krobot_geom.y = y_end} in
- let q = translate p (vector_of_polar d1 theta_start) in
- let r = translate s (vector_of_polar (-.d2) theta_end) in
- let dir = d1 >= 0. in
- match sim.bezier_curve with
- | None ->
- sim.command <- Bezier_cmd (sim.bezier_limits,dir);
- sim.bezier_u <- 0.;
- sim.bezier_curve <- Some (Bezier.of_vertices p q r s);
- | Some _ ->
- sim.bezier_next <- Some ((Bezier.of_vertices p q r s),dir)
-
-let move sim distance velocity acceleration =
- if distance <> 0. && velocity > 0. && acceleration > 0. then begin
- let t_acc = velocity /. acceleration in
- let t_end = (velocity *. velocity +. distance *. acceleration) /. (velocity *. acceleration) in
- if t_end > 2. *. t_acc then begin
- if t_acc <> 0. then begin
- sim.command <- Move(t_acc, velocity);
- sim.command_start <- sim.time;
- sim.command_end <- sim.time +. t_end
- end
- end else begin
- if t_acc <> 0. then begin
- let t_acc = sqrt (abs_float (distance) /. acceleration) in
- let t_end = 2. *. t_acc in
- let sign = if distance >= 0. then 1. else -1. in
- let velocity = sign *. acceleration *. t_acc in
- sim.command <- Move(t_acc, velocity);
- sim.command_start <- sim.time;
- sim.command_end <- sim.time +. t_end
- end
- end
- end
-
-let turn sim angle velocity acceleration =
- if angle <> 0. && velocity > 0. && acceleration > 0. then begin
- let t_acc = velocity /. acceleration in
- let t_end = (velocity *. velocity +. angle *. acceleration) /. (velocity *. acceleration) in
- if t_end > 2. *. t_acc then begin
- if t_acc <> 0. then begin
- sim.command <- Turn(t_acc, velocity);
- sim.command_start <- sim.time;
- sim.command_end <- sim.time +. t_end
- end
- end else begin
- let t_acc = sqrt (abs_float (angle) /. acceleration) in
- let t_end = 2. *. t_acc in
- let sign = if angle >= 0. then 1. else -1. in
- let velocity = sign *. acceleration *. t_acc in
- if t_acc <> 0. then begin
- sim.command <- Turn(t_acc, velocity);
- sim.command_start <- sim.time;
- sim.command_end <- sim.time +. t_end
- end
- end
- end
-
-let set_velocities sim left_velocity right_velocity duration =
- sim.command <- Speed(left_velocity, right_velocity);
- sim.command_start <- sim.time;
- sim.command_end <- sim.time +. duration
-
-let get_velocities sim dt =
- let (u1, u2) = velocities sim dt in
- let l_v = (4. *. u1 +. wheels_distance *. u2) /. (2. *. wheels_diameter)
- and r_v = (4. *. u1 -. wheels_distance *. u2) /. (2. *. wheels_diameter) in
- (l_v, r_v)
-
-let get_state sim =
- sim.state
-
-let get_encoders sim =
- let (theta_l, theta_r) = (sim.internal_state.theta_l, sim.internal_state.theta_r) in
- (theta_l *. wheels_diameter /. 2., theta_r *. wheels_diameter /. 2.)
-
-
-(* +-----------------------------------------------------------------+
- | Main loops |
- +-----------------------------------------------------------------+ *)
-
-let sim = ref None
-
-let loop bus sim =
- let rec aux () =
- let time = Unix.gettimeofday () in
- let delta = time -. sim.time in
- sim.time <- time;
-
- lwt () = print sim in
-
- let u1, u2 = if !hil then
- (sim.velocity_l +. sim.velocity_r) *. wheels_diameter /. 4. ,
- (sim.velocity_l -. sim.velocity_r) *. wheels_diameter /. wheels_distance /. 2.
- else
- velocities sim delta
- in
- let dx = u1 *. (cos sim.state.theta)
- and dy = u1 *. (sin sim.state.theta)
- and dtheta = u2 in
- let theta = sim.state.theta +. dtheta *. delta in
- let theta =
- if theta > pi then
- theta -. 2. *. pi
- else if theta < -.pi then
- theta +. 2. *. pi
- else
- theta
- in
- sim.state <- {
- x = sim.state.x +. dx *. delta;
- y = sim.state.y +. dy *. delta;
- theta = theta;
- };
- sim.internal_state <- {
- theta_l = sim.internal_state.theta_l +. delta *. (u1 *. 4. +. u2 *. wheels_distance) /. (2. *. wheels_diameter);
- theta_r = sim.internal_state.theta_r +. delta *. (u1 *. 4. -. u2 *. wheels_distance) /. (2. *. wheels_diameter);
- };
- (match sim.command with
- | Bezier_cmd _ -> sim.ghost <- sim.state
- | _ -> ());
- lwt () = Lwt_unix.sleep time_step in
- aux () in
- aux ()
-
-let send_CAN_messages sim bus =
- let rec aux () =
- (* Sends the state of the robot. *)
- lwt () = Krobot_message.send bus (sim.time, Odometry(sim.state.x, sim.state.y, sim.state.theta)) in
- lwt () = Krobot_message.send bus (sim.time, Odometry_indep(sim.state.x, sim.state.y, sim.state.theta)) in
- (* Wait before next batch of packets (emulate the electronic board behavior) *)
- lwt () = Lwt_unix.sleep 0.005 in
- (* Sends the state of the ghost. *)
- lwt () = Krobot_message.send bus (sim.time,
- Odometry_ghost(sim.ghost.x,
- sim.ghost.y,
- sim.ghost.theta,
- int_of_float (255. *. sim.bezier_u),
- match sim.command with Bezier_cmd _ -> true | _ -> false)) in
- (* Sends the state of the motors. *)
- lwt () = match sim.command with
- | Turn(a, b) ->
- Krobot_message.send bus (Unix.gettimeofday (), Motor_status(false, true, false, false))
- | Move(a, b) ->
- Krobot_message.send bus (Unix.gettimeofday (), Motor_status(true, false, false, false))
- | Bezier_cmd _ ->
- (* Motor State to be verifyed on the real Motor Controller card *)
- Krobot_message.send bus (Unix.gettimeofday (), Motor_status(true, true, false, false))
- | _ ->
- Krobot_message.send bus (Unix.gettimeofday (), Motor_status(false, false, false, false)) in
- lwt () = Lwt_unix.sleep 0.005 in
- aux () in
- aux ()
-
-(* +-----------------------------------------------------------------+
- | Message handling |
- +-----------------------------------------------------------------+ *)
-
-let handle_message bus (timestamp, message) =
- match message with
- | Kill "mc_simulator" ->
- exit 0
- | CAN(_, frame) -> begin
- match !sim with
- | Some sim -> begin
- (* Generic messages *)
- (match decode frame with
- | Set_odometry(x, y, theta) ->
- sim.state <- { x; y; theta }
- | _ ->
- ());
- (* Messages related to HIL mode *)
- (if !hil then begin
- match decode frame with
- | Encoder_position_speed_3(pos, speed) ->
- sim.velocity_r <- speed
- | Encoder_position_speed_4(pos, speed) ->
- sim.velocity_l <- speed
- | _ ->
- () end
- (* Message related to full software simulation mode *)
- else begin
- match decode frame with
- | Motor_move(dist, speed, acc) ->
- Lwt_unix.run (Lwt_log.info_f "received: move(%f, %f, %f)" dist speed acc);
- move sim dist speed acc
- | Motor_turn(angle, speed, acc) ->
- Lwt_unix.run (Lwt_log.info_f "received: turn(%f, %f, %f)" angle speed acc);
- turn sim angle speed acc
- | Motor_stop(lin_acc, rot_acc) ->
- sim.command <- Idle;
- sim.bezier_curve <- None;
- sim.bezier_next <- None;
- | Set_odometry(x, y, theta) ->
- sim.state_indep <- { x; y; theta }
- | Set_odometry_indep(x, y, theta) ->
- sim.state <- { x; y; theta }
- | Motor_bezier_limits(v_max, a_tan_max, a_rad_max) ->
- sim.bezier_limits <- (v_max, a_tan_max, a_rad_max)
- | Motor_bezier(x_end, y_end, d1, d2, theta_end, v_end) ->
- Lwt_unix.run (Lwt_log.info_f "received: bezier(%f, %f, %f, %f, %f, %f)" x_end y_end d1 d2 theta_end v_end);
- bezier sim (x_end, y_end, d1, d2, theta_end, v_end)
- | _ ->
- () end);
- Lwt.return () end
- | None ->
- Lwt.return ()
- end
- | _ -> Lwt.return ()
-
-(* +-----------------------------------------------------------------+
- | Hokuyo emulation |
- +-----------------------------------------------------------------+ *)
-
-let dist_x x y theta obstacle =
- let y_int = y +. (obstacle -. x) *. (tan theta) in
- if (y_int > 0. && y_int < Krobot_config.world_height) then
- let d = (obstacle -. x) /. (cos theta) in
- if d >= 0. then
- Some d
- else
- None
- else
- None
-
-let dist_y x y theta obstacle =
- let x_int = x +. (obstacle -. y) /. (tan theta) in
- if (x_int > 0. && x_int < Krobot_config.world_width) then
- let d = (obstacle -. y) /. (sin theta) in
- if d >= 0. then
- Some d
- else
- None
- else
- None
-
-let min_border_distance x y theta =
- let l = match dist_x x y theta 0. with Some v -> [v] | None -> [] in
- let l = match dist_x x y theta Krobot_config.world_width with Some v -> v::l | None -> l in
- let l = match dist_y x y theta 0. with Some v -> v::l | None -> l in
- let l = match dist_y x y theta Krobot_config.world_height with Some v -> v::l | None -> l in
- match l with
- | [] -> None
- | l -> Some (List.fold_left min max_float l)
-
-(*let gen_data robot =
- let dim = Array.length Krobot_config.urg_angles in
- let {Krobot_geom.x=urg_rel_x;
- Krobot_geom.y=urg_rel_y } = Krobot_config.urg_position in
- let urg_pos = [| urg_rel_x; urg_rel_y; 1. |] in
- let rot = rot_mat robot.theta in
- let urg_pos = mult rot urg_pos in
- let pos = Krobot_geom.translate
- { Krobot_geom.x=robot.x; Krobot_geom.y=robot.y }
- { Krobot_geom.vx = urg_pos.(0); Krobot_geom.vy = urg_pos.(1) } in
- let {Krobot_geom.x=cen_x;
- Krobot_geom.y=cen_y } = pos in
- let l = ref [] in
- for i = 0 to dim - 1 do
- let angle = Krobot_config.urg_angles.(i) in
- match min_border_distance cen_x cen_y (robot.theta +. angle) with
- | Some dist ->
- let x = dist *. cos angle +. urg_pos.(0) in
- let y = dist *. sin angle +. urg_pos.(1) in
- l := {Krobot_geom.x;Krobot_geom.y} :: !l
- | None ->
- ()
- done;
- Array.of_list !l*)
-
-let gen_data robot =
- let dim = Array.length Krobot_config.urg_angles in
- let l = ref [] in
- for i = 0 to dim - 1 do
- let angle = Krobot_config.urg_angles.(i) in
- match min_border_distance robot.x robot.y (robot.theta +. angle) with
- | Some dist ->
- let x = dist *. cos angle in
- let y = dist *. sin angle in
- l := {Krobot_geom.x=x;Krobot_geom.y=y} :: !l
- | None ->
- ()
- done;
- Array.of_list !l
-
-let loop_urg sim bus =
- let rec aux () =
- let time = Unix.gettimeofday () in
- let msg = Urg (gen_data sim.state) in
- lwt () = Krobot_bus.send bus (time, msg) in
- lwt () = Lwt_unix.sleep 0.01 in
- aux () in
- aux ()
-
-(* +-----------------------------------------------------------------+
- | Entry point |
- +-----------------------------------------------------------------+ *)
-
-lwt () =
- Arg.parse options ignore usage;
-
- (* Display all informative messages. *)
- Lwt_log.append_rule "*" Lwt_log.Info;
-
- (* Open the krobot bus. *)
- lwt bus = Krobot_bus.get () in
-
- (* Fork if not prevented. *)
- if !fork then Krobot_daemon.daemonize bus;
-
- (* Handle krobot message. *)
- E.keep (E.map (handle_message bus) (Krobot_bus.recv bus));
-
- (* Kill any running mc_simulator. *)
- lwt () = Krobot_bus.send bus (Unix.gettimeofday (), Krobot_bus.Kill "mc_simulator") in
-
- (* Wait a bit to let the other handler release the connection *)
- lwt () = Lwt_unix.sleep 0.4 in
-
- (* Set the motor_controller card in HIL mode if necessary *)
- if !hil then
- ignore (Krobot_message.send bus (Unix.gettimeofday (), Set_controller_mode true)) ;
-
- (* Initial state of the simulator *)
- let local_sim = {
- state = { x = 0.; y = 0.; theta = 0. };
- state_indep = { x = 0.; y = 0.; theta = 0. };
- internal_state = { theta_l = 0.; theta_r = 0. };
- velocity_l = 0.;
- velocity_r = 0.;
- ghost = { x = 0.; y = 0.; theta = 0. };
- bezier_u = 0.;
- bezier_curve = None;
- bezier_next = None;
- command = Idle;
- command_start = 0.;
- command_end = 0.;
- time = Unix.gettimeofday ();
- bezier_limits = 1., 1., 1.;
- } in
- sim := Some local_sim;
-
- ignore(send_CAN_messages local_sim bus);
- ignore(loop_urg local_sim bus);
-
- (* Loop forever. *)
- Lwt_unix.run (loop bus local_sim)
diff --git a/info/control2011/src/tools/krobot_simulator.ml b/info/control2011/src/tools/krobot_simulator.ml
index f40885e..c307a52 100644
--- a/info/control2011/src/tools/krobot_simulator.ml
+++ b/info/control2011/src/tools/krobot_simulator.ml
@@ -1,7 +1,7 @@
(*
* krobot_simulator.ml
- * -------------------
- * Copyright : (c) 2011, Jeremie Dimino <je...@di...>
+ * -----------------------
+ * Copyright : (c) 2013, Xavier Lagorce <Xav...@cr...>
* Licence : BSD3
*
* This file is a part of [kro]bot.
@@ -12,9 +12,29 @@
open Lwt
open Lwt_react
open Krobot_config
+open Krobot_bus
open Krobot_message
open Krobot_geom
+let section = Lwt_log.Section.make "krobot(simulator)"
+let time_step = 0.001
+
+(* +-----------------------------------------------------------------+
+ | Command-line arguments |
+ +-----------------------------------------------------------------+ *)
+
+let fork = ref true
+let hil = ref false
+
+let options = Arg.align [
+ "-no-fork", Arg.Clear fork, " Run in foreground";
+ "-hil", Arg.Set hil, " Run in hardware in the loop mode";
+]
+
+let usage = "\
+Usage: krobot-mc-simulator [options]
+options are:"
+
(* +-----------------------------------------------------------------+
| Types |
+-----------------------------------------------------------------+ *)
@@ -33,72 +53,153 @@ type internal_state = {
type command =
| Idle
- (* The robot is doint nothing. *)
+ (* The robot is doing nothing. *)
| Speed of float * float
(* [Speed(left_velocity, right_velocity)] *)
| Turn of float * float
(* [Turn(t_acc, velocity)] *)
| Move of float * float
(* [Move(t_acc, velocity)] *)
- | Bezier of
- ( float * float * float * float * float * float ) *
- ( float * float * float )
- (** [Motor_bezier(x_end, y_end, d1, d2, theta_end, v_end)]
- [Motor_bezier_limits(v_max, a_tan_max, a_rad_max)] *)
+ | Bezier_cmd of (float * float * float) * bool
+ (** [Motor_bezier_limits(v_max, a_tan_max, a_rad_max)] *)
(* Type of simulators. *)
type simulator = {
mutable state : state;
(* The state of the robot. *)
+ mutable state_indep : state;
+ (* The state of the robot for second set of encoders. *)
mutable internal_state : internal_state;
(* The state of the wheels. *)
+ mutable velocity_l : float;
+ (* Velocity of the left motor. *)
+ mutable velocity_r : float;
+ (* Velocity of the right motor. *)
+ mutable ghost : state;
+ (* The state of the ghost. *)
+ mutable bezier_u : float;
+ (* position on the Bezier's curve*)
+ mutable bezier_curve : Bezier.curve option;
+ (* Bezier's curve currently being followed if existing *)
+ mutable bezier_next : (Bezier.curve * bool) option;
+ (* Next Bezier's curve to follow *)
+ mutable time : float;
+ (* The current time. *)
mutable command : command;
(* The current command. *)
mutable command_start : float;
(* The start time of the current command. *)
mutable command_end : float;
(* The end time of the current command. *)
- mutable time : float;
- (* The current time. *)
mutable bezier_limits : float * float * float;
}
(* +-----------------------------------------------------------------+
- | Simulation |
+ | Utility functions |
+-----------------------------------------------------------------+ *)
-let sim_step = 1e-3
+let print sim =
+ Lwt_log.debug_f "
+time = %f
+state:
+ x = %f
+ y = %f
+ theta = %f
+internal_state:
+ theta_l = %f
+ theta_r = %f
+velocities:
+ left = %f
+ right = %f
+"
+ sim.time
+ sim.state.x
+ sim.state.y
+ sim.state.theta
+ sim.internal_state.theta_l
+ sim.internal_state.theta_r
+ sim.velocity_l
+ sim.velocity_r
+
-let velocities sim =
+(* +-----------------------------------------------------------------+
+ | Trajectory generation |
+ +-----------------------------------------------------------------+ *)
+
+let velocities sim dt =
+ (* Put the robot into idle if the last command is terminated. *)
+ (match sim.command with
+ | Bezier_cmd (_,cur_dir) ->
+ if sim.bezier_u >= 1. then begin
+ match sim.bezier_next with
+ | None ->
+ sim.command <- Idle
+ | Some (curve,dir) ->
+ sim.command <- Bezier_cmd (sim.bezier_limits, cur_dir);
+ sim.bezier_curve <- Some curve;
+ sim.bezier_next <- None;
+ sim.bezier_u <- 0.
+ end
+ | _ -> if sim.time > sim.command_end then sim.command <- Idle);
match sim.command with
| Idle ->
- (0., 0.)
+ (0., 0.)
| Speed(l_v, r_v) ->
- ((l_v +. r_v) *. wheels_diameter /. 4., (l_v -. r_v) *. wheels_diameter /. wheels_distance)
+ ((l_v +. r_v) *. wheels_diameter /. 4., (l_v -. r_v) *. wheels_diameter /. wheels_distance)
| Turn(t_acc, vel) ->
- if sim.time < (sim.command_start +. t_acc) then
- (0., vel *. (sim.time -. sim.command_start) /. t_acc)
- else if sim.time < (sim.command_end -. t_acc) then
- (0., vel)
- else
- (0., vel *. (sim.command_end -. sim.time) /. t_acc)
+ if sim.time < (sim.command_start +. t_acc) then
+ (0., vel *. (sim.time -. sim.command_start) /. t_acc)
+ else if sim.time < (sim.command_end -. t_acc) then
+ (0., vel)
+ else
+ (0., vel *. (sim.command_end -. sim.time) /. t_acc)
| Move(t_acc, vel) ->
- if sim.time < (sim.command_start +. t_acc) then
- (vel *. (sim.time -. sim.command_start) /. t_acc, 0.)
- else if sim.time < (sim.command_end -. t_acc) then
- (vel, 0.)
- else
- (vel *. (sim.command_end -. sim.time) /. t_acc, 0.)
- | Bezier(_,_) -> failwith "todo"
+ if sim.time < (sim.command_start +. t_acc) then
+ (vel *. (sim.time -. sim.command_start) /. t_acc, 0.)
+ else if sim.time < (sim.command_end -. t_acc) then
+ (vel, 0.)
+ else
+ (vel *. (sim.command_end -. sim.time) /. t_acc, 0.)
+ | Bezier_cmd (limits, dir) ->
+ (match sim.bezier_curve with
+ | None ->
+ sim.command <- Idle;
+ (0., 0.)
+ | Some curve ->
+ let (v_max,_,a_r_max) = limits in
+ let s' = norm (Bezier.dt curve sim.bezier_u) in
+ let { vx = x'; vy = y' } = Bezier.dt curve sim.bezier_u in
+ let { vx = x'';vy = y''} = Bezier.ddt curve sim.bezier_u in
+ let theta' = ( y'' *. x' -. x'' *. y' ) /. ( x' *. x' +. y' *. y' ) in
+ let cr = (x'*.x'+.y'*.y') ** 1.5 /. (x'*.y''-.y'*.x'') in
+ let vel = min v_max (sqrt (a_r_max *. (abs_float cr))) in
+ sim.bezier_u <- sim.bezier_u +. vel /. s' *. dt;
+ if dir then
+ (vel, theta' *. vel /. s')
+ else
+ (-.vel, theta' *. vel /. s'))
let bezier sim (x_end, y_end, d1, d2, theta_end, v_end) =
- assert false
-(*
- sim.command <- Bezier((x_end, y_end, d1, d2, theta_end, v_end),
- sim.bezier_limits);
- sim.command_start <- sim.time;
- sim.command_end <- sim.time +. t_end
-*)
+ let p,theta_start = match sim.bezier_curve with
+ | None ->
+ {Krobot_geom.x = sim.state.x; Krobot_geom.y = sim.state.y},
+ sim.state.theta
+ | Some curve ->
+ let _,_,r,s = Bezier.pqrs curve in
+ s,
+ (angle (vector r s))
+ in
+ let s = {Krobot_geom.x = x_end; Krobot_geom.y = y_end} in
+ let q = translate p (vector_of_polar d1 theta_start) in
+ let r = translate s (vector_of_polar (-.d2) theta_end) in
+ let dir = d1 >= 0. in
+ match sim.bezier_curve with
+ | None ->
+ sim.command <- Bezier_cmd (sim.bezier_limits,dir);
+ sim.bezier_u <- 0.;
+ sim.bezier_curve <- Some (Bezier.of_vertices p q r s);
+ | Some _ ->
+ sim.bezier_next <- Some ((Bezier.of_vertices p q r s),dir)
let move sim distance velocity acceleration =
if distance <> 0. && velocity > 0. && acceleration > 0. then begin
@@ -151,8 +252,8 @@ let set_velocities sim left_velocity right_velocity duration =
sim.command_start <- sim.time;
sim.command_end <- sim.time +. duration
-let get_velocities sim =
- let (u1, u2) = velocities sim in
+let get_velocities sim dt =
+ let (u1, u2) = velocities sim dt in
let l_v = (4. *. u1 +. wheels_distance *. u2) /. (2. *. wheels_diameter)
and r_v = (4. *. u1 -. wheels_distance *. u2) /. (2. *. wheels_diameter) in
(l_v, r_v)
@@ -164,138 +265,274 @@ let get_encoders sim =
let (theta_l, theta_r) = (sim.internal_state.theta_l, sim.internal_state.theta_r) in
(theta_l *. wheels_diameter /. 2., theta_r *. wheels_diameter /. 2.)
+
(* +-----------------------------------------------------------------+
- | Entry point |
+ | Main loops |
+-----------------------------------------------------------------+ *)
-let print sim =
- Lwt_log.debug_f "
-time = %f
-state:
- x = %f
- y = %f
- theta = %f
-internal_state:
- theta_l = %f
- theta_r = %f
-command:
- start = %f
- end = %f
- kind = %s
-"
- sim.time
- sim.state.x
- sim.state.y
- sim.state.theta
- sim.internal_state.theta_l
- sim.internal_state.theta_r
- sim.command_start
- sim.command_end
- (match sim.command with
- | Idle ->
- "Idle"
- | Speed(l, r) ->
- Printf.sprintf "Speed(%f, %f)" l r
- | Move(t, a) ->
- Printf.sprintf "Move(%f, %f)" t a
- | Turn(t, a) ->
- Printf.sprintf "Turn(%f, %f)" t a
- | Bezier((x_end, y_end, d1, d2, theta_end, v_end), (v_max, a_tan_max, a_rad_max)) ->
- Printf.sprintf "Bezier((%f, %f, %f, %f, %f, %f),(%f, %f, %f))"
- x_end y_end d1 d2 theta_end v_end v_max a_tan_max a_rad_max)
-
-lwt () =
- lwt bus = Krobot_bus.get () in
+let sim = ref None
- let sim = {
- state = { x = 0.; y = 0.; theta = 0. };
- internal_state = { theta_l = 0.; theta_r = 0. };
- command = Idle;
- command_start = 0.;
- command_end = 0.;
- time = Unix.gettimeofday ();
- bezier_limits = 1., 1., 1.;
- } in
-
- (* Handle commands. *)
- E.keep
- (E.map_s
- (fun (ts, msg) ->
- match msg with
- | Motor_move(dist, speed, acc) ->
- lwt () = Lwt_log.info_f "received: move(%f, %f, %f)" dist speed acc in
- move sim dist speed acc;
- return ()
- | Motor_turn(angle, speed, acc) ->
- lwt () = Lwt_log.info_f "received: turn(%f, %f, %f)" angle speed acc in
- turn sim angle speed acc;
- return ()
- | Motor_stop(lin_acc, rot_acc) ->
- sim.command <- Idle;
- return ()
- | Req_motor_status ->
- begin
- match sim.command with
- | Turn(a, b) ->
- Krobot_message.send bus (Unix.gettimeofday (), Motor_status(false, true, false, false))
- | Move(a, b) ->
- Krobot_message.send bus (Unix.gettimeofday (), Motor_status(true, false, false, false))
- | _ ->
- Krobot_message.send bus (Unix.gettimeofday (), Motor_status(false, false, false, false))
- end
- | Set_odometry(x, y, theta) ->
- sim.state <- { x; y; theta };
- return ()
- | Set_odometry_indep(x, y, theta) ->
- sim.state <- { x; y; theta };
- return ()
- | Motor_bezier_limits(v_max, a_tan_max, a_rad_max) ->
- sim.bezier_limits <- (v_max, a_tan_max, a_rad_max);
- return ()
- | Motor_bezier(x_end, y_end, d1, d2, theta_end, v_end) ->
- bezier sim (x_end, y_end, d1, d2, theta_end, v_end)
- | _ ->
- return ())
- (Krobot_message.recv bus));
-
- while_lwt true do
+let loop bus sim =
+ let rec aux () =
let time = Unix.gettimeofday () in
let delta = time -. sim.time in
sim.time <- time;
- (* Put the robot into idle if the last command is terminated. *)
- if sim.time > sim.command_end then sim.command <- Idle;
-
- (* Sends the state of the robot. *)
- lwt () = Krobot_message.send bus (sim.time, Odometry(sim.state.x, sim.state.y, sim.state.theta)) in
- lwt () = Krobot_message.send bus (sim.time, Odometry_indep(sim.state.x, sim.state.y, sim.state.theta)) in
-
- (* Sends the state of the motors. *)
- lwt () =
- match sim.command with
- | Turn(a, b) ->
- Krobot_message.send bus (Unix.gettimeofday (), Motor_status(false, true, false, false))
- | Move(a, b) ->
- Krobot_message.send bus (Unix.gettimeofday (), Motor_status(true, false, false, false))
- | _ ->
- Krobot_message.send bus (Unix.gettimeofday (), Motor_status(false, false, false, false))
- in
-
lwt () = print sim in
- let (u1, u2) = velocities sim in
+ let u1, u2 = if !hil then
+ (sim.velocity_l +. sim.velocity_r) *. wheels_diameter /. 4. ,
+ (sim.velocity_l -. sim.velocity_r) *. wheels_diameter /. wheels_distance /. 2.
+ else
+ velocities sim delta
+ in
let dx = u1 *. (cos sim.state.theta)
and dy = u1 *. (sin sim.state.theta)
and dtheta = u2 in
+ let theta = sim.state.theta +. dtheta *. delta in
+ let theta =
+ if theta > pi then
+ theta -. 2. *. pi
+ else if theta < -.pi then
+ theta +. 2. *. pi
+ else
+ theta
+ in
sim.state <- {
x = sim.state.x +. dx *. delta;
y = sim.state.y +. dy *. delta;
- theta = math_mod_float (sim.state.theta +. dtheta *. delta) (2. *. pi);
+ theta = theta;
};
sim.internal_state <- {
theta_l = sim.internal_state.theta_l +. delta *. (u1 *. 4. +. u2 *. wheels_distance) /. (2. *. wheels_diameter);
theta_r = sim.internal_state.theta_r +. delta *. (u1 *. 4. -. u2 *. wheels_distance) /. (2. *. wheels_diameter);
};
+ (match sim.command with
+ | Bezier_cmd _ -> sim.ghost <- sim.state
+ | _ -> ());
+ lwt () = Lwt_unix.sleep time_step in
+ aux () in
+ aux ()
+
+let send_CAN_messages sim bus =
+ let rec aux () =
+ (* Sends the state of the robot. *)
+ lwt () = Krobot_message.send bus (sim.time, Odometry(sim.state.x, sim.state.y, sim.state.theta)) in
+ lwt () = Krobot_message.send bus (sim.time, Odometry_indep(sim.state.x, sim.state.y, sim.state.theta)) in
+ (* Wait before next batch of packets (emulate the electronic board behavior) *)
+ lwt () = Lwt_unix.sleep 0.005 in
+ (* Sends the state of the ghost. *)
+ lwt () = Krobot_message.send bus (sim.time,
+ Odometry_ghost(sim.ghost.x,
+ sim.ghost.y,
+ sim.ghost.theta,
+ int_of_float (255. *. sim.bezier_u),
+ match sim.command with Bezier_cmd _ -> true | _ -> false)) in
+ (* Sends the state of the motors. *)
+ lwt () = match sim.command with
+ | Turn(a, b) ->
+ Krobot_message.send bus (Unix.gettimeofday (), Motor_status(false, true, false, false))
+ | Move(a, b) ->
+ Krobot_message.send bus (Unix.gettimeofday (), Motor_status(true, false, false, false))
+ | Bezier_cmd _ ->
+ (* Motor State to be verifyed on the real Motor Controller card *)
+ Krobot_message.send bus (Unix.gettimeofday (), Motor_status(true, true, false, false))
+ | _ ->
+ Krobot_message.send bus (Unix.gettimeofday (), Motor_status(false, false, false, false)) in
+ lwt () = Lwt_unix.sleep 0.005 in
+ aux () in
+ aux ()
+
+(* +-----------------------------------------------------------------+
+ | Message handling |
+ +-----------------------------------------------------------------+ *)
+
+let handle_message bus (timestamp, message) =
+ match message with
+ | Kill "mc_simulator" ->
+ exit 0
+ | CAN(_, frame) -> begin
+ match !sim with
+ | Some sim -> begin
+ (* Generic messages *)
+ (match decode frame with
+ | Set_odometry(x, y, theta) ->
+ sim.state <- { x; y; theta }
+ | _ ->
+ ());
+ (* Messages related to HIL mode *)
+ (if !hil then begin
+ match decode frame with
+ | Encoder_position_speed_3(pos, speed) ->
+ sim.velocity_r <- speed
+ | Encoder_position_speed_4(pos, speed) ->
+ sim.velocity_l <- speed
+ | _ ->
+ () end
+ (* Message related to full software simulation mode *)
+ else begin
+ match decode frame with
+ | Motor_move(dist, speed, acc) ->
+ Lwt_unix.run (Lwt_log.info_f "received: move(%f, %f, %f)" dist speed acc);
+ move sim dist speed acc
+ | Motor_turn(angle, speed, acc) ->
+ Lwt_unix.run (Lwt_log.info_f "received: turn(%f, %f, %f)" angle speed acc);
+ turn sim angle speed acc
+ | Motor_stop(lin_acc, rot_acc) ->
+ sim.command <- Idle;
+ sim.bezier_curve <- None;
+ sim.bezier_next <- None;
+ | Set_odometry(x, y, theta) ->
+ sim.state_indep <- { x; y; theta }
+ | Set_odometry_indep(x, y, theta) ->
+ sim.state <- { x; y; theta }
+ | Motor_bezier_limits(v_max, a_tan_max, a_rad_max) ->
+ sim.bezier_limits <- (v_max, a_tan_max, a_rad_max)
+ | Motor_bezier(x_end, y_end, d1, d2, theta_end, v_end) ->
+ Lwt_unix.run (Lwt_log.info_f "received: bezier(%f, %f, %f, %f, %f, %f)" x_end y_end d1 d2 theta_end v_end);
+ bezier sim (x_end, y_end, d1, d2, theta_end, v_end)
+ | _ ->
+ () end);
+ Lwt.return () end
+ | None ->
+ Lwt.return ()
+ end
+ | _ -> Lwt.return ()
+
+(* +-----------------------------------------------------------------+
+ | Hokuyo emulation |
+ +-----------------------------------------------------------------+ *)
+
+let dist_x x y theta obstacle =
+ let y_int = y +. (obstacle -. x) *. (tan theta) in
+ if (y_int > 0. && y_int < Krobot_config.world_height) then
+ let d = (obstacle -. x) /. (cos theta) in
+ if d >= 0. then
+ Some d
+ else
+ None
+ else
+ None
+
+let dist_y x y theta obstacle =
+ let x_int = x +. (obstacle -. y) /. (tan theta) in
+ if (x_int > 0. && x_int < Krobot_config.world_width) then
+ let d = (obstacle -. y) /. (sin theta) in
+ if d >= 0. then
+ Some d
+ else
+ None
+ else
+ None
+
+let min_border_distance x y theta =
+ let l = match dist_x x y theta 0. with Some v -> [v] | None -> [] in
+ let l = match dist_x x y theta Krobot_config.world_width with Some v -> v::l | None -> l in
+ let l = match dist_y x y theta 0. with Some v -> v::l | None -> l in
+ let l = match dist_y x y theta Krobot_config.world_height with Some v -> v::l | None -> l in
+ match l with
+ | [] -> None
+ | l -> Some (List.fold_left min max_float l)
+
+(*let gen_data robot =
+ let dim = Array.length Krobot_config.urg_angles in
+ let {Krobot_geom.x=urg_rel_x;
+ Krobot_geom.y=urg_rel_y } = Krobot_config.urg_position in
+ let urg_pos = [| urg_rel_x; urg_rel_y; 1. |] in
+ let rot = rot_mat robot.theta in
+ let urg_pos = mult rot urg_pos in
+ let pos = Krobot_geom.translate
+ { Krobot_geom.x=robot.x; Krobot_geom.y=robot.y }
+ { Krobot_geom.vx = urg_pos.(0); Krobot_geom.vy = urg_pos.(1) } in
+ let {Krobot_geom.x=cen_x;
+ Krobot_geom.y=cen_y } = pos in
+ let l = ref [] in
+ for i = 0 to dim - 1 do
+ let angle = Krobot_config.urg_angles.(i) in
+ match min_border_distance cen_x cen_y (robot.theta +. angle) with
+ | Some dist ->
+ let x = dist *. cos angle +. urg_pos.(0) in
+ let y = dist *. sin angle +. urg_pos.(1) in
+ l := {Krobot_geom.x;Krobot_geom.y} :: !l
+ | None ->
+ ()
+ done;
+ Array.of_list !l*)
+
+let gen_data robot =
+ let dim = Array.length Krobot_config.urg_angles in
+ let l = ref [] in
+ for i = 0 to dim - 1 do
+ let angle = Krobot_config.urg_angles.(i) in
+ match min_border_distance robot.x robot.y (robot.theta +. angle) with
+ | Some dist ->
+ let x = dist *. cos angle in
+ let y = dist *. sin angle in
+ l := {Krobot_geom.x=x;Krobot_geom.y=y} :: !l
+ | None ->
+ ()
+ done;
+ Array.of_list !l
+
+let loop_urg sim bus =
+ let rec aux () =
+ let time = Unix.gettimeofday () in
+ let msg = Urg (gen_data sim.state) in
+ lwt () = Krobot_bus.send bus (time, msg) in
+ lwt () = Lwt_unix.sleep 0.01 in
+ aux () in
+ aux ()
+
+(* +-----------------------------------------------------------------+
+ | Entry point |
+ +-----------------------------------------------------------------+ *)
+
+lwt () =
+ Arg.parse options ignore usage;
+
+ (* Display all informative messages. *)
+ Lwt_log.append_rule "*" Lwt_log.Info;
+
+ (* Open the krobot bus. *)
+ lwt bus = Krobot_bus.get () in
+
+ (* Fork if not prevented. *)
+ if !fork then Krobot_daemon.daemonize bus;
+
+ (* Handle krobot message. *)
+ E.keep (E.map (handle_message bus) (Krobot_bus.recv bus));
+
+ (* Kill any running mc_simulator. *)
+ lwt () = Krobot_bus.send bus (Unix.gettimeofday (), Krobot_bus.Kill "mc_simulator") in
+
+ (* Wait a bit to let the other handler release the connection *)
+ lwt () = Lwt_unix.sleep 0.4 in
+
+ (* Set the motor_controller card in HIL mode if necessary *)
+ if !hil then
+ ignore (Krobot_message.send bus (Unix.gettimeofday (), Set_controller_mode true)) ;
+
+ (* Initial state of the simulator *)
+ let local_sim = {
+ state = { x = 0.; y = 0.; theta = 0. };
+ state_indep = { x = 0.; y = 0.; theta = 0. };
+ internal_state = { theta_l = 0.; theta_r = 0. };
+ velocity_l = 0.;
+ velocity_r = 0.;
+ ghost = { x = 0.; y = 0.; theta = 0. };
+ bezier_u = 0.;
+ bezier_curve = None;
+ bezier_next = None;
+ command = Idle;
+ command_start = 0.;
+ command_end = 0.;
+ time = Unix.gettimeofday ();
+ ...
[truncated message content] |
