From: Jérémie D. <Ba...@us...> - 2010-02-11 00:31:21
|
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 17aec75ed2e973313bb0f7f2a877fa243ff045c1 (commit) via dc723ce8075a0868981993373334c3302af3f7d7 (commit) via ed02672ed43b8f4af8b64a1e50cda62011898637 (commit) from fbbe106af923be6ff62fc6923d0fe773d95ca5cf (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 17aec75ed2e973313bb0f7f2a877fa243ff045c1 Author: Jérémie Dimino <je...@di...> Date: Thu Feb 11 01:15:10 2010 +0100 [client-tools] record movement results commit dc723ce8075a0868981993373334c3302af3f7d7 Author: Jérémie Dimino <je...@di...> Date: Thu Feb 11 01:02:55 2010 +0100 typo need to go to bed... commit ed02672ed43b8f4af8b64a1e50cda62011898637 Author: Jérémie Dimino <je...@di...> Date: Thu Feb 11 00:59:11 2010 +0100 rewrite movement/blocking stuff ----------------------------------------------------------------------- Changes: diff --git a/PC_Mainboard/clients/lib-krobot/krobot.ml b/PC_Mainboard/clients/lib-krobot/krobot.ml index eacc4da..e45b1c3 100644 --- a/PC_Mainboard/clients/lib-krobot/krobot.ml +++ b/PC_Mainboard/clients/lib-krobot/krobot.ml @@ -168,6 +168,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); @@ -179,8 +184,8 @@ let obus_stop_mode = OBus_type.mapping obus_int (`Abrupt, 1); (`Smooth, 2)] -OP_method Turn : angle : int -> speed : int -> acc : int -> unit -OP_method Move : dist : int -> speed : int -> acc : int -> unit +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 InhibitForward : float -> unit diff --git a/PC_Mainboard/clients/lib-krobot/krobot.mli b/PC_Mainboard/clients/lib-krobot/krobot.mli index f5c8a53..ba5ca18 100644 --- a/PC_Mainboard/clients/lib-krobot/krobot.mli +++ b/PC_Mainboard/clients/lib-krobot/krobot.mli @@ -54,8 +54,10 @@ val ax12_goto : t -> id : int -> pos : int -> speed : int -> unit Lwt.t (** {6 Motors} *) -val turn : t -> angle : int -> speed : int -> acc : int -> unit Lwt.t -val move : t -> dist : int -> speed : int -> acc : int -> unit Lwt.t +type move_result = [ `OK | `Stopped ] + +val turn : t -> angle : int -> speed : int -> acc : int -> move_result Lwt.t +val move : t -> dist : int -> speed : int -> acc : int -> move_result Lwt.t type motor = [ `Left | `Right | `Both ] diff --git a/PC_Mainboard/clients/security/hard_stop.ml b/PC_Mainboard/clients/security/hard_stop.ml index 4421cf3..d2ee376 100644 --- a/PC_Mainboard/clients/security/hard_stop.ml +++ b/PC_Mainboard/clients/security/hard_stop.ml @@ -22,37 +22,18 @@ let usage = Printf.sprintf "Usage: %s [-n]\n\noptions are:" (Filename.basename ( type state = OK | Stopped -let state_forward = ref OK -let state_backward = ref OK - let handle_collide krobot sensors = join [ (if Util.front_collide sensors then begin Log#notice "front collision detected, inhibit motors"; - lwt () = Krobot.inhibit_forward krobot duration in - if !state_forward = OK then begin - Log#notice "front: stop motors"; - state_forward := Stopped; - Krobot.stop_motors krobot ~motor:`Both ~mode:`Smooth; - end else - return () - end else begin - state_forward := OK; - return () - end); + Krobot.inhibit_forward krobot duration + end else + return ()); (if Util.back_collide sensors then begin Log#notice "back collision detected, inhibit motors"; - lwt () = Krobot.inhibit_backward krobot duration in - if !state_backward = OK then begin - Log#notice "back: stop motors"; - state_backward := Stopped; - Krobot.stop_motors krobot ~motor:`Both ~mode:`Smooth; - end else - return () - end else begin - state_backward := OK; - return () - end); + Krobot.inhibit_backward krobot duration + end else + return ()); ] lwt () = diff --git a/PC_Mainboard/clients/tools/controller.ml b/PC_Mainboard/clients/tools/controller.ml index 3e36d5d..29d6c3e 100644 --- a/PC_Mainboard/clients/tools/controller.ml +++ b/PC_Mainboard/clients/tools/controller.ml @@ -266,7 +266,7 @@ let rec loop krobot history = else begin let history = Lwt_read_line.add_entry line history in set_engine_state (Engine.init history); - set_logs (line :: React.S.value logs); + Log#notice "%s" line; ignore (Script.exec krobot line); loop krobot history end @@ -297,6 +297,21 @@ lwt () = Unix.dup2 fdw Unix.stderr; Unix.close fdw; ignore (copy_logs (Lwt_io.of_unix_fd ~mode:Lwt_io.input fdr)); + (* Logs to the log window: *) + Lwt_log.default := + Lwt_log.make + ~output:(fun level lines -> + let buffer = Buffer.create 128 in + let lines = + List.map (fun line -> + Buffer.clear buffer; + Lwt_log.render ~buffer ~level ~message:line ~template:"$(date): $(message)"; + Buffer.contents buffer) lines + in + set_logs (lines @ React.S.value logs); + return ()) + ~close:return + (); lwt krobot = Krobot.create () in let signal = React.S.l5 draw diff --git a/PC_Mainboard/clients/tools/init_position.ml b/PC_Mainboard/clients/tools/init_position.ml index f5aec9f..2217b76 100644 --- a/PC_Mainboard/clients/tools/init_position.ml +++ b/PC_Mainboard/clients/tools/init_position.ml @@ -11,26 +11,32 @@ open Lwt +let move_backward_slowly krobot = + Log#notice "moving backward"; + Krobot.move krobot ~dist:(-1000) ~speed:100 ~acc:100 >>= function + | `OK -> + Log#error "where am i ???"; + exit 1 + | `Stopped -> + Log#notice "backward colisiton dectected"; + return () + lwt () = lwt krobot = Krobot.create () in - Log#notice "moving backward"; - lwt () = Krobot.move krobot ~dist:(-1000) ~speed:100 ~acc:100 in - Log#notice "backward colisiton dectected"; + lwt () = move_backward_slowly krobot in Log#notice "going to initial position on first axis"; - lwt () = Lwt_unix.sleep 2.0 in - lwt () = Krobot.move krobot ~dist:Config.initial_position ~speed:400 ~acc:800 in + lwt _ = Krobot.move krobot ~dist:Config.initial_position ~speed:400 ~acc:800 in Log#notice "turning"; - lwt () = Krobot.turn krobot ~angle:(-90) ~speed:400 ~acc:800 in - Log#notice "moving backward"; - lwt () = Krobot.move krobot ~dist:(-1000) ~speed:100 ~acc:50 in - Log#notice "backward colisiton dectected"; + lwt _ = Krobot.turn krobot ~angle:(-90) ~speed:400 ~acc:800 in - Log#notice "going to initial position on second axis"; - lwt () = Krobot.move krobot ~dist:Config.initial_position ~speed:400 ~acc:800 in + lwt () = move_backward_slowly krobot in - lwt () = Lwt_unix.sleep 2.0 in - Krobot.turn krobot ~angle:45 ~speed:400 ~acc:800 + Log#notice "going to initial position on second axis"; + lwt _ = Krobot.move krobot ~dist:Config.initial_position ~speed:400 ~acc:800 in + Log#notice "turning"; + lwt _ = Krobot.turn krobot ~angle:45 ~speed:400 ~acc:800 in + return () diff --git a/PC_Mainboard/clients/tools/script.ml b/PC_Mainboard/clients/tools/script.ml index 69bb06b..3ea209e 100644 --- a/PC_Mainboard/clients/tools/script.ml +++ b/PC_Mainboard/clients/tools/script.ml @@ -97,19 +97,28 @@ let motor_of_string = function | "right" -> `Right | _ -> failwith "Script.motor_of_strig: invalid motor" +let move thread = + thread >>= function + | `OK -> + Log#notice "done"; + return () + | `Stopped -> + Log#notice "stopped"; + return () + let exec krobot line = let action, args = Script_lexer.whole_command (Lexing.from_string line) in let arg_int key default = try int_of_string (List.assoc key args) with Not_found -> default in let arg_string key default = try List.assoc key args with Not_found -> default in match action with | "forward" -> - Krobot.move krobot ~dist:(arg_int "dist" 100) ~speed:(arg_int "speed" 400) ?acc:(arg_int "acc" 800) + move (Krobot.move krobot ~dist:(arg_int "dist" 100) ~speed:(arg_int "speed" 400) ?acc:(arg_int "acc" 800)) | "backward" -> - Krobot.move krobot ~dist:(-(arg_int "dist" 100)) ~speed:(arg_int "speed" 400) ?acc:(arg_int "acc" 800) + move (Krobot.move krobot ~dist:(-(arg_int "dist" 100)) ~speed:(arg_int "speed" 400) ?acc:(arg_int "acc" 800)) | "left" -> - Krobot.turn krobot ~angle:(arg_int "dist" 100) ~speed:(arg_int "speed" 400) ?acc:(arg_int "acc" 800) + move (Krobot.turn krobot ~angle:(arg_int "dist" 100) ~speed:(arg_int "speed" 400) ?acc:(arg_int "acc" 800)) | "right" -> - Krobot.turn krobot ~angle:(-(arg_int "dist" 100)) ~speed:(arg_int "speed" 400) ?acc:(arg_int "acc" 800) + move (Krobot.turn krobot ~angle:(-(arg_int "dist" 100)) ~speed:(arg_int "speed" 400) ?acc:(arg_int "acc" 800)) | "stop-motors" -> Krobot.stop_motors krobot ~motor:(motor_of_string (arg_string "motor" "both")) diff --git a/PC_Mainboard/driver/src/card.ml b/PC_Mainboard/driver/src/card.ml index a3b9cdb..4a2cabf 100644 --- a/PC_Mainboard/driver/src/card.ml +++ b/PC_Mainboard/driver/src/card.ml @@ -74,7 +74,6 @@ let forge_message msg = exception Card_closed module SerialMap = Map.Make(struct type t = serial let compare = compare end) -module CommandMap = Map.Make(struct type t = int let compare = compare end) type state = Running | Opening | Closed @@ -90,12 +89,9 @@ type card = { (* Pool de serial disponibles, comme il n'y a que 256 serial disponibles on évite de juste incrémenter un compteur au pif. *) - mutable reply_waiters : (string Lwt.t * string Lwt.u) SerialMap.t; + mutable reply_waiters : string Lwt.u SerialMap.t; (* Threads en attente d'une réponse *) - mutable command_waiters : (string Lwt.t * string Lwt.u) CommandMap.t; - (* Thread waiting for a command *) - handle : USB.handle; (* Handle pour le périphérique usb *) @@ -125,6 +121,9 @@ type wrapper = { (* Informations needed to reopen the card when it crashes *) name : string; + + events : (int * (string -> unit)) Lwt_sequence.t; + (* Connected events *) } type t = wrapper @@ -195,13 +194,12 @@ let rec reopen_card wakener wrapper = in loop 1); reply_waiters = SerialMap.empty; - command_waiters = CommandMap.empty; handle = handle; kernel_active = kernel_active; mutex = Lwt_mutex.create (); } in set_state wrapper (Ws_running card); - ignore (dispatch card); + ignore (dispatch wrapper card); Lwt.wakeup wakener (); return () with exn -> @@ -212,7 +210,7 @@ let rec reopen_card wakener wrapper = loop true (* Dispatch incomming messages *) -and dispatch card = +and dispatch wrapper card = let buffer = String.create 64 in lwt len = USB.interrupt_recv ~handle:card.handle ~endpoint:1 buffer 0 64 in if len <> 64 then fatal "read <> 64!"; @@ -220,21 +218,23 @@ and dispatch card = if msg.command = Protocol.cmd_respond then begin (* Réponse à un message *) match try Some(SerialMap.find msg.host_serial card.reply_waiters) with Not_found -> None with - | Some (_, w) -> + | Some wakener -> card.reply_waiters <- SerialMap.remove msg.host_serial card.reply_waiters; card.serial_pool <- card.serial_pool @ [msg.host_serial]; - Lwt.wakeup w msg.data + Lwt.wakeup wakener msg.data | None -> Log#warning "response dropped" end else begin - match try Some(CommandMap.find msg.command card.command_waiters) with Not_found -> None with - | Some(_, w) -> - card.command_waiters <- CommandMap.remove msg.command card.command_waiters; - Lwt.wakeup w msg.data - | None -> - Log#warning "command droppped" + Lwt_sequence.iter_l + (fun (command, push) -> + if command = msg.command then + try + push msg.data + with exn -> + Log#exn exn "pushing event %d failed with: " msg.command) + wrapper.events end; - dispatch card + dispatch wrapper card let open_card ~name ~vendor_id ~product_id = let waiter, wakener = Lwt.task () in @@ -246,6 +246,7 @@ let open_card ~name ~vendor_id ~product_id = vendor_id = vendor_id; product_id = product_id; name = name; + events = Lwt_sequence.create (); } in ignore (reopen_card wakener wrapper); wrapper @@ -284,8 +285,8 @@ let rec send_request wrapper command data = card.serial_pool <- l; s in - let (waiter, wakener) as w = Lwt.wait () in - card.reply_waiters <- SerialMap.add serial w card.reply_waiters; + let waiter, wakener = Lwt.wait () in + card.reply_waiters <- SerialMap.add serial wakener card.reply_waiters; let buffer = forge_message { host_serial = serial; device_serial = 0; command = command; @@ -297,10 +298,13 @@ let rec send_request wrapper command data = lwt len = USB.interrupt_send ~handle:card.handle ~endpoint:1 buffer 0 64 in if len <> 64 then fatal "write <> 64!"; waiter) - with exn -> - Log#exn exn "write to %s card failed" wrapper.name; - lwt () = restart wrapper in - send_request wrapper command data + with + | Canceled -> + fail Canceled + | exn -> + Log#exn exn "write to %s card failed" wrapper.name; + lwt () = restart wrapper in + send_request wrapper command data (* Send a command without waiting for the reply: *) let rec send_command wrapper command data = @@ -314,29 +318,18 @@ let rec send_command wrapper command data = lwt len = USB.interrupt_send ~handle:card.handle ~endpoint:1 buffer 0 64 in if len <> 64 then fatal "write <> 64!"; return () - with exn -> - Log#exn exn "write to %s card failed" wrapper.name; - lwt () = restart wrapper in - send_command wrapper command data - -let rec send_command_with_reply wrapper command data = - lwt card = get_card wrapper in - let buffer = forge_message { host_serial = 0; - device_serial = 0; - command = command; - error = 0; - data = data } in - let (waiter, wakener) as w = Lwt.wait () in - card.command_waiters <- CommandMap.add command w card.command_waiters; - try_lwt - Lwt_mutex.with_lock card.mutex - (fun () -> - lwt len = USB.interrupt_send ~handle:card.handle ~endpoint:1 buffer 0 64 in - if len <> 64 then fatal "write <> 64!"; - waiter) - with exn -> - Log#exn exn "write to %s card failed" wrapper.name; - lwt () = restart wrapper in - send_command_with_reply wrapper command data - -let connect card command = failwith "not implemented" + with + | Canceled -> + fail Canceled + | exn -> + Log#exn exn "write to %s card failed" wrapper.name; + lwt () = restart wrapper in + send_command wrapper command data + +let connect wrapper command = + let event, push = React.E.create () in + let node = Lwt_sequence.add_l (command, push) wrapper.events in + (object + method event = event + method stop = Lwt_sequence.remove node; React.E.stop event + end) diff --git a/PC_Mainboard/driver/src/card.mli b/PC_Mainboard/driver/src/card.mli index fcbd8b1..b3ca5bf 100644 --- a/PC_Mainboard/driver/src/card.mli +++ b/PC_Mainboard/driver/src/card.mli @@ -54,9 +54,6 @@ val send_request : t -> int -> string -> string Lwt.t val send_command : t -> int -> string -> unit Lwt.t (** Sends a command to the device *) -val send_command_with_reply : t -> int -> string -> string Lwt.t - (** Send a command and wait for the reply *) - val connect : t -> int -> < event : string React.event; stop : unit > (** [connect card command] connects to signals [command] emitted by [card] *) diff --git a/PC_Mainboard/driver/src/commands.ml b/PC_Mainboard/driver/src/commands.ml index 10d4c2a..5404b77 100644 --- a/PC_Mainboard/driver/src/commands.ml +++ b/PC_Mainboard/driver/src/commands.ml @@ -78,8 +78,7 @@ struct RW.set_int16 data 1 arg1; RW.set_int16 data 3 arg2; RW.set_int16 data 5 arg3; - lwt _ = Card.send_command_with_reply card Protocol.cmd_traj data in - return () + Card.send_command card Protocol.cmd_traj data let move card ~dist ~speed ~acc = if dist < 0 then diff --git a/PC_Mainboard/driver/src/driver.ml b/PC_Mainboard/driver/src/driver.ml index e652c0f..c445926 100644 --- a/PC_Mainboard/driver/src/driver.ml +++ b/PC_Mainboard/driver/src/driver.ml @@ -238,6 +238,13 @@ open Lwt module Motors = struct + type move_state = + | Ms_static + (* No movement *) + | Ms_moving of unit Lwt.u * [ `Forward | `Backward | `Turn ] + (* Currently moving. The argument is a wakener to stop the + mover. *) + type t = { obus : OBus_object.t; card : Card.t; @@ -252,8 +259,11 @@ struct mutable inhibit_backward_until : float; (* Date after which motor's inhition should be stopped *) - move_mutex : Lwt_mutex.t; - mutable move_stop : unit -> unit; + mutable move_state : move_state; + (* Move state *) + + commands : int React.event; + (* CMD_TRAJ commands sent by the card *) } module OBus = OBus_object.Make(struct @@ -267,25 +277,50 @@ struct | High-level movement | +---------------------------------------------------------------+ *) + let obus_move_result : [ `OK | `Stopped ] OBus_type.basic = + OBus_type.mapping obus_int [(`OK, 0); (`Stopped, 1)] + + let stop_move dev = + match dev.move_state with + | Ms_static -> + () + | Ms_moving(wakener, _) -> + dev.move_state <- Ms_static; + wakeup wakener () + let move dev dist speed acc = Log#info "motor: move(dist=%d, speed=%d, acc=%d)" dist speed acc; - let date = Unix.gettimeofday () in - if (dist > 0 && date < dev.inhibit_forward_until) || (dist < 0 && date < dev.inhibit_backward_until) then - return () - else - Lwt_mutex.with_lock dev.move_mutex - (fun () -> - let waiter, wakener = task () in - dev.move_stop <- (fun () -> wakeup wakener ()); - lwt () = choose [Commands.Motor.move dev.card dist speed acc; waiter] in - return ()) + match dev.move_state with + | Ms_moving _ -> + fail (Failure "already moving") + | Ms_static -> + let date = Unix.gettimeofday () in + if (dist > 0 && date < dev.inhibit_forward_until) || (dist < 0 && date < dev.inhibit_backward_until) then + fail (Failure "inhibited move") + else begin + let waiter, wakener = wait () in + dev.move_state <- Ms_moving(wakener, if dist > 0 then `Forward else `Backward); + lwt () = Commands.Motor.move dev.card dist speed acc in + lwt result = select [Lwt_event.next dev.commands >> return `OK; waiter >> return `Stopped] in + dev.move_state <- Ms_static; + return result + end let turn dev angle speed acc = Log#info "motor: turn(angle=%d, speed=%d, acc=%d)" angle speed acc; - Commands.Motor.turn dev.card angle speed acc - - OL_method Turn : int -> int -> int -> unit - OL_method Move : int -> int -> int -> unit + match dev.move_state with + | Ms_moving _ -> + fail (Failure "already moving") + | Ms_static -> + let waiter, wakener = wait () in + dev.move_state <- Ms_moving(wakener, `Turn); + lwt () = Commands.Motor.turn dev.card angle speed acc in + lwt result = select [Lwt_event.next dev.commands >> return `OK; waiter >> return `Stopped] in + dev.move_state <- Ms_static; + return result + + OL_method Turn : int -> int -> int -> move_result + OL_method Move : int -> int -> int -> move_result (* +---------------------------------------------------------------+ | Low-level control of speed and acceleration of each motor | @@ -313,16 +348,16 @@ struct let stop_motors dev motor mode = Log#info "motor: stopping(motor=%s, mode=%s)" (string_of_motor motor) (string_of_stop_mode mode); - let t = Commands.Motor.stop dev.card motor mode in - dev.move_stop (); - t + stop_move dev; + Commands.Motor.stop dev.card motor mode let set_speed dev motor speed acc = Log#info "motor: set_speed(motor=%s, speed=%d, acc=%d)" (string_of_motor motor) speed acc; + stop_move dev; let dir, speed = if speed < 0 then (`Backward, -speed) else (`Forward, speed) in let date = Unix.gettimeofday () in if (dir = `Forward && date < dev.inhibit_forward_until) || (dir = `Backward && date < dev.inhibit_backward_until) then - return () + fail (Failure "inhibited move") else lwt () = if acc <> dev.acceleration then begin @@ -353,13 +388,23 @@ struct fun dev delay -> let until = Unix.gettimeofday () +. delay in dev.inhibit_forward_until <- until; - inhibited_forward_changed dev until + ignore (inhibited_forward_changed dev until); + match dev.move_state with + | Ms_moving(_, `Forward) -> + stop_motors dev `Both `Smooth + | _ -> + return () OL_method InhibitBackward : float -> unit = fun dev delay -> let until = Unix.gettimeofday () +. delay in dev.inhibit_backward_until <- until; - inhibited_backward_changed dev until + ignore (inhibited_backward_changed dev until); + match dev.move_state with + | Ms_moving(_, `Backward) -> + stop_motors dev `Both `Smooth + | _ -> + return () let make card path = let dev = { @@ -369,10 +414,9 @@ struct state = React.S.const (); inhibit_forward_until = 0.0; inhibit_backward_until = 0.0; - move_mutex = Lwt_mutex.create (); - move_stop = ignore; - } - in + move_state = Ms_static; + commands = React.E.map (fun data -> Char.code data.[0]) (Card.connect card Protocol.cmd_traj)#event; + } in dev.state <- React.S.map (fun state -> dev.acceleration <- 0) (Card.state card); dev end hooks/post-receive -- krobot |