Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 67f7559b01cbcc94a3a9832903f18fc93eb63dc6
|
From: Jérémie D. <Ba...@us...> - 2011-04-10 16:48:24
|
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 67f7559b01cbcc94a3a9832903f18fc93eb63dc6 (commit)
via ab898e133c7988f154a43df090c3acf74d4bfb1b (commit)
from 09f53a63386e5040989f92aa9ef00498ec72931f (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 67f7559b01cbcc94a3a9832903f18fc93eb63dc6
Author: Jérémie Dimino <je...@di...>
Date: Sun Apr 10 18:47:45 2011 +0200
[info] add hil simulator
commit ab898e133c7988f154a43df090c3acf74d4bfb1b
Author: Jérémie Dimino <je...@di...>
Date: Tue Apr 5 22:30:56 2011 +0200
[krobot_message] add messages for the simulation mode
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/_oasis b/info/control2011/_oasis
index a1e18d0..e588a36 100644
--- a/info/control2011/_oasis
+++ b/info/control2011/_oasis
@@ -153,6 +153,13 @@ 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
+
# +-------------------------------------------------------------------+
# | Examples |
# +-------------------------------------------------------------------+
diff --git a/info/control2011/_tags b/info/control2011/_tags
index 593a279..29a64d4 100644
--- a/info/control2011/_tags
+++ b/info/control2011/_tags
@@ -2,7 +2,7 @@
<src/interfaces/*.ml>: -syntax_camlp4o
# OASIS_START
-# DO NOT EDIT (digest: c5fb31f60362628ba2becc8b6a98c80b)
+# DO NOT EDIT (digest: 37f25f8d73f8ace7a5648c2d0f189bda)
# Library krobot-interfaces
"src/interfaces": include
"src/interfaces/krobot-interfaces.cmxs": use_krobot-interfaces
@@ -63,6 +63,12 @@
<src/tools/krobot_viewer.{native,byte}>: pkg_lablgtk2.glade
<src/tools/krobot_viewer.{native,byte}>: pkg_cairo.lablgtk2
<src/tools/*.ml{,i}>: pkg_lablgtk2.glade
+# Executable krobot-hil-simulator
+<src/tools/krobot_hil_simulator.{native,byte}>: use_krobot
+<src/tools/krobot_hil_simulator.{native,byte}>: use_krobot-interfaces
+<src/tools/krobot_hil_simulator.{native,byte}>: pkg_obus
+<src/tools/krobot_hil_simulator.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_hil_simulator.{native,byte}>: pkg_lwt.syntax
# Executable send-can
<examples/send_can.{native,byte}>: use_krobot-can
<examples/send_can.{native,byte}>: use_krobot
diff --git a/info/control2011/setup.ml b/info/control2011/setup.ml
index 2ba8666..1ab732c 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: 4dcf80b777e23d14096fb05b034e73dc) *)
+(* DO NOT EDIT (digest: 2fd774d555ea6bdcec2ed57e38e9bc94) *)
(*
Regenerated by OASIS v0.2.0
Visit http://oasis.forge.ocamlcore.org for more information and
@@ -5323,6 +5323,36 @@ let setup_t =
{exec_custom = false; exec_main_is = "krobot_viewer.ml"; });
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 = "send-can";
cs_data = PropList.Data.create ();
cs_plugin_data = [];
diff --git a/info/control2011/src/lib/krobot_message.ml b/info/control2011/src/lib/krobot_message.ml
index 89abb84..9792c9d 100644
--- a/info/control2011/src/lib/krobot_message.ml
+++ b/info/control2011/src/lib/krobot_message.ml
@@ -32,6 +32,7 @@ type t =
| Motor_stop
| Odometry of float * float * float
| Set_odometry of float * float * float
+ | Set_controller_mode of bool
| Req_motor_status
| Unknown of frame
@@ -105,6 +106,10 @@ let to_string = function
sprintf
"Set_odometry(%f, %f, %f)"
x y theta
+ | Set_controller_mode b ->
+ sprintf
+ "Set_controller_mode(%B)"
+ b
| Req_motor_status ->
"Req_motor_status"
| Unknown frame ->
@@ -231,10 +236,10 @@ let encode = function
~data
| Battery1_voltages(elem1, elem2, elem3, elem4) ->
let data = String.create 8 in
- put_uint16 data 0 (truncate (elem1 *. 10000.));
- put_uint16 data 2 (truncate (elem2 *. 10000.));
- put_uint16 data 4 (truncate (elem3 *. 10000.));
- put_uint16 data 5 (truncate (elem4 *. 10000.));
+ put_uint16 data 0 (truncate (elem1 *. 10000.));
+ put_uint16 data 2 (truncate (elem2 *. 10000.));
+ put_uint16 data 4 (truncate (elem3 *. 10000.));
+ put_uint16 data 5 (truncate (elem4 *. 10000.));
frame
~identifier:401
~kind:Data
@@ -243,10 +248,10 @@ let encode = function
~data
| Battery2_voltages(elem1, elem2, elem3, elem4) ->
let data = String.create 8 in
- put_uint16 data 0 (truncate (elem1 *. 10000.));
- put_uint16 data 2 (truncate (elem2 *. 10000.));
- put_uint16 data 4 (truncate (elem3 *. 10000.));
- put_uint16 data 5 (truncate (elem4 *. 10000.));
+ put_uint16 data 0 (truncate (elem1 *. 10000.));
+ put_uint16 data 2 (truncate (elem2 *. 10000.));
+ put_uint16 data 4 (truncate (elem3 *. 10000.));
+ put_uint16 data 5 (truncate (elem4 *. 10000.));
frame
~identifier:402
~kind:Data
@@ -260,6 +265,13 @@ let encode = function
~remote:false
~format:F29bits
~data:"\x01"
+ | Set_controller_mode b ->
+ frame
+ ~identifier:205
+ ~kind:Data
+ ~remote:false
+ ~format:F29bits
+ ~data:(if b then "\x01" else "\x00")
| Req_motor_status ->
frame
~identifier:103
@@ -336,6 +348,9 @@ let decode frame =
float (get_sint16 frame.data 4) /. 10000.)
| 204 ->
Motor_stop
+ | 205 ->
+ Set_controller_mode
+ (get_uint8 frame.data 0 <> 0)
| 301 ->
Beacon_position
(float (get_uint16 frame.data 0) /. 10000.,
diff --git a/info/control2011/src/lib/krobot_message.mli b/info/control2011/src/lib/krobot_message.mli
index 9324094..fb116bf 100644
--- a/info/control2011/src/lib/krobot_message.mli
+++ b/info/control2011/src/lib/krobot_message.mli
@@ -52,6 +52,8 @@ type t =
| Set_odometry of float * float * float
(** [set_odometry(x, y, theta)] sets the parameters of the
odometry to the given ones. *)
+ | Set_controller_mode of bool
+ (** Put the card into simulation mode or not. *)
| Req_motor_status
(** Request the status of the motors. *)
diff --git a/info/control2011/src/tools/krobot_hil_simulator.ml b/info/control2011/src/tools/krobot_hil_simulator.ml
new file mode 100644
index 0000000..33b9e2e
--- /dev/null
+++ b/info/control2011/src/tools/krobot_hil_simulator.ml
@@ -0,0 +1,153 @@
+(*
+ * 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
+
+let pi = 4. *. atan 1.
+
+let math_mod_float a b =
+ let b2 = b /. 2. in
+ let modf = mod_float a b in
+ if modf > b2 then
+ modf -. b
+ else if modf < -. b2 then
+ modf +. b
+ else
+ modf;;
+
+(* +-----------------------------------------------------------------+
+ | 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 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
hooks/post-receive
--
krobot
|
