Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. f82b9539b1b2cc4537a68d5b77a39b78c5cbb2fb
|
From: Jérémie D. <Ba...@us...> - 2010-02-20 14:21:44
|
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 f82b9539b1b2cc4537a68d5b77a39b78c5cbb2fb (commit)
via 3c36a3407c82006185997e92c6135fc4b78564cf (commit)
from 530d98a475d5fd24a8a5aa9a04a3ae293cf0a022 (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 f82b9539b1b2cc4537a68d5b77a39b78c5cbb2fb
Author: Jérémie Dimino <je...@di...>
Date: Sat Feb 20 15:21:18 2010 +0100
add the goto command
commit 3c36a3407c82006185997e92c6135fc4b78564cf
Author: Jérémie Dimino <je...@di...>
Date: Sat Feb 20 14:41:49 2010 +0100
Better movements when using the joystick
-----------------------------------------------------------------------
Changes:
diff --git a/PC_Mainboard/clients/joy_control.ml b/PC_Mainboard/clients/joy_control.ml
index 3d7d355..42b9b4d 100644
--- a/PC_Mainboard/clients/joy_control.ml
+++ b/PC_Mainboard/clients/joy_control.ml
@@ -40,8 +40,10 @@ type button =
| ButtonRAxis
type event =
- | JoyRAxis of int
- | JoyLAxis of int
+ | JoyRAxisV of int
+ | JoyLAxisV of int
+ | JoyRAxisH of int
+ | JoyLAxisH of int
| JoyButtonPressed of button
| JoyButtonReleased of button
| KeyPressed of Sdlkey.t
@@ -51,8 +53,10 @@ type event =
| int --> button |
+-----------------------------------------------------------------+ *)
-let raxis = 3
-let laxis = 1
+let raxis_v = 3
+let raxis_h = 2
+let laxis_v = 1
+let laxis_h = 0
let axis_min = -32768
let axis_max = 32767
@@ -95,14 +99,20 @@ let child_loop pipe joy =
Sdl.quit ();
exit 0
end
- | JOYAXISMOTION { jae_axis = axis; jae_value = value } when axis = raxis || axis = laxis ->
+ | JOYAXISMOTION { jae_axis = axis; jae_value = value } ->
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)
+ if axis = laxis_h then
+ send (JoyLAxisH value)
+ else if axis = laxis_v then
+ send (JoyLAxisV value)
+ else if axis = raxis_h then
+ send (JoyRAxisH value)
+ else if axis = raxis_v then
+ send (JoyRAxisV value)
else
- send (JoyRAxis value)
+ ()
end
| JOYBUTTONUP { jbe_button = button } ->
send (JoyButtonPressed(button_of_num button))
@@ -116,7 +126,8 @@ let child_loop pipe joy =
| Handling events (in the parent process) |
+-----------------------------------------------------------------+ *)
-let axis_coef = 8
+let axis_coef = 6
+let axis_coef_turn = 4
let acceleration = 800
let try_call action f =
@@ -139,34 +150,46 @@ let parent_loop krobot pipe =
let rstop = ref false and lstop = ref false in
let rthread = ref (return ()) and lthread = ref (return ()) in
let rabort_wakener = ref None and labort_wakener = ref None in
+ let raxis_h = ref 0
+ and raxis_v = ref 0
+ and laxis_h = ref 0
+ and laxis_v = ref 0 in
+ let set_speeds () =
+ begin match !labort_wakener with
+ | None -> ()
+ | Some w -> wakeup_exn w Exit
+ end;
+ begin match !rabort_wakener with
+ | None -> ()
+ | Some w -> wakeup_exn w Exit
+ end;
+ let waiter, wakener = Lwt.wait () in
+ labort_wakener := Some wakener;
+ lthread := set_speed "set-speed-left" krobot `Left (!laxis_v * axis_coef - !raxis_h * axis_coef_turn) waiter;
+ let waiter, wakener = Lwt.wait () in
+ rabort_wakener := Some wakener;
+ rthread := set_speed "set-speed-right" krobot `Right (!laxis_v * axis_coef + !raxis_h * axis_coef_turn) waiter
+ in
let rec loop () =
Lwt_io.read_value pipe >>= function
| KeyPressed KEY_ESCAPE ->
return ()
- | JoyRAxis n ->
- if not !rstop then begin
- begin match !rabort_wakener with
- | None -> ()
- | Some w -> wakeup_exn w Exit
- end;
- let waiter, wakener = Lwt.wait () in
- rabort_wakener := Some wakener;
- rthread := set_speed "set-speed-right" krobot `Right (n * axis_coef) waiter;
- loop ()
- end else
- loop ()
- | JoyLAxis n ->
- if not !lstop then begin
- begin match !labort_wakener with
- | None -> ()
- | Some w -> wakeup_exn w Exit
- end;
- let waiter, wakener = Lwt.wait () in
- labort_wakener := Some wakener;
- lthread := set_speed "set-speed-left" krobot `Left (n * axis_coef) waiter;
- loop ()
- end else
- loop ()
+ | JoyLAxisV n ->
+ laxis_v := n;
+ set_speeds ();
+ loop ()
+ | JoyLAxisH n ->
+ laxis_h := n;
+ set_speeds ();
+ loop ()
+ | JoyRAxisV n ->
+ raxis_v := n;
+ set_speeds ();
+ loop ()
+ | JoyRAxisH n ->
+ raxis_h := n;
+ set_speeds ();
+ loop ()
| JoyButtonPressed ButtonL2 ->
lstop := true;
cancel !lthread;
diff --git a/PC_Mainboard/clients/script.ml b/PC_Mainboard/clients/script.ml
index 9c3f558..94d8d3d 100644
--- a/PC_Mainboard/clients/script.ml
+++ b/PC_Mainboard/clients/script.ml
@@ -30,6 +30,10 @@ let commands = [
args = [("dist", Arg_int); ("speed", Arg_int); ("acc", Arg_int)] };
{ name = "backward";
args = [("dist", Arg_int); ("speed", Arg_int); ("acc", Arg_int)] };
+ { name = "goto";
+ args = [("x", Arg_int); ("y", Arg_int); ("speed", Arg_int); ("acc", Arg_int);
+ ("mode", Arg_keyword ["straight"; "curve-left"; "curve-right"]);
+ ("bypass-dist", Arg_int)] };
{ name = "left";
args = [("angle", Arg_int); ("speed", Arg_int); ("acc", Arg_int)] };
{ name = "right";
@@ -130,6 +134,15 @@ let exec krobot line =
move (Krobot.turn krobot ~angle:(arg_int "angle" 100) ~speed:(arg_int "speed" 400) ?acc:(arg_int "acc" 800))
| "right" ->
move (Krobot.turn krobot ~angle:(-(arg_int "angle" 100)) ~speed:(arg_int "speed" 400) ?acc:(arg_int "acc" 800))
+ | "goto" ->
+ move (Krobot.goto krobot
+ ~x:(arg_int "x" 0) ~y:(arg_int "y" 0) ~speed:(arg_int "speed" 400) ?acc:(arg_int "acc" 800)
+ ~mode:(match arg_string "mode" "" with
+ | "straight" -> `Straight
+ | "curve-left" -> `Curve_left
+ | "curve-right" -> `Curve_right
+ | _ -> failwith "Script.exec: invalid goto mode")
+ ~bypass_dist:(arg_int "bypass-dist" 0))
| "stop-motors" ->
Krobot.stop_motors krobot
~motor:(motor_of_string (arg_string "motor" "both"))
diff --git a/PC_Mainboard/common/types.ml b/PC_Mainboard/common/types.ml
index 0a4f80a..98efd31 100644
--- a/PC_Mainboard/common/types.ml
+++ b/PC_Mainboard/common/types.ml
@@ -33,3 +33,10 @@ type card_state = [ `Present | `Absent ]
let obus_card_state = OBus_type.mapping obus_int
[(`Present, 0);
(`Absent, 1)]
+
+type goto_mode = [ `Straight | `Curve_right | `Curve_left ]
+
+let obus_goto_mode = OBus_type.mapping obus_int
+ [(`Straight, 0);
+ (`Curve_right, 1);
+ (`Curve_left, 2)]
diff --git a/PC_Mainboard/common/types.mli b/PC_Mainboard/common/types.mli
index 53a3511..3095cbc 100644
--- a/PC_Mainboard/common/types.mli
+++ b/PC_Mainboard/common/types.mli
@@ -27,3 +27,6 @@ type stop_mode = [ `Off | `Abrupt | `Smooth ] with obus(basic)
type card_state = [ `Present | `Absent ] with obus(basic)
(** State of a card *)
+
+type goto_mode = [ `Straight | `Curve_right | `Curve_left ] with obus(basic)
+ (** Form of the trajectory for the goto command *)
diff --git a/PC_Mainboard/driver/commands.ml b/PC_Mainboard/driver/commands.ml
index 97337c7..81a2ab8 100644
--- a/PC_Mainboard/driver/commands.ml
+++ b/PC_Mainboard/driver/commands.ml
@@ -120,6 +120,20 @@ struct
else
backend Protocol.traj_tr card angle speed acc
+ let goto card ~x ~y ~speed ~acc ~mode ~bypass_dist =
+ let data = Card.make_buffer () in
+ RW.set_uint8 data 0 Protocol.traj_goto;
+ RW.set_int16 data 1 x;
+ RW.set_int16 data 3 y;
+ RW.set_int16 data 5 speed;
+ RW.set_int16 data 7 acc;
+ RW.set_uint8 data 9 (match mode with (* lm629.h:157 *)
+ | `Straight -> 0
+ | `Curve_right -> 1
+ | `Curve_left -> 2);
+ RW.set_int16 data 10 bypass_dist;
+ Card.send_command card Protocol.cmd_traj data
+
(* Values comming from lm629.h *)
let int_of_direction motor dir = match motor, dir with
| `Right, `Forward -> -1
diff --git a/PC_Mainboard/driver/commands.mli b/PC_Mainboard/driver/commands.mli
index 43fc243..cce2301 100644
--- a/PC_Mainboard/driver/commands.mli
+++ b/PC_Mainboard/driver/commands.mli
@@ -42,6 +42,8 @@ module Motor : sig
val move : Card.t -> dist : int -> speed : int -> acc : int -> unit Lwt.t
val turn : Card.t -> angle : int -> speed : int -> acc : int -> unit Lwt.t
+ val goto : Card.t -> x : int -> y : int -> speed : int -> acc : int -> mode : Types.goto_mode -> bypass_dist : int -> unit Lwt.t
+
type stop_mode = [ `Off | `Abrupt | `Smooth ]
val stop : Card.t -> motor : t -> mode : stop_mode -> unit Lwt.t
diff --git a/PC_Mainboard/driver/driver.ml b/PC_Mainboard/driver/driver.ml
index e97a0cf..d354076 100644
--- a/PC_Mainboard/driver/driver.ml
+++ b/PC_Mainboard/driver/driver.ml
@@ -193,7 +193,7 @@ struct
(* No movement *)
| Ms_stopping
(* The trajectory has been stopped *)
- | Ms_moving of [ `Forward | `Backward | `Turn ]
+ | Ms_moving of [ `Forward | `Backward | `Turn | `Goto ]
(* Currently moving. The argument is a wakener to stop the
mover. *)
@@ -292,8 +292,26 @@ struct
dev.move_state <- Ms_static;
return result
+ let goto dev x y speed acc mode bypass_dist =
+ match dev.move_state with
+ | Ms_moving _ | Ms_stopping ->
+ fail (Failure "already moving")
+ | Ms_static ->
+ reset_speed dev;
+ dev.move_state <- Ms_moving `Goto;
+ lwt () = Commands.Motor.goto dev.card ~x ~y ~speed ~acc ~mode ~bypass_dist and _ = Lwt_event.next dev.traj_completed in
+ let result = match dev.move_state with
+ | Ms_stopping ->
+ `Stopped
+ | _ ->
+ `OK
+ in
+ dev.move_state <- Ms_static;
+ return result
+
OL_method Turn : int -> int -> int -> Types.move_result
OL_method Move : int -> int -> int -> Types.move_result
+ OL_method Goto : int -> int -> int -> int -> Types.goto_mode -> int -> Types.move_result
(* +---------------------------------------------------------------+
| Low-level control of speed and acceleration of each motor |
@@ -372,7 +390,7 @@ struct
dev.inhibit_forward_until <- until;
ignore (inhibited_forward_changed dev until);
match dev.move_state with
- | Ms_moving `Forward ->
+ | Ms_moving (`Forward | `Goto) ->
stop_motors dev `Both `Abrupt
| Ms_static when dev.speed_left > 0 || dev.speed_right > 0 ->
stop_motors dev `Both `Abrupt
@@ -385,7 +403,7 @@ struct
dev.inhibit_backward_until <- until;
ignore (inhibited_backward_changed dev until);
match dev.move_state with
- | Ms_moving `Backward ->
+ | Ms_moving (`Backward | `Goto) ->
stop_motors dev `Both `Abrupt
| Ms_static when dev.speed_left < 0 || dev.speed_right < 0 ->
stop_motors dev `Both `Abrupt
diff --git a/PC_Mainboard/lib_krobot/krobot.ml b/PC_Mainboard/lib_krobot/krobot.ml
index e3bf7f6..67ab2a0 100644
--- a/PC_Mainboard/lib_krobot/krobot.ml
+++ b/PC_Mainboard/lib_krobot/krobot.ml
@@ -183,27 +183,11 @@ OP_method AX12Goto : id : int -> pos : int -> speed : int -> unit
include MakeDevice(struct let name = "Motors" end)
-type motor = [ `Left | `Right | `Both ]
-type move_result = [ `OK | `Stopped ]
-
-let obus_move_result = OBus_type.mapping obus_int
- [(`OK, 0);
- (`Stopped, 1)]
-
-let obus_motor = OBus_type.mapping obus_int
- [(`Left, -1);
- (`Both, 0);
- (`Right, 1)]
-
-let obus_stop_mode = OBus_type.mapping obus_int
- [(`Off, 0);
- (`Abrupt, 1);
- (`Smooth, 2)]
-
-OP_method Turn : angle : int -> speed : int -> acc : int -> move_result
-OP_method Move : dist : int -> speed : int -> acc : int -> move_result
-OP_method StopMotors : motor : motor -> mode : stop_mode -> unit
-OP_method SetSpeed : motor : motor -> speed : int -> acc : int -> unit
+OP_method Turn : angle : int -> speed : int -> acc : int -> Types.move_result
+OP_method Move : dist : int -> speed : int -> acc : int -> Types.move_result
+OP_method Goto : x : int -> y : int -> speed : int -> acc : int -> mode : Types.goto_mode -> bypass_dist : int -> Types.move_result
+OP_method StopMotors : motor : Types.motor -> mode : Types.stop_mode -> unit
+OP_method SetSpeed : motor : Types.motor -> speed : int -> acc : int -> unit
OP_method InhibitForward : float -> unit
OP_method InhibitBackward : float -> unit
diff --git a/PC_Mainboard/lib_krobot/krobot.mli b/PC_Mainboard/lib_krobot/krobot.mli
index cb468bb..694bae6 100644
--- a/PC_Mainboard/lib_krobot/krobot.mli
+++ b/PC_Mainboard/lib_krobot/krobot.mli
@@ -67,6 +67,7 @@ val ax12_goto : t -> id : int -> pos : int -> speed : int -> unit Lwt.t
val turn : t -> angle : int -> speed : int -> acc : int -> Types.move_result Lwt.t
val move : t -> dist : int -> speed : int -> acc : int -> Types.move_result Lwt.t
+val goto : t -> x : int -> y : int -> speed : int -> acc : int -> mode : Types.goto_mode -> bypass_dist : int -> Types.move_result Lwt.t
val stop_motors : t -> motor : Types.motor -> mode : Types.stop_mode -> unit Lwt.t
(** [stop_motorw t motor mode] stop the given motor(s). *)
hooks/post-receive
--
krobot
|
