Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 285fe8ac86e7530b38627aa816aa6c3dbd893d10
|
From: Jérémie D. <Ba...@us...> - 2010-02-16 22:44:37
|
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 285fe8ac86e7530b38627aa816aa6c3dbd893d10 (commit)
via db7de41d9108ecb6f06cccd0bdd90b634fe1dc4a (commit)
via 10e03dcc5eec1847693344621c54cc9caa486b13 (commit)
from 60ddcdf6fc72e9ca1906fb6b470b5ee4d98ed6ca (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 285fe8ac86e7530b38627aa816aa6c3dbd893d10
Author: Jérémie Dimino <je...@di...>
Date: Tue Feb 16 23:43:56 2010 +0100
[joy_contorl] bugfixes
commit db7de41d9108ecb6f06cccd0bdd90b634fe1dc4a
Author: Jérémie Dimino <je...@di...>
Date: Tue Feb 16 22:35:39 2010 +0100
Bugfixes in manual control
commit 10e03dcc5eec1847693344621c54cc9caa486b13
Author: Jérémie Dimino <je...@di...>
Date: Tue Feb 16 21:50:10 2010 +0100
[driver] stop motors on inactivity
-----------------------------------------------------------------------
Changes:
diff --git a/PC_Mainboard/clients/joy_control.ml b/PC_Mainboard/clients/joy_control.ml
index 8c01b87..0eace2d 100644
--- a/PC_Mainboard/clients/joy_control.ml
+++ b/PC_Mainboard/clients/joy_control.ml
@@ -124,31 +124,56 @@ let try_call action f =
Log#error "action %s failed with: %s" action msg;
return ()
+let rec set_speed action krobot motor speed abort_waiter =
+ lwt () = choose [abort_waiter; try_call action (fun () -> Krobot.set_speed krobot ~motor ~speed ~acc:acceleration)] in
+ if speed = 0 then
+ return ()
+ else begin
+ lwt () = select [abort_waiter; Lwt_unix.sleep (Config.stop_motors_delay /. 2.)] in
+ set_speed action krobot motor speed abort_waiter
+ end
+
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 rec loop () =
Lwt_io.read_value pipe >>= function
| KeyPressed KEY_ESCAPE ->
return ()
| JoyRAxis n ->
- if not !rstop then
- lwt () = try_call "set-speed-right" (fun () -> Krobot.set_speed krobot ~motor:`Right ~speed:(n * axis_coef) ~acc:acceleration) in
+ 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 ()
- else
+ end else
loop ()
| JoyLAxis n ->
- if not !lstop then
- lwt () = try_call "set-speed-left" (fun () -> Krobot.set_speed krobot ~motor:`Left ~speed:(n * axis_coef) ~acc:acceleration) in
+ 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 ()
- else
+ end else
loop ()
| JoyButtonPressed ButtonL2 ->
lstop := true;
+ cancel !lthread;
lwt () = try_call "stop-left-motor" (fun () -> 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;
+ cancel !rthread;
lwt () = try_call "stop-right-motor" (fun () -> Krobot.stop_motors krobot ~motor:`Right ~mode:`Abrupt) in
ignore (lwt () = Lwt_unix.sleep 1.0 in rstop := false; return ());
loop ()
diff --git a/PC_Mainboard/common/config.ml b/PC_Mainboard/common/config.ml
index c248d6e..6c73d1d 100644
--- a/PC_Mainboard/common/config.ml
+++ b/PC_Mainboard/common/config.ml
@@ -13,3 +13,4 @@ let initial_position = 200
let bus_address = "unix:abstract=krobot"
let update_delay = 0.05
let reopen_delay = 1.0
+let stop_motors_delay = 0.2
diff --git a/PC_Mainboard/common/config.mli b/PC_Mainboard/common/config.mli
index 645ac10..f43c1eb 100644
--- a/PC_Mainboard/common/config.mli
+++ b/PC_Mainboard/common/config.mli
@@ -26,3 +26,7 @@ val update_delay : float
val reopen_delay : float
(** Time to wait before retrying to open a card *)
+
+val stop_motors_delay : float
+ (** Amount of time to wait after a set-speed command to stop the
+ motors *)
diff --git a/PC_Mainboard/driver/driver.ml b/PC_Mainboard/driver/driver.ml
index bd69ec1..adc9951 100644
--- a/PC_Mainboard/driver/driver.ml
+++ b/PC_Mainboard/driver/driver.ml
@@ -210,6 +210,12 @@ struct
mutable speed_left : int;
mutable speed_right : int;
(* Speed when manually controlled (by the joystick for example) *)
+
+ mutable stop_rthread : unit Lwt.t;
+ mutable stop_lthread : unit Lwt.t;
+ (* When manually setting speed with [set_speed], these threads stop
+ motors after a small delay, unless a new command is
+ received. *)
}
module OBus = OBus_object.Make(struct
@@ -316,7 +322,21 @@ struct
end else
Commands.Motor.traj_change_velocity dev.card motor abs_speed dir
in
- Commands.Motor.traj_start dev.card motor
+ lwt () = Commands.Motor.traj_start dev.card motor in
+ (* Stop motors after a small delay *)
+ if motor = `Both || motor = `Left then begin
+ cancel dev.stop_lthread;
+ if speed <> 0 then
+ dev.stop_lthread <- (lwt () = Lwt_unix.sleep Config.stop_motors_delay in
+ stop_motors dev `Left `Smooth)
+ end;
+ if motor = `Both || motor = `Right then begin
+ cancel dev.stop_rthread;
+ if speed <> 0 then
+ dev.stop_rthread <- (lwt () = Lwt_unix.sleep Config.stop_motors_delay in
+ stop_motors dev `Right `Smooth)
+ end;
+ return ()
end
OL_method StopMotors : Types.motor -> Types.stop_mode -> unit
@@ -374,6 +394,8 @@ struct
traj_completed = React.E.filter ((=) Protocol.traj_completed) commands;
speed_left = 0;
speed_right = 0;
+ stop_rthread = return ();
+ stop_lthread = return ();
} in
return dev
end
hooks/post-receive
--
krobot
|
