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: Xavier L. <Ba...@us...> - 2011-04-05 13:20:55
|
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 e4efb4a37ad629588ff4f3a2f0308754b236bdc1 (commit)
from 517ffeb0bb66fcbc50ebf1ff4e70f1279fb01358 (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 e4efb4a37ad629588ff4f3a2f0308754b236bdc1
Author: Xavier Lagorce <Xav...@cr...>
Date: Tue Apr 5 15:20:19 2011 +0200
[Controller_Motor_STM32] Removed debug code with LED2 in differential drive
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.c
index 9a03d1f..85b643d 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.c
@@ -96,9 +96,6 @@ void dd_move(float distance, float speed, float acceleration) {
}
void dd_turn(float angle, float speed, float acceleration) {
- if (angle > 1.0)
- LED2_ON();
-
if (params.enabled) {
params.last_rot_acceleration = acceleration;
tc_goto(DD_ROTATIONAL_SPEED_TC, angle, speed, params.last_rot_acceleration);
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2011-04-05 13:05:44
|
This is an automated email from the git hooks/post-receive script. It was
generated because a ref change was pushed to the repository containing
the project "krobot".
The branch, master has been updated
via 517ffeb0bb66fcbc50ebf1ff4e70f1279fb01358 (commit)
from 3b208ec28291deddce260e77fb3e05f8b810dfcd (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 517ffeb0bb66fcbc50ebf1ff4e70f1279fb01358
Author: Xavier Lagorce <Xav...@cr...>
Date: Tue Apr 5 15:04:41 2011 +0200
[krobot_viewer] Graphical adjustements to oponent representation (and init).
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index 08b3d16..00bb54c 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -569,12 +569,17 @@ module Board = struct
y = 1.9 +. Krobot_config.robot_size /. 2. -. Krobot_config.wheels_position };
theta = -0.5 *. pi
};
- beacon = { xbeacon = 1.; ybeacon = 1.; valid = true };
+ beacon = { xbeacon = 1.; ybeacon = 1.; valid = false };
points = [];
bezier = [];
event = E.never;
moving = false;
} in
+ board.ui#beacon_status#set_text "-";
+ board.ui#beacon_distance#set_text "-";
+ board.ui#beacon_angle#set_text "-";
+ board.ui#beacon_period#set_text "-";
+ queue_draw board;
(* Move the robot on the board when we receive odometry
informations. *)
board.event <- (
@@ -605,7 +610,7 @@ module Board = struct
let beacon = { xbeacon = x; ybeacon = y; valid; } in
if beacon <> board.beacon then begin
board.beacon <- beacon;
- board.ui#beacon_status#set_text (if valid then "valid" else "invalid");
+ board.ui#beacon_status#set_text (if valid then "valid" else "-");
board.ui#beacon_distance#set_text (string_of_float distance);
board.ui#beacon_angle#set_text (string_of_float angle);
board.ui#beacon_period#set_text (string_of_float period);
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2011-04-05 12:46:52
|
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 3b208ec28291deddce260e77fb3e05f8b810dfcd (commit)
via f462e4a5544e2ab3300fa71c30be6d64ae251405 (commit)
from 83d83da9e7a092de93bfda899bdbada8d233bcab (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 3b208ec28291deddce260e77fb3e05f8b810dfcd
Author: Xavier Lagorce <Xav...@cr...>
Date: Tue Apr 5 14:44:53 2011 +0200
[Controller_Motor_STM32] Added the possibility to limit wheel's speed.
This is to be used as a security to limit speed command reference in case of
wrong high level computation.
commit f462e4a5544e2ab3300fa71c30be6d64ae251405
Author: Xavier Lagorce <Xav...@cr...>
Date: Tue Apr 5 14:44:16 2011 +0200
[Controller_Motor_STM32] Normalized angle used in odometry for internal computation
-----------------------------------------------------------------------
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 264cf7d..bc54d89 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
@@ -8,7 +8,7 @@
*/
#include "command_generator.h"
-
+#include <stdlib.h>
command_generator_t* new_constant_generator(command_generator_t *generator, float value) {
generator->type.t = GEN_CONSTANT;
@@ -46,7 +46,7 @@ command_generator_t* new_dd_generator(command_generator_t *generator,
command_generator_t *linear_speed,
command_generator_t *rotational_pos,
command_generator_t *rotational_speed,
- float wheel_radius, float shaft_width,
+ float wheel_radius, float shaft_width, float max_speed,
uint8_t type) {
generator->type.t = (type == 1) ? GEN_DD_RIGHT : GEN_DD_LEFT;
generator->type.callback.type = GEN_CALLBACK_NONE;
@@ -56,6 +56,7 @@ command_generator_t* new_dd_generator(command_generator_t *generator,
generator->dd.rotational_speed = rotational_speed;
generator->dd.wheel_radius = wheel_radius;
generator->dd.shaft_width = shaft_width;
+ generator->dd.max_speed = max_speed;
return generator;
}
@@ -148,7 +149,12 @@ float get_output_value(command_generator_t *generator) {
// Compute output
u1 = get_output_value(generator->dd.linear_speed);
u2 = get_output_value(generator->dd.rotational_speed);
- generator->type.last_output = (4.0*u1+u2*generator->dd.shaft_width) / (4.0 * generator->dd.wheel_radius);
+ generator->type.last_output = (2.0*u1+u2*generator->dd.shaft_width) / (2.0 * generator->dd.wheel_radius);
+ if (generator->type.last_output >= 0) {
+ generator->type.last_output = MIN(generator->type.last_output, generator->dd.max_speed);
+ } else {
+ generator->type.last_output = MAX(generator->type.last_output, -generator->dd.max_speed);
+ }
break;
case GEN_DD_LEFT:
// Update position generators to allow callbacks
@@ -157,7 +163,12 @@ float get_output_value(command_generator_t *generator) {
// Compute output
u1 = get_output_value(generator->dd.linear_speed);
u2 = get_output_value(generator->dd.rotational_speed);
- generator->type.last_output = (4.0*u1-u2*generator->dd.shaft_width) / (4.0 * generator->dd.wheel_radius);
+ generator->type.last_output = (2.0*u1-u2*generator->dd.shaft_width) / (2.0 * generator->dd.wheel_radius);
+ if (generator->type.last_output >= 0) {
+ generator->type.last_output = MIN(generator->type.last_output, generator->dd.max_speed);
+ } else {
+ generator->type.last_output = MAX(generator->type.last_output, -generator->dd.max_speed);
+ }
break;
}
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 6d888d6..28aa810 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
@@ -77,6 +77,7 @@ typedef struct {
command_generator_t *rotational_speed;
float wheel_radius;
float shaft_width;
+ float max_speed;
} dd_generator_t;
// Usable generator meta-type
@@ -124,6 +125,7 @@ command_generator_t* new_ramp2_generator(command_generator_t *generator,
* - rotational_speed : pointer to the generator giving the rotational speed of the drive
* - wheel_radius : radius of the wheels
* - shaft_width : width of the propulsion shaft
+ * - max_speed : maximum wheel speed (in rad/s)
* - type : 1 for the right_wheel, -1 for the left_wheel
*/
command_generator_t* new_dd_generator(command_generator_t *generator,
@@ -131,7 +133,7 @@ command_generator_t* new_dd_generator(command_generator_t *generator,
command_generator_t *linear_speed,
command_generator_t *rotational_pos,
command_generator_t *rotational_speed,
- float wheel_radius, float shaft_width,
+ float wheel_radius, float shaft_width, float max_speed,
uint8_t type);
/*
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.c
index 3e43444..9a03d1f 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.c
@@ -19,7 +19,7 @@ typedef struct {
static dd_params_t params;
-void dd_start(float wheel_radius, float shaft_width) {
+void dd_start(float wheel_radius, float shaft_width, float max_wheel_speed) {
params.wheel_radius = wheel_radius;
params.shaft_radius = shaft_width;
params.last_lin_acceleration = 0.0;
@@ -32,14 +32,14 @@ void dd_start(float wheel_radius, float shaft_width) {
tc_get_speed_generator(DD_LINEAR_SPEED_TC),
tc_get_position_generator(DD_ROTATIONAL_SPEED_TC),
tc_get_speed_generator(DD_ROTATIONAL_SPEED_TC),
- wheel_radius, shaft_width,
+ wheel_radius, shaft_width, max_wheel_speed,
-1);
new_dd_generator(¶ms.right_wheel_speed,
tc_get_position_generator(DD_LINEAR_SPEED_TC),
tc_get_speed_generator(DD_LINEAR_SPEED_TC),
tc_get_position_generator(DD_ROTATIONAL_SPEED_TC),
tc_get_speed_generator(DD_ROTATIONAL_SPEED_TC),
- wheel_radius, shaft_width,
+ wheel_radius, shaft_width, max_wheel_speed,
1);
new_ramp2_generator(¶ms.left_wheel, 0.0, ¶ms.left_wheel_speed);
new_ramp2_generator(¶ms.right_wheel, 0.0, ¶ms.right_wheel_speed);
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.h b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.h
index 4e68b0a..61684ce 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.h
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.h
@@ -24,11 +24,12 @@
/* Initializes the differential drive
* - wheel_radius : radius of the wheels (in meters)
* - shaft_width : propulsion shaft radius (in meters)
+ * - max_speed : maximum wheel speed (in rad/s)
*
* Note : the differential drive system will use Trajectory controllers
* DD_LINEAR_SPEED_TC and DD_ROTATIONAL_SPEED_TC
*/
-void dd_start(float wheel_radius, float shaft_width);
+void dd_start(float wheel_radius, float shaft_width, float max_speed);
/* Pauses or Resumes the differential drive system.
* In pause mode, the drive will accept no further command and actions will be
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 5584084..647f9fa 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
@@ -47,7 +47,7 @@ static void init(void)
// Start control of drive motors
tc_init();
- dd_start(WHEEL_RADIUS, SHAFT_WIDTH);
+ dd_start(WHEEL_RADIUS, SHAFT_WIDTH, 4.8*2*M_PI); // limit wheel speed to 1.5 m/s
// Common parameters
params.encoder_gain = -2.0*M_PI/2000.0/15.0;
params.G0 = 0.0145;
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/odometry.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/odometry.c
index f6241fa..28d2038 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/odometry.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/odometry.c
@@ -91,6 +91,13 @@ static void NORETURN odometry_process(void) {
state.robot_state.x += state.wheel_radius * (delta_r + delta_l) / 2.0 * cos(state.robot_state.theta);
state.robot_state.y += state.wheel_radius * (delta_r + delta_l) / 2.0 * sin(state.robot_state.theta);
state.robot_state.theta += state.wheel_radius / state.shaft_width * (delta_r - delta_l);
+
+ // Normalization of theta
+ if (state.robot_state.theta > M_PI) {
+ state.robot_state.theta -= 2*M_PI;
+ } else if (state.robot_state.theta < -M_PI) {
+ state.robot_state.theta += 2*M_PI;
+ }
}
timer_waitEvent(&timer); // Wait for the remaining of the sample period
}
hooks/post-receive
--
krobot
|
|
From: Nicolas D. <Ba...@us...> - 2011-04-04 22:39: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 83d83da9e7a092de93bfda899bdbada8d233bcab (commit)
from df19a6189b6e7dcc5f1f0b9aad90d51201605e4a (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 83d83da9e7a092de93bfda899bdbada8d233bcab
Author: Nicolas Dandrimont <Nic...@cr...>
Date: Tue Apr 5 00:38:58 2011 +0200
[.gitignore] Add BeRTOS temp files.
-----------------------------------------------------------------------
Changes:
diff --git a/.gitignore b/.gitignore
index 40e4de9..a443cd9 100644
--- a/.gitignore
+++ b/.gitignore
@@ -18,3 +18,6 @@ setup.data
setup.log
_build
info/control2011/src/driver/bus.conf
+buildrev.h
+images
+obj
hooks/post-receive
--
krobot
|
|
From: Jérémie D. <Ba...@us...> - 2011-04-04 22:23: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 df19a6189b6e7dcc5f1f0b9aad90d51201605e4a (commit)
via 4f76274d2c7fc17b0265d59f47e7bd7c16b1e6d9 (commit)
via 2fc3f6a82ab36af5da8237093ded26ca547afa0e (commit)
via 2954ce9c54bcec2b8712e9a5a8842e8d19125809 (commit)
from dd7c5daac4fc87ee81874bfca82f20efde0a8f92 (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 df19a6189b6e7dcc5f1f0b9aad90d51201605e4a
Author: Jérémie Dimino <je...@di...>
Date: Tue Apr 5 00:13:52 2011 +0200
[info] add a plotter for batteries
commit 4f76274d2c7fc17b0265d59f47e7bd7c16b1e6d9
Author: Jérémie Dimino <je...@di...>
Date: Tue Apr 5 00:00:42 2011 +0200
[krobot_viewer] add bezier curve generation
It is currently commented since it does not work.
commit 2fc3f6a82ab36af5da8237093ded26ca547afa0e
Author: Jérémie Dimino <je...@di...>
Date: Mon Apr 4 16:35:36 2011 +0200
[krobot_viewer] disable antialias for drawing the board
commit 2954ce9c54bcec2b8712e9a5a8842e8d19125809
Author: Jérémie Dimino <je...@di...>
Date: Mon Apr 4 16:11:49 2011 +0200
[krobot_viewer] draw a black circle around the beacon
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/_oasis b/info/control2011/_oasis
index e74d55b..8eb1060 100644
--- a/info/control2011/_oasis
+++ b/info/control2011/_oasis
@@ -130,6 +130,14 @@ Executable "krobot-plot"
MainIs: krobot_plot.ml
BuildDepends: krobot, lwt.syntax, cairo.lablgtk2, lwt.glib, threads
+Executable "krobot-plot-battery"
+ Path: src/tools
+ Build$: flag(gtk)
+ Install$: flag(gtk)
+ CompiledObject: best
+ MainIs: krobot_plot_battery.ml
+ BuildDepends: krobot, lwt.syntax, cairo.lablgtk2, lwt.glib, threads
+
Executable "krobot-viewer"
Path: src/tools
Build$: flag(gtk)
diff --git a/info/control2011/_tags b/info/control2011/_tags
index 2f57243..7c5ae73 100644
--- a/info/control2011/_tags
+++ b/info/control2011/_tags
@@ -2,7 +2,7 @@
<src/interfaces/*.ml>: -syntax_camlp4o
# OASIS_START
-# DO NOT EDIT (digest: 9d40de9cfde679815e27aa44dbe6939f)
+# DO NOT EDIT (digest: 5cd0fff2e5e7303cfc985c8215a456e0)
# Library krobot-interfaces
"src/interfaces": include
"src/interfaces/krobot-interfaces.cmxs": use_krobot-interfaces
@@ -70,6 +70,15 @@
<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-plot-battery
+<src/tools/krobot_plot_battery.{native,byte}>: use_krobot
+<src/tools/krobot_plot_battery.{native,byte}>: use_krobot-interfaces
+<src/tools/krobot_plot_battery.{native,byte}>: pkg_threads
+<src/tools/krobot_plot_battery.{native,byte}>: pkg_obus
+<src/tools/krobot_plot_battery.{native,byte}>: pkg_lwt.unix
+<src/tools/krobot_plot_battery.{native,byte}>: pkg_lwt.syntax
+<src/tools/krobot_plot_battery.{native,byte}>: pkg_lwt.glib
+<src/tools/krobot_plot_battery.{native,byte}>: pkg_cairo.lablgtk2
# 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/setup.ml b/info/control2011/setup.ml
index d3ed546..05b8ab2 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: 698f29e4a2f614788e3b1ec0344c74ff) *)
+(* DO NOT EDIT (digest: ab615aa1d52e40116d1afa35b031733b) *)
(*
Regenerated by OASIS v0.2.0
Visit http://oasis.forge.ocamlcore.org for more information and
@@ -5360,6 +5360,47 @@ let setup_t =
});
Executable
({
+ cs_name = "krobot-plot-battery";
+ cs_data = PropList.Data.create ();
+ cs_plugin_data = [];
+ },
+ {
+ bs_build =
+ [
+ (OASISExpr.EBool true, false);
+ (OASISExpr.EFlag "gtk", true)
+ ];
+ bs_install =
+ [
+ (OASISExpr.EBool true, false);
+ (OASISExpr.EFlag "gtk", true)
+ ];
+ bs_path = "src/tools";
+ bs_compiled_object = Best;
+ bs_build_depends =
+ [
+ InternalLibrary "krobot";
+ FindlibPackage ("lwt.syntax", None);
+ FindlibPackage ("cairo.lablgtk2", None);
+ FindlibPackage ("lwt.glib", None);
+ FindlibPackage ("threads", 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_plot_battery.ml";
+ });
+ Executable
+ ({
cs_name = "krobot-remote";
cs_data = PropList.Data.create ();
cs_plugin_data = [];
diff --git a/info/control2011/src/lib/krobot_geom.ml b/info/control2011/src/lib/krobot_geom.ml
index c5a6941..7f04395 100644
--- a/info/control2011/src/lib/krobot_geom.ml
+++ b/info/control2011/src/lib/krobot_geom.ml
@@ -71,6 +71,13 @@ let vector a b = {
let distance a b =
sqrt (sqr (a.x -. b.x) +. sqr (a.y -. b.y))
+let tangent a b c =
+ let a = vector origin a
+ and b = vector origin b
+ and c = vector origin c in
+ let v = (b -| a) +| (b -| c) in
+ v /| norm v
+
(* +-----------------------------------------------------------------+
| Cubic bezier curves |
+-----------------------------------------------------------------+ *)
@@ -108,8 +115,8 @@ module Bezier = struct
let h2 = 2. *. (h0 *. vs.vx -. g0 *. vs.vy) in
(* The loop for finding d1 and d2. *)
let rec loop d1 d2 =
- let rho_p = 3. *. d1 *. d1 /. (h1 +. d2 *. g1)
- and rho_s = 3. *. d2 *. d2 /. (h2 +. d1 *. g2) in
+ let rho_p = 3. *. sqr d1 /. (h1 +. d2 *. g1)
+ and rho_s = 3. *. sqr d2 /. (h2 +. d1 *. g2) in
let err_1 = r_p -. rho_p and err_2 = r_s -. rho_s in
let error = max (abs_float err_1) (abs_float err_2) in
if error < error_max then
@@ -117,7 +124,7 @@ module Bezier = struct
and r = translate s (vs *| d2) in
of_vertices p q r s
else
- loop (d1 +. err_1 /. r_p) (d2 +. err_2 +. r_s)
+ loop (d1 +. err_1 /. r_p) (d2 +. err_2 /. r_s)
in
loop 1. 1.
diff --git a/info/control2011/src/lib/krobot_geom.mli b/info/control2011/src/lib/krobot_geom.mli
index 748b85e..b1a9936 100644
--- a/info/control2011/src/lib/krobot_geom.mli
+++ b/info/control2011/src/lib/krobot_geom.mli
@@ -35,6 +35,9 @@ val vector : vertice -> vertice -> vector
val norm : vector -> float
val distance : vertice -> vertice -> float
+val tangent : vertice -> vertice -> vertice -> vector
+ (** [tangent a b c] returns the tangent to the triangle abc in b. *)
+
(** {6 Cubic Bezier curves} *)
module Bezier : sig
diff --git a/info/control2011/src/lib/krobot_message.mli b/info/control2011/src/lib/krobot_message.mli
index 059b16a..9324094 100644
--- a/info/control2011/src/lib/krobot_message.mli
+++ b/info/control2011/src/lib/krobot_message.mli
@@ -17,7 +17,7 @@ type t =
| Battery1_voltages of float * float * float * float
(** The voltages of the elements of the first battery *)
| Battery2_voltages of float * float * float * float
- (** The voltages of the elements of the first battery *)
+ (** The voltages of the elements of the second battery *)
| Beacon_position of float * float * float
(** The position of the beacon relative to the robot *)
| Beacon_lowlevel_position of float * float * int
diff --git a/info/control2011/src/tools/krobot_graph.ml b/info/control2011/src/tools/krobot_graph.ml
new file mode 100644
index 0000000..327ae72
--- /dev/null
+++ b/info/control2011/src/tools/krobot_graph.ml
@@ -0,0 +1,80 @@
+(*
+ * krobot_graph.ml
+ * ---------------
+ * Copyright : (c) 2011, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(* +-----------------------------------------------------------------+
+ | Graphs |
+ +-----------------------------------------------------------------+ *)
+
+let graph_duration = 10.0
+ (* Which amount of data to keep in graphs. *)
+
+(* Type of graphs. *)
+type graph = {
+ points : (float * int) Queue.t array;
+ (* Queue of points with their time. They are ordered by increasing
+ date. *)
+ mutable max : int;
+ (* The maximum reached. *)
+}
+
+(* Remove old points. *)
+let update_graph graph time =
+ Array.iter
+ (fun q ->
+ while not (Queue.is_empty q) && fst (Queue.top q) +. graph_duration < time do
+ ignore (Queue.take q)
+ done)
+ graph.points
+
+(* +-----------------------------------------------------------------+
+ | Plotting |
+ +-----------------------------------------------------------------+ *)
+
+let rec colors = (1., 0., 0.) :: (0., 1., 0.) :: (0., 0., 1.) :: (1., 1., 0.) :: colors
+
+let plot ctx width height graph time =
+ Cairo.set_source_rgb ctx 1. 1. 1.;
+ Cairo.rectangle ctx 0. 0. width height;
+ Cairo.fill ctx;
+ let colors = ref colors in
+ Array.iter
+ (fun q ->
+ let r, g, b = List.hd !colors in
+ colors := List.tl !colors;
+ Cairo.set_source_rgb ctx r g b;
+ let prev = ref None in
+ Queue.iter
+ (fun (date, position) ->
+ let x = (date -. (time -. graph_duration)) /. graph_duration *. width
+ and y = height -. height *. (float position /. float graph.max) in
+ match !prev with
+ | None ->
+ prev := Some(x, y)
+ | Some(x', y') ->
+ prev := Some(x, y);
+ Cairo.move_to ctx x' y';
+ Cairo.line_to ctx x y;
+ Cairo.stroke ctx)
+ q)
+ graph.points
+
+(* +-----------------------------------------------------------------+
+ | Drawing |
+ +-----------------------------------------------------------------+ *)
+
+let draw window graph =
+ let { Gtk.width; Gtk.height } = window#misc#allocation in
+ let surface = Cairo.image_surface_create Cairo.FORMAT_ARGB32 width height in
+ let ctx = Cairo.create surface in
+ plot ctx (float width) (float height) graph (Unix.gettimeofday ());
+ let ctx = Cairo_lablgtk.create window#misc#window in
+ Cairo.set_source_surface ctx surface 0. 0.;
+ Cairo.rectangle ctx 0. 0. (float width) (float height);
+ Cairo.fill ctx;
+ Cairo.surface_finish surface
diff --git a/info/control2011/src/tools/krobot_plot_battery.ml b/info/control2011/src/tools/krobot_plot_battery.ml
new file mode 100644
index 0000000..fa79f9b
--- /dev/null
+++ b/info/control2011/src/tools/krobot_plot_battery.ml
@@ -0,0 +1,54 @@
+(*
+ * krobot_plot_battery.ml
+ * ----------------------
+ * Copyright : (c) 2011, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+open Lwt
+open Lwt_react
+open Krobot_message
+
+lwt () =
+ lwt bus = Krobot_bus.get () in
+ ignore (GMain.init ());
+ Lwt_glib.install ();
+
+ let waiter, wakener = wait () in
+
+ (* GTK stuff. *)
+ let window = GWindow.window ~title:"Krobot batteries" () in
+ ignore (window#connect#destroy ~callback:(wakeup wakener));
+ window#show ();
+
+ (* Create the graph. *)
+ let graph = { Krobot_graph.points = Array.init 2 (fun _ -> Queue.create ());
+ Krobot_graph.max = 1 } in
+
+ E.keep
+ (E.map
+ (fun (timestamp, msg) ->
+ match msg with
+ | Battery1_voltages(x1, x2, x3, x4) ->
+ let s = int_of_float ((x1 +. x2 +. x3 +. x4) *. 1000.) in
+ graph.Krobot_graph.max <- max graph.Krobot_graph.max s;
+ Queue.push (timestamp, s) graph.Krobot_graph.points.(0);
+ Krobot_graph.update_graph graph timestamp
+ | Battery2_voltages(x1, x2, x3, x4) ->
+ let s = int_of_float ((x1 +. x2 +. x3 +. x4) *. 1000.) in
+ graph.Krobot_graph.max <- max graph.Krobot_graph.max s;
+ Queue.push (timestamp, s) graph.Krobot_graph.points.(1);
+ Krobot_graph.update_graph graph timestamp
+ | _ ->
+ ())
+ (Krobot_message.recv bus));
+
+ pick [
+ waiter;
+ while_lwt true do
+ Krobot_graph.draw window graph;
+ Lwt_unix.sleep (1. /. 25.)
+ done;
+ ]
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index 6941356..08b3d16 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -10,6 +10,7 @@
open Lwt
open Lwt_react
open Krobot_message
+open Krobot_geom
let utf8 code =
let set_byte s o x = String.unsafe_set s o (Char.unsafe_chr x) in
@@ -181,8 +182,7 @@ module Board = struct
open Krobot_config
type state = {
- x : float;
- y : float;
+ pos : vertice;
theta : float;
}
@@ -197,7 +197,8 @@ module Board = struct
ui : Krobot_viewer_ui.window;
mutable state : state;
mutable beacon : beacon;
- mutable points : (float * float) list;
+ mutable points : vertice list;
+ mutable bezier : vertice array list;
mutable event : unit event;
mutable moving : bool;
}
@@ -235,6 +236,8 @@ module Board = struct
let ctx = Cairo.create surface in
let width = float width and height = float height in
+ Cairo.set_antialias ctx Cairo.ANTIALIAS_NONE;
+
(* Draw the background *)
Cairo.rectangle ctx 0. 0. width height;
set_color ctx White;
@@ -348,6 +351,8 @@ module Board = struct
Cairo.rel_line_to ctx 0.4 0.;
Cairo.stroke ctx;
+ Cairo.set_antialias ctx Cairo.ANTIALIAS_DEFAULT;
+
(* Draw circles on bonus cases *)
Cairo.arc ctx 0.975 0.875 0.05 0. (2. *. pi);
Cairo.fill ctx;
@@ -370,7 +375,7 @@ module Board = struct
Cairo.save ctx;
(* Draw the robot *)
- Cairo.translate ctx board.state.x board.state.y;
+ Cairo.translate ctx board.state.pos.x board.state.pos.y;
Cairo.rotate ctx board.state.theta;
Cairo.rectangle ctx (-. wheels_position) (-. robot_size /. 2.) robot_size robot_size;
set_color ctx White;
@@ -393,12 +398,18 @@ module Board = struct
Cairo.arc ctx board.beacon.xbeacon board.beacon.ybeacon 0.04 0. (2. *. pi);
set_color ctx Purple;
Cairo.fill ctx;
+ Cairo.arc ctx board.beacon.xbeacon board.beacon.ybeacon 0.04 0. (2. *. pi);
+ set_color ctx Black;
+ Cairo.stroke ctx
end;
(* 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.move_to ctx board.state.pos.x board.state.pos.y;
+ List.iter (fun { x; y } -> Cairo.line_to ctx x y) board.points;
+ Cairo.stroke ctx;
+ Cairo.set_source_rgb ctx 255. 0. 255.;
+ List.iter (Array.iter (fun { x; y } -> Cairo.line_to ctx x y)) board.bezier;
Cairo.stroke ctx;
let ctx = Cairo_lablgtk.create board.ui#scene#misc#window in
@@ -418,7 +429,7 @@ module Board = struct
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)];
+ board.points <- board.points @ [{ x; y }];
queue_draw board
end
@@ -432,11 +443,11 @@ module Board = struct
| _ :: l -> last l
let smooth board =
- let points = Array.of_list ((board.state.x, board.state.y) :: board.points) in
+ let points = Array.of_list ({ x = board.state.pos.x; y = board.state.pos.y } :: board.points) 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
+ let { x = x1; y = y1 } = points.(i1) and { x = x2; y = 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
@@ -444,7 +455,7 @@ module Board = struct
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 { 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;
@@ -466,6 +477,33 @@ module Board = struct
in
let result = List.tl (loop [0; Array.length points - 1]); in
board.points <- List.map (fun i -> points.(i)) result;
+(*
+ let speed = board.ui#moving_speed#adjustment#value
+ and acceleration = board.ui#moving_acceleration#adjustment#value in
+
+ (* Compute cubic bezier curves. *)
+ let rec loop = function
+ | p :: (q :: r :: s :: _ as rest) ->
+ (* Compute the speed vectors. *)
+ let v1 = tangent p q r and v2 = tangent q r s in
+ let v1 = { vx = -. v1.vy; vy = v1.vx } *| speed
+ and v2 = { vx = v2.vy; vy = -. v2.vx } *| speed in
+
+ (* Create the bezier curve. *)
+ let curve = Bezier.make ~p:q ~s:r ~vp:v1 ~vs:v2 ~a:acceleration ~error_max:0.1 in
+
+ (* Create vertices. *)
+ let vertices = Array.create 101 origin in
+ for i = 0 to 100 do
+ vertices.(i) <- Bezier.vertice curve (float i /. 100.)
+ done;
+
+ vertices :: loop rest
+ | _ ->
+ []
+ in
+ board.bezier <- [||] :: loop board.points;
+*)
queue_draw board
let wait_done board =
@@ -481,12 +519,12 @@ module Board = struct
let go board =
let rec loop () =
match board.points with
- | (x, y) :: rest ->
+ | { x; y } :: rest ->
let sqr x = x *. x in
let radius = sqrt (sqr (max wheels_position (robot_size -. wheels_position)) +. sqr (robot_size /. 2.)) in
if x >= radius && x <= world_width -. radius && y >= radius && y <= world_height -. radius then begin
(* Turn the robot. *)
- let alpha = math_mod_float (atan2 (y -. board.state.y) (x -. board.state.x) -. board.state.theta) (2. *. pi) in
+ let alpha = math_mod_float (atan2 (y -. board.state.pos.y) (x -. board.state.pos.x) -. board.state.theta) (2. *. pi) in
lwt () = Lwt_log.info_f "turning by %f radiants" alpha in
lwt () = Krobot_message.send board.bus (Unix.gettimeofday (),
Motor_turn(alpha,
@@ -495,7 +533,7 @@ module Board = struct
lwt () = wait_done board in
(* Move the robot. *)
- let dist = sqrt (sqr (x -. board.state.x) +. sqr (y -. board.state.y)) in
+ let dist = sqrt (sqr (x -. board.state.pos.x) +. sqr (y -. board.state.pos.y)) in
lwt () = Lwt_log.info_f "moving by %f meters" dist in
lwt () = Krobot_message.send board.bus (Unix.gettimeofday (),
Motor_move(dist,
@@ -507,6 +545,9 @@ module Board = struct
(match board.points with
| _ :: l -> board.points <- l
| [] -> ());
+ (match board.bezier with
+ | _ :: l -> board.bezier <- l
+ | [] -> ());
(* Redraw everything without the last point. *)
queue_draw board;
@@ -523,13 +564,14 @@ module Board = struct
let board ={
bus;
ui;
- state = {
- x = 0.2;
- y = 1.9 +. Krobot_config.robot_size /. 2. -. Krobot_config.wheels_position;
+ state = {
+ pos = { x = 0.2;
+ y = 1.9 +. Krobot_config.robot_size /. 2. -. Krobot_config.wheels_position };
theta = -0.5 *. pi
};
beacon = { xbeacon = 1.; ybeacon = 1.; valid = true };
points = [];
+ bezier = [];
event = E.never;
moving = false;
} in
@@ -541,7 +583,7 @@ module Board = struct
match frame with
| Odometry(x, y, theta) ->
let angle = math_mod_float (theta) (2. *. pi) in
- let state = { x; y; theta = angle; } in
+ let state = { pos = { x; y }; theta = angle } in
if state <> board.state then begin
board.state <- state;
board.ui#entry_x#set_text (string_of_float x);
@@ -557,8 +599,8 @@ module Board = struct
board.ui#entry_moving4#set_text (if m4 then "yes" else "no")
| Beacon_position(angle, distance, period) ->
let newangle = math_mod_float (board.state.theta +. Krobot_config.rotary_beacon_index_pos +. angle) (2. *. pi) in
- let x = board.state.x +. distance *. cos (newangle) in
- let y = board.state.y +. distance *. sin (newangle) in
+ let x = board.state.pos.x +. distance *. cos (newangle) in
+ let y = board.state.pos.y +. distance *. sin (newangle) in
let valid = distance <> 0. in
let beacon = { xbeacon = x; ybeacon = y; valid; } in
if beacon <> board.beacon then begin
hooks/post-receive
--
krobot
|
|
From: Nicolas D. <Ba...@us...> - 2011-04-04 22:06:12
|
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 dd7c5daac4fc87ee81874bfca82f20efde0a8f92 (commit)
via 7c91035c86358a5eafefe5df31fe77313cee59e8 (commit)
via 949acd815853f6f4a495c9a9f229635e726e7444 (commit)
via f7da3fccd16605b78cebeabb28ab0562fc3b96c8 (commit)
from 92ce3ad8d7d5a762d198226d65c5e3d721f7ecea (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 dd7c5daac4fc87ee81874bfca82f20efde0a8f92
Author: Nicolas Dandrimont <Nic...@cr...>
Date: Tue Apr 5 00:05:29 2011 +0200
[krobot_message] Implement the Battery monitoring messages.
commit 7c91035c86358a5eafefe5df31fe77313cee59e8
Author: Nicolas Dandrimont <Nic...@cr...>
Date: Tue Apr 5 00:03:52 2011 +0200
[USB_CAN] Add the battery monitoring logic to the USB CAN converter
This patch enables battery monitoring on the USB-CAN converter.
Corrseponding CAN packets are sent to the PC and to the CAN bus.
commit 949acd815853f6f4a495c9a9f229635e726e7444
Author: Nicolas Dandrimont <Nic...@cr...>
Date: Tue Apr 5 00:00:44 2011 +0200
[USB_CAN] Group the converter logic in one directory.
commit f7da3fccd16605b78cebeabb28ab0562fc3b96c8
Author: Nicolas Dandrimont <Nic...@cr...>
Date: Mon Apr 4 23:57:16 2011 +0200
[USB_CAN] Upgrade BeRTOS
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/USB_CAN/Firmware/bertos/cfg/cfg_adc.h b/elec/boards/USB_CAN/Firmware/bertos/cfg/cfg_adc.h
index df2587b..62ee6ed 100644
--- a/elec/boards/USB_CAN/Firmware/bertos/cfg/cfg_adc.h
+++ b/elec/boards/USB_CAN/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/USB_CAN/Firmware/bertos/cfg/cfg_dac.h b/elec/boards/USB_CAN/Firmware/bertos/cfg/cfg_dac.h
new file mode 100644
index 0000000..1ae1e98
--- /dev/null
+++ b/elec/boards/USB_CAN/Firmware/bertos/cfg/cfg_dac.h
@@ -0,0 +1,88 @@
+/**
+ * \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
+
+/**
+ * DAC Refresh Period = 1024*REFRESH/DACC Clock
+ *
+ * $WIZ$ type = "int"
+ * $WIZ$ supports = "sam3x"
+ * $WIZ$ min = 0
+ * $WIZ$ max = 65536
+ */
+#define CONFIG_DAC_REFRESH 16
+
+/**
+ * DAC Startup Time Selection.
+ * see datasheet table.
+ *
+ * $WIZ$ type = "int"
+ * $WIZ$ supports = "sam3x"
+ * $WIZ$ min = 0
+ * $WIZ$ max = 63
+ */
+#define CONFIG_DAC_STARTUP 0
+
+/**
+ * DAC Trigger Selection.
+ *
+ * $WIZ$ type = "enum"
+ * $WIZ$ value_list = "sam3x_dac_tc"
+ * $WIZ$ supports = "sam3x"
+ */
+#define CONFIG_DAC_TIMER DACC_TRGSEL_TIO_CH0
+
+#endif /* CFG_DAC_H */
diff --git a/elec/boards/USB_CAN/Firmware/bertos/cfg/cfg_lwip.h b/elec/boards/USB_CAN/Firmware/bertos/cfg/cfg_lwip.h
index b6b55f6..73acb91 100644
--- a/elec/boards/USB_CAN/Firmware/bertos/cfg/cfg_lwip.h
+++ b/elec/boards/USB_CAN/Firmware/bertos/cfg/cfg_lwip.h
@@ -1206,6 +1206,13 @@
* $WIZ$ type = "boolean"
*/
#define LWIP_SOCKET 1
+#if LWIP_SOCKET
+ /*
+ * The sockets.c file requires this macro to be defined to really
+ * set errno on errors.
+ */
+ #define ERRNO
+#endif
/**
* Enable BSD-style sockets functions names.
diff --git a/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/drv/adc_sam3.c b/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/drv/adc_sam3.c
new file mode 100644
index 0000000..303689f
--- /dev/null
+++ b/elec/boards/USB_CAN/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/USB_CAN/Firmware/bertos/cpu/cortex-m3/drv/adc_sam3.h b/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/drv/adc_sam3.h
new file mode 100644
index 0000000..f46f940
--- /dev/null
+++ b/elec/boards/USB_CAN/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/USB_CAN/Firmware/bertos/cpu/cortex-m3/drv/dac_cm3.h b/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/drv/dac_cm3.h
new file mode 100644
index 0000000..3cebff7
--- /dev/null
+++ b/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/drv/dac_cm3.h
@@ -0,0 +1,46 @@
+/**
+ * \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 Low-level DAC module for Cortex-m3.
+ *
+ * \author Daniele Basile <as...@de...>
+ *
+ */
+
+#include <cpu/detect.h>
+
+#if CPU_CM3_SAM3X
+ #include "adc_sam3.h"
+/*#elif Add other ARM families here */
+#else
+ #error Unknown CPU
+#endif
diff --git a/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/drv/dac_sam3.c b/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/drv/dac_sam3.c
new file mode 100644
index 0000000..b10ff62
--- /dev/null
+++ b/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/drv/dac_sam3.c
@@ -0,0 +1,238 @@
+/**
+ * \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 "dac_sam3.h"
+
+#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 <cpu/types.h>
+
+#include <io/cm3.h>
+
+#include <string.h>
+
+struct DacHardware
+{
+ uint16_t channels;
+ uint32_t rate;
+ bool end;
+};
+
+struct DacHardware dac_hw;
+
+#if CONFIG_DAC_TIMER == DACC_TRGSEL_TIO_CH0 /* Select Timer counter TIO Channel 0 */
+ #define DAC_TC_ID TC0_ID
+ #define DAC_TC_CCR TC0_CCR0
+ #define DAC_TC_IDR TC0_IDR0
+ #define DAC_TC_CMR TC0_CMR0
+ #define DAC_TC_SR TC0_SR0
+ #define DAC_TC_RA TC0_RA0
+ #define DAC_TC_RC TC0_RC0
+#elif CONFIG_DAC_TIMER == DACC_TRGSEL_TIO_CH1 /* Select Timer counter TIO Channel 1 */
+ #define DAC_TC_ID TC1_ID
+ #define DAC_TC_CCR TC0_CCR1
+ #define DAC_TC_IDR TC0_IDR1
+ #define DAC_TC_CMR TC0_CMR1
+ #define DAC_TC_SR TC0_SR1
+ #define DAC_TC_RA TC0_RA1
+ #define DAC_TC_RC TC0_RC1
+#elif CONFIG_DAC_TIMER == DACC_TRGSEL_TIO_CH2 /* Select Timer counter TIO Channel 2 */
+ #define DAC_TC_ID TC2_ID
+ #define DAC_TC_CCR TC0_CCR2
+ #define DAC_TC_IDR TC0_IDR2
+ #define DAC_TC_CMR TC0_CMR2
+ #define DAC_TC_SR TC0_SR2
+ #define DAC_TC_RA TC0_RA2
+ #define DAC_TC_RC TC0_RC2
+#elif CONFIG_DAC_TIMER == DACC_TRGSEL_PWM0 || CONFIG_DAC_TIMER == DACC_TRGSEL_PWM1
+ #error unimplemented pwm triger select.
+#endif
+
+INLINE void tc_setup(uint32_t freq, size_t n_sample)
+{
+ pmc_periphEnable(DAC_TC_ID);
+
+ /* Disable TC clock */
+ DAC_TC_CCR = TC_CCR_CLKDIS;
+ /* Disable interrupts */
+ DAC_TC_IDR = 0xFFFFFFFF;
+ /* Clear status register */
+ volatile uint32_t dummy = DAC_TC_SR;
+ (void)dummy;
+
+ /*
+ * Setup the timer counter:
+ * - select clock TCLK1 (MCK/2)
+ * - enable wave form mode
+ * - RA compare effect SET
+ * - RC compare effect CLEAR
+ * - UP mode with automatic trigger on RC Compare
+ */
+ DAC_TC_CMR = TC_TIMER_CLOCK1 | BV(TC_CMR_WAVE) | TC_CMR_ACPA_SET | TC_CMR_ACPC_CLEAR | BV(TC_CMR_CPCTRG);
+
+ /*
+ * Compute the sample frequency
+ * the RC counter will update every MCK/2 (see above)
+ * so to convert one sample at the user freq we generate
+ * the trigger every TC_CLK / (numer_of_sample * user_freq)
+ * where TC_CLK = MCK / 2.
+ */
+ uint32_t rc = DIV_ROUND((CPU_FREQ / 2), n_sample * freq);
+ DAC_TC_RC = rc;
+ /* generate the square wave with duty = 50% */
+ DAC_TC_RA = DIV_ROUND(50 * rc, 100);
+
+ PIOB_PDR = BV(25);
+ PIO_PERIPH_SEL(PIOB_BASE, BV(25), PIO_PERIPH_B);
+}
+
+INLINE void tc_start(void)
+{
+ DAC_TC_CCR = BV(TC_CCR_CLKEN)| BV(TC_CCR_SWTRG);
+}
+
+INLINE void tc_stop(void)
+{
+ DAC_TC_CCR = BV(TC_CCR_CLKDIS);
+}
+
+static int sam3x_dac_write(struct Dac *dac, unsigned channel, uint16_t sample)
+{
+ (void)dac;
+
+ ASSERT(channel <= DAC_MAXCH);
+
+ DACC_MR |= (channel << DACC_USER_SEL_SHIFT) & DACC_USER_SEL_MASK;
+ DACC_CHER |= BV(channel);
+
+ DACC_CDR = sample ;
+
+ return 0;
+}
+
+static void sam3x_dac_setCh(struct Dac *dac, uint32_t mask)
+{
+ /* we have only the ch0 and ch1 */
+ ASSERT(mask < BV(3));
+ dac->hw->channels = mask;
+}
+
+static void sam3x_dac_setSampleRate(struct Dac *dac, uint32_t rate)
+{
+ (void)dac;
+
+ /* Eneble hw trigger */
+ DACC_MR |= BV(DACC_TRGEN) | (CONFIG_DAC_TIMER << DACC_TRGSEL_SHIFT);
+ dac->hw->rate = rate;
+}
+
+static void sam3x_dac_conversion(struct Dac *dac, void *buf, size_t len)
+{
+ if (dac->hw->channels & BV(DACC_CH0))
+ DACC_MR |= (DACC_CH0 << DACC_USER_SEL_SHIFT) & DACC_USER_SEL_MASK;
+
+ if (dac->hw->channels & BV(DACC_CH1))
+ DACC_MR |= (DACC_CH1 << DACC_USER_SEL_SHIFT) & DACC_USER_SEL_MASK;
+
+ DACC_CHER |= dac->hw->channels;
+
+ /* setup timer and start it */
+ tc_setup(dac->hw->rate, len);
+ tc_start();
+
+ /* Setup dma and start it */
+ DACC_TPR = (uint32_t)buf ;
+ DACC_TCR = len;
+ DACC_PTCR |= BV(DACC_PTCR_TXTEN);
+}
+
+static bool sam3x_dac_isFinished(struct Dac *dac)
+{
+ (void)dac;
+ return 0;
+}
+
+static void sam3x_dac_start(struct Dac *dac, void *buf, size_t len, size_t slice_len)
+{
+ (void)dac;
+ (void)buf;
+ (void)len;
+ (void)slice_len;
+}
+
+static void sam3x_dac_stop(struct Dac *dac)
+{
+ (void)dac;
+}
+
+
+void dac_init(struct Dac *dac)
+{
+
+ /* Fill the virtual table */
+ dac->ctx.write = sam3x_dac_write;
+ dac->ctx.setCh = sam3x_dac_setCh;
+ dac->ctx.setSampleRate = sam3x_dac_setSampleRate;
+ dac->ctx.conversion = sam3x_dac_conversion;
+ dac->ctx.isFinished = sam3x_dac_isFinished;
+ dac->ctx.start = sam3x_dac_start;
+ dac->ctx.stop = sam3x_dac_stop;
+ dac->ctx._type = DAC_SAM3X;
+ dac->hw = &dac_hw;
+
+ /* Clock DAC peripheral */
+ pmc_periphEnable(DACC_ID);
+
+ /* Reset hw */
+ DACC_CR |= BV(DACC_SWRST);
+ DACC_MR = 0;
+
+ /* Configure the dac */
+ DACC_MR |= (CONFIG_DAC_REFRESH << DACC_REFRESH_SHIFT) & DACC_REFRESH_MASK;
+ DACC_MR |= (CONFIG_DAC_STARTUP << DACC_STARTUP_SHIFT) & DACC_STARTUP_MASK;
+}
diff --git a/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/drv/dac_sam3.h b/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/drv/dac_sam3.h
new file mode 100644
index 0000000..e5d68a2
--- /dev/null
+++ b/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/drv/dac_sam3.h
@@ -0,0 +1,51 @@
+/**
+ * \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 definition
+ *
+ * \author Daniele Basile <as...@de...>
+ */
+
+#ifndef DRV_DAC_SAM3_H
+#define DRV_DAC_SAM3_H
+
+#include <drv/dac.h>
+
+/**
+ * DAC config define.
+ */
+#define DAC_MAXCH 2 //Max number of channel for ADC.
+#define DAC_BITS 12 //Bit resolution for ADC converter.
+
+#define DAC_SAM3X MAKE_ID('S', 'D', '3', 'X')
+
+#endif /* DRV_DAC_SAM3_H */
diff --git a/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/drv/eth_sam3.c b/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/drv/eth_sam3.c
index 889a5c1..0247f61 100644
--- a/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/drv/eth_sam3.c
+++ b/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/drv/eth_sam3.c
@@ -77,7 +77,7 @@
*
* TODO: make this paramater user-configurable from the Wizard.
*/
-const uint8_t mac_addr[] = { 0x00, 0x23, 0x54, 0x6a, 0x77, 0x55 };
+const uint8_t mac_addr[] = { 0x00, 0x45, 0x56, 0x78, 0x9a, 0xbc };
/* Silent Doxygen bug... */
#ifndef __doxygen__
@@ -109,12 +109,9 @@ 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;
@@ -123,12 +120,7 @@ static DECLARE_ISR(emac_irqHandler)
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;
@@ -141,11 +133,13 @@ static DECLARE_ISR(emac_irqHandler)
*
* \return Contents of the specified register.
*/
-static uint16_t phy_hw_read(reg8_t reg)
+static uint16_t phy_hw_read(uint8_t phy_addr, reg8_t reg)
{
// PHY read command.
- EMAC_MAN = EMAC_SOF | EMAC_RW_READ | (NIC_PHY_ADDR << EMAC_PHYA_SHIFT)
- | ((reg << EMAC_REGA_SHIFT) & EMAC_REGA) | EMAC_CODE;
+ 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)))
@@ -155,46 +149,105 @@ static uint16_t phy_hw_read(reg8_t reg)
return (uint16_t)(EMAC_MAN & EMAC_DATA);
}
+#if 0
/*
* \brief Write value to PHY register.
*
* \param reg PHY register number.
* \param val Value to write.
*/
-static void phy_hw_write(reg8_t reg, uint16_t val)
+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 | (NIC_PHY_ADDR << EMAC_PHYA_SHIFT)
- | ((reg << EMAC_REGA_SHIFT) & EMAC_REGA) | EMAC_CODE | val;
+ 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();
}
+#endif
-static int emac_reset(void)
+/*
+ * Check link speed and duplex as negotiated by the PHY
+ * and configure CPU EMAC accordingly.
+ * Requires active PHY maintenance mode.
+ */
+static void emac_autoNegotiation(void)
{
- uint16_t phy_cr;
+ uint16_t reg;
+ time_t start;
+
+ // Wait for auto-negotation to complete
+ start = timer_clock();
+ do {
+ reg = phy_hw_read(NIC_PHY_ADDR, NIC_PHY_BMSR);
+ if (timer_clock() - start > 2000)
+ {
+ kprintf("eth error: auto-negotiation timeout\n");
+ return;
+ }
+ }
+ while (!(reg & NIC_PHY_BMSR_ANCOMPL));
+
+ reg = phy_hw_read(NIC_PHY_ADDR, NIC_PHY_ANLPAR);
+
+ if ((reg & NIC_PHY_ANLPAR_TX_FDX) || (reg & NIC_PHY_ANLPAR_TX_HDX))
+ {
+ LOG_INFO("eth: 100BASE-TX\n");
+ EMAC_NCFGR |= BV(EMAC_SPD);
+ }
+ else
+ {
+ LOG_INFO("eth: 10BASE-T\n");
+ EMAC_NCFGR &= ~BV(EMAC_SPD);
+ }
+
+ if ((reg & NIC_PHY_ANLPAR_TX_FDX) || (reg & NIC_PHY_ANLPAR_10_FDX))
+ {
+ LOG_INFO("eth: full duplex\n");
+ EMAC_NCFGR |= BV(EMAC_FD);
+ }
+ else
+ {
+ LOG_INFO("eth: half duplex\n");
+ EMAC_NCFGR &= ~BV(EMAC_FD);
+ }
+}
+
+static int emac_reset(void)
+{
+#if CPU_ARM_AT91
// 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);
+ PMC_PCER = BV(PIOA_ID);
+ PMC_PCER = BV(PIOB_ID);
+ PMC_PCER = BV(EMAC_ID);
- // Disable TESTMODE
+ // Disable TESTMODE and RMII
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);
+#else
+ pmc_periphEnable(PIOA_ID);
+ pmc_periphEnable(PIOB_ID);
+ pmc_periphEnable(PIOC_ID);
+ pmc_periphEnable(PIOD_ID);
+ pmc_periphEnable(EMAC_ID);
+
+ // Disable TESTMODE and RMII
+ PIOC_PUDR = BV(PHY_RXDV_TESTMODE_BIT);
+
+ // Disable PHY power down.
+ PIOD_PER = BV(PHY_PWRDN_BIT);
+ PIOD_OER = BV(PHY_PWRDN_BIT);
+ PIOD_CODR = BV(PHY_PWRDN_BIT);
#endif
// Toggle external hardware reset pin.
@@ -209,48 +262,30 @@ static int emac_reset(void)
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;
+ PIO_PERIPH_SEL(PIOB_BASE, PHY_MII_PINS_PORTB, PIO_PERIPH_A);
+ PIOB_PDR = PHY_MII_PINS_PORTB;
+
+ PIO_PERIPH_SEL(PIOC_BASE, PHY_MII_PINS_PORTC, PIO_PERIPH_A);
+ PIOC_PDR = PHY_MII_PINS_PORTC;
+
// 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_32;
+ 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);
-
- // Clear MII isolate.
- phy_hw_read(NIC_PHY_BMCR);
- phy_cr = phy_hw_read(NIC_PHY_BMCR);
-
- phy_cr &= ~NIC_PHY_BMCR_ISOLATE;
- phy_hw_write(NIC_PHY_BMCR, phy_cr);
-
- phy_cr = phy_hw_read(NIC_PHY_BMCR);
-
- LOG_INFO("%s: PHY ID %#04x %#04x\n",
- __func__,
- phy_hw_read(NIC_PHY_ID1), phy_hw_read(NIC_PHY_ID2));
-
- // Wait for auto negotiation completed.
- phy_hw_read(NIC_PHY_BMSR);
- for (;;)
- {
- if (phy_hw_read(NIC_PHY_BMSR) & NIC_PHY_BMSR_ANCOMPL)
- break;
- cpu_relax();
- }
+ emac_autoNegotiation();
// Disable management port.
EMAC_NCR &= ~BV(EMAC_MPE);
@@ -258,6 +293,7 @@ static int emac_reset(void)
return 0;
}
+
static int emac_start(void)
{
uint32_t addr;
diff --git a/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/drv/eth_sam3.h b/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/drv/eth_sam3.h
index f733df9..a756ffc 100644
--- a/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/drv/eth_sam3.h
+++ b/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/drv/eth_sam3.h
@@ -42,9 +42,9 @@
// Settings and definition for DAVICOM 9161A
// \{
-#define NIC_PHY_ADDR 31
+#define NIC_PHY_ADDR 0
-//Registry definition
+// Register bits definition
#define NIC_PHY_BMCR 0x00 // Basic mode control register.
#define NIC_PHY_BMCR_COLTEST 0x0080 // Collision test.
#define NIC_PHY_BMCR_FDUPLEX 0x0100 // Full duplex mode.
@@ -61,6 +61,11 @@
#define NIC_PHY_BMSR_ANEGCAPABLE 0x0008 // Able to do auto-negotiation
#define NIC_PHY_BMSR_LINKSTAT 0x0004 // Link status.
+#define NIC_PHY_ANLPAR_10_HDX BV(5) // 10BASE-T half duplex
+#define NIC_PHY_ANLPAR_10_FDX BV(6) // 10BASE-T full duplex
+#define NIC_PHY_ANLPAR_TX_HDX BV(7) // 100BASE-TX half duplex
+#define NIC_PHY_ANLPAR_TX_FDX BV(8) // 100BASE-TX full duplex
+
#define NIC_PHY_ID1 0x02 // PHY identifier register 1.
#define NIC_PHY_ID2 0x03 // PHY identifier register 2.
#define NIC_PHY_ANAR 0x04 // Auto negotiation advertisement register.
@@ -74,7 +79,6 @@
* See schematics for AT91SAM7X-EK evalution board.
*/
// All pins in port B
-#define PHY_TXCLK_ISOLATE_BIT 0
#define PHY_REFCLK_XT2_BIT 0
#define PHY_TXEN_BIT 1
#define PHY_TXD0_BIT 2
@@ -123,32 +127,36 @@
* See schematics for SAM3X-EK evalution board.
*/
// Port B
-#define PHY_TXCLK_ISOLATE_BIT 0
#define PHY_REFCLK_XT2_BIT 0
#define PHY_TXEN_BIT 1
#define PHY_TXD0_BIT 2
#define PHY_TXD1_BIT 3
-#define PHY_RXDV_TESTMODE_BIT 4
#define PHY_RXD0_AD0_BIT 5
#define PHY_RXD1_AD1_BIT 6
#define PHY_RXER_RXD4_RPTR_BIT 7
#define PHY_MDC_BIT 8
#define PHY_MDIO_BIT 9
+// Port C
+#define PHY_RXDV_TESTMODE_BIT 10
// Port A
#define PHY_MDINTR_BIT 5
+// Port D -- FIXME: Only on which revision?
+#define PHY_PWRDN_BIT 18
-#define PHY_MII_PINS \
+#define PHY_MII_PINS_PORTB \
BV(PHY_REFCLK_XT2_BIT) \
| BV(PHY_TXEN_BIT) \
| BV(PHY_TXD0_BIT) \
| BV(PHY_TXD1_BIT) \
- | BV(PHY_RXDV_TESTMODE_BIT) \
| BV(PHY_RXD0_AD0_BIT) \
| BV(PHY_RXD1_AD1_BIT) \
| BV(PHY_RXER_RXD4_RPTR_BIT) \
| BV(PHY_MDC_BIT) \
| BV(PHY_MDIO_BIT)
+#define PHY_MII_PINS_PORTC \
+ BV(PHY_RXDV_TESTMODE_BIT)
+
#endif /* CPU_ARM_AT91 */
// \}
diff --git a/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/info/SAM3N4.cdef b/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/info/SAM3N4.cdef
index 47f4c70..e13cc3e 100644
--- a/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/info/SAM3N4.cdef
+++ b/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/info/SAM3N4.cdef
@@ -51,7 +51,7 @@ MK_FLASH_SCRIPT = PRG_SCRIPTS_DIR + "arm/flash-sam3.sh"
CPU_DEFAULT_FREQ = "48000000UL"
# Special CPU related tags.
-CPU_TAGS += ["sam3"]
+CPU_TAGS += ["sam3n"]
# Additional hw drivers.
MK_CPU_CSRC += DRV_DIR + "clock_sam3.c "
diff --git a/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/info/SAM3X8.cdef b/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/info/SAM3X8.cdef
index dfd9a87..b64fb18 100644
--- a/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/info/SAM3X8.cdef
+++ b/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/info/SAM3X8.cdef
@@ -51,7 +51,7 @@ MK_FLASH_SCRIPT = PRG_SCRIPTS_DIR + "arm/flash-sam3.sh"
CPU_DEFAULT_FREQ = "84000000UL"
# Special CPU related tags.
-CPU_TAGS += ["sam3"]
+CPU_TAGS += [ "sam3", "sam3x" ]
# Additional hw drivers.
MK_CPU_CSRC += DRV_DIR + "clock_sam3.c "
diff --git a/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/io/sam3.h b/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/io/sam3.h
index 9437c61..59931f9 100644
--- a/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/io/sam3.h
+++ b/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/io/sam3.h
@@ -170,6 +170,8 @@
#include "sam3_emac.h"
#include "sam3_rstc.h"
#include "sam3_adc.h"
+#include "sam3_dacc.h"
+#include "sam3_tc.h"
/**
* U(S)ART I/O pins
diff --git a/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/io/sam3_adc.h b/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/io/sam3_adc.h
index 2545f23..30bac49 100644
--- a/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/io/sam3_adc.h
+++ b/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/io/sam3_adc.h
@@ -34,6 +34,8 @@
*
* SAM3 Analog to Digital Converter.
*
+ * $WIZ$
+ *
*/
@@ -64,12 +66,12 @@
#define ADC_TRGSEL_TIOA0 0x00000000 ///< TIOA output of the timer counter channel 0.
#define ADC_TRGSEL_TIOA1 0x00000002 ///< TIOA output of the timer counter channel 1.
#define ADC_TRGSEL_TIOA2 0x00000004 ///< TIOA output of the timer counter channel 2.
-#define ADC_TRGSEL_PWM0 0x0000000A ///< PWM Event Line 0.
-#define ADC_TRGSEL_PWM1 0x0000000C ///< PWM Event Line 1.
+#define ADC_TRGSEL_PWM0 0x0000000A ///< PWM Event Line 0.
+#define ADC_TRGSEL_PWM1 0x0000000C ///< PWM Event Line 1.
#define ADC_LOWRES 4 ///< Resolution 0: 12-bit, 1: 10-bit.
#define ADC_SLEEP 5 ///< Sleep mode.
-
+#define ADC_FREERUN 7 ///< Freerun.
/**
* Prescaler rate selection.
@@ -81,37 +83,59 @@
/**
* Start up timer.
*/
-#define ADC_STARTUP_MASK 0x000F0000 ///< Start up timer mask.
-#define ADC_STARTUP_SHIFT 16 ///< Start up timer shift.
-
-#define ADC_SUT0 0 ///< 0 period of ADCClock.
-#define ADC_SUT8 1 ///< 8 period of ADCClock.
-#define ADC_SUT16 2 ///< 16 period of ADCClock.
-#define ADC_SUT24 3 ///< 24 period of ADCClock.
-#define ADC_SUT64 4 ///< 64 period of ADCClock.
-#define ADC_SUT80 5 ///< 80 period of ADCClock.
-#define ADC_SUT96 6 ///< 96 period of ADCClock.
-#define ADC_SUT112 7 ///< 112 period of ADCClock.
-#define ADC_SUT512 8 ///< 512 period of ADCClock.
-#define ADC_SUT576 9 ///< 576 period of ADCClock.
+#define ADC_STARTUP_MASK 0x000F0000 ///< Start up timer mask.
+#define ADC_STARTUP_SHIFT 16 ///< Start up timer shift.
+
+/**
+ * Start up timer.
+ * $WIZ$ sam3_adc_sut = "ADC_SUT0","ADC_SUT8","ADC_SUT16","ADC_SUT24","ADC_SUT64","DC_SUT80","ADC_SUT96","ADC_SUT112","ADC_SUT512","ADC_SUT576","ADC_SUT640","ADC_SUT704","ADC_SUT768","ADC_SUT832","ADC_SUT896","ADC_SUT960"
+ * \{
+ */
+#define ADC_SUT0 0 ///< 0 period of ADCClock.
+#define ADC_SUT8 1 ///< 8 period of ADCClock.
+#define ADC_SUT16 2 ///< 16 period of ADCClock.
+#define ADC_SUT24 3 ///< 24 period of ADCClock.
+#define ADC_SUT64 4 ///< 64 period of ADCClock.
+#define ADC_SUT80 5 ///< 80 period of ADCClock.
+#define ADC_SUT96 6 ///< 96 period of ADCClock.
+#define ADC_SUT112 7 ///< 112 period of ADCClock.
+#define ADC_SUT512 8 ///< 512 period of ADCClock.
+#define ADC_SUT576 9 ///< 576 period of ADCClock.
#define ADC_SUT640 10 ///< 640 period of ADCClock.
#define ADC_SUT704 11 ///< 704 period of ADCClock.
#define ADC_SUT768 12 ///< 768 period of ADCClock.
#define ADC_SUT832 13 ///< 832 period of ADCClock.
#define ADC_SUT896 14 ///< 896 period of ADCClock.
#define ADC_SUT960 15 ///< 896 period of ADCClock.
+/** \} */
/**
- * Sample & hold time.
- */
-#define ADC_SHTIME_MASK 0x0F000000 ///< Sample & hold time mask.
-#define ADC_SHTIME_SHIFT 20 ///< Sample & hold time shift.
-#define ADC_AST3 0 ///< 3 period of ADCClock
-#define ADC_AST5 1 ///< 5 period of ADCClock
-#define ADC_AST9 2 ///< 9 period of ADCClock
-#define ADC_AST17 3 ///< 17 period of ADCClock
+ * Analog Settling Time.
+ * $WIZ$ sam3_adc_stt = "ADC_AST3", "ADC_AST5", "ADC_AST9", "ADC_AST17"
+ */
+#define ADC_SETTLING_MASK 0x00300000 ///< Analog Settling Time mask.
+#define ADC_SETTLING_SHIFT 20 ///< Analog Settling Time shift.
+#define ADC_AST3 0 ///< 3 period of ADCClock
+#define ADC_AST5 1 ///< 5 period of ADCClock
+#define ADC_AST9 2 ///< 9 period of ADCClock
+#define ADC_AST17 3 ///< 17 period of ADCClock
+
+/**
+ * Tracking Time.
+ * Tracking Time = (TRACKTIM + 1) * ADCClock periods.
+ */
+#define ADC_TRACKTIM_MASK 0x0F000000 ///< Tracking Time mask.
+#define ADC_TRACKTIM_SHIFT 24 ///< Tracking Time shift.
+
+/**
+ * Transfer Period.
+ * Transfer Period = (TRANSFER * 2 + 3) ADCClock periods.
+ */
+#define ADC_TRANSFER_MASK 0x30000000 ///< Transfer Period mask.
+#define ADC_TRANSFER_SHIFT 28 ///< Transfer Period shift.
/* \} */
+
/**
* ADC channel enable register
*/
@@ -130,6 +154,14 @@
#define ADC_CHSR_OFF 0x00000018 ///< Channel status register offeset.
#define ADC_CHSR (*((reg32_t *)(ADC_BASE + ADC_CHSR_OFF))) ///< Channel status register address.
+
+/**
+ * ADC status register
+ */
+#define ADC_SR_OFF 0x0000001C ///< Status register offeset.
+#define ADC_SR (*((reg32_t *)(ADC_BASE + ADC_SR_OFF))) ///< Status register address.
+
+
#define ADC_CH_MASK 0x000000FF ///< Channel mask.
#define ADC_CH0 0 ///< Channel 0
#define ADC_CH1 1 ///< Channel 1
@@ -191,10 +223,32 @@
/**
* ADC last convert data register.
+ * \{
*/
#define ADC_LCDR_OFF 0x00000020 ///< Last converted data register offeset.
#define ADC_LCDR (*((reg32_t *)(ADC_BASE + ADC_LCDR_OFF))) ///< Last converted RAW data register.
#define ADC_LDATA (ADC_LCDR & 0xFFF) ///< Last data converted register.
#define ADC_CHNB ((ADC_LCDR & 0xF000) >> 12) ///< Channel number.
+/* \} */
+
+
+/**
+ * ADC Channel data register.
+ * \{
+ */
+#define ADC_CDR_OFF 0x00000050 ///< Channel data register offeset.
+#define ADC_CDR (*((reg32_t *)(ADC_BASE + ADC_CDR_OFF))) ///< Channel data register.
+/* \} */
+
+
+/**
+ * ADC Analog Control register.
+ * \{
+ */
+#define ADC_ACR_OFF 0x00000094 ///< Analog control register offeset.
+#define ADC_ACR (*((reg32_t *)(ADC_BASE + ADC_ACR_OFF))) ///< Analog control register.
+#define ADC_TSON 4 ///< Temperature Sensor On.
+#define ADC_TEMPERATURE_CH 15 ///< Channel where is the internal sensor temperature
+/* \} */
#endif /* SAM3_ADC_H */
diff --git a/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/io/sam3_dacc.h b/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/io/sam3_dacc.h
new file mode 100644
index 0000000..0848a3b
--- /dev/null
+++ b/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/io/sam3_dacc.h
@@ -0,0 +1,225 @@
+/**
+ * \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/
+ *
+ * -->
+ *
+ * \author Daniele Basile <as...@de...>
+ *
+ * SAM3 Digital to Analog to Converter.
+ *
+ * $WIZ$
+ */
+
+
+#ifndef SAM3_DACC_H
+#define SAM3_DACC_H
+
+/** DACC registers base. */
+#define DACC_BASE 0x400C8000
+
+/**
+ * DACC control register
+ * \{
+ */
+#define DACC_CR_OFF 0x00000000 ///< Control register offeset.
+#define DACC_CR (*((reg32_t*)(DACC_BASE + DACC_CR_OFF))) ///< Control register address.
+#define DACC_SWRST 0 ///< Software reset.
+/* \} */
+
+/**
+ * DACC mode register
+ * \{
+ */
+#define DACC_MR_OFF 0x00000004 ///< Mode register offeset.
+#define DACC_MR (*((reg32_t*) (DACC_BASE + DACC_MR_OFF))) ///< Mode register address.
+#define DACC_TRGEN 0 ///< Trigger enable.
+#define DACC_TRGSEL_MASK 0x14 ///< Trigger selection mask.
+#define DACC_TRGSEL_SHIFT 1 ///< Trigger selection shift.
+#define DACC_WORD 4 ///< Word transfer.
+#define DACC_SLEEP 5 ///< Sleep mode.Fast Wake up Mode
+#define DACC_FASTWKUP 6 ///< Fast Wake up Mode
+#define DACC_REFRESH_MASK 0xFF00 ///< Refresh Period mask
+#define DACC_REFRESH_SHIFT 8 ///< Refresh Period shift
+#define DACC_USER_SEL_MASK 0x30000 ///< User Channel Selection mask
+#define DACC_USER_SEL_SHIFT 16 ///< User Channel Selection shift
+#define DACC_TAG 20 ///< Tag selection mode
+#define DACC_MAXS 21 ///< Max speed mode
+#define DACC_STARTUP_MASK 0x3F000000 ///< Startup time selection
+#define DACC_STARTUP_SHIFT 24 ///< Startup time selsection shift
+
+
+/**
+ * Trigger selection.
+ * $WIZ$ sam3x_dac_tc = "DACC_TRGSEL_TIO_CH0", "DACC_TRGSEL_TIO_CH1", "DACC_TRGSEL_TIO_CH2", "DACC_TRGSEL_PWM0", "DACC_TRGSEL_PWM1"
+ * \{
+ */
+ #define DACC_TRGSEL_TIO_CH0 1
+ #define DACC_TRGSEL_TIO_CH1 2
+ #define DACC_TRGSEL_TIO_CH2 3
+ #define DACC_TRGSEL_PWM0 4
+ #define DACC_TRGSEL_PWM1 5
+/* \} */
+
+#define DACC_MR_STARTUP_0 0 ///< 0 periods of DACClock
+#define DACC_MR_STARTUP_8 1 ///< 8 periods of DACClock
+#define DACC_MR_STARTUP_16 2 ///< 16 periods of of DACClock
+#define DACC_MR_STARTUP_24 3 ///< 24 periods of of DACClock
+#define DACC_MR_STARTUP_64 4 ///< 64 periods of of DACClock
+#define DACC_MR_STARTUP_80 5 ///< 70 periods of of DACClock
+#define DACC_MR_STARTUP_96 6 ///< 96 periods of of DACClock
+#define DACC_MR_STARTUP_112 7 ///< 112 periods of of DACClock
+#define DACC_MR_STARTUP_512 8 ///< 512 periods of DACClock
+#define DACC_MR_STARTUP_576 9 ///< 576 periods of DACClock
+#define DACC_MR_STARTUP_640 10 ///< 640 periods of DACClock
+#define DACC_MR_STARTUP_704 11 ///< 704 periods of DACClock
+#define DACC_MR_STARTUP_768 12 ///< 768 periods of DACClock
+#define DACC_MR_STARTUP_832 13 ///< 832 periods of DACClock
+#define DACC_MR_STARTUP_896 14 ///< 896 periods of DACClock
+#define DACC_MR_STARTUP_960 15 ///< 960 periods of DACClock
+#define DACC_MR_STARTUP_1024 16 ///< 1024 periods of DACClock
+#define DACC_MR_STARTUP_1088 17 ///< 1088 periods of DACClock
+#define DACC_MR_STARTUP_1152 18 ///< 1152 periods of DACClock
+#define DACC_MR_STARTUP_1216 19 ///< 1216 periods of DACClock
+#define DACC_MR_STARTUP_1280 20 ///< 1280 periods of DACClock
+#define DACC_MR_STARTUP_1344 21 ///< 1344 periods of DACClock
+#define DACC_MR_STARTUP_1408 22 ///< 1408 periods of DACClock
+#define DACC_MR_STARTUP_1472 23 ///< 1472 periods of DACClock
+#define DACC_MR_STARTUP_1536 24 ///< 1536 periods of DACClock
+#define DACC_MR_STARTUP_1600 25 ///< 1600 periods of DACClock
+#define DACC_MR_STARTUP_1664 26 ///< 1664 periods of DACClock
+#define DACC_MR_STARTUP_1728 27 ///< 1728 periods of DACClock
+#define DACC_MR_STARTUP_1792 28 ///< 1792 periods of DACClock
+#define DACC_MR_STARTUP_1856 29 ///< 1856 periods of DACClock
+#define DACC_MR_STARTUP_1920 30 ///< 1920 periods of DACClock
+#define DACC_MR_STARTUP_1984 31 ///< 1984 periods of DACClock
+/* \} */
+
+/**
+ * DACC channel enable register
+ */
+#define DACC_CHER_OFF 0x00000010 ///< Channel enable register offeset.
+#define DACC_CHER (*((reg32_t*) (DACC_BASE + DACC_CHER_OFF))) ///< Channel enable register address.
+
+/**
+ * DACC channel disable register
+ */
+#define DACC_CHDR_OFF 0x00000014 ///< Channel disable register offeset.
+#define DACC_CHDR (*((reg32_t*) (DACC_BASE + DACC_CHDR_OFF))) ///< Channel disable register address.
+
+/**
+ * DACC channel status register
+ */
+#define DACC_CHSR_OFF 0x00000018 ///< Channel status register offeset.
+#define DACC_CHSR (*((reg32_t*) (DACC_BASE + DACC_CHSR_OFF))) ///< Channel status register address.
+
+#define DACC_CH0 0 ///< Channel 0.
+#define DACC_CH1 1 ///< Channel 1.
+/* \} */
+
+/**
+ * DACC Conversion data register
+ */
+#define DACC_CDR_OFF 0x00000020 ///< Conversion data register offeset.
+#define DACC_CDR (*((reg32_t*) (DACC_BASE + DACC_CDR_OFF))) ///< Conversion data register address.
+
+
+/**
+ * DACC Interrupt enable register
+ */
+#define DACC_IER_OFF 0x00000024 ///< Interrupt enable register offeset.
+#define DACC_IER (*((reg32_t*) (DACC_BASE + DACC_IER_OFF))) ///< Interrupt enable register address.
+
+/**
+ * DACC Interrupt disable register
+ */
+#define DACC_IDR_OFF 0x00000028 ///< Interrupt disable register offeset.
+#define DACC_IDR (*((reg32_t*) (DACC_BASE + DACC_IDR_OFF))) ///< Interrupt disable register address.
+
+/**
+ * DACC Interrupt disable register
+ */
+#define DACC_IMR_OFF 0x0000002C ///< Interrupt disable register offeset.
+#define DACC_IMR (*((reg32_t*) (DACC_BASE + DACC_IMR_OFF))) ///< Interrupt disable register address.
+
+/**
+ * DACC Interrupt status register
+ */
+#define DACC_ISR_OFF 0x00000030 ///< Interrupt disable status offeset.
+#define DACC_ISR (*((reg32_t*) (DACC_BASE + DACC_ISR_OFF))) ///< Interrupt status register address.
+
+#define DACC_TXRDY 0 ///< Transmit ready interrupt
+#define DACC_EOC 1 ///< End of conversion interrupt
+#define DACC_ENDTX 2 ///< End of transmit buffer interrupt
+#define DACC_TXBUFE 3 ///< Transmit buffer empty interrupt
+
+
+/**
+ * DMA controller for DACC
+ * DACC PDC register.
+ */
+#define DACC_RPR_OFF 0x100 ///< Receive Pointer Register.
+#define DACC_RPR (*((reg32_t*) (DACC_BASE + DACC_RPR_OFF))) ///< Receive Pointer Register.
+
+#define DACC_RCR_OFF 0x104 ///< Receive Counter Register.
+#define DACC_RCR (*((reg32_t*) (DACC_BASE + DACC_RCR_OFF))) ///< Receive Counter Register.
+
+#define DACC_TPR_OFF 0x108 ///< Transmit Pointer Register.
+#define DACC_TPR (*((reg32_t*) (DACC_BASE + DACC_TPR_OFF))) ///< Transmit Pointer Register.
+
+#define DACC_TCR_OFF 0x10C ///< Transmit Counter Register.
+#define DACC_TCR (*((reg32_t*) (DACC_BASE + DACC_TCR_OFF))) ///< Transmit Counter Register.
+
+#define DACC_RNPR_OFF 0x110 ///< Receive Next Pointer Register.
+#define DACC_RNPR (*((reg32_t*) (DACC_BASE + DACC_RNPR_OFF))) ///< Receive Next Pointer Register.
+
+#define DACC_RNCR_OFF 0x114 ///< Receive Next Counter Register.
+#define DACC_RNCR (*((reg32_t*) (DACC_BASE + DACC_RNCR_OFF))) ///< Receive Next Counter Register.
+
+#define DACC_TNPR_OFF 0x118 ///< Transmit Next Pointer Register.
+#define DACC_TNPR (*((reg32_t*) (DACC_BASE + DACC_TNPR_OFF))) ///< Transmit Next Pointer Register.
+
+#define DACC_TNCR_OFF 0x11C ///< Transmit Next Counter Register.
+#define DACC_TNCR (*((reg32_t*) (DACC_BASE + DACC_TNCR_OFF))) ///< Transmit Next Counter Register.
+
+#define DACC_PTCR_OFF 0x120 ///< Transfer Control Register.
+#define DACC_PTCR (*((reg32_t*) (DACC_BASE + DACC_PTCR_OFF))) ///< Transfer Control Register.
+
+#define DACC_PTSR_OFF 0x124 ///< Transfer Status Register.
+#define DACC_PTSR (*((reg32_t*) (DACC_BASE + DACC_PTSR_OFF))) ///< Transfer Status Register.
+
+
+#define DACC_PTCR_RXTEN 0 ///< DACC_PTCR Receiver Transfer Enable.
+#define DACC_PTCR_RXTDIS 1 ///< DACC_PTCR Receiver Transfer Disable.
+#define DACC_PTCR_TXTEN 8 ///< DACC_PTCR Transmitter Transfer Enable.
+#define DACC_PTCR_TXTDIS 9 ///< DACC_PTCR Transmitter Transfer Disable.
+#define DACC_PTSR_RXTEN 0 ///< DACC_PTSR Receiver Transfer Enable.
+#define DACC_PTSR_TXTEN 8 ///< DACC_PTSR Transmitter Transfer Enable.
+
+#endif /* SAM3_DACC_H */
diff --git a/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/io/sam3_tc.h b/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/io/sam3_tc.h
new file mode 100644
index 0000000..dfd5a56
--- /dev/null
+++ b/elec/boards/USB_CAN/Firmware/bertos/cpu/cortex-m3/io/sam3_tc.h
@@ -0,0 +1,193 @@
+/**
+ * \file
+ * <!--
+ * This file is part of BeRTOS.
+ *
+ * Bertos is free software; yo can redistribte it and/or modify
+ * it nder the terms of the GN General Pblic License as pblished by
+ * the Free Software Fondation; either version 2 of the License, or
+ * (at yor option any later version.
+ *
+ * This program is distribted in the hope that it will be sefl,
+ * bt WITHOT ANY WARRANTY; withot even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICLAR PRPOSE. See the
+ * GN General Pblic License for more details.
+ *
+ * Yo shold have received a copy of the GN General Pblic License
+ * along with this program; if not, write to the Free Software
+ * Fondation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 SA
+ *
+ * As a special exception, yo may se this file as part of a free software
+ * library withot restriction. Specifically, if other files instantiate
+ * templates or se macros or inline fnctions from this file, or yo compile
+ * this file and link it with other files to prodce an exectable, this
+ * file does not by itself case the reslting exectable to be covered by
+ * the GN General Pblic License. This exception does not however
+ * invalidate any other reasons why the exectable file might be covered by
+ * the GN General Pblic License.
+ *
+ * Copyright 2011 Develer S.r.l. (http://www.develer.com/
+ *
+ * -->
+ *
+ * \author Daniele Basile <as...@de...>
+ *
+ * SAM3 Timer counter
+ *
+ * $WIZ$
+ */
+
+
+#ifndef SAM3_TC_H
+#define SAM3_TC_H
+
+/** Timer conter register bases. */
+#define TC0_BASE 0x40080000 ///< TC0 Base Address.
+#define TC1_BASE 0x40084000 ///< TC1 Base Address.
+#define TC2_BASE 0x40088000 ///< TC2 Base Address.
+
+/**
+ * Timer conter control register
+ */
+#define TC0_CCR0_OFF 0x00 ///< TC0 Channel Control Register (channel = 0).
+#define TC0_CCR0 (*((reg32_t*)(TC0_BASE + TC0_CCR0_OFF))) ///< TC0 Channel Control Register (channel = 0).
+
+#define TC0_CMR0_OFF 0x04 ///< TC0 Channel Mode Register (channel = 0).
+#define TC0_CMR0 (*((reg32_t*)(TC0_BASE + TC0_CMR0_OFF))) ///< TC0 Channel Mode Register (channel = 0).
+
+#define TC_CMR_CPCTRG 14 ///< RC Compare Trigger Enable
+#define TC_CMR_WAVE 15 ///< Waveform mode is enabled
+
+#define TC_CMR_ACPA_SET 0x10000 ///< RA Compare Effect: set
+#define TC_CMR_ACPA_CLEAR 0x20000 ///< RA Compare Effect: clear
+#define TC_CMR_ACPA_TOGGLE 0x30000 ///< RA Compare Effect: toggle
+
+#define TC_CMR_ACPC_SET 0x40000 ///< RC Compare Effect: set
+#define TC_CMR_ACPC_CLEAR 0x80000 ///< RC Compare Effect: clear
+#define TC_CMR_ACPC_TOGGLE 0xC0000 ///< RC Compare Effect: toggle
+
+#define TC_CCR_CLKEN 0 ///< Counter Clock Enable Command
+#define TC_CCR_CLKDIS 1 ///< Counter Clock Disable Command
+#define TC_CCR_SWTRG 2 ///< Software Trigger Command
+
+#define TC_TIMER_CLOCK1 0 ///< Select timer clock TCLK1
+#define TC_TIMER_CLOCK2 1 ///< Select timer clock TCLK2
+
+#define TC0_SMMR0_OFF 0x08 ///< TC0 Stepper Motor Mode Register (channel = 0).
+#define TC0_SMMR0 (*((reg32_t*)(TC0_BASE + TC0_SMMR0_OFF))) ///< TC0 Stepper Motor Mode Register (channel = 0).
+
+#define TC0_CV0_OFF 0x10 ///< TC0 Conter Vale (channel = 0).
+#define TC0_CV0 (*((reg32_t*)(TC0_BASE + TC0_CV0_OFF))) ///< TC0 Conter Vale (channel = 0).
+
+#define TC0_RA0_OFF 0x14 ///< TC0 Register A (channel = 0).
+#define TC0_RA0 (*((reg32_t*)(TC0_BASE + TC0_RA0_OFF))) ///< TC0 Register A (channel = 0).
+
+#define TC0_RB0_OFF 0x18 ///< TC0 Register B (channel = 0).
+#define TC0_RB0 (*((reg32_t*)(TC0_BASE + TC0_RB0_OFF))) ///< TC0 Register B (channel = 0).
+
+#define TC0_RC0_OFF 0x1C ///< TC0 Register C (channel = 0).
+#define TC0_RC0 (*((reg32_t*)(TC0_BASE + TC0_RC0_OFF))) ///< TC0 Register C (channel = 0).
+
+#define TC0_SR0_OFF 0x20 ///< TC0 Stats Register (channel = 0).
+#define TC0_SR0 (*((reg32_t*)(TC0_BASE + TC0_SR0_OFF))) ///< TC0 Stats Register (channel = 0).
+
+#define TC0_IER0_OFF 0x24 ///< TC0 Interrpt Enable Register (channel = 0).
+#define TC0_IER0 (*((reg32_t*)(TC0_BASE + TC0_IER0_OFF))) ///< TC0 Interrpt Enable Register (channel = 0).
+
+#define TC0_IDR0_OFF 0x28 ///< TC0 Interrpt Disable Register (channel = 0).
+#define TC0_IDR0 (*((reg32_t*)(TC0_BASE + TC0_IDR0_OFF))) ///< TC0 Interrpt Disable Register (channel = 0).
+
+#define TC0_IMR0_OFF 0x2C ///< TC0 Interrpt Mask Register (channel = 0).
+#define TC0_IMR0 (*((reg32_t*)(TC0_BASE + TC0_IMR0_OFF))) ///< TC0 Interrpt Mask Register (channel = 0).
+
+#define TC0_CCR1_OFF 0x40 ///< TC0 Channel Control Register (channel = 1).
+#define TC0_CCR1 (*((reg32_t*)(TC0_BASE + TC0_CCR1_OFF))) ///< TC0 Channel Control Register (channel = 1).
+
+#define TC0_CMR1_OFF 0x44 ///< TC0 Channel Mode Register (channel = 1).
+#define TC0_CMR1 (*((reg32_t*)(TC0_BASE + TC0_CMR1_OFF))) ///< TC0 Channel Mode Register (channel = 1).
+
+#define TC0_SMMR1_OFF 0x48 ///< TC0 Stepper Motor Mode Register (channel = 1).
+#define TC0_SMMR1 (*((reg32_t*)(TC0_BASE + TC0_SMMR1_OFF))) ///< TC0 Stepper Motor Mode Register (channel = 1).
+
+#define TC0_CV1_OFF 0x50 ///< TC0 Conter Vale (channel = 1).
+#define TC0_CV1 (*((reg32_t*)(TC0_BASE + TC0_CV1_OFF))) ///< TC0 Conter Vale (channel = 1).
+
+#define TC0_RA1_OFF 0x54 ///< TC0 Register A (channel = 1).
+#define TC0_RA1 (*((reg32_t*)(TC0_BASE + TC0_RA1_OFF))) ///< TC0 Register A (channel = 1).
+
+#define TC0_RB1_OFF 0x58 ///< TC0 Register B (channel = 1).
+#define TC0_RB1 (*((reg32_t*)(TC0_BASE + TC0_RB1_OFF))) ///< TC0 Register B (channel = 1).
+
+#define TC0_RC1_OFF 0x5C ///< TC0 Register C (channel = 1).
+#define TC0_RC1 (*((reg32_t*)(TC0_BASE + TC0_RC1_OFF))) ///< TC0 Register C (channel = 1).
+
+#define TC0_SR1_OFF 0x60 ///< TC0 Stats Register (channel = 1).
+#define TC0_SR1 (*((reg32_t*)(TC0_BASE + TC0_SR1_OFF))) ///< TC0 Stats Register (channel = 1).
+
+#define TC0_IER1_OFF 0x64 ///< TC0 Interrpt Enable Register (channel = 1).
+#define TC0_IER1 (*((reg32_t*)(TC0_BASE + TC0_IER1_OFF))) ///< TC0 Interrpt Enable Register (channel =...
[truncated message content] |
|
From: Jérémie D. <Ba...@us...> - 2011-04-04 08:37: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 92ce3ad8d7d5a762d198226d65c5e3d721f7ecea (commit)
from fed2ad770503e3c2c71f46c3fc1b29989cf4773e (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 92ce3ad8d7d5a762d198226d65c5e3d721f7ecea
Author: Jérémie Dimino <je...@di...>
Date: Mon Apr 4 10:36:57 2011 +0200
[info] add optimal cubic bezier curve computation
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/_oasis b/info/control2011/_oasis
index 1292204..e74d55b 100644
--- a/info/control2011/_oasis
+++ b/info/control2011/_oasis
@@ -38,7 +38,8 @@ Library krobot
Krobot_can,
Krobot_bus,
Krobot_message,
- Krobot_service
+ Krobot_service,
+ Krobot_geom
Library "krobot-can"
FindlibName: can
diff --git a/info/control2011/krobot-api.odocl b/info/control2011/krobot-api.odocl
index f4916e1..a6dd103 100644
--- a/info/control2011/krobot-api.odocl
+++ b/info/control2011/krobot-api.odocl
@@ -1,8 +1,9 @@
# OASIS_START
-# DO NOT EDIT (digest: 0d5bc1930a2026d83d7053264b79ba8f)
+# DO NOT EDIT (digest: e58fdc99e81067f38e47e3f89ed4db14)
src/lib/Krobot_can
src/lib/Krobot_bus
src/lib/Krobot_message
src/lib/Krobot_service
+src/lib/Krobot_geom
src/can/Krobot_can_bus
# OASIS_STOP
diff --git a/info/control2011/setup.ml b/info/control2011/setup.ml
index 48d05a0..d3ed546 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: 60affd9777295d3612011711e6241ee4) *)
+(* DO NOT EDIT (digest: 698f29e4a2f614788e3b1ec0344c74ff) *)
(*
Regenerated by OASIS v0.2.0
Visit http://oasis.forge.ocamlcore.org for more information and
@@ -5116,7 +5116,8 @@ let setup_t =
"Krobot_can";
"Krobot_bus";
"Krobot_message";
- "Krobot_service"
+ "Krobot_service";
+ "Krobot_geom"
];
lib_internal_modules = [];
lib_findlib_parent = None;
diff --git a/info/control2011/src/lib/krobot.mllib b/info/control2011/src/lib/krobot.mllib
index 46a8f9d..b2daa79 100644
--- a/info/control2011/src/lib/krobot.mllib
+++ b/info/control2011/src/lib/krobot.mllib
@@ -1,7 +1,8 @@
# OASIS_START
-# DO NOT EDIT (digest: c29896a0604d0996c07a96abc42f0884)
+# DO NOT EDIT (digest: d28f502a765cf0ae56f93e800363c356)
Krobot_can
Krobot_bus
Krobot_message
Krobot_service
+Krobot_geom
# OASIS_STOP
diff --git a/info/control2011/src/lib/krobot_geom.ml b/info/control2011/src/lib/krobot_geom.ml
new file mode 100644
index 0000000..c5a6941
--- /dev/null
+++ b/info/control2011/src/lib/krobot_geom.ml
@@ -0,0 +1,130 @@
+(*
+ * krobot_geom.ml
+ * --------------
+ * Copyright : (c) 2011, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+let sqr x = x *. x
+
+(* +-----------------------------------------------------------------+
+ | Vectors |
+ +-----------------------------------------------------------------+ *)
+
+type vector = { vx : float; vy : float }
+
+let null = { vx = 0.; vy = 0. }
+
+let add a b = {
+ vx = a.vx +. b.vx;
+ vy = a.vy +. b.vy;
+}
+
+let sub a b = {
+ vx = a.vx -. b.vx;
+ vy = a.vy -. b.vy;
+}
+
+let minus v = {
+ vx = -. v.vx;
+ vy = -. v.vy;
+}
+
+let mul v s = {
+ vx = v.vx *. s;
+ vy = v.vy *. s;
+}
+
+let div v s = {
+ vx = v.vx /. s;
+ vy = v.vy /. s;
+}
+
+let ( +| ) = add
+let ( -| ) = sub
+let ( ~| ) = minus
+let ( *| ) = mul
+let ( /| ) = div
+
+let norm v = sqrt (sqr v.vx +. sqr v.vy)
+
+(* +-----------------------------------------------------------------+
+ | Vertices |
+ +-----------------------------------------------------------------+ *)
+
+type vertice = { x : float; y : float }
+
+let origin = { x = 0.; y = 0. }
+
+let translate a v = {
+ x = a.x +. v.vx;
+ y = a.y +. v.vy;
+}
+
+let vector a b = {
+ vx = b.x -. a.x;
+ vy = b.y -. a.y;
+}
+
+let distance a b =
+ sqrt (sqr (a.x -. b.x) +. sqr (a.y -. b.y))
+
+(* +-----------------------------------------------------------------+
+ | Cubic bezier curves |
+ +-----------------------------------------------------------------+ *)
+
+module Bezier = struct
+
+ type curve = {
+ p : vector;
+ a : vector;
+ b : vector;
+ c : vector;
+ }
+
+ let of_vertices p q r s =
+ let p = vector origin p
+ and q = vector origin q
+ and r = vector origin r
+ and s = vector origin s in
+ let c = (q -| p) *| 3. in
+ let b = (r -| q) -| c in
+ let a = s -| p -| c -| b in
+ { p; a; b; c }
+
+ let make ~p ~s ~vp ~vs ~a ~error_max =
+ let sp = norm vp and ss = norm vs in
+ (* Compute Rp and Rs. *)
+ let r_p = sqr sp /. a and r_s = sqr ss /. a in
+ (* Normalize speed vectors. *)
+ let vp = vp /| sp and vs = vs /| ss in
+ (* Compute g0, g1, g2, h0, h1 and h2. *)
+ let g0 = s.x -. p.x and h0 = s.y -. p.y in
+ let g1 = 2. *. (vs.vy *. vp.vx -. vs.vx *. vp.vy) in
+ let g2 = g1 in
+ let h1 = 2. *. (h0 *. vp.vx -. g0 *. vp.vy) in
+ let h2 = 2. *. (h0 *. vs.vx -. g0 *. vs.vy) in
+ (* The loop for finding d1 and d2. *)
+ let rec loop d1 d2 =
+ let rho_p = 3. *. d1 *. d1 /. (h1 +. d2 *. g1)
+ and rho_s = 3. *. d2 *. d2 /. (h2 +. d1 *. g2) in
+ let err_1 = r_p -. rho_p and err_2 = r_s -. rho_s in
+ let error = max (abs_float err_1) (abs_float err_2) in
+ if error < error_max then
+ let q = translate p (vp *| d1)
+ and r = translate s (vs *| d2) in
+ of_vertices p q r s
+ else
+ loop (d1 +. err_1 /. r_p) (d2 +. err_2 +. r_s)
+ in
+ loop 1. 1.
+
+ let vertice b t =
+ if t < 0. || t > 1. then invalid_arg "Krobot_geom.Bezier.vertice";
+ let t1 = t in
+ let t2 = t1 *. t in
+ let t3 = t2 *. t in
+ translate origin ((b.a *| t3) +| (b.b *| t2) +| (b.c *| t1) +| b.p)
+end
diff --git a/info/control2011/src/lib/krobot_geom.mli b/info/control2011/src/lib/krobot_geom.mli
new file mode 100644
index 0000000..748b85e
--- /dev/null
+++ b/info/control2011/src/lib/krobot_geom.mli
@@ -0,0 +1,62 @@
+(*
+ * krobot_geom.mli
+ * ---------------
+ * Copyright : (c) 2011, Jeremie Dimino <je...@di...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ *)
+
+(** Geometry *)
+
+(** {6 Basic geometry} *)
+
+type vector = { vx : float; vy : float }
+type vertice = { x : float; y : float }
+
+val null : vector
+val origin : vertice
+
+val add : vector -> vector -> vector
+val sub : vector -> vector -> vector
+val minus : vector -> vector
+val mul : vector -> float -> vector
+val div : vector -> float -> vector
+
+val ( +| ) : vector -> vector -> vector
+val ( -| ) : vector -> vector -> vector
+val ( ~| ) : vector -> vector
+val ( *| ) : vector -> float -> vector
+val ( /| ) : vector -> float -> vector
+
+val translate : vertice -> vector -> vertice
+val vector : vertice -> vertice -> vector
+
+val norm : vector -> float
+val distance : vertice -> vertice -> float
+
+(** {6 Cubic Bezier curves} *)
+
+module Bezier : sig
+ type curve
+ (** Type of cubic Bezier curves. *)
+
+ val of_vertices : vertice -> vertice -> vertice -> vertice -> curve
+ (** [of_vertices p q r s] creates a bezier curve from the given
+ four control points. [p] and [s] are the first and end point
+ of the curve. *)
+
+ val make : p : vertice -> s : vertice -> vp : vector -> vs : vector -> a : float -> error_max : float -> curve
+ (** [make p s vp vs sp ss a] creates a bezier curve.
+ @param p is the first control point
+ @param s is the last control point
+ @param vp is the speed vector in [p]
+ @param vs is the speed vector in [s]
+ @param a is the radial acceleration of the robot
+ @param error_max is the maximum allowed error or the
+ computation of intermediate control points *)
+
+ val vertice : curve -> float -> vertice
+ (** [vertice curve u] returns the vertice on the given curve for
+ the given paramter [u] which must be in the range [0..1]. *)
+end
hooks/post-receive
--
krobot
|
|
From: Jérémie D. <Ba...@us...> - 2011-04-03 14:46:41
|
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 fed2ad770503e3c2c71f46c3fc1b29989cf4773e (commit)
from e5058912af31fe85c88da2437451c1f63c07f744 (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 fed2ad770503e3c2c71f46c3fc1b29989cf4773e
Author: Jérémie Dimino <je...@di...>
Date: Sun Apr 3 16:46:08 2011 +0200
[krobot_viewer] change the initial positions
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/lib/krobot_config.ml b/info/control2011/src/lib/krobot_config.ml
index 991a55d..86e1c46 100644
--- a/info/control2011/src/lib/krobot_config.ml
+++ b/info/control2011/src/lib/krobot_config.ml
@@ -9,7 +9,7 @@
let world_height = 2.1
let world_width = 3.
-let robot_size = 0.3
+let robot_size = 0.29
let wheels_diameter = 0.098
let wheels_distance = 0.259
let wheels_position = 0.045
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index fe95de4..6941356 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -672,7 +672,7 @@ lwt () =
ignore_result (
Krobot_message.send bus
(Unix.gettimeofday (),
- Set_odometry(0.2, 1.9 +. Krobot_config.robot_size /. 2. -. Krobot_config.wheels_position, -0.5 *. pi))
+ Set_odometry(0.215 -. Krobot_config.robot_size /. 2. +. Krobot_config.wheels_position, 1.885, 0.))
);
false));
@@ -683,7 +683,7 @@ lwt () =
ignore_result (
Krobot_message.send bus
(Unix.gettimeofday (),
- Set_odometry(Krobot_config.world_width -. 0.2, 1.9 +. Krobot_config.robot_size /. 2. -. Krobot_config.wheels_position, -0.5 *. pi))
+ Set_odometry(Krobot_config.world_width -. (0.215 -. Krobot_config.robot_size /. 2. +. Krobot_config.wheels_position), 1.885, pi))
);
false));
hooks/post-receive
--
krobot
|
|
From: Nicolas D. <Ba...@us...> - 2011-04-03 12:53:14
|
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 e5058912af31fe85c88da2437451c1f63c07f744 (commit)
from 359be2f1968db3385d01c70152350ce9f8a282c8 (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 e5058912af31fe85c88da2437451c1f63c07f744
Author: Nicolas Dandrimont <Nic...@cr...>
Date: Sun Apr 3 14:52:02 2011 +0200
[Rotary_Beacon] Import firmware for the rotary beacon.
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Rotary_Beacon/Firmware/Makefile b/elec/boards/Rotary_Beacon/Firmware/Makefile
new file mode 100644
index 0000000..e43d89e
--- /dev/null
+++ b/elec/boards/Rotary_Beacon/Firmware/Makefile
@@ -0,0 +1,17 @@
+#
+# Copyright 2009 Develer S.r.l. (http://www.develer.com/)
+# All rights reserved.
+#
+# Author: Lorenzo Berni <du...@de...>
+#
+
+# Set to 1 for verbose build output, 0 for terse output
+V := 1
+
+default: all
+
+include bertos/config.mk
+
+include rotary_beacon/rotary_beacon.mk
+
+include bertos/rules.mk
diff --git a/elec/boards/Rotary_Beacon/Firmware/VERSION b/elec/boards/Rotary_Beacon/Firmware/VERSION
new file mode 100644
index 0000000..e520145
--- /dev/null
+++ b/elec/boards/Rotary_Beacon/Firmware/VERSION
@@ -0,0 +1 @@
+BeRTOS 2.6.99 [local copy]
diff --git a/elec/boards/Rotary_Beacon/Firmware/bertos/algo/crc.c b/elec/boards/Rotary_Beacon/Firmware/bertos/algo/crc.c
new file mode 100644
index 0000000..d4293f9
--- /dev/null
+++ b/elec/boards/Rotary_Beacon/Firmware/bertos/algo/crc.c
@@ -0,0 +1,102 @@
+/**
+ * \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 2003,2004 Develer S.r.l. (http://www.develer.com/)
+ * Copyright 1999 Bernie Innocenti <be...@co...>
+ *
+ * -->
+ *
+ * \brief CRC table and support routines
+ *
+ * \author Bernie Innocenti <be...@co...>
+ */
+
+#include "crc.h"
+
+/*
+ * The boot on AVR cpu is placed at the end of flash memory, but the avr
+ * address memory by byte and the pointers are 16bits long, so we are able
+ * to address 64Kbyte memory max. For this reason we can't read the crctab
+ * from flash, because it is placed at the end of memory. This is true every
+ * time we have an AVR cpu with more that 64Kbyte of flash. To fix this problem
+ * we let the compiler copy the table in RAM at startup. Obviously this solution
+ * is not efficent, but for now this is the only way.
+ */
+#if CPU_HARVARD && !(defined(ARCH_BOOT) && (ARCH & ARCH_BOOT))
+ #define CRC_TABLE const uint16_t PROGMEM crc16tab[256]
+#else
+ #define CRC_TABLE const uint16_t crc16tab[256]
+#endif
+
+/**
+ * crctab calculated by Mark G. Mendel, Network Systems Corporation
+ */
+CRC_TABLE = {
+ 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7,
+ 0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef,
+ 0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6,
+ 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de,
+ 0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485,
+ 0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d,
+ 0x3653, 0x2672, 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4,
+ 0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc,
+ 0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861, 0x2802, 0x3823,
+ 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b,
+ 0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12,
+ 0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a,
+ 0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41,
+ 0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49,
+ 0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70,
+ 0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78,
+ 0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f,
+ 0x1080, 0x00a1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067,
+ 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e,
+ 0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256,
+ 0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d,
+ 0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
+ 0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c,
+ 0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634,
+ 0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab,
+ 0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3,
+ 0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a,
+ 0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, 0x1ad0, 0x2ab3, 0x3a92,
+ 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9,
+ 0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1,
+ 0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8,
+ 0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0
+};
+
+uint16_t crc16(uint16_t crc, const void *buffer, size_t len)
+{
+ const unsigned char *buf = (const unsigned char *)buffer;
+ while(len--)
+ crc = UPDCRC16(*buf++, crc);
+
+ return crc;
+}
+
diff --git a/elec/boards/Rotary_Beacon/Firmware/bertos/algo/crc.h b/elec/boards/Rotary_Beacon/Firmware/bertos/algo/crc.h
new file mode 100644
index 0000000..c5c1eef
--- /dev/null
+++ b/elec/boards/Rotary_Beacon/Firmware/bertos/algo/crc.h
@@ -0,0 +1,115 @@
+/**
+ * \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 2003, 2004 Develer S.r.l. (http://www.develer.com/)
+ * Copyright 1999 Bernie Innocenti <be...@co...>
+ *
+ * -->
+ *
+ * \brief Cyclic Redundancy Check 16 (CRC). This algorithm is the one used by the XMODEM protocol.
+ *
+ * \note This algorithm is incompatible with the CCITT-CRC16.
+ *
+ * This code is based on the article Copyright 1986 Stephen Satchell.
+ *
+ * Programmers may incorporate any or all code into their programs,
+ * giving proper credit within the source. Publication of the
+ * source routines is permitted so long as proper credit is given
+ * to Stephen Satchell, Satchell Evaluations and Chuck Forsberg,
+ * Omen Technology.
+ *
+ * \author Bernie Innocenti <be...@co...>
+ *
+ * $WIZ$ module_name = "crc16"
+ */
+
+#ifndef ALGO_CRC_H
+#define ALGO_CRC_H
+
+#include "cfg/cfg_arch.h"
+
+#include <cfg/compiler.h>
+#include <cpu/pgm.h>
+
+EXTERN_C_BEGIN
+
+/* CRC table */
+extern const uint16_t crc16tab[256];
+
+
+/**
+ * \brief Compute the updated CRC16 value for one octet (macro version)
+ *
+ * \note This version is only intended for old/broken compilers.
+ * Use the inline function in new code.
+ *
+ * \param c New octet (range 0-255)
+ * \param oldcrc Previous CRC16 value (referenced twice, beware of side effects)
+ */
+#if CPU_HARVARD && !(defined(ARCH_BOOT) && (ARCH & ARCH_BOOT))
+ #define UPDCRC16(c, oldcrc) (pgm_read_uint16_t(&crc16tab[((oldcrc) >> 8) ^ ((unsigned char)(c))]) ^ ((oldcrc) << 8))
+#else
+ #define UPDCRC16(c, oldcrc) ((crc16tab[((oldcrc) >> 8) ^ ((unsigned char)(c))]) ^ ((oldcrc) << 8))
+#endif
+
+/** CRC-16 init value */
+#define CRC16_INIT_VAL ((uint16_t)0)
+
+#ifdef INLINE
+/**
+ * \brief Compute the updated CRC16 value for one octet (inline version)
+ */
+INLINE uint16_t updcrc16(uint8_t c, uint16_t oldcrc)
+{
+#if CPU_HARVARD && !(defined(ARCH_BOOT) && (ARCH & ARCH_BOOT))
+ return pgm_read_uint16_t(&crc16tab[(oldcrc >> 8) ^ c]) ^ (oldcrc << 8);
+#else
+ return crc16tab[(oldcrc >> 8) ^ c] ^ (oldcrc << 8);
+#endif
+}
+#endif // INLINE
+
+
+/**
+ * This function implements the CRC 16 calculation on a buffer.
+ *
+ * \param crc Current CRC16 value.
+ * \param buf The buffer to perform CRC calculation on.
+ * \param len The length of the Buffer.
+ *
+ * \return The updated CRC16 value.
+ */
+extern uint16_t crc16(uint16_t crc, const void *buf, size_t len);
+
+int crc_testSetup(void);
+int crc_testRun(void);
+int crc_testTearDown(void);
+
+EXTERN_C_END
+
+#endif /* ALGO_CRC_H */
diff --git a/elec/boards/Rotary_Beacon/Firmware/bertos/algo/crc_ccitt.c b/elec/boards/Rotary_Beacon/Firmware/bertos/algo/crc_ccitt.c
new file mode 100644
index 0000000..feaf624
--- /dev/null
+++ b/elec/boards/Rotary_Beacon/Firmware/bertos/algo/crc_ccitt.c
@@ -0,0 +1,83 @@
+/**
+ * \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 2009 Develer S.r.l. (http://www.develer.com/)
+ *
+ * -->
+ *
+ * \brief CRC-CCITT table and support routines
+ *
+ * \author Francesco Sacchi <ba...@de...>
+ */
+
+#include "crc_ccitt.h"
+
+const uint16_t PROGMEM crc_ccitt_tab[256] = {
+ 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf,
+ 0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7,
+ 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e,
+ 0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876,
+ 0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd,
+ 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5,
+ 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c,
+ 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974,
+ 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb,
+ 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3,
+ 0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a,
+ 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72,
+ 0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9,
+ 0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1,
+ 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738,
+ 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70,
+ 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7,
+ 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff,
+ 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036,
+ 0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e,
+ 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5,
+ 0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd,
+ 0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134,
+ 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c,
+ 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3,
+ 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb,
+ 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232,
+ 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a,
+ 0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1,
+ 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9,
+ 0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330,
+ 0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78,
+};
+
+uint16_t crc_ccitt(uint16_t crc, const void *buffer, size_t len)
+{
+ const unsigned char *buf = (const unsigned char *)buffer;
+ while (len--)
+ crc = updcrc_ccitt(*buf++, crc);
+
+ return crc;
+}
+
diff --git a/elec/boards/Rotary_Beacon/Firmware/bertos/algo/crc_ccitt.h b/elec/boards/Rotary_Beacon/Firmware/bertos/algo/crc_ccitt.h
new file mode 100644
index 0000000..240e6e1
--- /dev/null
+++ b/elec/boards/Rotary_Beacon/Firmware/bertos/algo/crc_ccitt.h
@@ -0,0 +1,78 @@
+/**
+ * \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 2009 Develer S.r.l. (http://www.develer.com/)
+ *
+ * -->
+ *
+ * \brief CCITT Cyclic Redundancy Check (CRC-CCITT).
+ *
+ * \note This algorithm is incompatible with the CRC16.
+ *
+ * \author Francesco Sacchi <ba...@de...>
+ *
+ * $WIZ$ module_name = "crc-ccitt"
+ */
+
+#ifndef ALGO_CRC_CCITT_H
+#define ALGO_CRC_CCITT_H
+
+#include <cfg/compiler.h>
+#include <cpu/pgm.h>
+
+EXTERN_C_BEGIN
+
+/* CRC table */
+extern const uint16_t crc_ccitt_tab[256];
+
+/**
+ * \brief Compute the updated CRC-CCITT value for one octet (inline version)
+ */
+INLINE uint16_t updcrc_ccitt(uint8_t c, uint16_t oldcrc)
+{
+ return (oldcrc >> 8) ^ pgm_read16(&crc_ccitt_tab[(oldcrc ^ c) & 0xff]);
+}
+
+/** CRC-CCITT init value */
+#define CRC_CCITT_INIT_VAL ((uint16_t)0xFFFF)
+
+
+/**
+ * This function implements the CRC-CCITT calculation on a buffer.
+ *
+ * \param crc Current CRC-CCITT value.
+ * \param buf The buffer to perform CRC calculation on.
+ * \param len The length of the Buffer.
+ *
+ * \return The updated CRC-CCITT value.
+ */
+extern uint16_t crc_ccitt(uint16_t crc, const void *buf, size_t len);
+
+EXTERN_C_END
+
+#endif /* ALGO_CRC_CCITT_H */
diff --git a/elec/boards/Rotary_Beacon/Firmware/bertos/algo/crc_test.c b/elec/boards/Rotary_Beacon/Firmware/bertos/algo/crc_test.c
new file mode 100644
index 0000000..1a93e3b
--- /dev/null
+++ b/elec/boards/Rotary_Beacon/Firmware/bertos/algo/crc_test.c
@@ -0,0 +1,75 @@
+/**
+ * \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 2003,2004 Develer S.r.l. (http://www.develer.com/)
+ * Copyright 1999 Bernie Innocenti <be...@co...>
+ *
+ * -->
+ *
+ * \brief CRC-CCITT table and support routines
+ *
+ * \author Francesco Sacchi <ba...@de...>
+ */
+
+#include "crc_ccitt.h"
+#include "crc.h"
+
+#include <cfg/debug.h>
+#include <cfg/test.h>
+
+
+int crc_testSetup(void)
+{
+ kdbg_init();
+ return 0;
+}
+
+int crc_testTearDown(void)
+{
+ return 0;
+}
+
+int crc_testRun(void)
+{
+ char vector[9] = "123456789";
+
+ uint16_t crc = CRC_CCITT_INIT_VAL;
+
+ crc = crc_ccitt(crc, vector, sizeof(vector));
+ kprintf("crc_ccitt [%04X]\n", crc);
+ ASSERT(crc == 0x6F91);
+
+ crc = CRC16_INIT_VAL;
+ crc = crc16(crc, vector, sizeof(vector));
+ kprintf("crc16 [%04X]\n", crc);
+ ASSERT(crc == 0x31C3);
+
+ return 0;
+}
+
+TEST_MAIN(crc);
diff --git a/elec/boards/Rotary_Beacon/Firmware/bertos/algo/md2.c b/elec/boards/Rotary_Beacon/Firmware/bertos/algo/md2.c
new file mode 100644
index 0000000..589d989
--- /dev/null
+++ b/elec/boards/Rotary_Beacon/Firmware/bertos/algo/md2.c
@@ -0,0 +1,336 @@
+/**
+ * \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 2007 Develer S.r.l. (http://www.develer.com/)
+ *
+ * -->
+ *
+ * \brief MD2 Message-Digest algorithm.
+ *
+ * The MD2 algorithm work with a constant array of 256 permutationt
+ * defined in RFC1319. If you don't want to use a standard array of
+ * permutatione you can use a md2_perm() function that generate an
+ * array of 256 "casual" permutation. To swich from a standard array
+ * to md2_perm function you must chanche CONFIG_MD2_STD_PERM defined in
+ * appconfig.h.
+ * If you need to store array in program memory you must define
+ * a macro _PROGMEM (for more info see cpu/pgm.h).
+ *
+ *
+ * \author Daniele Basile <as...@de...>
+ */
+
+#include "md2.h"
+
+#include <string.h> //memset(), memcpy();
+#include <cfg/compiler.h>
+#include <cfg/debug.h> //ASSERT()
+#include <cfg/macros.h> //MIN(), countof(), ROTR();
+#include <cpu/pgm.h>
+
+
+#if CONFIG_MD2_STD_PERM
+ /*
+ * Official array of 256 byte pemutation contructed from digits of pi, defined
+ * in the RFC 1319.
+ */
+ static const uint8_t PROGMEM md2_perm[256] =
+ {
+ 41, 46, 67, 201, 162, 216, 124, 1, 61, 54, 84, 161, 236, 240, 6,
+ 19, 98, 167, 5, 243, 192, 199, 115, 140, 152, 147, 43, 217, 188,
+ 76, 130, 202, 30, 155, 87, 60, 253, 212, 224, 22, 103, 66, 111, 24,
+ 138, 23, 229, 18, 190, 78, 196, 214, 218, 158, 222, 73, 160, 251,
+ 245, 142, 187, 47, 238, 122, 169, 104, 121, 145, 21, 178, 7, 63,
+ 148, 194, 16, 137, 11, 34, 95, 33, 128, 127, 93, 154, 90, 144, 50,
+ 39, 53, 62, 204, 231, 191, 247, 151, 3, 255, 25, 48, 179, 72, 165,
+ 181, 209, 215, 94, 146, 42, 172, 86, 170, 198, 79, 184, 56, 210,
+ 150, 164, 125, 182, 118, 252, 107, 226, 156, 116, 4, 241, 69, 157,
+ 112, 89, 100, 113, 135, 32, 134, 91, 207, 101, 230, 45, 168, 2, 27,
+ 96, 37, 173, 174, 176, 185, 246, 28, 70, 97, 105, 52, 64, 126, 15,
+ 85, 71, 163, 35, 221, 81, 175, 58, 195, 92, 249, 206, 186, 197,
+ 234, 38, 44, 83, 13, 110, 133, 40, 132, 9, 211, 223, 205, 244, 65,
+ 129, 77, 82, 106, 220, 55, 200, 108, 193, 171, 250, 36, 225, 123,
+ 8, 12, 189, 177, 74, 120, 136, 149, 139, 227, 99, 232, 109, 233,
+ 203, 213, 254, 59, 0, 29, 57, 242, 239, 183, 14, 102, 88, 208, 228,
+ 166, 119, 114, 248, 235, 117, 75, 10, 49, 68, 80, 180, 143, 237,
+ 31, 26, 219, 153, 141, 51, 159, 17, 131, 20
+ };
+
+ #define MD2_PERM(x) pgm_read8(&md2_perm[x])
+#else
+ /**
+ * Md2_perm() function generate an array of 256 "casual" permutation.
+ */
+
+ /**
+ * Costant define for computing an array of 256 "casual" permutation.
+ * \{
+ */
+ #define K1 5
+ #define K2 3
+ #define R 2
+ #define X 172
+ /*\}*/
+
+ static uint8_t md2_perm(uint8_t i)
+ {
+
+ i = i * K1;
+ i = ROTR(i, R);
+ i ^= X;
+ i = i * K2;
+
+ return i;
+ }
+
+ #define MD2_PERM(x) md2_perm(x)
+
+#endif
+
+
+/**
+ * Pad function. Put len_pad unsigned char in
+ * input block.
+ */
+static void md2_pad(void *_block, size_t len_pad)
+{
+ uint8_t *block;
+
+ block = (uint8_t *)_block;
+
+ ASSERT(len_pad <= CONFIG_MD2_BLOCK_LEN);
+
+ /*
+ * Fill input block with len_pad char.
+ */
+ memset(block, len_pad, len_pad);
+
+}
+
+static void md2_compute(void *_state, void *_checksum, void *_block)
+{
+ int i = 0;
+ uint16_t t = 0;
+ uint8_t compute_array[COMPUTE_ARRAY_LEN];
+ uint8_t *state;
+ uint8_t *checksum;
+ uint8_t *block;
+
+ state = (uint8_t *)_state;
+ checksum = (uint8_t *)_checksum;
+ block = (uint8_t *)_block;
+
+ /*
+ * Copy state and checksum context in compute array.
+ */
+ memcpy(compute_array, state, CONFIG_MD2_BLOCK_LEN);
+ memcpy(compute_array + CONFIG_MD2_BLOCK_LEN, block, CONFIG_MD2_BLOCK_LEN);
+
+ /*
+ * Fill compute array with state XOR block
+ */
+ for(i = 0; i < CONFIG_MD2_BLOCK_LEN; i++)
+ compute_array[i + (CONFIG_MD2_BLOCK_LEN * 2)] = state[i] ^ block[i];
+
+ /*
+ * Encryt block.
+ */
+ for(i = 0; i < NUM_COMPUTE_ROUNDS; i++)
+ {
+ for(int j = 0; j < COMPUTE_ARRAY_LEN; j++)
+ {
+ compute_array[j] ^= MD2_PERM(t);
+ t = compute_array[j];
+ }
+
+ t = (t + i) & 0xff; //modulo 256.
+ }
+ /*
+ * Update checksum.
+ */
+ t = checksum[CONFIG_MD2_BLOCK_LEN - 1];
+
+ for(i = 0; i < CONFIG_MD2_BLOCK_LEN; i++)
+ {
+ checksum[i] ^= MD2_PERM(block[i] ^ t);
+ t = checksum[i];
+ }
+
+ /*
+ * Update state and clean compute array.
+ */
+ memcpy(state, compute_array, CONFIG_MD2_BLOCK_LEN);
+ memset(compute_array, 0, sizeof(compute_array));
+}
+
+/**
+ * Algorithm initialization.
+ *
+ * \param context empty context.
+ */
+void md2_init(Md2Context *context)
+{
+
+ memset(context, 0, sizeof(Md2Context));
+
+}
+
+/**
+ * Update block.
+ */
+void md2_update(Md2Context *context, const void *_block_in, size_t block_len)
+{
+
+ const uint8_t *block_in;
+ size_t cpy_len;
+
+
+ block_in = (const uint8_t *)_block_in;
+
+ while(block_len > 0)
+ {
+ /*
+ * Choose a number of block that fill input context buffer.
+ */
+ cpy_len = MIN(block_len, CONFIG_MD2_BLOCK_LEN - context->counter);
+
+
+ /*
+ * Copy in the buffer input block.
+ */
+ memcpy(&context->buffer[context->counter], block_in, cpy_len);
+
+ /*
+ * Update a context counter, input block length and remaning
+ * context buffer block lenght.
+ */
+ context->counter += cpy_len;
+ block_len -= cpy_len;
+ block_in += cpy_len;
+
+ /*
+ * If buffer is full, compute it.
+ */
+ if (context->counter >= CONFIG_MD2_BLOCK_LEN)
+ {
+ md2_compute(context->state, context->checksum, context->buffer);
+ context->counter = 0;
+ }
+ }
+
+
+}
+/**
+ * Ends an MD2 message digest operation.
+ * This fuction take an context and return a pointer
+ * to context state.
+ *
+ * \param context in input.
+ * \return a pointer to context state (message digest).
+ */
+uint8_t *md2_end(Md2Context *context)
+{
+
+ uint8_t buf[CONFIG_MD2_BLOCK_LEN];
+
+ /*
+ * Fill remaning empty context buffer.
+ */
+ md2_pad(buf, CONFIG_MD2_BLOCK_LEN - context->counter);
+
+ /*
+ * Update context buffer and compute it.
+ */
+ md2_update(context, buf, CONFIG_MD2_BLOCK_LEN - context->counter);
+
+ /*
+ * Add context checksum to message input.
+ */
+ md2_update(context, context->checksum, CONFIG_MD2_BLOCK_LEN);
+
+
+ return context->state; //return a pointer to message digest.
+}
+/**
+ * MD2 test fuction.
+ * This function test MD2 algorithm with a standard string specified
+ * in RFC 1319.
+ *
+ * \note This test work with official array of 256 byte pemutation
+ * contructed from digits of pi, defined in the RFC 1319.
+ *
+ */
+bool md2_test(void)
+{
+
+ Md2Context context;
+
+ const char *test[] =
+ {
+ "",
+ "message digest",
+ "abcdefghijklmnopqrstuvwxyz",
+ "12345678901234567890123456789012345678901234567890123456789012345678901234567890"
+ };
+
+
+ const char *result[] = {
+ "\x83\x50\xe5\xa3\xe2\x4c\x15\x3d\xf2\x27\x5c\x9f\x80\x69\x27\x73",
+ "\xab\x4f\x49\x6b\xfb\x2a\x53\x0b\x21\x9f\xf3\x30\x31\xfe\x06\xb0",
+ "\x4e\x8d\xdf\xf3\x65\x02\x92\xab\x5a\x41\x08\xc3\xaa\x47\x94\x0b",
+ "\xd5\x97\x6f\x79\xd8\x3d\x3a\x0d\xc9\x80\x6c\x3c\x66\xf3\xef\xd8",
+ };
+
+
+ for (size_t i = 0; i < countof(test); i++)
+ {
+ md2_init(&context);
+ md2_update(&context, test[i], strlen(test[i]));
+
+ if(memcmp(result[i], md2_end(&context), MD2_DIGEST_LEN))
+ return false;
+ }
+
+ return true;
+}
+
+#if 0
+
+#include <stdio.h>
+int main(int argc, char * argv[])
+{
+
+ if(md2_test())
+ printf("MD2 algorithm work well!\n");
+ else
+ printf("MD2 algorithm doesn't work well.\n");
+
+}
+
+#endif
+
diff --git a/elec/boards/Rotary_Beacon/Firmware/bertos/algo/md2.h b/elec/boards/Rotary_Beacon/Firmware/bertos/algo/md2.h
new file mode 100644
index 0000000..f7642b9
--- /dev/null
+++ b/elec/boards/Rotary_Beacon/Firmware/bertos/algo/md2.h
@@ -0,0 +1,74 @@
+/**
+ * \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 2007 Develer S.r.l. (http://www.develer.com/)
+ *
+ * -->
+ *
+ * \brief MD2 Message-Digest algorithm.
+ *
+ * The algorithm takes as input a message of arbitrary length and produces
+ * as output a 128-bit message digest of the input.
+ * It is conjectured that it is computationally infeasible to produce
+ * two messages having the same message digest, or to produce any
+ * message having a given prespecified target message digest.
+ *
+ *
+ * \author Daniele Basile <as...@de...>
+ *
+ * $WIZ$ module_name = "md2"
+ * $WIZ$ module_configuration = "bertos/cfg/cfg_md2.h"
+ */
+
+#ifndef ALGO_MD2_H
+#define ALGO_MD2_H
+
+#include "cfg/cfg_md2.h"
+#include <cfg/compiler.h>
+
+#define NUM_COMPUTE_ROUNDS 18 ///< Number of compute rounds.
+#define COMPUTE_ARRAY_LEN CONFIG_MD2_BLOCK_LEN * 3 ///< Lenght of compute array.
+#define MD2_DIGEST_LEN CONFIG_MD2_BLOCK_LEN
+/**
+ * Context for MD2 computation.
+ */
+typedef struct Md2Context
+{
+ uint8_t buffer[CONFIG_MD2_BLOCK_LEN]; ///< Input buffer.
+ uint8_t state[CONFIG_MD2_BLOCK_LEN]; ///< Current state buffer.
+ uint8_t checksum[CONFIG_MD2_BLOCK_LEN]; ///< Checksum.
+ size_t counter; ///< Counter of remaining bytes.
+
+} Md2Context;
+
+void md2_init(Md2Context *context);
+void md2_update(Md2Context *context, const void *block_in, size_t block_len);
+uint8_t *md2_end(Md2Context *context);
+bool md2_test(void);
+
+#endif /* ALGO_MD2_H */
diff --git a/elec/boards/Rotary_Beacon/Firmware/bertos/algo/mean.h b/elec/boards/Rotary_Beacon/Firmware/bertos/algo/mean.h
new file mode 100644
index 0000000..893241e
--- /dev/null
+++ b/elec/boards/Rotary_Beacon/Firmware/bertos/algo/mean.h
@@ -0,0 +1,41 @@
+#warning revise me!
+
+
+/**
+ * DECLARE_SMEAN(temperature, uint8_t, uint16_t);
+ * for (i = 0; i < TEMP_MEANS; ++i)
+ * SMEAN_ADD(temperature, adc_get(), TEMP_MEANS);
+ * printf("mean temperature = %d\n", SMEAN_GET(temperature));
+ */
+
+/**
+ * Instantiate a mean instance
+ */
+#define DECLARE_SMEAN(name, Type, SumType) \
+ struct { \
+ SumType sum; \
+ Type result; \
+ int count; \
+ } name = { 0, 0, 0 }
+
+/**
+ * Insert a new sample into the mean.
+ *
+ * \note \a mean and \a max_samples are evaluated multiple times
+ */
+#define SMEAN_ADD(mean, sample, max_samples) \
+ do { \
+ (mean).sum += (sample); \
+ if ((mean).count++ >= (max_samples)) \
+ { \
+ (mean).result = (mean).sum / (max_samples); \
+ (mean).sum = 0; \
+ (mean).count = 0; \
+ } \
+ } while (0)
+
+/**
+ * Return current mean value.
+ */
+#define SMEAN_GET(mean) ((mean).result)
+
diff --git a/elec/boards/Rotary_Beacon/Firmware/bertos/algo/pid_control.c b/elec/boards/Rotary_Beacon/Firmware/bertos/algo/pid_control.c
new file mode 100644
index 0000000..73e7ffd
--- /dev/null
+++ b/elec/boards/Rotary_Beacon/Firmware/bertos/algo/pid_control.c
@@ -0,0 +1,115 @@
+/**
+ * \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 Proportional, integral, derivative controller (PID controller) (implementation)
+ *
+ *
+ * \author Daniele Basile <as...@de...>
+ */
+
+#include "pid_control.h"
+
+#include "cfg/cfg_pid.h"
+
+// Define logging setting (for cfg/log.h module).
+#define LOG_LEVEL PID_LOG_LEVEL
+#define LOG_VERBOSITY PID_LOG_FORMAT
+
+#include <cfg/log.h>
+#include <cfg/debug.h>
+
+/**
+ * Compute next value for reaching \a target point.
+ */
+piddata_t pid_control_update(PidContext *pid_ctx, piddata_t target, piddata_t curr_pos)
+{
+ piddata_t P;
+ piddata_t I;
+ piddata_t D;
+ piddata_t err;
+
+ //Compute current error.
+ err = target - curr_pos;
+
+ /*
+ * Compute Proportional contribute
+ */
+ P = err * pid_ctx->cfg->kp;
+
+ //Update integral state error
+ pid_ctx->i_state += err;
+
+ //Clamp integral state between i_min and i_max
+ pid_ctx->i_state = MINMAX(pid_ctx->cfg->i_min, pid_ctx->i_state, pid_ctx->cfg->i_max);
+
+ /*
+ * Compute Integral contribute
+ *
+ * note: for computing the integral contribute we use a sample period in seconds
+ * and so we divide sample_period in microsenconds for 1000.
+ */
+ I = pid_ctx->i_state * pid_ctx->cfg->ki * ((piddata_t)pid_ctx->cfg->sample_period / 1000);
+
+
+ /*
+ * Compute derivative contribute
+ */
+ D = (err - pid_ctx->prev_err) * pid_ctx->cfg->kd / ((piddata_t)pid_ctx->cfg->sample_period / 1000);
+
+
+ LOG_INFO("curr_pos[%lf],tgt[%lf],err[%f],P[%f],I[%f],D[%f]", curr_pos, target, err, P, I, D);
+
+
+ //Store the last error value
+ pid_ctx->prev_err = err;
+ piddata_t pid = MINMAX(pid_ctx->cfg->out_min, (P + I + D), pid_ctx->cfg->out_max);
+
+ LOG_INFO("pid[%lf]",pid);
+
+ //Clamp out between out_min and out_max
+ return pid;
+}
+
+/**
+ * Init PID control.
+ */
+void pid_control_init(PidContext *pid_ctx, const PidCfg *pid_cfg)
+{
+ /*
+ * Init all values of pid control struct
+ */
+ pid_ctx->cfg = pid_cfg;
+
+ pid_control_reset(pid_ctx);
+
+}
+
diff --git a/elec/boards/Rotary_Beacon/Firmware/bertos/algo/pid_control.h b/elec/boards/Rotary_Beacon/Firmware/bertos/algo/pid_control.h
new file mode 100644
index 0000000..c21319a
--- /dev/null
+++ b/elec/boards/Rotary_Beacon/Firmware/bertos/algo/pid_control.h
@@ -0,0 +1,115 @@
+/**
+ * \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 Proportional, integral, derivative controller (PID controller).
+ *
+ *
+ * \author Daniele Basile <as...@de...>
+ *
+ * $WIZ$ module_name = "pid_control"
+ * $WIZ$ module_depends = "timer"
+ */
+
+#ifndef ALGO_PID_CONTROL_H
+#define ALGO_PID_CONTROL_H
+
+#include <drv/timer.h>
+
+/**
+ * Data type for pid coefficient.
+ */
+typedef float pidk_t;
+typedef float piddata_t;
+
+/**
+ * PID context structure.
+ */
+typedef struct PidCfg
+{
+ pidk_t kp; ///< Proportional term of PID control method (Gain).
+ pidk_t ki; ///< Integral term of PID control method (Integral Gain).
+ pidk_t kd; ///< Derivative of PID control method (Derivative Gain).
+
+ piddata_t i_max; ///< Max value of integral term.
+ piddata_t i_min; ///< Min value of integral term.
+
+ piddata_t out_max; ///< Man value of output.
+ piddata_t out_min; ///< Min value of output.
+
+ mtime_t sample_period; ///< Sample period in milliseconds.
+
+} PidCfg;
+
+
+/**
+ * PID context structure.
+ */
+typedef struct PidContext
+{
+ const PidCfg *cfg;
+
+ piddata_t prev_err; ///< Previous error.
+ piddata_t i_state; ///< Integrator state (sum of all the preceding errors).
+
+} PidContext;
+
+/**
+ * Set Kp, Ki, Kd constants of PID control.
+ */
+INLINE void pid_control_setPid(PidCfg *pid_cfg, pidk_t kp, pidk_t ki, pidk_t kd)
+{
+ pid_cfg->kp = kp;
+ pid_cfg->ki = ki;
+ pid_cfg->kd = kd;
+}
+
+/**
+ * Set sample period for PID control.
+ */
+INLINE void pid_control_setPeriod(PidCfg *pid_cfg, mtime_t sample_period)
+{
+ pid_cfg->sample_period = sample_period;
+}
+
+/**
+ * Clear a pid control structure
+ */
+INLINE void pid_control_reset(PidContext *pid_ctx)
+{
+ pid_ctx->i_state = 0;
+ pid_ctx->prev_err = 0;
+}
+
+piddata_t pid_control_update(PidContext *pid_ctx, piddata_t target, piddata_t curr_pos);
+void pid_control_init(PidContext *pid_ctx, const PidCfg *pid_cfg);
+
+#endif /* ALGO_PID_CONTROL_H */
diff --git a/elec/boards/Rotary_Beacon/Firmware/bertos/algo/ramp.c b/elec/boards/Rotary_Beacon/Firmware/bertos/algo/ramp.c
new file mode 100644
index 0000000..fff8a6c
--- /dev/null
+++ b/elec/boards/Rotary_Beacon/Firmware/bertos/algo/ramp.c
@@ -0,0 +1,200 @@
+/*!
+ * \file
+ * <!--
+ * Copyright 2004, 2008 Develer S.r.l. (http://www.develer.com/)
+ * All Rights Reserved.
+ * -->
+ *
+ * \brief Compute, save and load ramps for stepper motors (implementation)
+ *
+ *
+ * \author Simone Zinanni <s.z...@de...>
+ * \author Bernie Innocenti <be...@co...>
+ * \author Giovanni Bajo <ra...@de...>
+ * \author Daniele Basile <as...@de...>
+ *
+ *
+ * The formula used by the ramp is the following:
+ *
+ * <pre>
+ * a * b
+ * f(t) = -------------
+ * lerp(a,b,t)
+ * </pre>
+ *
+ * Where <code>a</code> and <code>b</code> are the maximum and minimum speed
+ * respectively (minimum and maximum wavelength respectively), and <code>lerp</code>
+ * is a linear interpolation with a factor:
+ *
+ * <pre>
+ * lerp(a,b,t) = a + t * (b - a) = (a * (1 - t)) + (b * t)
+ * </pre>
+ *
+ * <code>t</code> must be in the [0,1] interval. It is easy to see that the
+ * following holds true:
+ *
+ * <pre>
+ * f(0) = b, f(1) = a
+ * </pre>
+ *
+ * And that the function is monotonic. So, the function effectively interpolates
+ * between the maximum and minimum speed through its domain ([0,1] -> [b,a]).
+ *
+ * The curve drawn by this function is similar to 1 / (sqrt(n)), so it is slower
+ * than a linear acceleration (which would be 1/n).
+ *
+ * The floating point version uses a slightly modified function which accepts
+ * the parameter in the domain [0, MT] (where MT is maxTime, the length of the
+ * ramp, which is a setup parameter for the ramp). This is done to reduce the
+ * number of operations per step. The formula looks like this:
+ *
+ * <pre>
+ * a * b * MT
+ * g(t) = ----------------------------
+ * (a * MT) + t * (b - a)
+ * </pre>
+ *
+ * It can be shown that this <code>g(t) = f(t * MT)</code>. The denominator
+ * is a linear interpolation in the range [b*MT, a*MT], as t moves in the
+ * interval [0, MT]. So the interpolation interval of the function is again
+ * [b, a]. The implementation caches the value of the numerator and parts
+ * of the denominator, so that the formula becomes:
+ *
+ * <pre>
+ * alpha = a * b * MT
+ * beta = a * MT
+ * gamma = b - a
+ *
+ * alpha
+ * g(t) = ----------------------
+ * beta + t * gamma
+ * </pre>
+ *
+ * and <code>t</code> is exactly the parameter that ramp_evaluate() gets,
+ * that is the current time (in range [0, MT]). The operations performed
+ * for each step are just an addition, a multiplication and a division.
+ *
+ * The fixed point version of the formula instead transforms the original
+ * function as follows:
+ *
+ * <pre>
+ * a * b a
+ * f(t) = ------------------------- = --------------------
+ * a a
+ * b * ( - * (1 - t) + t ) - * (1 - t) + t
+ * b b
+ * </pre>
+ *
+ * <code>t</code> must be computed by dividing the current time (24 bit integer)
+ * by the maximum time (24 bit integer). This is done by precomputing the
+ * reciprocal of the maximum time as a 0.32 fixed point number, and multiplying
+ * it to the current time. Multiplication is performed 8-bits a time by
+ * FIX_MULT32(), so that we end up with a 0.16 fixed point number for
+ * <code>t</code> (and <code>1-t</code> is just its twos-complement negation).
+ * <code>a/b</code> is in the range [0,1] (because a is always less than b,
+ * being the minimum wavelength), so it is precomputed as a 0.16 fixed point.
+ * The final step is then computing the denominator and executing the division
+ * (32 cycles using the 1-step division instruction in the DSP).
+ *
+ * The assembly implementation is needed for efficiency, but a C version of it
+ * can be easily written, in case it is needed in the future.
+ *
+ */
+
+#include "ramp.h"
+#include <cfg/debug.h>
+
+#include <string.h> // memcpy()
+
+void ramp_compute(struct Ramp *ramp, uint32_t clocksRamp, uint16_t clocksMinWL, uint16_t clocksMaxWL)
+{
+ ASSERT(clocksMaxWL >= clocksMinWL);
+
+ // Save values in ramp struct
+ ramp->clocksRamp = clocksRamp;
+ ramp->clocksMinWL = clocksMinWL;
+ ramp->clocksMaxWL = clocksMaxWL;
+
+#if RAMP_USE_FLOATING_POINT
+ ramp->precalc.gamma = ramp->clocksMaxWL - ramp->clocksMinWL;
+ ramp->precalc.beta = (float)ramp->clocksMinWL * (float)ramp->clocksRamp;
+ ramp->precalc.alpha = ramp->precalc.beta * (float)ramp->clocksMaxWL;
+
+#else
+ ramp->precalc.max_div_min = ((uint32_t)clocksMinWL << 16) / (uint32_t)clocksMaxWL;
+
+ /* Calcola 1/total_time in fixed point .32. Assumiamo che la rampa possa al
+ * massimo avere 25 bit (cioé valore in tick fino a 2^25, che con il
+ * prescaler=3 sono circa 7 secondi). Inoltre, togliamo qualche bit di precisione
+ * da destra (secondo quanto specificato in RAMP_CLOCK_SHIFT_PRECISION).
+ */
+ ASSERT(ramp->clocksRamp < (1UL << (24 + RAMP_CLOCK_SHIFT_PRECISION)));
+ ramp->precalc.inv_total_time = 0xFFFFFFFFUL / (ramp->clocksRamp >> RAMP_CLOCK_SHIFT_PRECISION);
+ ASSERT(ramp->precalc.inv_total_time < 0x1000000UL);
+
+#endif
+}
+
+
+void ramp_setup(struct Ramp* ramp, uint32_t length, uint32_t minFreq, uint32_t maxFreq)
+{
+ uint32_t minWL, maxWL;
+
+ minWL = TIME2CLOCKS(FREQ2MICROS(maxFreq));
+ maxWL = TIME2CLOCKS(FREQ2MICROS(minFreq));
+
+ ASSERT2(minWL < 65536UL, "Maximum frequency too high");
+ ASSERT2(maxWL < 65536UL, "Minimum frequency too high");
+ ASSERT(maxFreq > minFreq);
+
+ ramp_compute(
+ ramp,
+ TIME2CLOCKS(length),
+ TIME2CLOCKS(FREQ2MICROS(maxFreq)),
+ TIME2CLOCKS(FREQ2MICROS(minFreq))
+ );
+}
+
+void ramp_default(struct Ramp *ramp)
+{
+ ramp_setup(ramp, RAMP_DEF_TIME, RAMP_DEF_MINFREQ, RAMP_DEF_MAXFREQ);
+}
+
+#if RAMP_USE_FLOATING_POINT
+
+float ramp_evaluate(const struct Ramp* ramp, float curClock)
+{
+ return ramp->precalc.alpha / (curClock * ramp->precalc.gamma + ramp->precalc.beta);
+}
+
+#else
+
+INLINE uint32_t fix_mult32(uint32_t m1, uint32_t m2)
+{
+ uint32_t accum = 0;
+ accum += m1 * ((m2 >> 0) & 0xFF);
+ accum >>= 8;
+ accum += m1 * ((m2 >> 8) & 0xFF);
+ accum >>= 8;
+ accum += m1 * ((m2 >> 16) & 0xFF);
+ return accum;
+}
+
+// a*b >> 16
+INLINE uint16_t fix_mult16(uint16_t a, uint32_t b)
+{
+ return (b*(uint32_t)a) >> 16;
+}
+
+uint16_t FAST_FUNC ramp_evaluate(const struct Ramp* ramp, uint32_t curClock)
+{
+ uint16_t t = FIX_MULT32(curClock >> RAMP_CLOCK_SHIFT_PRECISION, ramp->precalc.inv_total_time);
+ uint16_t denom = fix_mult16((uint16_t)~t + 1, ramp->precalc.max_div_min) + t;
+ uint16_t cur_delta = ((uint32_t)ramp->clocksMinWL << 16) / denom;
+
+ return cur_delta;
+}
+
+#endif
+
+
diff --git a/elec/boards/Rotary_Beacon/Firmware/bertos/algo/ramp.h b/elec/boards/Rotary_Beacon/Firmware/bertos/algo/ramp.h
new file mode 100644
index 0000000..ad3c919
--- /dev/null
+++ b/elec/boards/Rotary_Beacon/Firmware/bertos/algo/ramp.h
@@ -0,0 +1,166 @@
+/**
+ * \file
+ * <!--
+ * Copyright 2004, 2008 Develer S.r.l. (http://www.develer.com/)
+ * All Rights Reserved.
+ * -->
+ *
+ * \brief Compute, save and load ramps for stepper motors.
+ *
+ * The acceleration ramp is used to properly accelerate a stepper motor. The main
+ * entry point is the function ramp_evaluate(), which must be called at every step
+ * of the motor: it gets as input the time elapsed since the stepper started
+ * accelerating, and returns the time to wait before sending the next step. A pseudo
+ * usage pattern is as follows:
+ *
+ * <pre>
+ * float time = 0;
+ * while (1)
+ * {
+ * float delta = ramp_evaluate(&my_ramp, time);
+ * sleep(delta);
+ * do_motor_step();
+ * time += delta;
+ * }
+ * </pre>
+ *
+ * A similar pattern can be used to decelerate (it is sufficient to move the total
+ * time backward, such as "time -= delta").
+ *
+ * The ramp can be configured with ramp_setup(), providing it with the minimum and
+ * maximum operating frequency of the motor, and the total acceleration time in
+ * milliseconds (that is, the time that will be needed to accelerate from the
+ * minimum frequency to the maximum frequency).
+ *
+ * Both a very precise floating point and a very fast fixed point implementation
+ * of the ramp evaluation are provided. The fixed point is hand-optimized assembly
+ * for DSP56000 (but a portable C version of it can be easily written, see the
+ * comments in the code).
+ *
+ *
+ * \author Simone Zinanni <s.z...@de...>
+ * \author Giovanni Bajo <ra...@de...>
+ * \author Daniele Basile <as...@de...>
+ *
+ * $WIZ$ module_name = "ramp"
+ * $WIZ$ module_configuration = "bertos/cfg/cfg_ramp.h"
+ */
+
+#ifndef ALGO_RAMP_H
+#define ALGO_RAMP_H
+
+#include "hw/hw_stepper.h"
+
+#include "cfg/cfg_ramp.h"
+
+#include <cfg/compiler.h>
+
+
+/**
+ * Convert microseconds to timer clock ticks
+ */
+#define TIME2CLOCKS(micros) ((uint32_t)(micros) * (STEPPER_CLOCK / 1000000))
+
+/**
+ * Convert timer clock ticks back to microseconds
+ */
+#define CLOCKS2TIME(clocks) ((uint32_t)(clocks) / (STEPPER_CLOCK / 1000000))
+
+/**
+ * Convert microseconds to Hz
+ */
+#define MICROS2FREQ(micros) (1000000UL / ((uint32_t)(micros)))
+
+/**
+ * Convert frequency (in Hz) to time (in microseconds)
+ */
+#define FREQ2MICROS(hz) (1000000UL / ((uint32_t)(hz)))
+
+/**
+ * Multiply \p a and \p b two integer at 32 bit and extract the high 16 bit word.
+ */
+#define FIX_MULT32(a,b) (((uint64_t)(a)*(uint32_t)(b)) >> 16)
+
+/**
+ * Structure holding pre-calculated data for speeding up real-time evaluation
+ * of the ramp. This structure is totally different between the fixed and the
+ * floating point version of the code.
+ *
+ * Consult the file-level documentation of ramp.c for more information about
+ * the values of this structure.
+ */
+struct RampPrecalc
+{
+#if RAMP_USE_FLOATING_POINT
+ float beta;
+ float alpha;
+ float gamma;
+#else
+ uint16_t max_div_min;
+ uint32_t inv_total_time;
+#endif
+};
+
+
+/**
+ * Ramp structure
+ */
+struct Ramp
+{
+ uint32_t clocksRamp;
+ uint16_t clocksMinWL;
+ uint16_t clocksMaxWL;
+
+ struct RampPrecalc precalc; ///< pre-calculated values for speed
+};
+
+
+/*
+ * Function prototypes
+ */
+void ramp_compute(
+ struct Ramp * ramp,
+ uint32_t clocksInRamp,
+ uint16_t clocksInMinWavelength,
+ uint16_t clocksInMaxWavelength);
+
+
+/** Setup an acceleration ramp for a stepper motor
+ *
+ * \param ramp Ramp to fill
+ * \param length Length of the ramp (milliseconds)
+ * \param minFreq Minimum operating frequency of the motor (hertz)
+ * \param maxFreq Maximum operating frequency of the motor (hertz)
+ *
+ */
+void ramp_setup(struct Ramp* ramp, uint32_t length, uint32_t minFreq, uint32_t maxFreq);
+
+
+/**
+ * Initialize a new ramp with default values
+ */
+void ramp_default(struct Ramp *ramp);
+
+
+/**
+ * Evaluate the ramp at the given point. Given a \a ramp, and the current \a clock since
+ * the start of the acceleration, compute the next step, that is the interval at which
+ * send the signal to the motor.
+ *
+ * \note The fixed point version does not work when curClock is zero. Anyway,
+ * the first step is always clocksMaxWL, as stored within the ramp structure.
+ */
+#if RAMP_USE_FLOATING_POINT
+ float ramp_evaluate(const struct Ramp* ramp, float curClock);
+#else
+ uint16_t ramp_evaluate(const struct Ramp* ramp, uint32_t curClock);
+#endif
+
+
+/** Self test */
+int ramp_testSetup(void);
+int ramp_testRun(void);
+int ramp_testTearDown(void);
+
+#endif /* ALGO_RAMP_H */
+
diff --git a/elec/boards/Rotary_Beacon/Firmware/bertos/algo/ramp_test.c b/elec/boards/Rotary_Beacon/Firmware/bertos/algo/ramp_test.c
new file mode 100644
index 0000000..0e7d58c
--- /dev/null
+++ b/elec/boards/Rotary_Beacon/Firmware/bertos/algo/ramp_test.c
@@ -0,0 +1,186 @@
+/*!
+ * \file
+ * <!--
+ * Copyright 2004, 2008 Develer S.r.l. (http://www.develer.com/)
+ * All Rights Reserved.
+ * -->
+ *
+ * \brief Test for compute, save and load ramps for stepper motors (implementation)
+ *
+ *
+ * \author Simone Zinanni <s.z...@de...>
+ * \author Bernie Innocenti <be...@co...>
+ * \author Giovanni Bajo <ra...@de...>
+ * \author Daniele Basile <as...@de...>
+ *
+ *
+ * The formula used by the ramp is the following:
+ *
+ * <pre>
+ * a * b
+ * f(t) = -------------
+ * lerp(a,b,t)
+ * </pre>
+ *
+ * Where <code>a</code> and <code>b</code> are the maximum and minimum speed
+ * respectively (minimum and maximum wavelength respectively), and <code>lerp</code>
+ * is a linear interpolation with a factor:
+ *
+ * <pre>
+ * lerp(a,b,t) = a + t * (b - a) = (a * (1 - t)) + (b * t)
+ * </pre>
+ *
+ * <code>t</code> must be in the [0,1] interval. It is easy to see that the
+ * following holds true:
+ *
+ * <pre>
+ * f(0) = b, f(1) = a
+ * </pre>
+ *
+ * And that the function is monotonic. So, the function effectively interpolates
+ * between the maximum and minimum speed through its domain ([0,1] -> [b,a]).
+ *
+ * The curve drawn by this function is similar to 1 / (sqrt(n)), so it is slower
+ * than a linear acceleration (which would be 1/n).
+ *
+ * The floating point version uses a slightly modified function which accepts
+ * the parameter in the domain [0, MT] (where MT is maxTime, the length of the
+ * ramp, which is a setup parameter for the ramp). This is done to reduce the
+ * number of operations per step. The formula looks like this:
+ *
+ * <pre>
+ * a * b * MT
+ * g(t) = ----------------------------
+ * (a * MT) + t * (b - a)
+ * </pre>
+ *
+ * It can be shown that this <code>g(t) = f(t * MT)</code>. The denominator
+ * is a linear interpolation in the range [b*MT, a*MT], as t moves in the
+ * interval [0, MT]. So the interpolation interval of the function is again
+ * [b, a]. The implementation caches the value of the numerator and parts
+ * of the denominator, so that the formula becomes:
+ *
+ * <pre>
+ * alpha = a * b * MT
+ * beta = a * MT
+ * gamma = b - a
+ *
+ * alpha
+ * g(t) = ----------------------
+ * beta + t * gamma
+ * </pre>
+ *
+ * and <code>t</code> is exactly the parameter that ramp_evaluate() gets,
+ * that is the current time (in range [0, MT]). The operations performed
+ * for each step are just an addition, a multiplication and a division.
+ *
+ * The fixed point version of the formula instead transforms the original
+ * function as follows:
+ *
+ * <pre>
+ * a * b a
+ * f(t) = ------------------------- = --------------------
+ * a a
+ * b * ( - * (1 - t) + t ) - * (1 - t) + t
+ * b b
+ * </pre>
+ *
+ * <code>t</code> must be computed by dividing the current time (24 bit integer)
+ * by the maximum time (24 bit integer). This is done by precomputing the
+ * reciprocal of the maximum time as a 0.32 fixed point number, and multiplying
+ * it to the current time. Multiplication is performed 8-bits a time by
+ * FIX_MULT32(), so that we end up with a 0.16 fixed point number for
+ * <code>t</code> (and <code>1-t</code> is just its twos-complement negation).
+ * <code>a/b</code> is in the range [0,1] (because a is always less than b,
+ * being the minimum wavelength), so it is precomputed as a 0.16 fixed point.
+ * The final step is then computing the denominator and executing the division
+ * (32 cycles using the 1-step division instruction in the DSP).
+ *
+ * The assembly implementation is needed for efficiency, but a C version of it
+ * can be easily written, in case it is needed in the future.
+ *
+ */
+
+#include "ramp.h"
+#include <cfg/debug.h>
+#include <cfg/test.h>
+
+
+static bool ramp_test_single(uint32_t minFreq, uint32_t maxFreq, uint32_t length)
+{
+ struct Ramp r;
+ uint16_t cur, old;
+ uint32_t clock;
+ uint32_t oldclock;
+
+ ramp_setup(&r, length, minFreq, maxFreq);
+
+ cur = old = r.clocksMaxWL;
+ clock = 0;
+ oldclock = 0;
+
+ kprintf("testing ramp: (length=%lu, min=%lu, max=%lu)\n", (unsigned long)length, (unsigned long)minFreq, (unsigned long)maxFreq);
+ kprintf(" [length=%lu, max=%04x, min=%04x]\n", (unsigned long)r.clocksRamp, r.clocksMaxWL, r.clocksMinWL);
+
+ int i = 0;
+ int nonbyte = 0;
+
+ while (clock + cur < r.clocksRamp)
+ {
+ oldclock = clock;
+ old = cur;
+
+ clock += cur;
+ cur = ramp_evaluate(&r, clock);
+
+ if (old < cur)
+ {
+ uint16_t t1 = FIX_MULT32(oldclock >> RAMP_CLOCK_SHIFT_PRECISION, r.precalc.inv_total_time);
+ uint16_t t2 = FIX_MULT32(clock >> RAMP_CLOCK_SHIFT_PRECISION, r.precalc.inv_total_time);
+ uint16_t denom1 = FIX_MULT32((uint16_t)((~t1) + 1), r.precalc.max_div_min) + t1;
+ uint16_t denom2 = FIX_MULT32((uint16_t)((~t2) + 1), r.precalc.max_div_min) + t2;
+
+ kprintf(" Failed: %04x @ %lu --> %04x @ %lu\n", old, (unsigned long)oldclock, cur, (unsigned long)clock);
+ kprintf(" T: %04x -> %04x\n", t1, t2);
+ kprintf(" DENOM: %04x -> %04x\n", denom1, denom2);
+
+ cur = ramp_evaluate(&r, clock);
+ return false;
+ }
+ i++;
+ if ((old-cur) >= 256)
+ nonbyte++;
+ }
+
+
+
+ kprintf("Test finished: %04x @ %lu [min=%04x, totlen=%lu, numsteps:%d, nonbyte:%d]\n", cur, (unsigned long)clock, r.clocksMinWL, (unsigned long)r.clocksRamp, i, nonbyte);
+
+ return true;
+}
+
+int ramp_testSetup(void)
+{
+ kdbg_init();
+ return 0;
+}
+
+int ramp_testTearDown(void)
+{
+ return 0;
+}
+
+int ramp_testRun(void)
+{
+ #define TEST_RAMP(min, max, len) do { \
+ if (!ramp_test_single(min, max, len)) \
+ return -1; \
+ } while(0)
+
+ TEST_RAMP(200, 5000, 3000000);
+ TEST_RAMP(1000, 2000, 1000000);
+
+ return 0;
+}
+
+TEST_MAIN(ramp);
diff --git a/elec/boards/Rotary_Beacon/Firmware/bertos/algo/rand.c b/elec/boards/Rotary_Beacon/Firmware/bertos/algo/rand.c
new file mode 100644
index 0000000..901e23f
--- /dev/null
+++ b/elec/boards/Rotary_Beacon/Firmware/bertos/algo/rand.c
@@ -0,0 +1,52 @@
+/**
+ * \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 2006 Develer S.r.l. (http://www.develer.com/)
+ * Copyright 1999 Bernie Innocenti <be...@co...>
+ *
+ * -->
+ *
+ * \brief Very simple rand() algorithm.
+ *
+ * \author Bernie Innocenti <be...@co...>
+ */
+
+#include "rand.h"
+
+/**
+ * This would really belong to libc
+ */
+int rand(void)
+{
+ static unsigned long seed;
+
+ /* Randomize seed */
+ seed = (seed ^ 0x4BAD5A39UL) + 6513973UL;
+
+...
[truncated message content] |
|
From: Nicolas D. <Ba...@us...> - 2011-04-03 12:44: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 359be2f1968db3385d01c70152350ce9f8a282c8 (commit)
via 19089f6db1f39f24cf16f6205a818fb1ccac7032 (commit)
via 1219a1b834232b76b2f02e29f2e00c3c463a9d42 (commit)
from 5e6b68d0723980a8319d0f55cbdfb845d249f162 (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 359be2f1968db3385d01c70152350ce9f8a282c8
Author: Nicolas Dandrimont <Nic...@cr...>
Date: Sun Apr 3 14:43:18 2011 +0200
[krobot_viewer] Add more info about the beacon in the UI
commit 19089f6db1f39f24cf16f6205a818fb1ccac7032
Author: Nicolas Dandrimont <Nic...@cr...>
Date: Sun Apr 3 14:43:10 2011 +0200
[krobot_viewer] consider the rotary beacon index position
commit 1219a1b834232b76b2f02e29f2e00c3c463a9d42
Author: Nicolas Dandrimont <Nic...@cr...>
Date: Sun Apr 3 14:42:01 2011 +0200
[krobot_message] Use the right data size for the Beacon low level packet
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/lib/krobot_config.ml b/info/control2011/src/lib/krobot_config.ml
index acb9ff9..991a55d 100644
--- a/info/control2011/src/lib/krobot_config.ml
+++ b/info/control2011/src/lib/krobot_config.ml
@@ -13,3 +13,4 @@ let robot_size = 0.3
let wheels_diameter = 0.098
let wheels_distance = 0.259
let wheels_position = 0.045
+let rotary_beacon_index_pos = (4. *. atan 1.) /. 2. (* left side *)
diff --git a/info/control2011/src/lib/krobot_config.mli b/info/control2011/src/lib/krobot_config.mli
index bf336d5..266daaf 100644
--- a/info/control2011/src/lib/krobot_config.mli
+++ b/info/control2011/src/lib/krobot_config.mli
@@ -27,3 +27,7 @@ val wheels_distance : float
val wheels_position : float
(** The distance between the axe of the wheels and the back of the
robot. *)
+
+val rotary_beacon_index_pos : float
+ (** The angle of the rotary beacon index angle with respect to the
+ robot's front *)
diff --git a/info/control2011/src/lib/krobot_message.ml b/info/control2011/src/lib/krobot_message.ml
index 3c3e51b..111d91a 100644
--- a/info/control2011/src/lib/krobot_message.ml
+++ b/info/control2011/src/lib/krobot_message.ml
@@ -203,10 +203,10 @@ let encode = function
~format:F29bits
~data
| Beacon_lowlevel_position(angle, width, period) ->
- let data = String.create 6 in
+ let data = String.create 8 in
put_uint16 data 0 (truncate (angle *. 10000.));
put_uint16 data 2 (truncate (width *. 100000.));
- put_uint16 data 4 period;
+ put_uint32 data 4 period;
frame
~identifier:302
~kind:Data
@@ -305,7 +305,7 @@ let decode frame =
Beacon_lowlevel_position
(float (get_uint16 frame.data 0) /. 10000.,
float (get_uint16 frame.data 2) /. 100000.,
- get_uint16 frame.data 4)
+ get_uint32 frame.data 4)
| _ ->
Unknown frame
with Invalid_argument _ ->
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index c83ff15..fe95de4 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -556,14 +556,17 @@ module Board = struct
board.ui#entry_moving3#set_text (if m3 then "yes" else "no");
board.ui#entry_moving4#set_text (if m4 then "yes" else "no")
| Beacon_position(angle, distance, period) ->
- let newangle = math_mod_float (board.state.theta +. angle) (2. *. pi) in
+ let newangle = math_mod_float (board.state.theta +. Krobot_config.rotary_beacon_index_pos +. angle) (2. *. pi) in
let x = board.state.x +. distance *. cos (newangle) in
let y = board.state.y +. distance *. sin (newangle) in
let valid = distance <> 0. in
let beacon = { xbeacon = x; ybeacon = y; valid; } in
if beacon <> board.beacon then begin
board.beacon <- beacon;
- board.ui#entry_beacon#set_text (if valid then "valid" else "invalid");
+ board.ui#beacon_status#set_text (if valid then "valid" else "invalid");
+ board.ui#beacon_distance#set_text (string_of_float distance);
+ board.ui#beacon_angle#set_text (string_of_float angle);
+ board.ui#beacon_period#set_text (string_of_float period);
queue_draw board
end
| _ ->
diff --git a/info/control2011/src/tools/krobot_viewer_ui.glade b/info/control2011/src/tools/krobot_viewer_ui.glade
index 45fbd13..485278d 100644
--- a/info/control2011/src/tools/krobot_viewer_ui.glade
+++ b/info/control2011/src/tools/krobot_viewer_ui.glade
@@ -793,11 +793,56 @@
</packing>
</child>
<child>
- <widget class="GtkEntry" id="entry_beacon">
+ <widget class="GtkHBox" id="hbox17">
<property name="visible">True</property>
- <property name="can_focus">True</property>
- <property name="editable">False</property>
- <property name="invisible_char">•</property>
+ <child>
+ <widget class="GtkEntry" id="beacon_status">
+ <property name="width_request">1</property>
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="editable">False</property>
+ <property name="invisible_char">•</property>
+ </widget>
+ <packing>
+ <property name="position">0</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkEntry" id="beacon_distance">
+ <property name="width_request">1</property>
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="editable">False</property>
+ <property name="invisible_char">•</property>
+ </widget>
+ <packing>
+ <property name="position">1</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkEntry" id="beacon_angle">
+ <property name="width_request">1</property>
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="editable">False</property>
+ <property name="invisible_char">•</property>
+ </widget>
+ <packing>
+ <property name="position">2</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkEntry" id="beacon_period">
+ <property name="width_request">1</property>
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="editable">False</property>
+ <property name="invisible_char">•</property>
+ </widget>
+ <packing>
+ <property name="position">3</property>
+ </packing>
+ </child>
</widget>
<packing>
<property name="left_attach">1</property>
hooks/post-receive
--
krobot
|
|
From: Nicolas D. <Ba...@us...> - 2011-04-03 02:32:24
|
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 5e6b68d0723980a8319d0f55cbdfb845d249f162 (commit)
from 6d90b631730d84819ae86b701829dc35c1b173f8 (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 5e6b68d0723980a8319d0f55cbdfb845d249f162
Author: Nicolas Dandrimont <Nic...@cr...>
Date: Sun Apr 3 03:41:27 2011 +0200
[krobot_viewer] Initialize the robot to a correct position
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index 5fa46f5..c83ff15 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -523,7 +523,11 @@ module Board = struct
let board ={
bus;
ui;
- state = { x = 0.2; y = 1.9; theta = -0.5 *. pi };
+ state = {
+ x = 0.2;
+ y = 1.9 +. Krobot_config.robot_size /. 2. -. Krobot_config.wheels_position;
+ theta = -0.5 *. pi
+ };
beacon = { xbeacon = 1.; ybeacon = 1.; valid = true };
points = [];
event = E.never;
hooks/post-receive
--
krobot
|
|
From: Nicolas D. <Ba...@us...> - 2011-04-02 20:57: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 6d90b631730d84819ae86b701829dc35c1b173f8 (commit)
via b972d273f58b18254fb24218becefe72ba47dd06 (commit)
from de73d5b2e4253bee8b5a9db7b97cb2138db25b75 (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 6d90b631730d84819ae86b701829dc35c1b173f8
Author: Nicolas Dandrimont <Nic...@cr...>
Date: Sat Apr 2 22:57:13 2011 +0200
[krobot_viewer] Display the status of the rotary beacon
commit b972d273f58b18254fb24218becefe72ba47dd06
Author: Nicolas Dandrimont <Nic...@cr...>
Date: Sat Apr 2 22:56:47 2011 +0200
[krobot_message] Implement message types for the rotary beacon
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/lib/krobot_message.ml b/info/control2011/src/lib/krobot_message.ml
index 60d13dd..3c3e51b 100644
--- a/info/control2011/src/lib/krobot_message.ml
+++ b/info/control2011/src/lib/krobot_message.ml
@@ -19,6 +19,8 @@ open Lwt_react
type direction = Forward | Backward
type t =
+ | Beacon_position of float * float * float
+ | Beacon_lowlevel_position of float * float * int
| Encoder_position_direction_3_4 of int * direction * int * direction
| Encoder_position_speed_3 of float * float
| Encoder_position_speed_4 of float * float
@@ -40,6 +42,18 @@ let string_of_direction = function
| Backward -> "Backward"
let to_string = function
+ | Beacon_position(angle, distance, period) ->
+ sprintf
+ "Beacon_position(%f, %f, %f)"
+ angle
+ distance
+ period
+ | Beacon_lowlevel_position(angle, width, period) ->
+ sprintf
+ "Beacon_lowlevel_position(%f, %f, %d)"
+ angle
+ width
+ period
| Encoder_position_direction_3_4(pos3, dir3, pos4, dir4) ->
sprintf
"Encoder_position_direction_3_4(%d, %s, %d, %s)"
@@ -177,6 +191,28 @@ let encode = function
~remote:false
~format:F29bits
~data
+ | Beacon_position(angle, length, period) ->
+ let data = String.create 6 in
+ put_uint16 data 0 (truncate (angle *. 10000.));
+ put_uint16 data 2 (truncate (length *. 1000.));
+ put_uint16 data 4 (truncate (period *. 10000.));
+ frame
+ ~identifier:301
+ ~kind:Data
+ ~remote:false
+ ~format:F29bits
+ ~data
+ | Beacon_lowlevel_position(angle, width, period) ->
+ let data = String.create 6 in
+ put_uint16 data 0 (truncate (angle *. 10000.));
+ put_uint16 data 2 (truncate (width *. 100000.));
+ put_uint16 data 4 period;
+ frame
+ ~identifier:302
+ ~kind:Data
+ ~remote:false
+ ~format:F29bits
+ ~data
| Motor_stop ->
frame
~identifier:204
@@ -260,6 +296,16 @@ let decode frame =
float (get_sint16 frame.data 4) /. 10000.)
| 204 ->
Motor_stop
+ | 301 ->
+ Beacon_position
+ (float (get_uint16 frame.data 0) /. 10000.,
+ float (get_uint16 frame.data 2) /. 1000.,
+ float (get_uint16 frame.data 4) /. 10000.)
+ | 302 ->
+ Beacon_lowlevel_position
+ (float (get_uint16 frame.data 0) /. 10000.,
+ float (get_uint16 frame.data 2) /. 100000.,
+ get_uint16 frame.data 4)
| _ ->
Unknown frame
with Invalid_argument _ ->
diff --git a/info/control2011/src/lib/krobot_message.mli b/info/control2011/src/lib/krobot_message.mli
index 5d83d7e..70f56b6 100644
--- a/info/control2011/src/lib/krobot_message.mli
+++ b/info/control2011/src/lib/krobot_message.mli
@@ -14,6 +14,10 @@ type direction = Forward | Backward
(** Type of messages. *)
type t =
+ | Beacon_position of float * float * float
+ (** The position of the beacon relative to the robot *)
+ | Beacon_lowlevel_position of float * float * int
+ (** The position of the beacon as internally stored (for calibration) *)
| Encoder_position_direction_3_4 of int * direction * int * direction
(** The position and direction of encoders 3 and 4. *)
| Encoder_position_speed_3 of float * float
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index c3b1757..5fa46f5 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -186,10 +186,17 @@ module Board = struct
theta : float;
}
+ type beacon = {
+ xbeacon : float;
+ ybeacon : float;
+ valid : bool;
+ }
+
type t = {
bus : Krobot_bus.t;
ui : Krobot_viewer_ui.window;
mutable state : state;
+ mutable beacon : beacon;
mutable points : (float * float) list;
mutable event : unit event;
mutable moving : bool;
@@ -202,6 +209,7 @@ module Board = struct
| Red
| Blue
| Yellow
+ | Purple
let set_color ctx color =
let r, g, b = match color with
@@ -211,6 +219,7 @@ module Board = struct
| Red -> (199., 23., 18.)
| Blue -> (0., 59., 128.)
| Yellow -> (252., 189., 31.)
+ | Purple -> (128., 0., 128.)
in
Cairo.set_source_rgb ctx (r /. 255.) (g /. 255.) (b /. 255.)
@@ -379,6 +388,13 @@ module Board = struct
Cairo.restore ctx;
+ (* Draw the beacon *)
+ if board.beacon.valid then begin
+ Cairo.arc ctx board.beacon.xbeacon board.beacon.ybeacon 0.04 0. (2. *. pi);
+ set_color ctx Purple;
+ Cairo.fill ctx;
+ end;
+
(* Draw points. *)
Cairo.set_source_rgb ctx 255. 255. 0.;
Cairo.move_to ctx board.state.x board.state.y;
@@ -508,6 +524,7 @@ module Board = struct
bus;
ui;
state = { x = 0.2; y = 1.9; theta = -0.5 *. pi };
+ beacon = { xbeacon = 1.; ybeacon = 1.; valid = true };
points = [];
event = E.never;
moving = false;
@@ -534,6 +551,17 @@ module Board = struct
board.ui#entry_moving2#set_text (if m2 then "yes" else "no");
board.ui#entry_moving3#set_text (if m3 then "yes" else "no");
board.ui#entry_moving4#set_text (if m4 then "yes" else "no")
+ | Beacon_position(angle, distance, period) ->
+ let newangle = math_mod_float (board.state.theta +. angle) (2. *. pi) in
+ let x = board.state.x +. distance *. cos (newangle) in
+ let y = board.state.y +. distance *. sin (newangle) in
+ let valid = distance <> 0. in
+ let beacon = { xbeacon = x; ybeacon = y; valid; } in
+ if beacon <> board.beacon then begin
+ board.beacon <- beacon;
+ board.ui#entry_beacon#set_text (if valid then "valid" else "invalid");
+ queue_draw board
+ end
| _ ->
())
(Krobot_message.recv bus)
diff --git a/info/control2011/src/tools/krobot_viewer_ui.glade b/info/control2011/src/tools/krobot_viewer_ui.glade
index 56ec218..45fbd13 100644
--- a/info/control2011/src/tools/krobot_viewer_ui.glade
+++ b/info/control2011/src/tools/krobot_viewer_ui.glade
@@ -301,7 +301,7 @@
<child>
<widget class="GtkTable" id="table1">
<property name="visible">True</property>
- <property name="n_rows">9</property>
+ <property name="n_rows">10</property>
<property name="n_columns">2</property>
<child>
<widget class="GtkSpinButton" id="moving_speed">
@@ -315,8 +315,8 @@
<packing>
<property name="left_attach">1</property>
<property name="right_attach">2</property>
- <property name="top_attach">5</property>
- <property name="bottom_attach">6</property>
+ <property name="top_attach">6</property>
+ <property name="bottom_attach">7</property>
<property name="y_options">GTK_FILL</property>
</packing>
</child>
@@ -332,8 +332,8 @@
<packing>
<property name="left_attach">1</property>
<property name="right_attach">2</property>
- <property name="top_attach">6</property>
- <property name="bottom_attach">7</property>
+ <property name="top_attach">7</property>
+ <property name="bottom_attach">8</property>
<property name="y_options">GTK_FILL</property>
</packing>
</child>
@@ -349,8 +349,8 @@
<packing>
<property name="left_attach">1</property>
<property name="right_attach">2</property>
- <property name="top_attach">7</property>
- <property name="bottom_attach">8</property>
+ <property name="top_attach">8</property>
+ <property name="bottom_attach">9</property>
<property name="y_options">GTK_FILL</property>
</packing>
</child>
@@ -366,8 +366,8 @@
<packing>
<property name="left_attach">1</property>
<property name="right_attach">2</property>
- <property name="top_attach">8</property>
- <property name="bottom_attach">9</property>
+ <property name="top_attach">9</property>
+ <property name="bottom_attach">10</property>
<property name="y_options">GTK_FILL</property>
</packing>
</child>
@@ -397,8 +397,8 @@
</child>
</widget>
<packing>
- <property name="top_attach">5</property>
- <property name="bottom_attach">6</property>
+ <property name="top_attach">6</property>
+ <property name="bottom_attach">7</property>
<property name="x_options">GTK_FILL</property>
<property name="y_options">GTK_FILL</property>
</packing>
@@ -429,8 +429,8 @@
</child>
</widget>
<packing>
- <property name="top_attach">6</property>
- <property name="bottom_attach">7</property>
+ <property name="top_attach">7</property>
+ <property name="bottom_attach">8</property>
<property name="x_options">GTK_FILL</property>
<property name="y_options">GTK_FILL</property>
</packing>
@@ -461,8 +461,8 @@
</child>
</widget>
<packing>
- <property name="top_attach">7</property>
- <property name="bottom_attach">8</property>
+ <property name="top_attach">8</property>
+ <property name="bottom_attach">9</property>
<property name="x_options">GTK_FILL</property>
<property name="y_options">GTK_FILL</property>
</packing>
@@ -493,8 +493,8 @@
</child>
</widget>
<packing>
- <property name="top_attach">8</property>
- <property name="bottom_attach">9</property>
+ <property name="top_attach">9</property>
+ <property name="bottom_attach">10</property>
<property name="x_options">GTK_FILL</property>
<property name="y_options">GTK_FILL</property>
</packing>
@@ -555,8 +555,8 @@
</child>
</widget>
<packing>
- <property name="top_attach">2</property>
- <property name="bottom_attach">3</property>
+ <property name="top_attach">3</property>
+ <property name="bottom_attach">4</property>
</packing>
</child>
<child>
@@ -585,8 +585,8 @@
</child>
</widget>
<packing>
- <property name="top_attach">3</property>
- <property name="bottom_attach">4</property>
+ <property name="top_attach">4</property>
+ <property name="bottom_attach">5</property>
</packing>
</child>
<child>
@@ -615,8 +615,8 @@
</child>
</widget>
<packing>
- <property name="top_attach">4</property>
- <property name="bottom_attach">5</property>
+ <property name="top_attach">5</property>
+ <property name="bottom_attach">6</property>
</packing>
</child>
<child>
@@ -629,8 +629,8 @@
<packing>
<property name="left_attach">1</property>
<property name="right_attach">2</property>
- <property name="top_attach">2</property>
- <property name="bottom_attach">3</property>
+ <property name="top_attach">3</property>
+ <property name="bottom_attach">4</property>
</packing>
</child>
<child>
@@ -643,8 +643,8 @@
<packing>
<property name="left_attach">1</property>
<property name="right_attach">2</property>
- <property name="top_attach">3</property>
- <property name="bottom_attach">4</property>
+ <property name="top_attach">4</property>
+ <property name="bottom_attach">5</property>
</packing>
</child>
<child>
@@ -657,8 +657,8 @@
<packing>
<property name="left_attach">1</property>
<property name="right_attach">2</property>
- <property name="top_attach">4</property>
- <property name="bottom_attach">5</property>
+ <property name="top_attach">5</property>
+ <property name="bottom_attach">6</property>
</packing>
</child>
<child>
@@ -762,6 +762,50 @@
</child>
</widget>
</child>
+ <child>
+ <widget class="GtkHBox" id="hbox16">
+ <property name="visible">True</property>
+ <child>
+ <widget class="GtkLabel" id="label10">
+ <property name="visible">True</property>
+ <property name="label" translatable="yes">Beacon status:</property>
+ </widget>
+ <packing>
+ <property name="expand">False</property>
+ <property name="position">0</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkAlignment" id="alignment10">
+ <property name="visible">True</property>
+ <child>
+ <placeholder/>
+ </child>
+ </widget>
+ <packing>
+ <property name="position">1</property>
+ </packing>
+ </child>
+ </widget>
+ <packing>
+ <property name="top_attach">2</property>
+ <property name="bottom_attach">3</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkEntry" id="entry_beacon">
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="editable">False</property>
+ <property name="invisible_char">•</property>
+ </widget>
+ <packing>
+ <property name="left_attach">1</property>
+ <property name="right_attach">2</property>
+ <property name="top_attach">2</property>
+ <property name="bottom_attach">3</property>
+ </packing>
+ </child>
</widget>
<packing>
<property name="expand">False</property>
hooks/post-receive
--
krobot
|
|
From: Jérémie D. <Ba...@us...> - 2011-04-02 18:04: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 de73d5b2e4253bee8b5a9db7b97cb2138db25b75 (commit)
from fcc0f991f25242305a7c22757a00c38189b89e06 (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 de73d5b2e4253bee8b5a9db7b97cb2138db25b75
Author: Jérémie Dimino <je...@di...>
Date: Sat Apr 2 20:04:06 2011 +0200
[krobot_viewer] make the arrow to start between the wheels
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index cdda66b..c3b1757 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -369,7 +369,7 @@ module Board = struct
(* Draw an arrow on the robot *)
let d = robot_size /. 2. -. wheels_position in
- Cairo.move_to ctx (d -. robot_size /. 4.) 0.;
+ Cairo.move_to ctx 0. 0.;
Cairo.line_to ctx (d +. robot_size /. 4.) 0.;
Cairo.line_to ctx d (-. robot_size /. 4.);
Cairo.line_to ctx d (robot_size /. 4.);
hooks/post-receive
--
krobot
|
|
From: Jérémie D. <Ba...@us...> - 2011-04-02 16:28:16
|
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 fcc0f991f25242305a7c22757a00c38189b89e06 (commit)
from cdde1ef37393f7bd15d0fd5038c197b8a0e2d421 (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 fcc0f991f25242305a7c22757a00c38189b89e06
Author: Jérémie Dimino <je...@di...>
Date: Sat Apr 2 18:26:28 2011 +0200
[info] fix serialization of signed int32s on 32bits
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/lib/krobot_can.ml b/info/control2011/src/lib/krobot_can.ml
index 8b40c46..c5d9094 100644
--- a/info/control2011/src/lib/krobot_can.ml
+++ b/info/control2011/src/lib/krobot_can.ml
@@ -129,7 +129,7 @@ let put_uint8 = put_sint8
let put_sint16 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 asr 8)
let put_uint16 = put_sint16
@@ -137,7 +137,7 @@ let put_sint32 str ofs v =
str.[ofs + 0] <- Char.unsafe_chr v;
str.[ofs + 1] <- Char.unsafe_chr (v lsr 8);
str.[ofs + 2] <- Char.unsafe_chr (v lsr 16);
- str.[ofs + 3] <- Char.unsafe_chr (v lsr 24)
+ str.[ofs + 3] <- Char.unsafe_chr (v asr 24)
let put_uint32 = put_sint32
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2011-04-02 16:03: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 cdde1ef37393f7bd15d0fd5038c197b8a0e2d421 (commit)
from 275e297d6df8697614623704b7f136683ea71af1 (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 cdde1ef37393f7bd15d0fd5038c197b8a0e2d421
Author: Xavier Lagorce <Xav...@cr...>
Date: Sat Apr 2 18:02:55 2011 +0200
[Controller_Motor_STM32] Fixed bug in callback generation with differential drive
-----------------------------------------------------------------------
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 ac57d5d..264cf7d 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
@@ -42,13 +42,17 @@ command_generator_t* new_ramp2_generator(command_generator_t *generator, float s
}
command_generator_t* new_dd_generator(command_generator_t *generator,
+ command_generator_t *linear_pos,
command_generator_t *linear_speed,
+ command_generator_t *rotational_pos,
command_generator_t *rotational_speed,
float wheel_radius, float shaft_width,
uint8_t type) {
generator->type.t = (type == 1) ? GEN_DD_RIGHT : GEN_DD_LEFT;
generator->type.callback.type = GEN_CALLBACK_NONE;
+ generator->dd.linear_pos = linear_pos;
generator->dd.linear_speed = linear_speed;
+ generator->dd.rotational_pos = rotational_pos;
generator->dd.rotational_speed = rotational_speed;
generator->dd.wheel_radius = wheel_radius;
generator->dd.shaft_width = shaft_width;
@@ -138,11 +142,19 @@ float get_output_value(command_generator_t *generator) {
generator->ramp2.last_time = cur_time;
break;
case GEN_DD_RIGHT:
+ // Update position generators to allow callbacks
+ get_output_value(generator->dd.linear_pos);
+ get_output_value(generator->dd.rotational_pos);
+ // Compute output
u1 = get_output_value(generator->dd.linear_speed);
u2 = get_output_value(generator->dd.rotational_speed);
generator->type.last_output = (4.0*u1+u2*generator->dd.shaft_width) / (4.0 * generator->dd.wheel_radius);
break;
case GEN_DD_LEFT:
+ // Update position generators to allow callbacks
+ get_output_value(generator->dd.linear_pos);
+ get_output_value(generator->dd.rotational_pos);
+ // Compute output
u1 = get_output_value(generator->dd.linear_speed);
u2 = get_output_value(generator->dd.rotational_speed);
generator->type.last_output = (4.0*u1-u2*generator->dd.shaft_width) / (4.0 * generator->dd.wheel_radius);
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 38d4a00..6d888d6 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
@@ -71,7 +71,9 @@ typedef struct {
typedef struct {
placeholder_generator_t gen;
+ command_generator_t *linear_pos;
command_generator_t *linear_speed;
+ command_generator_t *rotational_pos;
command_generator_t *rotational_speed;
float wheel_radius;
float shaft_width;
@@ -112,14 +114,22 @@ command_generator_t* new_ramp2_generator(command_generator_t *generator,
/* Initializes a new Differential Drive generator.
* - generator : pointer to the generator to initialize
+ * - linear_pos : pointer to the generator giving the integrates of linear_speed. This
+ * generator will be called at each computation to allow update in parallel
+ * with linear_speed.
* - linear_speed : pointer to the generator giving the linear speed of the drive
+ * - rotational_pos : pointer to the generator giving the integrates of rotational_speed. This
+ * generator will be called at each computation to allow update in parallel
+ * with rotational_speed.
* - rotational_speed : pointer to the generator giving the rotational speed of the drive
* - wheel_radius : radius of the wheels
* - shaft_width : width of the propulsion shaft
* - type : 1 for the right_wheel, -1 for the left_wheel
*/
command_generator_t* new_dd_generator(command_generator_t *generator,
+ command_generator_t *linear_pos,
command_generator_t *linear_speed,
+ command_generator_t *rotational_pos,
command_generator_t *rotational_speed,
float wheel_radius, float shaft_width,
uint8_t type);
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.c
index ccf28e7..3e43444 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.c
@@ -28,12 +28,16 @@ void dd_start(float wheel_radius, float shaft_width) {
tc_new_controller(DD_LINEAR_SPEED_TC);
tc_new_controller(DD_ROTATIONAL_SPEED_TC);
new_dd_generator(¶ms.left_wheel_speed,
+ tc_get_position_generator(DD_LINEAR_SPEED_TC),
tc_get_speed_generator(DD_LINEAR_SPEED_TC),
+ tc_get_position_generator(DD_ROTATIONAL_SPEED_TC),
tc_get_speed_generator(DD_ROTATIONAL_SPEED_TC),
wheel_radius, shaft_width,
-1);
new_dd_generator(¶ms.right_wheel_speed,
+ tc_get_position_generator(DD_LINEAR_SPEED_TC),
tc_get_speed_generator(DD_LINEAR_SPEED_TC),
+ tc_get_position_generator(DD_ROTATIONAL_SPEED_TC),
tc_get_speed_generator(DD_ROTATIONAL_SPEED_TC),
wheel_radius, shaft_width,
1);
@@ -92,6 +96,9 @@ void dd_move(float distance, float speed, float acceleration) {
}
void dd_turn(float angle, float speed, float acceleration) {
+ if (angle > 1.0)
+ LED2_ON();
+
if (params.enabled) {
params.last_rot_acceleration = acceleration;
tc_goto(DD_ROTATIONAL_SPEED_TC, angle, speed, params.last_rot_acceleration);
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2011-04-02 15:17:01
|
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 275e297d6df8697614623704b7f136683ea71af1 (commit)
from c3191f76df8f1fa57a4f46ce16720611da4cc01c (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 275e297d6df8697614623704b7f136683ea71af1
Author: Xavier Lagorce <Xav...@cr...>
Date: Sat Apr 2 15:53:44 2011 +0200
[Controller_Motor_STM32] All angles are now in radians.
-----------------------------------------------------------------------
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 b1d7395..2617638 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
@@ -34,7 +34,7 @@ typedef struct {
typedef struct {
int16_t x __attribute__((__packed__)); // X position in mm (fixed point representation...)
int16_t y __attribute__((__packed__)); // Y position in mm
- int16_t theta __attribute__((__packed__)); // angle in 1/100 degrees
+ int16_t theta __attribute__((__packed__)); // angle in 1/10000 radians
} odometry_msg_t;
typedef struct {
@@ -44,9 +44,9 @@ typedef struct {
} 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
+ int32_t angle __attribute__((__packed__)); // angle in 1/10000 radians (fixed point representation...)
+ uint16_t speed __attribute__((__packed__)); // Speed in 1/1000 rad/s
+ uint16_t acceleration __attribute__((__packed__)); // Acceleration in 1/1000 radians/s^2
} turn_msg_t;
typedef struct {
@@ -175,12 +175,12 @@ static void NORETURN canMonitor_process(void) {
odo_getState(&odometry);
msg_odo.data.x = (int16_t)(odometry.x * 1000.0);
msg_odo.data.y = (int16_t)(odometry.y * 1000.0);
- odometry.theta = fmodf(odometry.theta, 360.0);
- if (odometry.theta > 180.0)
- odometry.theta -= 360.0;
- if (odometry.theta < -180.0)
- odometry.theta += 360.0;
- msg_odo.data.theta = (int16_t)(odometry.theta * 100.0);
+ odometry.theta = fmodf(odometry.theta, 2*M_PI);
+ if (odometry.theta > M_PI)
+ odometry.theta -= 2*M_PI;
+ if (odometry.theta < -M_PI)
+ odometry.theta += 2*M_PI;
+ msg_odo.data.theta = (int16_t)(odometry.theta * 10000.0);
txm.data32[0] = msg_odo.data32[0];
txm.data32[1] = msg_odo.data32[1];
@@ -205,20 +205,12 @@ static void NORETURN canMonitorListen_process(void) {
odometry_can_msg_t odometry_msg;
stop_can_msg_t stop_msg;
- tc_robot_t robot;
-
// Initialize constant parameters of TX frame
txm.dlc = 8;
txm.rtr = 0;
txm.ide = 1;
txm.sid = 0;
- // Initialize robot representation
- robot.left_wheel = 2;
- robot.right_wheel = 3;
- robot.wheel_radius = 0.049245;
- robot.shaft_width = 0.259;
-
while (1) {
received = can_receive(CAND1, &frame, ms_to_ticks(100));
if (received) {
@@ -239,14 +231,14 @@ static void NORETURN canMonitorListen_process(void) {
case CAN_MSG_MOVE:
move_msg.data32[0] = frame.data32[0];
move_msg.data32[1] = frame.data32[1];
- if (!tc_is_working(TC_MASK(2) | TC_MASK(3)))
- tc_move(&robot, move_msg.data.distance / 1000.0, move_msg.data.speed / 1000.0, move_msg.data.acceleration / 1000.0);
+ if (!tc_is_working(TC_MASK(DD_LINEAR_SPEED_TC) | TC_MASK(DD_ROTATIONAL_SPEED_TC)))
+ dd_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];
- if (!tc_is_working(TC_MASK(2) | TC_MASK(3)))
- tc_turn(&robot, turn_msg.data.angle / 100.0, turn_msg.data.speed / 100.0, turn_msg.data.acceleration / 100.0);
+ if (!tc_is_working(TC_MASK(DD_LINEAR_SPEED_TC) | TC_MASK(DD_ROTATIONAL_SPEED_TC)))
+ dd_turn(turn_msg.data.angle / 10000.0, turn_msg.data.speed / 1000.0, turn_msg.data.acceleration / 1000.0);
break;
case CAN_MSG_STOP:
stop_msg.data32[0] = frame.data32[0];
@@ -261,7 +253,7 @@ static void NORETURN canMonitorListen_process(void) {
odometry_msg.data32[1] = frame.data32[1];
odometry.x = ((float)odometry_msg.data.x) / 1000.0;
odometry.y = ((float)odometry_msg.data.y) / 1000.0;
- odometry.theta = ((float)odometry_msg.data.theta) / 100.0;
+ odometry.theta = ((float)odometry_msg.data.theta) / 10000.0;
odo_setState(&odometry);
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 fa8f973..2c83df0 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
@@ -17,6 +17,7 @@
#include "motor_controller.h"
#include "trajectory_controller.h"
#include "odometry.h"
+#include "differential_drive.h"
void canMonitorInit(void);
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 2c80192..38d4a00 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
@@ -14,6 +14,7 @@
#define __COMMAND_GENERATOR_H
#include <drv/timer.h>
+#include <math.h>
// Generator types
#define GEN_NONE 0 // No type, the generator is not initialized.
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 3a0c9a1..6f52c1d 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
@@ -17,6 +17,7 @@ controller_motor_stm32_USER_CSRC = \
$(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)/differential_drive.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/differential_drive.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.c
new file mode 100644
index 0000000..ccf28e7
--- /dev/null
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.c
@@ -0,0 +1,114 @@
+/*
+ * differential_drive.c
+ * --------------------
+ * Copyright : (c) 2011, Xavier Lagorce <Xav...@cr...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ */
+
+#include "differential_drive.h"
+
+typedef struct {
+ uint8_t initialized, enabled;
+ float wheel_radius, shaft_radius;
+ float last_lin_acceleration, last_rot_acceleration;
+ command_generator_t left_wheel_speed, right_wheel_speed;
+ command_generator_t left_wheel, right_wheel;
+} dd_params_t;
+
+static dd_params_t params;
+
+void dd_start(float wheel_radius, float shaft_width) {
+ params.wheel_radius = wheel_radius;
+ params.shaft_radius = shaft_width;
+ params.last_lin_acceleration = 0.0;
+ params.last_rot_acceleration = 0.0;
+
+ tc_new_controller(DD_LINEAR_SPEED_TC);
+ tc_new_controller(DD_ROTATIONAL_SPEED_TC);
+ new_dd_generator(¶ms.left_wheel_speed,
+ tc_get_speed_generator(DD_LINEAR_SPEED_TC),
+ tc_get_speed_generator(DD_ROTATIONAL_SPEED_TC),
+ wheel_radius, shaft_width,
+ -1);
+ new_dd_generator(¶ms.right_wheel_speed,
+ tc_get_speed_generator(DD_LINEAR_SPEED_TC),
+ tc_get_speed_generator(DD_ROTATIONAL_SPEED_TC),
+ wheel_radius, shaft_width,
+ 1);
+ new_ramp2_generator(¶ms.left_wheel, 0.0, ¶ms.left_wheel_speed);
+ new_ramp2_generator(¶ms.right_wheel, 0.0, ¶ms.right_wheel_speed);
+
+ start_generator(¶ms.left_wheel_speed);
+ start_generator(¶ms.right_wheel_speed);
+ start_generator(¶ms.left_wheel);
+ start_generator(¶ms.right_wheel);
+
+ params.initialized = 1;
+ params.enabled = 1;
+}
+
+void dd_pause(void) {
+ if (params.initialized) {
+ dd_set_linear_speed(0.0, params.last_lin_acceleration);
+ dd_set_rotational_speed(0.0, params.last_rot_acceleration);
+ params.enabled = 0;
+ }
+}
+
+void dd_resume(void) {
+ if (params.initialized && params.enabled) {
+ params.enabled = 1;
+ }
+}
+
+void dd_stop(void) {
+ if (params.initialized) {
+ pause_generator(¶ms.left_wheel);
+ pause_generator(¶ms.right_wheel);
+ pause_generator(¶ms.left_wheel_speed);
+ pause_generator(¶ms.right_wheel_speed);
+ tc_delete_controller(DD_LINEAR_SPEED_TC);
+ tc_delete_controller(DD_ROTATIONAL_SPEED_TC);
+ params.enabled = 0;
+ params.initialized = 0;
+ }
+}
+
+command_generator_t* dd_get_left_wheel_generator(void) {
+ return ¶ms.left_wheel;
+}
+
+command_generator_t* dd_get_right_wheel_generator(void) {
+ return ¶ms.right_wheel;
+}
+
+void dd_move(float distance, float speed, float acceleration) {
+ if (params.enabled) {
+ params.last_lin_acceleration = acceleration;
+ tc_goto(DD_LINEAR_SPEED_TC, distance, speed, params.last_lin_acceleration);
+ }
+}
+
+void dd_turn(float angle, float speed, float acceleration) {
+ if (params.enabled) {
+ params.last_rot_acceleration = acceleration;
+ tc_goto(DD_ROTATIONAL_SPEED_TC, angle, speed, params.last_rot_acceleration);
+ }
+}
+
+void dd_set_linear_speed(float speed, float acceleration) {
+ if (params.enabled) {
+ params.last_lin_acceleration = acceleration;
+ tc_goto_speed(DD_LINEAR_SPEED_TC, speed, params.last_lin_acceleration);
+ }
+}
+
+void dd_set_rotational_speed(float speed, float acceleration) {
+ if (params.enabled) {
+ params.last_rot_acceleration = acceleration;
+ tc_goto_speed(DD_ROTATIONAL_SPEED_TC, speed, params.last_rot_acceleration);
+ }
+}
+
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.h b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.h
new file mode 100644
index 0000000..4e68b0a
--- /dev/null
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/differential_drive.h
@@ -0,0 +1,76 @@
+/*
+ * differential_drive.h
+ * --------------------
+ * Copyright : (c) 2011, Xavier Lagorce <Xav...@cr...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ */
+
+#ifndef __DIFFERENTIAL_DRIVE_H
+#define __DIFFERENTIAL_DRIVE_H
+
+#include <math.h>
+#include "trajectory_controller.h"
+#include "command_generator.h"
+
+#ifndef DD_LINEAR_SPEED_TC
+ #define DD_LINEAR_SPEED_TC 0
+#endif
+#ifndef DD_ROTATIONAL_SPEED_TC
+ #define DD_ROTATIONAL_SPEED_TC 1
+#endif
+
+/* Initializes the differential drive
+ * - wheel_radius : radius of the wheels (in meters)
+ * - shaft_width : propulsion shaft radius (in meters)
+ *
+ * Note : the differential drive system will use Trajectory controllers
+ * DD_LINEAR_SPEED_TC and DD_ROTATIONAL_SPEED_TC
+ */
+void dd_start(float wheel_radius, float shaft_width);
+
+/* Pauses or Resumes the differential drive system.
+ * In pause mode, the drive will accept no further command and actions will be
+ * stopped.
+ * If the robot is moving, wheels will be slowed down with the last used
+ * acceleration (note that this won't necessarily give a straight line) when
+ * pausing.
+ */
+void dd_pause(void);
+void dd_resume(void);
+
+/*
+ * Stops the differential drive system.
+ */
+void dd_stop(void);
+
+/* Gets the generator correspondig to the wheels positions
+ * to use them in motor_control for instance
+ */
+command_generator_t* dd_get_left_wheel_generator(void);
+command_generator_t* dd_get_right_wheel_generator(void);
+
+/* Moves 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 dd_move(float distance, float speed, float acceleration);
+
+/* Turns 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 dd_turn(float angle, float speed, float acceleration);
+
+/*
+ * Modify a given speed of the robot using the specified acceleration
+ */
+void dd_set_linear_speed(float speed, float acceleration);
+void dd_set_rotational_speed(float speed, float acceleration);
+
+#endif /* __DIFFERENTIAL_DRIVE_H */
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 357f7b8..5584084 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
@@ -12,11 +12,13 @@
#include <math.h>
#include "hw/hw_led.h"
-#include "motor.h"
#include "motor_controller.h"
#include "can_monitor.h"
#include "command_generator.h"
-#include "trajectory_controller.h"
+#include "differential_drive.h"
+
+#define WHEEL_RADIUS 0.049245
+#define SHAFT_WIDTH 0.259
PROC_DEFINE_STACK(stack_ind, KERN_MINSTACKSIZE * 2);
@@ -45,12 +47,13 @@ static void init(void)
// Start control of drive motors
tc_init();
+ dd_start(WHEEL_RADIUS, SHAFT_WIDTH);
// Common parameters
- params.encoder_gain = -360.0/2000.0/15.0;
- params.G0 = 0.833;
+ params.encoder_gain = -2.0*M_PI/2000.0/15.0;
+ params.G0 = 0.0145;
params.tau = 0.015;
- params.k[0] = -68.0325;
- params.k[1] = -1.0205;
+ params.k[0] = -3898.0;
+ params.k[1] = -58.4696;
params.l = -params.k[0];
params.l0[0] = 0.0236;
params.l0[1] = 3.9715;
@@ -58,16 +61,14 @@ static void init(void)
// Initialize left motor
params.motor = MOTOR3;
params.encoder = ENCODER3;
- tc_new_controller(2);
- mc_new_controller(¶ms, tc_get_position_generator(2));
+ mc_new_controller(¶ms, dd_get_left_wheel_generator());
// Initialize right motor
params.motor = MOTOR4;
params.encoder = ENCODER4;
- tc_new_controller(3);
- mc_new_controller(¶ms, tc_get_position_generator(3));
+ mc_new_controller(¶ms, dd_get_right_wheel_generator());
// Start odometry
- odometryInit(1e-3, 0.049245, 0.259, -2.0*M_PI/2000.0/15.0);
+ odometryInit(1e-3, WHEEL_RADIUS, SHAFT_WIDTH, -2.0*M_PI/2000.0/15.0);
// Blink to say we are ready
for (uint8_t i=0; i < 5; i++) {
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/odometry.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/odometry.c
index 46f58b5..f6241fa 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/odometry.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/odometry.c
@@ -99,12 +99,12 @@ static void NORETURN odometry_process(void) {
void odo_setState(robot_state_t *new_state) {
state.robot_state.x = new_state->x;
state.robot_state.y = new_state->y;
- state.robot_state.theta = new_state->theta * M_PI / 180.0;
+ state.robot_state.theta = new_state->theta;
}
void odo_getState(robot_state_t *robot_state) {
robot_state->x = state.robot_state.x;
robot_state->y = state.robot_state.y;
- robot_state->theta = state.robot_state.theta * 180.0 / M_PI;
+ robot_state->theta = state.robot_state.theta;
}
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 5bd2734..5b01741 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
@@ -200,6 +200,19 @@ command_generator_t* tc_get_position_generator(uint8_t cntr_index) {
return &cont->position;
}
+command_generator_t* tc_get_speed_generator(uint8_t cntr_index) {
+ trajectory_controller_t *cont;
+
+ // Get the controller and verifies it is enabled
+ if (cntr_index >= NUM_TC_MAX)
+ return NULL;
+ cont = &controllers[cntr_index];
+ if (!cont->enabled)
+ return NULL;
+
+ return &cont->speed;
+}
+
void tc_goto(uint8_t cntr_index, float angle, float speed, float acceleration) {
float acc_dist, t_acc, t_end;
trajectory_controller_t *cont;
@@ -294,9 +307,9 @@ void tc_move(tc_robot_t *robot, float distance, float speed, float acceleration)
pause_generator(&controllers[robot->right_wheel].speed);
// Compute parameters
- dis_s = distance / robot->wheel_radius * 180.0 / M_PI;
- spe_s = speed / robot->wheel_radius * 180.0 / M_PI;
- acc_s = acceleration / robot->wheel_radius * 180.0 / M_PI;
+ dis_s = distance / robot->wheel_radius;
+ spe_s = speed / robot->wheel_radius;
+ acc_s = acceleration / robot->wheel_radius;
// Planify movements
tc_goto(robot->left_wheel, dis_s, spe_s, acc_s);
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
index 00b3a63..a4901a3 100644
--- 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
@@ -55,6 +55,13 @@ uint8_t tc_is_working(uint8_t cntr_indexes);
*/
command_generator_t* tc_get_position_generator(uint8_t cntr_index);
+/*
+ * Returns the speed generator outputting the intermediate result of the 'cntr_index'th
+ * Trajectory Generator.
+ */
+command_generator_t* tc_get_speed_generator(uint8_t cntr_index);
+
+
/* Moves of a given angle
* - angle : angle to move of, can be positive (forward) or negative
* (backward). The units corresponds to the given scaling factor
diff --git a/info/control2011/src/lib/krobot_message.ml b/info/control2011/src/lib/krobot_message.ml
index 16db1d2..60d13dd 100644
--- a/info/control2011/src/lib/krobot_message.ml
+++ b/info/control2011/src/lib/krobot_message.ml
@@ -137,7 +137,7 @@ let encode = function
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.));
+ put_sint16 data 4 (truncate (theta *. 10000.));
frame
~identifier:104
~kind:Data
@@ -157,9 +157,9 @@ let encode = function
~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.));
+ put_sint32 data 0 (truncate (angle *. 10000.));
+ put_uint16 data 4 (truncate (speed *. 1000.));
+ put_uint16 data 6 (truncate (acc *. 1000.));
frame
~identifier:202
~kind:Data
@@ -170,7 +170,7 @@ let encode = function
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.));
+ put_sint16 data 4 (truncate (theta *. 10000.));
frame
~identifier:203
~kind:Data
@@ -242,7 +242,7 @@ let decode frame =
Odometry
(float (get_sint16 frame.data 0) /. 1000.,
float (get_sint16 frame.data 2) /. 1000.,
- float (get_sint16 frame.data 4) *. pi /. 18000.)
+ float (get_sint16 frame.data 4) /. 10000.)
| 201 ->
Motor_move
(float (get_sint32 frame.data 0) /. 1000.,
@@ -250,14 +250,14 @@ let decode frame =
float (get_uint16 frame.data 6) /. 1000.)
| 202 ->
Motor_turn
- (float (get_sint32 frame.data 0) *. pi /. 18000.,
- float (get_uint16 frame.data 4) *. pi /. 18000.,
- float (get_uint16 frame.data 6) *. pi /. 18000.)
+ (float (get_sint32 frame.data 0) /. 10000.,
+ float (get_uint16 frame.data 4) /. 1000.,
+ float (get_uint16 frame.data 6) /. 1000.)
| 203 ->
Set_odometry
(float (get_sint16 frame.data 0) /. 1000.,
float (get_sint16 frame.data 2) /. 1000.,
- float (get_sint16 frame.data 4) *. pi /. 18000.)
+ float (get_sint16 frame.data 4) /. 10000.)
| 204 ->
Motor_stop
| _ ->
diff --git a/info/control2011/src/tools/krobot_simulator.ml b/info/control2011/src/tools/krobot_simulator.ml
index 0c1b484..d0f31d7 100644
--- a/info/control2011/src/tools/krobot_simulator.ml
+++ b/info/control2011/src/tools/krobot_simulator.ml
@@ -226,7 +226,7 @@ lwt () =
return ()
| Req_motor_status ->
let moving = sim.command <> Idle in
- Krobot_message.send bus (Unix.gettimeofday (), Motor_status(false, false, moving, moving))
+ Krobot_message.send bus (Unix.gettimeofday (), Motor_status(moving, moving, false, false))
| Set_odometry(x, y, theta) ->
sim.state <- { x; y; theta };
return ()
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index fcc9a27..cdda66b 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -529,7 +529,7 @@ module Board = struct
queue_draw board
end
| Motor_status(m1, m2, m3, m4) ->
- board.moving <- m3 || m4;
+ board.moving <- m1 || m2;
board.ui#entry_moving1#set_text (if m1 then "yes" else "no");
board.ui#entry_moving2#set_text (if m2 then "yes" else "no");
board.ui#entry_moving3#set_text (if m3 then "yes" else "no");
hooks/post-receive
--
krobot
|
|
From: Jérémie D. <Ba...@us...> - 2011-04-02 13:29:51
|
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 c3191f76df8f1fa57a4f46ce16720611da4cc01c (commit)
from f3ba0f4c3e5b991400c322baeaec5decc5eb787f (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 c3191f76df8f1fa57a4f46ce16720611da4cc01c
Author: Jérémie Dimino <je...@di...>
Date: Sat Apr 2 15:27:59 2011 +0200
[krobot_viewer] show all logs by default
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index de1696a..fcc9a27 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -553,6 +553,9 @@ lwt () =
~channel:Lwt_io.stderr
();
+ (* Display all informative messages. *)
+ Lwt_log.Section.set_level Lwt_log.Section.main Lwt_log.Info;
+
lwt bus = Krobot_bus.get () in
ignore (GMain.init ());
Lwt_glib.install ();
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2011-04-02 12:36:21
|
This is an automated email from the git hooks/post-receive script. It was
generated because a ref change was pushed to the repository containing
the project "krobot".
The branch, master has been updated
via f3ba0f4c3e5b991400c322baeaec5decc5eb787f (commit)
from cb5df192b9fad7e421db94ddf32b6643e2cda29a (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 f3ba0f4c3e5b991400c322baeaec5decc5eb787f
Author: Xavier Lagorce <Xav...@cr...>
Date: Sat Apr 2 14:35:50 2011 +0200
[Controller_Motor_STM32] Corrections
-----------------------------------------------------------------------
Changes:
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 347a7c3..357f7b8 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
@@ -56,13 +56,15 @@ static void init(void)
params.l0[1] = 3.9715;
params.T = 0.005;
// Initialize left motor
+ params.motor = MOTOR3;
params.encoder = ENCODER3;
tc_new_controller(2);
- mc_new_controller(¶ms, tc_get_position_generator(0));
+ mc_new_controller(¶ms, tc_get_position_generator(2));
// Initialize right motor
+ params.motor = MOTOR4;
params.encoder = ENCODER4;
tc_new_controller(3);
- mc_new_controller(¶ms, tc_get_position_generator(1));
+ mc_new_controller(¶ms, tc_get_position_generator(3));
// Start odometry
odometryInit(1e-3, 0.049245, 0.259, -2.0*M_PI/2000.0/15.0);
@@ -79,7 +81,7 @@ static void init(void)
LED3_OFF();
LED4_OFF();
timer_delay(100);
- }
+ }
}
static void NORETURN ind_process(void)
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 93d092b..537c174 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
@@ -35,7 +35,7 @@ typedef struct
ticks_t T; // sampling period in systicks
} control_params_t;
-control_params_t controllers[4];
+static control_params_t controllers[4];
static inline uint8_t get_motor_index(uint8_t motor_id) {
uint8_t motor_ind;
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 605913d..5bd2734 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
@@ -36,7 +36,7 @@ typedef struct {
void trapezoid_callback(command_generator_t *generator);
void acc_stop_callback(command_generator_t *generator);
-trajectory_controller_t controllers[NUM_TC_MAX];
+static trajectory_controller_t controllers[NUM_TC_MAX];
void trapezoid_callback(command_generator_t *generator) {
trajectory_controller_t *cont = NULL;
@@ -271,6 +271,11 @@ void tc_goto_speed(uint8_t cntr_index, float speed, float acceleration) {
if (!cont->enabled)
return;
+ // Disable a possibly running trapezoidal profile
+ cont->aut.state = TRAPEZOID_STATE_STOP;
+ remove_callback(&cont->position);
+ remove_callback(&cont->speed);
+
cont->aut.speed = speed;
cont->aut.dir = SIGN(speed - cont->speed.type.last_output);
// Set acceleration sign depending on the current speed
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2011-03-31 23:15:42
|
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 cb5df192b9fad7e421db94ddf32b6643e2cda29a (commit)
from d73605b0d6b8ce682a0db9516748f0c3636a30cd (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 cb5df192b9fad7e421db94ddf32b6643e2cda29a
Author: Xavier Lagorce <Xav...@cr...>
Date: Fri Apr 1 01:11:00 2011 +0200
[Controller_Motor_STM32] Refactored trajectory_generator
* trajectory generators are not linked to a specific motor controller
(preparation to a new differential driver)
* motor_controller now uses a structure to specify parameters
* adapted can_monitor and main to reflect the changes
-----------------------------------------------------------------------
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 ff3ffa8..b1d7395 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
@@ -214,8 +214,8 @@ static void NORETURN canMonitorListen_process(void) {
txm.sid = 0;
// Initialize robot representation
- robot.left_wheel = MOTOR3;
- robot.right_wheel = MOTOR4;
+ robot.left_wheel = 2;
+ robot.right_wheel = 3;
robot.wheel_radius = 0.049245;
robot.shaft_width = 0.259;
@@ -239,21 +239,21 @@ static void NORETURN canMonitorListen_process(void) {
case CAN_MSG_MOVE:
move_msg.data32[0] = frame.data32[0];
move_msg.data32[1] = frame.data32[1];
- if (!tc_is_working(MOTOR3 | MOTOR4))
+ if (!tc_is_working(TC_MASK(2) | TC_MASK(3)))
tc_move(&robot, 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];
- if (!tc_is_working(MOTOR3 | MOTOR4))
+ if (!tc_is_working(TC_MASK(2) | TC_MASK(3)))
tc_turn(&robot, turn_msg.data.angle / 100.0, turn_msg.data.speed / 100.0, turn_msg.data.acceleration / 100.0);
break;
case CAN_MSG_STOP:
stop_msg.data32[0] = frame.data32[0];
stop_msg.data32[1] = frame.data32[1];
if (stop_msg.data.stop == 1) {
- tc_delete_controller(MOTOR3);
- tc_delete_controller(MOTOR4);
+ mc_delete_controller(MOTOR3);
+ mc_delete_controller(MOTOR4);
}
break;
case CAN_MSG_ODOMETRY_SET:
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 a60c52e..347a7c3 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
@@ -22,7 +22,7 @@ PROC_DEFINE_STACK(stack_ind, KERN_MINSTACKSIZE * 2);
static void init(void)
{
- trajectory_controller_params_t params;
+ motor_controller_params_t params;
IRQ_ENABLE;
@@ -57,10 +57,12 @@ static void init(void)
params.T = 0.005;
// Initialize left motor
params.encoder = ENCODER3;
- tc_new_controller(MOTOR3, ¶ms);
+ tc_new_controller(2);
+ mc_new_controller(¶ms, tc_get_position_generator(0));
// Initialize right motor
params.encoder = ENCODER4;
- tc_new_controller(MOTOR4, ¶ms);
+ tc_new_controller(3);
+ mc_new_controller(¶ms, tc_get_position_generator(1));
// Start odometry
odometryInit(1e-3, 0.049245, 0.259, -2.0*M_PI/2000.0/15.0);
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 fdfcc9b..93d092b 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
@@ -171,43 +171,46 @@ void motorControllerInit() {
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, command_generator_t *generator) {
+uint8_t mc_new_controller(motor_controller_params_t *cntr_params, command_generator_t *generator) {
uint8_t motor_ind;
control_params_t *params;
+ float tau, T;
// Find the motor index
- motor_ind = get_motor_index(motor);
+ motor_ind = get_motor_index(cntr_params->motor);
params = &(controllers[motor_ind]);
if (params->enable == 0) {
// define user parameters
- params->motor = motor;
- params->encoder = encoder;
- params->encoder_gain = encoder_gain;
- params->tau = tau;
- params->T = ms_to_ticks((mtime_t)(T*1000));
- params->k[0] = k[0]; params->k[1] = k[1];
- params->l = l;
- params->l0[0] = l0[0]; params->l0[1] = l0[1];
+ params->motor = cntr_params->motor;
+ params->encoder = cntr_params->encoder;
+ params->encoder_gain = cntr_params->encoder_gain;
+ params->tau = cntr_params->tau;
+ params->T = ms_to_ticks((mtime_t)(cntr_params->T*1000));
+ params->k[0] = cntr_params->k[0]; params->k[1] = cntr_params->k[1];
+ params->l = cntr_params->l;
+ params->l0[0] = cntr_params->l0[0]; params->l0[1] = cntr_params->l0[1];
// compute other parameters
params->last_command = 0;
params->last_estimate[0] = 0; params->last_estimate[1] = 0;
params->last_output = params->last_estimate[0];
- params->last_encoder_pos = getEncoderPosition(encoder);
+ params->last_encoder_pos = getEncoderPosition(cntr_params->encoder);
params->reference = generator;
+ tau = cntr_params->tau;
+ T = cntr_params->T;
params->F[0] = 1;
params->F[1] = tau*(1-exp(-T/tau));
params->F[2] = 0;
params->F[3] = exp(-T/tau);
- params->G[0] = G0*(T+tau*exp(-T/tau)-tau);
- params->G[1] = G0*(1-exp(-T/tau));
+ params->G[0] = cntr_params->G0*(T+tau*exp(-T/tau)-tau);
+ params->G[1] = cntr_params->G0*(1-exp(-T/tau));
// enable the controller
params->enable = 1;
- enableMotor(motor);
- motorSetSpeed(motor, 0);
+ enableMotor(cntr_params->motor);
+ motorSetSpeed(cntr_params->motor, 0);
// start the controller
proc_new(motorController_process, params, KERN_MINSTACKSIZE * 16, NULL);
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 0c1a4e1..3b4df3f 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,12 +13,24 @@
#define CONTROLLER_OK 0
#define CONTROLLER_ALREADY_USED 1
+typedef struct {
+ 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 G0; // DC motor static gain
+ float tau; // DC motor time constant
+ float k[2]; // State control gain
+ float l; // Reference factoring gain
+ float l0[2]; // State observer gain
+ float T; // Sampling period of the controller in seconds
+} motor_controller_params_t;
+
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);
-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 mc_new_controller(motor_controller_params_t *cntr_params, command_generator_t *generator);
void mc_delete_controller(uint8_t motor);
#endif
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 a74dd55..605913d 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
@@ -36,22 +36,14 @@ typedef struct {
void trapezoid_callback(command_generator_t *generator);
void acc_stop_callback(command_generator_t *generator);
-trajectory_controller_t controllers[4];
-
-static inline uint8_t get_motor_index(uint8_t motor_id) {
- uint8_t motor_ind;
-
- for (motor_ind = 0; (motor_id >> (motor_ind+1)) != 0; motor_ind++) ;
-
- return motor_ind;
-}
+trajectory_controller_t controllers[NUM_TC_MAX];
void trapezoid_callback(command_generator_t *generator) {
trajectory_controller_t *cont = NULL;
uint8_t i;
// Select the correct controller depending on chich callbacl triggered
- for (i=0; i < 4; i++) {
+ for (i=0; i < NUM_TC_MAX; i++) {
if (generator == &(controllers[i].position)
|| generator == &(controllers[i].speed) ) {
cont = &controllers[i];
@@ -104,7 +96,7 @@ void acc_stop_callback(command_generator_t *generator) {
uint8_t i;
// Select the correct controller depending on chich callbacl triggered
- for (i=0; i < 4; i++) {
+ for (i=0; i < NUM_TC_MAX; i++) {
if (generator == &(controllers[i].position)
|| generator == &(controllers[i].speed) ) {
cont = &controllers[i];
@@ -132,7 +124,7 @@ void tc_init(void) {
uint8_t i;
// All Trajectory controllers are disabled
- for (i=0; i < 4; i++) {
+ for (i=0; i < NUM_TC_MAX; i++) {
controllers[i].enabled = 0;
}
@@ -140,11 +132,14 @@ void tc_init(void) {
motorControllerInit();
}
-void tc_new_controller(uint8_t motor, trajectory_controller_params_t *params) {
+void tc_new_controller(uint8_t cntr_index) {
trajectory_controller_t *cont;
+ if (cntr_index >= NUM_TC_MAX)
+ return;
+
// Get corresponding controller
- cont = &controllers[get_motor_index(motor)];
+ cont = &controllers[cntr_index];
// Do nothing if the controller already exists
if (cont->enabled == 1)
@@ -154,19 +149,7 @@ void tc_new_controller(uint8_t motor, trajectory_controller_params_t *params) {
new_ramp_generator(&cont->speed, 0., 0.);
new_ramp2_generator(&cont->position, 0., &cont->speed);
- // Start control of drive motor
- mc_new_controller(motor,
- params->encoder,
- params->encoder_gain,
- params->G0,
- params->tau,
- params->T,
- params->k,
- -params->k[0],
- params->l0,
- &cont->position);
-
- // Start the generator
+ // Start the generators
start_generator(&cont->position);
start_generator(&cont->speed);
@@ -176,31 +159,48 @@ void tc_new_controller(uint8_t motor, trajectory_controller_params_t *params) {
cont->enabled = 1;
}
-void tc_delete_controller(uint8_t motor) {
+void tc_delete_controller(uint8_t cntr_index) {
trajectory_controller_t *cont;
- cont = &controllers[get_motor_index(motor)];
+ if (cntr_index >= NUM_TC_MAX)
+ return;
+
+ cont = &controllers[cntr_index];
if (cont->enabled == 1) {
- mc_delete_controller(motor);
cont->working = 0;
cont->enabled = 0;
+ pause_generator(&cont->speed);
+ pause_generator(&cont->position);
}
}
-uint8_t tc_is_working(uint8_t motors) {
+uint8_t tc_is_working(uint8_t cntr_indexes) {
uint8_t state = 0, i;
// For each motor in motor, test if the corresponding controller is working
- for (i=0; i < 4; i++) {
- if ( ((motors & (1<<i)) != 0) && controllers[i].working == 1)
- state |= 1 << i;
+ for (i=0; i < NUM_TC_MAX; i++) {
+ if ( ((cntr_indexes & TC_MASK(i)) != 0) && controllers[i].working == 1)
+ state |= TC_MASK(i);
}
return state;
}
-void tc_goto(uint8_t motor, float angle, float speed, float acceleration) {
+command_generator_t* tc_get_position_generator(uint8_t cntr_index) {
+ trajectory_controller_t *cont;
+
+ // Get the controller and verifies it is enabled
+ if (cntr_index >= NUM_TC_MAX)
+ return NULL;
+ cont = &controllers[cntr_index];
+ if (!cont->enabled)
+ return NULL;
+
+ return &cont->position;
+}
+
+void tc_goto(uint8_t cntr_index, float angle, float speed, float acceleration) {
float acc_dist, t_acc, t_end;
trajectory_controller_t *cont;
@@ -209,7 +209,9 @@ void tc_goto(uint8_t motor, float angle, float speed, float acceleration) {
return;
// Get the controller and verifies it is enabled
- cont = &controllers[get_motor_index(motor)];
+ if (cntr_index >= NUM_TC_MAX)
+ return;
+ cont = &controllers[cntr_index];
if (!cont->enabled)
return;
@@ -255,7 +257,7 @@ void tc_goto(uint8_t motor, float angle, float speed, float acceleration) {
cont->working = 1;
}
-void tc_goto_speed(uint8_t motor, float speed, float acceleration) {
+void tc_goto_speed(uint8_t cntr_index, float speed, float acceleration) {
trajectory_controller_t *cont;
// Verify parameters
@@ -263,7 +265,9 @@ void tc_goto_speed(uint8_t motor, float speed, float acceleration) {
return;
// Get the controller and verifies it is enabled
- cont = &controllers[get_motor_index(motor)];
+ if (cntr_index >= NUM_TC_MAX)
+ return;
+ cont = &controllers[cntr_index];
if (!cont->enabled)
return;
@@ -281,8 +285,8 @@ void tc_move(tc_robot_t *robot, float distance, float speed, float acceleration)
float dis_s, spe_s, acc_s;
// Let's pause the wheel's speed generators to synchronize movement start
- pause_generator(&controllers[get_motor_index(robot->left_wheel)].speed);
- pause_generator(&controllers[get_motor_index(robot->right_wheel)].speed);
+ pause_generator(&controllers[robot->left_wheel].speed);
+ pause_generator(&controllers[robot->right_wheel].speed);
// Compute parameters
dis_s = distance / robot->wheel_radius * 180.0 / M_PI;
@@ -294,16 +298,16 @@ void tc_move(tc_robot_t *robot, float distance, float speed, float acceleration)
tc_goto(robot->right_wheel, dis_s, spe_s, acc_s);
// Go
- start_generator(&controllers[get_motor_index(robot->left_wheel)].speed);
- start_generator(&controllers[get_motor_index(robot->right_wheel)].speed);
+ start_generator(&controllers[robot->left_wheel].speed);
+ start_generator(&controllers[robot->right_wheel].speed);
}
void tc_turn(tc_robot_t *robot, float angle, float speed, float acceleration) {
float angle_s, spe_s, acc_s;
// Let's pause the wheel's speed generators to synchronize movement start
- pause_generator(&controllers[get_motor_index(robot->left_wheel)].speed);
- pause_generator(&controllers[get_motor_index(robot->right_wheel)].speed);
+ pause_generator(&controllers[robot->left_wheel].speed);
+ pause_generator(&controllers[robot->right_wheel].speed);
// Compute parameters
angle_s = angle * robot->shaft_width / 2.0 / robot->wheel_radius;
@@ -315,6 +319,6 @@ void tc_turn(tc_robot_t *robot, float angle, float speed, float acceleration) {
tc_goto(robot->right_wheel, angle_s, spe_s, acc_s);
// Go
- start_generator(&controllers[get_motor_index(robot->left_wheel)].speed);
- start_generator(&controllers[get_motor_index(robot->right_wheel)].speed);
+ start_generator(&controllers[robot->left_wheel].speed);
+ start_generator(&controllers[robot->right_wheel].speed);
}
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
index b2361d6..00b3a63 100644
--- 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
@@ -14,46 +14,46 @@
#include "command_generator.h"
#include <math.h>
-typedef struct {
- uint8_t encoder; // Encoder ID to measure motor position from
- float encoder_gain; // Gain to convert encoder value unit to reference unit
- float G0; // DC motor static gain
- float tau; // DC motor time constant
- float k[2]; // State control gain
- float l; // Reference factoring gain
- float l0[2]; // State observer gain
- float T; // Sampling period of the controller in seconds
-} trajectory_controller_params_t;
+#define NUM_TC_MAX 4
+#define TC_MASK(num) (1 << (num))
typedef struct {
- uint8_t left_wheel; // Left wheel motor ID
- uint8_t right_wheel; // Right wheel motor ID
+ uint8_t left_wheel; // Left wheel trajectory controller index
+ uint8_t right_wheel; // Right wheel trajectory controller index
float wheel_radius; // Radius of the wheels
float shaft_width; // Width of the propulsion shaft
} tc_robot_t;
-/* Initialize the trajectory controller system
- *
+/*
+ * Initialize the trajectory controller system
*/
void tc_init(void);
/* Initialize a new trajectory controller with given parameters
*
- * This function will initialize a new trajectory controller and the
- * necessary motor controller and generators.
+ * This function will initialize a new trajectory controller
+ * - cntr_index : index of the trajectory controller to initialize. It should
+ * be between 0 and (NUM_TC_MAX-1)
*/
-void tc_new_controller(uint8_t motor, trajectory_controller_params_t *params);
+void tc_new_controller(uint8_t cntr_index);
-/* Deletes a given trajectory controller
- *
+/*
+ * Deletes a given trajectory controller
*/
-void tc_delete_controller(uint8_t motor);
+void tc_delete_controller(uint8_t cntr_index);
-/* Indicates if the last planified action on 'motors' is finished
+/* Indicates if the last planified action on 'cntr_indexes' is finished
+ * - cntr_indexes : logical OR of TC_MASK(cntr_index) for interesting indexes
*
* Returns the logical OR of working controllers among the ones specified in motors
*/
-uint8_t tc_is_working(uint8_t motors);
+uint8_t tc_is_working(uint8_t cntr_indexes);
+
+/*
+ * Returns the position generator outputting the result of the 'cntr_index'th
+ * Trajectory Generator.
+ */
+command_generator_t* tc_get_position_generator(uint8_t cntr_index);
/* Moves of a given angle
* - angle : angle to move of, can be positive (forward) or negative
@@ -62,12 +62,12 @@ uint8_t tc_is_working(uint8_t motors);
* - speed : moving speed (in units per second) should be positive.
* - acceleration : in units per second per second, should be positive.
*/
-void tc_goto(uint8_t motor, float angle, float speed, float acceleration);
+void tc_goto(uint8_t cntr_index, float angle, float speed, float acceleration);
/* Accelerates or brakes the robot to the given speed
*
*/
-void tc_goto_speed(uint8_t motor, float speed, float acceleration);
+void tc_goto_speed(uint8_t cntr_index, float speed, float acceleration);
/* Moves along a line
* - robot : pointer to a structure describing the robot configuration
hooks/post-receive
--
krobot
|
|
From: Jérémie D. <Ba...@us...> - 2011-03-31 20:12: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 d73605b0d6b8ce682a0db9516748f0c3636a30cd (commit)
from eb90593156e38fbbfaa7e6573d3f12c65e2d3a5d (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 d73605b0d6b8ce682a0db9516748f0c3636a30cd
Author: Jérémie Dimino <je...@di...>
Date: Thu Mar 31 22:11:33 2011 +0200
[krobot_viewer] fix computations of limits
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index 6b3dd20..de1696a 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -466,7 +466,8 @@ module Board = struct
let rec loop () =
match board.points with
| (x, y) :: rest ->
- let radius = sqrt (2. *. robot_size *. robot_size) in
+ let sqr x = x *. x in
+ let radius = sqrt (sqr (max wheels_position (robot_size -. wheels_position)) +. sqr (robot_size /. 2.)) in
if x >= radius && x <= world_width -. radius && y >= radius && y <= world_height -. radius then begin
(* Turn the robot. *)
let alpha = math_mod_float (atan2 (y -. board.state.y) (x -. board.state.x) -. board.state.theta) (2. *. pi) in
@@ -478,7 +479,6 @@ module Board = struct
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 () = Lwt_log.info_f "moving by %f meters" dist in
lwt () = Krobot_message.send board.bus (Unix.gettimeofday (),
hooks/post-receive
--
krobot
|
|
From: Jérémie D. <Ba...@us...> - 2011-03-31 20:04: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 eb90593156e38fbbfaa7e6573d3f12c65e2d3a5d (commit)
via 70fa41645cedd6cfb4814deff7f298908395818c (commit)
via 453a2c70ceb9276c7a597ee1471c0419318b67c7 (commit)
from 4b62957c82d4ee890ccd36c31370350a07b72deb (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 eb90593156e38fbbfaa7e6573d3f12c65e2d3a5d
Author: Jérémie Dimino <je...@di...>
Date: Thu Mar 31 22:01:32 2011 +0200
[krobot_viewer] reorganisation of the UI
commit 70fa41645cedd6cfb4814deff7f298908395818c
Author: Jérémie Dimino <je...@di...>
Date: Thu Mar 31 21:46:28 2011 +0200
[krobot_viewer] display logs in the UI
commit 453a2c70ceb9276c7a597ee1471c0419318b67c7
Author: Jérémie Dimino <je...@di...>
Date: Thu Mar 31 21:27:13 2011 +0200
[krobot_viewer] do not move outside of the board
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index ea76cc9..6b3dd20 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -466,34 +466,38 @@ module Board = struct
let rec loop () =
match board.points with
| (x, y) :: rest ->
- (* Turn the robot. *)
- let alpha = math_mod_float (atan2 (y -. board.state.y) (x -. board.state.x) -. board.state.theta) (2. *. pi) in
- lwt () = Lwt_log.info_f "turning by %f radiants" alpha in
- lwt () = Krobot_message.send board.bus (Unix.gettimeofday (),
- Motor_turn(alpha,
- board.ui#rotation_speed#adjustment#value,
- board.ui#rotation_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 () = Lwt_log.info_f "moving by %f meters" dist in
- lwt () = Krobot_message.send board.bus (Unix.gettimeofday (),
- Motor_move(dist,
- board.ui#moving_speed#adjustment#value,
- board.ui#moving_acceleration#adjustment#value)) in
- lwt () = wait_done board in
-
- (* Remove the point. *)
- (match board.points with
- | _ :: l -> board.points <- l
- | [] -> ());
-
- (* Redraw everything without the last point. *)
- queue_draw board;
-
- loop ()
+ let radius = sqrt (2. *. robot_size *. robot_size) in
+ if x >= radius && x <= world_width -. radius && y >= radius && y <= world_height -. radius then begin
+ (* Turn the robot. *)
+ let alpha = math_mod_float (atan2 (y -. board.state.y) (x -. board.state.x) -. board.state.theta) (2. *. pi) in
+ lwt () = Lwt_log.info_f "turning by %f radiants" alpha in
+ lwt () = Krobot_message.send board.bus (Unix.gettimeofday (),
+ Motor_turn(alpha,
+ board.ui#rotation_speed#adjustment#value,
+ board.ui#rotation_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 () = Lwt_log.info_f "moving by %f meters" dist in
+ lwt () = Krobot_message.send board.bus (Unix.gettimeofday (),
+ Motor_move(dist,
+ board.ui#moving_speed#adjustment#value,
+ board.ui#moving_acceleration#adjustment#value)) in
+ lwt () = wait_done board in
+
+ (* Remove the point. *)
+ (match board.points with
+ | _ :: l -> board.points <- l
+ | [] -> ());
+
+ (* Redraw everything without the last point. *)
+ queue_draw board;
+
+ loop ()
+ end else
+ Lwt_log.warning_f "can not move to (%f, %f)" x y
| [] ->
return ()
in
@@ -559,6 +563,21 @@ lwt () =
ignore (ui#window#connect#destroy ~callback:(wakeup wakener));
ui#window#show ();
+ Lwt_log.default :=
+ Lwt_log.broadcast [
+ !Lwt_log.default;
+ Lwt_log.make
+ ~output:(fun section level lines ->
+ List.iter
+ (fun line ->
+ ui#logs#buffer#insert
+ (Printf.sprintf "%s: %s\n" (Lwt_log.Section.name section) line))
+ lines;
+ ui#scrolled_logs#vadjustment#set_value ui#scrolled_logs#vadjustment#upper;
+ return ())
+ ~close:return
+ ];
+
let lcd = LCD.create ui in
ignore (ui#lcd#event#connect#expose (fun ev -> LCD.draw lcd; true));
diff --git a/info/control2011/src/tools/krobot_viewer_ui.glade b/info/control2011/src/tools/krobot_viewer_ui.glade
index 61d06f6..56ec218 100644
--- a/info/control2011/src/tools/krobot_viewer_ui.glade
+++ b/info/control2011/src/tools/krobot_viewer_ui.glade
@@ -189,87 +189,119 @@
</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">Simplify 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="GtkHBox" id="hbox2">
+ <widget class="GtkTable" id="table2">
<property name="visible">True</property>
+ <property name="n_rows">3</property>
+ <property name="n_columns">2</property>
<child>
- <widget class="GtkLabel" id="label1">
+ <widget class="GtkButton" id="button_clear">
+ <property name="label" translatable="yes">Clear trajectory</property>
<property name="visible">True</property>
- <property name="label" translatable="yes">Tolerance:</property>
+ <property name="can_focus">True</property>
+ <property name="receives_default">True</property>
+ </widget>
+ </child>
+ <child>
+ <widget class="GtkButton" id="button_smooth">
+ <property name="label" translatable="yes">Simplify 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">0</property>
+ <property name="left_attach">1</property>
+ <property name="right_attach">2</property>
</packing>
</child>
<child>
- <widget class="GtkHScale" id="tolerance">
+ <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="adjustment">0.10000000000000001 0 1 0.10000000000000001 0.20000000000000001 0.5</property>
- <property name="draw_value">False</property>
+ <property name="receives_default">True</property>
+ </widget>
+ <packing>
+ <property name="top_attach">1</property>
+ <property name="bottom_attach">2</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkButton" id="button_stop">
+ <property name="label" translatable="yes">Stop</property>
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="receives_default">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>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkButton" id="button_start_red">
+ <property name="label" translatable="yes">Goto red initial position</property>
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="receives_default">True</property>
+ </widget>
+ <packing>
+ <property name="top_attach">2</property>
+ <property name="bottom_attach">3</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkButton" id="button_start_blue">
+ <property name="label" translatable="yes">Goto blue initial position</property>
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="receives_default">True</property>
</widget>
<packing>
- <property name="position">1</property>
+ <property name="left_attach">1</property>
+ <property name="right_attach">2</property>
+ <property name="top_attach">2</property>
+ <property name="bottom_attach">3</property>
</packing>
</child>
</widget>
<packing>
<property name="expand">False</property>
- <property name="position">3</property>
+ <property name="position">1</property>
</packing>
</child>
<child>
- <widget class="GtkButton" id="button_go">
- <property name="label" translatable="yes">Go!</property>
+ <widget class="GtkHBox" id="hbox13">
<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>
+ <property name="position">2</property>
</packing>
</child>
<child>
- <widget class="GtkButton" id="button_stop">
- <property name="label" translatable="yes">Stop</property>
+ <widget class="GtkHBox" id="hbox2">
<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">5</property>
+ <property name="position">3</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkHBox" id="hbox12">
+ <property name="visible">True</property>
+ </widget>
+ <packing>
+ <property name="expand">False</property>
+ <property name="position">4</property>
</packing>
</child>
<child>
<widget class="GtkTable" id="table1">
<property name="visible">True</property>
- <property name="n_rows">8</property>
+ <property name="n_rows">9</property>
<property name="n_columns">2</property>
<child>
<widget class="GtkSpinButton" id="moving_speed">
@@ -283,8 +315,8 @@
<packing>
<property name="left_attach">1</property>
<property name="right_attach">2</property>
- <property name="top_attach">4</property>
- <property name="bottom_attach">5</property>
+ <property name="top_attach">5</property>
+ <property name="bottom_attach">6</property>
<property name="y_options">GTK_FILL</property>
</packing>
</child>
@@ -300,8 +332,8 @@
<packing>
<property name="left_attach">1</property>
<property name="right_attach">2</property>
- <property name="top_attach">5</property>
- <property name="bottom_attach">6</property>
+ <property name="top_attach">6</property>
+ <property name="bottom_attach">7</property>
<property name="y_options">GTK_FILL</property>
</packing>
</child>
@@ -317,8 +349,8 @@
<packing>
<property name="left_attach">1</property>
<property name="right_attach">2</property>
- <property name="top_attach">6</property>
- <property name="bottom_attach">7</property>
+ <property name="top_attach">7</property>
+ <property name="bottom_attach">8</property>
<property name="y_options">GTK_FILL</property>
</packing>
</child>
@@ -334,8 +366,8 @@
<packing>
<property name="left_attach">1</property>
<property name="right_attach">2</property>
- <property name="top_attach">7</property>
- <property name="bottom_attach">8</property>
+ <property name="top_attach">8</property>
+ <property name="bottom_attach">9</property>
<property name="y_options">GTK_FILL</property>
</packing>
</child>
@@ -365,8 +397,8 @@
</child>
</widget>
<packing>
- <property name="top_attach">4</property>
- <property name="bottom_attach">5</property>
+ <property name="top_attach">5</property>
+ <property name="bottom_attach">6</property>
<property name="x_options">GTK_FILL</property>
<property name="y_options">GTK_FILL</property>
</packing>
@@ -397,8 +429,8 @@
</child>
</widget>
<packing>
- <property name="top_attach">5</property>
- <property name="bottom_attach">6</property>
+ <property name="top_attach">6</property>
+ <property name="bottom_attach">7</property>
<property name="x_options">GTK_FILL</property>
<property name="y_options">GTK_FILL</property>
</packing>
@@ -429,8 +461,8 @@
</child>
</widget>
<packing>
- <property name="top_attach">6</property>
- <property name="bottom_attach">7</property>
+ <property name="top_attach">7</property>
+ <property name="bottom_attach">8</property>
<property name="x_options">GTK_FILL</property>
<property name="y_options">GTK_FILL</property>
</packing>
@@ -461,8 +493,8 @@
</child>
</widget>
<packing>
- <property name="top_attach">7</property>
- <property name="bottom_attach">8</property>
+ <property name="top_attach">8</property>
+ <property name="bottom_attach">9</property>
<property name="x_options">GTK_FILL</property>
<property name="y_options">GTK_FILL</property>
</packing>
@@ -492,6 +524,10 @@
</packing>
</child>
</widget>
+ <packing>
+ <property name="top_attach">1</property>
+ <property name="bottom_attach">2</property>
+ </packing>
</child>
<child>
<widget class="GtkHBox" id="hbox3">
@@ -519,8 +555,8 @@
</child>
</widget>
<packing>
- <property name="top_attach">1</property>
- <property name="bottom_attach">2</property>
+ <property name="top_attach">2</property>
+ <property name="bottom_attach">3</property>
</packing>
</child>
<child>
@@ -549,8 +585,8 @@
</child>
</widget>
<packing>
- <property name="top_attach">2</property>
- <property name="bottom_attach">3</property>
+ <property name="top_attach">3</property>
+ <property name="bottom_attach">4</property>
</packing>
</child>
<child>
@@ -579,8 +615,8 @@
</child>
</widget>
<packing>
- <property name="top_attach">3</property>
- <property name="bottom_attach">4</property>
+ <property name="top_attach">4</property>
+ <property name="bottom_attach">5</property>
</packing>
</child>
<child>
@@ -593,8 +629,8 @@
<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="top_attach">2</property>
+ <property name="bottom_attach">3</property>
</packing>
</child>
<child>
@@ -607,8 +643,8 @@
<packing>
<property name="left_attach">1</property>
<property name="right_attach">2</property>
- <property name="top_attach">2</property>
- <property name="bottom_attach">3</property>
+ <property name="top_attach">3</property>
+ <property name="bottom_attach">4</property>
</packing>
</child>
<child>
@@ -621,8 +657,8 @@
<packing>
<property name="left_attach">1</property>
<property name="right_attach">2</property>
- <property name="top_attach">3</property>
- <property name="bottom_attach">4</property>
+ <property name="top_attach">4</property>
+ <property name="bottom_attach">5</property>
</packing>
</child>
<child>
@@ -684,48 +720,89 @@
<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>
</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="left_attach">1</property>
+ <property name="right_attach">2</property>
+ </packing>
+ </child>
+ <child>
+ <widget class="GtkHBox" id="hbox15">
+ <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="GtkAlignment" id="alignment9">
+ <property name="visible">True</property>
+ <child>
+ <placeholder/>
+ </child>
+ </widget>
+ <packing>
+ <property name="position">1</property>
+ </packing>
+ </child>
+ </widget>
+ </child>
</widget>
<packing>
<property name="expand">False</property>
- <property name="position">6</property>
+ <property name="position">5</property>
</packing>
</child>
<child>
- <widget class="GtkButton" id="button_start_red">
- <property name="label" translatable="yes">Goto red initial position</property>
+ <widget class="GtkHBox" id="hbox14">
<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">7</property>
+ <property name="position">6</property>
</packing>
</child>
<child>
- <widget class="GtkButton" id="button_start_blue">
- <property name="label" translatable="yes">Goto blue initial position</property>
+ <widget class="GtkHSeparator" id="hseparator1">
<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">8</property>
+ <property name="position">7</property>
</packing>
</child>
<child>
- <widget class="GtkViewport" id="viewport1">
+ <widget class="GtkScrolledWindow" id="scrolled_logs">
<property name="visible">True</property>
- <property name="resize_mode">queue</property>
+ <property name="can_focus">True</property>
+ <property name="hscrollbar_policy">automatic</property>
+ <property name="vscrollbar_policy">automatic</property>
<child>
- <placeholder/>
+ <widget class="GtkTextView" id="logs">
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="editable">False</property>
+ <property name="cursor_visible">False</property>
+ </widget>
</child>
</widget>
<packing>
- <property name="position">9</property>
+ <property name="position">8</property>
</packing>
</child>
</widget>
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2011-03-31 15:49:27
|
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 4b62957c82d4ee890ccd36c31370350a07b72deb (commit)
from d8a4ed321466b4ff28df5fad95e14a763c15ade3 (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 4b62957c82d4ee890ccd36c31370350a07b72deb
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu Mar 31 17:49:02 2011 +0200
[Controller_Motor_STM32] Corrected a function description
-----------------------------------------------------------------------
Changes:
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
index 8c1eef6..b2361d6 100644
--- 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
@@ -55,12 +55,12 @@ void tc_delete_controller(uint8_t motor);
*/
uint8_t tc_is_working(uint8_t motors);
-/* Moves of a given distance
- * - distance : distance to move of, can be positive (forward) or negative
- * (backward). The units corresponds to the given scaling factor
- * during initialization.
- * - speed : moving speed (in meters per second) should be positive.
- * - acceleration : in meters per second per second, should be positive.
+/* Moves of a given angle
+ * - angle : angle to move of, can be positive (forward) or negative
+ * (backward). The units corresponds to the given scaling factor
+ * during initialization.
+ * - speed : moving speed (in units per second) should be positive.
+ * - acceleration : in units per second per second, should be positive.
*/
void tc_goto(uint8_t motor, float angle, float speed, float acceleration);
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2011-03-31 15:05:00
|
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 d8a4ed321466b4ff28df5fad95e14a763c15ade3 (commit)
from 260df9f53355a424386ccc2b72d403d43ba6b83f (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 d8a4ed321466b4ff28df5fad95e14a763c15ade3
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu Mar 31 17:02:51 2011 +0200
[Controller_Motor_STM32] new features in command_generator
* When a composition generator is started (like GEN_RAMP2), it will not
automatically start the other generator ot reads the output from.
* Added a Differential drive generator to compute wheel speeds based
on robot global parameters (i.e. linear and rotational speeds).
-----------------------------------------------------------------------
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 4d07057..ac57d5d 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
@@ -41,13 +41,31 @@ command_generator_t* new_ramp2_generator(command_generator_t *generator, float s
return generator;
}
-
-command_generator_t* adjust_value(command_generator_t *generator, float value) {
- generator->type.last_output = value;
+command_generator_t* new_dd_generator(command_generator_t *generator,
+ command_generator_t *linear_speed,
+ command_generator_t *rotational_speed,
+ float wheel_radius, float shaft_width,
+ uint8_t type) {
+ generator->type.t = (type == 1) ? GEN_DD_RIGHT : GEN_DD_LEFT;
+ generator->type.callback.type = GEN_CALLBACK_NONE;
+ generator->dd.linear_speed = linear_speed;
+ generator->dd.rotational_speed = rotational_speed;
+ generator->dd.wheel_radius = wheel_radius;
+ generator->dd.shaft_width = shaft_width;
return generator;
}
+
+command_generator_t* adjust_value(command_generator_t *generator, float value) {
+ if (generator->type.t != GEN_DD_RIGHT && generator->type.t != GEN_DD_LEFT) {
+ generator->type.last_output = value;
+ return generator;
+ } else {
+ return NULL;
+ }
+}
+
command_generator_t* adjust_speed(command_generator_t *generator, float speed) {
switch (generator->type.t) {
case GEN_RAMP:
@@ -64,7 +82,6 @@ command_generator_t* start_generator(command_generator_t *generator) {
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;
@@ -78,9 +95,6 @@ command_generator_t* pause_generator(command_generator_t *generator) {
// pause the generator
generator->type.state = GEN_STATE_PAUSE;
- if (generator->type.t == GEN_RAMP2)
- pause_generator(generator->ramp2.speed);
-
return generator;
}
@@ -101,7 +115,7 @@ command_generator_t* remove_callback(command_generator_t *generator) {
float get_output_value(command_generator_t *generator) {
int32_t cur_time;
- float speed, dt;
+ float speed, dt, u1, u2;
if (generator->type.state != GEN_STATE_RUNNING)
return generator->type.last_output;
@@ -123,6 +137,16 @@ float get_output_value(command_generator_t *generator) {
generator->type.last_output += dt*speed;
generator->ramp2.last_time = cur_time;
break;
+ case GEN_DD_RIGHT:
+ u1 = get_output_value(generator->dd.linear_speed);
+ u2 = get_output_value(generator->dd.rotational_speed);
+ generator->type.last_output = (4.0*u1+u2*generator->dd.shaft_width) / (4.0 * generator->dd.wheel_radius);
+ break;
+ case GEN_DD_LEFT:
+ u1 = get_output_value(generator->dd.linear_speed);
+ u2 = get_output_value(generator->dd.rotational_speed);
+ generator->type.last_output = (4.0*u1-u2*generator->dd.shaft_width) / (4.0 * generator->dd.wheel_radius);
+ break;
}
switch (generator->type.callback.type) {
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2011-03-31 14:40:21
|
This is an automated email from the git hooks/post-receive script. It was
generated because a ref change was pushed to the repository containing
the project "krobot".
The branch, master has been updated
via 260df9f53355a424386ccc2b72d403d43ba6b83f (commit)
from a51709e923cc4020fdae14ea2ab455f132fc7c45 (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 260df9f53355a424386ccc2b72d403d43ba6b83f
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu Mar 31 16:39:43 2011 +0200
[Controller_Motor_STM32] Comments in command_generator.h
-----------------------------------------------------------------------
Changes:
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 38e2564..2c80192 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
@@ -15,26 +15,36 @@
#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
-
-#define GEN_CALLBACK_NONE 0
-#define GEN_CALLBACK_SUP 1
-#define GEN_CALLBACK_INF 2
+// Generator types
+#define GEN_NONE 0 // No type, the generator is not initialized.
+#define GEN_CONSTANT 1 // Outputs a constant.
+#define GEN_RAMP 2 // Outputs a ramp with the given slope.
+#define GEN_RAMP2 3 // Same as GEN_RAMP, but the slope is given by another
+ // generator.
+#define GEN_DD_LEFT 4 // Outputs the left wheel speed for a differential
+ // drive.
+#define GEN_DD_RIGHT 5 // Outputs the right wheel speed for a differential
+ // drive.
+
+// Generator states
+#define GEN_STATE_PAUSE 0 // The output is freezed.
+#define GEN_STATE_RUNNING 1 // The generator is running.
+
+// Threshold comparison for callback trigger
+#define GEN_CALLBACK_NONE 0 // There is no callback.
+#define GEN_CALLBACK_SUP 1 // The callback is triggered if output > threshold.
+#define GEN_CALLBACK_INF 2 // The callback is triggered if output < threshold.
typedef union _command_generator_t command_generator_t;
+// Callback description
typedef struct {
uint8_t type;
float threshold;
void (*callback_function)(command_generator_t*);
} generator_callback_t;
+// Generator descriptions
typedef struct {
uint8_t t;
float last_output;
@@ -58,26 +68,93 @@ typedef struct {
command_generator_t *speed;
} ramp2_generator_t;
+typedef struct {
+ placeholder_generator_t gen;
+ command_generator_t *linear_speed;
+ command_generator_t *rotational_speed;
+ float wheel_radius;
+ float shaft_width;
+} dd_generator_t;
+
+// Usable generator meta-type
union _command_generator_t {
placeholder_generator_t type;
constant_generator_t constant;
ramp_generator_t ramp;
ramp2_generator_t ramp2;
+ dd_generator_t dd;
};
-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);
+/* Initializes a new Constant Generator.
+ * - generator : pointer to the generator to initialize
+ * - value : output value of the generator
+ */
+command_generator_t* new_constant_generator(command_generator_t *generator,
+ float value);
+
+/* Initializes a new Ramp Generator.
+ * - generator : pointer to the generator to initialize
+ * - starting_value : initial output value
+ * - speed : slope
+ */
+command_generator_t* new_ramp_generator(command_generator_t *generator,
+ float starting_value, float speed);
+
+/* Initializes a new Ramp2 Generator.
+ * - generator : pointer to the generator to initialize
+ * - starting_value : initial output value
+ * - speed : pointer to the generator which output is used as this generator's slope
+ */
+command_generator_t* new_ramp2_generator(command_generator_t *generator,
+ float starting_value,
+ command_generator_t *speed);
+
+/* Initializes a new Differential Drive generator.
+ * - generator : pointer to the generator to initialize
+ * - linear_speed : pointer to the generator giving the linear speed of the drive
+ * - rotational_speed : pointer to the generator giving the rotational speed of the drive
+ * - wheel_radius : radius of the wheels
+ * - shaft_width : width of the propulsion shaft
+ * - type : 1 for the right_wheel, -1 for the left_wheel
+ */
+command_generator_t* new_dd_generator(command_generator_t *generator,
+ command_generator_t *linear_speed,
+ command_generator_t *rotational_speed,
+ float wheel_radius, float shaft_width,
+ uint8_t type);
+/*
+ * Adjusts the current output value of 'generator' to 'value'.
+ */
command_generator_t* adjust_value(command_generator_t *generator, float value);
+
+/*
+ * Adjusts the current slope of the ramp generator 'generator' to 'speed'.
+ */
command_generator_t* adjust_speed(command_generator_t *generator, float speed);
+/*
+ * Starts or pauses 'generator'.
+ */
command_generator_t* start_generator(command_generator_t *generator);
command_generator_t* pause_generator(command_generator_t *generator);
+/* Adds a callback to a generator
+ * - generator : pointer to the generator to add a callback to
+ * - type : threshold comparison type
+ * - threshold : comparison threshold
+ * - callback_function : callback to be called when the event is triggered
+ */
command_generator_t* add_callback(command_generator_t *generator, uint8_t type, float threshold, void (*callback_function)(command_generator_t*));
+
+/*
+ * Removes the callback from 'generator'
+ */
command_generator_t* remove_callback(command_generator_t *generator);
+/*
+ * Gets (and computes) the current output value of 'generator'
+ */
float get_output_value(command_generator_t *generator);
#endif /* __COMMAND_GENERATOR_H */
hooks/post-receive
--
krobot
|
|
From: Xavier L. <Ba...@us...> - 2011-03-31 13:47: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 a51709e923cc4020fdae14ea2ab455f132fc7c45 (commit)
from 3d188dae29a66f3d1e491bd210699469799dfd2f (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 a51709e923cc4020fdae14ea2ab455f132fc7c45
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu Mar 31 15:46:53 2011 +0200
[Controller_Motor_STM32] Added a tc_goto_speed function
This function provides a simple way to change the speed with a given acceleration.
-----------------------------------------------------------------------
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 6186796..a74dd55 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
@@ -124,6 +124,7 @@ void acc_stop_callback(command_generator_t *generator) {
// Stopping acceleration
adjust_speed(&cont->speed, 0.);
+ adjust_value(&cont->speed, cont->aut.speed);
cont->working = 0;
}
@@ -254,6 +255,28 @@ void tc_goto(uint8_t motor, float angle, float speed, float acceleration) {
cont->working = 1;
}
+void tc_goto_speed(uint8_t motor, float speed, float acceleration) {
+ trajectory_controller_t *cont;
+
+ // Verify parameters
+ if (acceleration <= 0)
+ return;
+
+ // Get the controller and verifies it is enabled
+ cont = &controllers[get_motor_index(motor)];
+ if (!cont->enabled)
+ return;
+
+ cont->aut.speed = speed;
+ cont->aut.dir = SIGN(speed - cont->speed.type.last_output);
+ // Set acceleration sign depending on the current speed
+ cont->aut.acceleration = cont->aut.dir * acceleration;
+ adjust_speed(&cont->speed, cont->aut.acceleration);
+ add_callback(&cont->speed, SELECT_THRESHOLD(cont->aut.dir), cont->aut.speed, acc_stop_callback);
+
+ cont->working = 1;
+}
+
void tc_move(tc_robot_t *robot, float distance, float speed, float acceleration) {
float dis_s, spe_s, acc_s;
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
index 77b73e9..8c1eef6 100644
--- 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
@@ -64,6 +64,11 @@ uint8_t tc_is_working(uint8_t motors);
*/
void tc_goto(uint8_t motor, float angle, float speed, float acceleration);
+/* Accelerates or brakes the robot to the given speed
+ *
+ */
+void tc_goto_speed(uint8_t motor, float speed, float acceleration);
+
/* Moves along a line
* - robot : pointer to a structure describing the robot configuration
* - distance : distance to move of (in meters), can be positive (forward)
hooks/post-receive
--
krobot
|
