Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. f8b4c4bc985a9e6f73229e03035f91f102ad745d
|
From: Jérémie D. <Ba...@us...> - 2011-03-18 20:34:54
|
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 f8b4c4bc985a9e6f73229e03035f91f102ad745d (commit)
from 79416ac728228e9789e2603a6697289572013d9a (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 f8b4c4bc985a9e6f73229e03035f91f102ad745d
Author: Jérémie Dimino <je...@di...>
Date: Fri Mar 18 21:33:59 2011 +0100
[info] implements new can messages
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/lib/krobot_can.ml b/info/control2011/src/lib/krobot_can.ml
index a9de544..02a64f6 100644
--- a/info/control2011/src/lib/krobot_can.ml
+++ b/info/control2011/src/lib/krobot_can.ml
@@ -108,6 +108,20 @@ let get_uint32 str ofs =
and v3 = Char.code str.[ofs + 3] in
v0 lor (v1 lsl 8) lor (v2 lsl 16) lor (v3 lsl 24)
+let get_float32 str ofs =
+ let v0 = Char.code str.[ofs + 0]
+ and v1 = Char.code str.[ofs + 1]
+ and v2 = Char.code str.[ofs + 2]
+ and v3 = Char.code str.[ofs + 3] in
+ Int32.float_of_bits
+ (Int32.logor
+ (Int32.logor
+ (Int32.of_int v0)
+ (Int32.shift_left (Int32.of_int v1) 8))
+ (Int32.logor
+ (Int32.shift_left (Int32.of_int v2) 16)
+ (Int32.shift_left (Int32.of_int v3) 24)))
+
let put_sint8 str ofs v =
str.[ofs] <- Char.unsafe_chr v
@@ -127,6 +141,13 @@ let put_sint32 str ofs v =
let put_uint32 = put_sint32
+let put_float32 str ofs v =
+ let v = Int32.bits_of_float v in
+ str.[ofs + 0] <- Char.unsafe_chr (Int32.to_int v);
+ str.[ofs + 1] <- Char.unsafe_chr (Int32.to_int (Int32.shift_right v 8));
+ str.[ofs + 2] <- Char.unsafe_chr (Int32.to_int (Int32.shift_right v 16));
+ str.[ofs + 3] <- Char.unsafe_chr (Int32.to_int (Int32.shift_right v 24))
+
(* +-----------------------------------------------------------------+
| D-Bus value conversion |
+-----------------------------------------------------------------+ *)
diff --git a/info/control2011/src/lib/krobot_can.mli b/info/control2011/src/lib/krobot_can.mli
index 1d2d7f0..717092d 100644
--- a/info/control2011/src/lib/krobot_can.mli
+++ b/info/control2011/src/lib/krobot_can.mli
@@ -76,6 +76,8 @@ val get_uint16 : string -> int -> int
val get_sint32 : string -> int -> int
val get_uint32 : string -> int -> int
+val get_float32 : string -> int -> float
+
val put_sint8 : string -> int -> int -> unit
val put_uint8 : string -> int -> int -> unit
@@ -85,6 +87,8 @@ val put_uint16 : string -> int -> int -> unit
val put_sint32 : string -> int -> int -> unit
val put_uint32 : string -> int -> int -> unit
+val put_float32 : string -> int -> float -> unit
+
(** {6 D-Bus frame conversion} *)
val value_of_frame : float * frame -> OBus_value.V.single
diff --git a/info/control2011/src/lib/krobot_message.ml b/info/control2011/src/lib/krobot_message.ml
index 99defa0..5536716 100644
--- a/info/control2011/src/lib/krobot_message.ml
+++ b/info/control2011/src/lib/krobot_message.ml
@@ -17,57 +17,74 @@ open Lwt_react
type direction = Forward | Backward
-type encoder_state = {
- es_position : int;
- es_direction : direction;
-}
-
type t =
- | Encoder_state_1_2 of encoder_state * encoder_state
- | Encoder_state_3_4 of encoder_state * encoder_state
+ | Encoder_position_direction_3_4 of int * direction * int * direction
+ | Encoder_position_speed_3 of float * float
+ | Encoder_position_speed_4 of float * float
| Unknown of frame
(* +-----------------------------------------------------------------+
| Message --> string |
+-----------------------------------------------------------------+ *)
-let string_of_encoder_state es =
- sprintf "{ es_position = %d; es_direction = %s }" es.es_position (match es.es_direction with Forward -> "Forward" | Backward -> "Backward")
+let string_of_direction = function
+ | Forward -> "Forward"
+ | Backward -> "Backward"
let to_string = function
- | Encoder_state_1_2(c1, c2) -> sprintf "Encoder_state_1_2(%s, %s)" (string_of_encoder_state c1) (string_of_encoder_state c2)
- | Encoder_state_3_4(c3, c4) -> sprintf "Encoder_state_3_4(%s, %s)" (string_of_encoder_state c3) (string_of_encoder_state c4)
- | Unknown frame -> Krobot_can.string_of_frame frame
+ | Encoder_position_direction_3_4(pos3, dir3, pos4, dir4) ->
+ sprintf
+ "Encoder_position_direction_3_4(%d, %s, %d, %s)"
+ pos3 (string_of_direction dir3)
+ pos4 (string_of_direction dir4)
+ | Encoder_position_speed_3(pos, speed) ->
+ sprintf
+ "Encoder_position_speed_3(%f, %f)"
+ pos speed
+ | Encoder_position_speed_4(pos, speed) ->
+ sprintf
+ "Encoder_position_speed_4(%f, %f)"
+ pos speed
+ | Unknown frame ->
+ Krobot_can.string_of_frame frame
(* +-----------------------------------------------------------------+
| Encoding |
+-----------------------------------------------------------------+ *)
let encode = function
- | Encoder_state_1_2(c1, c2) ->
+ | Encoder_position_direction_3_4(pos3, dir3, pos4, dir4) ->
let data = String.create 6 in
- put_uint16 data 0 c1.es_position;
- put_uint16 data 2 c2.es_position;
- put_uint8 data 4 (match c1.es_direction with Forward -> 0 | Backward -> 1);
- put_uint8 data 5 (match c2.es_direction with Forward -> 0 | Backward -> 1);
+ put_uint16 data 0 pos3;
+ put_uint16 data 2 pos4;
+ put_uint8 data 4 (match dir3 with Forward -> 0 | Backward -> 1);
+ put_uint8 data 5 (match dir4 with Forward -> 0 | Backward -> 1);
frame
~identifier:100
~kind:Data
~remote:false
~format:F11bits
~data
- | Encoder_state_3_4(c3, c4) ->
- let data = String.create 6 in
- put_uint16 data 0 c3.es_position;
- put_uint16 data 2 c4.es_position;
- put_uint8 data 4 (match c3.es_direction with Forward -> 0 | Backward -> 1);
- put_uint8 data 5 (match c4.es_direction with Forward -> 0 | Backward -> 1);
+ | Encoder_position_speed_3(pos, speed) ->
+ let data = String.create 8 in
+ put_float32 data 0 pos;
+ put_float32 data 4 speed;
frame
~identifier:101
~kind:Data
~remote:false
~format:F11bits
~data
+ | Encoder_position_speed_4(pos, speed) ->
+ let data = String.create 8 in
+ put_float32 data 0 pos;
+ put_float32 data 4 speed;
+ frame
+ ~identifier:102
+ ~kind:Data
+ ~remote:false
+ ~format:F11bits
+ ~data
| Unknown frame ->
frame
@@ -78,17 +95,19 @@ let encode = function
let decode frame =
match frame.identifier with
| 100 ->
- Encoder_state_1_2
- ({ es_position = get_uint16 frame.data 0;
- es_direction = if get_uint8 frame.data 4 = 0 then Forward else Backward },
- { es_position = get_uint16 frame.data 2;
- es_direction = if get_uint8 frame.data 5 = 0 then Forward else Backward })
+ Encoder_position_direction_3_4
+ (get_uint16 frame.data 0,
+ (if get_uint8 frame.data 4 = 0 then Forward else Backward),
+ get_uint16 frame.data 2,
+ (if get_uint8 frame.data 5 = 0 then Forward else Backward))
| 101 ->
- Encoder_state_3_4
- ({ es_position = get_uint16 frame.data 0;
- es_direction = if get_uint8 frame.data 4 = 0 then Forward else Backward },
- { es_position = get_uint16 frame.data 2;
- es_direction = if get_uint8 frame.data 5 = 0 then Forward else Backward })
+ Encoder_position_speed_3
+ (get_float32 frame.data 0,
+ get_float32 frame.data 4)
+ | 102 ->
+ Encoder_position_speed_4
+ (get_float32 frame.data 0,
+ get_float32 frame.data 4)
| _ ->
Unknown frame
diff --git a/info/control2011/src/lib/krobot_message.mli b/info/control2011/src/lib/krobot_message.mli
index 399c0c4..eb56dee 100644
--- a/info/control2011/src/lib/krobot_message.mli
+++ b/info/control2011/src/lib/krobot_message.mli
@@ -12,20 +12,14 @@
type direction = Forward | Backward
(** Type of directions. *)
-(** Type of state of an encoder. *)
-type encoder_state = {
- es_position : int;
- (** The position of the encoder. *)
- es_direction : direction;
- (** The direction of the encoder. *)
-}
-
(** Type of messages. *)
type t =
- | Encoder_state_1_2 of encoder_state * encoder_state
- (** State of the encoder 1 and 2. *)
- | Encoder_state_3_4 of encoder_state * encoder_state
- (** State of the encoder 3 and 4. *)
+ | Encoder_position_direction_3_4 of int * direction * int * direction
+ (** The position and direction of encoders 3 and 4. *)
+ | Encoder_position_speed_3 of float * float
+ (** The position and speed of encoder 3. *)
+ | Encoder_position_speed_4 of float * float
+ (** The position and speed of encoder 4. *)
| Unknown of Krobot_can.frame
(** An unknown can frame. *)
diff --git a/info/control2011/src/tools/krobot_plot.ml b/info/control2011/src/tools/krobot_plot.ml
index f99d06e..875c217 100644
--- a/info/control2011/src/tools/krobot_plot.ml
+++ b/info/control2011/src/tools/krobot_plot.ml
@@ -104,7 +104,7 @@ lwt () =
window#show ();
(* Create the graph. *)
- let graph = { points = Array.init 4 (fun _ -> Queue.create ()); max = 1 } in
+ let graph = { points = Array.init 2 (fun _ -> Queue.create ()); max = 1 } in
(* Draw in a separate thread. *)
ignore (Thread.create (fun () -> draw window graph) ());
@@ -113,15 +113,10 @@ lwt () =
(E.map
(fun (timestamp, msg) ->
match msg with
- | Encoder_state_1_2(enc1, enc2) ->
- graph.max <- max graph.max (max enc1.es_position enc2.es_position);
- Queue.push (timestamp, enc1.es_position) graph.points.(0);
- Queue.push (timestamp, enc2.es_position) graph.points.(1);
- update_graph graph timestamp
- | Encoder_state_3_4(enc3, enc4) ->
- graph.max <- max graph.max (max enc3.es_position enc4.es_position);
- Queue.push (timestamp, enc3.es_position) graph.points.(2);
- Queue.push (timestamp, enc4.es_position) graph.points.(3);
+ | Encoder_position_direction_3_4(pos3, dir3, pos4, dir4) ->
+ graph.max <- max graph.max (max pos3 pos4);
+ Queue.push (timestamp, pos3) graph.points.(0);
+ Queue.push (timestamp, pos4) graph.points.(1);
update_graph graph timestamp
| _ ->
())
hooks/post-receive
--
krobot
|
