Menu
▾
▴
krobot-commits — code commits
You can subscribe to this list here.
| 2009 |
Jan
|
Feb
|
Mar
|
Apr
|
May
|
Jun
|
Jul
|
Aug
|
Sep
|
Oct
|
Nov
|
Dec
(13) |
|---|---|---|---|---|---|---|---|---|---|---|---|---|
| 2010 |
Jan
(50) |
Feb
(137) |
Mar
(84) |
Apr
(36) |
May
(100) |
Jun
(5) |
Jul
|
Aug
(4) |
Sep
(13) |
Oct
(1) |
Nov
(4) |
Dec
(22) |
| 2011 |
Jan
(4) |
Feb
(9) |
Mar
(113) |
Apr
(76) |
May
(31) |
Jun
(19) |
Jul
|
Aug
|
Sep
|
Oct
|
Nov
|
Dec
|
| 2012 |
Jan
(4) |
Feb
|
Mar
(2) |
Apr
(6) |
May
(19) |
Jun
|
Jul
|
Aug
|
Sep
|
Oct
(4) |
Nov
|
Dec
|
| 2013 |
Jan
|
Feb
|
Mar
(2) |
Apr
(22) |
May
(6) |
Jun
|
Jul
|
Aug
|
Sep
|
Oct
(1) |
Nov
|
Dec
|
| 2014 |
Jan
|
Feb
|
Mar
|
Apr
|
May
(1) |
Jun
|
Jul
|
Aug
|
Sep
|
Oct
|
Nov
|
Dec
|
|
From: Jérémie D. <Ba...@us...> - 2011-03-29 12:37:43
|
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 03506f5a382fb6d9d49d8fdcaa332db3e1a6c112 (commit)
via 6b7c42e83c690eaef242e34d131915e49ad6bfc6 (commit)
via f8bbb29d82579f741c524a0347cbb91478526789 (commit)
from e6cd96db90c7985e0c0daabcdf4f37b5092dee02 (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 03506f5a382fb6d9d49d8fdcaa332db3e1a6c112
Author: Jérémie Dimino <je...@di...>
Date: Tue Mar 29 14:31:56 2011 +0200
[info] disable the go button while moving in the viewer
commit 6b7c42e83c690eaef242e34d131915e49ad6bfc6
Author: Jérémie Dimino <je...@di...>
Date: Tue Mar 29 14:21:57 2011 +0200
[info] handle all division by 0 in the simulator
commit f8bbb29d82579f741c524a0347cbb91478526789
Author: Jérémie Dimino <je...@di...>
Date: Tue Mar 29 13:58:58 2011 +0200
[info] send an initial request for the motor status in the viewer
This way the "moving: " label is updated at the beginning.
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/tools/krobot_simulator.ml b/info/control2011/src/tools/krobot_simulator.ml
index c2f1f68..055acba 100644
--- a/info/control2011/src/tools/krobot_simulator.ml
+++ b/info/control2011/src/tools/krobot_simulator.ml
@@ -31,19 +31,29 @@ type internal_state = {
}
type command =
- | Speed of float * float * float * float
- (* [Speed(start_time, end_time, left_velocity, right_velocity)] *)
- | Turn of float * float * float * float
- (* [Turn(start_time, t_acc, end_time, velocity)] *)
- | Move of float * float * float * float
- (* [Move(start_time, t_acc, end_time, velocity)] *)
+ | Idle
+ (* The robot is doint nothing. *)
+ | Speed of float * float
+ (* [Speed(left_velocity, right_velocity)] *)
+ | Turn of float * float
+ (* [Turn(t_acc, velocity)] *)
+ | Move of float * float
+ (* [Move(t_acc, velocity)] *)
(* Type of simulators. *)
type simulator = {
mutable state : state;
+ (* The state of the robot. *)
mutable internal_state : internal_state;
+ (* The state of the wheels. *)
mutable command : command;
+ (* The current command. *)
+ mutable command_start : float;
+ (* The start time of the current command. *)
+ mutable command_end : float;
+ (* The end time of the current command. *)
mutable time : float;
+ (* The current time. *)
}
(* +-----------------------------------------------------------------+
@@ -52,65 +62,78 @@ type simulator = {
let sim_step = 0.01
-let velocities_of_command cmd time =
- match cmd with
- | Speed (start, tend, l_v, r_v) ->
- if time < start || time > tend then
- (0., 0.)
- else
- ((l_v +. r_v) *. wheels_diameter /. 4., (l_v -. r_v) *. wheels_diameter /. wheels_distance)
- | Turn (start, t_acc, tend, vel) ->
- if time < start || time > tend then
- (0., 0.)
- else if time < (start +. t_acc) then
- (0., vel *. (time -. start) /. t_acc)
- else if time < (tend -. t_acc) then
+let velocities sim =
+ match sim.command with
+ | Idle ->
+ (0., 0.)
+ | Speed(l_v, r_v) ->
+ ((l_v +. r_v) *. wheels_diameter /. 4., (l_v -. r_v) *. wheels_diameter /. wheels_distance)
+ | Turn(t_acc, vel) ->
+ if sim.time < (sim.command_start +. t_acc) then
+ (0., vel *. (sim.time -. sim.command_start) /. t_acc)
+ else if sim.time < (sim.command_end -. t_acc) then
(0., vel)
else
- (0., vel *. (tend -. time) /. t_acc)
- | Move (start, t_acc, tend, vel) ->
- if time < start || time > tend then
- (0., 0.)
- else if time < (start +. t_acc) then
- (vel *. (time -. start) /. t_acc, 0.)
- else if time < (tend -. t_acc) then
+ (0., vel *. (sim.command_end -. sim.time) /. t_acc)
+ | Move(t_acc, vel) ->
+ if sim.time < (sim.command_start +. t_acc) then
+ (vel *. (sim.time -. sim.command_start) /. t_acc, 0.)
+ else if sim.time < (sim.command_end -. t_acc) then
(vel, 0.)
else
- (vel *. (tend -. time) /. t_acc, 0.)
+ (vel *. (sim.command_end -. sim.time) /. t_acc, 0.)
let move sim distance velocity acceleration =
- let t_acc = velocity /. acceleration in
- let t_end = (velocity *. velocity +. distance *. acceleration) /. (velocity *. acceleration) in
- sim.command <-
- if t_end > 2. *. t_acc
- then
- Move (sim.time, t_acc, sim.time +. t_end, velocity)
- else begin
- let t_acc = sqrt (distance /. acceleration) in
- let t_end = 2. *. t_acc in
- let velocity = acceleration *. t_acc in
- Move (sim.time, t_acc, sim.time +. t_end, velocity)
+ if velocity <> 0. && acceleration <> 0. then begin
+ let t_acc = velocity /. acceleration in
+ let t_end = (velocity *. velocity +. distance *. acceleration) /. (velocity *. acceleration) in
+ if t_end > 2. *. t_acc then begin
+ if t_acc <> 0. then begin
+ sim.command <- Move(t_acc, velocity);
+ sim.command_start <- sim.time;
+ sim.command_end <- sim.time +. t_end
+ end
+ end else begin
+ if t_acc <> 0. then begin
+ let t_acc = sqrt (distance /. acceleration) in
+ let t_end = 2. *. t_acc in
+ let velocity = acceleration *. t_acc in
+ sim.command <- Move(t_acc, velocity);
+ sim.command_start <- sim.time;
+ sim.command_end <- sim.time +. t_end
+ end
end
+ end
let turn sim angle velocity acceleration =
- let t_acc = velocity /. acceleration in
- let t_end = (velocity *. velocity +. angle *. acceleration) /. (velocity *. acceleration) in
- sim.command <-
- if t_end > 2. *. t_acc
- then
- Turn (sim.time, t_acc, sim.time +. t_end, velocity)
- else begin
+ if velocity <> 0. && acceleration <> 0. then begin
+ let t_acc = velocity /. acceleration in
+ let t_end = (velocity *. velocity +. angle *. acceleration) /. (velocity *. acceleration) in
+ if t_end > 2. *. t_acc then begin
+ if t_acc <> 0. then begin
+ sim.command <- Turn(t_acc, velocity);
+ sim.command_start <- sim.time;
+ sim.command_end <- sim.time +. t_end
+ end
+ end else begin
let t_acc = sqrt (angle /. acceleration) in
let t_end = 2. *. t_acc in
let velocity = acceleration *. t_acc in
- Turn (sim.time, t_acc, sim.time +. t_end, velocity)
+ if t_acc <> 0. then begin
+ sim.command <- Turn(t_acc, velocity);
+ sim.command_start <- sim.time;
+ sim.command_end <- sim.time +. t_end
+ end
end
+ end
let set_velocities sim left_velocity right_velocity duration =
- sim.command <- Speed (sim.time, sim.time +. duration, left_velocity, right_velocity)
+ sim.command <- Speed(left_velocity, right_velocity);
+ sim.command_start <- sim.time;
+ sim.command_end <- sim.time +. duration
let get_velocities sim =
- let (u1, u2) = velocities_of_command sim.command sim.time in
+ let (u1, u2) = velocities sim in
let l_v = (4. *. u1 +. wheels_distance *. u2) /. (2. *. wheels_diameter)
and r_v = (4. *. u1 -. wheels_distance *. u2) /. (2. *. wheels_diameter) in
(l_v, r_v)
@@ -136,7 +159,10 @@ state:
internal_state:
theta_l = %f
theta_r = %f
-command = %s
+command:
+ start = %f
+ end = %f
+ kind = %s
"
sim.time
sim.state.x
@@ -144,13 +170,17 @@ command = %s
sim.state.theta
sim.internal_state.theta_l
sim.internal_state.theta_r
+ sim.command_start
+ sim.command_end
(match sim.command with
- | Speed(s, e, l, r) ->
- Printf.sprintf "Speed(%f, %f, %f, %f)" s e l r
- | Move(s, x, e, a) ->
- Printf.sprintf "Move(%f, %f, %f, %f)" s x e a
- | Turn(s, x, e, a) ->
- Printf.sprintf "Turn(%f, %f, %f, %f)" s x e a)
+ | Idle ->
+ "Idle"
+ | Speed(l, r) ->
+ Printf.sprintf "Speed(%f, %f)" l r
+ | Move(t, a) ->
+ Printf.sprintf "Move(%f, %f)" t a
+ | Turn(t, a) ->
+ Printf.sprintf "Turn(%f, %f)" t a)
lwt () =
lwt bus = Krobot_bus.get () in
@@ -158,7 +188,9 @@ lwt () =
let sim = {
state = { x = 0.; y = 0.; theta = 0. };
internal_state = { theta_l = 0.; theta_r = 0. };
- command = Speed(0., 0., 0., 0.);
+ command = Idle;
+ command_start = 0.;
+ command_end = 0.;
time = Unix.gettimeofday ();
} in
@@ -176,14 +208,7 @@ lwt () =
turn sim angle speed acc;
return ()
| Req_motor_status ->
- let s, e =
- match sim.command with
- | Speed(s, e, _, _) -> (s, e)
- | Move(s, _, e, _) -> (s, e)
- | Turn(s, _, e, _) -> (s, e)
- in
- let t= Unix.gettimeofday () in
- Krobot_message.send bus (t, Motor_status(s <= t && t < e))
+ Krobot_message.send bus (Unix.gettimeofday (), Motor_status(sim.command <> Idle))
| _ ->
return ())
(Krobot_message.recv bus));
@@ -191,12 +216,15 @@ lwt () =
while_lwt true do
sim.time <- Unix.gettimeofday ();
+ (* Put the robot into idle if the last command is terminated. *)
+ if sim.time > sim.command_end then sim.command <- Idle;
+
(* Sends the state of the robot. *)
lwt () = Krobot_message.send bus (sim.time, Odometry(sim.state.x, sim.state.y, sim.state.theta)) in
lwt () = print sim in
- let (u1, u2) = velocities_of_command sim.command sim.time in
+ let (u1, u2) = velocities sim in
let dx = u1 *. (cos sim.state.theta)
and dy = u1 *. (sin sim.state.theta)
and dtheta = u2 in
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index 0bbe687..df3b383 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -557,17 +557,20 @@ lwt () =
Board.smooth board;
false));
- let moving = ref false in
ignore
(ui#button_go#event#connect#button_release
(fun ev ->
- if GdkEvent.Button.button ev = 1 && not !moving then
+ if GdkEvent.Button.button ev = 1 then
ignore_result (
- moving := true;
+ ui#button_go#misc#set_sensitive false;
lwt () = Board.go board in
- moving := false;
+ ui#button_go#misc#set_sensitive true;
return ()
);
false));
+ (* Ask for the status of the motor in order to display the correct
+ status initially. *)
+ lwt () = Krobot_message.send bus (Unix.gettimeofday (), Req_motor_status) in
+
waiter
hooks/post-receive
--
krobot
|
|
From: Jérémie D. <Ba...@us...> - 2011-03-29 10:05:40
|
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 e6cd96db90c7985e0c0daabcdf4f37b5092dee02 (commit)
from a6796d7cb6dffc2bf443214c459f9df9922163b1 (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 e6cd96db90c7985e0c0daabcdf4f37b5092dee02
Author: Jérémie Dimino <je...@di...>
Date: Tue Mar 29 12:04:16 2011 +0200
[info] add a simulator
It is an adaptation of the simulator of last year.
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/_oasis b/info/control2011/_oasis
index d883c70..710ebee 100644
--- a/info/control2011/_oasis
+++ b/info/control2011/_oasis
@@ -127,6 +127,13 @@ Executable "krobot-viewer"
MainIs: krobot_viewer.ml
BuildDepends: krobot, lwt.syntax, cairo.lablgtk2, lwt.glib, threads, lablgtk2.glade
+Executable "krobot-simulator"
+ Path: src/tools
+ Install: true
+ CompiledObject: best
+ MainIs: krobot_simulator.ml
+ BuildDepends: krobot, lwt.syntax
+
# +-------------------------------------------------------------------+
# | Examples |
# +-------------------------------------------------------------------+
diff --git a/info/control2011/_tags b/info/control2011/_tags
index 34b9c0b..20baf17 100644
--- a/info/control2011/_tags
+++ b/info/control2011/_tags
@@ -2,7 +2,7 @@
<src/interfaces/*.ml>: -syntax_camlp4o
# OASIS_START
-# DO NOT EDIT (digest: 8a8ef494b18055a22e7cd7b9c87b8a13)
+# DO NOT EDIT (digest: 255c5daac028047b8fd81335cc7a3a1d)
# Library krobot-interfaces
"src/interfaces": include
"src/interfaces/krobot-interfaces.cmxs": use_krobot-interfaces
@@ -117,12 +117,18 @@
<src/tools/krobot_plot.{native,byte}>: pkg_lwt.syntax
<src/tools/krobot_plot.{native,byte}>: pkg_lwt.glib
<src/tools/krobot_plot.{native,byte}>: pkg_cairo.lablgtk2
+<src/tools/*.ml{,i}>: pkg_threads
+<src/tools/*.ml{,i}>: pkg_lwt.glib
+<src/tools/*.ml{,i}>: pkg_cairo.lablgtk2
+# Executable krobot-simulator
+<src/tools/krobot_simulator.{native,byte}>: use_krobot
+<src/tools/krobot_simulator.{native,byte}>: use_krobot-interfaces
+<src/tools/krobot_simulator.{native,byte}>: pkg_obus
+<src/tools/krobot_simulator.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_simulator.{native,byte}>: pkg_lwt.syntax
<src/tools/*.ml{,i}>: use_krobot
<src/tools/*.ml{,i}>: use_krobot-interfaces
-<src/tools/*.ml{,i}>: pkg_threads
<src/tools/*.ml{,i}>: pkg_obus
<src/tools/*.ml{,i}>: pkg_lwt.unix
<src/tools/*.ml{,i}>: pkg_lwt.syntax
-<src/tools/*.ml{,i}>: pkg_lwt.glib
-<src/tools/*.ml{,i}>: pkg_cairo.lablgtk2
# OASIS_STOP
diff --git a/info/control2011/myocamlbuild.ml b/info/control2011/myocamlbuild.ml
index 58c15ed..08cc900 100644
--- a/info/control2011/myocamlbuild.ml
+++ b/info/control2011/myocamlbuild.ml
@@ -1,7 +1,7 @@
(* OASIS_START *)
-(* DO NOT EDIT (digest: 7b180d572761c507057af7fdf11b0222) *)
+(* DO NOT EDIT (digest: 570b31c8f48685aca9662d28e560f425) *)
module OASISGettext = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISGettext.ml"
+# 21 "/home/dim/sources/oasis/src/oasis/OASISGettext.ml"
let ns_ str =
str
@@ -24,7 +24,7 @@ module OASISGettext = struct
end
module OASISExpr = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISExpr.ml"
+# 21 "/home/dim/sources/oasis/src/oasis/OASISExpr.ml"
@@ -115,7 +115,7 @@ end
module BaseEnvLight = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseEnvLight.ml"
+# 21 "/home/dim/sources/oasis/src/base/BaseEnvLight.ml"
module MapString = Map.Make(String)
@@ -212,7 +212,7 @@ end
module MyOCamlbuildFindlib = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/plugins/ocamlbuild/MyOCamlbuildFindlib.ml"
+# 21 "/home/dim/sources/oasis/src/plugins/ocamlbuild/MyOCamlbuildFindlib.ml"
(** OCamlbuild extension, copied from
* http://brion.inria.fr/gallium/index.php/Using_ocamlfind_with_ocamlbuild
@@ -320,7 +320,7 @@ module MyOCamlbuildFindlib = struct
end
module MyOCamlbuildBase = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/plugins/ocamlbuild/MyOCamlbuildBase.ml"
+# 21 "/home/dim/sources/oasis/src/plugins/ocamlbuild/MyOCamlbuildBase.ml"
(** Base functions for writing myocamlbuild.ml
@author Sylvain Le Gall
@@ -335,7 +335,7 @@ module MyOCamlbuildBase = struct
type name = string
type tag = string
-# 55 "/home/krobot/ocaml-stuff/oasis/src/plugins/ocamlbuild/MyOCamlbuildBase.ml"
+# 55 "/home/dim/sources/oasis/src/plugins/ocamlbuild/MyOCamlbuildBase.ml"
type t =
{
diff --git a/info/control2011/setup.ml b/info/control2011/setup.ml
index 650e561..6f7ed53 100644
--- a/info/control2011/setup.ml
+++ b/info/control2011/setup.ml
@@ -1,14 +1,14 @@
(* setup.ml generated for the first time by OASIS v0.2.0~alpha1 *)
(* OASIS_START *)
-(* DO NOT EDIT (digest: 2a8cc2d164b159f24cb072891525da9a) *)
+(* DO NOT EDIT (digest: 4353dde9f1283d0e97c3df83edda68fd) *)
(*
Regenerated by OASIS v0.2.0
Visit http://oasis.forge.ocamlcore.org for more information and
documentation about functions used in this file.
*)
module OASISGettext = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISGettext.ml"
+# 21 "/home/dim/sources/oasis/src/oasis/OASISGettext.ml"
let ns_ str =
str
@@ -31,7 +31,7 @@ module OASISGettext = struct
end
module OASISContext = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISContext.ml"
+# 21 "/home/dim/sources/oasis/src/oasis/OASISContext.ml"
open OASISGettext
@@ -90,7 +90,7 @@ module OASISContext = struct
end
module OASISUtils = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISUtils.ml"
+# 21 "/home/dim/sources/oasis/src/oasis/OASISUtils.ml"
module MapString = Map.Make(String)
@@ -242,7 +242,7 @@ module OASISUtils = struct
end
module PropList = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/PropList.ml"
+# 21 "/home/dim/sources/oasis/src/oasis/PropList.ml"
open OASISGettext
@@ -277,7 +277,7 @@ module PropList = struct
let clear t =
Hashtbl.clear t
-# 59 "/home/krobot/ocaml-stuff/oasis/src/oasis/PropList.ml"
+# 59 "/home/dim/sources/oasis/src/oasis/PropList.ml"
end
module Schema =
@@ -518,7 +518,7 @@ module PropList = struct
end
module OASISMessage = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISMessage.ml"
+# 21 "/home/dim/sources/oasis/src/oasis/OASISMessage.ml"
open OASISGettext
@@ -567,7 +567,7 @@ module OASISMessage = struct
end
module OASISVersion = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISVersion.ml"
+# 21 "/home/dim/sources/oasis/src/oasis/OASISVersion.ml"
open OASISGettext
@@ -751,7 +751,7 @@ module OASISVersion = struct
end
module OASISLicense = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISLicense.ml"
+# 21 "/home/dim/sources/oasis/src/oasis/OASISLicense.ml"
(** License for _oasis fields
@author Sylvain Le Gall
@@ -784,7 +784,7 @@ module OASISLicense = struct
end
module OASISExpr = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISExpr.ml"
+# 21 "/home/dim/sources/oasis/src/oasis/OASISExpr.ml"
@@ -874,7 +874,7 @@ module OASISExpr = struct
end
module OASISTypes = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISTypes.ml"
+# 21 "/home/dim/sources/oasis/src/oasis/OASISTypes.ml"
@@ -951,7 +951,7 @@ module OASISTypes = struct
type plugin_data = (all_plugin * plugin_data_purpose * (unit -> unit)) list
-# 102 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISTypes.ml"
+# 102 "/home/dim/sources/oasis/src/oasis/OASISTypes.ml"
type 'a conditional = 'a OASISExpr.choices
@@ -1105,7 +1105,7 @@ module OASISTypes = struct
end
module OASISUnixPath = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISUnixPath.ml"
+# 21 "/home/dim/sources/oasis/src/oasis/OASISUnixPath.ml"
type unix_filename = string
type unix_dirname = string
@@ -1184,7 +1184,7 @@ module OASISUnixPath = struct
end
module OASISSection = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISSection.ml"
+# 21 "/home/dim/sources/oasis/src/oasis/OASISSection.ml"
(** Manipulate section
@author Sylvain Le Gall
@@ -1246,12 +1246,12 @@ module OASISSection = struct
end
module OASISBuildSection = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISBuildSection.ml"
+# 21 "/home/dim/sources/oasis/src/oasis/OASISBuildSection.ml"
end
module OASISExecutable = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISExecutable.ml"
+# 21 "/home/dim/sources/oasis/src/oasis/OASISExecutable.ml"
open OASISTypes
@@ -1282,7 +1282,7 @@ module OASISExecutable = struct
end
module OASISLibrary = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISLibrary.ml"
+# 21 "/home/dim/sources/oasis/src/oasis/OASISLibrary.ml"
open OASISTypes
open OASISUtils
@@ -1573,33 +1573,33 @@ module OASISLibrary = struct
end
module OASISFlag = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISFlag.ml"
+# 21 "/home/dim/sources/oasis/src/oasis/OASISFlag.ml"
end
module OASISPackage = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISPackage.ml"
+# 21 "/home/dim/sources/oasis/src/oasis/OASISPackage.ml"
end
module OASISSourceRepository = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISSourceRepository.ml"
+# 21 "/home/dim/sources/oasis/src/oasis/OASISSourceRepository.ml"
end
module OASISTest = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISTest.ml"
+# 21 "/home/dim/sources/oasis/src/oasis/OASISTest.ml"
end
module OASISDocument = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISDocument.ml"
+# 21 "/home/dim/sources/oasis/src/oasis/OASISDocument.ml"
end
module BaseEnvLight = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseEnvLight.ml"
+# 21 "/home/dim/sources/oasis/src/base/BaseEnvLight.ml"
module MapString = Map.Make(String)
@@ -1696,7 +1696,7 @@ end
module BaseContext = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseContext.ml"
+# 21 "/home/dim/sources/oasis/src/base/BaseContext.ml"
open OASISContext
@@ -1707,7 +1707,7 @@ module BaseContext = struct
end
module BaseMessage = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseMessage.ml"
+# 21 "/home/dim/sources/oasis/src/base/BaseMessage.ml"
(** Message to user, overrid for Base
@author Sylvain Le Gall
@@ -1728,7 +1728,7 @@ module BaseMessage = struct
end
module BaseFilePath = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseFilePath.ml"
+# 21 "/home/dim/sources/oasis/src/base/BaseFilePath.ml"
open Filename
@@ -1760,7 +1760,7 @@ module BaseFilePath = struct
end
module BaseEnv = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseEnv.ml"
+# 21 "/home/dim/sources/oasis/src/base/BaseEnv.ml"
open OASISTypes
open OASISGettext
@@ -2219,7 +2219,7 @@ module BaseEnv = struct
end
module BaseExec = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseExec.ml"
+# 21 "/home/dim/sources/oasis/src/base/BaseExec.ml"
open OASISGettext
open OASISUtils
@@ -2279,7 +2279,7 @@ module BaseExec = struct
end
module BaseFileUtil = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseFileUtil.ml"
+# 21 "/home/dim/sources/oasis/src/base/BaseFileUtil.ml"
open OASISGettext
@@ -2457,7 +2457,7 @@ module BaseFileUtil = struct
end
module BaseArgExt = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseArgExt.ml"
+# 21 "/home/dim/sources/oasis/src/base/BaseArgExt.ml"
open OASISUtils
open OASISGettext
@@ -2485,7 +2485,7 @@ module BaseArgExt = struct
end
module BaseCheck = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseCheck.ml"
+# 21 "/home/dim/sources/oasis/src/base/BaseCheck.ml"
open BaseEnv
open BaseMessage
@@ -2611,7 +2611,7 @@ module BaseCheck = struct
end
module BaseOCamlcConfig = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseOCamlcConfig.ml"
+# 21 "/home/dim/sources/oasis/src/base/BaseOCamlcConfig.ml"
open BaseEnv
@@ -2713,7 +2713,7 @@ module BaseOCamlcConfig = struct
end
module BaseStandardVar = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseStandardVar.ml"
+# 21 "/home/dim/sources/oasis/src/base/BaseStandardVar.ml"
open OASISGettext
@@ -2973,7 +2973,7 @@ module BaseStandardVar = struct
end
module BaseFileAB = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseFileAB.ml"
+# 21 "/home/dim/sources/oasis/src/base/BaseFileAB.ml"
open BaseEnv
open OASISGettext
@@ -3021,7 +3021,7 @@ module BaseFileAB = struct
end
module BaseLog = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseLog.ml"
+# 21 "/home/dim/sources/oasis/src/base/BaseLog.ml"
open OASISUtils
@@ -3140,7 +3140,7 @@ module BaseLog = struct
end
module BaseBuilt = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseBuilt.ml"
+# 21 "/home/dim/sources/oasis/src/base/BaseBuilt.ml"
open OASISTypes
open OASISGettext
@@ -3287,7 +3287,7 @@ module BaseBuilt = struct
end
module BaseCustom = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseCustom.ml"
+# 21 "/home/dim/sources/oasis/src/base/BaseCustom.ml"
open BaseEnv
open BaseMessage
@@ -3337,7 +3337,7 @@ module BaseCustom = struct
end
module BaseDynVar = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseDynVar.ml"
+# 21 "/home/dim/sources/oasis/src/base/BaseDynVar.ml"
open OASISTypes
@@ -3381,7 +3381,7 @@ module BaseDynVar = struct
end
module BaseTest = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseTest.ml"
+# 21 "/home/dim/sources/oasis/src/base/BaseTest.ml"
open BaseEnv
open BaseMessage
@@ -3463,7 +3463,7 @@ module BaseTest = struct
end
module BaseDoc = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseDoc.ml"
+# 21 "/home/dim/sources/oasis/src/base/BaseDoc.ml"
open BaseEnv
open BaseMessage
@@ -3493,7 +3493,7 @@ module BaseDoc = struct
end
module BaseSetup = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseSetup.ml"
+# 21 "/home/dim/sources/oasis/src/base/BaseSetup.ml"
open BaseEnv
open BaseMessage
@@ -3911,7 +3911,7 @@ module BaseSetup = struct
end
module BaseDev = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseDev.ml"
+# 21 "/home/dim/sources/oasis/src/base/BaseDev.ml"
@@ -3969,7 +3969,7 @@ end
module InternalConfigurePlugin = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/plugins/internal/InternalConfigurePlugin.ml"
+# 21 "/home/dim/sources/oasis/src/plugins/internal/InternalConfigurePlugin.ml"
(** Configure using internal scheme
@author Sylvain Le Gall
@@ -4185,7 +4185,7 @@ module InternalConfigurePlugin = struct
end
module InternalInstallPlugin = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/plugins/internal/InternalInstallPlugin.ml"
+# 21 "/home/dim/sources/oasis/src/plugins/internal/InternalInstallPlugin.ml"
(** Install using internal scheme
@author Sylvain Le Gall
@@ -4584,7 +4584,7 @@ end
module OCamlbuildCommon = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/plugins/ocamlbuild/OCamlbuildCommon.ml"
+# 21 "/home/dim/sources/oasis/src/plugins/ocamlbuild/OCamlbuildCommon.ml"
(** Functions common to OCamlbuild build and doc plugin
*)
@@ -4684,7 +4684,7 @@ module OCamlbuildCommon = struct
end
module OCamlbuildPlugin = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/plugins/ocamlbuild/OCamlbuildPlugin.ml"
+# 21 "/home/dim/sources/oasis/src/plugins/ocamlbuild/OCamlbuildPlugin.ml"
(** Build using ocamlbuild
@author Sylvain Le Gall
@@ -4928,7 +4928,7 @@ module OCamlbuildPlugin = struct
end
module OCamlbuildDocPlugin = struct
-# 21 "/home/krobot/ocaml-stuff/oasis/src/plugins/ocamlbuild/OCamlbuildDocPlugin.ml"
+# 21 "/home/dim/sources/oasis/src/plugins/ocamlbuild/OCamlbuildDocPlugin.ml"
(* Create documentation using ocamlbuild .odocl files
@author Sylvain Le Gall
@@ -5504,7 +5504,37 @@ let setup_t =
bs_byteopt = [(OASISExpr.EBool true, [])];
bs_nativeopt = [(OASISExpr.EBool true, [])];
},
- {exec_custom = false; exec_main_is = "krobot_plot.ml"; })
+ {exec_custom = false; exec_main_is = "krobot_plot.ml"; });
+ Executable
+ ({
+ cs_name = "krobot-simulator";
+ cs_data = PropList.Data.create ();
+ cs_plugin_data = [];
+ },
+ {
+ bs_build = [(OASISExpr.EBool true, true)];
+ bs_install = [(OASISExpr.EBool true, true)];
+ bs_path = "src/tools";
+ bs_compiled_object = Best;
+ bs_build_depends =
+ [
+ InternalLibrary "krobot";
+ FindlibPackage ("lwt.syntax", None)
+ ];
+ bs_build_tools = [ExternalTool "ocamlbuild"];
+ bs_c_sources = [];
+ bs_data_files = [];
+ bs_ccopt = [(OASISExpr.EBool true, [])];
+ bs_cclib = [(OASISExpr.EBool true, [])];
+ bs_dlllib = [(OASISExpr.EBool true, [])];
+ bs_dllpath = [(OASISExpr.EBool true, [])];
+ bs_byteopt = [(OASISExpr.EBool true, [])];
+ bs_nativeopt = [(OASISExpr.EBool true, [])];
+ },
+ {
+ exec_custom = false;
+ exec_main_is = "krobot_simulator.ml";
+ })
];
plugins =
[(`Extra, "DevFiles", Some "0.2"); (`Extra, "META", Some "0.2")];
diff --git a/info/control2011/src/lib/krobot_config.ml b/info/control2011/src/lib/krobot_config.ml
new file mode 100644
index 0000000..05e699d
--- /dev/null
+++ b/info/control2011/src/lib/krobot_config.ml
@@ -0,0 +1,14 @@
+(*
+ * krobot_config.ml
+ * ----------------
+ * Copyright : (c) 2011, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+let world_height = 2.1
+let world_width = 3.
+let robot_size = 0.3
+let wheels_diameter = 0.098
+let wheels_distance = 0.259
diff --git a/info/control2011/src/lib/krobot_config.mli b/info/control2011/src/lib/krobot_config.mli
new file mode 100644
index 0000000..7876fdd
--- /dev/null
+++ b/info/control2011/src/lib/krobot_config.mli
@@ -0,0 +1,25 @@
+(*
+ * krobot_config.mli
+ * -----------------
+ * Copyright : (c) 2011, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(** Parameters *)
+
+val world_width : float
+ (** The width of the board. *)
+
+val world_height : float
+ (** THe height of the board. *)
+
+val robot_size : float
+ (** The size of the robot, which is a square. *)
+
+val wheels_diameter : float
+ (** The diameter of the wheels. *)
+
+val wheels_distance : float
+ (** The distance between the two wheels. *)
diff --git a/info/control2011/src/lib/krobot_message.ml b/info/control2011/src/lib/krobot_message.ml
index 76eccc5..debdc1d 100644
--- a/info/control2011/src/lib/krobot_message.ml
+++ b/info/control2011/src/lib/krobot_message.ml
@@ -203,9 +203,9 @@ let decode frame =
float (get_uint16 frame.data 6) /. 1000.)
| 202 ->
Motor_turn
- (float (get_sint32 frame.data 0) *. pi /. 1800.,
- float (get_uint16 frame.data 4) *. pi /. 1800.,
- float (get_uint16 frame.data 6) *. pi /. 1800.)
+ (float (get_sint32 frame.data 0) *. pi /. 18000.,
+ float (get_uint16 frame.data 4) *. pi /. 18000.,
+ float (get_uint16 frame.data 6) *. pi /. 18000.)
| _ ->
Unknown frame
diff --git a/info/control2011/src/tools/krobot_simulator.ml b/info/control2011/src/tools/krobot_simulator.ml
new file mode 100644
index 0000000..c2f1f68
--- /dev/null
+++ b/info/control2011/src/tools/krobot_simulator.ml
@@ -0,0 +1,214 @@
+(*
+ * krobot_simulator.ml
+ * -------------------
+ * Copyright : (c) 2011, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(* Simulate the robot. *)
+
+open Lwt
+open Lwt_react
+open Krobot_config
+open Krobot_message
+
+(* +-----------------------------------------------------------------+
+ | Types |
+ +-----------------------------------------------------------------+ *)
+
+(* State of the robot. *)
+type state = {
+ x : float;
+ y : float;
+ theta : float;
+}
+
+type internal_state = {
+ theta_l : float;
+ theta_r : float;
+}
+
+type command =
+ | Speed of float * float * float * float
+ (* [Speed(start_time, end_time, left_velocity, right_velocity)] *)
+ | Turn of float * float * float * float
+ (* [Turn(start_time, t_acc, end_time, velocity)] *)
+ | Move of float * float * float * float
+ (* [Move(start_time, t_acc, end_time, velocity)] *)
+
+(* Type of simulators. *)
+type simulator = {
+ mutable state : state;
+ mutable internal_state : internal_state;
+ mutable command : command;
+ mutable time : float;
+}
+
+(* +-----------------------------------------------------------------+
+ | Simulation |
+ +-----------------------------------------------------------------+ *)
+
+let sim_step = 0.01
+
+let velocities_of_command cmd time =
+ match cmd with
+ | Speed (start, tend, l_v, r_v) ->
+ if time < start || time > tend then
+ (0., 0.)
+ else
+ ((l_v +. r_v) *. wheels_diameter /. 4., (l_v -. r_v) *. wheels_diameter /. wheels_distance)
+ | Turn (start, t_acc, tend, vel) ->
+ if time < start || time > tend then
+ (0., 0.)
+ else if time < (start +. t_acc) then
+ (0., vel *. (time -. start) /. t_acc)
+ else if time < (tend -. t_acc) then
+ (0., vel)
+ else
+ (0., vel *. (tend -. time) /. t_acc)
+ | Move (start, t_acc, tend, vel) ->
+ if time < start || time > tend then
+ (0., 0.)
+ else if time < (start +. t_acc) then
+ (vel *. (time -. start) /. t_acc, 0.)
+ else if time < (tend -. t_acc) then
+ (vel, 0.)
+ else
+ (vel *. (tend -. time) /. t_acc, 0.)
+
+let move sim distance velocity acceleration =
+ let t_acc = velocity /. acceleration in
+ let t_end = (velocity *. velocity +. distance *. acceleration) /. (velocity *. acceleration) in
+ sim.command <-
+ if t_end > 2. *. t_acc
+ then
+ Move (sim.time, t_acc, sim.time +. t_end, velocity)
+ else begin
+ let t_acc = sqrt (distance /. acceleration) in
+ let t_end = 2. *. t_acc in
+ let velocity = acceleration *. t_acc in
+ Move (sim.time, t_acc, sim.time +. t_end, velocity)
+ end
+
+let turn sim angle velocity acceleration =
+ let t_acc = velocity /. acceleration in
+ let t_end = (velocity *. velocity +. angle *. acceleration) /. (velocity *. acceleration) in
+ sim.command <-
+ if t_end > 2. *. t_acc
+ then
+ Turn (sim.time, t_acc, sim.time +. t_end, velocity)
+ else begin
+ let t_acc = sqrt (angle /. acceleration) in
+ let t_end = 2. *. t_acc in
+ let velocity = acceleration *. t_acc in
+ Turn (sim.time, t_acc, sim.time +. t_end, velocity)
+ end
+
+let set_velocities sim left_velocity right_velocity duration =
+ sim.command <- Speed (sim.time, sim.time +. duration, left_velocity, right_velocity)
+
+let get_velocities sim =
+ let (u1, u2) = velocities_of_command sim.command sim.time in
+ let l_v = (4. *. u1 +. wheels_distance *. u2) /. (2. *. wheels_diameter)
+ and r_v = (4. *. u1 -. wheels_distance *. u2) /. (2. *. wheels_diameter) in
+ (l_v, r_v)
+
+let get_state sim =
+ sim.state
+
+let get_encoders sim =
+ let (theta_l, theta_r) = (sim.internal_state.theta_l, sim.internal_state.theta_r) in
+ (theta_l *. wheels_diameter /. 2., theta_r *. wheels_diameter /. 2.)
+
+(* +-----------------------------------------------------------------+
+ | Entry point |
+ +-----------------------------------------------------------------+ *)
+
+let print sim =
+ Lwt_log.debug_f "
+time = %f
+state:
+ x = %f
+ y = %f
+ theta = %f
+internal_state:
+ theta_l = %f
+ theta_r = %f
+command = %s
+"
+ sim.time
+ sim.state.x
+ sim.state.y
+ sim.state.theta
+ sim.internal_state.theta_l
+ sim.internal_state.theta_r
+ (match sim.command with
+ | Speed(s, e, l, r) ->
+ Printf.sprintf "Speed(%f, %f, %f, %f)" s e l r
+ | Move(s, x, e, a) ->
+ Printf.sprintf "Move(%f, %f, %f, %f)" s x e a
+ | Turn(s, x, e, a) ->
+ Printf.sprintf "Turn(%f, %f, %f, %f)" s x e a)
+
+lwt () =
+ lwt bus = Krobot_bus.get () in
+
+ let sim = {
+ state = { x = 0.; y = 0.; theta = 0. };
+ internal_state = { theta_l = 0.; theta_r = 0. };
+ command = Speed(0., 0., 0., 0.);
+ time = Unix.gettimeofday ();
+ } in
+
+ (* Handle commands. *)
+ E.keep
+ (E.map_s
+ (fun (ts, msg) ->
+ match msg with
+ | Motor_move(dist, speed, acc) ->
+ lwt () = Lwt_log.info_f "received: move(%f, %f, %f)" dist speed acc in
+ move sim dist speed acc;
+ return ()
+ | Motor_turn(angle, speed, acc) ->
+ lwt () = Lwt_log.info_f "received: turn(%f, %f, %f)" angle speed acc in
+ turn sim angle speed acc;
+ return ()
+ | Req_motor_status ->
+ let s, e =
+ match sim.command with
+ | Speed(s, e, _, _) -> (s, e)
+ | Move(s, _, e, _) -> (s, e)
+ | Turn(s, _, e, _) -> (s, e)
+ in
+ let t= Unix.gettimeofday () in
+ Krobot_message.send bus (t, Motor_status(s <= t && t < e))
+ | _ ->
+ return ())
+ (Krobot_message.recv bus));
+
+ while_lwt true do
+ sim.time <- Unix.gettimeofday ();
+
+ (* Sends the state of the robot. *)
+ lwt () = Krobot_message.send bus (sim.time, Odometry(sim.state.x, sim.state.y, sim.state.theta)) in
+
+ lwt () = print sim in
+
+ let (u1, u2) = velocities_of_command sim.command sim.time in
+ let dx = u1 *. (cos sim.state.theta)
+ and dy = u1 *. (sin sim.state.theta)
+ and dtheta = u2 in
+ sim.state <- {
+ x = sim.state.x +. dx *. sim_step;
+ y = sim.state.y +. dy *. sim_step;
+ theta = sim.state.theta +. dtheta *. sim_step;
+ };
+ sim.internal_state <- {
+ theta_l = sim.internal_state.theta_l +. sim_step *. (u1 *. 4. +. u2 *. wheels_distance) /. (2. *. wheels_diameter);
+ theta_r = sim.internal_state.theta_r +. sim_step *. (u1 *. 4. -. u2 *. wheels_distance) /. (2. *. wheels_diameter);
+ };
+
+ Lwt_unix.sleep sim_step
+ done
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index cd1b61f..0bbe687 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -166,6 +166,8 @@ end
+-----------------------------------------------------------------+ *)
module Board = struct
+ open Krobot_config
+
type state = {
x : float;
y : float;
@@ -180,14 +182,6 @@ module Board = struct
mutable event : unit event;
}
- let world_height = 2.1
- let world_width = 3.
- let robot_size = 0.3
- let wheels_diam = 0.098
- let wheels_dist = 0.259
- let sim_step = 0.01
- let time = ref 0.
-
type color =
| Black
| White
hooks/post-receive
--
krobot
|
|
From: Jérémie D. <Ba...@us...> - 2011-03-29 08:02:25
|
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 a6796d7cb6dffc2bf443214c459f9df9922163b1 (commit)
from a295b3aaa02fad89f9f2ce4eeec2f5b3c2ed6ff9 (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 a6796d7cb6dffc2bf443214c459f9df9922163b1
Author: Jérémie Dimino <je...@di...>
Date: Tue Mar 29 10:01:55 2011 +0200
[info] allow to set the speed/acceleration in the viewer
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index a8352d8..cd1b61f 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -460,13 +460,19 @@ module Board = struct
| (x, y) :: rest ->
(* Turn the robot. *)
let alpha = atan2 (y -. board.state.y) (x -. board.state.x) -. board.state.theta in
- lwt () = Krobot_message.send board.bus (Unix.gettimeofday (), Motor_turn(alpha, 0.5, 1.0)) in
+ lwt () = Krobot_message.send board.bus (Unix.gettimeofday (),
+ Motor_turn(alpha,
+ board.ui#speed#adjustment#value,
+ board.ui#acceleration#adjustment#value)) in
lwt () = wait_done board in
(* Move the robot. *)
let sqr x = x *. x in
let dist = sqrt (sqr (x -. board.state.x) +. sqr (y -. board.state.y)) in
- lwt () = Krobot_message.send board.bus (Unix.gettimeofday (), Motor_move(dist, 0.5, 1.0)) in
+ lwt () = Krobot_message.send board.bus (Unix.gettimeofday (),
+ Motor_move(dist,
+ board.ui#speed#adjustment#value,
+ board.ui#acceleration#adjustment#value)) in
lwt () = wait_done board in
(* Remove the point. *)
diff --git a/info/control2011/src/tools/krobot_viewer_ui.glade b/info/control2011/src/tools/krobot_viewer_ui.glade
index 00b6b06..a08240e 100644
--- a/info/control2011/src/tools/krobot_viewer_ui.glade
+++ b/info/control2011/src/tools/krobot_viewer_ui.glade
@@ -161,6 +161,16 @@
</packing>
</child>
<child>
+ <widget class="GtkVSeparator" id="vseparator1">
+ <property name="visible">True</property>
+ <property name="orientation">vertical</property>
+ </widget>
+ <packing>
+ <property name="expand">False</property>
+ <property name="position">1</property>
+ </packing>
+ </child>
+ <child>
<widget class="GtkVBox" id="vbox2">
<property name="visible">True</property>
<property name="orientation">vertical</property>
@@ -274,6 +284,71 @@
</packing>
</child>
<child>
+ <widget class="GtkTable" id="table1">
+ <property name="visible">True</property>
+ <property name="n_rows">2</property>
+ <property name="n_columns">2</property>
+ <child>
+ <widget class="GtkLabel" id="label3">
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">Speed: </property>
+ </widget>
+ <packing>
+ <property name="x_options">GTK_FILL</property>
+ <property name="y_options">GTK_FILL</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkLabel" id="label4">
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">Acceleration: </property>
+ </widget>
+ <packing>
+ <property name="top_attach">1</property>
+ <property name="bottom_attach">2</property>
+ <property name="x_options">GTK_FILL</property>
+ <property name="y_options">GTK_FILL</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkSpinButton" id="speed">
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="invisible_char">●</property>
+ <property name="adjustment">0.5 0 2 0.10000000000000001 0.20000000000000001 0.20000000000000001</property>
+ <property name="digits">2</property>
+ <property name="numeric">True</property>
+ </widget>
+ <packing>
+ <property name="left_attach">1</property>
+ <property name="right_attach">2</property>
+ <property name="y_options">GTK_FILL</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkSpinButton" id="acceleration">
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="invisible_char">●</property>
+ <property name="adjustment">1 0 10 0.10000000000000001 0.20000000000000001 0.20000000000000001</property>
+ <property name="digits">2</property>
+ <property name="numeric">True</property>
+ </widget>
+ <packing>
+ <property name="left_attach">1</property>
+ <property name="right_attach">2</property>
+ <property name="top_attach">1</property>
+ <property name="bottom_attach">2</property>
+ <property name="y_options">GTK_FILL</property>
+ </packing>
+ </child>
+ </widget>
+ <packing>
+ <property name="expand">False</property>
+ <property name="position">6</property>
+ </packing>
+ </child>
+ <child>
<widget class="GtkViewport" id="viewport1">
<property name="visible">True</property>
<property name="resize_mode">queue</property>
@@ -282,14 +357,14 @@
</child>
</widget>
<packing>
- <property name="position">6</property>
+ <property name="position">7</property>
</packing>
</child>
</widget>
<packing>
<property name="expand">False</property>
<property name="fill">False</property>
- <property name="position">1</property>
+ <property name="position">2</property>
</packing>
</child>
</widget>
hooks/post-receive
--
krobot
|
|
From: Jérémie D. <Ba...@us...> - 2011-03-29 01:26:33
|
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 a295b3aaa02fad89f9f2ce4eeec2f5b3c2ed6ff9 (commit)
via 0f2309b479272a88a8c7a5a55c9039568c087760 (commit)
from 13cde82ae51c5f3ea9bf0c360cf4437390bce7e5 (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 a295b3aaa02fad89f9f2ce4eeec2f5b3c2ed6ff9
Author: Jérémie Dimino <je...@di...>
Date: Tue Mar 29 03:26:07 2011 +0200
[info] display the moving status in the viewer
commit 0f2309b479272a88a8c7a5a55c9039568c087760
Author: Jérémie Dimino <je...@di...>
Date: Tue Mar 29 03:06:22 2011 +0200
[info] allow to prints raw messages in krobot-dump
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/tools/krobot_dump.ml b/info/control2011/src/tools/krobot_dump.ml
index 226b554..c8c9f39 100644
--- a/info/control2011/src/tools/krobot_dump.ml
+++ b/info/control2011/src/tools/krobot_dump.ml
@@ -12,13 +12,41 @@
open Lwt
open Lwt_react
+let raw = ref false
+let decoded = ref true
+
+let options = Arg.align [
+ "-raw", Arg.Set raw, " prints raw CAN frames";
+ "-no-decoded", Arg.Clear decoded, " do not prints decoded frames";
+]
+
+let usage = "\
+Usage: krobot-dump [options]
+options are:"
+
lwt () =
+ Arg.parse options ignore usage;
+
lwt bus = Krobot_bus.get () in
E.keep
(E.map_s
- (fun (timestamp, msg) ->
- Lwt_io.printlf "%f, %s" timestamp (Krobot_message.to_string msg))
- (Krobot_message.recv bus));
+ (fun (timestamp, frame) ->
+ let msg = Krobot_message.decode frame in
+ lwt () = Lwt_io.printf "%f" timestamp in
+ lwt () =
+ if !decoded then
+ Lwt_io.printf ", %s" (Krobot_message.to_string msg)
+ else
+ return ()
+ in
+ lwt () =
+ if !raw then
+ Lwt_io.printf ", %s" (Krobot_can.string_of_frame frame)
+ else
+ return ()
+ in
+ Lwt_io.printl "")
+ (Krobot_can.recv bus));
fst (wait ())
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index 986d84d..a8352d8 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -44,7 +44,7 @@ let utf8 code =
module LCD = struct
type t = {
- widget : GMisc.drawing_area;
+ ui : Krobot_viewer_ui.window;
chars : char array array;
mutable line : int;
mutable column : int;
@@ -57,8 +57,8 @@ module LCD = struct
let inter = 4.
let border = 2.
- let create widget = {
- widget;
+ let create ui = {
+ ui;
chars = Array.make_matrix lines columns ' ';
line = 0;
column = 0;
@@ -89,7 +89,7 @@ module LCD = struct
let draw lcd =
let colors = if lcd.backlight then colors_light else colors_dark in
- let { Gtk.width; Gtk.height } = lcd.widget#misc#allocation in
+ let { Gtk.width; Gtk.height } = lcd.ui#lcd#misc#allocation in
let surface = Cairo.image_surface_create Cairo.FORMAT_ARGB32 width height in
let ctx = Cairo.create surface in
Cairo.select_font_face ctx "Monospace" Cairo.FONT_SLANT_NORMAL Cairo.FONT_WEIGHT_NORMAL;
@@ -119,7 +119,7 @@ module LCD = struct
Cairo.rectangle ctx x y (fw +. border *. 2.0) (fh +. border *. 2.0);
Cairo.fill ctx
end;
- let ctx = Cairo_lablgtk.create lcd.widget#misc#window in
+ let ctx = Cairo_lablgtk.create lcd.ui#lcd#misc#window in
Cairo.set_source_surface ctx surface 0. 0.;
Cairo.rectangle ctx 0. 0. (float width) (float height);
Cairo.fill ctx;
@@ -174,8 +174,7 @@ module Board = struct
type t = {
bus : Krobot_bus.t;
- widget : GMisc.drawing_area;
- tolerance : GRange.scale;
+ ui : Krobot_viewer_ui.window;
mutable state : state;
mutable points : (float * float) list;
mutable event : unit event;
@@ -217,7 +216,7 @@ module Board = struct
(width, width /. (world_width +. 0.204) *. (world_height +. 0.204))
let draw board =
- let { Gtk.width; Gtk.height } = board.widget#misc#allocation in
+ let { Gtk.width; Gtk.height } = board.ui#scene#misc#allocation in
let surface = Cairo.image_surface_create Cairo.FORMAT_ARGB32 width height in
let ctx = Cairo.create surface in
let width = float width and height = float height in
@@ -380,17 +379,17 @@ module Board = struct
List.iter (fun (x, y) -> Cairo.line_to ctx x y) board.points;
Cairo.stroke ctx;
- let ctx = Cairo_lablgtk.create board.widget#misc#window in
+ let ctx = Cairo_lablgtk.create board.ui#scene#misc#window in
Cairo.set_source_surface ctx surface 0. 0.;
Cairo.rectangle ctx 0. 0. width height;
Cairo.fill ctx;
Cairo.surface_finish surface
let queue_draw board =
- GtkBase.Widget.queue_draw board.widget#as_widget
+ GtkBase.Widget.queue_draw board.ui#scene#as_widget
let add_point board x y =
- let { Gtk.width; Gtk.height } = board.widget#misc#allocation in
+ let { Gtk.width; Gtk.height } = board.ui#scene#misc#allocation in
let width = float width and height = float height in
let dw, dh = optimal_size width height in
let scale = dw /. (world_width +. 0.204) in
@@ -412,7 +411,7 @@ module Board = struct
let smooth board =
let points = Array.of_list ((board.state.x, board.state.y) :: board.points) in
- let tolerance = board.tolerance#adjustment#value in
+ let tolerance = board.ui#tolerance#adjustment#value in
let rec loop = function
| i1 :: i2 :: rest ->
let (x1, y1) = points.(i1) and (x2, y2) = points.(i2) in
@@ -471,7 +470,7 @@ module Board = struct
lwt () = wait_done board in
(* Remove the point. *)
- board.points <- rest;
+ board.points <- List.tl board.points;
(* Redraw everything without the last point. *)
queue_draw board;
@@ -482,11 +481,10 @@ module Board = struct
in
loop ()
- let create bus widget tolerance =
+ let create bus ui =
let board ={
bus;
- widget;
- tolerance;
+ ui;
state = { x = 0.2; y = 1.9; theta = 2. *. atan (-1.) };
points = [];
event = E.never;
@@ -503,6 +501,10 @@ module Board = struct
board.state <- state;
queue_draw board
end
+ | Motor_status true ->
+ board.ui#label_moving#set_text "yes"
+ | Motor_status false ->
+ board.ui#label_moving#set_text "no"
| _ ->
())
(Krobot_message.recv bus)
@@ -525,10 +527,10 @@ lwt () =
ignore (ui#window#connect#destroy ~callback:(wakeup wakener));
ui#window#show ();
- let lcd = LCD.create ui#lcd in
+ let lcd = LCD.create ui in
ignore (ui#lcd#event#connect#expose (fun ev -> LCD.draw lcd; true));
- let board = Board.create bus ui#scene ui#tolerance in
+ let board = Board.create bus ui in
ignore (ui#scene#event#connect#expose (fun ev -> Board.draw board; true));
ignore
(ui#scene#event#connect#button_press
diff --git a/info/control2011/src/tools/krobot_viewer_ui.glade b/info/control2011/src/tools/krobot_viewer_ui.glade
index 7cd2fa4..00b6b06 100644
--- a/info/control2011/src/tools/krobot_viewer_ui.glade
+++ b/info/control2011/src/tools/krobot_viewer_ui.glade
@@ -245,6 +245,35 @@
</packing>
</child>
<child>
+ <widget class="GtkHBox" id="hbox3">
+ <property name="visible">True</property>
+ <child>
+ <widget class="GtkLabel" id="label2">
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">Moving: </property>
+ </widget>
+ <packing>
+ <property name="expand">False</property>
+ <property name="position">0</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkLabel" id="label_moving">
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">no</property>
+ </widget>
+ <packing>
+ <property name="expand">False</property>
+ <property name="position">1</property>
+ </packing>
+ </child>
+ </widget>
+ <packing>
+ <property name="expand">False</property>
+ <property name="position">5</property>
+ </packing>
+ </child>
+ <child>
<widget class="GtkViewport" id="viewport1">
<property name="visible">True</property>
<property name="resize_mode">queue</property>
@@ -253,7 +282,7 @@
</child>
</widget>
<packing>
- <property name="position">5</property>
+ <property name="position">6</property>
</packing>
</child>
</widget>
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2011-03-29 00:34:49
|
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 13cde82ae51c5f3ea9bf0c360cf4437390bce7e5 (commit)
from 000af2663b8c1fca34915a304f0bd5f4c27ef542 (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 13cde82ae51c5f3ea9bf0c360cf4437390bce7e5
Author: Xavier Lagorce <Xav...@cr...>
Date: Tue Mar 29 02:30:53 2011 +0200
[Controller_Motor_STM32] Minor modifications
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.c
index 5a2b879..f3e9e55 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.c
@@ -189,8 +189,6 @@ static void NORETURN canMonitorListen_process(void) {
move_can_msg_t move_msg;
turn_can_msg_t turn_msg;
- uint8_t i = 0;
-
// Initialize constant parameters of TX frame
txm.dlc = 8;
txm.rtr = 0;
@@ -199,14 +197,6 @@ static void NORETURN canMonitorListen_process(void) {
while (1) {
received = can_receive(CAND1, &frame, ms_to_ticks(100));
- if (i == 0) {
- i = 1;
- LED2_ON();
- } else {
- i = 0;
- LED2_OFF();
- }
- timer_delay(200);
if (received) {
if (frame.rtr == 1) {
// Handle requests
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/main.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/main.c
index be160f8..75c88cf 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/main.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/main.c
@@ -50,7 +50,7 @@ static void init(void)
odometryInit(1e-3, 0.049245, 0.259, -2.0*M_PI/2000.0/15.0);
// Blink to say we are ready
- for (uint8_t i=0; i < 10; i++) {
+ for (uint8_t i=0; i < 5; i++) {
LED1_ON();
LED2_ON();
LED3_ON();
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.c
index 8d74104..f09051b 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.c
@@ -86,6 +86,8 @@ void trapezoid_callback(command_generator_t *generator) {
adjust_value(trap_speed, 0.);
//adjust_value(trap, automaton->angle);
automaton->state = TRAPEZOID_STATE_STOP;
+ if (tc_is_finished())
+ LED1_OFF();
break;
}
}
@@ -191,6 +193,8 @@ void tc_move(float distance, float speed, float acceleration) {
add_callback(&left_wheel, SELECT_THRESHOLD(left_trap.dir), left_trap.init_val + left_trap.angle/2.0, trapezoid_callback);
add_callback(&left_wheel_speed, SELECT_THRESHOLD(left_trap.dir), left_trap.speed, trapezoid_callback);
}
+
+ LED1_ON();
}
void tc_turn(float angle, float speed, float acceleration) {
@@ -260,5 +264,7 @@ void tc_turn(float angle, float speed, float acceleration) {
add_callback(&left_wheel, SELECT_THRESHOLD(left_trap.dir), left_trap.init_val + left_trap.angle/2.0, trapezoid_callback);
add_callback(&left_wheel_speed, SELECT_THRESHOLD(left_trap.dir), left_trap.speed, trapezoid_callback);
}
+
+ LED1_ON();
}
hooks/post-receive
--
krobot
|
|
From: Jérémie D. <Ba...@us...> - 2011-03-29 00:29:03
|
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 000af2663b8c1fca34915a304f0bd5f4c27ef542 (commit)
via 6af11a4ea56df703f2563f8f6383c81de0b45741 (commit)
via 9e10c69b254deffdd5165cec562b5e1249ba6acf (commit)
from 98cd4fb2991a9b603bc9589096ef037851b59d98 (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 000af2663b8c1fca34915a304f0bd5f4c27ef542
Author: Jérémie Dimino <je...@di...>
Date: Tue Mar 29 02:28:08 2011 +0200
[info] make the robot to move faster
commit 6af11a4ea56df703f2563f8f6383c81de0b45741
Author: [Kro]bot <kr...@wa...>
Date: Tue Mar 29 02:27:25 2011 +0200
[info] typo in serialization/deserialization of CAN frames
commit 9e10c69b254deffdd5165cec562b5e1249ba6acf
Author: [Kro]bot <kr...@wa...>
Date: Tue Mar 29 02:03:31 2011 +0200
[info] add an example to send a can frame
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/_oasis b/info/control2011/_oasis
index c3ac98f..d883c70 100644
--- a/info/control2011/_oasis
+++ b/info/control2011/_oasis
@@ -138,6 +138,13 @@ Executable "dump-can"
MainIs: dump_can.ml
BuildDepends: krobot.can, lwt.syntax
+Executable "send-can"
+ Path: examples
+ Install: false
+ CompiledObject: best
+ MainIs: send_can.ml
+ BuildDepends: krobot.can, lwt.syntax
+
# +-------------------------------------------------------------------+
# | Doc |
# +-------------------------------------------------------------------+
diff --git a/info/control2011/_tags b/info/control2011/_tags
index 16e64a1..34b9c0b 100644
--- a/info/control2011/_tags
+++ b/info/control2011/_tags
@@ -2,7 +2,7 @@
<src/interfaces/*.ml>: -syntax_camlp4o
# OASIS_START
-# DO NOT EDIT (digest: 9d263765d4deabae48324e53c88ea807)
+# DO NOT EDIT (digest: 8a8ef494b18055a22e7cd7b9c87b8a13)
# Library krobot-interfaces
"src/interfaces": include
"src/interfaces/krobot-interfaces.cmxs": use_krobot-interfaces
@@ -64,6 +64,13 @@
<src/tools/krobot_viewer.{native,byte}>: pkg_lablgtk2.glade
<src/tools/krobot_viewer.{native,byte}>: pkg_cairo.lablgtk2
<src/tools/*.ml{,i}>: pkg_lablgtk2.glade
+# Executable send-can
+<examples/send_can.{native,byte}>: use_krobot-can
+<examples/send_can.{native,byte}>: use_krobot
+<examples/send_can.{native,byte}>: use_krobot-interfaces
+<examples/send_can.{native,byte}>: pkg_obus
+<examples/send_can.{native,byte}>: pkg_lwt.unix
+<examples/send_can.{native,byte}>: pkg_lwt.syntax
# Executable krobot-remote
<src/tools/krobot_remote.{native,byte}>: use_krobot
<src/tools/krobot_remote.{native,byte}>: use_krobot-interfaces
diff --git a/info/control2011/examples/send_can.ml b/info/control2011/examples/send_can.ml
new file mode 100644
index 0000000..9147a81
--- /dev/null
+++ b/info/control2011/examples/send_can.ml
@@ -0,0 +1,31 @@
+(*
+ * send_can.ml
+ * -----------
+ * Copyright : (c) 2011, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+open Lwt
+open Krobot_can
+
+
+lwt () =
+ if Array.length Sys.argv <> 2 then begin
+ print_endline "usage: dump-can <interface>";
+ exit 2;
+ end;
+ try_lwt
+ lwt can = Krobot_can_bus.open_can Sys.argv.(1) in
+ Krobot_can_bus.send can
+ (Unix.gettimeofday (),
+ frame
+ ~identifier:103
+ ~kind:Data
+ ~remote:true
+ ~format:F29bits
+ ~data:"")
+ with Unix.Unix_error(error, func, arg) ->
+ Lwt_log.error_f "'%s' failed with: %s" func (Unix.error_message error)
+
diff --git a/info/control2011/myocamlbuild.ml b/info/control2011/myocamlbuild.ml
index 08cc900..58c15ed 100644
--- a/info/control2011/myocamlbuild.ml
+++ b/info/control2011/myocamlbuild.ml
@@ -1,7 +1,7 @@
(* OASIS_START *)
-(* DO NOT EDIT (digest: 570b31c8f48685aca9662d28e560f425) *)
+(* DO NOT EDIT (digest: 7b180d572761c507057af7fdf11b0222) *)
module OASISGettext = struct
-# 21 "/home/dim/sources/oasis/src/oasis/OASISGettext.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISGettext.ml"
let ns_ str =
str
@@ -24,7 +24,7 @@ module OASISGettext = struct
end
module OASISExpr = struct
-# 21 "/home/dim/sources/oasis/src/oasis/OASISExpr.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISExpr.ml"
@@ -115,7 +115,7 @@ end
module BaseEnvLight = struct
-# 21 "/home/dim/sources/oasis/src/base/BaseEnvLight.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseEnvLight.ml"
module MapString = Map.Make(String)
@@ -212,7 +212,7 @@ end
module MyOCamlbuildFindlib = struct
-# 21 "/home/dim/sources/oasis/src/plugins/ocamlbuild/MyOCamlbuildFindlib.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/plugins/ocamlbuild/MyOCamlbuildFindlib.ml"
(** OCamlbuild extension, copied from
* http://brion.inria.fr/gallium/index.php/Using_ocamlfind_with_ocamlbuild
@@ -320,7 +320,7 @@ module MyOCamlbuildFindlib = struct
end
module MyOCamlbuildBase = struct
-# 21 "/home/dim/sources/oasis/src/plugins/ocamlbuild/MyOCamlbuildBase.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/plugins/ocamlbuild/MyOCamlbuildBase.ml"
(** Base functions for writing myocamlbuild.ml
@author Sylvain Le Gall
@@ -335,7 +335,7 @@ module MyOCamlbuildBase = struct
type name = string
type tag = string
-# 55 "/home/dim/sources/oasis/src/plugins/ocamlbuild/MyOCamlbuildBase.ml"
+# 55 "/home/krobot/ocaml-stuff/oasis/src/plugins/ocamlbuild/MyOCamlbuildBase.ml"
type t =
{
diff --git a/info/control2011/setup.ml b/info/control2011/setup.ml
index 506fa79..650e561 100644
--- a/info/control2011/setup.ml
+++ b/info/control2011/setup.ml
@@ -1,14 +1,14 @@
(* setup.ml generated for the first time by OASIS v0.2.0~alpha1 *)
(* OASIS_START *)
-(* DO NOT EDIT (digest: f14e1278f404cfe11dc5fe0e2cf46a8e) *)
+(* DO NOT EDIT (digest: 2a8cc2d164b159f24cb072891525da9a) *)
(*
Regenerated by OASIS v0.2.0
Visit http://oasis.forge.ocamlcore.org for more information and
documentation about functions used in this file.
*)
module OASISGettext = struct
-# 21 "/home/dim/sources/oasis/src/oasis/OASISGettext.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISGettext.ml"
let ns_ str =
str
@@ -31,7 +31,7 @@ module OASISGettext = struct
end
module OASISContext = struct
-# 21 "/home/dim/sources/oasis/src/oasis/OASISContext.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISContext.ml"
open OASISGettext
@@ -90,7 +90,7 @@ module OASISContext = struct
end
module OASISUtils = struct
-# 21 "/home/dim/sources/oasis/src/oasis/OASISUtils.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISUtils.ml"
module MapString = Map.Make(String)
@@ -242,7 +242,7 @@ module OASISUtils = struct
end
module PropList = struct
-# 21 "/home/dim/sources/oasis/src/oasis/PropList.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/PropList.ml"
open OASISGettext
@@ -277,7 +277,7 @@ module PropList = struct
let clear t =
Hashtbl.clear t
-# 59 "/home/dim/sources/oasis/src/oasis/PropList.ml"
+# 59 "/home/krobot/ocaml-stuff/oasis/src/oasis/PropList.ml"
end
module Schema =
@@ -518,7 +518,7 @@ module PropList = struct
end
module OASISMessage = struct
-# 21 "/home/dim/sources/oasis/src/oasis/OASISMessage.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISMessage.ml"
open OASISGettext
@@ -567,7 +567,7 @@ module OASISMessage = struct
end
module OASISVersion = struct
-# 21 "/home/dim/sources/oasis/src/oasis/OASISVersion.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISVersion.ml"
open OASISGettext
@@ -751,7 +751,7 @@ module OASISVersion = struct
end
module OASISLicense = struct
-# 21 "/home/dim/sources/oasis/src/oasis/OASISLicense.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISLicense.ml"
(** License for _oasis fields
@author Sylvain Le Gall
@@ -784,7 +784,7 @@ module OASISLicense = struct
end
module OASISExpr = struct
-# 21 "/home/dim/sources/oasis/src/oasis/OASISExpr.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISExpr.ml"
@@ -874,7 +874,7 @@ module OASISExpr = struct
end
module OASISTypes = struct
-# 21 "/home/dim/sources/oasis/src/oasis/OASISTypes.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISTypes.ml"
@@ -951,7 +951,7 @@ module OASISTypes = struct
type plugin_data = (all_plugin * plugin_data_purpose * (unit -> unit)) list
-# 102 "/home/dim/sources/oasis/src/oasis/OASISTypes.ml"
+# 102 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISTypes.ml"
type 'a conditional = 'a OASISExpr.choices
@@ -1105,7 +1105,7 @@ module OASISTypes = struct
end
module OASISUnixPath = struct
-# 21 "/home/dim/sources/oasis/src/oasis/OASISUnixPath.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISUnixPath.ml"
type unix_filename = string
type unix_dirname = string
@@ -1184,7 +1184,7 @@ module OASISUnixPath = struct
end
module OASISSection = struct
-# 21 "/home/dim/sources/oasis/src/oasis/OASISSection.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISSection.ml"
(** Manipulate section
@author Sylvain Le Gall
@@ -1246,12 +1246,12 @@ module OASISSection = struct
end
module OASISBuildSection = struct
-# 21 "/home/dim/sources/oasis/src/oasis/OASISBuildSection.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISBuildSection.ml"
end
module OASISExecutable = struct
-# 21 "/home/dim/sources/oasis/src/oasis/OASISExecutable.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISExecutable.ml"
open OASISTypes
@@ -1282,7 +1282,7 @@ module OASISExecutable = struct
end
module OASISLibrary = struct
-# 21 "/home/dim/sources/oasis/src/oasis/OASISLibrary.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISLibrary.ml"
open OASISTypes
open OASISUtils
@@ -1573,33 +1573,33 @@ module OASISLibrary = struct
end
module OASISFlag = struct
-# 21 "/home/dim/sources/oasis/src/oasis/OASISFlag.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISFlag.ml"
end
module OASISPackage = struct
-# 21 "/home/dim/sources/oasis/src/oasis/OASISPackage.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISPackage.ml"
end
module OASISSourceRepository = struct
-# 21 "/home/dim/sources/oasis/src/oasis/OASISSourceRepository.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISSourceRepository.ml"
end
module OASISTest = struct
-# 21 "/home/dim/sources/oasis/src/oasis/OASISTest.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISTest.ml"
end
module OASISDocument = struct
-# 21 "/home/dim/sources/oasis/src/oasis/OASISDocument.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/oasis/OASISDocument.ml"
end
module BaseEnvLight = struct
-# 21 "/home/dim/sources/oasis/src/base/BaseEnvLight.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseEnvLight.ml"
module MapString = Map.Make(String)
@@ -1696,7 +1696,7 @@ end
module BaseContext = struct
-# 21 "/home/dim/sources/oasis/src/base/BaseContext.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseContext.ml"
open OASISContext
@@ -1707,7 +1707,7 @@ module BaseContext = struct
end
module BaseMessage = struct
-# 21 "/home/dim/sources/oasis/src/base/BaseMessage.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseMessage.ml"
(** Message to user, overrid for Base
@author Sylvain Le Gall
@@ -1728,7 +1728,7 @@ module BaseMessage = struct
end
module BaseFilePath = struct
-# 21 "/home/dim/sources/oasis/src/base/BaseFilePath.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseFilePath.ml"
open Filename
@@ -1760,7 +1760,7 @@ module BaseFilePath = struct
end
module BaseEnv = struct
-# 21 "/home/dim/sources/oasis/src/base/BaseEnv.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseEnv.ml"
open OASISTypes
open OASISGettext
@@ -2219,7 +2219,7 @@ module BaseEnv = struct
end
module BaseExec = struct
-# 21 "/home/dim/sources/oasis/src/base/BaseExec.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseExec.ml"
open OASISGettext
open OASISUtils
@@ -2279,7 +2279,7 @@ module BaseExec = struct
end
module BaseFileUtil = struct
-# 21 "/home/dim/sources/oasis/src/base/BaseFileUtil.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseFileUtil.ml"
open OASISGettext
@@ -2457,7 +2457,7 @@ module BaseFileUtil = struct
end
module BaseArgExt = struct
-# 21 "/home/dim/sources/oasis/src/base/BaseArgExt.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseArgExt.ml"
open OASISUtils
open OASISGettext
@@ -2485,7 +2485,7 @@ module BaseArgExt = struct
end
module BaseCheck = struct
-# 21 "/home/dim/sources/oasis/src/base/BaseCheck.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseCheck.ml"
open BaseEnv
open BaseMessage
@@ -2611,7 +2611,7 @@ module BaseCheck = struct
end
module BaseOCamlcConfig = struct
-# 21 "/home/dim/sources/oasis/src/base/BaseOCamlcConfig.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseOCamlcConfig.ml"
open BaseEnv
@@ -2713,7 +2713,7 @@ module BaseOCamlcConfig = struct
end
module BaseStandardVar = struct
-# 21 "/home/dim/sources/oasis/src/base/BaseStandardVar.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseStandardVar.ml"
open OASISGettext
@@ -2973,7 +2973,7 @@ module BaseStandardVar = struct
end
module BaseFileAB = struct
-# 21 "/home/dim/sources/oasis/src/base/BaseFileAB.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseFileAB.ml"
open BaseEnv
open OASISGettext
@@ -3021,7 +3021,7 @@ module BaseFileAB = struct
end
module BaseLog = struct
-# 21 "/home/dim/sources/oasis/src/base/BaseLog.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseLog.ml"
open OASISUtils
@@ -3140,7 +3140,7 @@ module BaseLog = struct
end
module BaseBuilt = struct
-# 21 "/home/dim/sources/oasis/src/base/BaseBuilt.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseBuilt.ml"
open OASISTypes
open OASISGettext
@@ -3287,7 +3287,7 @@ module BaseBuilt = struct
end
module BaseCustom = struct
-# 21 "/home/dim/sources/oasis/src/base/BaseCustom.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseCustom.ml"
open BaseEnv
open BaseMessage
@@ -3337,7 +3337,7 @@ module BaseCustom = struct
end
module BaseDynVar = struct
-# 21 "/home/dim/sources/oasis/src/base/BaseDynVar.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseDynVar.ml"
open OASISTypes
@@ -3381,7 +3381,7 @@ module BaseDynVar = struct
end
module BaseTest = struct
-# 21 "/home/dim/sources/oasis/src/base/BaseTest.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseTest.ml"
open BaseEnv
open BaseMessage
@@ -3463,7 +3463,7 @@ module BaseTest = struct
end
module BaseDoc = struct
-# 21 "/home/dim/sources/oasis/src/base/BaseDoc.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseDoc.ml"
open BaseEnv
open BaseMessage
@@ -3493,7 +3493,7 @@ module BaseDoc = struct
end
module BaseSetup = struct
-# 21 "/home/dim/sources/oasis/src/base/BaseSetup.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseSetup.ml"
open BaseEnv
open BaseMessage
@@ -3911,7 +3911,7 @@ module BaseSetup = struct
end
module BaseDev = struct
-# 21 "/home/dim/sources/oasis/src/base/BaseDev.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/base/BaseDev.ml"
@@ -3969,7 +3969,7 @@ end
module InternalConfigurePlugin = struct
-# 21 "/home/dim/sources/oasis/src/plugins/internal/InternalConfigurePlugin.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/plugins/internal/InternalConfigurePlugin.ml"
(** Configure using internal scheme
@author Sylvain Le Gall
@@ -4185,7 +4185,7 @@ module InternalConfigurePlugin = struct
end
module InternalInstallPlugin = struct
-# 21 "/home/dim/sources/oasis/src/plugins/internal/InternalInstallPlugin.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/plugins/internal/InternalInstallPlugin.ml"
(** Install using internal scheme
@author Sylvain Le Gall
@@ -4584,7 +4584,7 @@ end
module OCamlbuildCommon = struct
-# 21 "/home/dim/sources/oasis/src/plugins/ocamlbuild/OCamlbuildCommon.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/plugins/ocamlbuild/OCamlbuildCommon.ml"
(** Functions common to OCamlbuild build and doc plugin
*)
@@ -4684,7 +4684,7 @@ module OCamlbuildCommon = struct
end
module OCamlbuildPlugin = struct
-# 21 "/home/dim/sources/oasis/src/plugins/ocamlbuild/OCamlbuildPlugin.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/plugins/ocamlbuild/OCamlbuildPlugin.ml"
(** Build using ocamlbuild
@author Sylvain Le Gall
@@ -4928,7 +4928,7 @@ module OCamlbuildPlugin = struct
end
module OCamlbuildDocPlugin = struct
-# 21 "/home/dim/sources/oasis/src/plugins/ocamlbuild/OCamlbuildDocPlugin.ml"
+# 21 "/home/krobot/ocaml-stuff/oasis/src/plugins/ocamlbuild/OCamlbuildDocPlugin.ml"
(* Create documentation using ocamlbuild .odocl files
@author Sylvain Le Gall
@@ -5315,6 +5315,33 @@ let setup_t =
{exec_custom = false; exec_main_is = "krobot_viewer.ml"; });
Executable
({
+ cs_name = "send-can";
+ cs_data = PropList.Data.create ();
+ cs_plugin_data = [];
+ },
+ {
+ bs_build = [(OASISExpr.EBool true, true)];
+ bs_install = [(OASISExpr.EBool true, false)];
+ bs_path = "examples";
+ bs_compiled_object = Best;
+ bs_build_depends =
+ [
+ InternalLibrary "krobot-can";
+ FindlibPackage ("lwt.syntax", None)
+ ];
+ bs_build_tools = [ExternalTool "ocamlbuild"];
+ bs_c_sources = [];
+ bs_data_files = [];
+ bs_ccopt = [(OASISExpr.EBool true, [])];
+ bs_cclib = [(OASISExpr.EBool true, [])];
+ bs_dlllib = [(OASISExpr.EBool true, [])];
+ bs_dllpath = [(OASISExpr.EBool true, [])];
+ bs_byteopt = [(OASISExpr.EBool true, [])];
+ bs_nativeopt = [(OASISExpr.EBool true, [])];
+ },
+ {exec_custom = false; exec_main_is = "send_can.ml"; });
+ Executable
+ ({
cs_name = "krobot-remote";
cs_data = PropList.Data.create ();
cs_plugin_data = [];
diff --git a/info/control2011/src/can/can_stubs.c b/info/control2011/src/can/can_stubs.c
index 2b6c711..6bdf027 100644
--- a/info/control2011/src/can/can_stubs.c
+++ b/info/control2011/src/can/can_stubs.c
@@ -70,9 +70,9 @@ CAMLprim value ocaml_can_recv(value val_fd)
/* Build the caml frame. */
val_frame = caml_alloc_tuple(5);
Field(val_frame, 0) = Val_int(frame.can_id & CAN_EFF_MASK);
- Field(val_frame, 1) = Val_int((frame.can_id << 29) & 1);
- Field(val_frame, 2) = Val_int((frame.can_id << 30) & 1);
- Field(val_frame, 3) = Val_int((frame.can_id << 31) & 1);
+ Field(val_frame, 1) = Val_int((frame.can_id >> 29) & 1);
+ Field(val_frame, 2) = Val_int((frame.can_id >> 30) & 1);
+ Field(val_frame, 3) = Val_int((frame.can_id >> 31) & 1);
val = caml_alloc_string(frame.can_dlc);
memcpy(String_val(val), frame.data, frame.can_dlc);
Field(val_frame, 4) = val;
@@ -92,9 +92,9 @@ CAMLprim value ocaml_can_send(value val_fd, value val_frame)
/* Build the can frame. */
frame.can_id = Int_val(Field(val_frame, 0)) |
- (Int_val(Field(val_frame, 1)) >> 29) |
- (Int_val(Field(val_frame, 2)) >> 30) |
- (Int_val(Field(val_frame, 3)) >> 31);
+ (Int_val(Field(val_frame, 1)) << 29) |
+ (Int_val(Field(val_frame, 2)) << 30) |
+ (Int_val(Field(val_frame, 3)) << 31);
value val_data = Field(val_frame, 4);
frame.can_dlc = caml_string_length(val_data);
memcpy(frame.data, String_val(val_data), caml_string_length(val_data));
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index a65f342..986d84d 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -461,13 +461,13 @@ module Board = struct
| (x, y) :: rest ->
(* Turn the robot. *)
let alpha = atan2 (y -. board.state.y) (x -. board.state.x) -. board.state.theta in
- lwt () = Krobot_message.send board.bus (Unix.gettimeofday (), Motor_turn(alpha, 0.1, 0.2)) in
+ lwt () = Krobot_message.send board.bus (Unix.gettimeofday (), Motor_turn(alpha, 0.5, 1.0)) in
lwt () = wait_done board in
(* Move the robot. *)
let sqr x = x *. x in
let dist = sqrt (sqr (x -. board.state.x) +. sqr (y -. board.state.y)) in
- lwt () = Krobot_message.send board.bus (Unix.gettimeofday (), Motor_move(dist, 0.1, 0.2)) in
+ lwt () = Krobot_message.send board.bus (Unix.gettimeofday (), Motor_move(dist, 0.5, 1.0)) in
lwt () = wait_done board in
(* Remove the point. *)
hooks/post-receive
--
krobot
|
|
From: Nicolas D. <Ba...@us...> - 2011-03-29 00:08:19
|
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 98cd4fb2991a9b603bc9589096ef037851b59d98 (commit)
from a2b6a8fdf2fde7a3df6bfbc187d2b6fe6babe349 (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 98cd4fb2991a9b603bc9589096ef037851b59d98
Author: Nicolas Dandrimont <Nic...@cr...>
Date: Tue Mar 29 02:05:07 2011 +0200
[USB_CAN] Use correct protocol for packet send ACK.
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/USB_CAN/Firmware/usb_can/main.c b/elec/boards/USB_CAN/Firmware/usb_can/main.c
index 2da2acd..278d29d 100644
--- a/elec/boards/USB_CAN/Firmware/usb_can/main.c
+++ b/elec/boards/USB_CAN/Firmware/usb_can/main.c
@@ -95,7 +95,7 @@ static void init(void)
static void NORETURN serial_receive_process(void)
{
- int nbytes, retval, i = 0;
+ int nbytes = 0, retval, i = 0;
char command[MAX_CMD_SIZE+1];
for (;;) {
@@ -107,8 +107,6 @@ static void NORETURN serial_receive_process(void)
LED1_ON();
else
LED1_OFF();
- } else {
- kprintf("got EOF :(\n");
}
}
}
@@ -124,7 +122,6 @@ static void NORETURN can_receive_process(void) {
received = can_receive(usbcan.can, &frame, ms_to_ticks(100));
if (received) {
retval = usb_can_emit(&usbcan, &frame);
- kprintf("received something... %d %08lx %08lx\n", frame.ide ? frame.eid:frame.sid, frame.data32[0], frame.data32[1]);
}
}
}
diff --git a/elec/boards/USB_CAN/Firmware/usb_can/usb_can.c b/elec/boards/USB_CAN/Firmware/usb_can/usb_can.c
index 75d3c4a..07564ec 100644
--- a/elec/boards/USB_CAN/Firmware/usb_can/usb_can.c
+++ b/elec/boards/USB_CAN/Firmware/usb_can/usb_can.c
@@ -89,7 +89,7 @@ void usb_can_set_baudrate(usb_can *usbcan, char *baudrate) {
int usb_can_execute_command(usb_can *usbcan, char *command) {
can_tx_frame frame;
- bool send, ret;
+ bool send = false, ret = false;
int i;
frame.rtr = 0;
@@ -168,7 +168,7 @@ int usb_can_execute_command(usb_can *usbcan, char *command) {
if (send) {
ret = can_transmit(usbcan->can, &frame, ms_to_ticks(10));
if (ret)
- kfile_write(&usbcan->ser->fd, frame.ide ? "Z\r" : "z\r", 2);
+ kfile_write(&usbcan->ser->fd, frame.ide ? "\r" : "\r", 1);
else
kfile_write(&usbcan->ser->fd, "\a", 1);
}
hooks/post-receive
--
krobot
|
|
From: Jérémie D. <Ba...@us...> - 2011-03-28 23:21:10
|
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 a2b6a8fdf2fde7a3df6bfbc187d2b6fe6babe349 (commit)
via b83b2f61af4e53a1be00ca12acf4af47ba239a55 (commit)
via 14d2b497db894d53b3bbcf04dfe2f42b60357d1d (commit)
from 78319e20cdca5e7f40a071a8881e9307a0a79edd (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 a2b6a8fdf2fde7a3df6bfbc187d2b6fe6babe349
Author: Jérémie Dimino <je...@di...>
Date: Tue Mar 29 00:56:48 2011 +0200
[info] typo in error messages for the can
commit b83b2f61af4e53a1be00ca12acf4af47ba239a55
Author: Jérémie Dimino <je...@di...>
Date: Tue Mar 29 00:47:22 2011 +0200
[info] do not redraw when not needed in the viewer
commit 14d2b497db894d53b3bbcf04dfe2f42b60357d1d
Author: Jérémie Dimino <je...@di...>
Date: Tue Mar 29 00:45:04 2011 +0200
[info] send extend can frame instead of small ones
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/can/can_stubs.c b/info/control2011/src/can/can_stubs.c
index 9cdc4b3..2b6c711 100644
--- a/info/control2011/src/can/can_stubs.c
+++ b/info/control2011/src/can/can_stubs.c
@@ -61,7 +61,7 @@ CAMLprim value ocaml_can_recv(value val_fd)
if (ret < 0) uerror("recv", Nothing);
/* It is an error if we do not receive exactly one frame. */
- if (ret != sizeof(frame)) caml_failwith("recvmsg: invalid size");
+ if (ret != sizeof(frame)) caml_failwith("recv: invalid size");
/* Receive the timestamp. */
struct timeval tv;
@@ -101,10 +101,10 @@ CAMLprim value ocaml_can_send(value val_fd, value val_frame)
/* Send the frame. */
int ret = send(Int_val(val_fd), &frame, sizeof(frame), 0);
- if (ret < 0) uerror("sendmsg", Nothing);
+ if (ret < 0) uerror("send", Nothing);
/* It is an error if we do not sent exactly one frame. */
- if (ret != sizeof(frame)) caml_failwith("sendmsg: invalid size");
+ if (ret != sizeof(frame)) caml_failwith("send: invalid size");
return Val_unit;
}
diff --git a/info/control2011/src/lib/krobot_message.ml b/info/control2011/src/lib/krobot_message.ml
index ae4fe99..76eccc5 100644
--- a/info/control2011/src/lib/krobot_message.ml
+++ b/info/control2011/src/lib/krobot_message.ml
@@ -88,7 +88,7 @@ let encode = function
~identifier:100
~kind:Data
~remote:false
- ~format:F11bits
+ ~format:F29bits
~data
| Encoder_position_speed_3(pos, speed) ->
let data = String.create 8 in
@@ -98,7 +98,7 @@ let encode = function
~identifier:101
~kind:Data
~remote:false
- ~format:F11bits
+ ~format:F29bits
~data
| Encoder_position_speed_4(pos, speed) ->
let data = String.create 8 in
@@ -108,7 +108,7 @@ let encode = function
~identifier:102
~kind:Data
~remote:false
- ~format:F11bits
+ ~format:F29bits
~data
| Motor_status moving ->
let data = String.create 1 in
@@ -117,7 +117,7 @@ let encode = function
~identifier:103
~kind:Data
~remote:false
- ~format:F11bits
+ ~format:F29bits
~data
| Odometry(x, y, theta) ->
let data = String.create 6 in
@@ -128,7 +128,7 @@ let encode = function
~identifier:104
~kind:Data
~remote:false
- ~format:F11bits
+ ~format:F29bits
~data
| Motor_move(dist, speed, acc) ->
let data = String.create 8 in
@@ -139,7 +139,7 @@ let encode = function
~identifier:201
~kind:Data
~remote:false
- ~format:F11bits
+ ~format:F29bits
~data
| Motor_turn(angle, speed, acc) ->
let data = String.create 8 in
@@ -150,14 +150,14 @@ let encode = function
~identifier:202
~kind:Data
~remote:false
- ~format:F11bits
+ ~format:F29bits
~data
| Req_motor_status ->
frame
~identifier:103
~kind:Data
~remote:true
- ~format:F11bits
+ ~format:F29bits
~data:""
| Unknown frame ->
frame
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index c5561c2..a65f342 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -498,8 +498,11 @@ module Board = struct
(fun (ts, frame) ->
match frame with
| Odometry(x, y, theta) ->
- board.state <- { x; y; theta };
- queue_draw board
+ let state = { x; y; theta } in
+ if state <> board.state then begin
+ board.state <- state;
+ queue_draw board
+ end
| _ ->
())
(Krobot_message.recv bus)
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2011-03-28 23:17:58
|
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 78319e20cdca5e7f40a071a8881e9307a0a79edd (commit)
via 8ded60a69aed3948eade6a03ff399ca366925af5 (commit)
from 00945a9816ab345b174312f946a0eba895b8b045 (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 78319e20cdca5e7f40a071a8881e9307a0a79edd
Author: Xavier Lagorce <Xav...@cr...>
Date: Tue Mar 29 00:57:14 2011 +0200
[Controller_Motor_STM32] Updated BeRTOS
commit 8ded60a69aed3948eade6a03ff399ca366925af5
Author: Xavier Lagorce <Xav...@cr...>
Date: Tue Mar 29 00:54:06 2011 +0200
[Controller_Motor_STM32] Added odometry and readying for tests
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/bertos/cfg/cfg_adc.h b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cfg/cfg_adc.h
index df2587b..62ee6ed 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/bertos/cfg/cfg_adc.h
+++ b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cfg/cfg_adc.h
@@ -109,4 +109,41 @@
*/
#define CONFIG_ADC_STROBE 0
+
+/**
+ * Start up timer[s] = startup value / ADCClock [Hz]
+ *
+ * $WIZ$ type = "enum"
+ * $WIZ$ value_list = "sam3_adc_sut"
+ * $WIZ$ supports = "sam3"
+ */
+#define CONFIG_ADC_SUT ADC_SUT512
+
+/**
+ * Analog Settling Time[s] = settling value / ADCClock[Hz]
+ *
+ * $WIZ$ type = "enum"
+ * $WIZ$ value_list = "sam3_adc_stt"
+ * $WIZ$ supports = "sam3"
+ */
+#define CONFIG_ADC_STTLING ADC_AST17
+
+/**
+ * Tracking Time[s] = (TRACKTIM + 1) / ADCClock[Hz]
+ *
+ * $WIZ$ type = "int"
+ * $WIZ$ min = 0
+ * $WIZ$ supports = "sam3"
+ */
+#define CONFIG_ADC_TRACKTIM 0
+
+/**
+ * Transfer Period[s] = (TRANSFER * 2 + 3) ADCClock[Hz]
+ *
+ * $WIZ$ type = "int"
+ * $WIZ$ min = 0
+ * $WIZ$ supports = "sam3"
+ */
+#define CONFIG_ADC_TRANSFER 1
+
#endif /* CFG_ADC_H */
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/bertos/cfg/cfg_can.h b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cfg/cfg_can.h
index 827bc96..9d92690 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/bertos/cfg/cfg_can.h
+++ b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cfg/cfg_can.h
@@ -33,6 +33,7 @@
* \brief Configuration file for the CAN module.
*
* \author Nicolas Dandrimont <Nic...@cr...>
+ *
*/
#ifndef CFG_CAN_H
@@ -54,4 +55,13 @@
*/
#define CAN_LOG_FORMAT LOG_FMT_VERBOSE
+/**
+ * CAN remapping
+ *
+ * $WIZ$ type = "enum"
+ * $WIZ$ value_list = "can_stm32_remaps"
+ * $WIZ$ supports = "stm32"
+ */
+#define CAN_STM32_REMAP CAN_STM32_PORTA
+
#endif /* CFG_CAN_H */
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/bertos/cfg/cfg_dac.h b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cfg/cfg_dac.h
new file mode 100644
index 0000000..8eff7bc
--- /dev/null
+++ b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cfg/cfg_dac.h
@@ -0,0 +1,58 @@
+/**
+ * \file
+ * <!--
+ * This file is part of BeRTOS.
+ *
+ * Bertos is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ * As a special exception, you may use this file as part of a free software
+ * library without restriction. Specifically, if other files instantiate
+ * templates or use macros or inline functions from this file, or you compile
+ * this file and link it with other files to produce an executable, this
+ * file does not by itself cause the resulting executable to be covered by
+ * the GNU General Public License. This exception does not however
+ * invalidate any other reasons why the executable file might be covered by
+ * the GNU General Public License.
+ *
+ * Copyright 2011 Develer S.r.l. (http://www.develer.com/)
+ * All Rights Reserved.
+ * -->
+ *
+ * \brief Configuration file for DAC module.
+ *
+ *
+ * \author Daniele Basile <as...@de...>
+ */
+
+#ifndef CFG_DAC_H
+#define CFG_DAC_H
+
+/**
+ * Module logging level.
+ *
+ * $WIZ$ type = "enum"
+ * $WIZ$ value_list = "log_level"
+ */
+#define DAC_LOG_LEVEL LOG_LVL_WARN
+
+/**
+ * Module logging format.
+ *
+ * $WIZ$ type = "enum"
+ * $WIZ$ value_list = "log_format"
+ */
+#define DAC_LOG_FORMAT LOG_FMT_TERSE
+
+#endif /* CFG_DAC_H */
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/bertos/cfg/cfg_led_7seg.h b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cfg/cfg_led_7seg.h
new file mode 100644
index 0000000..d854b11
--- /dev/null
+++ b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cfg/cfg_led_7seg.h
@@ -0,0 +1,80 @@
+/**
+ * \file cfg_led_7seg.h
+ * <!--
+ * This file is part of BeRTOS.
+ *
+ * Bertos is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ * As a special exception, you may use this file as part of a free software
+ * library without restriction. Specifically, if other files instantiate
+ * templates or use macros or inline functions from this file, or you compile
+ * this file and link it with other files to produce an executable, this
+ * file does not by itself cause the resulting executable to be covered by
+ * the GNU General Public License. This exception does not however
+ * invalidate any other reasons why the executable file might be covered by
+ * the GNU General Public License.
+ *
+ * Copyright 2010 Develer S.r.l. (http://www.develer.com/)
+ * -->
+ *
+ * \brief Configuration file for led 7 segment display.
+ *
+ * \author Fabio Bizzi <fb...@bi...>
+ *
+ * \addtogroup SevenSegDisplay 7 Segments LED Displays Driver
+ * \{
+ *
+ */
+
+#ifndef CFG_LED_7SEG_H
+#define CFG_LED_7SEG_H
+
+/**
+ * Use a Common Cathode display.
+ * $WIZ$ type = "boolean"
+ */
+#define CONFIG_LED_7SEG_CCAT 0
+
+/**
+ * Number of digit present in the LED display.
+ * $WIZ$ type = "int"
+ * $WIZ$ min = 1
+ * $WIZ$ max = 8
+ */
+#define CONFIG_LED_7SEG_DIGIT 4
+
+/**
+ * Max lenght of the string to be displayed.
+ * $WIZ$ type = "int"
+ * $WIZ$ min = 16
+ * $WIZ$ max = 255
+ */
+#define CONFIG_LED_7SEG_STRLEN 255
+
+/**
+ * Default scrolling speed (ms * CONFIG_LED_7SEG_RTIME).
+ * $WIZ$ type = "int"
+ */
+#define CONFIG_LED_7SEG_SSPEED 10
+
+/**
+ * Default refresh time (ms).
+ * $WIZ$ type = "int"
+ */
+#define CONFIG_LED_7SEG_RTIME 5
+
+#endif /* CFG_LED_7SEG_H */
+ /** \} */ //defgroup drivers
+
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/bertos/cfg/cfg_pwm.h b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cfg/cfg_pwm.h
index 595189e..68d6283 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/bertos/cfg/cfg_pwm.h
+++ b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cfg/cfg_pwm.h
@@ -53,4 +53,12 @@
*/
#define PWM_LOG_FORMAT LOG_FMT_VERBOSE
+/**
+ * Enable the OLD pwm API.
+ * Not recommended for new projects.
+ *
+ * $WIZ$ type = "boolean"
+ */
+#define CFG_PWM_ENABLE_OLD_API 1
+
#endif /* CFG_PWM_H */
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/bertos/cfg/compiler.h b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cfg/compiler.h
index ac60848..3a2f34c 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/bertos/cfg/compiler.h
+++ b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cfg/compiler.h
@@ -450,6 +450,9 @@
#endif
#endif
+/** User defined callback type */
+typedef void (*Hook)(void *);
+
/** Bulk storage large enough for both pointers or integers. */
typedef void * iptr_t;
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/bertos/cfg/debug.h b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cfg/debug.h
index efa26e1..ef6717c 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/bertos/cfg/debug.h
+++ b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cfg/debug.h
@@ -55,16 +55,21 @@
#include <cfg/os.h>
#include <cfg/compiler.h>
+#include "cfg/cfg_debug.h" /* CONFIG_KDEBUG_* */
/*
* Defaults for rarely used config stuff.
*/
-#ifndef CONFIG_KDEBUG_DISABLE_TRACE
-#define CONFIG_KDEBUG_DISABLE_TRACE 0
+#ifndef CONFIG_KDEBUG_TRACE
+#define CONFIG_KDEBUG_TRACE 1
#endif
-#ifndef CONFIG_KDEBUG_ASSERT_NO_TEXT
-#define CONFIG_KDEBUG_ASSERT_NO_TEXT 0
+#ifndef CONFIG_KDEBUG_VERBOSE_ASSERT
+#define CONFIG_KDEBUG_VERBOSE_ASSERT 1
+#endif
+
+#ifndef CONFIG_KDEBUG_WALLS
+#define CONFIG_KDEBUG_WALLS 1
#endif
#if defined(__doxygen__)
@@ -127,7 +132,6 @@
*/
#define DB(x) x
- #include "cfg/cfg_debug.h" /* CONFIG_KDEBUG_ASSERT_NO_TEXT */
#include <cpu/attr.h> /* CPU_HARVARD */
/* These are implemented in drv/kdebug.c */
@@ -163,7 +167,7 @@
int __check_wall(long *wall, int size, const char *name, const char *file, int line);
#endif /* !CPU_HARVARD */
- #if !CONFIG_KDEBUG_ASSERT_NO_TEXT
+ #if CONFIG_KDEBUG_VERBOSE_ASSERT
/**
* Assert a pre-condition on code.
*/
@@ -203,7 +207,7 @@
|| ((void *)(p) >= (void *)CPU_RAM_START)) \
? 0 : __invalid_ptr((p), #p, THIS_FILE, __LINE__)))
- #if !CONFIG_KDEBUG_DISABLE_TRACE
+ #if CONFIG_KDEBUG_TRACE
#define TRACE __trace(__func__)
#define TRACEMSG(msg,...) __tracemsg(__func__, msg, ## __VA_ARGS__)
#else
@@ -211,19 +215,6 @@
#define TRACEMSG(...) do {} while(0)
#endif
-
- /**
- * \name Walls to detect data corruption
- * \{
- */
- #define WALL_SIZE 8
- #define WALL_VALUE (long)0xABADCAFEL
- #define DECLARE_WALL(name,size) long name[(size) / sizeof(long)];
- #define FWD_DECLARE_WALL(name,size) extern long name[(size) / sizeof(long)];
- #define INIT_WALL(name) __init_wall((name), countof(name))
- #define CHECK_WALL(name) __check_wall((name), countof(name), #name, THIS_FILE, __LINE__)
- /*\}*/
-
/**
* Check that the given pointer actually points to an object
* of the specified type.
@@ -308,11 +299,6 @@
}
#endif
- #define DECLARE_WALL(name, size) /* nothing */
- #define FWD_DECLARE_WALL(name, size) /* nothing */
- #define INIT_WALL(name) do {} while (0)
- #define CHECK_WALL(name) do {} while (0)
-
#define NEW_INSTANCE(CLASS) do {} while (0)
#define DELETE_INSTANCE(CLASS) do {} while (0)
#define ASSERT_ZERO_INSTANCES(CLASS) do {} while (0)
@@ -336,6 +322,25 @@
#endif /* _DEBUG */
+#if CONFIG_KDEBUG_WALLS
+ /**
+ * \name Walls to detect data corruption
+ * \{
+ */
+ #define WALL_SIZE 8
+ #define WALL_VALUE (long)0xABADCAFEL
+ #define DECLARE_WALL(name,size) long name[(size) / sizeof(long)];
+ #define FWD_DECLARE_WALL(name,size) extern long name[(size) / sizeof(long)];
+ #define INIT_WALL(name) __init_wall((name), countof(name))
+ #define CHECK_WALL(name) __check_wall((name), countof(name), #name, THIS_FILE, __LINE__)
+ /*\}*/
+#else
+ #define DECLARE_WALL(name, size) /* nothing */
+ #define FWD_DECLARE_WALL(name, size) /* nothing */
+ #define INIT_WALL(name) do {} while (0)
+ #define CHECK_WALL(name) do {} while (0)
+#endif
+
/** \} */ // defgroup debug
#endif /* BERTOS_DEBUG_H */
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/arm/drv/pwm_at91.c b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/arm/drv/pwm_at91.c
index 397240e..c5780c3 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/arm/drv/pwm_at91.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/arm/drv/pwm_at91.c
@@ -26,7 +26,7 @@
* invalidate any other reasons why the executable file might be covered by
* the GNU General Public License.
*
- * Copyright 2008 Develer S.r.l. (http://www.develer.com/)
+ * Copyright 2011 Develer S.r.l. (http://www.develer.com/)
*
* -->
*
@@ -34,10 +34,11 @@
* \brief PWM hardware-specific implementation
*
* \author Daniele Basile <as...@de...>
+ * \author Francesco Sacchi <ba...@de...>
*/
+#include <drv/pwm.h>
#include "pwm_at91.h"
-#include "hw/pwm_map.h"
#include <hw/hw_cpufreq.h>
#include "cfg/cfg_pwm.h"
@@ -50,201 +51,337 @@
#include <cfg/debug.h>
#include <io/arm.h>
+#include <cpu/irq.h>
+#define PWM_HW_MAX_PRESCALER_STEP 10
+#define PWM_HW_MAX_PERIOD 0xFFFF
-/**
- * Register structure for pwm driver.
- * This array content all data and register pointer
- * to manage pwm peripheral device.
- */
-static PwmChannel pwm_map[PWM_CNT] =
-{
- {//PWM Channel 0
- .duty_zero = false,
- .pol = false,
- .pwm_pin = BV(PWM0),
- .mode_reg = &PWM_CMR0,
- .duty_reg = &PWM_CDTY0,
- .period_reg = &PWM_CPRD0,
- .update_reg = &PWM_CUPD0,
- },
- {//PWM Channel 1
- .duty_zero = false,
- .pol = false,
- .pwm_pin = BV(PWM1),
- .mode_reg = &PWM_CMR1,
- .duty_reg = &PWM_CDTY1,
- .period_reg = &PWM_CPRD1,
- .update_reg = &PWM_CUPD1,
- },
- {//PWM Channel 2
- .duty_zero = false,
- .pol = false,
- .pwm_pin = BV(PWM2),
- .mode_reg = &PWM_CMR2,
- .duty_reg = &PWM_CDTY2,
- .period_reg = &PWM_CPRD2,
- .update_reg = &PWM_CUPD2,
- },
- {//PWM Channel 3
- .duty_zero = false,
- .pol = false,
- .pwm_pin = BV(PWM3),
- .mode_reg = &PWM_CMR3,
- .duty_reg = &PWM_CDTY3,
- .period_reg = &PWM_CPRD3,
- .update_reg = &PWM_CUPD3,
- }
-};
+#if CFG_PWM_ENABLE_OLD_API
+ #include "hw/pwm_map.h"
+ /**
+ * Register structure for pwm driver.
+ * This array content all data and register pointer
+ * to manage pwm peripheral device.
+ */
+ static PwmChannel pwm_map[PWM_CNT] =
+ {
+ {//PWM Channel 0
+ .duty_zero = false,
+ .pol = false,
+ .pwm_pin = BV(PWM0),
+ .mode_reg = &PWM_CMR0,
+ .duty_reg = &PWM_CDTY0,
+ .period_reg = &PWM_CPRD0,
+ .update_reg = &PWM_CUPD0,
+ },
+ {//PWM Channel 1
+ .duty_zero = false,
+ .pol = false,
+ .pwm_pin = BV(PWM1),
+ .mode_reg = &PWM_CMR1,
+ .duty_reg = &PWM_CDTY1,
+ .period_reg = &PWM_CPRD1,
+ .update_reg = &PWM_CUPD1,
+ },
+ {//PWM Channel 2
+ .duty_zero = false,
+ .pol = false,
+ .pwm_pin = BV(PWM2),
+ .mode_reg = &PWM_CMR2,
+ .duty_reg = &PWM_CDTY2,
+ .period_reg = &PWM_CPRD2,
+ .update_reg = &PWM_CUPD2,
+ },
+ {//PWM Channel 3
+ .duty_zero = false,
+ .pol = false,
+ .pwm_pin = BV(PWM3),
+ .mode_reg = &PWM_CMR3,
+ .duty_reg = &PWM_CDTY3,
+ .period_reg = &PWM_CPRD3,
+ .update_reg = &PWM_CUPD3,
+ }
+ };
-/**
- * Get preiod from select channel
- *
- * \a dev channel
- */
-pwm_period_t pwm_hw_getPeriod(PwmDev dev)
-{
- return *pwm_map[dev].period_reg;
-}
-/**
- * Set pwm waveform frequecy.
- *
- * \a freq in Hz
- */
-void pwm_hw_setFrequency(PwmDev dev, uint32_t freq)
-{
- uint32_t period = 0;
+ /**
+ * Get preiod from select channel
+ *
+ * \a dev channel
+ */
+ pwm_period_t pwm_hw_getPeriod(PwmDev dev)
+ {
+ return *pwm_map[dev].period_reg;
+ }
- for(int i = 0; i <= PWM_HW_MAX_PRESCALER_STEP; i++)
+ /**
+ * Set pwm waveform frequecy.
+ *
+ * \a freq in Hz
+ */
+ void pwm_hw_setFrequency(PwmDev dev, uint32_t freq)
{
- period = CPU_FREQ / (BV(i) * freq);
- LOG_INFO("period[%ld], prescale[%d]\n", period, i);
- if ((period < PWM_HW_MAX_PERIOD) && (period != 0))
+ uint32_t period = 0;
+
+ for(int i = 0; i <= PWM_HW_MAX_PRESCALER_STEP; i++)
{
- //Clean previous channel prescaler, and set new
- *pwm_map[dev].mode_reg &= ~PWM_CPRE_MCK_MASK;
- *pwm_map[dev].mode_reg |= i;
- //Set pwm period
- *pwm_map[dev].period_reg = period;
- break;
+ period = CPU_FREQ / (BV(i) * freq);
+ LOG_INFO("period[%ld], prescale[%d]\n", period, i);
+ if ((period < PWM_HW_MAX_PERIOD) && (period != 0))
+ {
+ //Clean previous channel prescaler, and set new
+ *pwm_map[dev].mode_reg &= ~PWM_CPRE_MCK_MASK;
+ *pwm_map[dev].mode_reg |= i;
+ //Set pwm period
+ *pwm_map[dev].period_reg = period;
+ break;
+ }
}
+
+ LOG_INFO("PWM ch[%d] period[%ld]\n", dev, period);
}
- LOG_INFO("PWM ch[%d] period[%ld]\n", dev, period);
-}
+ /**
+ * Set pwm duty cycle.
+ *
+ * \a duty value 0 - 2^16
+ */
+ void pwm_hw_setDutyUnlock(PwmDev dev, uint16_t duty)
+ {
+ ASSERT(duty <= (uint16_t)*pwm_map[dev].period_reg);
+
-/**
- * Set pwm duty cycle.
- *
- * \a duty value 0 - 2^16
- */
-void pwm_hw_setDutyUnlock(PwmDev dev, uint16_t duty)
-{
- ASSERT(duty <= (uint16_t)*pwm_map[dev].period_reg);
+ /*
+ * If polarity flag is true we must invert
+ * PWM polarity.
+ */
+ if (pwm_map[dev].pol)
+ {
+ duty = (uint16_t)*pwm_map[dev].period_reg - duty;
+ LOG_INFO("Inverted duty[%d], pol[%d]\n", duty, pwm_map[dev].pol);
+ }
+ /*
+ * WARNING: is forbidden to write 0 to duty cycle value,
+ * and so for duty = 0 we must enable PIO and clear output!
+ */
+ if (!duty)
+ {
+ PWM_PIO_CODR = pwm_map[dev].pwm_pin;
+ PWM_PIO_PER = pwm_map[dev].pwm_pin;
+ pwm_map[dev].duty_zero = true;
+ }
+ else
+ {
+ PWM_PIO_PDR = pwm_map[dev].pwm_pin;
+ PWM_PIO_ABSR = pwm_map[dev].pwm_pin;
- /*
- * If polarity flag is true we must invert
- * PWM polarity.
+ *pwm_map[dev].update_reg = duty;
+ pwm_map[dev].duty_zero = false;
+ }
+
+ PWM_ENA = BV(dev);
+ LOG_INFO("PWM ch[%d] duty[%d], period[%ld]\n", dev, duty, *pwm_map[dev].period_reg);
+ }
+
+
+ /**
+ * Enable select pwm channel
*/
- if (pwm_map[dev].pol)
+ void pwm_hw_enable(PwmDev dev)
{
- duty = (uint16_t)*pwm_map[dev].period_reg - duty;
- LOG_INFO("Inverted duty[%d], pol[%d]\n", duty, pwm_map[dev].pol);
+ if (!pwm_map[dev].duty_zero)
+ {
+ PWM_PIO_PDR = pwm_map[dev].pwm_pin;
+ PWM_PIO_ABSR = pwm_map[dev].pwm_pin;
+ }
}
- /*
- * WARNING: is forbidden to write 0 to duty cycle value,
- * and so for duty = 0 we must enable PIO and clear output!
+ /**
+ * Disable select pwm channel
*/
- if (!duty)
+ void pwm_hw_disable(PwmDev dev)
{
- PWM_PIO_CODR = pwm_map[dev].pwm_pin;
- PWM_PIO_PER = pwm_map[dev].pwm_pin;
- pwm_map[dev].duty_zero = true;
+ PWM_PIO_PER = pwm_map[dev].pwm_pin;
}
- else
- {
- PWM_PIO_PDR = pwm_map[dev].pwm_pin;
- PWM_PIO_ABSR = pwm_map[dev].pwm_pin;
- *pwm_map[dev].update_reg = duty;
- pwm_map[dev].duty_zero = false;
+ /**
+ * Set PWM polarity to select pwm channel
+ */
+ void pwm_hw_setPolarity(PwmDev dev, bool pol)
+ {
+ pwm_map[dev].pol = pol;
+ LOG_INFO("Set pol[%d]\n", pwm_map[dev].pol);
}
- PWM_ENA = BV(dev);
- LOG_INFO("PWM ch[%d] duty[%d], period[%ld]\n", dev, duty, *pwm_map[dev].period_reg);
-}
+ /**
+ * Init pwm.
+ */
+ void pwm_hw_init(void)
+ {
+ /*
+ * Init pwm:
+ * WARNING: is forbidden to write 0 to duty cycle value,
+ * and so for duty = 0 we must enable PIO and clear output!
+ * - clear PIO outputs
+ * - enable PIO outputs
+ * - Disable PIO and enable PWM functions
+ * - Power on PWM
+ */
+ PWM_PIO_CODR = BV(PWM0) | BV(PWM1) | BV(PWM2) | BV(PWM3);
+ PWM_PIO_OER = BV(PWM0) | BV(PWM1) | BV(PWM2) | BV(PWM3);
+ PWM_PIO_PDR = BV(PWM0) | BV(PWM1) | BV(PWM2) | BV(PWM3);
+ PWM_PIO_ABSR = BV(PWM0) | BV(PWM1) | BV(PWM2) | BV(PWM3);
+ PMC_PCER |= BV(PWMC_ID);
+
+ /* Disable all channels. */
+ PWM_DIS = 0xFFFFFFFF;
+ /* Disable prescalers A and B */
+ PWM_MR = 0;
+
+ /*
+ * Set pwm mode:
+ * - set period alidned to left
+ * - set output waveform to start at high level
+ * - allow duty cycle modify at next period event
+ */
+ for (int ch = 0; ch < PWM_CNT; ch++)
+ {
+ *pwm_map[ch].mode_reg = 0;
+ *pwm_map[ch].mode_reg = BV(PWM_CPOL);
+ }
-/**
- * Enable select pwm channel
- */
-void pwm_hw_enable(PwmDev dev)
-{
- if (!pwm_map[dev].duty_zero)
- {
- PWM_PIO_PDR = pwm_map[dev].pwm_pin;
- PWM_PIO_ABSR = pwm_map[dev].pwm_pin;
}
-}
-/**
- * Disable select pwm channel
- */
-void pwm_hw_disable(PwmDev dev)
-{
- PWM_PIO_PER = pwm_map[dev].pwm_pin;
-}
+#else
-/**
- * Set PWM polarity to select pwm channel
- */
-void pwm_hw_setPolarity(PwmDev dev, bool pol)
-{
- pwm_map[dev].pol = pol;
- LOG_INFO("Set pol[%d]\n", pwm_map[dev].pol);
-}
+ typedef struct PwmChannelRegs
+ {
+ reg32_t CMR;
+ reg32_t CDTY;
+ reg32_t CPRD;
+ reg32_t CCNT;
+ reg32_t CUPD;
+ } PwmChannelRegs;
-/**
- * Init pwm.
- */
-void pwm_hw_init(void)
-{
/*
- * Init pwm:
- * WARNING: is forbidden to write 0 to duty cycle value,
- * and so for duty = 0 we must enable PIO and clear output!
- * - clear PIO outputs
- * - enable PIO outputs
- * - Disable PIO and enable PWM functions
- * - Power on PWM
+ * Set pwm waveform frequecy.
*/
- PWM_PIO_CODR = BV(PWM0) | BV(PWM1) | BV(PWM2) | BV(PWM3);
- PWM_PIO_OER = BV(PWM0) | BV(PWM1) | BV(PWM2) | BV(PWM3);
- PWM_PIO_PDR = BV(PWM0) | BV(PWM1) | BV(PWM2) | BV(PWM3);
- PWM_PIO_ABSR = BV(PWM0) | BV(PWM1) | BV(PWM2) | BV(PWM3);
- PMC_PCER |= BV(PWMC_ID);
+ void pwm_hw_setFrequency(Pwm *ctx, pwm_freq_t freq)
+ {
+ uint32_t period = 0;
+
+ for(int i = 0; i <= PWM_HW_MAX_PRESCALER_STEP; i++)
+ {
+ period = CPU_FREQ / (BV(i) * freq);
+ LOG_INFO("period[%ld], prescale[%d]\n", period, i);
+ if ((period < PWM_HW_MAX_PERIOD) && (period != 0))
+ {
+ //Clear previous channel prescaler, and set new
+ ctx->hw->base->CMR &= ~PWM_CPRE_MCK_MASK;
+ ctx->hw->base->CMR |= i;
+ //Set pwm period
+ ATOMIC(
+ ctx->hw->base->CPRD = period;
+ ctx->hw->base->CDTY = period;
+ );
+ break;
+ }
+ }
+
+ LOG_INFO("PWM ch[%d] period[%ld]\n", ctx->ch, period);
+ }
- /* Disable all channels. */
- PWM_DIS = 0xFFFFFFFF;
- /* Disable prescalers A and B */
- PWM_MR = 0;
+ pwm_hwreg_t pwm_hw_getPeriod(Pwm *ctx)
+ {
+ return ctx->hw->base->CPRD;
+ }
/*
- * Set pwm mode:
- * - set period alidned to left
- * - set output waveform to start at high level
- * - allow duty cycle modify at next period event
+ * Set pwm duty cycle.
+ *
+ * duty value 0 - (2^16 - 1)
*/
- for (int ch = 0; ch < PWM_CNT; ch++)
+ void pwm_hw_setDuty(Pwm *ctx, pwm_hwreg_t hw_duty)
{
- *pwm_map[ch].mode_reg = 0;
- *pwm_map[ch].mode_reg = BV(PWM_CPOL);
+ ASSERT(hw_duty <= ctx->hw->base->CPRD);
+
+ /*
+ * WARNING: is forbidden to write 0 or 1 to duty cycle value,
+ * and so for duty < 2 we must enable PIO and clear output!
+ */
+ if (hw_duty < 2)
+ {
+ hw_duty = 2;
+ PWM_PIO_PER = ctx->hw->pwm_pin;
+ }
+ else
+ PWM_PIO_PDR = ctx->hw->pwm_pin;
+
+ ctx->hw->base->CUPD = hw_duty;
+ LOG_INFO("PWM ch[%d] duty[%d], period[%ld]\n", ctx->ch, hw_duty, ctx->hw->base->CPRD);
}
-}
+ static PwmHardware pwm_channels[] =
+ {
+ {//PWM Channel 0
+ .pwm_pin = BV(PWM0),
+ .base = (volatile PwmChannelRegs *)&PWM_CMR0,
+ },
+ {//PWM Channel 1
+ .pwm_pin = BV(PWM1),
+ .base = (volatile PwmChannelRegs *)&PWM_CMR1,
+ },
+ {//PWM Channel 2
+ .pwm_pin = BV(PWM2),
+ .base = (volatile PwmChannelRegs *)&PWM_CMR2,
+ },
+ {//PWM Channel 3
+ .pwm_pin = BV(PWM3),
+ .base = (volatile PwmChannelRegs *)&PWM_CMR3,
+ },
+ };
+
+ /*
+ * Init pwm.
+ */
+ void pwm_hw_init(Pwm *ctx, unsigned ch)
+ {
+
+ ctx->hw = &pwm_channels[ch];
+
+ /*
+ * Init pwm:
+ * - clear PIO outputs
+ * - enable PIO outputs
+ * - Enable PWM functions
+ * - Power on PWM
+ */
+ PWM_PIO_CODR = ctx->hw->pwm_pin;
+ PWM_PIO_OER = ctx->hw->pwm_pin;
+ PWM_PIO_PER = ctx->hw->pwm_pin;
+ PWM_PIO_ABSR = ctx->hw->pwm_pin;
+
+ PMC_PCER |= BV(PWMC_ID);
+
+ /* Disable prescalers A and B */
+ PWM_MR = 0;
+
+ /*
+ * Set pwm mode:
+ * WARNING: is forbidden to write 0 or 1 to duty cycle value,
+ * and so for start we set duty to 2.
+ * Also:
+ * - set period aligned to left
+ * - set output waveform to start at high level
+ * - allow duty cycle modify at next period event
+ */
+ ctx->hw->base->CDTY = 2;
+ ctx->hw->base->CMR = BV(PWM_CPOL);
+ PWM_ENA = BV(ch);
+ }
+#endif
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/arm/drv/pwm_at91.h b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/arm/drv/pwm_at91.h
index 5d36289..403b9f5 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/arm/drv/pwm_at91.h
+++ b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/arm/drv/pwm_at91.h
@@ -40,44 +40,64 @@
#ifndef DRV_PWM_AT91_H
#define DRV_PWM_AT91_H
-#include "hw/pwm_map.h"
-
#include <cfg/compiler.h>
#include <cfg/macros.h>
+#include "cfg/cfg_pwm.h"
+
#include <io/arm.h>
+#if CFG_PWM_ENABLE_OLD_API
-#define PWM_HW_MAX_PRESCALER_STEP 10
-#define PWM_HW_MAX_PERIOD 0xFFFF
+ #include "hw/pwm_map.h"
-/**
- * Type definition for pwm period.
- */
-typedef uint16_t pwm_period_t;
+ /**
+ * Type definition for pwm period.
+ */
+ typedef uint16_t pwm_period_t;
-/**
- * Structur definition for pwm driver.
- */
-typedef struct PwmChannel
-{
- bool duty_zero; ///< True if duty cyle is zero, false otherwise.
- bool pol; ///< PWM polarty flag.
- int pwm_pin; ///< PWM pin.
- reg32_t *mode_reg; ///< PWM mode register.
- reg32_t *duty_reg; ///< PWM duty cycle register.
- reg32_t *period_reg; ///< PWM periodic register.
- reg32_t *update_reg; ///< Update setting register for PWM.
-
-} PwmChannel;
-
-
-void pwm_hw_init(void);
-void pwm_hw_setFrequency(PwmDev dev, uint32_t freq);
-void pwm_hw_setDutyUnlock(PwmDev dev, uint16_t duty);
-void pwm_hw_disable(PwmDev dev);
-void pwm_hw_enable(PwmDev dev);
-void pwm_hw_setPolarity(PwmDev dev, bool pol);
-pwm_period_t pwm_hw_getPeriod(PwmDev dev);
+ /**
+ * Structur definition for pwm driver.
+ */
+ typedef struct PwmChannel
+ {
+ bool duty_zero; ///< True if duty cyle is zero, false otherwise.
+ bool pol; ///< PWM polarty flag.
+ int pwm_pin; ///< PWM pin.
+ reg32_t *mode_reg; ///< PWM mode register.
+ reg32_t *duty_reg; ///< PWM duty cycle register.
+ reg32_t *period_reg; ///< PWM periodic register.
+ reg32_t *update_reg; ///< Update setting register for PWM.
+
+ } PwmChannel;
+
+
+ void pwm_hw_init(void);
+ void pwm_hw_setFrequency(PwmDev dev, uint32_t freq);
+ void pwm_hw_setDutyUnlock(PwmDev dev, uint16_t duty);
+ void pwm_hw_disable(PwmDev dev);
+ void pwm_hw_enable(PwmDev dev);
+ void pwm_hw_setPolarity(PwmDev dev, bool pol);
+ pwm_period_t pwm_hw_getPeriod(PwmDev dev);
+
+#else
+ #include <drv/pwm.h>
+
+ typedef uint16_t pwm_hwreg_t;
+
+ struct PwmChannelRegs; //fwd decl
+
+ typedef struct PwmHardware
+ {
+ uint32_t pwm_pin; ///< PWM pin.
+ volatile struct PwmChannelRegs *base;
+ } PwmHardware;
+
+ pwm_hwreg_t pwm_hw_getPeriod(Pwm *ctx);
+ void pwm_hw_setFrequency(struct Pwm *ctx, pwm_freq_t freq);
+ void pwm_hw_setDuty(Pwm *ctx, pwm_hwreg_t duty);
+ void pwm_hw_init(struct Pwm *ctx, unsigned ch);
+
+#endif
#endif /* DRV_ADC_AT91_H */
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/avr/drv/kdebug_avr.c b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/avr/drv/kdebug_avr.c
index 8fa1a4b..e5718c9 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/avr/drv/kdebug_avr.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/avr/drv/kdebug_avr.c
@@ -355,6 +355,7 @@ INLINE void kdbg_hw_init(void)
#if CONFIG_KDEBUG_PORT == 0
UBRR0H = (uint8_t)(period>>8);
UBRR0L = (uint8_t)period;
+ UCSR0A = 0; /* The Arduino Uno bootloader turns on U2X0 */
KDBG_UART0_BUS_INIT;
#else
#error Only CONFIG_KDEBUG_PORT 0 is supported for this cpu
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/avr/drv/ser_avr.c b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/avr/drv/ser_avr.c
index 7f8cf93..ce44fd1 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/avr/drv/ser_avr.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/avr/drv/ser_avr.c
@@ -146,6 +146,7 @@
* - Enable only the RX complete interrupt
*/
#define SER_UART0_BUS_TXINIT do { \
+ UCSR0A = 0; /* The Arduino Uno bootloader turns on U2X0 */ \
UCSR0B = BV(BIT_RXCIE0) | BV(BIT_RXEN0) | BV(BIT_TXEN0); \
} while (0)
#endif
@@ -457,7 +458,20 @@ struct AvrSerial
volatile bool sending;
};
+static uint16_t uart_period(unsigned long bps)
+{
+ uint16_t period = DIV_ROUND(CPU_FREQ / 16UL, bps) - 1;
+
+ #ifdef _DEBUG
+ long skew = bps - (long)(period + 1) * (CPU_FREQ / 16);
+ /* 8N1 is reliable within 3% skew */
+ if ((unsigned long)ABS(skew) > bps / (100 / 3))
+ kprintf("Baudrate off by %ldbps\n", skew);
+ #endif
+ //DB(kprintf("uart_period(bps=%lu): period=%u\n", bps, period);)
+ return period;
+}
/*
* Callbacks
@@ -494,15 +508,12 @@ static void uart0_enabletxirq(struct SerialHardware *_hw)
static void uart0_setbaudrate(UNUSED_ARG(struct SerialHardware *, _hw), unsigned long rate)
{
- /* Compute baud-rate period */
- uint16_t period = DIV_ROUND(CPU_FREQ / 16UL, rate) - 1;
+ uint16_t period = uart_period(rate);
#if !CPU_AVR_ATMEGA103
- UBRR0H = (period) >> 8;
+ UBRR0H = period >> 8;
#endif
- UBRR0L = (period);
-
- //DB(kprintf("uart0_setbaudrate(rate=%lu): period=%d\n", rate, period);)
+ UBRR0L = period;
}
static void uart0_setparity(UNUSED_ARG(struct SerialHardware *, _hw), int parity)
@@ -547,13 +558,9 @@ static void uart1_enabletxirq(struct SerialHardware *_hw)
static void uart1_setbaudrate(UNUSED_ARG(struct SerialHardware *, _hw), unsigned long rate)
{
- /* Compute baud-rate period */
- uint16_t period = DIV_ROUND(CPU_FREQ / 16UL, rate) - 1;
-
- UBRR1H = (period) >> 8;
- UBRR1L = (period);
-
- //DB(kprintf("uart1_setbaudrate(rate=%ld): period=%d\n", rate, period);)
+ uint16_t period = uart_period(rate);
+ UBRR1H = period >> 8;
+ UBRR1L = period;
}
static void uart1_setparity(UNUSED_ARG(struct SerialHardware *, _hw), int parity)
@@ -598,13 +605,9 @@ static void uart2_enabletxirq(struct SerialHardware *_hw)
static void uart2_setbaudrate(UNUSED_ARG(struct SerialHardware *, _hw), unsigned long rate)
{
- /* Compute baud-rate period */
- uint16_t period = DIV_ROUND(CPU_FREQ / 16UL, rate) - 1;
-
- UBRR2H = (period) >> 8;
- UBRR2L = (period);
-
- //DB(kprintf("uart2_setbaudrate(rate=%ld): period=%d\n", rate, period);)
+ uint16_t period = uart_period(rate);
+ UBRR2H = period >> 8;
+ UBRR2L = period;
}
static void uart2_setparity(UNUSED_ARG(struct SerialHardware *, _hw), int parity)
@@ -649,13 +652,9 @@ static void uart3_enabletxirq(struct SerialHardware *_hw)
static void uart3_setbaudrate(UNUSED_ARG(struct SerialHardware *, _hw), unsigned long rate)
{
- /* Compute baud-rate period */
- uint16_t period = DIV_ROUND(CPU_FREQ / 16UL, rate) - 1;
-
- UBRR3H = (period) >> 8;
- UBRR3L = (period);
-
- //DB(kprintf("uart3_setbaudrate(rate=%ld): period=%d\n", rate, period);)
+ uint16_t period = uart_period(rate);
+ UBRR3H = period >> 8;
+ UBRR3L = period;
}
static void uart3_setparity(UNUSED_ARG(struct SerialHardware *, _hw), int parity)
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/cortex-m3/drv/adc_cm3.h b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/cortex-m3/drv/adc_cm3.h
index b9987e4..168a5da 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/cortex-m3/drv/adc_cm3.h
+++ b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/cortex-m3/drv/adc_cm3.h
@@ -42,6 +42,8 @@
#include "adc_lm3s.h"
#elif CPU_CM3_STM32
#include "adc_stm32.h"
+#elif CPU_CM3_SAM3X
+ #include "adc_sam3.h"
/*#elif Add other ARM families here */
#else
#error Unknown CPU
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/cortex-m3/drv/adc_sam3.c b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/cortex-m3/drv/adc_sam3.c
new file mode 100644
index 0000000..303689f
--- /dev/null
+++ b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/cortex-m3/drv/adc_sam3.c
@@ -0,0 +1,157 @@
+/**
+ * \file
+ * <!--
+ * This file is part of BeRTOS.
+ *
+ * Bertos is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ * As a special exception, you may use this file as part of a free software
+ * library without restriction. Specifically, if other files instantiate
+ * templates or use macros or inline functions from this file, or you compile
+ * this file and link it with other files to produce an executable, this
+ * file does not by itself cause the resulting executable to be covered by
+ * the GNU General Public License. This exception does not however
+ * invalidate any other reasons why the executable file might be covered by
+ * the GNU General Public License.
+ *
+ * Copyright 2011 Develer S.r.l. (http://www.develer.com/)
+ *
+ * -->
+ *
+ * \brief ADC hardware-specific implementation
+ *
+ * \author Daniele Basile <as...@de...>
+ */
+
+
+#include "adc_sam3.h"
+
+#include "cfg/cfg_adc.h"
+
+#include <cfg/macros.h>
+#include <cfg/compiler.h>
+
+// Define log settings for cfg/log.h.
+#define LOG_LEVEL ADC_LOG_LEVEL
+#define LOG_FORMAT ADC_LOG_FORMAT
+#include <cfg/log.h>
+
+#include <drv/adc.h>
+#include <drv/irq_cm3.h>
+
+#include <cpu/irq.h>
+
+#include <mware/event.h>
+
+#include <io/cm3.h>
+
+
+/* We use event to signal the end of conversion */
+static Event data_ready;
+/* The last converted data */
+static uint32_t data;
+
+/**
+ * ADC ISR.
+ *
+ * The interrupt is connected to ready data, so when the
+ * adc ends the conversion we generate an event and then
+ * we return the converted value.
+ *
+ * \note to clear the Ready data bit and End of conversion
+ * bit we should read the Last Converted Data register, otherwise
+ * the ready data interrupt loop on this call.
+ */
+static DECLARE_ISR(adc_conversion_end_irq)
+{
+ data = 0;
+ if (ADC_ISR & BV(ADC_DRDY))
+ {
+ data = ADC_LDATA;
+ event_do(&data_ready);
+ }
+}
+
+/**
+ * Select mux channel \a ch.
+ */
+void adc_hw_select_ch(uint8_t ch)
+{
+ /* Disable all channels */
+ ADC_CHDR = ADC_CH_MASK;
+ /* Enable select channel */
+ ADC_CHER = BV(ch);
+}
+
+/**
+ * Start an ADC convertion.
+ */
+uint16_t adc_hw_read(void)
+{
+ ADC_CR = BV(ADC_START);
+ event_wait(&data_ready);
+ return(data);
+}
+
+/**
+ * Init ADC hardware.
+ */
+void adc_hw_init(void)
+{
+ /* Make sure that interrupt are enabled */
+ IRQ_ASSERT_ENABLED();
+
+ /* Initialize the dataready event */
+ event_initGeneric(&data_ready);
+
+ /* Clock ADC peripheral */
+ pmc_periphEnable(ADC_ID);
+
+ /* Reset adc controller */
+ ADC_CR = ADC_SWRST;
+
+ /*
+ * Set adc mode register:
+ * - Disable hardware trigger and enable software trigger.
+ * - Select normal mode.
+ */
+ ADC_MR = 0;
+
+ /* Set ADC_BITS bit convertion resolution. */
+ #if ADC_BITS == 12
+ ADC_MR &= ~BV(ADC_LOWRES);
+ #elif ADC_BITS == 10
+ ADC_MR |= BV(ADC_LOWRES);
+ #else
+ #error No select bit resolution is supported to this CPU
+ #endif
+
+ /* Setup ADC */
+ LOG_INFO("Computed ADC_CLOCK %ld\n", ADC_CLOCK);
+ ADC_MR |= ((ADC_PRESCALER << ADC_PRESCALER_SHIFT) & ADC_PRESCALER_MASK);
+ LOG_INFO("prescaler[%ld]\n", ADC_PRESCALER);
+ ADC_MR |= ((CONFIG_ADC_SUT << ADC_STARTUP_SHIFT) & ADC_STARTUP_MASK);
+ LOG_INFO("starup[%d]\n", CONFIG_ADC_SUT);
+ ADC_MR |= ((CONFIG_ADC_STTLING << ADC_SETTLING_SHIFT) & ADC_SETTLING_MASK);
+ LOG_INFO("sttime[%d]\n", CONFIG_ADC_STTLING);
+ ADC_MR |= ((CONFIG_ADC_TRACKTIM << ADC_TRACKTIM_SHIFT) & ADC_TRACKTIM_MASK);
+ LOG_INFO("tracking[%d]\n", CONFIG_ADC_TRACKTIM);
+ ADC_MR |= ((CONFIG_ADC_TRANSFER << ADC_TRANSFER_SHIFT) & ADC_TRANSFER_MASK);
+ LOG_INFO("tranfer[%d]\n", CONFIG_ADC_TRANSFER);
+
+ /* Register and enable irq for adc. */
+ sysirq_setHandler(INT_ADC, adc_conversion_end_irq);
+ ADC_IER = BV(ADC_DRDY);
+}
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/cortex-m3/drv/adc_sam3.h b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/cortex-m3/drv/adc_sam3.h
new file mode 100644
index 0000000..f46f940
--- /dev/null
+++ b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/cortex-m3/drv/adc_sam3.h
@@ -0,0 +1,64 @@
+/**
+ * \file
+ * <!--
+ * This file is part of BeRTOS.
+ *
+ * Bertos is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ * As a special exception, you may use this file as part of a free software
+ * library without restriction. Specifically, if other files instantiate
+ * templates or use macros or inline functions from this file, or you compile
+ * this file and link it with other files to produce an executable, this
+ * file does not by itself cause the resulting executable to be covered by
+ * the GNU General Public License. This exception does not however
+ * invalidate any other reasons why the executable file might be covered by
+ * the GNU General Public License.
+ *
+ * Copyright 2008 Develer S.r.l. (http://www.develer.com/)
+ *
+ * -->
+ *
+ * \brief ADC hardware-specific definition
+ *
+ * \author Daniele Basile <as...@de...>
+ */
+
+#ifndef DRV_ADC_SAM3_H
+#define DRV_ADC_SAM3_H
+
+#include <hw/hw_cpufreq.h>
+
+#include "cfg/cfg_adc.h"
+
+#include <cfg/compiler.h>
+
+/**
+ * ADC config define.
+ */
+#define ADC_MUX_MAXCH 16 //Max number of channel for ADC.
+#define ADC_BITS 12 //Bit resolution for ADC converter.
+
+/**
+ * Macro for computing correct value to write into ADC
+ * register.
+ */
+#define ADC_PRESCALER (DIV_ROUNDUP(CPU_FREQ, 2 * CONFIG_ADC_CLOCK) - 1)
+#define ADC_CLOCK (CPU_FREQ / ((ADC_PRESCALER + 1) * 2))
+
+void adc_hw_select_ch(uint8_t ch);
+uint16_t adc_hw_read(void);
+void adc_hw_init(void);
+
+#endif /* DRV_ADC_SAM3_H */
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/cortex-m3/drv/can_stm32.c b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/cortex-m3/drv/can_stm32.c
index f47e99a..494c68e 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/cortex-m3/drv/can_stm32.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/cortex-m3/drv/can_stm32.c
@@ -33,6 +33,8 @@
* \brief CAN hardware-specific implementation
*
* \author Nicolas Dandrimont <Nic...@cr...>
+ *
+ * $WIZ$
*/
#include "can_stm32.h"
@@ -52,6 +54,7 @@
#include <cfg/log.h>
#include <drv/can.h>
+#include <drv/timer.h>
#include <drv/clock_stm32.h>
#include <drv/gpio_stm32.h>
@@ -62,6 +65,16 @@
#include <drv/irq_cm3.h>
+/**
+ * CAN Remapping values for STM32
+ *
+ * $WIZ$ can_stm32_remaps = "CAN_STM32_PORTA", "CAN_STM32_PORTB", "CAN_STM32_PORTD"
+ * \{
+ */
+#define CAN_STM32_PORTA 0
+#define CAN_STM32_PORTB 1
+#define CAN_STM32_PORTD 2
+/* \} */
static can_driver _cand1;
can_driver *CAND1 = &_cand1;
@@ -139,11 +152,33 @@ void can_hw_init(void)
// Enable the clocks
RCC->APB2ENR |= RCC_APB2_AFIO;
+
+#if CAN_STM32_REMAP == CAN_STM32_PORTA
RCC->APB2ENR |= RCC_APB2_GPIOA;
+#elif CAN_STM32_REMAP == CAN_STM32_PORTB
+ RCC->APB2ENR |= RCC_APB2_GPIOB;
+#elif CAN_STM32_REMAP == CAN_STM32_PORTD
+ RCC->APB2ENR |= RCC_APB2_GPIOD;
+#else
+ #error "CAN remapping not supported for stm32"
+#endif
// Set the pins to the right mode for CAN.
- stm32_gpioPinConfig((struct stm32_gpio *)GPIOA_BASE, BV(11), GPIO_MODE_IN_FLOATING, GPIO_SPEED_50MHZ);
+#if CAN_STM32_REMAP == CAN_STM32_PORTA
+ stm32_gpioPinConfig((struct stm32_gpio *)GPIOA_BASE, BV(11), GPIO_MODE_IPU, GPIO_SPEED_50MHZ);
stm32_gpioPinConfig((struct stm32_gpio *)GPIOA_BASE, BV(12), GPIO_MODE_AF_PP, GPIO_SPEED_50MHZ);
+#elif CAN_STM32_REMAP == CAN_STM32_PORTB
+ stm32_gpioRemap(GPIO_REMAP1_CAN1, GPIO_REMAP_ENABLE);
+ stm32_gpioPinConfig((struct stm32_gpio *)GPIOB_BASE, BV(8), GPIO_MODE_IPU, GPIO_SPEED_50MHZ);
+ stm32_gpioPinConfig((struct stm32_gpio *)GPIOB_BASE, BV(9), GPIO_MODE_AF_PP, GPIO_SPEED_50MHZ);
+#elif CAN_STM32_REMAP == CAN_STM32_PORTD
+ stm32_gpioRemap(GPIO_REMAP2_CAN1, GPIO_REMAP_ENABLE);
+ stm32_gpioPinConfig((struct stm32_gpio *)GPIOD_BASE, BV(0), GPIO_MODE_IPU, GPIO_SPEED_50MHZ);
+ stm32_gpioPinConfig((struct stm32_gpio *)GPIOD_BASE, BV(1), GPIO_MODE_AF_PP, GPIO_SPEED_50MHZ);
+#else
+ #error "CAN remapping not supported for stm32"
+#endif
+
}
void can_hw_start(can_driver *drv) {
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/cortex-m3/drv/dac_sam3.c b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/cortex-m3/drv/dac_sam3.c
new file mode 100644
index 0000000..98e4fa8
--- /dev/null
+++ b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/cortex-m3/drv/dac_sam3.c
@@ -0,0 +1,93 @@
+/**
+ * \file
+ * <!--
+ * This file is part of BeRTOS.
+ *
+ * Bertos is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ * As a special exception, you may use this file as part of a free software
+ * library without restriction. Specifically, if other files instantiate
+ * templates or use macros or inline functions from this file, or you compile
+ * this file and link it with other files to produce an executable, this
+ * file does not by itself cause the resulting executable to be covered by
+ * the GNU General Public License. This exception does not however
+ * invalidate any other reasons why the executable file might be covered by
+ * the GNU General Public License.
+ *
+ * Copyright 2011 Develer S.r.l. (http://www.develer.com/)
+ *
+ * -->
+ *
+ * \brief DAC hardware-specific implementation
+ *
+ * \author Daniele Basile <as...@de...>
+ */
+
+
+#include "cfg/cfg_dac.h"
+
+#include <cfg/macros.h>
+#include <cfg/compiler.h>
+
+// Define log settings for cfg/log.h.
+#define LOG_LEVEL DAC_LOG_LEVEL
+#define LOG_FORMAT DAC_LOG_FORMAT
+#include <cfg/log.h>
+
+#include <drv/dac.h>
+
+#include <drv/irq_cm3.h>
+
+#include <io/cm3.h>
+
+#include <string.h>
+
+int dac_write(int ch, void *buf, size_t len)
+{
+ ASSERT(ch <= 1);
+
+ DACC_MR |= (ch << DACC_USER_SEL_SHIFT) & DACC_USER_SEL_MASK;
+ DACC_CHER |= BV(ch);
+
+ kprintf("mr: %08lx\n", DACC_MR);
+
+ if (len <= sizeof(uint32_t))
+ {
+ memcpy((void *)DACC_CDR, buf, len);
+ }
+ else
+ {
+ DACC_TPR = (uint32_t)buf ;
+ DACC_TCR = len ;
+ DACC_PTCR |= BV(DACC_PTCR_TXTEN);
+ }
+
+ return 0;
+}
+
+void dac_init(void)
+{
+ /* Clock ADC peripheral */
+ pmc_periphEnable(DACC_ID);
+
+ DACC_CR |= BV(DACC_SWRST);
+ DACC_MR = 0;
+
+ /* Refresh period */
+ DACC_MR |= (16 << DACC_REFRESH_SHIFT) & DACC_REFRESH_MASK;
+ /* Start up */
+ DACC_MR |= (DACC_MR_STARTUP_0 << DACC_STARTUP_SHIFT) & DACC_STARTUP_MASK;
+
+}
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/cortex-m3/drv/eth_sam3.c b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/cortex-m3/drv/eth_sam3.c
new file mode 100644
index 0000000..5e18dfe
--- /dev/null
+++ b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/cortex-m3/drv/eth_sam3.c
@@ -0,0 +1,539 @@
+/**
+ * \file
+ * <!--
+ * This file is part of BeRTOS.
+ *
+ * Bertos is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ * As a special exception, you may use this file as part of a free software
+ * library without restriction. Specifically, if other files instantiate
+ * templates or use macros or inline functions from this file, or you compile
+ * this file and link it with other files to produce an executable, this
+ * file does not by itself cause the resulting executable to be covered by
+ * the GNU General Public License. This exception does not however
+ * invalidate any other reasons why the executable file might be covered by
+ * the GNU General Public License.
+ *
+ * Copyright 2010,2011 Develer S.r.l. (http://www.develer.com/)
+ * All Rights Reserved.
+ * -->
+ *
+ * \brief EMAC driver for AT91SAM family with Davicom 9161A phy.
+ *
+ * \author Daniele Basile <as...@de...>
+ * \author Andrea Righi <ar...@de...>
+ * \author Stefano Fedrigo <al...@de...>
+ */
+
+#include "cfg/cfg_eth.h"
+
+#define LOG_LEVEL ETH_LOG_LEVEL
+#define LOG_FORMAT ETH_LOG_FORMAT
+
+#include <cfg/log.h>
+
+#include <cfg/debug.h>
+#include <cfg/log.h>
+#include <cfg/macros.h>
+#include <cfg/compiler.h>
+
+// TODO: unify includes
+//#include <io/at91sam7.h>
+//#include <io/arm.h>
+//#include <io/include.h>
+#include <io/sam3.h>
+#include <drv/irq_cm3.h>
+
+#include <cpu/power.h>
+#include <cpu/types.h>
+#include <cpu/irq.h>
+
+#include <drv/timer.h>
+#include <drv/eth.h>
+
+#include <mware/event.h>
+
+#include <string.h>
+
+#include "eth_sam3.h"
+
+#define EMAC_RX_INTS (BV(EMAC_RCOMP) | BV(EMAC_ROVR) | BV(EMAC_RXUBR))
+#define EMAC_TX_INTS (BV(EMAC_TCOMP) | BV(EMAC_TXUBR) | BV(EMAC_RLEX))
+
+/*
+ * MAC address configuration (please change this in your project!).
+ *
+ * TODO: make this paramater user-configurable from the Wizard.
+ */
+const uint8_t mac_addr[] = { 0x00, 0x23, 0x54, 0x6a, 0x77, 0x55 };
+
+/* Silent Doxygen bug... */
+#ifndef __doxygen__
+/*
+ * NOTE: this buffer should be declared as 'volatile' because it is read by the
+ * hardware. However, this is accessed only via memcpy() that should guarantee
+ * coherency when copying from/to buffers.
+ */
+static uint8_t tx_buf[EMAC_TX_BUFFERS * EMAC_TX_BUFSIZ] ALIGNED(8);
+static volatile BufDescriptor tx_buf_tab[EMAC_TX_DESCRIPTORS] ALIGNED(8);
+
+/*
+ * NOTE: this buffer should be declared as 'volatile' because it is wrote by
+ * the hardware. However, this is accessed only via memcpy() that should
+ * guarantee coherency when copying from/to buffers.
+ */
+static uint8_t rx_buf[EMAC_RX_BUFFERS * EMAC_RX_BUFSIZ] ALIGNED(8);
+static volatile BufDescriptor rx_buf_tab[EMAC_RX_DESCRIPTORS] ALIGNED(8);
+#endif
+
+static int tx_buf_idx;
+static int tx_buf_offset;
+static int rx_buf_idx;
+
+static Event recv_wait, send_wait;
+
+static DECLARE_ISR(emac_irqHandler)
+{
+ /* Read interrupt status and disable interrupts. */
+ uint32_t isr = EMAC_ISR;
+
+ kprintf("irq: %x\n", isr);
+
+ /* Receiver interrupt */
+ if ((isr & EMAC_RX_INTS))
+ {
+ kprintf("emac: rx %x\n", isr);
+ if (isr & BV(EMAC_RCOMP))
+ event_do(&recv_wait);
+ EMAC_RSR = EMAC_RX_INTS;
+ }
+ /* Transmitter interrupt */
+ if (isr & EMAC_TX_INTS)
+ {
+ if (isr & BV(EMAC_TCOMP))
+ {
+ kprintf("emac: tcomp\n");
+ event_do(&send_wait);
+ }
+ if (isr & BV(EMAC_RLEX))
+ kprintf("emac: rlex\n");
+ EMAC_TSR = EMAC_TX_INTS;
+ }
+ //AIC_EOICR = 0;
+}
+
+/*
+ * \brief Read contents of PHY register.
+ *
+ * \param reg PHY register number.
+ *
+ * \return Contents of the specified register.
+ */
+static uint16_t phy_hw_read(uint8_t phy_addr, reg8_t reg)
+{
+ // PHY read command.
+ EMAC_MAN = EMAC_SOF | EMAC_RW_READ
+ | ((phy_addr << EMAC_PHYA_SHIFT) & EMAC_PHYA)
+ | ((reg << EMAC_REGA_SHIFT) & EMAC_REGA)
+ | EMAC_CODE;
+
+ // Wait until PHY logic completed.
+ while (!(EMAC_NSR & BV(EMAC_IDLE)))
+ cpu_relax();
+
+ // Get data from PHY maintenance register.
+ return (uint16_t)(EMAC_MAN & EMAC_DATA);
+}
+
+/*
+ * \brief Write value to PHY register.
+ *
+ * \param reg PHY register number.
+ * \param val Value to write.
+ */
+static void phy_hw_write(uint8_t phy_addr, reg8_t reg, uint16_t val)
+{
+ // PHY write command.
+ EMAC_MAN = EMAC_SOF | EMAC_RW_WRITE
+ | ((phy_addr << EMAC_PHYA_SHIFT) & EMAC_PHYA)
+ | ((reg << EMAC_REGA_SHIFT) & EMAC_REGA)
+ | EMAC_CODE | val;
+
+ // Wait until PHY logic completed.
+ while (!(EMAC_NSR & BV(EMAC_IDLE)))
+ cpu_relax();
+}
+
+static int emac_reset(void)
+{
+ uint16_t phy_cr;
+ unsigned i;
+
+ // Enable devices
+ //PMC_PCER = BV(PIOA_ID);
+ //PMC_PCER = BV(PIOB_ID);
+ //PMC_PCER = BV(EMAC_ID);
+ // TOOD: Implement in sam7x
+ pmc_periphEnable(PIOA_ID);
+ pmc_periphEnable(PIOB_ID);
+ pmc_periphEnable(EMAC_ID);
+
+ // Disable TESTMODE
+ PIOB_PUDR = BV(PHY_RXDV_TESTMODE_BIT);
+#if CPU_ARM_AT91
+ // Disable RMII
+ PIOB_PUDR = BV(PHY_COL_RMII_BIT);
+
+ // Disable PHY power down.
+ PIOB_PER = BV(PHY_PWRDN_BIT);
+ PIOB_OER = BV(PHY_PWRDN_BIT);
+ PIOB_CODR = BV(PHY_PWRDN_BIT);
+#endif
+
+ // Toggle external hardware reset pin.
+ RSTC_MR = RSTC_KEY | (1 << RSTC_ERSTL_SHIFT) | BV(RSTC_URSTEN);
+ RSTC_CR = RSTC_KEY | BV(RSTC_EXTRST);
+
+ while ((RSTC_SR & BV(RSTC_NRSTL)) == 0)
+ cpu_relax();
+
+ // Configure MII ports.
+#if CPU_ARM_AT91
+ PIOB_ASR = PHY_MII_PINS;
+ PIOB_BSR = 0;
+ PIOB_PDR = PHY_MII_PINS;
+ // Enable receive and transmit clocks.
+ EMAC_USRIO = BV(EMAC_CLKEN);
+#else
+ PIO_PERIPH_SEL(PIOB_BASE, PHY_MII_PINS, PIO_PERIPH_A);
+ PIOB_PDR = PHY_MII_PINS;
+ // Enable receive, transmit clocks and RMII mode.
+ EMAC_USRIO = BV(EMAC_CLKEN) | BV(EMAC_RMII);
+#endif
+
+ // Enable management port.
+ EMAC_NCR |= BV(EMAC_MPE);
+ EMAC_NCFGR |= EMAC_CLK_HCLK_64;
+
+ // Set local MAC address.
+ EMAC_SA1L = (mac_addr[3] << 24) | (mac_addr[2] << 16) |
+ (mac_addr[1] << 8) | mac_addr[0];
+ EMAC_SA1H = (mac_addr[5] << 8) | mac_addr[4];
+
+ // Wait for PHY ready
+ timer_delay(255);
+
+#if 0 // debug test
+ for (;;)
+ {
+ for (i = 0; i < 32; i++)
+ {
+ // Clear MII isolate.
+ phy_hw_read(i, NIC_PHY_BMCR);
+ phy_cr = phy_hw_read(i, NIC_PHY_BMCR);
+
+ phy_cr &= ~NIC_PHY_BMCR_ISOLATE;
+ phy_hw_write(i, NIC_PHY_BMCR, phy_cr);
+
+ phy_cr = phy_hw_read(i, NIC_PHY_BMCR);
+
+ LOG_INFO("%s: PHY ID %d %#04x %#04x\n",
+ __func__, i,
+ phy_hw_read(i, NIC_PHY_ID1), phy_hw_read(i, NIC_PHY_ID2));
+ }
+ timer_delay(1000);
+ }
+#endif
+
+ // Clear MII isolate.
+ phy_hw_read(NIC_PHY_ADDR, NIC_PHY_BMCR);
+ phy_cr = phy_hw_read(NIC_PHY_ADDR, NIC_PHY_BMCR);
+
+ phy_cr &= ~NIC_PHY_BMCR_ISOLATE;
+ phy_hw_write(NIC_PHY_ADDR, NIC_PHY_BMCR, phy_cr);
+
+ phy_cr = phy_hw_read(NIC_PHY_ADDR, NIC_PHY_BMCR);
+
+ LOG_INFO("%s: PHY ID %#04x %#04x\n",
+ __func__,
+ phy_hw_read(NIC_PHY_ADDR, NIC_PHY_ID1), phy_hw_read(NIC_PHY_ADDR, NIC_PHY_ID2));
+
+ // Wait for auto negotiation completed.
+ phy_hw_read(NIC_PHY_ADDR, NIC_PHY_BMSR);
+ for (;;)
+ {
+ if (phy_hw_read(NIC_PHY_ADDR, NIC_PHY_BMSR) & NIC_PHY_BMSR_ANCOMPL)
+ break;
+ cpu_relax();
+ }
+
+ // Disable management port.
+ EMAC_NCR &= ~BV(EMAC_MPE);
+
+ return 0;
+}
+
+static int emac_start(void)
+{
+ uint32_t addr;
+ int i;
+
+ for (i = 0; i < EMAC_RX_DESCRIPTORS; i++)
+ {
+ addr = (uint32_t)(rx_buf + (i * EMAC_RX_BUFSIZ));
+ rx_buf_tab[i].addr = addr & BUF_ADDRMASK;
+ }
+ rx_buf_tab[EMAC_RX_DESCRIPTORS - 1].addr |= RXBUF_WRAP;
+
+ for (i = 0; i < EMAC_TX_DESCRIPTORS; i++)
+ {
+ addr = (uint32_t)(tx_buf + (i * EMAC_TX_BUFSIZ));
+ tx_buf_tab[i].addr = addr & BUF_ADDRMASK;
+ tx_buf_tab[i].stat = TXS_USED;
+ }
+ tx_buf_tab[EMAC_TX_DESCRIPTORS - 1].stat = TXS_USED | TXS_WRAP;
+
+ /* Tell the EMAC where to find the descriptors. */
+ EMAC_RBQP = (uint32_t)rx_buf_tab;
+ EMAC_TBQP = (uint32_t)tx_buf_tab;
+
+ /* Clear receiver status. */
+ EMAC_RSR = BV(EMAC_OVR) | BV(EMAC_REC) | BV(EMAC_BNA);
+
+ /* Copy all frames and discard FCS. */
+ EMAC_NCFGR |= BV(EMAC_CAF) | BV(EMAC_DRFCS);
+
+ /* Enable receiver, transmitter and statistics. */
+ EMAC_NCR |= BV(EMAC_TE) | BV(EMAC_RE) | BV(EMAC_WESTAT);
+
+ return 0;
+}
+
+ssize_t eth_putFrame(const uint8_t *buf, size_t len)
+{
+ size_t wr_len;
+
+ if (UNLIKELY(!len))
+ return -1;
+ ASSERT(len <= sizeof(tx_buf));
+
+ /* Check if the transmit buffer is available */
+ while (!(tx_buf_tab[tx_buf_idx].stat & TXS_USED))
+ event_wait(&send_wait);
+
+ /* Copy the data into the buffer and prepare descriptor */
+ wr_len = MIN(len, (size_t)EMAC_TX_BUFSIZ - tx_buf_offset);
+ memcpy((uint8_t *)tx_buf_tab[tx_buf_idx].addr + tx_buf_offset,
+ buf, wr_len);
+ tx_buf_offset += wr_len;
+
+ return wr_len;
+}
+
+void eth_sendFrame(void)
+{
+ tx_buf_tab[tx_buf_idx].stat = (tx_buf_offset & TXS_LENGTH_FRAME) |
+ TXS_LAST_BUFF |
+ ((tx_buf_idx == EMAC_TX_DESCRIPTORS - 1) ? TXS_WRAP : 0);
+ EMAC_NCR |= BV(EMAC_TSTART);
+
+ tx_buf_offset = 0;
+ if (++tx_buf_idx >= EMAC_TX_DESCRIPTORS)
+ tx_buf_idx = 0;
+}
+
+ssize_t eth_send(const uint8_t *buf, size_t len)
+ {
+ if (UNLIKELY(!len))
+ return -1;
+
+ len = eth_putFrame(buf, len);
+ eth_sendFrame();
+
+ return len;
+}
+
+static void eth_buf_realign(int idx)
+{
+ /* Empty buffer found. Realign. */
+ do {
+ rx_buf_tab[rx_buf_idx].addr &= ~RXBUF_OWNERSHIP;
+ if (++rx_buf_idx >= EMAC_RX_BUFFERS)
+ rx_buf_idx = 0;
+ } while (idx != rx_buf_idx);
+}
+
+static size_t __eth_getFrameLen(void)
+{
+ int idx, n = EMAC_RX_BUFFERS;
+
+skip:
+ /* Skip empty buffers */
+ while ((n > 0) && !(rx_buf_tab[rx_buf_idx].addr & RXBUF_OWNERSHIP))
+ {
+ if (++rx_buf_idx >= EMAC_RX_BUFFERS)
+ rx_buf_idx = 0;
+ n--;
+ }
+ if (UNLIKELY(!n))
+ {
+ LOG_INFO("no frame found\n");
+ return 0;
+ }
+ /* Search the start of frame and cleanup fragments */
+ while ((n > 0) && (rx_buf_tab[rx_buf_idx].addr & RXBUF_OWNERSHIP) &&
+ !(rx_buf_tab[rx_buf_idx].stat & RXS_SOF))
+ {
+ rx_buf_tab[rx_buf_idx].addr &= ~RXBUF_OWNERSHIP;
+ if (++rx_buf_idx >= EMAC_RX_BUFFERS)
+ rx_buf_idx = 0;
+ n--;
+ }
+ if (UNLIKELY(!n))
+ {
+ LOG_INFO("no SOF found\n");
+ return 0;
+ }
+ /* Search end of frame to evaluate the total frame size */
+ idx = rx_buf_idx;
+restart:
+ while (n > 0)
+ {
+ if (UNLIKELY(!(rx_buf_tab[idx].addr & RXBUF_OWNERSHIP)))
+ {
+ /* Empty buffer found. Realign. */
+ eth_buf_realign(idx);
+ goto skip;
+ }
+ if (rx_buf_tab[idx].stat & RXS_EOF)
+ return rx_buf_tab[idx].stat & RXS_LENGTH_FRAME;
+ if (UNLIKELY((idx != rx_buf_idx) &&
+ (rx_buf_tab[idx].stat & RXS_SOF)))
+ {
+ /* Another start of frame found. Realign. */
+ eth_buf_realign(idx);
+ goto restart;
+ }
+ if (++idx >= EMAC_RX_BUFFERS)
+ idx = 0;
+ n--;
+ }
+ LOG_INFO("no EOF found\n");
+ return 0;
+}
+
+size_t eth_getFrameLen(void)
+{
+ size_t len;
+
+ /* Check if there is at least one available frame in the buffer */
+ while (1)
+ {
+ len = __eth_getFrameLen();
+ if (LIKELY(len))
+ break;
+ /* Wait for RX interrupt */
+ event_wait(&recv_wait);
+ }
+ return len;
+}
+
+ssize_t eth_getFrame(uint8_t *buf, size_t len)
+{
+ uint8_t *addr;
+ size_t rd_len = 0;
+
+ if (UNLIKELY(!len))
+ return -1;
+ ASSERT(len <= sizeof(rx_buf));
+
+ /* Copy data from the RX buffer */
+ addr = (uint8_t *)(rx_buf_tab[rx_buf_idx].addr & BUF_ADDRMASK);
+ if (addr + len > &rx_buf[countof(rx_buf)])
+ {
+ size_t count = &rx_buf[countof(rx_buf)] - addr;
+
+ memcpy(buf, addr, count);
+ memcpy(buf + count, rx_buf, len - count);
+ }
+ else
+ {
+ memcpy(buf, addr, len);
+ }
+ /* Update descriptors */
+ while (rd_len < len)
+ {
+ if (len - rd_len >= EMAC_RX_BUFSIZ)
+ rd_len += EMAC_RX_BUFSIZ;
+ else
+ rd_len += len - rd_len;
+ if (UNLIKELY(!(rx_buf_tab[rx_buf_idx].addr & RXBUF_OWNERSHIP)))
+ {
+ LOG_INFO("bad frame found\n");
+ return 0;
+ }
+ rx_buf_tab[rx_buf_idx].addr &= ~RXBUF_OWNERSHIP;
+ if (++rx_buf_idx >= EMAC_RX_DESCRIPTORS)
+ rx_buf_idx = 0;
+ }
+
+ return rd_len;
+}
+
+ssize_t eth_recv(uint8_t *buf, size_t len)
+{
+ if (UNLIKELY(!len))
+ return -1;
+ len = MIN(len, eth_getFrameLen());
+ return len ? eth_getFrame(buf, len) : 0;
+}
+
+int eth_init()
+{
+ cpu_flags_t flags;
+
+ emac_reset();
+ emac_start();
+
+ event_initGeneric(&recv_wait);
+ event_initGeneric(&send_wait);
+
+ // Register interrupt vector
+ IRQ_SAVE_DISABLE(flags);
+
+ /* Disable all emac interrupts */
+ EMAC_IDR = 0xFFFFFFFF;
+
+#if CPU_ARM_AT91
+ // TODO: define sysirq_set...
+ /* Set the vector. */
+ AIC_SVR(EMAC_ID) = emac_irqHandler;
+ /* Initialize to edge triggered with defined priority. */
+ AIC_SMR(EMAC_ID) = AIC_SRCTYPE_INT_EDGE_TRIGGERED;
+ /* Clear pending interrupt */
+ AIC_ICCR = BV(EMAC_ID);
+ /* Enable the system IRQ */
+ AIC_IECR = BV(EMAC_ID);
+#else
+ sysirq_setHandler(INT_EMAC, emac_irqHandler);
+#endif
+
+ /* Enable interrupts */
+ EMAC_IER = EMAC_RX_INTS | EMAC_TX_INTS;
+
+ IRQ_RESTORE(flags);
+
+ return 0;
+}
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/cortex-m3/drv/eth_sam3.h b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/cortex-m3/drv/eth_sam3.h
new file mode 100644
index 0000000..04d4aab
--- /dev/null
+++ b/elec/boards/Controller_Motor_STM32/Firmware/bertos/cpu/cortex-m3/drv/eth_sam3.h
@@ -0,0 +1,208 @@
+/**
+ * \file
+ * <!--
+ * This file is part of BeRTOS.
+ *
+ * Bertos is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ *
+ * As a special exception, you may use this file as part of a free software
+ * library without restriction. Specifically, if other files instantiate
+ * templates or use macros or inline functions from this file, or you compile
+ * this file and link it with other files to produce an executable, this
+ * file does not by itself cau...
[truncated message content] |
|
From: Jérémie D. <Ba...@us...> - 2011-03-28 22:29:08
|
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 00945a9816ab345b174312f946a0eba895b8b045 (commit)
via 9f60a4a3de1ef5cdbf1a77cd99e440b99291c17c (commit)
via de4cdd7965da9a169fe732a1d015da1fa221f3b2 (commit)
from bafd157aea6b314a6972bcf90b29e33e946c2909 (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 00945a9816ab345b174312f946a0eba895b8b045
Author: Jérémie Dimino <je...@di...>
Date: Tue Mar 29 00:28:30 2011 +0200
[info] typo in serialization of int32s
commit 9f60a4a3de1ef5cdbf1a77cd99e440b99291c17c
Author: Jérémie Dimino <je...@di...>
Date: Tue Mar 29 00:22:54 2011 +0200
[info] add Krobot_interface_service to krobot.interfaces
commit de4cdd7965da9a169fe732a1d015da1fa221f3b2
Author: Jérémie Dimino <je...@di...>
Date: Tue Mar 29 00:18:04 2011 +0200
[info] ignore sigpipe in krobot-remote
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/_oasis b/info/control2011/_oasis
index 6c88cb8..c3ac98f 100644
--- a/info/control2011/_oasis
+++ b/info/control2011/_oasis
@@ -52,7 +52,8 @@ Library "krobot-interfaces"
Path: src/interfaces
Install: true
Modules:
- Krobot_interface_can
+ Krobot_interface_can,
+ Krobot_interface_service
# +-------------------------------------------------------------------+
# | The driver |
diff --git a/info/control2011/_tags b/info/control2011/_tags
index 0388686..16e64a1 100644
--- a/info/control2011/_tags
+++ b/info/control2011/_tags
@@ -2,18 +2,21 @@
<src/interfaces/*.ml>: -syntax_camlp4o
# OASIS_START
-# DO NOT EDIT (digest: 689823deb2ad1bb10042fed50bc857aa)
+# DO NOT EDIT (digest: 9d263765d4deabae48324e53c88ea807)
# Library krobot-interfaces
"src/interfaces": include
+"src/interfaces/krobot-interfaces.cmxs": use_krobot-interfaces
<src/interfaces/*.ml{,i}>: pkg_obus
# Library krobot
"src/lib": include
+"src/lib/krobot.cmxs": use_krobot
<src/lib/*.ml{,i}>: use_krobot-interfaces
<src/lib/*.ml{,i}>: pkg_obus
<src/lib/*.ml{,i}>: pkg_lwt.unix
<src/lib/*.ml{,i}>: pkg_lwt.syntax
# Library krobot-can
"src/can": include
+"src/can/krobot-can.cmxs": use_krobot-can
<src/can/krobot-can.{cma,cmxa}>: use_libkrobot-can
<src/can/*.ml{,i}>: use_krobot
<src/can/*.ml{,i}>: use_krobot-interfaces
diff --git a/info/control2011/setup.ml b/info/control2011/setup.ml
index 8f530a8..506fa79 100644
--- a/info/control2011/setup.ml
+++ b/info/control2011/setup.ml
@@ -1,7 +1,7 @@
(* setup.ml generated for the first time by OASIS v0.2.0~alpha1 *)
(* OASIS_START *)
-(* DO NOT EDIT (digest: 0af52517b361f6b2a84eef09e15eb0d2) *)
+(* DO NOT EDIT (digest: f14e1278f404cfe11dc5fe0e2cf46a8e) *)
(*
Regenerated by OASIS v0.2.0
Visit http://oasis.forge.ocamlcore.org for more information and
@@ -1172,6 +1172,15 @@ module OASISUnixPath = struct
with Not_found ->
f
+ let capitalize_file f =
+ let dir = dirname f in
+ let base = basename f in
+ concat dir (String.capitalize base)
+
+ let uncapitalize_file f =
+ let dir = dirname f in
+ let base = basename f in
+ concat dir (String.uncapitalize base)
end
module OASISSection = struct
@@ -1298,8 +1307,8 @@ module OASISLibrary = struct
(List.map
(OASISUnixPath.concat bs.bs_path)
[modul;
- String.uncapitalize modul;
- String.capitalize modul])
+ OASISUnixPath.uncapitalize_file modul;
+ OASISUnixPath.capitalize_file modul])
in
[base_fn^".cmi"] :: hdrs
with Not_found ->
@@ -1310,8 +1319,8 @@ module OASISLibrary = struct
modul cs.cs_name;
(List.map (OASISUnixPath.concat bs.bs_path)
[modul^".cmi";
- String.uncapitalize modul ^ ".cmi";
- String.capitalize modul ^ ".cmi"])
+ OASISUnixPath.uncapitalize_file modul ^ ".cmi";
+ OASISUnixPath.capitalize_file modul ^ ".cmi"])
:: hdrs)
[]
lib.lib_modules
@@ -1327,7 +1336,7 @@ module OASISLibrary = struct
[cs.cs_name^".cma"] :: acc
in
let native acc =
- [cs.cs_name^".cmxa"] :: [cs.cs_name^(ext_lib ())] :: acc
+ [cs.cs_name^".cmxs"] :: [cs.cs_name^".cmxa"] :: [cs.cs_name^(ext_lib ())] :: acc
in
match bs.bs_compiled_object with
| Native ->
@@ -4302,10 +4311,10 @@ module InternalInstallPlugin = struct
(Filename.concat path)
[modul^".mli";
modul^".ml";
- String.uncapitalize modul^".mli";
- String.capitalize modul^".mli";
- String.uncapitalize modul^".ml";
- String.capitalize modul^".ml"])
+ OASISUnixPath.uncapitalize_file modul^".mli";
+ OASISUnixPath.capitalize_file modul^".mli";
+ OASISUnixPath.uncapitalize_file modul^".ml";
+ OASISUnixPath.capitalize_file modul^".ml"])
:: acc
with Not_found ->
begin
@@ -4742,6 +4751,7 @@ module OCamlbuildPlugin = struct
(fun fn ->
ends_with ".cma" fn ||
ends_with ".cmxa" fn ||
+ ends_with ".cmxs" fn ||
ends_with (ext_lib ()) fn ||
ends_with (ext_dll ()) fn))
unix_files)
@@ -5065,7 +5075,8 @@ let setup_t =
bs_nativeopt = [(OASISExpr.EBool true, [])];
},
{
- lib_modules = ["Krobot_interface_can"];
+ lib_modules =
+ ["Krobot_interface_can"; "Krobot_interface_service"];
lib_internal_modules = [];
lib_findlib_parent = Some "krobot";
lib_findlib_name = Some "interfaces";
diff --git a/info/control2011/src/interfaces/krobot-interfaces.mllib b/info/control2011/src/interfaces/krobot-interfaces.mllib
index d9a7ba4..b25ba41 100644
--- a/info/control2011/src/interfaces/krobot-interfaces.mllib
+++ b/info/control2011/src/interfaces/krobot-interfaces.mllib
@@ -1,4 +1,5 @@
# OASIS_START
-# DO NOT EDIT (digest: 4c91fe13e6ec5025ba97566f5b8f3875)
+# DO NOT EDIT (digest: ca580479098ade9ac95ba1a5fb876b9b)
Krobot_interface_can
+Krobot_interface_service
# OASIS_STOP
diff --git a/info/control2011/src/lib/krobot_can.ml b/info/control2011/src/lib/krobot_can.ml
index b533422..8b40c46 100644
--- a/info/control2011/src/lib/krobot_can.ml
+++ b/info/control2011/src/lib/krobot_can.ml
@@ -136,8 +136,8 @@ let put_uint16 = put_sint16
let put_sint32 str ofs v =
str.[ofs + 0] <- Char.unsafe_chr v;
str.[ofs + 1] <- Char.unsafe_chr (v lsr 8);
- str.[ofs + 1] <- Char.unsafe_chr (v lsr 16);
- str.[ofs + 1] <- Char.unsafe_chr (v lsr 24)
+ str.[ofs + 2] <- Char.unsafe_chr (v lsr 16);
+ str.[ofs + 3] <- Char.unsafe_chr (v lsr 24)
let put_uint32 = put_sint32
diff --git a/info/control2011/src/lib/krobot_message.ml b/info/control2011/src/lib/krobot_message.ml
index 38b3d50..ae4fe99 100644
--- a/info/control2011/src/lib/krobot_message.ml
+++ b/info/control2011/src/lib/krobot_message.ml
@@ -202,7 +202,7 @@ let decode frame =
float (get_uint16 frame.data 4) /. 1000.,
float (get_uint16 frame.data 6) /. 1000.)
| 202 ->
- Motor_move
+ Motor_turn
(float (get_sint32 frame.data 0) *. pi /. 1800.,
float (get_uint16 frame.data 4) *. pi /. 1800.,
float (get_uint16 frame.data 6) *. pi /. 1800.)
diff --git a/info/control2011/src/tools/krobot_remote.ml b/info/control2011/src/tools/krobot_remote.ml
index 9931ce6..f9ad3a6 100644
--- a/info/control2011/src/tools/krobot_remote.ml
+++ b/info/control2011/src/tools/krobot_remote.ml
@@ -17,7 +17,7 @@ let rec copy ta tb =
let handle_connection server ta =
ignore (
lwt () = Lwt_log.info "new connection" in
- let process = Lwt_process.open_process ("ssh", [|"ssh"; "krobot"; "krobot-local"|]) in
+ let process = Lwt_process.open_process ("ssh", [|"ssh"; "krobot"; "/home/krobot/bin/krobot-local"|]) in
try_lwt
lwt _ = Lwt_io.read_char process#stdout in
let tb =
@@ -41,6 +41,7 @@ let handle_connection server ta =
)
lwt () =
+ Sys.set_signal Sys.sigpipe Sys.Signal_ignore;
lwt server =
OBus_server.make_lowlevel
~addresses:[OBus_address.make "unix" [("abstract", "krobot")]]
hooks/post-receive
--
krobot
|
|
From: Jérémie D. <Ba...@us...> - 2011-03-28 21:34:46
|
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 bafd157aea6b314a6972bcf90b29e33e946c2909 (commit)
from e81a37c02c8309be6b43cf092ba3c1d1d4dc5c11 (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 bafd157aea6b314a6972bcf90b29e33e946c2909
Author: Jérémie Dimino <je...@di...>
Date: Mon Mar 28 23:34:00 2011 +0200
[info] add messages for odometry + use odometry in the viewer
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/lib/krobot_message.ml b/info/control2011/src/lib/krobot_message.ml
index 346a706..38b3d50 100644
--- a/info/control2011/src/lib/krobot_message.ml
+++ b/info/control2011/src/lib/krobot_message.ml
@@ -24,6 +24,7 @@ type t =
| Motor_status of bool
| Motor_move of float * float * float
| Motor_turn of float * float * float
+ | Odometry of float * float * float
| Req_motor_status
| Unknown of frame
@@ -61,6 +62,10 @@ let to_string = function
sprintf
"Motor_turn(%f, %f, %f)"
angle speed acc
+ | Odometry(x, y, theta) ->
+ sprintf
+ "Odometry(%f, %f, %f)"
+ x y theta
| Req_motor_status ->
"Req_motor_status"
| Unknown frame ->
@@ -114,6 +119,17 @@ let encode = function
~remote:false
~format:F11bits
~data
+ | Odometry(x, y, theta) ->
+ let data = String.create 6 in
+ put_sint16 data 0 (truncate (x *. 1000.));
+ put_sint16 data 2 (truncate (y *. 1000.));
+ put_sint16 data 4 (truncate (theta /. pi *. 18000.));
+ frame
+ ~identifier:104
+ ~kind:Data
+ ~remote:false
+ ~format:F11bits
+ ~data
| Motor_move(dist, speed, acc) ->
let data = String.create 8 in
put_sint32 data 0 (truncate (dist *. 1000.));
@@ -175,6 +191,11 @@ let decode frame =
get_float32 frame.data 4)
| 103 ->
Motor_status(get_uint8 frame.data 0 <> 0)
+ | 104 ->
+ Odometry
+ (float (get_sint16 frame.data 0) /. 1000.,
+ float (get_sint16 frame.data 2) /. 1000.,
+ float (get_sint16 frame.data 4) *. pi /. 18000.)
| 201 ->
Motor_move
(float (get_sint32 frame.data 0) /. 1000.,
diff --git a/info/control2011/src/lib/krobot_message.mli b/info/control2011/src/lib/krobot_message.mli
index a5c840d..e48cc4c 100644
--- a/info/control2011/src/lib/krobot_message.mli
+++ b/info/control2011/src/lib/krobot_message.mli
@@ -36,6 +36,9 @@ type t =
- [speed] is in rad/s
- [acceleration] is in rad/s^2
*)
+ | Odometry of float * float * float
+ (** [Odometry(x, y, theta)] the position of the robot on the
+ table. *)
| Req_motor_status
(** Request the status of the motors. *)
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index 263e917..c5561c2 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -178,14 +178,7 @@ module Board = struct
tolerance : GRange.scale;
mutable state : state;
mutable points : (float * float) list;
- }
-
- let create bus widget tolerance = {
- bus;
- widget;
- tolerance;
- state = { x = 0.2; y = 1.9; theta = 2. *. atan (-1.) };
- points = [];
+ mutable event : unit event;
}
let world_height = 2.1
@@ -477,13 +470,10 @@ module Board = struct
lwt () = Krobot_message.send board.bus (Unix.gettimeofday (), Motor_move(dist, 0.1, 0.2)) in
lwt () = wait_done board in
- (* Virtually move the robot. *)
- board.state <- { x; y; theta = board.state.theta +. alpha };
-
(* Remove the point. *)
board.points <- rest;
- (* Redraw everything. *)
+ (* Redraw everything without the last point. *)
queue_draw board;
loop ()
@@ -491,6 +481,30 @@ module Board = struct
return ()
in
loop ()
+
+ let create bus widget tolerance =
+ let board ={
+ bus;
+ widget;
+ tolerance;
+ state = { x = 0.2; y = 1.9; theta = 2. *. atan (-1.) };
+ points = [];
+ event = E.never;
+ } in
+ (* Move the robot on the board when we receive odometry
+ informations. *)
+ board.event <- (
+ E.map
+ (fun (ts, frame) ->
+ match frame with
+ | Odometry(x, y, theta) ->
+ board.state <- { x; y; theta };
+ queue_draw board
+ | _ ->
+ ())
+ (Krobot_message.recv bus)
+ );
+ board
end
(* +-----------------------------------------------------------------+
hooks/post-receive
--
krobot
|
|
From: Jérémie D. <Ba...@us...> - 2011-03-28 19:41:50
|
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 e81a37c02c8309be6b43cf092ba3c1d1d4dc5c11 (commit)
from a357d4d7598b0785e1350a8adeff05b2302b58ae (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 e81a37c02c8309be6b43cf092ba3c1d1d4dc5c11
Author: Jérémie Dimino <je...@di...>
Date: Mon Mar 28 21:41:19 2011 +0200
[info] log new connections in krobot-remote
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/tools/krobot_remote.ml b/info/control2011/src/tools/krobot_remote.ml
index c66de43..9931ce6 100644
--- a/info/control2011/src/tools/krobot_remote.ml
+++ b/info/control2011/src/tools/krobot_remote.ml
@@ -16,6 +16,7 @@ let rec copy ta tb =
let handle_connection server ta =
ignore (
+ lwt () = Lwt_log.info "new connection" in
let process = Lwt_process.open_process ("ssh", [|"ssh"; "krobot"; "krobot-local"|]) in
try_lwt
lwt _ = Lwt_io.read_char process#stdout in
hooks/post-receive
--
krobot
|
|
From: Jérémie D. <Ba...@us...> - 2011-03-28 19:10:02
|
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 a357d4d7598b0785e1350a8adeff05b2302b58ae (commit)
from 08f3d0878e9e62d6675c46d4ce887b5192491a66 (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 a357d4d7598b0785e1350a8adeff05b2302b58ae
Author: Jérémie Dimino <je...@di...>
Date: Mon Mar 28 21:09:12 2011 +0200
[info] implement robot controlling in the viewer
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index 17e93de..263e917 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -9,6 +9,7 @@
open Lwt
open Lwt_react
+open Krobot_message
let utf8 code =
let set_byte s o x = String.unsafe_set s o (Char.unsafe_chr x) in
@@ -172,13 +173,15 @@ module Board = struct
}
type t = {
+ bus : Krobot_bus.t;
widget : GMisc.drawing_area;
tolerance : GRange.scale;
mutable state : state;
mutable points : (float * float) list;
}
- let create widget tolerance = {
+ let create bus widget tolerance = {
+ bus;
widget;
tolerance;
state = { x = 0.2; y = 1.9; theta = 2. *. atan (-1.) };
@@ -450,6 +453,44 @@ module Board = struct
let result = List.tl (loop [0; Array.length points - 1]); in
board.points <- List.map (fun i -> points.(i)) result;
queue_draw board
+
+ let rec wait_done board =
+ lwt () = Lwt_unix.sleep 0.05 in
+ lwt ts, moving = Krobot_message.motor_status board.bus in
+ if moving then
+ wait_done board
+ else
+ return ()
+
+ let go board =
+ let rec loop () =
+ match board.points with
+ | (x, y) :: rest ->
+ (* Turn the robot. *)
+ let alpha = atan2 (y -. board.state.y) (x -. board.state.x) -. board.state.theta in
+ lwt () = Krobot_message.send board.bus (Unix.gettimeofday (), Motor_turn(alpha, 0.1, 0.2)) in
+ lwt () = wait_done board in
+
+ (* Move the robot. *)
+ let sqr x = x *. x in
+ let dist = sqrt (sqr (x -. board.state.x) +. sqr (y -. board.state.y)) in
+ lwt () = Krobot_message.send board.bus (Unix.gettimeofday (), Motor_move(dist, 0.1, 0.2)) in
+ lwt () = wait_done board in
+
+ (* Virtually move the robot. *)
+ board.state <- { x; y; theta = board.state.theta +. alpha };
+
+ (* Remove the point. *)
+ board.points <- rest;
+
+ (* Redraw everything. *)
+ queue_draw board;
+
+ loop ()
+ | [] ->
+ return ()
+ in
+ loop ()
end
(* +-----------------------------------------------------------------+
@@ -470,7 +511,7 @@ lwt () =
let lcd = LCD.create ui#lcd in
ignore (ui#lcd#event#connect#expose (fun ev -> LCD.draw lcd; true));
- let board = Board.create ui#scene ui#tolerance in
+ let board = Board.create bus ui#scene ui#tolerance in
ignore (ui#scene#event#connect#expose (fun ev -> Board.draw board; true));
ignore
(ui#scene#event#connect#button_press
@@ -497,4 +538,17 @@ lwt () =
Board.smooth board;
false));
+ let moving = ref false in
+ ignore
+ (ui#button_go#event#connect#button_release
+ (fun ev ->
+ if GdkEvent.Button.button ev = 1 && not !moving then
+ ignore_result (
+ moving := true;
+ lwt () = Board.go board in
+ moving := false;
+ return ()
+ );
+ false));
+
waiter
diff --git a/info/control2011/src/tools/krobot_viewer_ui.glade b/info/control2011/src/tools/krobot_viewer_ui.glade
index 8b16c21..7cd2fa4 100644
--- a/info/control2011/src/tools/krobot_viewer_ui.glade
+++ b/info/control2011/src/tools/krobot_viewer_ui.glade
@@ -233,6 +233,18 @@
</packing>
</child>
<child>
+ <widget class="GtkButton" id="button_go">
+ <property name="label" translatable="yes">Go!</property>
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="receives_default">True</property>
+ </widget>
+ <packing>
+ <property name="expand">False</property>
+ <property name="position">4</property>
+ </packing>
+ </child>
+ <child>
<widget class="GtkViewport" id="viewport1">
<property name="visible">True</property>
<property name="resize_mode">queue</property>
@@ -241,7 +253,7 @@
</child>
</widget>
<packing>
- <property name="position">4</property>
+ <property name="position">5</property>
</packing>
</child>
</widget>
hooks/post-receive
--
krobot
|
|
From: Jérémie D. <Ba...@us...> - 2011-03-28 18:07:18
|
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 08f3d0878e9e62d6675c46d4ce887b5192491a66 (commit)
from 89bbb8c217384777fc65196b9cdde754dc32052a (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 08f3d0878e9e62d6675c46d4ce887b5192491a66
Author: Jérémie Dimino <je...@di...>
Date: Mon Mar 28 20:06:46 2011 +0200
[info] implements requests
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/lib/krobot_message.ml b/info/control2011/src/lib/krobot_message.ml
index 953b0a1..346a706 100644
--- a/info/control2011/src/lib/krobot_message.ml
+++ b/info/control2011/src/lib/krobot_message.ml
@@ -24,6 +24,7 @@ type t =
| Motor_status of bool
| Motor_move of float * float * float
| Motor_turn of float * float * float
+ | Req_motor_status
| Unknown of frame
(* +-----------------------------------------------------------------+
@@ -60,6 +61,8 @@ let to_string = function
sprintf
"Motor_turn(%f, %f, %f)"
angle speed acc
+ | Req_motor_status ->
+ "Req_motor_status"
| Unknown frame ->
sprintf "Unknown%s" (Krobot_can.string_of_frame frame)
@@ -133,6 +136,13 @@ let encode = function
~remote:false
~format:F11bits
~data
+ | Req_motor_status ->
+ frame
+ ~identifier:103
+ ~kind:Data
+ ~remote:true
+ ~format:F11bits
+ ~data:""
| Unknown frame ->
frame
@@ -141,35 +151,42 @@ let encode = function
+-----------------------------------------------------------------+ *)
let decode frame =
- match frame.identifier with
- | 100 ->
- Encoder_position_direction_3_4
- (get_uint16 frame.data 0,
- (if get_uint8 frame.data 4 = 0 then Forward else Backward),
- get_uint16 frame.data 2,
- (if get_uint8 frame.data 5 = 0 then Forward else Backward))
- | 101 ->
- Encoder_position_speed_3
- (get_float32 frame.data 0,
- get_float32 frame.data 4)
- | 102 ->
- Encoder_position_speed_4
- (get_float32 frame.data 0,
- get_float32 frame.data 4)
- | 103 ->
- Motor_status(get_uint8 frame.data 0 <> 0)
- | 201 ->
- Motor_move
- (float (get_sint32 frame.data 0) /. 1000.,
- float (get_uint16 frame.data 4) /. 1000.,
- float (get_uint16 frame.data 6) /. 1000.)
- | 202 ->
- Motor_move
- (float (get_sint32 frame.data 0) *. pi /. 1800.,
- float (get_uint16 frame.data 4) *. pi /. 1800.,
- float (get_uint16 frame.data 6) *. pi /. 1800.)
- | _ ->
- Unknown frame
+ if frame.remote then
+ match frame.identifier with
+ | 103 ->
+ Req_motor_status
+ | _ ->
+ Unknown frame
+ else
+ match frame.identifier with
+ | 100 ->
+ Encoder_position_direction_3_4
+ (get_uint16 frame.data 0,
+ (if get_uint8 frame.data 4 = 0 then Forward else Backward),
+ get_uint16 frame.data 2,
+ (if get_uint8 frame.data 5 = 0 then Forward else Backward))
+ | 101 ->
+ Encoder_position_speed_3
+ (get_float32 frame.data 0,
+ get_float32 frame.data 4)
+ | 102 ->
+ Encoder_position_speed_4
+ (get_float32 frame.data 0,
+ get_float32 frame.data 4)
+ | 103 ->
+ Motor_status(get_uint8 frame.data 0 <> 0)
+ | 201 ->
+ Motor_move
+ (float (get_sint32 frame.data 0) /. 1000.,
+ float (get_uint16 frame.data 4) /. 1000.,
+ float (get_uint16 frame.data 6) /. 1000.)
+ | 202 ->
+ Motor_move
+ (float (get_sint32 frame.data 0) *. pi /. 1800.,
+ float (get_uint16 frame.data 4) *. pi /. 1800.,
+ float (get_uint16 frame.data 6) *. pi /. 1800.)
+ | _ ->
+ Unknown frame
(* +-----------------------------------------------------------------+
| Sending/receiving messages |
@@ -177,3 +194,20 @@ let decode frame =
let send bus (timestamp, msg) = Krobot_can.send bus (timestamp, encode msg)
let recv bus = E.map (fun (timestamp, frame) -> (timestamp, decode frame)) (Krobot_can.recv bus)
+
+(* +-----------------------------------------------------------------+
+ | Calls |
+ +-----------------------------------------------------------------+ *)
+
+let motor_status bus =
+ let t =
+ E.next
+ (E.fmap
+ (fun (ts, frame) ->
+ match frame with
+ | Motor_status status -> Some(ts, status)
+ | _ -> None)
+ (recv bus))
+ in
+ lwt () = send bus (Unix.gettimeofday (), Req_motor_status) in
+ t
diff --git a/info/control2011/src/lib/krobot_message.mli b/info/control2011/src/lib/krobot_message.mli
index c5f46ca..a5c840d 100644
--- a/info/control2011/src/lib/krobot_message.mli
+++ b/info/control2011/src/lib/krobot_message.mli
@@ -36,6 +36,10 @@ type t =
- [speed] is in rad/s
- [acceleration] is in rad/s^2
*)
+
+ | Req_motor_status
+ (** Request the status of the motors. *)
+
| Unknown of Krobot_can.frame
(** An unknown can frame. *)
@@ -54,3 +58,9 @@ val send : Krobot_bus.t -> (float * t) -> unit Lwt.t
val recv : Krobot_bus.t -> (float * t) React.event
(** [recv bus] is the event which receive messages. *)
+
+(** {6 Calls} *)
+
+(** The following functions send a request and wait for the result. *)
+
+val motor_status : Krobot_bus.t -> (float * bool) Lwt.t
hooks/post-receive
--
krobot
|
|
From: Jérémie D. <Ba...@us...> - 2011-03-28 17:51:59
|
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 89bbb8c217384777fc65196b9cdde754dc32052a (commit)
from 3ecc19e986bd7244c1f73e2aba4acee436ff9fb1 (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 89bbb8c217384777fc65196b9cdde754dc32052a
Author: Jeremie Dimino <di...@vo...>
Date: Mon Mar 28 19:51:03 2011 +0200
[info] add messages for making the robot to move/turn
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/lib/krobot_message.ml b/info/control2011/src/lib/krobot_message.ml
index 121d531..953b0a1 100644
--- a/info/control2011/src/lib/krobot_message.ml
+++ b/info/control2011/src/lib/krobot_message.ml
@@ -21,6 +21,9 @@ type t =
| Encoder_position_direction_3_4 of int * direction * int * direction
| Encoder_position_speed_3 of float * float
| Encoder_position_speed_4 of float * float
+ | Motor_status of bool
+ | Motor_move of float * float * float
+ | Motor_turn of float * float * float
| Unknown of frame
(* +-----------------------------------------------------------------+
@@ -45,6 +48,18 @@ let to_string = function
sprintf
"Encoder_position_speed_4(%f, %f)"
pos speed
+ | Motor_status moving ->
+ sprintf
+ "Motor_status(%B)"
+ moving
+ | Motor_move(dist, speed, acc) ->
+ sprintf
+ "Motor_move(%f, %f, %f)"
+ dist speed acc
+ | Motor_turn(angle, speed, acc) ->
+ sprintf
+ "Motor_turn(%f, %f, %f)"
+ angle speed acc
| Unknown frame ->
sprintf "Unknown%s" (Krobot_can.string_of_frame frame)
@@ -52,6 +67,8 @@ let to_string = function
| Encoding |
+-----------------------------------------------------------------+ *)
+let pi = 4. *. atan 1.
+
let encode = function
| Encoder_position_direction_3_4(pos3, dir3, pos4, dir4) ->
let data = String.create 6 in
@@ -85,6 +102,37 @@ let encode = function
~remote:false
~format:F11bits
~data
+ | Motor_status moving ->
+ let data = String.create 1 in
+ put_uint8 data 0 (if moving then 1 else 0);
+ frame
+ ~identifier:103
+ ~kind:Data
+ ~remote:false
+ ~format:F11bits
+ ~data
+ | Motor_move(dist, speed, acc) ->
+ let data = String.create 8 in
+ put_sint32 data 0 (truncate (dist *. 1000.));
+ put_uint16 data 4 (truncate (speed *. 1000.));
+ put_uint16 data 6 (truncate (acc *. 1000.));
+ frame
+ ~identifier:201
+ ~kind:Data
+ ~remote:false
+ ~format:F11bits
+ ~data
+ | Motor_turn(angle, speed, acc) ->
+ let data = String.create 8 in
+ put_sint32 data 0 (truncate (angle /. pi *. 18000.));
+ put_uint16 data 4 (truncate (speed /. pi *. 18000.));
+ put_uint16 data 6 (truncate (acc /. pi *. 18000.));
+ frame
+ ~identifier:202
+ ~kind:Data
+ ~remote:false
+ ~format:F11bits
+ ~data
| Unknown frame ->
frame
@@ -108,6 +156,18 @@ let decode frame =
Encoder_position_speed_4
(get_float32 frame.data 0,
get_float32 frame.data 4)
+ | 103 ->
+ Motor_status(get_uint8 frame.data 0 <> 0)
+ | 201 ->
+ Motor_move
+ (float (get_sint32 frame.data 0) /. 1000.,
+ float (get_uint16 frame.data 4) /. 1000.,
+ float (get_uint16 frame.data 6) /. 1000.)
+ | 202 ->
+ Motor_move
+ (float (get_sint32 frame.data 0) *. pi /. 1800.,
+ float (get_uint16 frame.data 4) *. pi /. 1800.,
+ float (get_uint16 frame.data 6) *. pi /. 1800.)
| _ ->
Unknown frame
diff --git a/info/control2011/src/lib/krobot_message.mli b/info/control2011/src/lib/krobot_message.mli
index eb56dee..c5f46ca 100644
--- a/info/control2011/src/lib/krobot_message.mli
+++ b/info/control2011/src/lib/krobot_message.mli
@@ -20,6 +20,22 @@ type t =
(** The position and speed of encoder 3. *)
| Encoder_position_speed_4 of float * float
(** The position and speed of encoder 4. *)
+ | Motor_status of bool
+ (** [true] iff a movement is in progress. *)
+ | Motor_move of float * float * float
+ (** [Motor_move(distance, speed, acceleration)] command to make
+ the robot to move.
+ - [distance] is in m
+ - [speed] is in m/s
+ - [acceleration] is in m/s^2
+ *)
+ | Motor_turn of float * float * float
+ (** [Motor_turn(angle, speed, acceleration)] command to make the
+ robot to turn.
+ - [angle] is in rad
+ - [speed] is in rad/s
+ - [acceleration] is in rad/s^2
+ *)
| Unknown of Krobot_can.frame
(** An unknown can frame. *)
hooks/post-receive
--
krobot
|
|
From: Jérémie D. <Ba...@us...> - 2011-03-28 17:20:30
|
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 3ecc19e986bd7244c1f73e2aba4acee436ff9fb1 (commit)
from 15f45fcb56d9fda61896461646f393788bc5001d (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 3ecc19e986bd7244c1f73e2aba4acee436ff9fb1
Author: Jérémie Dimino <je...@di...>
Date: Mon Mar 28 19:18:36 2011 +0200
[info] implement significant points computation
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index ff73838..17e93de 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -173,12 +173,14 @@ module Board = struct
type t = {
widget : GMisc.drawing_area;
+ tolerance : GRange.scale;
mutable state : state;
mutable points : (float * float) list;
}
- let create widget = {
+ let create widget tolerance = {
widget;
+ tolerance;
state = { x = 0.2; y = 1.9; theta = 2. *. atan (-1.) };
points = [];
}
@@ -406,6 +408,48 @@ module Board = struct
let clear board =
board.points <- [];
queue_draw board
+
+ let rec last = function
+ | [] -> failwith "Krobot_viewer.Board.last"
+ | [p] -> p
+ | _ :: l -> last l
+
+ let smooth board =
+ let points = Array.of_list ((board.state.x, board.state.y) :: board.points) in
+ let tolerance = board.tolerance#adjustment#value in
+ let rec loop = function
+ | i1 :: i2 :: rest ->
+ let (x1, y1) = points.(i1) and (x2, y2) = points.(i2) in
+ let a = y2 -. y1 and b = x1 -. x2 and c = x2 *. y1 -. x1 *. y2 in
+ let r = sqrt (a *. a +. b *. b) in
+ if r <> 0. then begin
+ (* Search the furthest point from the line passing by (x1,
+ y1) and (x2, y2) *)
+ let max_dist = ref 0. and at_max = ref i1 in
+ for i = i1 + 1 to i2 - 1 do
+ let (x, y) = points.(i) in
+ let d = abs_float (a *. x +. b *. y +. c) /. r in
+ if d > !max_dist then begin
+ max_dist := d;
+ at_max := i
+ end
+ done;
+ if !max_dist > tolerance then
+ (* The furthest point is out of tolerance, we split the
+ current region with it. *)
+ loop (i1 :: !at_max :: i2 :: rest)
+ else
+ (* The point is acceptable, we pass the next region. *)
+ i1:: loop (i2 :: rest)
+ end else
+ (* The two point are the same so we drop one. *)
+ loop (i2 :: rest)
+ | rest ->
+ rest
+ in
+ let result = List.tl (loop [0; Array.length points - 1]); in
+ board.points <- List.map (fun i -> points.(i)) result;
+ queue_draw board
end
(* +-----------------------------------------------------------------+
@@ -426,7 +470,7 @@ lwt () =
let lcd = LCD.create ui#lcd in
ignore (ui#lcd#event#connect#expose (fun ev -> LCD.draw lcd; true));
- let board = Board.create ui#scene in
+ let board = Board.create ui#scene ui#tolerance in
ignore (ui#scene#event#connect#expose (fun ev -> Board.draw board; true));
ignore
(ui#scene#event#connect#button_press
@@ -446,4 +490,11 @@ lwt () =
Board.clear board;
false));
+ ignore
+ (ui#button_smooth#event#connect#button_release
+ (fun ev ->
+ if GdkEvent.Button.button ev = 1 then
+ Board.smooth board;
+ false));
+
waiter
diff --git a/info/control2011/src/tools/krobot_viewer_ui.glade b/info/control2011/src/tools/krobot_viewer_ui.glade
index f4a7692..8b16c21 100644
--- a/info/control2011/src/tools/krobot_viewer_ui.glade
+++ b/info/control2011/src/tools/krobot_viewer_ui.glade
@@ -203,6 +203,36 @@
</packing>
</child>
<child>
+ <widget class="GtkHBox" id="hbox2">
+ <property name="visible">True</property>
+ <child>
+ <widget class="GtkLabel" id="label1">
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">Tolerance:</property>
+ </widget>
+ <packing>
+ <property name="expand">False</property>
+ <property name="position">0</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkHScale" id="tolerance">
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="adjustment">0.10000000000000001 0 1 0.10000000000000001 0.20000000000000001 0.5</property>
+ <property name="draw_value">False</property>
+ </widget>
+ <packing>
+ <property name="position">1</property>
+ </packing>
+ </child>
+ </widget>
+ <packing>
+ <property name="expand">False</property>
+ <property name="position">3</property>
+ </packing>
+ </child>
+ <child>
<widget class="GtkViewport" id="viewport1">
<property name="visible">True</property>
<property name="resize_mode">queue</property>
@@ -211,7 +241,7 @@
</child>
</widget>
<packing>
- <property name="position">3</property>
+ <property name="position">4</property>
</packing>
</child>
</widget>
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2011-03-28 15:36:39
|
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 15f45fcb56d9fda61896461646f393788bc5001d (commit)
from a8f1bf31bc0d2e833b0d9fc84a170e0d29c63038 (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 15f45fcb56d9fda61896461646f393788bc5001d
Author: Xavier Lagorce <Xav...@cr...>
Date: Mon Mar 28 17:35:22 2011 +0200
[Controller_Motor_STM32] Extended can_monitor to accept commands
Rmq : this is not usable with the current demo process
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.c
index 03cfe90..504e9b8 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.c
@@ -11,7 +11,9 @@
#include "hw/hw_led.h"
PROC_DEFINE_STACK(stack_can_send, KERN_MINSTACKSIZE * 8);
+PROC_DEFINE_STACK(stack_can_receive, KERN_MINSTACKSIZE * 8);
+// Structures to represent data into CAN messages
typedef struct {
uint16_t encoder1_pos __attribute__((__packed__));
uint16_t encoder2_pos __attribute__((__packed__));
@@ -25,6 +27,23 @@ typedef struct {
float speed __attribute__((__packed__));
} motor_msg_t;
+typedef struct {
+ uint8_t is_moving; // 1 if a movement action is in progress, 0 if it's finished
+} status_msg_t;
+
+typedef struct {
+ int32_t distance __attribute__((__packed__)); // Distance in mm (fixed point representation...)
+ uint16_t speed __attribute__((__packed__)); // Speed in mm/s
+ uint16_t acceleration __attribute__((__packed__)); // Acceleration in mm/s^2
+} move_msg_t;
+
+typedef struct {
+ int32_t angle __attribute__((__packed__)); // angle in 1/100 degrees (fixed point representation...)
+ uint16_t speed __attribute__((__packed__)); // Speed in 1/100 degrees/s
+ uint16_t acceleration __attribute__((__packed__)); // Acceleration in 1/100 degrees/s^2
+} turn_msg_t;
+
+// Union to manipulate CAN messages' data easily
typedef union {
encoder_msg_t data;
uint32_t data32[2];
@@ -35,8 +54,32 @@ typedef union {
uint32_t data32[2];
} motor_can_msg_t;
+typedef union {
+ status_msg_t data;
+ uint32_t data32[2];
+} status_can_msg_t;
+
+typedef union {
+ move_msg_t data;
+ uint32_t data32[2];
+} move_can_msg_t;
+
+typedef union {
+ turn_msg_t data;
+ uint32_t data32[2];
+} turn_can_msg_t;
+
+// Can messages IDs
+#define CAN_MSG_ENCODERS34 100 // encoder_can_msg_t
+#define CAN_MSG_MOTOR3 101 // motor_can_msg_t
+#define CAN_MSG_MOTOR4 102 // motor_can_msg_t
+#define CAN_MSG_STATUS 103 // status_can_msg_t
+#define CAN_MSG_MOVE 201 // move_can_msg_t
+#define CAN_MSG_TURN 202 // turn_can_msg_t
+
// Process for communication
static void NORETURN canMonitor_process(void);
+static void NORETURN canMonitorListen_process(void);
void canMonitorInit(void) {
can_config cfg;
@@ -53,10 +96,11 @@ void canMonitorInit(void) {
// Start communication process
proc_new(canMonitor_process, NULL, sizeof(stack_can_send), stack_can_send);
+ proc_new(canMonitorListen_process, NULL, sizeof(stack_can_receive), stack_can_receive);
}
static void NORETURN canMonitor_process(void) {
- encoder_can_msg_t msg_enc;
+ //encoder_can_msg_t msg_enc;
motor_can_msg_t msg_mot;
can_tx_frame txm;
Timer timer_can;
@@ -74,15 +118,15 @@ static void NORETURN canMonitor_process(void) {
timer_add(&timer_can);
// Sending ENCODER3 and ENCODER4 data
- msg_enc.data.encoder1_pos = getEncoderPosition(ENCODER3);
+ /*msg_enc.data.encoder1_pos = getEncoderPosition(ENCODER3);
msg_enc.data.encoder2_pos = getEncoderPosition(ENCODER4);
msg_enc.data.encoder1_dir = getEncoderDirection(ENCODER3);
msg_enc.data.encoder2_dir = getEncoderDirection(ENCODER4);
txm.data32[0] = msg_enc.data32[0];
txm.data32[1] = msg_enc.data32[1];
- txm.eid = 100;
- can_transmit(CAND1, &txm, ms_to_ticks(10));
+ txm.eid = CAN_MSG_ENCODERS34;
+ can_transmit(CAND1, &txm, ms_to_ticks(10));*/
// Sending MOTOR3 data
msg_mot.data.position = mc_getPosition(MOTOR3);
@@ -90,7 +134,7 @@ static void NORETURN canMonitor_process(void) {
txm.data32[0] = msg_mot.data32[0];
txm.data32[1] = msg_mot.data32[1];
- txm.eid = 101;
+ txm.eid = CAN_MSG_MOTOR3;
can_transmit(CAND1, &txm, ms_to_ticks(10));
// Sending MOTOR4 data
@@ -99,7 +143,7 @@ static void NORETURN canMonitor_process(void) {
txm.data32[0] = msg_mot.data32[0];
txm.data32[1] = msg_mot.data32[1];
- txm.eid = 102;
+ txm.eid = CAN_MSG_MOTOR4;
can_transmit(CAND1, &txm, ms_to_ticks(10));
// Wait for the next transmission timer
@@ -107,3 +151,51 @@ static void NORETURN canMonitor_process(void) {
}
}
+static void NORETURN canMonitorListen_process(void) {
+
+ can_rx_frame frame;
+ bool received = false;
+ can_tx_frame txm;
+
+ status_can_msg_t status_msg;
+ move_can_msg_t move_msg;
+ turn_can_msg_t turn_msg;
+
+ // Initialize constant parameters of TX frame
+ txm.dlc = 8;
+ txm.rtr = 0;
+ txm.ide = 1;
+ txm.sid = 0;
+
+ while (1) {
+ received = can_receive(CAND1, &frame, ms_to_ticks(100));
+ if (received) {
+ if (frame.rtr == 1) {
+ // Handle requests
+ switch (frame.eid) {
+ case CAN_MSG_STATUS:
+ status_msg.data.is_moving = !tc_is_finished();
+ txm.data32[0] = status_msg.data32[0];
+ txm.data32[1] = status_msg.data32[1];
+ txm.eid = CAN_MSG_STATUS;
+ can_transmit(CAND1, &txm, ms_to_ticks(10));
+ break;
+ }
+ } else {
+ // Handle commands and other informations
+ switch (frame.eid) {
+ case CAN_MSG_MOVE:
+ move_msg.data32[0] = frame.data32[0];
+ move_msg.data32[1] = frame.data32[1];
+ tc_move(move_msg.data.distance / 1000.0, move_msg.data.speed / 1000.0, move_msg.data.acceleration / 1000.0);
+ break;
+ case CAN_MSG_TURN:
+ turn_msg.data32[0] = frame.data32[0];
+ turn_msg.data32[1] = frame.data32[1];
+ tc_turn(turn_msg.data.angle / 100.0, turn_msg.data.speed / 100.0, turn_msg.data.acceleration / 100.0);
+ break;
+ }
+ }
+ }
+ }
+}
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.h b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.h
index 9f5be0c..84375a9 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.h
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.h
@@ -15,6 +15,7 @@
#include "encoder.h"
#include "motor_controller.h"
+#include "trajectory_controller.h"
void canMonitorInit(void);
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2011-03-28 14:48:23
|
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 a8f1bf31bc0d2e833b0d9fc84a170e0d29c63038 (commit)
from 9ad17cbb015d86465e927fe8c7c6ef4922208319 (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 a8f1bf31bc0d2e833b0d9fc84a170e0d29c63038
Author: Xavier Lagorce <Xav...@cr...>
Date: Mon Mar 28 16:47:20 2011 +0200
[Controller_Motor_STM32] Correctly manage degeneration of trapezoidal speed profile into a triangle.
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.c
index fabfa7e..83b825f 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.c
@@ -9,10 +9,11 @@
#include "trajectory_controller.h"
-#define TRAPEZOID_STATE_STOP 0
-#define TRAPEZOID_STATE_ACC 1
-#define TRAPEZOID_STATE_CONST 2
-#define TRAPEZOID_STATE_DEC 3
+#define TRAPEZOID_STATE_STOP 0
+#define TRAPEZOID_STATE_ACC 1
+#define TRAPEZOID_STATE_CONST 2
+#define TRAPEZOID_STATE_DEC 3
+#define TRAPEZOID_STATE_TRIANGLE 4
#define SIGN(val) ((val) >= 0 ? 1 : -1)
#define SELECT_THRESHOLD(dir) ((dir) == 1 ? GEN_CALLBACK_SUP : GEN_CALLBACK_INF)
@@ -23,7 +24,7 @@ void trapezoid_callback(command_generator_t *generator);
typedef struct {
float angle, speed, acceleration, init_val;
int8_t dir;
- uint8_t state;
+ uint8_t state, is_triangle;
} ramp_automaton_t;
command_generator_t right_wheel, left_wheel, right_wheel_speed, left_wheel_speed;
@@ -64,6 +65,14 @@ void trapezoid_callback(command_generator_t *generator) {
add_callback(trap, SELECT_THRESHOLD(automaton->dir), automaton->angle - automaton->speed*automaton->speed/automaton->acceleration/2., trapezoid_callback);
automaton->state = TRAPEZOID_STATE_CONST;
break;
+ case TRAPEZOID_STATE_TRIANGLE:
+ // Special case of triangle profile, we have to slow down at the end of the acceleration phase
+ adjust_speed(trap_speed, -automaton->acceleration);
+ adjust_value(trap_speed, automaton->speed);
+ add_callback(trap, SELECT_THRESHOLD(automaton->dir), automaton->angle, trapezoid_callback);
+ add_callback(trap_speed, SELECT_THRESHOLD_DEC(automaton->dir), 0.1*automaton->speed, trapezoid_callback);
+ automaton->state = TRAPEZOID_STATE_DEC;
+ break;
case TRAPEZOID_STATE_CONST:
// End of the constant speed phase, we have to slow down
adjust_speed(trap_speed, -automaton->acceleration);
@@ -116,72 +125,140 @@ uint8_t tc_is_finished(void) {
}
void tc_move(float distance, float speed, float acceleration) {
- float acc_dist;
+ float acc_dist, t_acc, t_end;
// Verify parameters
if (distance == 0 || speed <= 0 || acceleration <= 0)
return;
- // Compute trapezoid parameters for right motor
+ // Compute some common parameters
+ // For the right motor
right_trap.init_val = get_output_value(&right_wheel);
right_trap.dir = SIGN(distance);
right_trap.angle = right_trap.init_val + distance / WHEEL_R * 180 / M_PI;
- right_trap.speed = right_trap.dir * speed / WHEEL_R * 180 / M_PI;
right_trap.acceleration = right_trap.dir * acceleration / WHEEL_R * 180 / M_PI;
- right_trap.state = TRAPEZOID_STATE_ACC;
-
- // Compute trapezoid parameters for left motor
+ // For the left motor
left_trap.init_val = get_output_value(&left_wheel);
left_trap.dir = SIGN(distance);
left_trap.angle = left_trap.init_val + distance / WHEEL_R * 180 / M_PI;
- left_trap.speed = left_trap.dir * speed / WHEEL_R * 180 / M_PI;
left_trap.acceleration = left_trap.dir * acceleration / WHEEL_R * 180 / M_PI;
- left_trap.state = TRAPEZOID_STATE_ACC;
-
- // This is distance during which the robot will accelerate
- acc_dist = SIGN(distance) * speed * speed / acceleration / 2.0 / WHEEL_R * 180.0 / M_PI;
-
- // Set accelerations for the trapezoid's first phase and associated callbacks
- adjust_speed(&right_wheel_speed, right_trap.acceleration);
- add_callback(&right_wheel, SELECT_THRESHOLD(right_trap.dir), right_trap.init_val + acc_dist, trapezoid_callback);
- add_callback(&right_wheel_speed, SELECT_THRESHOLD(right_trap.dir), right_trap.speed, trapezoid_callback);
- adjust_speed(&left_wheel_speed, left_trap.acceleration);
- add_callback(&left_wheel, SELECT_THRESHOLD(left_trap.dir), left_trap.init_val + acc_dist, trapezoid_callback);
- add_callback(&left_wheel_speed, SELECT_THRESHOLD(left_trap.dir), left_trap.speed, trapezoid_callback);
+
+ // Is the trapezoidal speed profile posible ?
+ t_acc = speed / acceleration;
+ t_end = (speed * speed + distance * acceleration) / (speed * acceleration);
+
+ if (t_end > (2. * t_acc)) {
+ // A trapezoidal speed profile is possible
+ right_trap.is_triangle = 0;
+ left_trap.is_triangle = 0;
+
+ // Compute trapezoid parameters for right motor
+ right_trap.speed = right_trap.dir * speed / WHEEL_R * 180 / M_PI;
+ right_trap.state = TRAPEZOID_STATE_ACC;
+
+ // Compute trapezoid parameters for left motor
+ left_trap.speed = left_trap.dir * speed / WHEEL_R * 180 / M_PI;
+ left_trap.state = TRAPEZOID_STATE_ACC;
+
+ // This is distance during which the robot will accelerate
+ acc_dist = SIGN(distance) * speed * speed / acceleration / 2.0 / WHEEL_R * 180.0 / M_PI;
+
+ // Set accelerations for the trapezoid's first phase and associated callbacks
+ adjust_speed(&right_wheel_speed, right_trap.acceleration);
+ add_callback(&right_wheel, SELECT_THRESHOLD(right_trap.dir), right_trap.init_val + acc_dist, trapezoid_callback);
+ add_callback(&right_wheel_speed, SELECT_THRESHOLD(right_trap.dir), right_trap.speed, trapezoid_callback);
+ adjust_speed(&left_wheel_speed, left_trap.acceleration);
+ add_callback(&left_wheel, SELECT_THRESHOLD(left_trap.dir), left_trap.init_val + acc_dist, trapezoid_callback);
+ add_callback(&left_wheel_speed, SELECT_THRESHOLD(left_trap.dir), left_trap.speed, trapezoid_callback);
+ } else {
+ // A trapezoidal speed profile is not possible with the given acceleration, let's go for a triangle
+ right_trap.is_triangle = 1;
+ left_trap.is_triangle = 1;
+
+ // Compute triangle parameters for right motor
+ right_trap.speed = right_trap.dir * sqrt(right_trap.angle * right_trap.acceleration);
+ right_trap.state = TRAPEZOID_STATE_TRIANGLE;
+
+ // Compute triangle parameters for left motor
+ left_trap.speed = left_trap.dir * sqrt(left_trap.angle * left_trap.acceleration);
+ left_trap.state = TRAPEZOID_STATE_TRIANGLE;
+
+ // Set accelerations for the triangle's first phase and associated callbacks
+ adjust_speed(&right_wheel_speed, right_trap.acceleration);
+ add_callback(&right_wheel, SELECT_THRESHOLD(right_trap.dir), right_trap.init_val + right_trap.angle/2.0, trapezoid_callback);
+ add_callback(&right_wheel_speed, SELECT_THRESHOLD(right_trap.dir), right_trap.speed, trapezoid_callback);
+ adjust_speed(&left_wheel_speed, left_trap.acceleration);
+ add_callback(&left_wheel, SELECT_THRESHOLD(left_trap.dir), left_trap.init_val + left_trap.angle/2.0, trapezoid_callback);
+ add_callback(&left_wheel_speed, SELECT_THRESHOLD(left_trap.dir), left_trap.speed, trapezoid_callback);
+ }
}
void tc_turn(float angle, float speed, float acceleration) {
- float acc_angle;
+ float acc_angle, t_acc, t_end;
// Verify parameters
if (angle == 0 || speed <= 0 || acceleration <= 0)
return;
- // Compute trapezoid parameters for right motor
+ // Compute some common parameters
+ // For the right motor
right_trap.init_val = get_output_value(&right_wheel);
right_trap.dir = SIGN(angle);
right_trap.angle = right_trap.init_val + angle * STRUCT_B / 2.0 / WHEEL_R;
- right_trap.speed = right_trap.dir * speed * STRUCT_B / 2.0 / WHEEL_R;
right_trap.acceleration = right_trap.dir * acceleration * STRUCT_B / 2.0 / WHEEL_R;
- right_trap.state = TRAPEZOID_STATE_ACC;
-
- // Compute trapezoid parameters for left motor
+ // For the left motor
left_trap.init_val = get_output_value(&left_wheel);
left_trap.dir = -right_trap.dir;
left_trap.angle = left_trap.init_val - angle * STRUCT_B / 2.0 / WHEEL_R;
- left_trap.speed = left_trap.dir * speed * STRUCT_B / 2.0 / WHEEL_R;
left_trap.acceleration = left_trap.dir * acceleration * STRUCT_B / 2.0 / WHEEL_R;
- left_trap.state = TRAPEZOID_STATE_ACC;
-
- // This is the angle during which the robot will accelerate
- acc_angle = SIGN(angle) * speed * speed / acceleration / 2.0 * STRUCT_B / 2.0 / WHEEL_R;
-
- // Set accelerations for the trapezoid's first phase and associated callbacks
- adjust_speed(&right_wheel_speed, right_trap.acceleration);
- add_callback(&right_wheel, SELECT_THRESHOLD(right_trap.dir), right_trap.init_val + acc_angle, trapezoid_callback);
- add_callback(&right_wheel_speed, SELECT_THRESHOLD(right_trap.dir), right_trap.speed, trapezoid_callback);
- adjust_speed(&left_wheel_speed, left_trap.acceleration);
- add_callback(&left_wheel, SELECT_THRESHOLD(left_trap.dir), left_trap.init_val - acc_angle, trapezoid_callback);
- add_callback(&left_wheel_speed, SELECT_THRESHOLD(left_trap.dir), left_trap.speed, trapezoid_callback);
+
+ // Is the trapezoidal speed profile posible ?
+ t_acc = speed / acceleration;
+ t_end = (speed * speed + angle * acceleration) / (speed * acceleration);
+
+ if (t_end > (2. * t_acc)) {
+ // A trapezoidal speed profile is possible
+ right_trap.is_triangle = 0;
+ left_trap.is_triangle = 0;
+
+ // Compute trapezoid parameters for right motor
+ right_trap.speed = right_trap.dir * speed * STRUCT_B / 2.0 / WHEEL_R;
+ right_trap.state = TRAPEZOID_STATE_ACC;
+
+ // Compute trapezoid parameters for left motor
+ left_trap.speed = left_trap.dir * speed * STRUCT_B / 2.0 / WHEEL_R;
+ left_trap.state = TRAPEZOID_STATE_ACC;
+
+ // This is the angle during which the robot will accelerate
+ acc_angle = SIGN(angle) * speed * speed / acceleration / 2.0 * STRUCT_B / 2.0 / WHEEL_R;
+
+ // Set accelerations for the trapezoid's first phase and associated callbacks
+ adjust_speed(&right_wheel_speed, right_trap.acceleration);
+ add_callback(&right_wheel, SELECT_THRESHOLD(right_trap.dir), right_trap.init_val + acc_angle, trapezoid_callback);
+ add_callback(&right_wheel_speed, SELECT_THRESHOLD(right_trap.dir), right_trap.speed, trapezoid_callback);
+ adjust_speed(&left_wheel_speed, left_trap.acceleration);
+ add_callback(&left_wheel, SELECT_THRESHOLD(left_trap.dir), left_trap.init_val - acc_angle, trapezoid_callback);
+ add_callback(&left_wheel_speed, SELECT_THRESHOLD(left_trap.dir), left_trap.speed, trapezoid_callback);
+ } else {
+ // A trapezoidal speed profile is not possible with the given acceleration, let's go for a triangle
+ right_trap.is_triangle = 1;
+ left_trap.is_triangle = 1;
+
+ // Compute triangle parameters for right motor
+ right_trap.speed = right_trap.dir * sqrt(right_trap.angle * right_trap.acceleration);
+ right_trap.state = TRAPEZOID_STATE_TRIANGLE;
+
+ // Compute triangle parameters for left motor
+ left_trap.speed = left_trap.dir * sqrt(left_trap.angle * left_trap.acceleration);
+ left_trap.state = TRAPEZOID_STATE_TRIANGLE;
+
+ // Set accelerations for the triangle's first phase and associated callbacks
+ adjust_speed(&right_wheel_speed, right_trap.acceleration);
+ add_callback(&right_wheel, SELECT_THRESHOLD(right_trap.dir), right_trap.init_val + right_trap.angle/2.0, trapezoid_callback);
+ add_callback(&right_wheel_speed, SELECT_THRESHOLD(right_trap.dir), right_trap.speed, trapezoid_callback);
+ adjust_speed(&left_wheel_speed, left_trap.acceleration);
+ add_callback(&left_wheel, SELECT_THRESHOLD(left_trap.dir), left_trap.init_val + left_trap.angle/2.0, trapezoid_callback);
+ add_callback(&left_wheel_speed, SELECT_THRESHOLD(left_trap.dir), left_trap.speed, trapezoid_callback);
+ }
}
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2011-03-28 13:56:28
|
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 9ad17cbb015d86465e927fe8c7c6ef4922208319 (commit)
from 5d9ac6bda6ff0f893d0f659d37010cfd3ea4a080 (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 9ad17cbb015d86465e927fe8c7c6ef4922208319
Author: Xavier Lagorce <Xav...@cr...>
Date: Mon Mar 28 15:49:25 2011 +0200
[Controller_Motor_STM32] First trajectory controller and minor changes
* Renamed the motor controller's initialization function
* Cleaned main.c and documented the demo process
* First implementation of a trajectory controller. It should be able to :
- Generate some trapezoidal speed profiles
- Move along a line (forward and backward) given a distance, a speed and
an acceleration.
- Turn around the propulsion shaft center given an angle, a speed and an
acceleration (CW and CCW).
- Indicate if an action is in progress or if it is finished
/!\ For moving distances/angles to be correct, the trapezoidal speed profile
has to be possible, if not, the real distances/angles will be bigger.
(et bim ! la bordure...).
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/controller_motor_stm32_user.mk b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/controller_motor_stm32_user.mk
index 6387ba7..5ad6b22 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/controller_motor_stm32_user.mk
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/controller_motor_stm32_user.mk
@@ -15,6 +15,7 @@ controller_motor_stm32_USER_CSRC = \
$(controller_motor_stm32_SRC_PATH)/encoder.c \
$(controller_motor_stm32_SRC_PATH)/motor_controller.c \
$(controller_motor_stm32_SRC_PATH)/command_generator.c \
+ $(controller_motor_stm32_SRC_PATH)/trajectory_controller.c \
$(controller_motor_stm32_SRC_PATH)/can_monitor.c \
$(controller_motor_stm32_SRC_PATH)/main.c \
#
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/main.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/main.c
index 9dd248c..6b6ca72 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/main.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/main.c
@@ -16,6 +16,7 @@
#include "motor_controller.h"
#include "can_monitor.h"
#include "command_generator.h"
+#include "trajectory_controller.h"
command_generator_t genR, genL;
@@ -37,18 +38,13 @@ static void init(void)
proc_init();
// Initialize CONTROL driver (will initialize MOTOR and ENCODER subsystems)
- speedControlInit();
+ motorControllerInit();
// Initialize CAN_MONITOR
canMonitorInit();
// Start control of drive motors
- new_ramp_generator(&genR, 0., 180.);
- new_ramp_generator(&genL, 0., 180.);
- float k[] = {-68.0325, -1.0205};
- float l0[] = {0.0236, 3.9715};
- mc_new_controller(MOTOR3, ENCODER3, -360.0/2000.0/15.0, 0.833, 0.015, 0.005, k, -k[0], l0, &genL);
- mc_new_controller(MOTOR4, ENCODER4, -360.0/2000.0/15.0, 0.833, 0.015, 0.005, k, -k[0], l0, &genR);
+ init_trajectory_controller();
// Blink to say we are ready
for (uint8_t i=0; i < 2; i++) {
@@ -65,57 +61,39 @@ static void init(void)
}
}
-static void NORETURN square_process(void)
+static void NORETURN demo_process(void)
{
- // Let's roll !
- while (1)
- {
- motorSetSpeed(MOTOR3 | MOTOR4, 1500);
- timer_delay(1000);
- motorSetSpeed(MOTOR3 | MOTOR4, -1500);
- timer_delay(1000);
- motorSetSpeed(MOTOR3 | MOTOR4, 1500);
- timer_delay(1000);
- motorSetSpeed(MOTOR3 | MOTOR4, -1500);
- timer_delay(1000);
- motorSetSpeed(MOTOR3 | MOTOR4, 1500);
- timer_delay(1000);
- motorSetSpeed(MOTOR3 | MOTOR4, -1500);
- timer_delay(1000);
- disableMotor(MOTOR3 | MOTOR4);
- break;
- }
-}
+ while (1) {
+ // Light a LED on an unused motor to indicate we should be doing something
+ // with the motors very soon
+ LED1_ON();
+ timer_delay(1000);
-static void NORETURN goto_process(void)
-{
- LED1_ON();
- start_generator(&genL);
- start_generator(&genR);
- timer_delay(2000);
- pause_generator(&genL);
- pause_generator(&genR);
- adjust_speed(&genL, 360.);
- adjust_speed(&genR, 360.);
- timer_delay(1000);
- start_generator(&genL);
- start_generator(&genR);
- timer_delay(2000);
- adjust_speed(&genL, 0.);
- adjust_speed(&genR, 0.);
- timer_delay(200);
- mc_delete_controller(MOTOR3);
- mc_delete_controller(MOTOR4);
- LED1_OFF();
-}
+ // Move the robot !
+ tc_move(2., 1., 1.);
+
+ // Wait until the movement is finished
+ while (!tc_is_finished())
+ timer_delay(1000);
+
+ // Stop motor controllers to be able to freely move the robot
+ mc_delete_controller(MOTOR3);
+ mc_delete_controller(MOTOR4);
+ // Doing nothing more, so shutdown the LED
+ LED1_OFF();
+
+ // Cleanly stop the demo process
+ proc_exit();
+ }
+}
int main(void)
{
init();
/* Create a new child process */
- proc_new(goto_process, NULL, sizeof(stack_op), stack_op);
+ proc_new(demo_process, NULL, sizeof(stack_op), stack_op);
/*
* The main process is kept to periodically report the stack
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor_controller.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor_controller.c
index 8246a8c..fdfcc9b 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor_controller.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor_controller.c
@@ -161,7 +161,7 @@ static void NORETURN motorController_process(void) {
}
}
-void speedControlInit() {
+void motorControllerInit() {
uint8_t motor_ind;
for (motor_ind = 0; motor_ind < 4; motor_ind++)
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor_controller.h b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor_controller.h
index 749bb41..0c1a4e1 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor_controller.h
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor_controller.h
@@ -13,7 +13,7 @@
#define CONTROLLER_OK 0
#define CONTROLLER_ALREADY_USED 1
-void speedControlInit(void);
+void motorControllerInit(void);
float mc_getSpeed(uint8_t motor);
float mc_getPosition(uint8_t motor);
void mc_setReference(uint8_t motor, command_generator_t *generator);
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.c
new file mode 100644
index 0000000..fabfa7e
--- /dev/null
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.c
@@ -0,0 +1,187 @@
+/*
+ * trajectory_controller.c
+ * -----------------------
+ * Copyright : (c) 2011, Xavier Lagorce <Xav...@cr...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ */
+
+#include "trajectory_controller.h"
+
+#define TRAPEZOID_STATE_STOP 0
+#define TRAPEZOID_STATE_ACC 1
+#define TRAPEZOID_STATE_CONST 2
+#define TRAPEZOID_STATE_DEC 3
+
+#define SIGN(val) ((val) >= 0 ? 1 : -1)
+#define SELECT_THRESHOLD(dir) ((dir) == 1 ? GEN_CALLBACK_SUP : GEN_CALLBACK_INF)
+#define SELECT_THRESHOLD_DEC(dir) ((dir) == 1 ? GEN_CALLBACK_INF : GEN_CALLBACK_SUP)
+
+void trapezoid_callback(command_generator_t *generator);
+
+typedef struct {
+ float angle, speed, acceleration, init_val;
+ int8_t dir;
+ uint8_t state;
+} ramp_automaton_t;
+
+command_generator_t right_wheel, left_wheel, right_wheel_speed, left_wheel_speed;
+ramp_automaton_t right_trap, left_trap;
+
+uint8_t controller_state;
+
+void trapezoid_callback(command_generator_t *generator) {
+ ramp_automaton_t *automaton;
+ command_generator_t *trap, *trap_speed;
+
+ // Select the correct trapezoid depending on which callback triggered
+ if (generator == &right_wheel || generator == &right_wheel_speed) {
+ automaton = &right_trap;
+ trap = &right_wheel;
+ trap_speed = &right_wheel_speed;
+ } else if (generator == &left_wheel || generator == &left_wheel_speed) {
+ automaton = &left_trap;
+ trap = &left_wheel;
+ trap_speed = &left_wheel_speed;
+ } else {
+ return;
+ }
+
+ // Unregistering callbacks on the current ramp
+ remove_callback(trap);
+ remove_callback(trap_speed);
+
+ // Automaton evolution
+ switch (automaton->state) {
+ case TRAPEZOID_STATE_STOP:
+ // The automaton is already stopped, this shouldn't happen
+ break;
+ case TRAPEZOID_STATE_ACC:
+ // Speed is inceasing, we have to go to the constant speed phase of the movement
+ adjust_speed(trap_speed, 0.);
+ adjust_value(trap_speed, automaton->speed);
+ add_callback(trap, SELECT_THRESHOLD(automaton->dir), automaton->angle - automaton->speed*automaton->speed/automaton->acceleration/2., trapezoid_callback);
+ automaton->state = TRAPEZOID_STATE_CONST;
+ break;
+ case TRAPEZOID_STATE_CONST:
+ // End of the constant speed phase, we have to slow down
+ adjust_speed(trap_speed, -automaton->acceleration);
+ add_callback(trap, SELECT_THRESHOLD(automaton->dir), automaton->angle, trapezoid_callback);
+ add_callback(trap_speed, SELECT_THRESHOLD_DEC(automaton->dir), 0.1*automaton->speed, trapezoid_callback);
+ automaton->state = TRAPEZOID_STATE_DEC;
+ break;
+ case TRAPEZOID_STATE_DEC:
+ // End of the movement, stop the motor
+ adjust_speed(trap_speed, 0.);
+ adjust_value(trap_speed, 0.);
+ adjust_value(trap, automaton->angle);
+ automaton->state = TRAPEZOID_STATE_STOP;
+ break;
+ }
+}
+
+void init_trajectory_controller(void) {
+ float k[] = {-68.0325, -1.0205};
+ float l0[] = {0.0236, 3.9715};
+
+ // Create generator for position and speed manipulation
+ new_ramp_generator(&right_wheel_speed, 0., 0.);
+ new_ramp_generator(&left_wheel_speed, 0., 0.);
+ new_ramp2_generator(&right_wheel, 0., &right_wheel_speed);
+ new_ramp2_generator(&left_wheel, 0., &left_wheel_speed);
+
+ // Start control of drive motors
+ mc_new_controller(MOTOR3, ENCODER3, -360.0/2000.0/15.0, 0.833, 0.015, 0.005, k, -k[0], l0, &left_wheel);
+ mc_new_controller(MOTOR4, ENCODER4, -360.0/2000.0/15.0, 0.833, 0.015, 0.005, k, -k[0], l0, &right_wheel);
+
+ // Start the generator
+ start_generator(&right_wheel);
+ start_generator(&left_wheel);
+ start_generator(&right_wheel_speed);
+ start_generator(&left_wheel_speed);
+
+ // Initial state of automatons
+ right_trap.state = TRAPEZOID_STATE_STOP;
+ left_trap.state = TRAPEZOID_STATE_STOP;
+}
+
+uint8_t tc_is_finished(void) {
+ // Is there currently a trapezoidal command running ?
+ if (right_trap.state == TRAPEZOID_STATE_STOP && left_trap.state == TRAPEZOID_STATE_STOP)
+ return 1;
+
+ // No running process, last planified action should be finished
+ return 0;
+}
+
+void tc_move(float distance, float speed, float acceleration) {
+ float acc_dist;
+
+ // Verify parameters
+ if (distance == 0 || speed <= 0 || acceleration <= 0)
+ return;
+
+ // Compute trapezoid parameters for right motor
+ right_trap.init_val = get_output_value(&right_wheel);
+ right_trap.dir = SIGN(distance);
+ right_trap.angle = right_trap.init_val + distance / WHEEL_R * 180 / M_PI;
+ right_trap.speed = right_trap.dir * speed / WHEEL_R * 180 / M_PI;
+ right_trap.acceleration = right_trap.dir * acceleration / WHEEL_R * 180 / M_PI;
+ right_trap.state = TRAPEZOID_STATE_ACC;
+
+ // Compute trapezoid parameters for left motor
+ left_trap.init_val = get_output_value(&left_wheel);
+ left_trap.dir = SIGN(distance);
+ left_trap.angle = left_trap.init_val + distance / WHEEL_R * 180 / M_PI;
+ left_trap.speed = left_trap.dir * speed / WHEEL_R * 180 / M_PI;
+ left_trap.acceleration = left_trap.dir * acceleration / WHEEL_R * 180 / M_PI;
+ left_trap.state = TRAPEZOID_STATE_ACC;
+
+ // This is distance during which the robot will accelerate
+ acc_dist = SIGN(distance) * speed * speed / acceleration / 2.0 / WHEEL_R * 180.0 / M_PI;
+
+ // Set accelerations for the trapezoid's first phase and associated callbacks
+ adjust_speed(&right_wheel_speed, right_trap.acceleration);
+ add_callback(&right_wheel, SELECT_THRESHOLD(right_trap.dir), right_trap.init_val + acc_dist, trapezoid_callback);
+ add_callback(&right_wheel_speed, SELECT_THRESHOLD(right_trap.dir), right_trap.speed, trapezoid_callback);
+ adjust_speed(&left_wheel_speed, left_trap.acceleration);
+ add_callback(&left_wheel, SELECT_THRESHOLD(left_trap.dir), left_trap.init_val + acc_dist, trapezoid_callback);
+ add_callback(&left_wheel_speed, SELECT_THRESHOLD(left_trap.dir), left_trap.speed, trapezoid_callback);
+}
+
+void tc_turn(float angle, float speed, float acceleration) {
+ float acc_angle;
+
+ // Verify parameters
+ if (angle == 0 || speed <= 0 || acceleration <= 0)
+ return;
+
+ // Compute trapezoid parameters for right motor
+ right_trap.init_val = get_output_value(&right_wheel);
+ right_trap.dir = SIGN(angle);
+ right_trap.angle = right_trap.init_val + angle * STRUCT_B / 2.0 / WHEEL_R;
+ right_trap.speed = right_trap.dir * speed * STRUCT_B / 2.0 / WHEEL_R;
+ right_trap.acceleration = right_trap.dir * acceleration * STRUCT_B / 2.0 / WHEEL_R;
+ right_trap.state = TRAPEZOID_STATE_ACC;
+
+ // Compute trapezoid parameters for left motor
+ left_trap.init_val = get_output_value(&left_wheel);
+ left_trap.dir = -right_trap.dir;
+ left_trap.angle = left_trap.init_val - angle * STRUCT_B / 2.0 / WHEEL_R;
+ left_trap.speed = left_trap.dir * speed * STRUCT_B / 2.0 / WHEEL_R;
+ left_trap.acceleration = left_trap.dir * acceleration * STRUCT_B / 2.0 / WHEEL_R;
+ left_trap.state = TRAPEZOID_STATE_ACC;
+
+ // This is the angle during which the robot will accelerate
+ acc_angle = SIGN(angle) * speed * speed / acceleration / 2.0 * STRUCT_B / 2.0 / WHEEL_R;
+
+ // Set accelerations for the trapezoid's first phase and associated callbacks
+ adjust_speed(&right_wheel_speed, right_trap.acceleration);
+ add_callback(&right_wheel, SELECT_THRESHOLD(right_trap.dir), right_trap.init_val + acc_angle, trapezoid_callback);
+ add_callback(&right_wheel_speed, SELECT_THRESHOLD(right_trap.dir), right_trap.speed, trapezoid_callback);
+ adjust_speed(&left_wheel_speed, left_trap.acceleration);
+ add_callback(&left_wheel, SELECT_THRESHOLD(left_trap.dir), left_trap.init_val - acc_angle, trapezoid_callback);
+ add_callback(&left_wheel_speed, SELECT_THRESHOLD(left_trap.dir), left_trap.speed, trapezoid_callback);
+}
+
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.h b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.h
new file mode 100644
index 0000000..0e494e8
--- /dev/null
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.h
@@ -0,0 +1,51 @@
+/*
+ * trajectory_controller.h
+ * -----------------------
+ * Copyright : (c) 2011, Xavier Lagorce <Xav...@cr...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ */
+
+#ifndef __TRAJECTORY_CONTROLLER_H
+#define __TRAJECTORY_CONTROLLER_H
+
+// Robot parameters
+#define WHEEL_R 0.049245
+#define STRUCT_B 0.259
+
+#include "motor_controller.h"
+#include "command_generator.h"
+#include <math.h>
+
+/* Initialize the trajectory controller
+ *
+ * This function will initialize the trajectory controller and the
+ * necessary motor controller.
+ */
+void init_trajectory_controller(void);
+
+/* Indicates if the last planified action is finished
+ *
+ * Returns 1 if the last planified action is finished, 0 if it is not.
+ */
+uint8_t tc_is_finished(void);
+
+/* Move along a line
+ * - distance : distance to move of (in meters), can be positive (forward)
+ * or negative (backward).
+ * - speed : moving speed (in meters per second) should be positive.
+ * - acceleration : in meters per second per second, should be positive.
+ */
+void tc_move(float distance, float speed, float acceleration);
+
+
+/* Turn around the propulsion shaft center
+ * - angle : angle to turn of (in degrees), can be positive (CCW)
+ * or negative (CW).
+ * - speed : turning speed (in degrees per second) should be positive.
+ * - acceleration : in degrees per second per second, should be positive.
+ */
+void tc_turn(float angle, float speed, float acceleration);
+
+#endif /* __TRAJECTORY_CONTROLLER_H */
hooks/post-receive
--
krobot
|
|
From: Jérémie D. <Ba...@us...> - 2011-03-28 13:29:08
|
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 5d9ac6bda6ff0f893d0f659d37010cfd3ea4a080 (commit)
via 93d8beb88ddf5f4fdd7ae09433f793870593fdcd (commit)
from ab463462205439a5320d1635a5812580f24aaa7a (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 5d9ac6bda6ff0f893d0f659d37010cfd3ea4a080
Author: Jérémie Dimino <je...@di...>
Date: Mon Mar 28 15:24:16 2011 +0200
[info] implement trajectory capturing
Allow to capture a trajectory in the viewer using the mouse.
commit 93d8beb88ddf5f4fdd7ae09433f793870593fdcd
Author: Jérémie Dimino <je...@di...>
Date: Mon Mar 28 14:20:07 2011 +0200
[info] add bus.conf to ignored files
-----------------------------------------------------------------------
Changes:
diff --git a/.gitignore b/.gitignore
index 6db98fc..40e4de9 100644
--- a/.gitignore
+++ b/.gitignore
@@ -17,3 +17,4 @@
setup.data
setup.log
_build
+info/control2011/src/driver/bus.conf
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index b66383d..ff73838 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -43,6 +43,7 @@ let utf8 code =
module LCD = struct
type t = {
+ widget : GMisc.drawing_area;
chars : char array array;
mutable line : int;
mutable column : int;
@@ -55,7 +56,8 @@ module LCD = struct
let inter = 4.
let border = 2.
- let create () = {
+ let create widget = {
+ widget;
chars = Array.make_matrix lines columns ' ';
line = 0;
column = 0;
@@ -84,9 +86,9 @@ module LCD = struct
let set_color ctx (r, g, b) =
Cairo.set_source_rgb ctx r g b
- let draw widget lcd =
+ let draw lcd =
let colors = if lcd.backlight then colors_light else colors_dark in
- let { Gtk.width; Gtk.height } = widget#misc#allocation in
+ let { Gtk.width; Gtk.height } = lcd.widget#misc#allocation in
let surface = Cairo.image_surface_create Cairo.FORMAT_ARGB32 width height in
let ctx = Cairo.create surface in
Cairo.select_font_face ctx "Monospace" Cairo.FONT_SLANT_NORMAL Cairo.FONT_WEIGHT_NORMAL;
@@ -116,7 +118,7 @@ module LCD = struct
Cairo.rectangle ctx x y (fw +. border *. 2.0) (fh +. border *. 2.0);
Cairo.fill ctx
end;
- let ctx = Cairo_lablgtk.create widget#misc#window in
+ let ctx = Cairo_lablgtk.create lcd.widget#misc#window in
Cairo.set_source_surface ctx surface 0. 0.;
Cairo.rectangle ctx 0. 0. (float width) (float height);
Cairo.fill ctx;
@@ -170,7 +172,15 @@ module Board = struct
}
type t = {
+ widget : GMisc.drawing_area;
mutable state : state;
+ mutable points : (float * float) list;
+ }
+
+ let create widget = {
+ widget;
+ state = { x = 0.2; y = 1.9; theta = 2. *. atan (-1.) };
+ points = [];
}
let world_height = 2.1
@@ -202,8 +212,14 @@ module Board = struct
let pi = 4. *. atan 1.
- let draw widget board =
- let { Gtk.width; Gtk.height } = widget#misc#allocation in
+ let optimal_size width height =
+ if width /. height >= (world_width +. 0.204) /. (world_height +. 0.204) then
+ ((world_width +. 0.204) /. (world_height +. 0.204) *. height, height)
+ else
+ (width, width /. (world_width +. 0.204) *. (world_height +. 0.204))
+
+ let draw board =
+ let { Gtk.width; Gtk.height } = board.widget#misc#allocation in
let surface = Cairo.image_surface_create Cairo.FORMAT_ARGB32 width height in
let ctx = Cairo.create surface in
let width = float width and height = float height in
@@ -214,12 +230,7 @@ module Board = struct
Cairo.fill ctx;
(* Compute the optimal width and height *)
- let dw, dh =
- if width /. height >= (world_width +. 0.204) /. (world_height +. 0.204) then
- ((world_width +. 0.204) /. (world_height +. 0.204) *. height, height)
- else
- (width, width /. (world_width +. 0.204) *. (world_height +. 0.204))
- in
+ let dw, dh = optimal_size width height in
(* Translation to have the board at the center and scaling to match the window sizes *)
let x0 = (width -. dw) /. 2. and y0 = (height -. dh) /. 2. in
@@ -345,6 +356,8 @@ module Board = struct
Cairo.arc ctx 1.675 0.175 0.05 0. (2. *. pi);
Cairo.fill ctx;
+ Cairo.save ctx;
+
(* Draw the robot *)
Cairo.translate ctx board.state.x board.state.y;
Cairo.rotate ctx board.state.theta;
@@ -361,11 +374,38 @@ module Board = struct
set_color ctx Black;
Cairo.stroke ctx;
- let ctx = Cairo_lablgtk.create widget#misc#window in
+ Cairo.restore ctx;
+
+ (* Draw points. *)
+ Cairo.set_source_rgb ctx 255. 255. 0.;
+ Cairo.move_to ctx board.state.x board.state.y;
+ List.iter (fun (x, y) -> Cairo.line_to ctx x y) board.points;
+ Cairo.stroke ctx;
+
+ let ctx = Cairo_lablgtk.create board.widget#misc#window in
Cairo.set_source_surface ctx surface 0. 0.;
Cairo.rectangle ctx 0. 0. width height;
Cairo.fill ctx;
Cairo.surface_finish surface
+
+ let queue_draw board =
+ GtkBase.Widget.queue_draw board.widget#as_widget
+
+ let add_point board x y =
+ let { Gtk.width; Gtk.height } = board.widget#misc#allocation in
+ let width = float width and height = float height in
+ let dw, dh = optimal_size width height in
+ let scale = dw /. (world_width +. 0.204) in
+ let x0 = (width -. dw) /. 2. and y0 = (height -. dh) /. 2. in
+ let x = (x -. x0) /. scale -. 0.102 and y = world_height -. ((y -. y0) /. scale -. 0.102) in
+ if x >= 0. && x < world_width && y >= 0. && y < world_height then begin
+ board.points <- board.points @ [(x, y)];
+ queue_draw board
+ end
+
+ let clear board =
+ board.points <- [];
+ queue_draw board
end
(* +-----------------------------------------------------------------+
@@ -383,10 +423,27 @@ lwt () =
ignore (ui#window#connect#destroy ~callback:(wakeup wakener));
ui#window#show ();
- let lcd = LCD.create () in
- ignore (ui#lcd#event#connect#expose (fun ev -> LCD.draw ui#lcd lcd; true));
-
- let board = Board.({ state = { x = 0.2; y = 1.9; theta = 2. *. atan (-1.) } }) in
- ignore (ui#scene#event#connect#expose (fun ev -> Board.draw ui#scene board; true));
+ let lcd = LCD.create ui#lcd in
+ ignore (ui#lcd#event#connect#expose (fun ev -> LCD.draw lcd; true));
+
+ let board = Board.create ui#scene in
+ ignore (ui#scene#event#connect#expose (fun ev -> Board.draw board; true));
+ ignore
+ (ui#scene#event#connect#button_press
+ (fun ev ->
+ Board.add_point board (GdkEvent.Button.x ev) (GdkEvent.Button.y ev);
+ true));
+ ignore
+ (ui#scene#event#connect#motion_notify
+ (fun ev ->
+ Board.add_point board (GdkEvent.Motion.x ev) (GdkEvent.Motion.y ev);
+ true));
+
+ ignore
+ (ui#button_clear#event#connect#button_release
+ (fun ev ->
+ if GdkEvent.Button.button ev = 1 then
+ Board.clear board;
+ false));
waiter
diff --git a/info/control2011/src/tools/krobot_viewer_ui.glade b/info/control2011/src/tools/krobot_viewer_ui.glade
index ade135a..f4a7692 100644
--- a/info/control2011/src/tools/krobot_viewer_ui.glade
+++ b/info/control2011/src/tools/krobot_viewer_ui.glade
@@ -153,6 +153,7 @@
<widget class="GtkDrawingArea" id="scene">
<property name="visible">True</property>
<property name="app_paintable">True</property>
+ <property name="events">GDK_BUTTON1_MOTION_MASK | GDK_BUTTON_PRESS_MASK | GDK_STRUCTURE_MASK</property>
<property name="double_buffered">False</property>
</widget>
<packing>
@@ -178,6 +179,30 @@
</packing>
</child>
<child>
+ <widget class="GtkButton" id="button_clear">
+ <property name="label" translatable="yes">Clear trajectory</property>
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="receives_default">True</property>
+ </widget>
+ <packing>
+ <property name="expand">False</property>
+ <property name="position">1</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkButton" id="button_smooth">
+ <property name="label" translatable="yes">Smooth trajectory</property>
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="receives_default">True</property>
+ </widget>
+ <packing>
+ <property name="expand">False</property>
+ <property name="position">2</property>
+ </packing>
+ </child>
+ <child>
<widget class="GtkViewport" id="viewport1">
<property name="visible">True</property>
<property name="resize_mode">queue</property>
@@ -186,7 +211,7 @@
</child>
</widget>
<packing>
- <property name="position">1</property>
+ <property name="position">3</property>
</packing>
</child>
</widget>
hooks/post-receive
--
krobot
|
|
From: Jérémie D. <Ba...@us...> - 2011-03-28 12:18:22
|
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 ab463462205439a5320d1635a5812580f24aaa7a (commit)
from af2f09b9bb1cb78cd68a0085bf7dbc87a4635d7d (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 ab463462205439a5320d1635a5812580f24aaa7a
Author: Jérémie Dimino <je...@di...>
Date: Mon Mar 28 14:17:22 2011 +0200
[info] info/control --> info/control2010
-----------------------------------------------------------------------
Changes:
diff --git a/info/control/.gitignore b/info/control/.gitignore
deleted file mode 100644
index e35d885..0000000
--- a/info/control/.gitignore
+++ /dev/null
@@ -1 +0,0 @@
-_build
diff --git a/info/control/META b/info/control/META
deleted file mode 100644
index 3a4d1c1..0000000
--- a/info/control/META
+++ /dev/null
@@ -1,9 +0,0 @@
-# -*- conf -*-
-name = "krobot"
-version = "0.1"
-description = "Krobot client library"
-requires = "obus"
-archive(byte) = "krobot.cma"
-archive(native) = "krobot.cmxa"
-archive(plugin,byte) = "krobot.cma"
-archive(plugin,native) = "krobot.cmxs"
diff --git a/info/control/Makefile b/info/control/Makefile
deleted file mode 100644
index 5c9de2e..0000000
--- a/info/control/Makefile
+++ /dev/null
@@ -1,46 +0,0 @@
-# Makefile
-# --------
-# Copyright : (c) 2009, Jeremie Dimino <je...@di...>
-# Licence : BSD3
-#
-# This file is a part of [kro]bot.
-
-PREFIX := $(HOME)
-
-OC := ocamlbuild -classic-display
-OF := ocamlfind
-
-.PHONY: all
-all:
- $(OC) all
-
-.PHONY: clean
-clean:
- $(OC) -clean
-
-.PHONY: install
-install:
- $(OF) install krobot META \
- $(wildcard lib-krobot/*.mli) \
- $(wildcard common/*.mli) \
- $(wildcard protocol/*.mli) \
- $(wildcard _build/lib-krobot/*.cmi) \
- $(wildcard _build/common/*.cmi) \
- $(wildcard _build/protocol/*.cmi) \
- $(wildcard _build/lib-krobot/*.cmx) \
- $(wildcard _build/common/*.cmx) \
- $(wildcard _build/protocol/*.cmx) \
- $(wildcard _build/*.cma) \
- $(wildcard _build/*.cmxa) \
- $(wildcard _build/*.cmxs) \
- $(wildcard _build/*.a)
- @/bin/bash install-programs.sh $(PREFIX)
-
-.PHONY: uninstall
-uninstall:
- $(OF) remove krobot
- rm -vf $(PREFIX)/bin/krobot-*
- rm -rvf $(PREFIX)/share/krobot
-
-.PHONY: reinstall
-reinstall: uninstall install
diff --git a/info/control/README b/info/control/README
deleted file mode 100644
index c952455..0000000
--- a/info/control/README
+++ /dev/null
@@ -1,22 +0,0 @@
-Organisation des dossiers:
-
-* "card-tools" contient des outils pour les cartes usb, notamment pour
- flasher les firmwares
-
-* "clients" contient les divers programmes pour monitorer et
- controller le robot
-
-* "common" contient les modules partagés par plusieurs composantes du
- robot
-
-* "driver" contient le driver qui accède au cartes usb
-
-* "lib-krobot" contient la librairie cliente pour utiliser le robot,
- qui se connecte au driver via D-Bus
-
-* "services" contient des services tels que l'arrêt des moteurs en cas
- de colision ou le logging d'un match
-
-* "tools" contient divers outils
-
-* "www" contient une interface web pour le krobot
diff --git a/info/control/_tags b/info/control/_tags
deleted file mode 100644
index 8c8133a..0000000
--- a/info/control/_tags
+++ /dev/null
@@ -1,23 +0,0 @@
-# -*- conf -*-
-
-# Uses the lwt syntax extension for all files:
-<**/*.ml>: syntax(camlp4o), package(lwt.syntax), package(lwt.syntax.log)
-
-# Use the obus syntax extension for all files:
-<**/*.ml>: package(obus.syntax)
-
-# Libraries used by all part of the robot:
-<**/*> and not <**/*.cm{a,xa,xs}>: package(obus), package(lwt.text)
-
-# Only the driver and card tools have access to the hardware. The rest
-# should be compilable even without ocaml-usb and ocaml-serial.
-<{driver,usb-tools}/**/*>: package(usb), package(serial)
-
-# SDL is used to access the sixaxis controller
-<clients/krobot_joystick.*>: package(sdl)
-
-# The python generator needs to read IDL files
-<python/generate.*>: package(obus.idl)
-
-# The simulator uses GTK
-<simulator/*>: package(lablgtk2), package(lablgtk2.glade), package(cairo), package(cairo.lablgtk2), package(lwt.glib)
diff --git a/info/control/clients/commands/krobot_move.ml b/info/control/clients/commands/krobot_move.ml
deleted file mode 100644
index ccec16a..0000000
--- a/info/control/clients/commands/krobot_move.ml
+++ /dev/null
@@ -1,53 +0,0 @@
-(*
- * krobot_move.ml
- * --------------
- * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- *)
-
-open Lwt
-
-lwt () =
- let distance =
- Krobot_arg.float_d
- ~key:"-distance"
- ~doc:"distance to move, in meters"
- ~default:0.1
- ()
- and velocity =
- Krobot_arg.float_d
- ~key:"-velocity"
- ~doc:"velocity during the move"
- ~default:0.4
- ()
- and acceleration =
- Krobot_arg.float_d
- ~key:"-acceleration"
- ~doc:"acceleration during the move"
- ~default:0.8
- ()
- in
- Krobot_arg.parse ();
-
- lwt krobot = Krobot.create () in
- lwt result, distance =
- Krobot_motors.move krobot
- (Lazy.force distance)
- (Lazy.force velocity)
- (Lazy.force acceleration)
- in
- lwt () = Lwt_io.printlf "%f" distance in
- match result with
- | `Success ->
- exit 0
- | `Stopped ->
- lwt () = Lwt_log.warning "movemenet stopped" in
- exit 1
- | `Inhibited ->
- lwt () = Lwt_log.warning "movemenet inhibited" in
- exit 2
- | `Replaced ->
- lwt () = Lwt_log.warning "movemenet replaced" in
- exit 3
diff --git a/info/control/clients/commands/krobot_turn.ml b/info/control/clients/commands/krobot_turn.ml
deleted file mode 100644
index cbd45c2..0000000
--- a/info/control/clients/commands/krobot_turn.ml
+++ /dev/null
@@ -1,53 +0,0 @@
-(*
- * krobot_turn.ml
- * --------------
- * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- *)
-
-open Lwt
-
-lwt () =
- let angle =
- Krobot_arg.float_d
- ~key:"-angle"
- ~doc:"angle to turn, in radiants"
- ~default:0.1
- ()
- and velocity =
- Krobot_arg.float_d
- ~key:"-velocity"
- ~doc:"velocity during the move"
- ~default:0.4
- ()
- and acceleration =
- Krobot_arg.float_d
- ~key:"-acceleration"
- ~doc:"acceleration during the move"
- ~default:0.8
- ()
- in
- Krobot_arg.parse ();
-
- lwt krobot = Krobot.create () in
- lwt result, distance =
- Krobot_motors.turn krobot
- (Lazy.force angle)
- (Lazy.force velocity)
- (Lazy.force acceleration)
- in
- lwt () = Lwt_io.printlf "%f" distance in
- match result with
- | `Success ->
- exit 10
- | `Stopped ->
- lwt () = Lwt_log.warning "movemenet stopped" in
- exit 11
- | `Inhibited ->
- lwt () = Lwt_log.warning "movemenet inhibited" in
- exit 12
- | `Replaced ->
- lwt () = Lwt_log.warning "movemenet replaced" in
- exit 13
diff --git a/info/control/clients/krobot_controller.ml b/info/control/clients/krobot_controller.ml
deleted file mode 100644
index 0c11d6a..0000000
--- a/info/control/clients/krobot_controller.ml
+++ /dev/null
@@ -1,540 +0,0 @@
-(*
- * controller.ml
- * -------------
- * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- *)
-
-open Lwt
-open Lwt_term
-open Lwt_read_line
-
-module TextSet = Set.Make(Text)
-
-(* +-----------------------------------------------------------------+
- | Logging |
- +-----------------------------------------------------------------+ *)
-
-(* Maximum number of lines to keep in logs: *)
-let log_count = 16384
-
-(* Signal holding the current logs: *)
-let logs, set_logs = React.S.create []
-
-let add_date line =
- let buffer = Buffer.create 42 in
- Lwt_log.render ~buffer ~level:Lwt_log.Info ~message:"" ~template:"$(date): " ~section:Lwt_log.Section.main;
- text (Buffer.contents buffer) :: line
-
-(* Add a list of lines to logs *)
-let log_add_lines lines =
- let rec truncate n = function
- | [] ->
- []
- | line :: rest ->
- if n = log_count then
- []
- else
- line :: truncate (n + 1) rest
- in
- set_logs (truncate 0 (List.rev_map add_date lines @ (React.S.value logs)))
-
-let log_add_line line =
- log_add_lines [line]
-
-(* Redirect stderr to logs *)
-let redirect_stderr () =
- let rec copy_logs ic =
- lwt line = Lwt_io.read_line ic in
- log_add_line [text line];
- copy_logs ic
- in
- let fdr, fdw = Unix.pipe () in
- Unix.dup2 fdw Unix.stderr;
- Unix.close fdw;
- ignore (copy_logs (Lwt_io.of_unix_fd ~mode:Lwt_io.input fdr))
-
-(* Make the default logger to logs into the log buffer *)
-let init_logger () =
- Lwt_log.default :=
- Lwt_log.make
- ~output:(fun section level lines ->
- log_add_lines
- (List.map
- (fun line ->
- if level >= Lwt_log.Warning then
- (* Colorize error in red: *)
- [fg lred; text line]
- else
- [text line])
- lines);
- return ())
- ~close:return
-
-(* +-----------------------------------------------------------------+
- | Read-line |
- +-----------------------------------------------------------------+ *)
-
-let engine_state, set_engine_state = React.S.create (Engine.init [])
-
-let history_file_name =
- Filename.concat (try Unix.getenv "HOME" with _ -> "") ".krobot-controller-history"
-
-let save_history history =
- Lwt_read_line.save_history history_file_name history
-
-let rec loop interpreter completion history =
- lwt key = read_key () in
- if key = key_escape then
- save_history history
- else
- match Command.of_key key with
- | Command.Accept_line ->
- let line = Text.strip (Engine.all_input (React.S.value engine_state)) in
- if line = "exit" then
- save_history history
- else if line <> "" then begin
- let history = Lwt_read_line.add_entry line history in
- set_engine_state (Engine.init history);
- lwt () = Lwt_log.notice line in
- ignore (Krobot_script.exec ~interpreter ~logger:(fun line -> log_add_line line; return ()) ~command:line);
- loop interpreter completion history
- end else
- loop interpreter completion history
- | Command.Complete ->
- let engine_state = Engine.reset (React.S.value engine_state) in
- let comp = React.S.value completion in
- set_engine_state { engine_state with Engine.mode = Engine.Edition comp.comp_state };
- loop interpreter completion history
- | command ->
- set_engine_state (Engine.update (React.S.value engine_state) command ());
- loop interpreter completion history
-
-(* +-----------------------------------------------------------------+
- | Service monitoring |
- +-----------------------------------------------------------------+ *)
-
-let services, set_services = React.S.create []
-let set_services l = set_services (List.sort Pervasives.compare l)
-
-let check_services bus =
- lwt l = OBus_bus.list_names bus in
- set_services (List.fold_left (fun acc name ->
- if Text.starts_with name "fr.krobot." then
- String.sub name 10 (String.length name - 10) :: acc
- else
- acc) [] l);
- return ()
-
-(* +-----------------------------------------------------------------+
- | State of all sensors/properties of the robot |
- +-----------------------------------------------------------------+ *)
-
-type state = {
- box : Lwt_read_line.Terminal.box;
- services : string list;
- logs : Lwt_term.styled_text list;
- size : Lwt_term.size;
- engine_state : Lwt_read_line.Engine.state;
- color : [ `Yellow | `Blue ] option;
- infrareds : int array option;
- logic_sensors : bool array option;
- range_finders : int array option;
- card_interface : [ `Up | `Down ] option;
- card_sensors : [ `Up | `Down ] option;
- card_motors : [ `Up | `Down ] option;
- card_monitoring : [ `Up | `Down ] option;
- card_rx64 : [ `Up | `Down ] option;
- inhibited_forward : bool option;
- inhibited_backward : bool option;
-}
-
-(* +-----------------------------------------------------------------+
- | Drawing |
- +-----------------------------------------------------------------+ *)
-
-let current_points = ref [||]
-
-(* Draw the whole screen *)
-let draw state =
- let size = state.size in
- let screen = Zone.make ~width:size.columns ~height:size.lines in
- let points = Zone.points screen in
-
- let line_color = lblue in
- let line = { blank with style = { blank.style with foreground = line_color } } in
- let name_color = lwhite in
-
- (* ===== Borders ===== *)
-
- for i = 1 to size.columns - 2 do
- points.(0).(i) <- { line with char = "─" };
- points.(size.lines - 1).(i) <- { line with char = "─" }
- done;
- for i = 1 to size.lines - 2 do
- points.(i).(0) <- { line with char = "│" };
- points.(i).(size.columns - 1) <- { line with char = "│" }
- done;
- points.(0).(0) <- { line with char = "┌" };
- points.(size.lines - 1).(0) <- { line with char = "└" };
- points.(size.lines - 1).(size.columns - 1) <- { line with char = "┘" };
- points.(0).(size.columns - 1) <- { line with char = "┐" };
-
- (* ===== Status ===== *)
-
- Draw.textc screen 1 0 [fg line_color; text "─[ ";
- fg name_color; text "Range finders";
- fg line_color; text " ]─┬─[ ";
- fg name_color; text "Logic Sensors";
- fg line_color; text " ]─┬─[ ";
- fg name_color; text "Services";
- fg line_color; text " ]─┬─[ ";
- fg name_color; text "Cards";
- fg line_color; text " ]─┬─[ ";
- fg name_color; text "Status";
- fg line_color; text " ]─"];
- points.(9).(0) <- { line with char = "├" };
- points.(9).(size.columns - 1) <- { line with char = "┤" };
- for i = 1 to size.columns - 2 do
- points.(9).(i) <- { line with char = "─" }
- done;
- for i = 1 to 8 do
- points.(i).(20) <- { line with char = "│" };
- points.(i).(40) <- { line with char = "│" };
- points.(i).(55) <- { line with char = "│" };
- points.(i).(67) <- { line with char = "│" }
- done;
- Draw.textc screen 1 9 [fg line_color; text "───────────────────┴───────────────────┴──────────────┴───────────┴"];
-
- let zone = Zone.inner screen in
-
- begin
- match state.range_finders with
- | Some range_finders ->
- for i = 0 to 7 do
- Draw.textc zone 0 i [textf "%d : %d" i range_finders.(i)]
- done
- | None ->
- for i = 0 to 7 do
- Draw.textc zone 0 i [fg red; text "unavailable"]
- done
- end;
-
- begin
- match state.logic_sensors with
- | Some logic_sensors ->
- for i = 0 to 7 do
- let j = i * 2 in
- Draw.textf zone 20 i "%02d : %s %02d : %s"
- (j + 0) (if logic_sensors.(j + 0) then "O" else ".")
- (j + 1) (if logic_sensors.(j + 1) then "O" else ".")
- done
- | None ->
- for i = 0 to 7 do
- Draw.textc zone 20 i [fg red; text "unavailable"]
- done
- end;
-
- let zone' = Zone.sub ~zone ~x:40 ~y:0 ~width:14 ~height:8 in
- let rec loop y = function
- | [] ->
- ()
- | name :: rest ->
- Draw.text ~zone:zone' ~x:0 ~y ~text:name;
- loop (y + 1) rest
- in
- loop 0 state.services;
-
- let x = 55 in
- let text_of_state name = function
- | Some `Down -> [fg lred; text name]
- | Some `Up -> [text name]
- | None -> [fg red; text "unavailable"]
- in
- Draw.textc zone x 0 (text_of_state "interface" state.card_interface);
- Draw.textc zone x 1 (text_of_state "sensor" state.card_sensors);
- Draw.textc zone x 2 (text_of_state "motor" state.card_motors);
- Draw.textc zone x 3 (text_of_state "monitoring" state.card_monitoring);
- Draw.textc zone x 4 (text_of_state "rx64" state.card_rx64);
-
- let x = x + 12 in
- begin
- match state.color with
- | Some color ->
- Draw.textc zone x 0 [text "color: "; text (match color with
- | `Yellow -> "yellow"
- | `Blue -> "blue")]
- | None ->
- Draw.textc zone x 0 [fg red; text "unavailable"]
- end;
- let text_of_motor_state mode = function
- | Some true ->
- [text mode; fg lyellow; text "inhibited"]
- | Some false ->
- [text mode; text "OK"]
- | None ->
- [fg red; text "unavailable"]
- in
- Draw.textc zone x 1 (text_of_motor_state "move forward: " state.inhibited_forward);
- Draw.textc zone x 2 (text_of_motor_state "move backward: " state.inhibited_backward);
-
- begin
- match state.infrareds with
- | Some infrareds ->
- for i = 0 to 3 do
- Draw.textc zone x (i + 3) [textf "infrared %d: %d" i infrareds.(i)]
- done
- | None ->
- for i = 3 to 6 do
- Draw.textc zone x i [fg red; text "unavailable"]
- done
- end;
-
- (* ===== History ===== *)
-
- let zone = Zone.sub ~zone:screen ~x:1 ~y:10 ~width:(Zone.width screen - 2) ~height:(Zone.height screen - 15) in
- let rec loop y = function
- | [] ->
- ()
- | line :: rest ->
- if y < 0 then
- ()
- else begin
- Draw.textc zone 0 y line;
- loop (y - 1) rest
- end
- in
- loop (Zone.height zone - 1) state.logs;
-
- (* ===== Read-line ===== *)
-
- points.(size.lines - 3).(0) <- { line with char = "├" };
- points.(size.lines - 3).(size.columns - 1) <- { line with char = "┤" };
- points.(size.lines - 5).(0) <- { line with char = "├" };
- points.(size.lines - 5).(size.columns - 1) <- { line with char = "┤" };
- for i = 1 to size.columns - 2 do
- points.(size.lines - 5).(i) <- { line with char = "─" };
- points.(size.lines - 3).(i) <- { line with char = "─" }
- done;
-
- let zone = Zone.sub ~zone:screen ~x:1 ~y:(size.lines - 4) ~width:(size.columns - 2) ~height:1 in
- let engine_state = state.engine_state in
- let cursor_position =
- match engine_state.Engine.mode with
- | Engine.Edition(before, after) ->
- let len = Text.length before in
- Draw.textc zone 0 0 [Text before; Text after];
- len
- | Engine.Selection state ->
- let a = min state.Engine.sel_cursor state.Engine.sel_mark
- and b = max state.Engine.sel_cursor state.Engine.sel_mark in
- let part_before = Text.chunk (Text.pointer_l state.Engine.sel_text) a
- and part_selected = Text.chunk a b
- and part_after = Text.chunk (Text.pointer_r state.Engine.sel_text) b in
- Draw.textc zone 0 0 [Text part_before; Underlined; Text part_selected; Reset; Text part_after];
- if state.Engine.sel_cursor < state.Engine.sel_mark then
- Text.length part_before
- else
- Text.length part_before + Text.length part_selected
- | Engine.Search state ->
- let len = Text.length state.Engine.search_word in
- Draw.text zone 0 0 (Printf.sprintf "(reverse-i-search)'%s'" state.Engine.search_word);
- begin match state.Engine.search_history with
- | [] ->
- 19 + len
- | phrase :: _ ->
- let ptr_start = match Text.find phrase state.Engine.search_word with
- | Some ptr ->
- ptr
- | None ->
- assert false
- in
- let ptr_end = Text.move len ptr_start in
- let before = Text.chunk (Text.pointer_l phrase) ptr_start
- and selected = Text.chunk ptr_start ptr_end
- and after = Text.chunk ptr_end (Text.pointer_r phrase) in
- Draw.textc zone (20 + len) 0 [
- Text ": ";
- Text before;
- Underlined;
- Text selected;
- Reset;
- Text after;
- ];
- 19 + len
- end
- in
- Draw.map zone cursor_position 0 (fun point -> { point with style = { point.style with inverse = true } });
-
- let zone = Zone.sub ~zone:screen ~x:1 ~y:(size.lines - 3) ~width:(size.columns - 2) ~height:3 in
- begin
- match state.box with
- | Terminal.Box_none | Terminal.Box_empty ->
- ()
- | Terminal.Box_message msg ->
- Draw.text zone 0 1 msg
- | Terminal.Box_words(words, _) ->
- ignore (TextSet.fold
- (fun word i ->
- let len = Text.length word in
- Draw.text zone i 1 word;
- let i = i + len in
- Draw.textc zone i 0 [fg line_color; text "┬"];
- Draw.textc zone i 1 [fg line_color; text "│"];
- Draw.textc zone i 2 [fg line_color; text "┴"];
- i + 1)
- words 0)
- end;
-
- lwt () = Lwt_term.render_update !current_points (Zone.points screen) in
- current_points := Zone.points screen;
- return ()
-
-(* +-----------------------------------------------------------------+
- | Entry point |
- +-----------------------------------------------------------------+ *)
-
-lwt () =
- Krobot_arg.parse ();
-
- lwt () = Lwt_log.notice "connecting to the krobot bus..." in
- lwt krobot = Krobot.create () and bus = Krobot_dbus.open_bus () in
-
- lwt interpreter = Krobot_script.make krobot in
- let engine_mode = React.S.map (fun state -> state.Engine.mode) engine_state in
- let completion =
- Krobot_script.completion
- interpreter
- (React.S.fmap
- (function
- | Engine.Edition state -> Some state
- | _ -> None)
- ("", "")
- engine_mode)
- in
- let box =
- React.S.l2
- (fun mode comp ->
- match mode with
- | Engine.Edition _ ->
- Terminal.Box_words(comp.comp_words, 0)
- | Engine.Selection _ ->
- Terminal.Box_message "<selection>"
- | Engine.Search _ ->
- Terminal.Box_message "<backward search>")
- engine_mode completion
- in
-
- (* Put the terminal into drawing mode: *)
- lwt () = Lwt_term.enter_drawing_mode () in
- lwt () = Lwt_term.hide_cursor () in
-
- init_logger ();
- redirect_stderr ();
-
- (* Dump all logs to stdout on abnormal exit: *)
- let node =
- Lwt_sequence.add_l
- (fun () ->
- lwt () = Lwt_term.leave_drawing_mode () in
- Lwt_list.iter_s printlc (List.rev (React.S.value logs)))
- Lwt_main.exit_hooks
- in
-
- (* Service monitoring *)
- lwt () = Lwt_event.always_notify_p (fun _ -> check_services bus) =|< OBus_signal.connect (OBus_bus.name_owner_changed bus) in
-
- (* State of robot *)
- let monitor name dummy make =
- React.S.switch
- (React.S.const dummy)
- (Lwt_event.map_s
- (function
- | "" ->
- return (React.S.const dummy)
- | _ ->
- try_lwt
- make ()
- with exn ->
- lwt () = Lwt_log.error_f ~exn "signal maker failed with" in
- raise_lwt exn)
- (Lwt_signal.delay (OBus_resolver.make bus name)))
- in
- let state =
- React.S.l4
- (fun
- (box, services, logs, engine_state, size)
- (card_interface, card_sensors, card_motors, card_monitoring, card_rx64)
- (color, infrareds, logic_sensors, range_finders)
- (inhibited_forward, inhibited_backward) ->
- {
- box = box;
- logs = logs;
- size = size;
- engine_state = engine_state;
- services = services;
- color = color;
- infrareds = infrareds;
- logic_sensors = logic_sensors;
- range_finders = range_finders;
- card_interface = card_interface;
- card_sensors = card_sensors;
- card_motors = card_motors;
- card_monitoring = card_monitoring;
- card_rx64 = card_rx64;
- inhibited_forward = inhibited_forward;
- inhibited_backward = inhibited_backward;
- })
- (React.S.l5 (fun a b c d e -> (a, b, c, d, e)) box services logs engine_state size)
- (monitor "fr.krobot.Driver" (None, None, None, None, None)
- (fun () ->
- let make card = OBus_property.monitor (Krobot_driver.Card.state (card krobot)) in
- lwt interface = make Krobot_driver.card_interface
- and sensors = make Krobot_driver.card_sensors
- and motors = make Krobot_driver.card_motors
- and monitoring = make Krobot_driver.card_monitoring
- and rx64 = make Krobot_driver.card_rx64 in
- return (React.S.l5
- (fun a b c d e -> (Some a, Some b, Some c, Some d, Some e))
- interface sensors motors monitoring rx64)))
- (monitor "fr.krobot.Service.Sensors" (None, None, None, None)
- (fun () ->
- lwt color = OBus_property.monitor (Krobot_sensors.color krobot)
- and infrareds = OBus_property.monitor (Krobot_sensors.infrareds krobot)
- and logic_sensors = OBus_property.monitor (Krobot_sensors.logic_sensors krobot)
- and range_finders = OBus_property.monitor (Krobot_sensors.range_finders krobot) in
- return (React.S.l4 (fun a b c d -> (Some a, Some b, Some c, Some d)) color infrareds logic_sensors range_finders)))
- (monitor "fr.krobot.Service.Motors" (None, None)
- (fun () ->
- lwt inhibited_forward = OBus_property.monitor (Krobot_motors.inhibited_forward krobot)
- and inhibited_backward = OBus_property.monitor (Krobot_motors.inhibited_backward krobot) in
- return (React.S.l2 (fun a b -> (Some a, Some b)) inhibited_forward inhibited_backward)))
- in
-
- (* Initialize the list of running services: *)
- ignore (check_services bus);
-
- (* Redraw the screen when the state changes *)
- Lwt_signal.always_notify_s draw state;
-
- (* Display log comming from daemons *)
- let daemons = OBus_proxy.make (OBus_peer.anonymous bus) ["fr"; "krobot"; "Daemon"] in
- lwt () =
- Lwt_event.always_notify
- (fun msg -> log_add_lines (List.map (fun line -> [text line]) (Text.split ~sep:"\n" msg)))
- =|< OBus_signal.connect (OBus_signal.make Krobot_dbus_daemon.Fr_krobot_Daemon.s_log daemons)
- in
-
- lwt history = Lwt_read_line.load_history history_file_name in
- set_engine_state (Engine.init history);
-
- (* User input loop *)
- lwt () = Lwt_term.with_raw_mode (fun () -> loop interpreter completion history) in
-
- (* Normal exit, do not dump logs on stdout: *)
- Lwt_sequence.remove node;
-
- (* Leave drawing mode *)
- Lwt_term.leave_drawing_mode ()
diff --git a/info/control/clients/krobot_jack.ml b/info/control/clients/krobot_jack.ml
deleted file mode 100644
index 7fb8d29..0000000
--- a/info/control/clients/krobot_jack.ml
+++ /dev/null
@@ -1,20 +0,0 @@
-(*
- * krobot_jack.ml
- * --------------
- * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- *)
-
-(* Wait for the jack to be removed *)
-
-open Lwt
-
-lwt () =
- Krobot_arg.parse ();
- lwt krobot = Krobot.create () in
- let waiter, wakener = wait () in
- lwt logic_sensors = OBus_property.monitor (Krobot_sensors.logic_sensors krobot) in
- Lwt_signal.always_notify (fun ls -> if not ls.(1) then wakeup wakener ()) logic_sensors;
- waiter
diff --git a/info/control/clients/krobot_joystick.ml b/info/control/clients/krobot_joystick.ml
deleted file mode 100644
index 093d786..0000000
--- a/info/control/clients/krobot_joystick.ml
+++ /dev/null
@@ -1,276 +0,0 @@
-(*
- * 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 =
- | JoyRAxisV of float
- | JoyLAxisV of float
- | JoyRAxisH of float
- | JoyLAxisH of float
- | JoyButtonPressed of button
- | JoyButtonReleased of button
- | KeyPressed of Sdlkey.t
- | KeyReleased of Sdlkey.t
-
-(* +-----------------------------------------------------------------+
- | int --> button |
- +-----------------------------------------------------------------+ *)
-
-let raxis_v = 3
-let raxis_h = 2
-let laxis_v = 1
-let laxis_h = 0
-
-let axis_min = -32768.0
-let axis_max = 32767.0
-
-let button_of_num = function
- | 14 -> Some ButtonCross
- | 15 -> Some ButtonSquare
- | 12 -> Some ButtonTriangle
- | 13 -> Some ButtonCircle
- | 6 -> Some ButtonDown
- | 7 -> Some ButtonLeft
- | 4 -> Some ButtonUp
- | 5 -> Some ButtonRight
- | 0 -> Some ButtonSelect
- | 3 -> Some ButtonStart
- | 11 -> Some ButtonR1
- | 9 -> Some ButtonR2
- | 10 -> Some ButtonL1
- | 8 -> Some ButtonL2
- | 16 -> Some ButtonPS3
- | 1 -> Some ButtonLAxis
- | 2 -> Some ButtonRAxis
- | n -> None
-
-(* +-----------------------------------------------------------------+
- | SDL events (executed in a child process) |
- +-----------------------------------------------------------------+ *)
-
-let child_loop pipe joy =
- let axis_state = Array.make (num_axes joy) 0.0 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 } ->
- let value = 0.1 -. ((float_of_int value -. axis_min) *. 0.2 /. (axis_max -. axis_min)) in
- if value <> axis_state.(axis) then begin
- axis_state.(axis) <- 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
- ()
- end
- | JOYBUTTONUP { jbe_button = button } -> begin
- match button_of_num button with
- | Some button ->
- send (JoyButtonReleased button)
- | None ->
- ()
- end
- | JOYBUTTONDOWN { jbe_button = button } -> begin
- match button_of_num button with
- | Some button ->
- send (JoyButtonPressed button)
- | None ->
- ()
- end
- | _ ->
- ()
- done
-
-(* +-----------------------------------------------------------------+
- | Handling events (in the parent process) |
- +-----------------------------------------------------------------+ *)
-
-let axis_coef = 6.0
-let axis_coef_turn = 4.0
-let duration = 0.2
-
-lwt record_oc =
- if Array.length Sys.argv >= 2 && Sys.argv.(1) = "record" then
- Lwt_io.open_file ~mode:Lwt_io.output "krobot.record"
- else
- return Lwt_io.null
-
-let date_last_command = ref (Unix.gettimeofday ())
-
-let robot_call record f =
- let date = Unix.gettimeofday () in
- let delta = date -. !date_last_command in
- date_last_command := date;
- lwt () = Lwt_io.fprintf record_oc "%Ld\n%s\n" (Int64.bits_of_float delta) record in
- try_lwt
- f ()
- with Failure msg ->
- lwt () = Lwt_log.error_f "action << %s >> failed with: %s" record msg in
- return ()
-
-let rec set_velocities krobot velocity_l velocity_r =
- lwt () = Lwt_log.info_f "set-velocities: left=%f right=%f" velocity_l velocity_r in
- lwt () =
- robot_call
- (Printf.sprintf
- "set-velocities velocity-left=%f velocity-right=%f duration=%f"
- velocity_l velocity_r duration)
- (fun () ->
- Krobot_motors.set_velocities krobot ~velocity_l ~velocity_r ~duration)
- in
- if velocity_l = 0.0 && velocity_r = 0.0 then
- return ()
- else begin
- lwt () = Lwt_unix.sleep (duration /. 2.) in
- set_velocities krobot velocity_l velocity_r
- end
-
-let parent_loop krobot pipe =
- let stop = ref false in
- let thread = ref (return ()) in
- let raxis_h = ref 0.0
- and raxis_v = ref 0.0
- and laxis_h = ref 0.0
- and laxis_v = ref 0.0 in
- let set_velocities () =
- cancel !thread;
- if not !stop then
- thread :=
- set_velocities krobot
- (!laxis_v *. axis_coef -. !raxis_h *. axis_coef_turn)
- (!laxis_v *. axis_coef +. !raxis_h *. axis_coef_turn)
- in
- let enable_claws = lazy(robot_call "claws.enable" (fun () -> Krobot_claws.enable krobot)) in
- let rec loop () =
- Lwt_io.read_value pipe >>= function
- | KeyPressed KEY_ESCAPE ->
- return ()
- | JoyLAxisV n ->
- laxis_v := n;
- set_velocities ();
- loop ()
- | JoyLAxisH n ->
- laxis_h := n;
- set_velocities ();
- loop ()
- | JoyRAxisV n ->
- raxis_v := n;
- set_velocities ();
- loop ()
- | JoyRAxisH n ->
- raxis_h := n;
- set_velocities ();
- loop ()
- | JoyButtonPressed ButtonSquare ->
- stop := true;
- cancel !thread;
- lwt () = robot_call "stop-motors mode=abrupt" (fun () -> Krobot_motors.stop krobot ~mode:`Abrupt) in
- loop ()
- | JoyButtonReleased ButtonSquare ->
- stop := false;
- loop ()
- | JoyButtonPressed ButtonCross ->
- lwt () = Lazy.force enable_claws in
- lwt () = robot_call "claws.take" (fun () -> Krobot_claws.take krobot) in
- loop ()
- | JoyButtonPressed ButtonCircle ->
- lwt () = Lazy.force enable_claws in
- lwt () = robot_call "claws.open" (fun () -> Krobot_claws.open_ krobot) in
- loop ()
- | JoyButtonPressed ButtonTriangle ->
- lwt () = Lazy.force enable_claws in
- lwt () = robot_call "claws.close" (fun () -> Krobot_claws.close krobot) in
- loop ()
- | JoyButtonPressed ButtonR1 ->
- lwt () = robot_call "grip.up" (fun () -> Krobot_grip.up krobot >> return ()) in
- loop ()
- | JoyButtonPressed ButtonR2 ->
- lwt () = robot_call "grip.down" (fun () -> Krobot_grip.down krobot) in
- loop ()
- | JoyButtonPressed ButtonL1 ->
- lwt () = robot_call "grip.close" (fun () -> Krobot_grip.close krobot) in
- loop ()
- | JoyButtonPressed ButtonL2 ->
- lwt () = robot_call "grip.release" (fun () -> Krobot_grip.release krobot) in
- loop ()
- | _ ->
- loop ()
- in
- loop ()
-
-(* +-----------------------------------------------------------------+
- | Entry-point |
- +-----------------------------------------------------------------+ *)
-
-let () =
- Krobot_arg.parse ();
- let fd_r, fd_w = Unix.pipe () in
- match Unix.fork () with
- | 0 ->
- Unix.close fd_r;
- Sdl.init [`JOYSTICK;`VIDEO];
- Sdljoystick.set_event_state true;
- let joy =
- try
- open_joystick 0
- with exn ->
- Printf.eprintf "cannot open joystick: %s\n%!" (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
- lwt () = Lwt_log.notice "ready to process event" in
- parent_loop krobot (Lwt_io.of_unix_fd ~mode:Lwt_io.input fd_r)
- end
diff --git a/info/control/clients/krobot_record_infrared.ml b/info/control/clients/krobot_record_infrared.ml
deleted file mode 100644
index 8301ed2..0000000
--- a/info/control/clients/krobot_record_infrared.ml
+++ /dev/null
@@ -1,45 +0,0 @@
-(*
- * krobot_record_infrared.ml
- * -------------------------
- * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- *)
-
-(* Record measures of the infrared sensors at each centimerters *)
-
-open Lwt
-open Lwt_io
-
-let step = 0.001
-
-let measure_infrared krobot infrareds =
- let rec loop acc = function
- | 0 ->
- return (acc / 20)
- | n ->
- let measure = (React.S.value infrareds).(0) in
- let acc = acc + measure in
- lwt () = Lwt_unix.sleep 0.05 in
- loop acc (n - 1)
- in
- loop 0 20
-
-lwt () =
- Krobot_arg.parse ();
-
- lwt () = printl "type Ctrl+C to stop" in
- lwt krobot = Krobot.create () in
- lwt infrareds = OBus_property.monitor (Krobot_sensors.infrareds krobot) in
- lwt oc = Lwt_io.open_file ~mode:Lwt_io.output "infrareds-record" in
- let rec loop dist =
- lwt () = Lwt_unix.sleep 1.0 in
- lwt measure = measure_infrared krobot infrareds in
- lwt () = printlf "at distance %fm: %d" dist measure in
- lwt () = fprintlf oc "%f %d" dist measure in
- lwt () = Lwt_io.flush oc in
- lwt _ = Krobot_motors.move krobot ~distance:(-.step) ~velocity:0.2 ~acceleration:0.4 in
- loop (dist +. step)
- in
- loop 0.0
diff --git a/info/control/clients/krobot_recorder.ml b/info/control/clients/krobot_recorder.ml
deleted file mode 100644
index 034976c..0000000
--- a/info/control/clients/krobot_recorder.ml
+++ /dev/null
@@ -1,35 +0,0 @@
-(*
- * krobot_recorder.ml
- * ------------------
- * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- *)
-
-(* Records all messages that goes through the krobot bus *)
-
-open Lwt
-
-let count = ref 0
-
-let filter message =
- incr count;
- ignore (
- lwt () = Lwt_io.eprintf "\r%d messages received" !count in
- let str, _ = OBus_wire.string_of_message message in
- Lwt_io.print str
- );
- Some message
-
-lwt () =
- Krobot_arg.parse ();
- lwt bus = Krobot_dbus.open_bus () in
- lwt () = Lwt_io.eprint "0 messages received" in
- let _ = Lwt_sequence.add_r filter (OBus_connection.incoming_filters bus) in
- lwt () =
- Lwt_list.iter_p
- (fun typ -> OBus_bus.add_match bus (OBus_match.rule ~typ ()))
- [`Method_call; `Method_return; `Error; `Signal]
- in
- fst (wait ())
diff --git a/info/control/clients/krobot_script.ml b/info/control/clients/krobot_script.ml
deleted file mode 100644
index 8f9f839..0000000
--- a/info/control/clients/krobot_script.ml
+++ /dev/null
@@ -1,350 +0,0 @@
-(*
- * script.ml
- * ---------
- * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- *)
-
-open Lwt
-open Lwt_term
-open OBus_value
-open OBus_introspect_ext
-
-let section = Lwt_log.Section.make "script"
-
-module Text_set = Set.Make(Text)
-
-(* +-----------------------------------------------------------------+
- | Types |
- +-----------------------------------------------------------------+ *)
-
-type logger = Lwt_term.styled_text -> unit Lwt.t
-
-type command = {
- cmd_path : string list;
- (* The name of the command with its path *)
-
- cmd_iargs : (string * single * string list) list;
- (* List of input arguments, with their types and constants they accept *)
-
- cmd_oargs : (string * single * string list) list;
- (* List of output arguments, with their types and constants they accept *)
-
- cmd_annotations : (string * string) list;
- (* List of annotations of the method *)
-
- cmd_proxy : OBus_proxy.t;
- (* The proxy on which the command should be executed *)
-
- cmd_method : (V.sequence, V.sequence) OBus_member.Method.t;
- (* The corresponding D-Bus method *)
-}
-
-module String_map = Map.Make(String)
-
-type interpreter = {
- itp_krobot : Krobot.t;
- itp_commands : command String_map.t React.signal;
- itp_set_commands : command String_map.t -> unit;
- mutable itp_monitor_names : unit Lwt.t React.event;
-}
-
-(* +-----------------------------------------------------------------+
- | D-Bus extended introspection --> commands |
- +-----------------------------------------------------------------+ *)
-
-(* Convert a D-Bus name into a command name *)
-let convert_name name =
- Text.concat "."
- (List.map
- (fun elt ->
- Text.concat "-"
- (List.map
- String.lowercase
- (List.flatten
- (List.map
- OBus_name.split
- (Text.split ~sep:"_" elt)))))
- (Text.split ~sep:"." name))
-
-let convert_arguments args =
- let make_constants = function
- | Basic(Enum(typ, l)) -> List.map snd l
- | _ -> []
- in
- let rec loop i = function
- | [] ->
- []
- | (Some name, typ) :: rest ->
- (convert_name name, typ, make_constants typ) :: loop (i + 1) rest
- | (None, typ) :: rest ->
- ("_" ^ string_of_int i, typ, make_constants typ) :: loop (i + 1) rest
- in
- loop 0 args
-
-let add_commands_of_interface proxy path (iface_name, members, env, annotations) acc =
- let rec loop acc = function
- | [] ->
- acc
- | Method(name, isig, osig, annotations) :: rest ->
- let isig = List.map (fun (name, term) -> (name, resolve env term)) isig
- and osig = List.map (fun (name, term) -> (name, resolve env term)) osig in
- let path = path @ [convert_name name] in
- loop
- (String_map.add (Text.concat "." path)
- { cmd_path = path;
- cmd_iargs = convert_arguments isig;
- cmd_oargs = convert_arguments osig;
- cmd_annotations = annotations;
- cmd_proxy = proxy;
- cmd_method = (
- OBus_member.Method.make
- ~interface:iface_name
- ~member:name
- ~i_args:(arguments
- ~arg_types:(C.dyn_sequence (strip_sequence (List.map snd isig)))
- ~arg_names:(List.map fst isig))
- ~o_args:(arguments
- ~arg_types:(C.dyn_sequence (strip_sequence (List.map snd osig)))
- ~arg_names:(List.map fst osig))
- ~annotations
- )} acc)
- rest
- | _ :: rest ->
- loop acc rest
- in
- loop acc members
-
-(* +-----------------------------------------------------------------+
- | Service monitoring |
- +-----------------------------------------------------------------+ *)
-
-let collect interpreter name path prefix =
- try_lwt
- let peer = OBus_peer.make (Krobot.to_bus interpreter.itp_krobot) name in
- lwt _, children = OBus_proxy.introspect (OBus_proxy.make peer prefix) in
- lwt commands =
- Lwt_list.map_p
- (fun child ->
- let proxy = OBus_proxy.make peer (prefix @ [child]) in
- lwt interfaces, _ = OBus_proxy.introspect proxy in
- let path = path @ [convert_name child] in
- return (proxy, path, interfaces))
- children
- in
- interpreter.itp_set_commands
- (List.fold_left
- (fun acc (proxy, path, interfaces) ->
- List.fold_left
- (fun acc ((iface_name, _, _) as interface) ->
- if Text.starts_with iface_name "fr.krobot." then
- add_commands_of_interface proxy path (decode interface) acc
- else
- acc)
- acc interfaces)
- (React.S.value interpreter.itp_commands) commands);
- return ()
- with exn ->
- Lwt_log.error_f ~section ~exn "failed to collect objects from %S" name
-
-let add_service interpreter name =
- match Text.split ~sep:"." name with
- | ["fr"; "krobot"; "Service"; _] ->
- collect interpreter name [] ["fr"; "krobot"; "Services"]
- | ["fr"; "krobot"; "Driver"] ->
- lwt () = collect interpreter name ["unsafe"] ["fr"; "krobot"; "Devices"]
- and () = collect interpreter name ["cards"] ["fr"; "krobot"; "Cards"] in
- return ()
- | _ ->
- return ()
-
-let monitor_names interpreter (name, old_owner, new_owner) =
- interpreter.itp_set_commands
- (String_map.fold
- (fun cmd_name command acc ->
- if OBus_proxy.name command.cmd_proxy = name then
- acc
- else
- String_map.add cmd_name command acc)
- (React.S.value interpreter.itp_commands)
- String_map.empty);
- match new_owner with
- | "" ->
- return ()
- | _ ->
- add_service interpreter name
-
-(* +-----------------------------------------------------------------+
- | Interpreter creation |
- +-----------------------------------------------------------------+ *)
-
-let command_equal cmd1 cmd2 =
- cmd1.cmd_path = cmd2.cmd_path && cmd1.cmd_iargs = cmd2.cmd_iargs
-
-let make krobot =
- let commands, set_commands = React.S.create ~eq:(String_map.equal command_equal) String_map.empty in
- let rec interpreter = {
- itp_krobot = krobot;
- itp_commands = commands;
- itp_set_commands = set_commands;
- itp_monitor_names = React.E.never;
- } in
- lwt event = OBus_signal.connect (OBus_bus.name_owner_changed (Krobot.to_bus krobot)) in
- interpreter.itp_monitor_names <- React.E.map (monitor_names interpreter) event;
- lwt names = OBus_bus.list_names (Krobot.to_bus krobot) in
- lwt () = Lwt_list.iter_p (add_service interpreter) names in
- return interpreter
-
-(* +-----------------------------------------------------------------+
- | Completion |
- +-----------------------------------------------------------------+ *)
-
-let complete (before, after) commands =
- try
- match Krobot_script_lexer.partial_command (Lexing.from_string before) with
- | `Command(before, name) ->
- let path, last = match Text.rev_split ~sep:"." ~max:2 name with
- | [] -> ("", "")
- | [last] -> ("", last)
- | [path; last] -> (path ^ ".", last)
- | _ -> assert false
- in
- let n = Text.count ((=) ".") path in
- let names =
- String_map.fold
- (fun name' cmd acc ->
- if Text.starts_with name' name then
- Text_set.add (List.nth cmd.cmd_path n) acc
- else
- acc)
- commands Text_set.empty
- in
- Lwt_read_line.complete ~suffix:"" (before ^ path) last after names
-
- | `Arg(before, name, args, `Key key) ->
- let cmd = String_map.find name commands in
- let args' = List.fold_left (fun set (name, _, _) -> Text_set.add name set) Text_set.empty cmd.cmd_iargs in
- (* Remove already passed arguments *)
- let args = Text_set.diff args' args in
- Lwt_read_line.complete ~suffix:"=" before key after args
-
- | `Arg(before, name, args, `Value(key, value)) ->
- let cmd = String_map.find name commands in
- let name, typ, constants = List.find (fun (name, typ, constants) -> name = key) cmd.cmd_iargs in
- Lwt_read_line.complete ~suffix:" " before value after
- (List.fold_left (fun set constant -> Text_set.add constant set) Text_set.empty constants)
-
- | `Arg(before, name, args, `Nothing) ->
- raise Exit
-
- with Exit | Not_found | Krobot_script_lexer.Parse_failure _ ->
- { Lwt_read_line.comp_state = (before, after);
- Lwt_read_line.comp_words = Text_set.empty }
-
-let completion interpreter edition_state =
- React.S.l2 complete edition_state interpreter.itp_commands
-
-(* +-----------------------------------------------------------------+
- | Execution |
- +-----------------------------------------------------------------+ *)
-
-exception Arg_error of string * string
-
-let rec basic_of_string typ str =
- match typ with
- | Byte ->
- if String.length str = 1 then
- V.Byte(str.[0])
- else
- failwith "latin-1 character expected"
- | Boolean ->
- V.Boolean(bool_of_string str)
- | Int16 ->
- V.Int16(int_of_string str)
- | Uint16 ->
- V.Uint16(int_of_string str)
- | Int32 ->
- V.Int32(Int32.of_string str)
- | Uint32 ->
- V.Uint32(Int32.of_string str)
- | Int64 ->
- V.Int64(Int64.of_string str)
- | Uint64 ->
- V.Uint64(Int64.of_string str)
- | Double ->
- V.Double(float_of_string str)
- | String ->
- V.String str
- | Object_path ->
- V.Object_path (OBus_path.of_string str)
- | Signature ->
- V.Signature (signature_of_string str)
- | Enum(typ, l) ->
- let rec loop = function
- | [] ->
- basic_of_string (project_basic typ) str
- | (v, n) :: _ when n = str ->
- v
- | _ :: rest ->
- loop rest
- in
- loop l
- | Flag _ ->
- failwith "cannot yet parse flags"
- | Unix_fd ->
- failwith "how i am supposed to parse a file descriptor ?"
-
-let sequence_of_args cmd args =
- List.map
- (fun (name, typ, constants) ->
- try
- match typ with
- | Basic typ ->
- V.basic
- (basic_of_string typ
- (try
- List.assoc name args
- with Not_found ->
- List.assoc ("default." ^ name) cmd.cmd_annotations))
- | _ ->
- failwith "cannot yet parse D-Bus container values"
- with
- | Failure msg -> raise (Arg_error(name, msg))
- | Not_found -> raise (Arg_error(name, "missing value"))
- | exn -> raise (Arg_error(name, Printexc.to_string exn)))
- cmd.cmd_iargs
-
-let exec ~interpreter ~logger ~command =
- match try `OK(Krobot_script_lexer.command (Lexing.from_string command)) with exn -> `Fail exn with
- | `Fail(Krobot_script_lexer.Parse_failure msg) ->
- logger [fg lred; textf "parse failure: %s" msg]
- | `Fail exn ->
- logger [fg lred; textf "parse failure: %s" (Printexc.to_string exn)]
- | `OK(name, args) ->
- match try Some(String_map.find name (React.S.value interpreter.itp_commands)) with Not_found -> None with
- | None ->
- logger [fg lred; textf "command '%s' does not exist or is currently unavailable" name]
- | Some cmd ->
- try_lwt
- lwt result = OBus_method.call cmd.cmd_method cmd.cmd_proxy (sequence_of_args cmd args) in
- lwt () = logger [textf "%s: " name] in
- let rec print values names =
- match values, names with
- | [], [] ->
- return ()
- | v :: lv, (name, _, _) :: ln ->
- logger [textf " %s = " name; text (V.string_of_single v)]
- | _ ->
- Printf.ksprintf failwith "invalid number of arguments retuned, expected %d, got %d"
- (List.length cmd.cmd_oargs) (List.length result)
- in
- print result cmd.cmd_oargs
- with
- | OBus_error.DBus(_, msg) ->
- logger [fg lred; textf "%s: %s" name msg]
- | Arg_error(arg, msg) ->
- logger [fg lred; textf "%s: invalid argument %s: %s" name arg msg]
- | exn ->
- logger [fg lred; textf "command '%s' failed with: %s" name (Printexc.to_string exn)]
diff --git a/info/control/clients/krobot_script.mli b/info/control/clients/krobot_script.mli
deleted file mode 100644
index bb1a438..0000000
--- a/info/control/clients/krobot_script.mli
+++ /dev/null
@@ -1,26 +0,0 @@
-(*
- * krobot_script.mli
- * -----------------
- * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- *)
-
-(** Minit script language for the monitor *)
-
-type interpreter
- (** Type of a script interpreter *)
-
-val make : Krobot.t -> interpreter Lwt.t
- (** Creates an interpreter from the given krobot object *)
-
-val completion : interpreter -> Lwt_read_line.edition_state React.signal -> Lwt_read_line.completion_result React.signal
- (** [completion interpreter edition_state] takes a signal holding
- the current edition state and returns a signal holding the
- current completion. The completion is updated dynamically when
- services come up/down. *)
-
-val exec : interpreter : interpreter -> logger : (Lwt_term.styled_text -> unit Lwt.t) -> command : string -> unit Lwt.t
- (** [exec ~interpreter ~logger ~command] parses [command] and
- execute it. The result is logged with [logger]. *)
diff --git a/info/control/clients/krobot_script_lexer.mll b/info/control/clients/krobot_script_lexer.mll
deleted file mode 100644
index 065b0d8..0000000
--- a/info/control/clients/krobot_script_lexer.mll
+++ /dev/null
@@ -1,72 +0,0 @@
-(*
- * krobot_script_lexer.mll
- * -----------------------
- * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- *)
-
-{
- module Text_set = Set.Make(Text)
-
- exception Parse_failure of string
-}
-
-let lower = ['a'-'z']
-let upper = ['A'-'Z']
-let alpha = lower | upper
-let digit = ['0'-'9']
-let alnum = alpha | digit
-let punct = ['!' '"' '#' '$' '%' '&' '\'' '(' ')' '*' '+' ',' '-' '.' '/' ':' ';' '<' '=' '>' '?' '@' '[' '\\' ']' '^' '_' '`' '{' '|' '}' '~']
-let graph = alnum | punct
-let print = graph | ' '
-let blank = ' ' | '\t'
-let cntrl = ['\x00'-'\x1F' '\x7F']
-let xdigit = digit | ['a'-'f' 'A'-'F']
-let space = blank | ['\n' '\x0b' '\x0c' '\r']
-
-let identstart = [ 'A'-'Z' 'a'-'z' '_' ]
-let identbody = [ 'A'-'Z' 'a'-'z' '_' '-' '\'' '0' - '9' '.' ]
-let ident = identstart identbody*
-let maybe_ident = "" | ident
-
-let value = (alpha | digit | "-" | "." | "_")+
-
-rule partial_command = parse
- | blank* as before (maybe_ident as id) eof
- { `Command(before, id) }
- | blank* (ident as command) as s
- { let buf = Buffer.create 42 in
- Buffer.add_string buf s;
- let args, last = partial_arguments buf lexbuf in
- `Arg(Buffer.contents buf, command, args, last) }
- | ""
- { raise (Parse_failure "command expceted") }
-
-and partial_arguments buf = parse
- | (blank+ as before) (maybe_ident as key) eof
- { Buffer.add_string buf before;
- (Text_set.empty, `Key key) }
- | (blank+ (ident as key) blank* "=" blank* as before) ((value | "") as value) eof
- { Buffer.add_string buf before;
- (Text_set.empty, `Value(key, value)) }
- | blank+ (ident as key) blank* "=" blank* value as s
- { Buffer.add_string buf s;
- let set, x = partial_arguments buf lexbuf in
- (Text_set.add key set, x) }
- | ""
- { (Text_set.empty, `Nothing) }
-
-and command = parse
- | blank* (ident as command)
- { (command, arguments lexbuf) }
- | ""
- { raise (Parse_failure "command expceted") }
-
-and arguments = parse
- | blank+ (ident as key) blank* "=" blank* (value as value)
- { (key, value) :: arguments lexbuf }
- | blank* eof
- { [] }
- | "" { raise (Parse_failure "syntax error") }
diff --git a/info/control/clients/krobot_servos_control.ml b/info/control/clients/krobot_servos_control.ml
deleted file mode 100644
index 41509af..0000000
--- a/info/control/clients/krobot_servos_control.ml
+++ /dev/null
@@ -1,95 +0,0 @@
-(*
- * krobot_servos_control.ml
- * ------------------------
- * Copyright : (c) 2010, Jeremie Dimino <je...@di...>
- * Licence : BSD3
- *
- * This file is a part of [kro]bot.
- *)
-
-(* AX12/RX64 interactive controller *)
-
-open Lwt
-open Lwt_io
-open Lwt_term
-
-let render dynamixels positions selected =
- for_lwt i = 0 to 3 do
- let name, dev, id = dynamixels.(i) in
- let line = Printf.sprintf "%s.(%d) : %4d" name id positions.(i) in
- if i = selected then
- printlc [inverse; text line]
- else
- printl line
- done
-
-lwt () =
- Krobot_arg.parse ();
-
- lwt krobot = Krobot.create () in
-
- let ax12 = Krobot_driver.ax12 krobot and rx64 = Krobot_driver.rx64 krobot in
- let dynamixels = [|("ax12", ax12, 1); ("ax12", ax12, 2); ("ax12", ax12, 3); ("rx64", rx64, 1)|] in
-
- lwt () = Lwt_log.notice "reading current servos positions" in
- let positions = [|0; 0; 0 ;0|] in
- lwt () =
- for_lwt i = 0 to 3 do
- let name, dev, id = dynamixels.(i) in
- lwt position = Krobot_driver.Dynamixel.get_position dev id in
- positions.(i) <- position;
- return ()
- done
- in
-
- lwt () = printl "use arrow keys to control the servos" in
-
- let rec loop selected =
- lwt () = render dynamixels positions selected in
- Lwt_term.read_key () >>= function
- | Key_up ->
- if selected > 0 then
- loop2 (selected - 1)
- else
- loop2 selected
- | Key_down ->
- if selected < Array.length positions - 1 then
- loop2 (selected + 1)
- else
- loop2 selected
- | Key_left ->
- positions.(selected) <- max 0 (positions.(selected) - 10);
- let name, dev, id = dynamixels.(selected) in
- lwt () = Krobot_driver.Dynamixel.goto dev ~id ~position:positions.(selected) ~speed:50 ~mode:`Now in
- loop2 selected
- | Key_right ->
- positions.(selected) <- min 1023 (positions.(selected) + 10);
- let name, dev, id = dynamixels.(selected) in
- lwt () = Krobot_driver.Dynamixel.goto dev ~id ~position:positions.(selected) ~speed:50 ~mode:`Now in
- loop2 selected
- | Key ("\027[d" | "\027[1;2D") ->
- positions.(selected) <- max 0 (positions.(selected) - 1);
- let name, dev, id = dynamixels.(selected) in
- lwt () = Krobot_driver.Dynamixel.goto dev ~id ~position:positions.(selected) ~speed:50 ~mode:`Now in
- loop2 selected
- | Key ("\027[c" | "\027[1;2C") ->
- positions.(selected) <- min 1023 (positions.(selected) + 1);
- let name, dev, id = dynamixels.(selected) in
- lwt () = Krobot_driver.Dynamixel.goto dev ~id ~position:positions.(selected) ~speed:50 ~mode:`Now in
- loop selected
- | Key_control '[' ->
- return ()
- | _ ->
- loop2 selected
- and loop2 selected =
- lwt () = goto_beginning_of_line 4 in
- loop selected
- in
-
- lwt () = Lwt_term.enter_d...
[truncated message content] |
|
From: Xavier L. <Ba...@us...> - 2011-03-25 21:15:11
|
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 af2f09b9bb1cb78cd68a0085bf7dbc87a4635d7d (commit)
from 941e8e22c085789a822b680edd67dfdf93f6d8eb (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 af2f09b9bb1cb78cd68a0085bf7dbc87a4635d7d
Author: Xavier Lagorce <Xav...@cr...>
Date: Fri Mar 25 22:12:02 2011 +0100
[Controller_Motor_STM32] Command generator operational
* Move some stacks for dynamic to static
* Fixed some bugs
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.c
index dca6596..03cfe90 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/can_monitor.c
@@ -10,6 +10,8 @@
#include "can_monitor.h"
#include "hw/hw_led.h"
+PROC_DEFINE_STACK(stack_can_send, KERN_MINSTACKSIZE * 8);
+
typedef struct {
uint16_t encoder1_pos __attribute__((__packed__));
uint16_t encoder2_pos __attribute__((__packed__));
@@ -50,7 +52,7 @@ void canMonitorInit(void) {
can_start(CAND1, &cfg);
// Start communication process
- proc_new(canMonitor_process, NULL, KERN_MINSTACKSIZE * 8, NULL);
+ proc_new(canMonitor_process, NULL, sizeof(stack_can_send), stack_can_send);
}
static void NORETURN canMonitor_process(void) {
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/command_generator.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/command_generator.c
index da0fd67..6542070 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/command_generator.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/command_generator.c
@@ -103,6 +103,9 @@ float get_output_value(command_generator_t *generator) {
int32_t cur_time;
float speed, dt;
+ if (generator->type.state != GEN_STATE_RUNNING)
+ return generator->type.last_output;
+
switch (generator->type.t) {
case GEN_CONSTANT:
// Constant generator, no update needed
@@ -112,12 +115,14 @@ float get_output_value(command_generator_t *generator) {
dt = (cur_time - generator->ramp.last_time)*1e-6;
generator->type.last_output += dt*generator->ramp.speed;
generator->ramp.last_time = cur_time;
+ break;
case GEN_RAMP2:
cur_time = ticks_to_us(timer_clock());
speed = get_output_value(generator->ramp2.speed);
dt = (cur_time - generator->ramp.last_time)*1e-6;
generator->type.last_output += dt*speed;
generator->ramp.last_time = cur_time;
+ break;
}
switch (generator->type.callback.type) {
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/controller_motor_stm32.mk b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/controller_motor_stm32.mk
index 7d1317b..cbfc980 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/controller_motor_stm32.mk
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/controller_motor_stm32.mk
@@ -22,14 +22,14 @@ controller_motor_stm32_WIZARD_CSRC = \
bertos/cpu/cortex-m3/drv/can_stm32.c \
bertos/cpu/cortex-m3/hw/switch_ctx_cm3.c \
bertos/mware/event.c \
+ bertos/kern/monitor.c \
bertos/cpu/cortex-m3/drv/timer_cm3.c \
bertos/struct/heap.c \
bertos/drv/can.c \
- bertos/kern/monitor.c \
+ bertos/mware/formatwr.c \
bertos/drv/timer.c \
bertos/kern/signal.c \
bertos/kern/proc.c \
- bertos/mware/formatwr.c \
bertos/mware/hex.c \
#
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/main.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/main.c
index ecbc3a7..9dd248c 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/main.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/main.c
@@ -17,7 +17,9 @@
#include "can_monitor.h"
#include "command_generator.h"
-command_generator_t generator;
+command_generator_t genR, genL;
+
+PROC_DEFINE_STACK(stack_op, KERN_MINSTACKSIZE * 8);
static void init(void)
{
@@ -41,11 +43,12 @@ static void init(void)
canMonitorInit();
// Start control of drive motors
- new_ramp_generator(&generator, 0., 180.);
+ new_ramp_generator(&genR, 0., 180.);
+ new_ramp_generator(&genL, 0., 180.);
float k[] = {-68.0325, -1.0205};
float l0[] = {0.0236, 3.9715};
- mc_new_controller(MOTOR3, ENCODER3, -360.0/2000.0/15.0, 0.833, 0.015, 0.005, k, -k[0], l0, &generator);
- mc_new_controller(MOTOR4, ENCODER4, -360.0/2000.0/15.0, 0.833, 0.015, 0.005, k, -k[0], l0, &generator);
+ mc_new_controller(MOTOR3, ENCODER3, -360.0/2000.0/15.0, 0.833, 0.015, 0.005, k, -k[0], l0, &genL);
+ mc_new_controller(MOTOR4, ENCODER4, -360.0/2000.0/15.0, 0.833, 0.015, 0.005, k, -k[0], l0, &genR);
// Blink to say we are ready
for (uint8_t i=0; i < 2; i++) {
@@ -87,13 +90,21 @@ static void NORETURN square_process(void)
static void NORETURN goto_process(void)
{
LED1_ON();
- start_generator(&generator);
+ start_generator(&genL);
+ start_generator(&genR);
timer_delay(2000);
- pause_generator(&generator);
- adjust_speed(&generator, 360.);
+ pause_generator(&genL);
+ pause_generator(&genR);
+ adjust_speed(&genL, 360.);
+ adjust_speed(&genR, 360.);
timer_delay(1000);
- start_generator(&generator);
+ start_generator(&genL);
+ start_generator(&genR);
timer_delay(2000);
+ adjust_speed(&genL, 0.);
+ adjust_speed(&genR, 0.);
+ timer_delay(200);
+ mc_delete_controller(MOTOR3);
mc_delete_controller(MOTOR4);
LED1_OFF();
}
@@ -104,7 +115,7 @@ int main(void)
init();
/* Create a new child process */
- proc_new(goto_process, NULL, KERN_MINSTACKSIZE * 8, NULL);
+ proc_new(goto_process, NULL, sizeof(stack_op), stack_op);
/*
* The main process is kept to periodically report the stack
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor_controller.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor_controller.c
index 99fc75b..8246a8c 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor_controller.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor_controller.c
@@ -210,7 +210,7 @@ uint8_t mc_new_controller(uint8_t motor, uint8_t encoder, float encoder_gain, fl
motorSetSpeed(motor, 0);
// start the controller
- proc_new(motorController_process, params, KERN_MINSTACKSIZE * 8, NULL);
+ proc_new(motorController_process, params, KERN_MINSTACKSIZE * 16, NULL);
return CONTROLLER_OK;
} else {
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/project.bertos b/elec/boards/Controller_Motor_STM32/Firmware/project.bertos
index e2a1b7f..33b0c75 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/project.bertos
+++ b/elec/boards/Controller_Motor_STM32/Firmware/project.bertos
@@ -29,11 +29,11 @@ p13
(lp14
S'signal'
p15
-aS'kernel'
+aS'debug'
p16
-aS'monitor'
+aS'kernel'
p17
-aS'debug'
+aS'monitor'
p18
aS'timer'
p19
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2011-03-25 12:44:36
|
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 941e8e22c085789a822b680edd67dfdf93f6d8eb (commit)
from bca01c2266c150b35776c5ab5befd4cd82478583 (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 941e8e22c085789a822b680edd67dfdf93f6d8eb
Author: Xavier Lagorce <Xav...@cr...>
Date: Fri Mar 25 13:38:16 2011 +0100
[Controller_Motor_STM32] Command_generator : added callbacks
Added the possibility to have some callbacks in command generator which are
triggered by the value of the output
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/command_generator.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/command_generator.c
index 3153ed7..da0fd67 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/command_generator.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/command_generator.c
@@ -12,6 +12,7 @@
command_generator_t* new_constant_generator(command_generator_t *generator, float value) {
generator->type.t = GEN_CONSTANT;
+ generator->type.callback.type = GEN_CALLBACK_NONE;
generator->constant.gen.last_output = value;
generator->constant.gen.state = GEN_STATE_PAUSE;
@@ -20,6 +21,7 @@ command_generator_t* new_constant_generator(command_generator_t *generator, floa
command_generator_t* new_ramp_generator(command_generator_t *generator, float starting_value, float speed) {
generator->type.t = GEN_RAMP;
+ generator->type.callback.type = GEN_CALLBACK_NONE;
generator->ramp.gen.state = GEN_STATE_PAUSE;
generator->ramp.gen.last_output = starting_value;
generator->ramp.last_time = 0;
@@ -30,6 +32,7 @@ command_generator_t* new_ramp_generator(command_generator_t *generator, float st
command_generator_t* new_ramp2_generator(command_generator_t *generator, float starting_value, command_generator_t *speed) {
generator->type.t = GEN_RAMP2;
+ generator->type.callback.type = GEN_CALLBACK_NONE;
generator->ramp2.gen.state = GEN_STATE_PAUSE;
generator->ramp2.gen.last_output = starting_value;
generator->ramp2.last_time = 0;
@@ -81,6 +84,20 @@ command_generator_t* pause_generator(command_generator_t *generator) {
return generator;
}
+command_generator_t* add_callback(command_generator_t *generator, uint8_t type, float threshold, void (*callback_function)(command_generator_t*)) {
+ generator->type.callback.callback_function = callback_function;
+ generator->type.callback.threshold = threshold;
+ generator->type.callback.type = type;
+
+ return generator;
+}
+
+command_generator_t* remove_callback(command_generator_t *generator) {
+ generator->type.callback.type = GEN_CALLBACK_NONE;
+
+ return generator;
+}
+
float get_output_value(command_generator_t *generator) {
int32_t cur_time;
@@ -103,5 +120,18 @@ float get_output_value(command_generator_t *generator) {
generator->ramp.last_time = cur_time;
}
+ switch (generator->type.callback.type) {
+ case GEN_CALLBACK_NONE :
+ break;
+ case GEN_CALLBACK_SUP :
+ if (generator->type.last_output > generator->type.callback.threshold)
+ generator->type.callback.callback_function(generator);
+ break;
+ case GEN_CALLBACK_INF :
+ if (generator->type.last_output < generator->type.callback.threshold)
+ generator->type.callback.callback_function(generator);
+ break;
+ }
+
return generator->type.last_output;
}
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/command_generator.h b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/command_generator.h
index 65287b1..38e2564 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/command_generator.h
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/command_generator.h
@@ -23,12 +23,23 @@
#define GEN_STATE_PAUSE 0
#define GEN_STATE_RUNNING 1
+#define GEN_CALLBACK_NONE 0
+#define GEN_CALLBACK_SUP 1
+#define GEN_CALLBACK_INF 2
+
typedef union _command_generator_t command_generator_t;
typedef struct {
+ uint8_t type;
+ float threshold;
+ void (*callback_function)(command_generator_t*);
+} generator_callback_t;
+
+typedef struct {
uint8_t t;
float last_output;
uint8_t state;
+ generator_callback_t callback;
} placeholder_generator_t;
typedef struct {
@@ -64,6 +75,9 @@ command_generator_t* adjust_speed(command_generator_t *generator, float speed);
command_generator_t* start_generator(command_generator_t *generator);
command_generator_t* pause_generator(command_generator_t *generator);
+command_generator_t* add_callback(command_generator_t *generator, uint8_t type, float threshold, void (*callback_function)(command_generator_t*));
+command_generator_t* remove_callback(command_generator_t *generator);
+
float get_output_value(command_generator_t *generator);
#endif /* __COMMAND_GENERATOR_H */
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2011-03-24 16:35:18
|
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 bca01c2266c150b35776c5ab5befd4cd82478583 (commit)
from 30533f7559e9e1dead16e3c01253fca68dc09982 (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 bca01c2266c150b35776c5ab5befd4cd82478583
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu Mar 24 17:34:55 2011 +0100
[Controller_Motor_STM32] Missing files...
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/command_generator.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/command_generator.c
new file mode 100644
index 0000000..3153ed7
--- /dev/null
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/command_generator.c
@@ -0,0 +1,107 @@
+/*
+ * command_generator.c
+ * -------------------
+ * Copyright : (c) 2011, Xavier Lagorce <Xav...@cr...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ */
+
+#include "command_generator.h"
+
+
+command_generator_t* new_constant_generator(command_generator_t *generator, float value) {
+ generator->type.t = GEN_CONSTANT;
+ generator->constant.gen.last_output = value;
+ generator->constant.gen.state = GEN_STATE_PAUSE;
+
+ return generator;
+}
+
+command_generator_t* new_ramp_generator(command_generator_t *generator, float starting_value, float speed) {
+ generator->type.t = GEN_RAMP;
+ generator->ramp.gen.state = GEN_STATE_PAUSE;
+ generator->ramp.gen.last_output = starting_value;
+ generator->ramp.last_time = 0;
+ generator->ramp.speed = speed;
+
+ return generator;
+}
+
+command_generator_t* new_ramp2_generator(command_generator_t *generator, float starting_value, command_generator_t *speed) {
+ generator->type.t = GEN_RAMP2;
+ generator->ramp2.gen.state = GEN_STATE_PAUSE;
+ generator->ramp2.gen.last_output = starting_value;
+ generator->ramp2.last_time = 0;
+ generator->ramp2.speed = speed;
+
+ return generator;
+}
+
+
+command_generator_t* adjust_value(command_generator_t *generator, float value) {
+ generator->type.last_output = value;
+
+ return generator;
+}
+
+command_generator_t* adjust_speed(command_generator_t *generator, float speed) {
+ switch (generator->type.t) {
+ case GEN_RAMP:
+ generator->ramp.speed = speed;
+ return generator;
+ default:
+ return NULL;
+ }
+}
+
+command_generator_t* start_generator(command_generator_t *generator) {
+ switch (generator->type.t) {
+ case GEN_RAMP:
+ generator->ramp.last_time = ticks_to_us(timer_clock());
+ break;
+ case GEN_RAMP2:
+ start_generator(generator->ramp2.speed);
+ generator->ramp2.last_time = ticks_to_us(timer_clock());
+ }
+ generator->type.state = GEN_STATE_RUNNING;
+
+ return generator;
+}
+
+command_generator_t* pause_generator(command_generator_t *generator) {
+ // update generator->type.last_output
+ get_output_value(generator);
+ // pause the generator
+ generator->type.state = GEN_STATE_PAUSE;
+
+ if (generator->type.t == GEN_RAMP2)
+ pause_generator(generator->ramp2.speed);
+
+ return generator;
+}
+
+
+float get_output_value(command_generator_t *generator) {
+ int32_t cur_time;
+ float speed, dt;
+
+ switch (generator->type.t) {
+ case GEN_CONSTANT:
+ // Constant generator, no update needed
+ break;
+ case GEN_RAMP:
+ cur_time = ticks_to_us(timer_clock());
+ dt = (cur_time - generator->ramp.last_time)*1e-6;
+ generator->type.last_output += dt*generator->ramp.speed;
+ generator->ramp.last_time = cur_time;
+ case GEN_RAMP2:
+ cur_time = ticks_to_us(timer_clock());
+ speed = get_output_value(generator->ramp2.speed);
+ dt = (cur_time - generator->ramp.last_time)*1e-6;
+ generator->type.last_output += dt*speed;
+ generator->ramp.last_time = cur_time;
+ }
+
+ return generator->type.last_output;
+}
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/command_generator.h b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/command_generator.h
new file mode 100644
index 0000000..65287b1
--- /dev/null
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/command_generator.h
@@ -0,0 +1,69 @@
+/*
+ * command_generator.h
+ * -------------------
+ * Copyright : (c) 2011, Xavier Lagorce <Xav...@cr...>
+ * Licence : BSD3
+ *
+ * This module aims at providing a simple way to generate some command signal
+ * for control processes (like steps, ramps, ...)
+ *
+ * This file is a part of [kro]bot.
+ */
+
+#ifndef __COMMAND_GENERATOR_H
+#define __COMMAND_GENERATOR_H
+
+#include <drv/timer.h>
+
+#define GEN_NONE 0
+#define GEN_CONSTANT 1
+#define GEN_RAMP 2
+#define GEN_RAMP2 3
+
+#define GEN_STATE_PAUSE 0
+#define GEN_STATE_RUNNING 1
+
+typedef union _command_generator_t command_generator_t;
+
+typedef struct {
+ uint8_t t;
+ float last_output;
+ uint8_t state;
+} placeholder_generator_t;
+
+typedef struct {
+ placeholder_generator_t gen;
+} constant_generator_t;
+
+typedef struct {
+ placeholder_generator_t gen;
+ int32_t last_time;
+ float speed;
+} ramp_generator_t;
+
+typedef struct {
+ placeholder_generator_t gen;
+ int32_t last_time;
+ command_generator_t *speed;
+} ramp2_generator_t;
+
+union _command_generator_t {
+ placeholder_generator_t type;
+ constant_generator_t constant;
+ ramp_generator_t ramp;
+ ramp2_generator_t ramp2;
+};
+
+command_generator_t* new_constant_generator(command_generator_t *generator, float value);
+command_generator_t* new_ramp_generator(command_generator_t *generator, float starting_value, float speed);
+command_generator_t* new_ramp2_generator(command_generator_t *generator, float starting_value, command_generator_t *speed);
+
+command_generator_t* adjust_value(command_generator_t *generator, float value);
+command_generator_t* adjust_speed(command_generator_t *generator, float speed);
+
+command_generator_t* start_generator(command_generator_t *generator);
+command_generator_t* pause_generator(command_generator_t *generator);
+
+float get_output_value(command_generator_t *generator);
+
+#endif /* __COMMAND_GENERATOR_H */
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2011-03-24 16:08:17
|
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 30533f7559e9e1dead16e3c01253fca68dc09982 (commit)
from aad13dd94ac9a04ac7ce9baf72c2013a003ca3e5 (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 30533f7559e9e1dead16e3c01253fca68dc09982
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu Mar 24 17:06:35 2011 +0100
[Controller_Motor_STM32] Added a command generator.
This command generator is aimed at providing signal generator for
motor controller reference. It should be able to generate :
* a constant value
* a ramp
* a ramp of which the speed is given by another generator
Demo program included, yet to be tested
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/controller_motor_stm32_user.mk b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/controller_motor_stm32_user.mk
index 408ef8f..6387ba7 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/controller_motor_stm32_user.mk
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/controller_motor_stm32_user.mk
@@ -14,6 +14,7 @@ controller_motor_stm32_USER_CSRC = \
$(controller_motor_stm32_SRC_PATH)/motor.c \
$(controller_motor_stm32_SRC_PATH)/encoder.c \
$(controller_motor_stm32_SRC_PATH)/motor_controller.c \
+ $(controller_motor_stm32_SRC_PATH)/command_generator.c \
$(controller_motor_stm32_SRC_PATH)/can_monitor.c \
$(controller_motor_stm32_SRC_PATH)/main.c \
#
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/main.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/main.c
index d6a4116..ecbc3a7 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/main.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/main.c
@@ -15,6 +15,9 @@
#include "motor.h"
#include "motor_controller.h"
#include "can_monitor.h"
+#include "command_generator.h"
+
+command_generator_t generator;
static void init(void)
{
@@ -38,10 +41,11 @@ static void init(void)
canMonitorInit();
// Start control of drive motors
+ new_ramp_generator(&generator, 0., 180.);
float k[] = {-68.0325, -1.0205};
float l0[] = {0.0236, 3.9715};
- mc_new_controller(MOTOR3, ENCODER3, -360.0/2000.0/15.0, 0.833, 0.015, 0.005, k, -k[0], l0);
- mc_new_controller(MOTOR4, ENCODER4, -360.0/2000.0/15.0, 0.833, 0.015, 0.005, k, -k[0], l0);
+ mc_new_controller(MOTOR3, ENCODER3, -360.0/2000.0/15.0, 0.833, 0.015, 0.005, k, -k[0], l0, &generator);
+ mc_new_controller(MOTOR4, ENCODER4, -360.0/2000.0/15.0, 0.833, 0.015, 0.005, k, -k[0], l0, &generator);
// Blink to say we are ready
for (uint8_t i=0; i < 2; i++) {
@@ -82,22 +86,14 @@ static void NORETURN square_process(void)
static void NORETURN goto_process(void)
{
- float inc = 1.8;
- float ref = 0;
- int32_t dt= 5;
- uint8_t ind = 0;
-
LED1_ON();
- // Make the wheels turn
- while(ref < 7200)
- {
- mc_gotoPosition(MOTOR3, ref);
- mc_gotoPosition(MOTOR4, ref);
- ref = ref + inc;
- timer_delay(dt);
- }
+ start_generator(&generator);
+ timer_delay(2000);
+ pause_generator(&generator);
+ adjust_speed(&generator, 360.);
timer_delay(1000);
- //mc_delete_controller(MOTOR3);
+ start_generator(&generator);
+ timer_delay(2000);
mc_delete_controller(MOTOR4);
LED1_OFF();
}
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor_controller.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor_controller.c
index 680f3e1..99fc75b 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor_controller.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor_controller.c
@@ -17,22 +17,22 @@
typedef struct
{
- uint8_t enable; // Is this controller enabled ?
- uint8_t motor; // Motor ID to control
- uint8_t encoder; // Encoder ID to measure motor position from
- float encoder_gain; // Gain to convert encoder value unit to reference unit
- float reference; // Goal position
- float tau; // DC motor time constant
- float k[2]; // State control gain
- float l; // Reference factoring gain
- float l0[2]; // State observer gain
- float last_command; // Last applyed command
- float last_estimate[2]; // Last state estimate
- float last_output; // Last measured output
- uint16_t last_encoder_pos; // Last encoder position measured
- float F[4]; // evolution matrix
- float G[2]; // command application matrix
- ticks_t T; // sampling period in systicks
+ uint8_t enable; // Is this controller enabled ?
+ uint8_t motor; // Motor ID to control
+ uint8_t encoder; // Encoder ID to measure motor position from
+ float encoder_gain; // Gain to convert encoder value unit to reference unit
+ command_generator_t *reference; // Goal position
+ float tau; // DC motor time constant
+ float k[2]; // State control gain
+ float l; // Reference factoring gain
+ float l0[2]; // State observer gain
+ float last_command; // Last applyed command
+ float last_estimate[2]; // Last state estimate
+ float last_output; // Last measured output
+ uint16_t last_encoder_pos; // Last encoder position measured
+ float F[4]; // evolution matrix
+ float G[2]; // command application matrix
+ ticks_t T; // sampling period in systicks
} control_params_t;
control_params_t controllers[4];
@@ -88,20 +88,20 @@ float mc_getPosition(uint8_t motor) {
}
}
-void mc_gotoPosition(uint8_t motor, float position) {
+void mc_setReference(uint8_t motor, command_generator_t *generator) {
switch(motor) {
case MOTOR1:
- controllers[0].reference = position;
+ controllers[0].reference = generator;
break;
case MOTOR2:
- controllers[1].reference = position;
+ controllers[1].reference = generator;
break;
case MOTOR3:
- controllers[2].reference = position;
+ controllers[2].reference = generator;
break;
case MOTOR4:
- controllers[3].reference = position;
+ controllers[3].reference = generator;
break;
}
}
@@ -147,7 +147,7 @@ static void NORETURN motorController_process(void) {
params->last_output += params->encoder_gain*delta;
// Command computation
- params->last_command = params->l*params->reference + params->k[0]*estimate[0] + params->k[1]*estimate[1];
+ params->last_command = params->l*get_output_value(params->reference) + params->k[0]*estimate[0] + params->k[1]*estimate[1];
// Keep estimate and data
params->last_estimate[0] = estimate[0];
@@ -171,7 +171,7 @@ void speedControlInit() {
motorsInit();
}
-uint8_t mc_new_controller(uint8_t motor, uint8_t encoder, float encoder_gain, float G0, float tau, float T, float *k, float l, float *l0) {
+uint8_t mc_new_controller(uint8_t motor, uint8_t encoder, float encoder_gain, float G0, float tau, float T, float *k, float l, float *l0, command_generator_t *generator) {
uint8_t motor_ind;
control_params_t *params;
@@ -195,7 +195,7 @@ uint8_t mc_new_controller(uint8_t motor, uint8_t encoder, float encoder_gain, fl
params->last_estimate[0] = 0; params->last_estimate[1] = 0;
params->last_output = params->last_estimate[0];
params->last_encoder_pos = getEncoderPosition(encoder);
- params->reference = 0;
+ params->reference = generator;
params->F[0] = 1;
params->F[1] = tau*(1-exp(-T/tau));
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor_controller.h b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor_controller.h
index 9a1334b..749bb41 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor_controller.h
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor_controller.h
@@ -8,6 +8,7 @@
#include "motor.h"
#include "encoder.h"
+#include "command_generator.h"
#define CONTROLLER_OK 0
#define CONTROLLER_ALREADY_USED 1
@@ -15,9 +16,9 @@
void speedControlInit(void);
float mc_getSpeed(uint8_t motor);
float mc_getPosition(uint8_t motor);
-void mc_gotoPosition(uint8_t motor, float position);
+void mc_setReference(uint8_t motor, command_generator_t *generator);
-uint8_t mc_new_controller(uint8_t motor, uint8_t encoder, float encoder_gain, float G0, float tau, float T, float *k, float l, float *l0);
+uint8_t mc_new_controller(uint8_t motor, uint8_t encoder, float encoder_gain, float G0, float tau, float T, float *k, float l, float *l0, command_generator_t *generator);
void mc_delete_controller(uint8_t motor);
#endif
hooks/post-receive
--
krobot
|
