[Krobot-commits] [SCM] krobot branch, master, updated. 5fb766cfa9a3523c9143ff2aae1e0b3cd343b693

[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  5fb766cfa9a3523c9143ff2aae1e0b3cd343b693 (commit)
       via  fea2e32147df3277fb54354c019fe6ec2ae60d8f (commit)
      from  17aec75ed2e973313bb0f7f2a877fa243ff045c1 (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 5fb766cfa9a3523c9143ff2aae1e0b3cd343b693
Author: Jérémie Dimino <je...@di...>
Date:   Thu Feb 11 08:33:24 2010 +0100

    [driver] also stop motors on collision when manually controlled

commit fea2e32147df3277fb54354c019fe6ec2ae60d8f
Author: Jérémie Dimino <je...@di...>
Date:   Thu Feb 11 08:12:11 2010 +0100

    [driver] wait only for TRAJ_COMPLETED when moving

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

Changes:
diff --git a/PC_Mainboard/driver/src/driver.ml b/PC_Mainboard/driver/src/driver.ml
index c445926..1c2200c 100644
--- a/PC_Mainboard/driver/src/driver.ml
+++ b/PC_Mainboard/driver/src/driver.ml
@@ -264,6 +264,14 @@ struct
 
     commands : int React.event;
     (* CMD_TRAJ commands sent by the card *)
+
+    traj_completed : int React.event;
+    (* Event which occurs each time a TRAJ_COMPLETED command is
+       received *)
+
+    mutable speed_left : int;
+    mutable speed_right : int;
+    (* Speed when manually controlled (by the joystick for example) *)
   }
 
   module OBus = OBus_object.Make(struct
@@ -288,6 +296,10 @@ struct
           dev.move_state <- Ms_static;
           wakeup wakener ()
 
+  let reset_speed dev =
+    dev.speed_left <- 0;
+    dev.speed_right <- 0
+
   let move dev dist speed acc =
     Log#info "motor: move(dist=%d, speed=%d, acc=%d)" dist speed acc;
     match dev.move_state with
@@ -298,10 +310,11 @@ struct
           if (dist > 0 && date < dev.inhibit_forward_until) || (dist < 0 && date < dev.inhibit_backward_until) then
             fail (Failure "inhibited move")
           else begin
+            reset_speed dev;
             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
+            lwt result = select [Lwt_event.next dev.traj_completed >> return `OK; waiter >> return `Stopped] in
             dev.move_state <- Ms_static;
             return result
           end
@@ -312,10 +325,11 @@ struct
       | Ms_moving _ ->
           fail (Failure "already moving")
       | Ms_static ->
+          reset_speed dev;
           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
+          lwt result = select [Lwt_event.next dev.traj_completed >> return `OK; waiter >> return `Stopped] in
           dev.move_state <- Ms_static;
           return result
 
@@ -354,19 +368,24 @@ struct
   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 dir, abs_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
       fail (Failure "inhibited move")
-    else
+    else begin
+      if motor = `Both || motor = `Left then
+        dev.speed_left <- speed;
+      if motor = `Both || motor = `Right then
+        dev.speed_right <- speed;
       lwt () =
         if acc <> dev.acceleration then begin
           dev.acceleration <- acc;
-          Commands.Motor.traj_new_velocity dev.card motor speed acc dir
+          Commands.Motor.traj_new_velocity dev.card motor abs_speed acc dir
         end else
-          Commands.Motor.traj_change_velocity dev.card motor speed dir
+          Commands.Motor.traj_change_velocity dev.card motor abs_speed dir
       in
       Commands.Motor.traj_start dev.card motor
+    end
 
   OL_method StopMotors : motor -> stop_mode -> unit
   OL_method SetSpeed : motor -> int -> int -> unit
@@ -392,6 +411,8 @@ struct
       match dev.move_state with
         | Ms_moving(_, `Forward) ->
             stop_motors dev `Both `Smooth
+        | Ms_static when dev.speed_left > 0 || dev.speed_right > 0 ->
+            stop_motors dev `Both `Smooth
         | _ ->
             return ()
 
@@ -403,10 +424,13 @@ struct
       match dev.move_state with
         | Ms_moving(_, `Backward) ->
             stop_motors dev `Both `Smooth
+        | Ms_static when dev.speed_left < 0 || dev.speed_right < 0 ->
+            stop_motors dev `Both `Smooth
         | _ ->
             return ()
 
   let make card path =
+    let commands = React.E.map (fun data -> Char.code data.[0]) (Card.connect card Protocol.cmd_traj)#event in
     let dev = {
       obus = OBus_object.make path;
       card = card;
@@ -415,7 +439,10 @@ struct
       inhibit_forward_until = 0.0;
       inhibit_backward_until = 0.0;
       move_state = Ms_static;
-      commands = React.E.map (fun data -> Char.code data.[0]) (Card.connect card Protocol.cmd_traj)#event;
+      commands = commands;
+      traj_completed = React.E.filter ((=) Protocol.traj_completed) commands;
+      speed_left = 0;
+      speed_right = 0;
     } in
     dev.state <- React.S.map (fun state -> dev.acceleration <- 0) (Card.state card);
     dev


hooks/post-receive
-- 
krobot