Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 09a86b1ab86b522c506e66519d27257f77a7d02b
|
From: Jérémie D. <Ba...@us...> - 2011-03-30 19:52:53
|
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 09a86b1ab86b522c506e66519d27257f77a7d02b (commit)
via 142d0e7c7e61cdb9b001c7485f52ff1c1abb3456 (commit)
via 611e5acb6ef9d0220194bc51318667b8caebd01a (commit)
from 6745d46fc81891fd948daf587b9397fd8cb5dee4 (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 09a86b1ab86b522c506e66519d27257f77a7d02b
Author: Jérémie Dimino <je...@di...>
Date: Wed Mar 30 21:49:10 2011 +0200
[info] fix the simulation
Use the real time delta instead of the wanted one.
commit 142d0e7c7e61cdb9b001c7485f52ff1c1abb3456
Author: Jérémie Dimino <je...@di...>
Date: Wed Mar 30 21:36:30 2011 +0200
[info] add messages for stopping the motors
commit 611e5acb6ef9d0220194bc51318667b8caebd01a
Author: Jérémie Dimino <je...@di...>
Date: Wed Mar 30 21:03:58 2011 +0200
[info] implement the set-odometry CAN packet
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/lib/krobot_message.ml b/info/control2011/src/lib/krobot_message.ml
index aff2580..a68a8b3 100644
--- a/info/control2011/src/lib/krobot_message.ml
+++ b/info/control2011/src/lib/krobot_message.ml
@@ -25,7 +25,9 @@ type t =
| Motor_status of bool
| Motor_move of float * float * float
| Motor_turn of float * float * float
+ | Motor_stop
| Odometry of float * float * float
+ | Set_odometry of float * float * float
| Req_motor_status
| Unknown of frame
@@ -63,10 +65,16 @@ let to_string = function
sprintf
"Motor_turn(%f, %f, %f)"
angle speed acc
+ | Motor_stop ->
+ "Motor_stop"
| Odometry(x, y, theta) ->
sprintf
"Odometry(%f, %f, %f)"
x y theta
+ | Set_odometry(x, y, theta) ->
+ sprintf
+ "Set_odometry(%f, %f, %f)"
+ x y theta
| Req_motor_status ->
"Req_motor_status"
| Unknown frame ->
@@ -131,6 +139,17 @@ let encode = function
~remote:false
~format:F29bits
~data
+ | Set_odometry(x, y, theta) ->
+ let data = String.create 6 in
+ put_sint16 data 0 (truncate (x *. 1000.));
+ put_sint16 data 2 (truncate (y *. 1000.));
+ put_sint16 data 4 (truncate (theta /. pi *. 18000.));
+ frame
+ ~identifier:105
+ ~kind:Data
+ ~remote:false
+ ~format:F29bits
+ ~data
| Motor_move(dist, speed, acc) ->
let data = String.create 8 in
put_sint32 data 0 (truncate (dist *. 1000.));
@@ -153,6 +172,13 @@ let encode = function
~remote:false
~format:F29bits
~data
+ | Motor_stop ->
+ frame
+ ~identifier:203
+ ~kind:Data
+ ~remote:false
+ ~format:F29bits
+ ~data:""
| Req_motor_status ->
frame
~identifier:103
@@ -197,6 +223,11 @@ let decode frame =
(float (get_sint16 frame.data 0) /. 1000.,
float (get_sint16 frame.data 2) /. 1000.,
float (get_sint16 frame.data 4) *. pi /. 18000.)
+ | 105 ->
+ Set_odometry
+ (float (get_sint16 frame.data 0) /. 1000.,
+ float (get_sint16 frame.data 2) /. 1000.,
+ float (get_sint16 frame.data 4) *. pi /. 18000.)
| 201 ->
Motor_move
(float (get_sint32 frame.data 0) /. 1000.,
@@ -207,6 +238,8 @@ let decode frame =
(float (get_sint32 frame.data 0) *. pi /. 18000.,
float (get_uint16 frame.data 4) *. pi /. 18000.,
float (get_uint16 frame.data 6) *. pi /. 18000.)
+ | 203 ->
+ Motor_stop
| _ ->
Unknown frame
diff --git a/info/control2011/src/lib/krobot_message.mli b/info/control2011/src/lib/krobot_message.mli
index e48cc4c..d1cb523 100644
--- a/info/control2011/src/lib/krobot_message.mli
+++ b/info/control2011/src/lib/krobot_message.mli
@@ -36,9 +36,14 @@ type t =
- [speed] is in rad/s
- [acceleration] is in rad/s^2
*)
+ | Motor_stop
+ (** Stops the motors. *)
| Odometry of float * float * float
(** [Odometry(x, y, theta)] the position of the robot on the
table. *)
+ | Set_odometry of float * float * float
+ (** [set_odometry(x, y, theta)] sets the parameters of the
+ odometry to the given ones. *)
| Req_motor_status
(** Request the status of the motors. *)
diff --git a/info/control2011/src/tools/krobot_simulator.ml b/info/control2011/src/tools/krobot_simulator.ml
index 806c364..beaa8ec 100644
--- a/info/control2011/src/tools/krobot_simulator.ml
+++ b/info/control2011/src/tools/krobot_simulator.ml
@@ -72,7 +72,7 @@ type simulator = {
| Simulation |
+-----------------------------------------------------------------+ *)
-let sim_step = 0.01
+let sim_step = 1e-3
let velocities sim =
match sim.command with
@@ -221,14 +221,22 @@ lwt () =
lwt () = Lwt_log.info_f "received: turn(%f, %f, %f)" angle speed acc in
turn sim angle speed acc;
return ()
+ | Motor_stop ->
+ sim.command <- Idle;
+ return ()
| Req_motor_status ->
Krobot_message.send bus (Unix.gettimeofday (), Motor_status(sim.command <> Idle))
+ | Set_odometry(x, y, theta) ->
+ sim.state <- { x; y; theta };
+ return ()
| _ ->
return ())
(Krobot_message.recv bus));
while_lwt true do
- sim.time <- Unix.gettimeofday ();
+ 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;
@@ -243,13 +251,13 @@ lwt () =
and dy = u1 *. (sin sim.state.theta)
and dtheta = u2 in
sim.state <- {
- x = sim.state.x +. dx *. sim_step;
- y = sim.state.y +. dy *. sim_step;
- theta = math_mod_float (sim.state.theta +. dtheta *. sim_step) (2. *. pi);
+ 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 +. sim_step *. (u1 *. 4. +. u2 *. wheels_distance) /. (2. *. wheels_diameter);
- theta_r = sim.internal_state.theta_r +. sim_step *. (u1 *. 4. -. u2 *. wheels_distance) /. (2. *. wheels_diameter);
+ 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 sim_step
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index fa853cd..2665d0f 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -38,6 +38,8 @@ let utf8 code =
end else
invalid_arg "utf8"
+let pi = 4. *. atan 1.
+
let math_mod_float a b =
let b2 = b /. 2. in
let modf = mod_float a b in
@@ -211,8 +213,6 @@ module Board = struct
in
Cairo.set_source_rgb ctx (r /. 255.) (g /. 255.) (b /. 255.)
- let pi = 4. *. atan 1.
-
let optimal_size width height =
if width /. height >= (world_width +. 0.204) /. (world_height +. 0.204) then
((world_width +. 0.204) /. (world_height +. 0.204) *. height, height)
@@ -580,6 +580,37 @@ lwt () =
);
false));
+ ignore
+ (ui#button_start_red#event#connect#button_release
+ (fun ev ->
+ if GdkEvent.Button.button ev = 1 then
+ ignore_result (
+ Krobot_message.send bus
+ (Unix.gettimeofday (),
+ Set_odometry(0.2, 1.9, -0.5 *. pi))
+ );
+ 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(Krobot_config.world_width -. 0.2, 1.9, -0.5 *. pi))
+ );
+ false));
+
+ ignore
+ (ui#button_stop#event#connect#button_release
+ (fun ev ->
+ if GdkEvent.Button.button ev = 1 then
+ ignore_result (
+ Krobot_message.send bus (Unix.gettimeofday (), Motor_stop)
+ );
+ false));
+
(* Ask for the status of the motor in order to display the correct
status initially. *)
lwt () = Krobot_message.send bus (Unix.gettimeofday (), Req_motor_status) in
diff --git a/info/control2011/src/tools/krobot_viewer_ui.glade b/info/control2011/src/tools/krobot_viewer_ui.glade
index 19227fb..f63e673 100644
--- a/info/control2011/src/tools/krobot_viewer_ui.glade
+++ b/info/control2011/src/tools/krobot_viewer_ui.glade
@@ -255,6 +255,18 @@
</packing>
</child>
<child>
+ <widget class="GtkButton" id="button_stop">
+ <property name="label" translatable="yes">Stop</property>
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="receives_default">True</property>
+ </widget>
+ <packing>
+ <property name="expand">False</property>
+ <property name="position">5</property>
+ </packing>
+ </child>
+ <child>
<widget class="GtkTable" id="table1">
<property name="visible">True</property>
<property name="n_rows">5</property>
@@ -497,7 +509,31 @@
</widget>
<packing>
<property name="expand">False</property>
- <property name="position">5</property>
+ <property name="position">6</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkButton" id="button_start_red">
+ <property name="label" translatable="yes">Goto red initial position</property>
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="receives_default">True</property>
+ </widget>
+ <packing>
+ <property name="expand">False</property>
+ <property name="position">7</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkButton" id="button_start_blue">
+ <property name="label" translatable="yes">Goto blue initial position</property>
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="receives_default">True</property>
+ </widget>
+ <packing>
+ <property name="expand">False</property>
+ <property name="position">8</property>
</packing>
</child>
<child>
@@ -509,7 +545,7 @@
</child>
</widget>
<packing>
- <property name="position">6</property>
+ <property name="position">9</property>
</packing>
</child>
</widget>
hooks/post-receive
--
krobot
|
