Commit from zer0 on branch b_zer0 (2009-05-18 14:19 CEST)
=================================
better ABS() macro
next step is to definitely rewrite these fu..ing MAX() and MIN()
aversive include/aversive.h 1.1.2.6
===========================
aversive/include/aversive.h (1.1.2.5 -> 1.1.2.6)
===========================
@@ -15,7 +15,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: aversive.h,v 1.1.2.5 2008-05-11 15:04:52 zer0 Exp $
+ * Revision : $Id: aversive.h,v 1.1.2.6 2009-05-18 12:19:51 zer0 Exp $
*
*/
@@ -107,8 +107,12 @@
* while the abs() function in the libc works only with int type
* this macro works with every numerical type including floats
*/
-#define ABS(val) ( ((val) < 0) ? -(val) : (val) )
-
+#define ABS(val) ({ \
+ __typeof(val) __val = (val); \
+ if (__val < 0) \
+ __val = - __val; \
+ __val; \
+ })
/*
* Extract bytes and u16 from larger integer
Commit from zer0 (2009-05-18 14:26 CEST)
================
file geometry_config.h was initially added on branch b_zer0.
- aversive modules/devices/robot/obstacle_avoidance/test/geometry_config.h 1.1
Commit from zer0 on branch b_zer0 (2009-05-18 14:26 CEST)
=================================
update geometry module: some fixes and some new features.
also remove geometry_config.h file
aversive modules/base/math/geometry/test/main.c 1.1.2.2
===============================================
aversive/modules/base/math/geometry/test/main.c (1.1.2.1 -> 1.1.2.2)
===============================================
@@ -10,7 +10,6 @@
#define EPSILON 0.000001
-#define MAX_COEF 50000
int main(void)
{
vect_t v, w;
@@ -28,7 +27,7 @@
point_t poly_pts1[4];
point_t poly_pts2[5];
-
+ polygon_set_boundingbox(25, 25, 275, 185);
/* basic vect test */
v.x = 1;
@@ -83,15 +82,15 @@
p2.x = 0;
p2.y = 10;
- pts2line(&p1, &p2, &l1, MAX_COEF);
- printf("%" PRIi32 " %" PRIi32 " %" PRIi32 "\r\n", l1.a, l1.b, l1.c);
+ pts2line(&p1, &p2, &l1);
+ printf("%2.2f %2.2f %2.2f\r\n", l1.a, l1.b, l1.c);
p3.x = 10;
p3.y = 0;
- pts2line(&p1, &p3, &l2, MAX_COEF);
- printf("%" PRIi32 " %" PRIi32 " %" PRIi32 "\r\n", l2.a, l2.b, l2.c);
+ pts2line(&p1, &p3, &l2);
+ printf("%2.2f %2.2f %2.2f\r\n", l2.a, l2.b, l2.c);
p4.x = 10;
@@ -100,8 +99,8 @@
p5.x = 20;
p5.y = 20;
- pts2line(&p4, &p5, &l3, MAX_COEF);
- printf("%" PRIi32 " %" PRIi32 " %" PRIi32 "\r\n", l3.a, l3.b, l3.c);
+ pts2line(&p4, &p5, &l3);
+ printf("%2.2f %2.2f %2.2f\r\n", l3.a, l3.b, l3.c);
p6.x = 30;
p6.y = 0;
@@ -109,8 +108,8 @@
p7.x = 0;
p7.y = 30;
- pts2line(&p6, &p7, &l4, MAX_COEF);
- printf("%" PRIi32 " %" PRIi32 " %" PRIi32 "\r\n", l4.a, l4.b, l4.c);
+ pts2line(&p6, &p7, &l4);
+ printf("%2.2f %2.2f %2.2f\r\n", l4.a, l4.b, l4.c);
intersect_line(&l1, &l2, &p);
@@ -125,17 +124,17 @@
intersect_line(&l3, &l4, &p);
printf("* %" PRIi32 " %" PRIi32 "\r\n", p.x, p.y);
- ret = intersect_segment(&p1, &p2, &p4, &p5, &p, MAX_COEF);
+ ret = intersect_segment(&p1, &p2, &p4, &p5, &p);
printf("%d (%" PRIi32 " %" PRIi32 ")\r\n", ret, p.x, p.y);
if (ret != 0)
printf("error in segment cros\r\n");
- ret = intersect_segment(&p4, &p5, &p6, &p7, &p, MAX_COEF);
+ ret = intersect_segment(&p4, &p5, &p6, &p7, &p);
printf("%d (%" PRIi32 " %" PRIi32 ")\r\n", ret, p.x, p.y);
if (ret != 1)
printf("error in segment cros\r\n");
- ret = intersect_segment(&p1, &p2, &p1, &p3, &p, MAX_COEF);
+ ret = intersect_segment(&p1, &p2, &p1, &p3, &p);
printf("%d (%" PRIi32 " %" PRIi32 ")\r\n", ret, p.x, p.y);
if (ret != 2)
printf("error in segment cros\r\n");
@@ -148,13 +147,81 @@
p10.y = 150;
p11.x = 195;
p11.y = 60;
- ret = intersect_segment(&p8, &p9, &p10, &p11, &p, MAX_COEF);
+ ret = intersect_segment(&p8, &p9, &p10, &p11, &p);
printf("%d (%" PRIi32 " %" PRIi32 ")\r\n", ret, p.x, p.y);
if (ret != 1)
printf("error in segment cros\r\n");
+
+ p6.x = 30;
+ p6.y = 0;
+
+ p7.x = 0;
+ p7.y = 30;
+
+ p8.x = 10;
+ p8.y = 10;
+
+ pts2line(&p6, &p7, &l3);
+ proj_pt_line(&p8, &l3, &p);
+ printf("proj: %" PRIi32 " %" PRIi32 "\r\n", p.x, p.y);
+ if (p.x != 15 || p.y != 15)
+ printf("error in proj 1\r\n");
+
+
+ p6.x = 0;
+ p6.y = 0;
+
+ p7.x = 0;
+ p7.y = 30;
+
+ p8.x = 10;
+ p8.y = 10;
+
+ pts2line(&p6, &p7, &l3);
+ proj_pt_line(&p8, &l3, &p);
+ printf("proj: %" PRIi32 " %" PRIi32 "\r\n", p.x, p.y);
+ if (p.x != 0 || p.y != 10)
+ printf("error in proj 2\r\n");
+
+
+ p6.x = 30;
+ p6.y = 0;
+
+ p7.x = 0;
+ p7.y = 0;
+
+ p8.x = 10;
+ p8.y = 10;
+
+ pts2line(&p6, &p7, &l3);
+ proj_pt_line(&p8, &l3, &p);
+ printf("proj: %" PRIi32 " %" PRIi32 "\r\n", p.x, p.y);
+ if (p.x != 10 || p.y != 0)
+ printf("error in proj 3\r\n");
+
+
+ p1.x = 0;
+ p1.y = 10;
+
+ p2.x = 20;
+ p2.y = 20;
+
+ pts2line(&p1, &p2, &l1);
+ printf("%2.2f %2.2f %2.2f\r\n", l1.a, l1.b, l1.c);
+
+ p2.x = 0;
+ p2.y = 10;
+
+ p1.x = 10;
+ p1.y = -10;
+
+ pts2line(&p1, &p2, &l1);
+ printf("%2.2f %2.2f %2.2f\r\n", l1.a, l1.b, l1.c);
+
+
/* basic poly tests */
poly1.pts = poly_pts1;
@@ -210,6 +277,8 @@
printf("error in is in poly\r\n");
+
+
return 0;
Commit from zer0 (2009-05-18 14:26 CEST)
================
file geometry_config.h was initially added on branch b_zer0.
- aversive modules/base/math/geometry/test/geometry_config.h 1.1
Commit from zer0 on branch b_zer0 (2009-05-18 14:26 CEST)
=================================
update geometry module: some fixes and some new features.
also remove geometry_config.h file
aversive modules/base/math/geometry/test/.config 1.1.2.2
================================================
aversive/modules/base/math/geometry/test/.config (1.1.2.1 -> 1.1.2.2)
================================================
@@ -1,5 +1,5 @@
#
-# Automatically generated by make menuconfig: don't edit
+# Automatically generated make config: don't edit
#
#
@@ -74,6 +74,10 @@
#
# Base modules
#
+
+#
+# Enable math library in generation options to see all modules
+#
# CONFIG_MODULE_CIRBUF is not set
# CONFIG_MODULE_CIRBUF_LARGE is not set
# CONFIG_MODULE_FIXED_POINT is not set
@@ -93,6 +97,10 @@
#
# Communication modules
#
+
+#
+# uart needs circular buffer, mf2 client may need scheduler
+#
# CONFIG_MODULE_UART is not set
# CONFIG_MODULE_UART_9BITS is not set
# CONFIG_MODULE_UART_CREATE_CONFIG is not set
@@ -175,6 +183,10 @@
# Control system modules
#
# CONFIG_MODULE_CONTROL_SYSTEM_MANAGER is not set
+
+#
+# Filters
+#
# CONFIG_MODULE_PID is not set
# CONFIG_MODULE_PID_CREATE_CONFIG is not set
# CONFIG_MODULE_RAMP is not set
@@ -185,12 +197,20 @@
#
# Radio devices
#
+
+#
+# Some radio devices require SPI to be activated
+#
# CONFIG_MODULE_CC2420 is not set
# CONFIG_MODULE_CC2420_CREATE_CONFIG is not set
#
# Crypto modules
#
+
+#
+# Crypto modules depend on utils module
+#
# CONFIG_MODULE_AES is not set
# CONFIG_MODULE_AES_CTR is not set
# CONFIG_MODULE_MD5 is not set
@@ -200,12 +220,20 @@
#
# Encodings modules
#
+
+#
+# Encoding modules depend on utils module
+#
# CONFIG_MODULE_BASE64 is not set
# CONFIG_MODULE_HAMMING is not set
#
# Debug modules
#
+
+#
+# Debug modules depend on utils module
+#
# CONFIG_MODULE_DIAGNOSTIC is not set
# CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG is not set
CONFIG_MODULE_ERROR=y
@@ -239,6 +267,7 @@
# CONFIG_AVRDUDE_PROG_JTAG1 is not set
# CONFIG_AVRDUDE_PROG_AVR109 is not set
CONFIG_AVRDUDE_PORT="/dev/parport0"
+CONFIG_AVRDUDE_BAUDRATE=19200
#
# Avarice
@@ -247,3 +276,4 @@
CONFIG_AVARICE_DEBUG_PORT=1234
CONFIG_AVARICE_PROG_MKI=y
# CONFIG_AVARICE_PROG_MKII is not set
+# CONFIG_AVRDUDE_CHECK_SIGNATURE is not set
Commit from zer0 (2009-05-18 14:26 CEST)
================
file geometry_config.h was initially added on branch b_zer0.
- aversive modules/base/math/geometry/config/geometry_config.h 1.1
Commit from zer0 on branch b_zer0 (2009-05-18 14:26 CEST)
=================================
update geometry module: some fixes and some new features.
also remove geometry_config.h file
aversive modules/base/math/geometry/polygon.h 1.1.2.2
aversive modules/base/math/geometry/polygon.c 1.1.2.2
aversive modules/base/math/geometry/lines.h 1.1.2.2
aversive modules/base/math/geometry/lines.c 1.1.2.2
=============================================
aversive/modules/base/math/geometry/polygon.h (1.1.2.1 -> 1.1.2.2)
=============================================
@@ -1,8 +1,6 @@
#ifndef _POLYGON_H_
#define _POLYGON_H_
-#include <geometry_config.h>
-
typedef struct _poly {
point_t * pts;
uint8_t l;
@@ -38,17 +36,15 @@
is_crossing_poly(point_t p1, point_t p2, point_t *intersect_pt,
poly_t *pol);
-/* XXX
-#define IS_IN_BOUNDINGBOX(pt) ( (pt).x >= PLAYGROUND_X_MIN && \
- (pt).x<=PLAYGROUND_X_MAX && \
- (pt).y >= PLAYGROUND_Y_MIN && (pt).y<=PLAYGROUND_Y_MAX )
-*/
-
-#define IS_IN_BOUNDINGBOX(pt) ( (pt).x >= PLAYGROUND_X_MIN && \
- (pt).x<=PLAYGROUND_X_MAX && \
- (pt).y >= PLAYGROUND_Y_MIN && (pt).y<=PLAYGROUND_Y_MAX )
+/*
+ * set coords of bounding box.
+ */
+void polygon_set_boundingbox(int32_t x1, int32_t y1, int32_t x2, int32_t y2);
-//#define IS_IN_BOUNDINGBOX(pt) (1)
+/*
+ * return 1 if a point is in the bounding box.
+ */
+uint8_t is_in_boundingbox(const point_t *p);
/* Giving the list of poygons, compute the graph of "visibility rays".
* This rays array is composed of indexes representing 2 polygon
=============================================
aversive/modules/base/math/geometry/polygon.c (1.1.2.1 -> 1.1.2.2)
=============================================
@@ -15,6 +15,29 @@
#define debug_printf(args...)
#endif
+/* default bounding box is (0,0) (100,100) */
+static int32_t bbox_x1 = 0;
+static int32_t bbox_y1 = 0;
+static int32_t bbox_x2 = 100;
+static int32_t bbox_y2 = 100;
+
+void polygon_set_boundingbox(int32_t x1, int32_t y1, int32_t x2, int32_t y2)
+{
+ bbox_x1 = x1;
+ bbox_y1 = y1;
+ bbox_x2 = x2;
+ bbox_y2 = y2;
+}
+
+uint8_t is_in_boundingbox(const point_t *p)
+{
+ if (p->x >= bbox_x1 &&
+ p->x <= bbox_x2 &&
+ p->y >= bbox_y1 &&
+ p->y <= bbox_y2)
+ return 1;
+ return 0;
+}
/* Test if a point is in a polygon (including edges)
* 0 not inside
@@ -93,19 +116,24 @@
" return %d\n", pol->pts[i].x, pol->pts[i].y,
pol->pts[(i+1)%pol->l].x, pol->pts[(i+1)%pol->l].y, ret);
- if (intersect_pt)
- *intersect_pt = p;
switch(ret) {
case 0:
break;
case 1:
+ if (intersect_pt)
+ *intersect_pt = p;
return 1;
break;
case 2:
cpt++;
+ if (intersect_pt)
+ *intersect_pt = p;
+
break;
case 3:
+ if (intersect_pt)
+ *intersect_pt = p;
return 2;
break;
}
@@ -118,6 +146,11 @@
/* XXX opt */
ret2 = is_in_poly(&p2, pol);
+ debug_printf("is in poly: p1 %d p2: %d cpt %d\r\n", ret1, ret2, cpt);
+
+ debug_printf("p intersect: %"PRIi32" %"PRIi32"\r\n", p.x, p.y);
+
+
if (cpt==0) {
if (ret1==1 || ret2==1)
return 1;
@@ -140,13 +173,6 @@
return 1;
}
-/* XXX
-#define IS_IN_BOUNDINGBOX(pt) ( (pt).x >= PLAYGROUND_X_MIN && \
- (pt).x<=PLAYGROUND_X_MAX && \
- (pt).y >= PLAYGROUND_Y_MIN && (pt).y<=PLAYGROUND_Y_MAX )
-*/
-
-//#define IS_IN_BOUNDINGBOX(pt) (1)
/* Giving the list of poygons, compute the graph of "visibility rays".
* This rays array is composed of indexes representing 2 polygon
@@ -184,12 +210,12 @@
debug_printf("%s(): poly num %d/%d\n", __FUNCTION__, i, npolys);
for (ii=0; ii<polys[i].l; ii++) {
debug_printf("%s() line num %d/%d\n", __FUNCTION__, ii,
polys[i].l);
- if (! IS_IN_BOUNDINGBOX(polys[i].pts[ii]))
+ if (! is_in_boundingbox(&polys[i].pts[ii]))
continue;
is_ok = 1;
n = (ii+1)%polys[i].l;
- if (!(IS_IN_BOUNDINGBOX(polys[i].pts[n])))
+ if (!(is_in_boundingbox(&polys[i].pts[n])))
continue;
@@ -224,14 +250,14 @@
for (i=0; i<npolys-1; i++) {
for (pt1=0;pt1<polys[i].l;pt1++) {
- if (!(IS_IN_BOUNDINGBOX(polys[i].pts[pt1])))
+ if (!(is_in_boundingbox(&polys[i].pts[pt1])))
continue;
/* for next poly */
for (ii=i+1; ii<npolys; ii++) {
for (pt2=0;pt2<polys[ii].l;pt2++) {
- if
(!(IS_IN_BOUNDINGBOX(polys[ii].pts[pt2])))
+ if
(!(is_in_boundingbox(&polys[ii].pts[pt2])))
continue;
is_ok=1;
===========================================
aversive/modules/base/math/geometry/lines.h (1.1.2.1 -> 1.1.2.2)
===========================================
@@ -8,6 +8,9 @@
void
pts2line(const point_t *p1, const point_t *p2, line_t *l);
+void
+proj_pt_line(const point_t * p, const line_t * l, point_t * p_out);
+
uint8_t
intersect_line(const line_t *l1, const line_t *l2, point_t *p);
===========================================
aversive/modules/base/math/geometry/lines.c (1.1.2.1 -> 1.1.2.2)
===========================================
@@ -1,3 +1,5 @@
+#include <aversive.h>
+
#include <stdint.h>
#include <inttypes.h>
#include <stdlib.h>
@@ -15,10 +17,6 @@
#define debug_printf(args...)
#endif
-#ifndef ABS
-#define ABS(val) ( ((val) < 0) ? -(val) : (val) )
-#endif
-
/* return values :
* 0 dont cross
* 1 cross
@@ -109,6 +107,19 @@
__FUNCTION__, l->a, l->b, l->c);
}
+void proj_pt_line(const point_t * p, const line_t * l, point_t * p_out)
+{
+ line_t l_tmp;
+
+ l_tmp.a = l->b;
+ l_tmp.b = -l->a;
+ l_tmp.c = -l_tmp.a*p->x - l_tmp.b*p->y;
+
+ p_out->y = (l_tmp.a*l->c - l->a*l_tmp.c) / (l->a*l_tmp.b -
l_tmp.a*l->b);
+ p_out->x = (l->b*l_tmp.c - l_tmp.b*l->c) / (l->a*l_tmp.b -
l_tmp.a*l->b);
+
+}
+
/* return values:
Commit from zer0 on branch b_zer0 (2009-05-18 14:27 CEST)
=================================
compensate centrifugal force
aversive modules/devices/robot/position_manager/position_manager.h 1.5.4.4
aversive modules/devices/robot/position_manager/position_manager.c 1.6.4.7
aversive config/config.in 1.42.4.31
==================================================================
aversive/modules/devices/robot/position_manager/position_manager.h (1.5.4.3 ->
1.5.4.4)
==================================================================
@@ -15,7 +15,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: position_manager.h,v 1.5.4.3 2009-05-02 10:03:04 zer0 Exp $
+ * Revision : $Id: position_manager.h,v 1.5.4.4 2009-05-18 12:27:26 zer0 Exp $
*
*/
@@ -71,12 +71,20 @@
struct xya_position_s16 pos_s16;
struct rs_polar prev_encoders;
struct robot_system *rs;
+#ifdef CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE
+ double centrifugal_coef;
+#endif
};
/** initialization of the robot_position pos, everthing is set to 0 */
void position_init(struct robot_position *pos);
+#ifdef CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE
+/** set arbitrary coef to compensate the centrifugal force */
+void position_set_centrifugal_coef(struct robot_position *pos, double coef);
+#endif
+
/** Set a new robot position */
void position_set(struct robot_position *pos, int16_t x, int16_t y, int16_t a);
==================================================================
aversive/modules/devices/robot/position_manager/position_manager.c (1.6.4.6 ->
1.6.4.7)
==================================================================
@@ -15,7 +15,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: position_manager.c,v 1.6.4.6 2009-05-02 10:03:04 zer0 Exp $
+ * Revision : $Id: position_manager.c,v 1.6.4.7 2009-05-18 12:27:26 zer0 Exp $
*
*/
@@ -48,6 +48,13 @@
IRQ_UNLOCK(flags);
}
+#ifdef CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE
+void position_set_centrifugal_coef(struct robot_position *pos, double coef)
+{
+ pos->centrifugal_coef = coef;
+}
+#endif
+
/**
* Save in pos structure the pointer to the associated robot_system.
* The robot_system structure is used to get values from virtual encoders
@@ -112,6 +119,7 @@
void position_manage(struct robot_position *pos)
{
double x, y, a, r, arc_angle;
+ double dx, dy;
s16 x_s16, y_s16, a_s16;
struct rs_polar encoders;
struct rs_polar delta;
@@ -156,23 +164,53 @@
if (delta.angle == 0) {
/* we go straight */
- x = x + cos(a) * ((double) delta.distance /
(pos->phys.distance_imp_per_mm)) ;
- y = y + sin(a) * ((double) delta.distance /
(pos->phys.distance_imp_per_mm)) ;
+ dx = cos(a) * ((double) delta.distance /
(pos->phys.distance_imp_per_mm)) ;
+ dy = sin(a) * ((double) delta.distance /
(pos->phys.distance_imp_per_mm)) ;
+ x += dx;
+ y += dy;
}
else {
/* r the radius of the circle arc */
r = (double)delta.distance * pos->phys.track_mm / ((double)
delta.angle * 2);
arc_angle = 2 * (double) delta.angle / (pos->phys.track_mm *
pos->phys.distance_imp_per_mm);
- x += r * (-sin(a) + sin(a+arc_angle));
- y += r * (cos(a) - cos(a+arc_angle));
+ dx = r * (-sin(a) + sin(a+arc_angle));
+ dy = r * (cos(a) - cos(a+arc_angle));
+
+ x += dx;
+ y += dy;
a += arc_angle;
- }
- if (a < -M_PI)
- a += (M_PI*2);
- else if (a > (M_PI))
- a -= (M_PI*2);
+ if (a < -M_PI)
+ a += (M_PI*2);
+ else if (a > (M_PI))
+ a -= (M_PI*2);
+
+#ifdef CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE
+ /* This part compensate the centrifugal force when we
+ * turn very quickly. Idea is from Gargamel (RCVA). */
+ if (pos->centrifugal_coef && r != 0) {
+ double k;
+
+ /*
+ * centrifugal force is F = (m.v^2 / R)
+ * with v: angular speed
+ * R: radius of the circle
+ */
+
+ k = ((double) delta.distance);
+ k = k * k;
+ k /= r;
+ k *= pos->centrifugal_coef;
+
+ /*
+ * F acts perpendicularly to the vector
+ */
+ x += k * sin(a);
+ y -= k * cos(a);
+ }
+#endif
+ }
/* update int position */
x_s16 = (int16_t)x;
@@ -239,3 +277,4 @@
{
return pos->pos_d.a;
}
+
=========================
aversive/config/config.in (1.42.4.30 -> 1.42.4.31)
=========================
@@ -131,9 +131,6 @@
dep_bool 'Geometry lib' CONFIG_MODULE_GEOMETRY \
$CONFIG_MATH_LIB
-dep_bool ' |-- Create Default geometry config'
CONFIG_MODULE_GEOMETRY_CREATE_CONFIG \
- $CONFIG_MODULE_GEOMETRY
-
#### SCHEDULER
bool 'Scheduler' CONFIG_MODULE_SCHEDULER
@@ -370,6 +367,9 @@
dep_bool 'Position manager' CONFIG_MODULE_POSITION_MANAGER \
$CONFIG_MODULE_ROBOT_SYSTEM
+dep_bool ' |-- Compensate centrifugal force'
CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE \
+ $CONFIG_MODULE_POSITION_MANAGER
+
#### TRAJECTORY MANAGER
dep_bool 'Trajectory manager' CONFIG_MODULE_TRAJECTORY_MANAGER \
$CONFIG_MODULE_POSITION_MANAGER \
Commit from zer0 on branch b_zer0 (2009-05-18 14:28 CEST)
=================================
Fix trajectory windows in trajectory manager
aversive modules/devices/robot/trajectory_manager/trajectory_manager.c
1.4.4.17
======================================================================
aversive/modules/devices/robot/trajectory_manager/trajectory_manager.c
(1.4.4.16 -> 1.4.4.17)
======================================================================
@@ -15,7 +15,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: trajectory_manager.c,v 1.4.4.16 2009-05-02 10:03:04 zer0
Exp $
+ * Revision : $Id: trajectory_manager.c,v 1.4.4.17 2009-05-18 12:28:36 zer0
Exp $
*
*/
@@ -179,7 +179,8 @@
/** near the target (dist) ? */
static uint8_t is_robot_in_dist_window(struct trajectory *traj, double d_win)
{
- double d = ABS(traj->target.pol.distance-rs_get_distance(traj->robot));
+ double d = traj->target.pol.distance - rs_get_distance(traj->robot);
+ d = ABS(d);
d = d / traj->position->phys.distance_imp_per_mm;
return (d < d_win);
}
@@ -194,21 +195,19 @@
return ( sqrt ((x2-x1) * (x2-x1) + (y2-y1) * (y2-y1)) < d_win );
}
-/** near the target (angle) ? */
-static inline uint8_t
-__is_robot_in_angle_window(struct trajectory *traj, double target_angle,
double a_win_rad)
-{
- return ( ABS(target_angle*2) < a_win_rad );
-}
-
/** near the angle target in radian ? Only valid if
* traj->target.pol.angle is set (i.e. an angle command, not an xy
* command) */
static uint8_t is_robot_in_angle_window(struct trajectory *traj, double
a_win_rad)
{
- return __is_robot_in_angle_window(traj, traj->target.pol.angle -
-
position_get_a_rad_double(traj->position),
- a_win_rad);
+ double a;
+
+ /* convert relative angle from imp to rad */
+ a = traj->target.pol.angle - rs_get_angle(traj->robot);
+ a /= traj->position->phys.distance_imp_per_mm;
+ a /= traj->position->phys.track_mm;
+ a *= 2.;
+ return ABS(a) < a_win_rad;
}
@@ -227,17 +226,14 @@
* a_rad : angle in radian
* flags : what to update (UPDATE_A, UPDATE_D)
*/
-void __trajectory_goto_d_a_rel(struct trajectory *traj, double d_mm, double
a_rad, uint8_t flags)
+void __trajectory_goto_d_a_rel(struct trajectory *traj, double d_mm,
+ double a_rad, uint8_t state, uint8_t flags)
{
int32_t a_consign, d_consign;
- /* XXX only used for debug */
- double posx = position_get_x_double(traj->position);
- double posy = position_get_y_double(traj->position);
- double posa = position_get_a_rad_double(traj->position);
DEBUG(E_TRAJECTORY, "Goto DA/RS rel to d=%f a_rad=%f", d_mm, a_rad);
- DEBUG(E_TRAJECTORY, "current pos = x=%f, y=%f, a_rad=%f", posx, posy,
posa);
delete_event(traj);
+ traj->state = state;
if (flags & UPDATE_A) {
if (flags & RESET_A) {
a_consign = 0;
@@ -245,9 +241,10 @@
else {
a_consign = (int32_t)(a_rad *
(traj->position->phys.distance_imp_per_mm) *
(traj->position->phys.track_mm) /
2);
- traj->target.pol.angle = a_consign;
}
- cs_set_consign(traj->csm_angle, a_consign +
rs_get_angle(traj->robot));
+ a_consign += rs_get_angle(traj->robot);
+ traj->target.pol.angle = a_consign;
+ cs_set_consign(traj->csm_angle, a_consign);
}
if (flags & UPDATE_D) {
if (flags & RESET_D) {
@@ -255,31 +252,31 @@
}
else {
d_consign = (int32_t)((d_mm) *
(traj->position->phys.distance_imp_per_mm));
- traj->target.pol.distance = d_consign;
}
- cs_set_consign(traj->csm_distance, d_consign +
rs_get_distance(traj->robot));
+ d_consign += rs_get_distance(traj->robot);
+ traj->target.pol.distance = d_consign;
+ cs_set_consign(traj->csm_distance, d_consign);
}
}
/** go straight forward (d is in mm) */
void trajectory_d_rel(struct trajectory *traj, double d_mm)
{
- traj->state = RUNNING_D;
- __trajectory_goto_d_a_rel(traj, d_mm, 0, UPDATE_D | UPDATE_A | RESET_A);
+ __trajectory_goto_d_a_rel(traj, d_mm, 0, RUNNING_D,
+ UPDATE_D | UPDATE_A | RESET_A);
}
/** update distance consign without changing angle consign */
void trajectory_only_d_rel(struct trajectory *traj, double d_mm)
{
- traj->state = RUNNING_D;
- __trajectory_goto_d_a_rel(traj, d_mm, 0, UPDATE_D);
+ __trajectory_goto_d_a_rel(traj, d_mm, 0, RUNNING_D, UPDATE_D);
}
/** turn by 'a' degrees */
void trajectory_a_rel(struct trajectory *traj, double a_deg_rel)
{
- traj->state = RUNNING_A;
- __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg_rel), UPDATE_A | UPDATE_D
| RESET_D);
+ __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg_rel), RUNNING_A,
+ UPDATE_A | UPDATE_D | RESET_D);
}
/** turn by 'a' degrees */
@@ -288,10 +285,10 @@
double posa = position_get_a_rad_double(traj->position);
double a;
- traj->state = RUNNING_A;
a = RAD(a_deg_abs) - posa;
a = modulo_2pi(a);
- __trajectory_goto_d_a_rel(traj, 0, a, UPDATE_A | UPDATE_D | RESET_D);
+ __trajectory_goto_d_a_rel(traj, 0, a, RUNNING_A,
+ UPDATE_A | UPDATE_D | RESET_D);
}
/** turn the robot until the point x,y is in front of us */
@@ -301,10 +298,10 @@
double posy = position_get_y_double(traj->position);
double posa = position_get_a_rad_double(traj->position);
- traj->state = RUNNING_A;
DEBUG(E_TRAJECTORY, "Goto Turn To xy %f %f", x_abs_mm, y_abs_mm);
__trajectory_goto_d_a_rel(traj, 0,
simple_modulo_2pi(atan2(y_abs_mm - posy, x_abs_mm -
posx) - posa),
+ RUNNING_A,
UPDATE_A | UPDATE_D | RESET_D);
}
@@ -315,18 +312,18 @@
double posy = position_get_y_double(traj->position);
double posa = position_get_a_rad_double(traj->position);
- traj->state = RUNNING_A;
DEBUG(E_TRAJECTORY, "Goto Turn To xy %f %f", x_abs_mm, y_abs_mm);
__trajectory_goto_d_a_rel(traj, 0,
modulo_2pi(atan2(y_abs_mm - posy, x_abs_mm - posx) -
posa + M_PI),
+ RUNNING_A,
UPDATE_A | UPDATE_D | RESET_D);
}
/** update angle consign without changing distance consign */
void trajectory_only_a_rel(struct trajectory *traj, double a_deg)
{
- traj->state = RUNNING_A;
- __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg), UPDATE_A);
+ __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg), RUNNING_A,
+ UPDATE_A);
}
/** update angle consign without changing distance consign */
@@ -335,24 +332,23 @@
double posa = position_get_a_rad_double(traj->position);
double a;
- traj->state = RUNNING_A;
a = RAD(a_deg_abs) - posa;
a = modulo_2pi(a);
- __trajectory_goto_d_a_rel(traj, 0, a, UPDATE_A);
+ __trajectory_goto_d_a_rel(traj, 0, a, RUNNING_A, UPDATE_A);
}
/** turn by 'a' degrees */
void trajectory_d_a_rel(struct trajectory *traj, double d_mm, double a_deg)
{
- traj->state = RUNNING_AD;
- __trajectory_goto_d_a_rel(traj, d_mm, RAD(a_deg), UPDATE_A | UPDATE_D);
+ __trajectory_goto_d_a_rel(traj, d_mm, RAD(a_deg),
+ RUNNING_AD, UPDATE_A | UPDATE_D);
}
/** set relative angle and distance consign to 0 */
void trajectory_stop(struct trajectory *traj)
{
- traj->state = READY;
- __trajectory_goto_d_a_rel(traj, 0, 0, UPDATE_A | UPDATE_D | RESET_D |
RESET_A);
+ __trajectory_goto_d_a_rel(traj, 0, 0, READY,
+ UPDATE_A | UPDATE_D | RESET_D | RESET_A);
}
/** set relative angle and distance consign to 0, and break any
@@ -361,10 +357,10 @@
{
struct quadramp_filter *q_d, *q_a;
- traj->state = READY;
q_d = traj->csm_distance->consign_filter_params;
q_a = traj->csm_angle->consign_filter_params;
- __trajectory_goto_d_a_rel(traj, 0, 0, UPDATE_A | UPDATE_D | RESET_D |
RESET_A);
+ __trajectory_goto_d_a_rel(traj, 0, 0, READY,
+ UPDATE_A | UPDATE_D | RESET_D | RESET_A);
q_d->previous_var = 0;
q_d->previous_out = rs_get_distance(traj->robot);
Commit from zer0 on branch b_zer0 (2009-05-18 14:29 CEST)
=================================
simple cast to avoid warning
aversive
modules/devices/robot/blocking_detection_manager/blocking_detection_manager.c
1.1.2.7
======================================================================================
aversive/modules/devices/robot/blocking_detection_manager/blocking_detection_manager.c
(1.1.2.6 -> 1.1.2.7)
======================================================================================
@@ -15,7 +15,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: blocking_detection_manager.c,v 1.1.2.6 2008-05-09 08:25:10
zer0 Exp $
+ * Revision : $Id: blocking_detection_manager.c,v 1.1.2.7 2009-05-18 12:29:10
zer0 Exp $
*
* Olivier MATZ <[email protected]>
*/
@@ -82,13 +82,13 @@
/* if current-based blocking_detection enabled */
if ( bd->cpt_thres ) {
i = bd->k1 * cmd - bd->k2 * speed;
- if (ABS(i) > bd->i_thres &&
+ if ((uint32_t)ABS(i) > bd->i_thres &&
(bd->speed_thres == 0 || ABS(speed) < bd->speed_thres)) {
if (bd->cpt == bd->cpt_thres - 1)
WARNING(E_BLOCKING_DETECTION_MANAGER,
"BLOCKING cmd=%ld, speed=%ld i=%ld",
cmd, speed, i);
- if(bd->cpt < bd->cpt_thres)
+ if (bd->cpt < bd->cpt_thres)
bd->cpt++;
}
else {
Commit from zer0 on branch b_zer0 (2009-05-18 14:29 CEST)
=================================
add quadramp_reset()
aversive modules/devices/control_system/filters/quadramp/quadramp.h 1.3.4.4
aversive modules/devices/control_system/filters/quadramp/quadramp.c 1.4.4.7
===================================================================
aversive/modules/devices/control_system/filters/quadramp/quadramp.h (1.3.4.3
-> 1.3.4.4)
===================================================================
@@ -15,7 +15,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: quadramp.h,v 1.3.4.3 2007-12-31 16:25:00 zer0 Exp $
+ * Revision : $Id: quadramp.h,v 1.3.4.4 2009-05-18 12:29:51 zer0 Exp $
*
*/
@@ -39,6 +39,8 @@
/** Initialization of the filter */
void quadramp_init(struct quadramp_filter *q);
+void quadramp_reset(struct quadramp_filter * q);
+
void quadramp_set_2nd_order_vars(struct quadramp_filter *q,
uint32_t var_2nd_ord_pos,
uint32_t var_2nd_ord_neg);
===================================================================
aversive/modules/devices/control_system/filters/quadramp/quadramp.c (1.4.4.6
-> 1.4.4.7)
===================================================================
@@ -1,3 +1,4 @@
+
/*
* Copyright Droids Corporation, Microb Technology, Eirbot (2005)
*
@@ -15,7 +16,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: quadramp.c,v 1.4.4.6 2009-02-02 22:21:20 zer0 Exp $
+ * Revision : $Id: quadramp.c,v 1.4.4.7 2009-05-18 12:29:51 zer0 Exp $
*
*/
@@ -36,6 +37,14 @@
IRQ_UNLOCK(flags);
}
+
+void quadramp_reset(struct quadramp_filter * q)
+{
+ q->previous_var = 0;
+ q->previous_out = 0;
+ q->previous_in = 0;
+}
+
void quadramp_set_2nd_order_vars(struct quadramp_filter * q,
uint32_t var_2nd_ord_pos,
uint32_t var_2nd_ord_neg)
Commit from zer0 on branch b_zer0 (2009-05-18 14:30 CEST)
=================================
make dump_event() public
aversive modules/base/scheduler/scheduler_private.h 1.1.2.8
aversive modules/base/scheduler/scheduler_dump.c 1.1.2.3
aversive modules/base/scheduler/scheduler.h 1.8.4.11
===================================================
aversive/modules/base/scheduler/scheduler_private.h (1.1.2.7 -> 1.1.2.8)
===================================================
@@ -15,7 +15,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: scheduler_private.h,v 1.1.2.7 2007-11-27 23:16:15 zer0 Exp
$
+ * Revision : $Id: scheduler_private.h,v 1.1.2.8 2009-05-18 12:30:36 zer0 Exp
$
*
*/
@@ -64,8 +64,7 @@
/* define dump_events() if we are in debug mode */
#ifdef SCHEDULER_DEBUG
-#define DUMP_EVENTS() dump_events()
-void dump_events(void);
+#define DUMP_EVENTS() scheduler_dump_events()
#else /* SCHEDULER_DEBUG */
#define DUMP_EVENTS() do {} while(0)
================================================
aversive/modules/base/scheduler/scheduler_dump.c (1.1.2.2 -> 1.1.2.3)
================================================
@@ -15,37 +15,37 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: scheduler_dump.c,v 1.1.2.2 2007-05-23 17:18:11 zer0 Exp $
+ * Revision : $Id: scheduler_dump.c,v 1.1.2.3 2009-05-18 12:30:36 zer0 Exp $
*
*/
#include <stdio.h>
#include <aversive.h>
+#include <aversive/pgmspace.h>
+
#include <scheduler_config.h>
#include <scheduler_private.h>
/** Dump the event structure table */
-void
-dump_events(void)
+void scheduler_dump_events(void)
{
int i;
- printf("== Dump events ==\n");
+ printf_P(PSTR("== Dump events ==\r\n"));
for (i=0 ; i<SCHEDULER_NB_MAX_EVENT ; i++) {
- printf(" [...@%p : ", i, &g_tab_event[i]);
- printf(" state=%d", g_tab_event[i].state);
+ printf_P(PSTR(" [...@%p : "), i, &g_tab_event[i]);
+ printf_P(PSTR(" state=%d"), g_tab_event[i].state);
if (g_tab_event[i].state >= SCHEDULER_EVENT_ACTIVE ) {
- printf(", ");
- printf("f=%p, ", g_tab_event[i].f);
- printf("data=%p, ", g_tab_event[i].data);
- printf("period=%d, ", g_tab_event[i].period);
- printf("current_time=%d, ",
g_tab_event[i].current_time);
- printf("priority=%d, ", g_tab_event[i].priority);
- printf("list_next=%p\n", SLIST_NEXT(&g_tab_event[i],
next));
+ printf_P(PSTR(", f=%p, "), g_tab_event[i].f);
+ printf_P(PSTR("data=%p, "), g_tab_event[i].data);
+ printf_P(PSTR("period=%d, "), g_tab_event[i].period);
+ printf_P(PSTR("current_time=%d, "),
g_tab_event[i].current_time);
+ printf_P(PSTR("priority=%d, "),
g_tab_event[i].priority);
+ printf_P(PSTR("list_next=%p\r\n"),
SLIST_NEXT(&g_tab_event[i], next));
}
else {
- printf("\n");
+ printf_P(PSTR("\r\n"));
}
}
}
===========================================
aversive/modules/base/scheduler/scheduler.h (1.8.4.10 -> 1.8.4.11)
===========================================
@@ -15,7 +15,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: scheduler.h,v 1.8.4.10 2009-04-24 19:26:01 zer0 Exp $
+ * Revision : $Id: scheduler.h,v 1.8.4.11 2009-05-18 12:30:36 zer0 Exp $
*
*/
@@ -127,6 +127,9 @@
/** Initialisation of the module */
void scheduler_init(void);
+/** dump all loaded events */
+void scheduler_dump_events(void);
+
/**
* Add an event to the event table.
* Return the id of the event on succes and -1 on error
_______________________________________________
Avr-list mailing list
[email protected]
CVSWEB : http://cvsweb.droids-corp.org/cgi-bin/viewcvs.cgi/aversive
WIKI : http://wiki.droids-corp.org/index.php/Aversive
DOXYGEN : http://zer0.droids-corp.org/doxygen_aversive/html/
BUGZILLA : http://bugzilla.droids-corp.org
COMMIT LOGS : http://zer0.droids-corp.org/aversive_commitlog
