[Krobot-commits] [SCM] krobot branch, master, updated. 7901902fd761df3dac5b3e8ac95a3863a1e8afd0

[Jérémie D. <Ba...@us...>]

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  7901902fd761df3dac5b3e8ac95a3863a1e8afd0 (commit)
      from  54394d487afb514fc3d7ffb36f15ad8f304c1972 (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 7901902fd761df3dac5b3e8ac95a3863a1e8afd0
Author: Jérémie Dimino <je...@di...>
Date:   Wed Feb 3 15:53:47 2010 +0100

    added joystick control

-----------------------------------------------------------------------

Changes:
diff --git a/PC_Mainboard/clients/_tags b/PC_Mainboard/clients/_tags
index a232f9b..8582d38 100644
--- a/PC_Mainboard/clients/_tags
+++ b/PC_Mainboard/clients/_tags
@@ -10,3 +10,5 @@
 # lib-krobot uses obus
 <**/*>: pkg_obus
 <tools/*>: pkg_lwt.text, pkg_text.pcre
+
+<tools/joy_control.*>: pkg_sdl
diff --git a/PC_Mainboard/clients/myocamlbuild.ml b/PC_Mainboard/clients/myocamlbuild.ml
index ff52740..593f6b9 100644
--- a/PC_Mainboard/clients/myocamlbuild.ml
+++ b/PC_Mainboard/clients/myocamlbuild.ml
@@ -39,6 +39,7 @@ let packages = [
   "obus.syntax";
   "bitstring";
   "bitstring.syntax";
+  "sdl";
 ]
 
 (* List of available syntaxes :*)
@@ -98,6 +99,7 @@ let _ =
           "tools/status.native";
           "tools/controller.native";
           "remote/forward_dbus.native";
+          "tools/joy_control.native";
         ];
 
         (* +---------------------------------------------------------+
diff --git a/PC_Mainboard/clients/tools/joy_control.ml b/PC_Mainboard/clients/tools/joy_control.ml
new file mode 100644
index 0000000..f6ce1c1
--- /dev/null
+++ b/PC_Mainboard/clients/tools/joy_control.ml
@@ -0,0 +1,182 @@
+(*
+ * joy_control.ml
+ * --------------
+ * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
+ * Licence   : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(* Control the robot with a joystick *)
+
+open Lwt
+open Sdljoystick
+open Sdlevent
+open Sdlkey
+
+(* +-----------------------------------------------------------------+
+   | Joystick events                                                 |
+   +-----------------------------------------------------------------+ *)
+
+type button =
+  | ButtonCross
+  | ButtonSquare
+  | ButtonTriangle
+  | ButtonCircle
+  | ButtonDown
+  | ButtonLeft
+  | ButtonUp
+  | ButtonRight
+  | ButtonSelect
+  | ButtonStart
+  | ButtonR1
+  | ButtonR2
+  | ButtonL1
+  | ButtonL2
+  | ButtonPS3
+  | ButtonLAxis
+  | ButtonRAxis
+
+type event =
+  | JoyRAxis of int
+  | JoyLAxis of int
+  | JoyButtonPressed of button
+  | JoyButtonReleased of button
+  | KeyPressed of Sdlkey.t
+  | KeyReleased of Sdlkey.t
+
+(* +-----------------------------------------------------------------+
+   | int --> button                                                  |
+   +-----------------------------------------------------------------+ *)
+
+let raxis = 3
+let laxis = 1
+
+let axis_min = -32768
+let axis_max = 32767
+
+let button_of_num = function
+  | 14 -> ButtonCross
+  | 15 -> ButtonSquare
+  | 12 -> ButtonTriangle
+  | 13 -> ButtonCircle
+  | 6 -> ButtonDown
+  | 7 -> ButtonLeft
+  | 4 -> ButtonUp
+  | 5 -> ButtonRight
+  | 0 -> ButtonSelect
+  | 3 -> ButtonStart
+  | 11 -> ButtonR1
+  | 9 -> ButtonR2
+  | 10 -> ButtonL1
+  | 8 -> ButtonL2
+  | 16 -> ButtonPS3
+  | 1 -> ButtonLAxis
+  | 2 -> ButtonRAxis
+  | n -> Printf.ksprintf failwith "unknown button %d" n
+
+(* +-----------------------------------------------------------------+
+   | SDL events (executed in a child process)                        |
+   +-----------------------------------------------------------------+ *)
+
+let child_loop pipe joy =
+  let axis_state = Array.init (num_axes joy) (get_axis joy) in
+  let send ev =
+    Pervasives.output_value pipe ev;
+    Pervasives.flush pipe
+  in
+  while true do
+    match wait_event () with
+      | KEYDOWN { keysym = key } ->
+          send (KeyPressed key);
+          if key = KEY_ESCAPE then begin
+            Sdl.quit ();
+            exit 0
+          end
+      | JOYAXISMOTION { jae_axis = axis; jae_value = value } when axis = raxis || axis = laxis ->
+          let value = 100 - ((value - axis_min) * 200 / (axis_max - axis_min)) in
+          if value <> axis_state.(axis) then begin
+            axis_state.(axis) <- value;
+            if axis = laxis then
+              send (JoyLAxis value)
+            else
+              send (JoyRAxis value)
+          end
+      | JOYBUTTONUP { jbe_button = button } ->
+          send (JoyButtonPressed(button_of_num button))
+      | JOYBUTTONDOWN { jbe_button = button } ->
+          send (JoyButtonReleased(button_of_num button))
+      | _ ->
+          ()
+  done
+
+(* +-----------------------------------------------------------------+
+   | Handling events (in the parent process)                         |
+   +-----------------------------------------------------------------+ *)
+
+let axis_coef = 8
+let acceleration = 800
+
+let parent_loop krobot pipe =
+  let rstop = ref false and lstop = ref false in
+  let rec loop () =
+    Lwt_io.read_value pipe >>= function
+      | KeyPressed KEY_ESCAPE ->
+          return ()
+      | JoyRAxis n ->
+          if not !rstop then
+            lwt () = Krobot.set_speed krobot ~motor:`Right ~speed:(n * axis_coef) ~acc:acceleration in
+            loop ()
+          else
+            loop ()
+      | JoyLAxis n ->
+          if not !lstop then
+            lwt () = Krobot.set_speed krobot ~motor:`Left ~speed:(n * axis_coef) ~acc:acceleration in
+            loop ()
+          else
+            loop ()
+      | JoyButtonPressed ButtonL2 ->
+          lstop := true;
+          lwt () = Krobot.stop_motors krobot ~motor:`Left ~mode:`Abrupt in
+          ignore (lwt () = Lwt_unix.sleep 1.0 in lstop := false; return ());
+          loop ()
+      | JoyButtonPressed ButtonR2 ->
+          rstop := true;
+          lwt () = Krobot.stop_motors krobot ~motor:`Right ~mode:`Abrupt in
+          ignore (lwt () = Lwt_unix.sleep 1.0 in rstop := false; return ());
+          loop ()
+      | JoyButtonReleased ButtonCircle ->
+          rstop := false;
+          lstop := false;
+          loop ()
+      | _ ->
+          loop ()
+  in
+  loop ()
+
+(* +-----------------------------------------------------------------+
+   | Entry-point                                                     |
+   +-----------------------------------------------------------------+ *)
+
+let () =
+  let fd_r, fd_w = Unix.pipe () in
+  match Unix.fork () with
+    | 0 ->
+        Unix.close fd_r;
+        Sdl.init [`JOYSTICK;`VIDEO];
+        ignore (Sdlvideo.set_video_mode 100 100 []);
+        Sdljoystick.set_event_state true;
+        let joy =
+          try
+            open_joystick 0
+          with exn ->
+            Log#error "cannot open joystick: %S" (Printexc.to_string exn);
+            raise exn
+        in
+        child_loop (Unix.out_channel_of_descr fd_w) joy
+    | pid ->
+        Unix.close fd_w;
+        Lwt_main.run begin
+          lwt krobot = Krobot.create () in
+          parent_loop krobot (Lwt_io.of_unix_fd ~mode:Lwt_io.input fd_r)
+        end


hooks/post-receive
-- 
krobot