#elif defined (__AVR_ATxmega64A3__)
#include <aversive/parts/ATxmega64A3.h>
#else
+#ifndef HOST_VERSION
#error "This arch is not implemented yet"
#endif
+#endif
#endif /* _AVERSIVE_PARTS_H_ */
#ifndef HOST_VERSION
#include <avr/pgmspace.h>
+#define PGMS_FMT "%S"
#else
#include <stdint.h>
-
-#define printf_P printf
-#define memcmp_P memcmp
-#define strcat_P strcat
-#define strcmp_P strcmp
-#define strncmp_P strncmp
-#define strlen_P strlen
-#define vfprintf_P vfprintf
-#define vsprintf_P vsprintf
-#define PGM_P const char *
-#define PSTR(x) x
-#define PROGMEM
-
-/* XXX don't define it, it's dangerous because it can be used to read
- * an address that have not the same size */
-/* #define pgm_read_word(x) (*(x)) */
-/* #define pgm_read_byte(x) (*(x)) */
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdarg.h>
typedef void prog_void;
typedef char prog_char;
typedef uint32_t prog_uint32_t;
typedef int64_t prog_int64_t;
+
+static inline int memcmp_P(const void *s1,
+ const prog_void *s2, unsigned n)
+{
+ return memcmp(s1, s2, n);
+}
+
+static inline void *memcpy_P(void *s1,
+ const prog_void *s2, unsigned n)
+{
+ return memcpy(s1, s2, n);
+}
+
+static inline char *strcat_P(char *s1, const prog_char *s2)
+{
+ return strcat(s1, s2);
+}
+
+static inline char *strcpy_P(char *s1, const prog_char *s2)
+{
+ return strcpy(s1, s2);
+}
+
+static inline char *strncpy_P(char *s1, const prog_char *s2,
+ unsigned n)
+{
+ return strncpy(s1, s2, n);
+}
+
+static inline int strcmp_P(const char *s1, const prog_char *s2)
+{
+ return strcmp(s1, s2);
+}
+
+static inline int strncmp_P(const char *s1, const prog_char *s2,
+ unsigned n)
+{
+ return strncmp(s1, s2, n);
+}
+
+static inline unsigned strlen_P(const prog_char *s)
+{
+ return strlen(s);
+}
+
+static inline int vfprintf_P(FILE *stream,
+ const prog_char *s, va_list ap)
+{
+ return vfprintf(stream, s, ap);
+}
+
+static inline int vsprintf_P(char *buf, const prog_char *s,
+ va_list ap)
+{
+ return vsprintf(buf, s, ap);
+}
+
+#define PGM_P const char *
+#define PSTR(x) x
+#define PROGMEM
+#define printf_P(arg...) printf(args)
+#define sprintf_P(buf, args...) sprintf(buf, args)
+#define snprintf_P(buf, n, args...) snprintf(buf, n, args)
+
+static inline uint32_t pgm_read_dword(const prog_void *x)
+{
+ return *(uint32_t *)x;
+}
+
+static inline uint16_t pgm_read_word(const prog_void *x)
+{
+ return *(uint16_t *)x;
+}
+
+static inline uint8_t pgm_read_byte(const prog_void *x)
+{
+ return *(uint8_t *)x;
+}
+
+#define PGMS_FMT "%s"
+
#endif /* HOST_VERSION */
#endif /* _AVERSIVE_PGMSPACE_H_ */
# CONFIG_MCU_ATMEGA645 is not set
# CONFIG_MCU_ATMEGA6450 is not set
CONFIG_MCU_ATMEGA128=y
+# CONFIG_MCU_ATMEGA1281 is not set
# CONFIG_MCU_AT90CAN128 is not set
# CONFIG_MCU_AT94K is not set
# CONFIG_MCU_AT90S1200 is not set
+# CONFIG_MCU_ATMEGA2560 is not set
+# CONFIG_MCU_ATMEGA256 is not set
CONFIG_QUARTZ=16000000
#
CONFIG_OPTM_S=y
CONFIG_MATH_LIB=y
# CONFIG_FDEVOPEN_COMPAT is not set
+# CONFIG_NO_PRINTF is not set
# CONFIG_MINIMAL_PRINTF is not set
# CONFIG_STANDARD_PRINTF is not set
CONFIG_ADVANCED_PRINTF=y
# CONFIG_MODULE_CIRBUF_LARGE is not set
CONFIG_MODULE_FIXED_POINT=y
# CONFIG_MODULE_VECT2 is not set
+# CONFIG_MODULE_GEOMETRY is not set
# CONFIG_MODULE_SCHEDULER is not set
+# CONFIG_MODULE_SCHEDULER_STATS is not set
# CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set
# CONFIG_MODULE_SCHEDULER_USE_TIMERS is not set
CONFIG_MODULE_SCHEDULER_TIMER0=y
# CONFIG_MODULE_SCHEDULER_MANUAL is not set
# CONFIG_MODULE_TIME is not set
# CONFIG_MODULE_TIME_CREATE_CONFIG is not set
+# CONFIG_MODULE_TIME_EXT is not set
+# CONFIG_MODULE_TIME_EXT_CREATE_CONFIG is not set
#
# Communication modules
# uart needs circular buffer, mf2 client may need scheduler
#
CONFIG_MODULE_UART=y
+# CONFIG_MODULE_UART_9BITS is not set
CONFIG_MODULE_UART_CREATE_CONFIG=y
+# CONFIG_MODULE_SPI is not set
+# CONFIG_MODULE_SPI_CREATE_CONFIG is not set
# CONFIG_MODULE_I2C is not set
# CONFIG_MODULE_I2C_MASTER is not set
# CONFIG_MODULE_I2C_MULTIMASTER is not set
# CONFIG_MODULE_TIMER_DYNAMIC is not set
# CONFIG_MODULE_PWM is not set
# CONFIG_MODULE_PWM_CREATE_CONFIG is not set
+# CONFIG_MODULE_PWM_NG is not set
# CONFIG_MODULE_ADC is not set
# CONFIG_MODULE_ADC_CREATE_CONFIG is not set
# CONFIG_MODULE_RDLINE_KILL_BUF is not set
# CONFIG_MODULE_RDLINE_HISTORY is not set
# CONFIG_MODULE_PARSE is not set
+# CONFIG_MODULE_PARSE_NO_FLOAT is not set
#
# External devices modules
# CONFIG_MODULE_LCD_CREATE_CONFIG is not set
# CONFIG_MODULE_MULTISERVO is not set
# CONFIG_MODULE_MULTISERVO_CREATE_CONFIG is not set
+# CONFIG_MODULE_AX12 is not set
+# CONFIG_MODULE_AX12_CREATE_CONFIG is not set
#
# Brushless motor drivers (you should enable pwm modules to see all)
# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE_CREATE_CONFIG is not set
#
-# Encoders
+# Encoders (you need comm/spi for encoders_spi)
#
# CONFIG_MODULE_ENCODERS_MICROB is not set
# CONFIG_MODULE_ENCODERS_MICROB_CREATE_CONFIG is not set
# CONFIG_MODULE_ENCODERS_EIRBOT is not set
# CONFIG_MODULE_ENCODERS_EIRBOT_CREATE_CONFIG is not set
+# CONFIG_MODULE_ENCODERS_SPI is not set
+# CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG is not set
#
-# Robot specific modules
+# Robot specific modules (fixed point lib may be needed)
#
# CONFIG_MODULE_ROBOT_SYSTEM is not set
+# CONFIG_MODULE_ROBOT_SYSTEM_USE_F64 is not set
# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set
# CONFIG_MODULE_POSITION_MANAGER is not set
+# CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE is not set
# CONFIG_MODULE_TRAJECTORY_MANAGER is not set
# CONFIG_MODULE_BLOCKING_DETECTION_MANAGER is not set
# CONFIG_MODULE_OBSTACLE_AVOIDANCE is not set
+# CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG is not set
#
# Control system modules
# CONFIG_MODULE_QUADRAMP_DERIVATE is not set
# CONFIG_MODULE_BIQUAD is not set
+#
+# 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
#
# CONFIG_AVRDUDE_PROG_BSD is not set
# CONFIG_AVRDUDE_PROG_DAPA is not set
# CONFIG_AVRDUDE_PROG_JTAG1 is not set
+# CONFIG_AVRDUDE_PROG_AVR109 is not set
CONFIG_AVRDUDE_PORT="/dev/parport0"
+CONFIG_AVRDUDE_BAUDRATE=19200
#
# Avarice
CONFIG_AVARICE_DEBUG_PORT=1234
CONFIG_AVARICE_PROG_MKI=y
# CONFIG_AVARICE_PROG_MKII is not set
+# CONFIG_AVRDUDE_CHECK_SIGNATURE is not set
TARGET = geometry
# List C source files here. (C dependencies are automatically generated.)
-SRC = vect_base.c lines.c polygon.c
+SRC = vect_base.c lines.c polygon.c circles.c
###########################################
--- /dev/null
+/*
+ * Copyright Droids Corporation (2009)
+ *
+ * This program 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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: f16.h,v 1.6.4.3 2008-05-10 15:06:26 zer0 Exp $
+ *
+ */
+
+#include <math.h>
+
+#include <aversive.h>
+
+#include <vect_base.h>
+#include <circles.h>
+
+static inline float sq(float x)
+{
+ return x*x;
+}
+
+uint8_t pt_is_inside_circle(const point_t *p, circle_t *c)
+{
+ vect_t v;
+ v.x = p->x - c->x;
+ v.y = p->y - c->y;
+ if ((v.x * v.x + v.y * v.y) < (c->r * c->r))
+ return 1;
+ return 0;
+}
+
+/*
+ * return values:
+ * 0 dont cross
+ * 1 one intersection point
+ * 2 two intersection points
+ *
+ * p1, p2 arguments are the crossing points coordinates. Both p1 and
+ * p2 are dummy for 0 result. When result is 1, p1 and p2 are set to
+ * the same value.
+ */
+uint8_t circle_intersect(const circle_t *c1, const circle_t *c2,
+ point_t *p1, point_t *p2)
+{
+ circle_t ca, cb;
+ float a, b, c, d, e;
+ uint8_t ret = 0;
+
+ /* create circles with same radius, but centered on 0,0 : it
+ * will make process easier */
+ ca.x = 0;
+ ca.y = 0;
+ ca.r = c1->r;
+ cb.x = c2->x - c1->x;
+ cb.y = c2->y - c1->y;
+ cb.r = c2->r;
+
+ /* inspired from
+ http://www.loria.fr/~roegel/notes/note0001.pdf */
+ a = 2. * cb.x;
+ b = 2. * cb.y;
+ c = sq(cb.x) + sq(cb.y) - sq(cb.r) + sq(ca.r);
+ d = sq(2. * a * c) -
+ (4. * (sq(a) + sq(b)) * (sq(c) - sq(b) * sq(ca.r)) );
+
+ /* no intersection */
+ if (d < 0)
+ return 0;
+
+ if (b == 0) {
+ /* special case */
+ e = sq(cb.r) - sq((2. * c - sq(a)) / (2. * a));
+
+ /* no intersection */
+ if (e < 0)
+ return 0;
+
+ p1->x = (2. * a * c - sqrt(d)) / (2. * (sq(a) + sq(b)));
+ p1->y = sqrt(e);
+ p2->x = p1->x;
+ p2->y = p1->y;
+ ret = 1;
+ }
+ else {
+ /* usual case */
+ p1->x = (2. * a * c - sqrt(d)) / (2. * (sq(a) + sq(b)));
+ p1->y = (c - a * p1->x) / b;
+ p2->x = (2. * a * c + sqrt(d)) / (2. * (sq(a) + sq(b)));
+ p2->y = (c - a * p2->x) / b;
+ ret = 2;
+ }
+
+ /* retranslate */
+ p1->x += c1->x;
+ p1->y += c1->y;
+ p2->x += c1->x;
+ p2->y += c1->y;
+
+ return ret;
+}
--- /dev/null
+/*
+ * Copyright Droids Corporation (2009)
+ *
+ * This program 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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: f16.h,v 1.6.4.3 2008-05-10 15:06:26 zer0 Exp $
+ *
+ */
+
+#ifndef _CIRCLES_H_
+#define _CIRCLES_H_
+
+typedef struct _circle {
+ double x;
+ double y;
+ double r;
+} circle_t;
+
+/* return true if a point is in the disc */
+uint8_t pt_is_inside_circle(const point_t *p, circle_t *c);
+
+/*
+ * return values:
+ * 0 dont cross
+ * 1 one intersection point
+ * 2 two intersection points
+ *
+ * p1, p2 arguments are the crossing points coordinates. Both p1 and
+ * p2 are dummy for 0 result. When result is 1, p1 and p2 are set to
+ * the same value.
+ */
+uint8_t circle_intersect(const circle_t *c1, const circle_t *c2,
+ point_t *p1, point_t *p2);
+
+#endif /* _CIRCLES_H_ */
+/*
+ * Copyright Droids Corporation (2009)
+ *
+ * This program 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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: f16.h,v 1.6.4.3 2008-05-10 15:06:26 zer0 Exp $
+ *
+ */
+
#include <aversive.h>
#include <stdint.h>
+/*
+ * Copyright Droids Corporation (2009)
+ *
+ * This program 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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: f16.h,v 1.6.4.3 2008-05-10 15:06:26 zer0 Exp $
+ *
+ */
+
+#ifndef _LINES_H_
+#define _LINES_H_
+
typedef struct _line {
double a;
double b;
void
proj_pt_line(const point_t * p, const line_t * l, point_t * p_out);
-uint8_t
+/*
+ * return values:
+ * 0 dont cross
+ * 1 cross
+ * 2 "parallel crossing"
+ *
+ * p argument is the crossing point coordinates (dummy for 0 or 2
+ * result)
+ */
+uint8_t
intersect_line(const line_t *l1, const line_t *l2, point_t *p);
uint8_t
intersect_segment(const point_t *s1, const point_t *s2,
const point_t *t1, const point_t *t2,
point_t *p);
+
+#endif /* _LINES_H_ */
+/*
+ * Copyright Droids Corporation (2009)
+ *
+ * This program 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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: f16.h,v 1.6.4.3 2008-05-10 15:06:26 zer0 Exp $
+ *
+ */
+
#include <stdint.h>
#include <inttypes.h>
#include <stdlib.h>
+/*
+ * Copyright Droids Corporation (2009)
+ *
+ * This program 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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: f16.h,v 1.6.4.3 2008-05-10 15:06:26 zer0 Exp $
+ *
+ */
+
#ifndef _POLYGON_H_
#define _POLYGON_H_
+/*
+ * Copyright Droids Corporation (2009)
+ *
+ * This program 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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: f16.h,v 1.6.4.3 2008-05-10 15:06:26 zer0 Exp $
+ *
+ */
+
#include <stdint.h>
#include <math.h>
#include <vect_base.h>
/* Return scalar product */
-int32_t
+float
vect_pscal(vect_t *v, vect_t *w)
{
return v->x * w->x + v->y * w->y;
}
/* Return Z of vectorial product */
-int32_t
+float
vect_pvect(vect_t *v, vect_t *w)
{
return v->x*w->y - v->y*w->x;
int8_t
vect_pscal_sign(vect_t *v, vect_t *w)
{
- int32_t z;
+ float z;
z = vect_pscal(v, w);
if (z==0)
return 0;
int8_t
vect_pvect_sign(vect_t *v, vect_t *w)
{
- int32_t z;
+ float z;
z = vect_pvect(v, w);
if (z==0)
return 0;
return z>0?1:-1;
}
+/* float norm(float x1, float y1, float x2, float y2) */
+/* { */
+/* float x = x2 - x1; */
+/* float y = y2 - y1; */
+/* return sqrt(x*x + y*y); */
+/* } */
+
+float pt_norm(point_t *p1, point_t *p2)
+{
+ float x = p2->x - p1->x;
+ float y = p2->y - p1->y;
+ return sqrt(x*x + y*y);
+}
+
/* norm of a vector */
float
vect_norm(vect_t *v)
return sqrt(v->x*v->x+v->y*v->y);
}
-
-
void vect_rot_trigo(vect_t *v)
{
- int32_t s;
+ float s;
s = v->x;
v->x= -v->y;
void vect_rot_retro(vect_t *v)
{
- int32_t s;
+ float s;
s = v->x;
v->x= v->y;
float vect_get_angle(vect_t *v, vect_t *w)
{
- int32_t ps;
+ float ps;
float n;
ps = vect_pscal(v, w);
return acos((float)ps/n);
}
+
+void vect_resize(vect_t *v, float l)
+{
+ float old_l = vect_norm(v);
+ float x = v->x, y = v->y;
+ v->x = x * l / old_l;
+ v->y = y * l / old_l;
+}
+/*
+ * Copyright Droids Corporation (2009)
+ *
+ * This program 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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: f16.h,v 1.6.4.3 2008-05-10 15:06:26 zer0 Exp $
+ *
+ */
+
+#ifndef _VECT_BASE_H_
+#define _VECT_BASE_H_
+
typedef struct _vect_t {
- int32_t x;
- int32_t y;
-}vect_t;
+ float x;
+ float y;
+} vect_t;
typedef struct _point_t {
- int32_t x;
- int32_t y;
-}point_t;
-
+ float x;
+ float y;
+} point_t;
-
/* Return scalar product */
-int32_t
+float
vect_pscal(vect_t *v, vect_t *w);
/* Return Z of vectorial product */
-int32_t
+float
vect_pvect(vect_t *v, vect_t *w);
/* Return scalar product */
vect_pvect_sign(vect_t *v, vect_t *w);
/* norm of a vector */
+//float norm(float x1, float y1, float x2, float y2);
+float pt_norm(point_t *p1, point_t *p2);
float vect_norm(vect_t *v);
void vect_rot_trigo(vect_t *v);
void vect_rot_retro(vect_t *v);
float vect_get_angle(vect_t *v, vect_t *w);
-
+
+void vect_resize(vect_t *v, float l);
+
+#endif /* _VECT_BASE_H_ */
TARGET = spi
# List C source files here. (C dependencies are automatically generated.)
+ifeq ($(HOST),avr)
SRC = spi.c
+else
+SRC = spi_host.c
+endif
###########################################
* For more information on SPI formats, please see your CPU datasheet.
*/
typedef enum {
+#ifdef HOST_VERSION
+ SPI_FORMAT_0,
+ SPI_FORMAT_1,
+ SPI_FORMAT_2,
+ SPI_FORMAT_3,
+#else
SPI_FORMAT_0 = 0x00, /* Sample rising Setup falling */
SPI_FORMAT_1 = _BV(CPHA), /* Setup rising Sample falling */
SPI_FORMAT_2 = _BV(CPOL), /* Sample falling Setup rising */
SPI_FORMAT_3 = _BV(CPHA) | _BV(CPOL), /* Setup falling Sample rising*/
+#endif
} spi_format_t;
* interface. Basically, frames are formatted with 4 words of 16 bits,
* describing the values of the 4 encoders. */
+#ifndef HOST_VERSION
+
#include <string.h>
#include <aversive.h>
g_encoders_spi_values[(int)encoder] = val;
IRQ_UNLOCK(flags);
}
+
+#endif
TARGET = trajectory_manager
# List C source files here. (C dependencies are automatically generated.)
-SRC = trajectory_manager.c
+SRC = trajectory_manager.c trajectory_manager_utils.c
+SRC += trajectory_manager_core.c
##########################################
-/*
+/*
* Copyright Droids Corporation, Microb Technology, Eirbot (2005)
- *
+ *
* This program 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
*
*/
-/* Trajectory Manager v2 - zer0 - for Eurobot 2008 */
+/* Trajectory Manager v3 - zer0 - for Eurobot 2010 */
#include <string.h>
#include <stdlib.h>
#include <quadramp.h>
#include <trajectory_manager.h>
-
-#define M_2PI (2*M_PI)
-
-#define DEG(x) ((x) * (180.0 / M_PI))
-#define RAD(x) ((x) * (M_PI / 180.0))
-
-static void trajectory_manager_event(void *param);
+#include "trajectory_manager_utils.h"
/************ INIT FUNCS */
}
/** structure initialization */
-void trajectory_set_cs(struct trajectory *traj, struct cs *cs_d,
+void trajectory_set_cs(struct trajectory *traj, struct cs *cs_d,
struct cs *cs_a)
{
uint8_t flags;
-
+
IRQ_LOCK(flags);
traj->csm_distance = cs_d;
traj->csm_angle = cs_a;
}
/** structure initialization */
-void trajectory_set_robot_params(struct trajectory *traj,
- struct robot_system *rs,
- struct robot_position *pos)
+void trajectory_set_robot_params(struct trajectory *traj,
+ struct robot_system *rs,
+ struct robot_position *pos)
{
uint8_t flags;
IRQ_LOCK(flags);
}
/** set speed consign */
-void trajectory_set_speed( struct trajectory *traj, int16_t d_speed, int16_t a_speed)
+void trajectory_set_speed(struct trajectory *traj, int16_t d_speed, int16_t a_speed)
{
uint8_t flags;
IRQ_LOCK(flags);
IRQ_UNLOCK(flags);
}
-/************ STATIC [ AND USEFUL ] FUNCS */
-
-/** set speed consign in quadramp filter */
-static void set_quadramp_speed(struct trajectory *traj, int16_t d_speed, int16_t a_speed)
-{
- struct quadramp_filter * q_d, * q_a;
- q_d = traj->csm_distance->consign_filter_params;
- q_a = traj->csm_angle->consign_filter_params;
- quadramp_set_1st_order_vars(q_d, ABS(d_speed), ABS(d_speed));
- quadramp_set_1st_order_vars(q_a, ABS(a_speed), ABS(a_speed));
-}
-
-/** get angle speed consign in quadramp filter */
-static uint32_t get_quadramp_angle_speed(struct trajectory *traj)
-{
- struct quadramp_filter *q_a;
- q_a = traj->csm_angle->consign_filter_params;
- return q_a->var_1st_ord_pos;
-}
-
-/** get distance speed consign in quadramp filter */
-static uint32_t get_quadramp_distance_speed(struct trajectory *traj)
-{
- struct quadramp_filter *q_d;
- q_d = traj->csm_distance->consign_filter_params;
- return q_d->var_1st_ord_pos;
-}
-
-/** remove event if any */
-static void delete_event(struct trajectory *traj)
-{
- set_quadramp_speed(traj, traj->d_speed, traj->a_speed);
- if ( traj->scheduler_task != -1) {
- DEBUG(E_TRAJECTORY, "Delete event");
- scheduler_del_event(traj->scheduler_task);
- traj->scheduler_task = -1;
- }
-}
-
-/** schedule the trajectory event */
-static void schedule_event(struct trajectory *traj)
-{
- if ( traj->scheduler_task != -1) {
- DEBUG(E_TRAJECTORY, "Schedule event, already scheduled");
- }
- else {
- traj->scheduler_task =
- scheduler_add_periodical_event_priority(&trajectory_manager_event,
- (void*)traj,
- 100000L/SCHEDULER_UNIT, 30);
- }
-}
-
-/** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */
-static double simple_modulo_2pi(double a)
-{
- if (a < -M_PI) {
- a += M_2PI;
- }
- else if (a > M_PI) {
- a -= M_2PI;
- }
- return a;
-}
-
-/** do a modulo 2.pi -> [-Pi,+Pi] */
-static double modulo_2pi(double a)
-{
- double res = a - (((int32_t) (a/M_2PI)) * M_2PI);
- return simple_modulo_2pi(res);
-}
-
-#include <aversive/wait.h>
-/** near the target (dist) ? */
-static uint8_t is_robot_in_dist_window(struct trajectory *traj, double d_win)
-{
- 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);
-}
-
-/** near the target (dist in x,y) ? */
-static uint8_t is_robot_in_xy_window(struct trajectory *traj, double d_win)
-{
- double x1 = traj->target.cart.x;
- double y1 = traj->target.cart.y;
- double x2 = position_get_x_double(traj->position);
- double y2 = position_get_y_double(traj->position);
- return ( sqrt ((x2-x1) * (x2-x1) + (y2-y1) * (y2-y1)) < d_win );
-}
-
-/** 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)
-{
- 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;
-}
-
-
-/************ SIMPLE TRAJS, NO EVENT */
-
-#define UPDATE_D 1
-#define UPDATE_A 2
-#define RESET_D 4
-#define RESET_A 8
-
-/**
- * update angle and/or distance
- * this function is not called directly by the user
- * traj : pointer to the trajectory structure
- * d_mm : distance in mm
- * 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 state, uint8_t flags)
-{
- int32_t a_consign, d_consign;
-
- DEBUG(E_TRAJECTORY, "Goto DA/RS rel to d=%f a_rad=%f", d_mm, a_rad);
- delete_event(traj);
- traj->state = state;
- if (flags & UPDATE_A) {
- if (flags & RESET_A) {
- a_consign = 0;
- }
- else {
- a_consign = (int32_t)(a_rad * (traj->position->phys.distance_imp_per_mm) *
- (traj->position->phys.track_mm) / 2);
- }
- 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) {
- d_consign = 0;
- }
- else {
- d_consign = (int32_t)((d_mm) * (traj->position->phys.distance_imp_per_mm));
- }
- 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)
-{
- __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)
-{
- __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)
-{
- __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg_rel), RUNNING_A,
- UPDATE_A | UPDATE_D | RESET_D);
-}
-
-/** turn by 'a' degrees */
-void trajectory_a_abs(struct trajectory *traj, double a_deg_abs)
-{
- double posa = position_get_a_rad_double(traj->position);
- double a;
-
- a = RAD(a_deg_abs) - posa;
- a = modulo_2pi(a);
- __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 */
-void trajectory_turnto_xy(struct trajectory *traj, double x_abs_mm, double y_abs_mm)
-{
- 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 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);
-}
-
-/** turn the robot until the point x,y is behind us */
-void trajectory_turnto_xy_behind(struct trajectory *traj, double x_abs_mm, double y_abs_mm)
-{
- 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 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)
-{
- __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg), RUNNING_A,
- UPDATE_A);
-}
-
-/** update angle consign without changing distance consign */
-void trajectory_only_a_abs(struct trajectory *traj, double a_deg_abs)
-{
- double posa = position_get_a_rad_double(traj->position);
- double a;
-
- a = RAD(a_deg_abs) - posa;
- a = modulo_2pi(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)
-{
- __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)
-{
- __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
- * deceleration ramp in quadramp filter */
-void trajectory_hardstop(struct trajectory *traj)
-{
- struct quadramp_filter *q_d, *q_a;
-
- q_d = traj->csm_distance->consign_filter_params;
- q_a = traj->csm_angle->consign_filter_params;
- __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);
- q_a->previous_var = 0;
- q_a->previous_out = rs_get_angle(traj->robot);
-}
-
-
-/************ GOTO XY, USE EVENTS */
-
-/** goto a x,y point, using a trajectory event */
-void trajectory_goto_xy_abs(struct trajectory *traj, double x, double y)
-{
- DEBUG(E_TRAJECTORY, "Goto XY");
- delete_event(traj);
- traj->target.cart.x = x;
- traj->target.cart.y = y;
- traj->state = RUNNING_XY_START;
- trajectory_manager_event(traj);
- schedule_event(traj);
-}
-
-/** go forward to a x,y point, using a trajectory event */
-void trajectory_goto_forward_xy_abs(struct trajectory *traj, double x, double y)
-{
- DEBUG(E_TRAJECTORY, "Goto XY_F");
- delete_event(traj);
- traj->target.cart.x = x;
- traj->target.cart.y = y;
- traj->state = RUNNING_XY_F_START;
- trajectory_manager_event(traj);
- schedule_event(traj);
-}
-
-/** go backward to a x,y point, using a trajectory event */
-void trajectory_goto_backward_xy_abs(struct trajectory *traj, double x, double y)
+/** set corrective coef for circle */
+void trajectory_set_circle_coef(struct trajectory *traj, double coef)
{
- DEBUG(E_TRAJECTORY, "Goto XY_B");
- delete_event(traj);
- traj->target.cart.x = x;
- traj->target.cart.y = y;
- traj->state = RUNNING_XY_B_START;
- trajectory_manager_event(traj);
- schedule_event(traj);
-}
-
-/** go forward to a d,a point, using a trajectory event */
-void trajectory_goto_d_a_rel(struct trajectory *traj, double d, double a)
-{
- vect2_pol p;
- double x = position_get_x_double(traj->position);
- double y = position_get_y_double(traj->position);
-
- DEBUG(E_TRAJECTORY, "Goto DA rel");
-
- delete_event(traj);
- p.r = d;
- p.theta = RAD(a) + position_get_a_rad_double(traj->position);
- vect2_pol2cart(&p, &traj->target.cart);
- traj->target.cart.x += x;
- traj->target.cart.y += y;
-
- traj->state = RUNNING_XY_START;
- trajectory_manager_event(traj);
- schedule_event(traj);
-}
-
-/** go forward to a x,y relative point, using a trajectory event */
-void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_mm, double y_rel_mm)
-{
- vect2_cart c;
- vect2_pol p;
- double x = position_get_x_double(traj->position);
- double y = position_get_y_double(traj->position);
-
- DEBUG(E_TRAJECTORY, "Goto XY rel");
-
- delete_event(traj);
- c.x = x_rel_mm;
- c.y = y_rel_mm;
-
- vect2_cart2pol(&c, &p);
- p.theta += position_get_a_rad_double(traj->position);;
- vect2_pol2cart(&p, &traj->target.cart);
-
- traj->target.cart.x += x;
- traj->target.cart.y += y;
-
- traj->state = RUNNING_XY_START;
- trajectory_manager_event(traj);
- schedule_event(traj);
-}
-
-/************ FUNCS FOR GETTING TRAJ STATE */
-
-/** return true if the position consign is equal to the filtered
- * position consign (after quadramp filter), for angle and
- * distance. */
-uint8_t trajectory_finished(struct trajectory *traj)
-{
- return cs_get_consign(traj->csm_angle) == cs_get_filtered_consign(traj->csm_angle) &&
- cs_get_consign(traj->csm_distance) == cs_get_filtered_consign(traj->csm_distance) ;
-}
-
-/** return true if traj is nearly finished */
-uint8_t trajectory_in_window(struct trajectory *traj, double d_win, double a_win_rad)
-{
- switch(traj->state) {
-
- case RUNNING_XY_ANGLE_OK:
- case RUNNING_XY_F_ANGLE_OK:
- case RUNNING_XY_B_ANGLE_OK:
- /* if robot coordinates are near the x,y target */
- return is_robot_in_xy_window(traj, d_win);
-
- case RUNNING_A:
- return is_robot_in_angle_window(traj, a_win_rad);
-
- case RUNNING_D:
- return is_robot_in_dist_window(traj, d_win);
-
- case RUNNING_AD:
- return is_robot_in_dist_window(traj, d_win) &&
- is_robot_in_angle_window(traj, a_win_rad);
-
- case RUNNING_XY_START:
- case RUNNING_XY_F_START:
- case RUNNING_XY_B_START:
- case RUNNING_XY_ANGLE:
- case RUNNING_XY_F_ANGLE:
- case RUNNING_XY_B_ANGLE:
- default:
- return 0;
- }
-}
-
-/*********** *TRAJECTORY EVENT FUNC */
-
-/** event called for xy trajectories */
-static void trajectory_manager_event(void * param)
-{
- struct trajectory *traj = (struct trajectory *)param;
- double coef=1.0;
- double x = position_get_x_double(traj->position);
- double y = position_get_y_double(traj->position);
- double a = position_get_a_rad_double(traj->position);
- int32_t d_consign=0, a_consign=0;
-
- /* These vectors contain target position of the robot in
- * its own coordinates */
- vect2_cart v2cart_pos;
- vect2_pol v2pol_target;
-
- /* step 1 : process new commands to quadramps */
-
- switch (traj->state) {
- case RUNNING_XY_START:
- case RUNNING_XY_ANGLE:
- case RUNNING_XY_ANGLE_OK:
- case RUNNING_XY_F_START:
- case RUNNING_XY_F_ANGLE:
- case RUNNING_XY_F_ANGLE_OK:
- case RUNNING_XY_B_START:
- case RUNNING_XY_B_ANGLE:
- case RUNNING_XY_B_ANGLE_OK:
-
- /* process the command vector from current position to
- * absolute target, or to the center of the circle. */
- v2cart_pos.x = traj->target.cart.x - x;
- v2cart_pos.y = traj->target.cart.y - y;
- vect2_cart2pol(&v2cart_pos, &v2pol_target);
- v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a);
-
- /* asked to go backwards */
- if (traj->state >= RUNNING_XY_B_START &&
- traj->state <= RUNNING_XY_B_ANGLE_OK ) {
- v2pol_target.r = -v2pol_target.r;
- v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta + M_PI);
- }
-
- /* if we don't need to go forward */
- if (traj->state >= RUNNING_XY_START &&
- traj->state <= RUNNING_XY_ANGLE_OK ) {
- /* If the target is behind the robot, we need to go
- * backwards. 0.52 instead of 0.5 because we prefer to
- * go forward */
- if ((v2pol_target.theta > 0.52*M_PI) ||
- (v2pol_target.theta < -0.52*M_PI ) ) {
- v2pol_target.r = -v2pol_target.r;
- v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta + M_PI);
- }
- }
-
- /* XXX circle */
-
- /* If the robot is correctly oriented to start moving in distance */
- /* here limit dist speed depending on v2pol_target.theta */
- if (ABS(v2pol_target.theta) > traj->a_start_rad) // || ABS(v2pol_target.r) < traj->d_win)
- set_quadramp_speed(traj, 0, traj->a_speed);
- else {
- coef = (traj->a_start_rad - ABS(v2pol_target.theta)) / traj->a_start_rad;
- set_quadramp_speed(traj, traj->d_speed * coef, traj->a_speed);
- }
-
- d_consign = (int32_t)(v2pol_target.r * (traj->position->phys.distance_imp_per_mm));
- d_consign += rs_get_distance(traj->robot);
-
- /* angle consign */
- /* XXX here we specify 2.2 instead of 2.0 to avoid oscillations */
- a_consign = (int32_t)(v2pol_target.theta *
- (traj->position->phys.distance_imp_per_mm) *
- (traj->position->phys.track_mm) / 2.2);
- a_consign += rs_get_angle(traj->robot);
-
- break;
-
- default:
- /* hmmm quite odd, delete the event */
- DEBUG(E_TRAJECTORY, "GNI ???");
- delete_event(traj);
- traj->state = READY;
- }
-
-
- /* step 2 : update state, or delete event if we reached the
- * destination */
-
- /* XXX if target is our pos !! */
-
- switch (traj->state) {
- case RUNNING_XY_START:
- case RUNNING_XY_F_START:
- case RUNNING_XY_B_START:
- /* START -> ANGLE */
- DEBUG(E_TRAJECTORY, "-> ANGLE");
- traj->state ++;
- break;
-
- case RUNNING_XY_ANGLE:
- case RUNNING_XY_F_ANGLE:
- case RUNNING_XY_B_ANGLE: {
- struct quadramp_filter *q_a;
- q_a = traj->csm_angle->consign_filter_params;
- /* if d_speed is not 0, we are in start_angle_win */
- if (get_quadramp_distance_speed(traj)) {
- if(is_robot_in_xy_window(traj, traj->d_win)) {
- delete_event(traj);
- }
- /* ANGLE -> ANGLE_OK */
- traj->state ++;
- DEBUG(E_TRAJECTORY, "-> ANGLE_OK");
- }
- break;
- }
-
- case RUNNING_XY_ANGLE_OK:
- case RUNNING_XY_F_ANGLE_OK:
- case RUNNING_XY_B_ANGLE_OK:
- /* If we reached the destination */
- if(is_robot_in_xy_window(traj, traj->d_win)) {
- delete_event(traj);
- }
- break;
-
- default:
- break;
- }
-
- /* step 3 : send the processed commands to cs */
-
- DEBUG(E_TRAJECTORY, "EVENT XY cur=(%f,%f,%f) cart=(%f,%f) pol=(%f,%f)",
- x, y, a, v2cart_pos.x, v2cart_pos.y, v2pol_target.r, v2pol_target.theta);
-
- DEBUG(E_TRAJECTORY,"d_cur=%" PRIi32 ", d_consign=%" PRIi32 ", d_speed=%" PRIi32 ", "
- "a_cur=%" PRIi32 ", a_consign=%" PRIi32 ", a_speed=%" PRIi32,
- rs_get_distance(traj->robot), d_consign, get_quadramp_distance_speed(traj),
- rs_get_angle(traj->robot), a_consign, get_quadramp_angle_speed(traj));
-
- cs_set_consign(traj->csm_angle, a_consign);
- cs_set_consign(traj->csm_distance, d_consign);
-}
-
-/*********** *CIRCLE */
-
-/*
- * Compute the fastest distance and angle speeds matching the radius
- * from current traj_speed
- */
-/* static */void circle_get_da_speed_from_radius(struct trajectory *traj,
- double radius_mm,
- double *speed_d,
- double *speed_a)
-{
- /* speed_d = coef * speed_a */
- double coef;
- double speed_d2, speed_a2;
-
- coef = 2. * radius_mm / traj->position->phys.track_mm;
-
- speed_d2 = traj->a_speed * coef;
- if (speed_d2 < traj->d_speed) {
- *speed_d = speed_d2;
- *speed_a = traj->a_speed;
- }
- else {
- speed_a2 = traj->d_speed / coef;
- *speed_d = traj->d_speed;
- *speed_a = speed_a2;
- }
-}
-
-/* return the distance in millimeters that corresponds to an angle in
- * degree and a radius in mm */
-/* static */double circle_get_dist_from_degrees(double radius_mm, double a_deg)
-{
- double a_rad = RAD(a_deg);
- return a_rad * radius_mm;
-}
-
-/*
- * Start a circle of specified radius around the specified center
- * (relative with d,a). The distance is specified in mm.
- */
-void trajectory_circle(struct trajectory *traj,
- double center_d_mm, double center_a_rad,
- double radius_mm, double dist_mm)
-{
-/* double */
-
-/* DEBUG(E_TRAJECTORY, "CIRCLE to d=%f a_rad=%f", center_d_mm, */
-/* center_a_rad); */
-/* delete_event(traj); */
-/* traj->state = RUNNING_CIRCLE; */
-
-
-}
-
-/*
- * Start a circle of specified radius around the specified center
- * (absolute). The distance is specified in mm.
- */
-void trajectory_circle_abs_dist_mm(struct trajectory *traj,
- double x_rel_mm, double y_rel_mm,
- double radius_mm, double dist_mm)
-{
-}
-
-/*
- * Start a circle of specified radius around the specified center
- * (absolute). The distance is specified in degrees.
- */
-void trajectory_circle_abs_dist_deg(struct trajectory *traj,
- double x_rel_mm, double y_rel_mm,
- double radius_mm, double dist_degrees)
-{
-
+ uint8_t flags;
+ IRQ_LOCK(flags);
+ traj->circle_coef = coef ;
+ IRQ_UNLOCK(flags);
}
RUNNING_CIRCLE,
};
+struct circle_target {
+ vect2_cart center; /**< center of the circle */
+ double radius; /**< radius of the circle */
+ int32_t dest_angle; /**< dst angle in inc */
+
+#define TRIGO 1 /* rotation is counterclockwise */
+#define FORWARD 2 /* go forward or backward */
+ uint8_t flags; /**< flags for this trajectory */
+};
struct trajectory {
enum trajectory_state state; /*<< describe the type of target, and if we reached the target */
union {
vect2_cart cart; /**<< target, if it is a x,y vector */
struct rs_polar pol; /**<< target, if it is a d,a vector */
+ struct circle_target circle; /**<< target, if it is a circle */
} target;
double d_win; /**<< distance window (for END_NEAR) */
double a_win_rad; /**<< angle window (for END_NEAR) */
double a_start_rad;/**<< in xy consigns, start to move in distance
* when a_target < a_start */
-
+ double circle_coef;/**<< corrective circle coef */
+
uint16_t d_speed; /**<< distance speed consign */
uint16_t a_speed; /**<< angle speed consign */
struct robot_system *robot; /**<< associated robot_system */
struct cs *csm_angle; /**<< associated control system (angle) */
struct cs *csm_distance; /**<< associated control system (distance) */
-
+
int8_t scheduler_task; /**<< id of current task (-1 if no running task) */
};
void trajectory_set_windows(struct trajectory *traj, double d_win,
double a_win_deg, double a_start_deg);
+/**
+ * Set coef for circle trajectory. The objective of this value is to
+ * fix the radius of the circle which is not correctly what we asked.
+ */
+void trajectory_set_circle_coef(struct trajectory *traj, double coef);
+
/** return true if the position consign is equal to the filtered
* position consign (after quadramp filter), for angle and
* distance. */
/** go forward to a x,y relative point, using a trajectory event */
void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_mm, double y_rel_mm);
+/** make the robot orbiting around (x,y) on a circle whose radius is
+ * radius_mm, and exit when relative destination angle is reached. The
+ * flags set if we go forward or backwards, and CW/CCW. */
+void trajectory_circle_rel(struct trajectory *traj, double x, double y,
+ double radius_mm, double rel_a_deg, uint8_t flags);
+
#endif //TRAJECTORY_MANAGER
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ *
+ * This program 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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: trajectory_manager.c,v 1.4.4.17 2009-05-18 12:28:36 zer0 Exp $
+ *
+ */
+
+/* Trajectory Manager v3 - zer0 - for Eurobot 2010 */
+
+#include <string.h>
+#include <stdlib.h>
+#include <math.h>
+
+#include <aversive.h>
+#include <aversive/error.h>
+#include <scheduler.h>
+#include <vect2.h>
+
+#include <position_manager.h>
+#include <robot_system.h>
+#include <control_system_manager.h>
+#include <quadramp.h>
+
+#include <trajectory_manager.h>
+#include "trajectory_manager_utils.h"
+#include "trajectory_manager_core.h"
+
+/************ SIMPLE TRAJS, NO EVENT */
+
+#define UPDATE_D 1
+#define UPDATE_A 2
+#define RESET_D 4
+#define RESET_A 8
+
+static uint8_t evt_debug_cpt = 0;
+#define EVT_DEBUG(args...) do { \
+ if (((evt_debug_cpt ++) & 0x07) == 0) { \
+ DEBUG(args); \
+ } \
+ } while (0)
+
+
+/**
+ * update angle and/or distance
+ * this function is not called directly by the user
+ * traj : pointer to the trajectory structure
+ * d_mm : distance in mm
+ * 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 state, uint8_t flags)
+{
+ int32_t a_consign, d_consign;
+
+ DEBUG(E_TRAJECTORY, "Goto DA/RS rel to d=%f a_rad=%f", d_mm, a_rad);
+ delete_event(traj);
+ traj->state = state;
+ if (flags & UPDATE_A) {
+ if (flags & RESET_A) {
+ a_consign = 0;
+ }
+ else {
+ a_consign = (int32_t)(a_rad * (traj->position->phys.distance_imp_per_mm) *
+ (traj->position->phys.track_mm) / 2);
+ }
+ 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) {
+ d_consign = 0;
+ }
+ else {
+ d_consign = (int32_t)((d_mm) * (traj->position->phys.distance_imp_per_mm));
+ }
+ 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)
+{
+ __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)
+{
+ __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)
+{
+ __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg_rel), RUNNING_A,
+ UPDATE_A | UPDATE_D | RESET_D);
+}
+
+/** turn by 'a' degrees */
+void trajectory_a_abs(struct trajectory *traj, double a_deg_abs)
+{
+ double posa = position_get_a_rad_double(traj->position);
+ double a;
+
+ a = RAD(a_deg_abs) - posa;
+ a = modulo_2pi(a);
+ __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 */
+void trajectory_turnto_xy(struct trajectory *traj, double x_abs_mm, double y_abs_mm)
+{
+ 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 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);
+}
+
+/** turn the robot until the point x,y is behind us */
+void trajectory_turnto_xy_behind(struct trajectory *traj, double x_abs_mm, double y_abs_mm)
+{
+ 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 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)
+{
+ __trajectory_goto_d_a_rel(traj, 0, RAD(a_deg), RUNNING_A,
+ UPDATE_A);
+}
+
+/** update angle consign without changing distance consign */
+void trajectory_only_a_abs(struct trajectory *traj, double a_deg_abs)
+{
+ double posa = position_get_a_rad_double(traj->position);
+ double a;
+
+ a = RAD(a_deg_abs) - posa;
+ a = modulo_2pi(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)
+{
+ __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)
+{
+ __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
+ * deceleration ramp in quadramp filter */
+void trajectory_hardstop(struct trajectory *traj)
+{
+ struct quadramp_filter *q_d, *q_a;
+
+ q_d = traj->csm_distance->consign_filter_params;
+ q_a = traj->csm_angle->consign_filter_params;
+ __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);
+ q_a->previous_var = 0;
+ q_a->previous_out = rs_get_angle(traj->robot);
+}
+
+
+/************ GOTO XY, USE EVENTS */
+
+/** goto a x,y point, using a trajectory event */
+void trajectory_goto_xy_abs(struct trajectory *traj, double x, double y)
+{
+ DEBUG(E_TRAJECTORY, "Goto XY");
+ delete_event(traj);
+ traj->target.cart.x = x;
+ traj->target.cart.y = y;
+ traj->state = RUNNING_XY_START;
+ trajectory_manager_event(traj);
+ schedule_event(traj);
+}
+
+/** go forward to a x,y point, using a trajectory event */
+void trajectory_goto_forward_xy_abs(struct trajectory *traj, double x, double y)
+{
+ DEBUG(E_TRAJECTORY, "Goto XY_F");
+ delete_event(traj);
+ traj->target.cart.x = x;
+ traj->target.cart.y = y;
+ traj->state = RUNNING_XY_F_START;
+ trajectory_manager_event(traj);
+ schedule_event(traj);
+}
+
+/** go backward to a x,y point, using a trajectory event */
+void trajectory_goto_backward_xy_abs(struct trajectory *traj, double x, double y)
+{
+ DEBUG(E_TRAJECTORY, "Goto XY_B");
+ delete_event(traj);
+ traj->target.cart.x = x;
+ traj->target.cart.y = y;
+ traj->state = RUNNING_XY_B_START;
+ trajectory_manager_event(traj);
+ schedule_event(traj);
+}
+
+/** go forward to a d,a point, using a trajectory event */
+void trajectory_goto_d_a_rel(struct trajectory *traj, double d, double a)
+{
+ vect2_pol p;
+ double x = position_get_x_double(traj->position);
+ double y = position_get_y_double(traj->position);
+
+ DEBUG(E_TRAJECTORY, "Goto DA rel");
+
+ delete_event(traj);
+ p.r = d;
+ p.theta = RAD(a) + position_get_a_rad_double(traj->position);
+ vect2_pol2cart(&p, &traj->target.cart);
+ traj->target.cart.x += x;
+ traj->target.cart.y += y;
+
+ traj->state = RUNNING_XY_START;
+ trajectory_manager_event(traj);
+ schedule_event(traj);
+}
+
+/** go forward to a x,y relative point, using a trajectory event */
+void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_mm, double y_rel_mm)
+{
+ vect2_cart c;
+ vect2_pol p;
+ double x = position_get_x_double(traj->position);
+ double y = position_get_y_double(traj->position);
+
+ DEBUG(E_TRAJECTORY, "Goto XY rel");
+
+ delete_event(traj);
+ c.x = x_rel_mm;
+ c.y = y_rel_mm;
+
+ vect2_cart2pol(&c, &p);
+ p.theta += position_get_a_rad_double(traj->position);;
+ vect2_pol2cart(&p, &traj->target.cart);
+
+ traj->target.cart.x += x;
+ traj->target.cart.y += y;
+
+ traj->state = RUNNING_XY_START;
+ trajectory_manager_event(traj);
+ schedule_event(traj);
+}
+
+/************ FUNCS FOR GETTING TRAJ STATE */
+
+/** return true if the position consign is equal to the filtered
+ * position consign (after quadramp filter), for angle and
+ * distance. */
+uint8_t trajectory_finished(struct trajectory *traj)
+{
+ return cs_get_consign(traj->csm_angle) == cs_get_filtered_consign(traj->csm_angle) &&
+ cs_get_consign(traj->csm_distance) == cs_get_filtered_consign(traj->csm_distance) ;
+}
+
+/** return true if traj is nearly finished */
+uint8_t trajectory_in_window(struct trajectory *traj, double d_win, double a_win_rad)
+{
+ switch(traj->state) {
+
+ case RUNNING_XY_ANGLE_OK:
+ case RUNNING_XY_F_ANGLE_OK:
+ case RUNNING_XY_B_ANGLE_OK:
+ /* if robot coordinates are near the x,y target */
+ return is_robot_in_xy_window(traj, d_win);
+
+ case RUNNING_A:
+ return is_robot_in_angle_window(traj, a_win_rad);
+
+ case RUNNING_D:
+ return is_robot_in_dist_window(traj, d_win);
+
+ case RUNNING_AD:
+ return is_robot_in_dist_window(traj, d_win) &&
+ is_robot_in_angle_window(traj, a_win_rad);
+
+ case RUNNING_XY_START:
+ case RUNNING_XY_F_START:
+ case RUNNING_XY_B_START:
+ case RUNNING_XY_ANGLE:
+ case RUNNING_XY_F_ANGLE:
+ case RUNNING_XY_B_ANGLE:
+ default:
+ return 0;
+ }
+}
+
+/*********** *TRAJECTORY EVENT FUNC */
+
+/** event called for xy trajectories */
+void trajectory_manager_xy_event(struct trajectory *traj)
+{
+ double coef = 1.0;
+ double x = position_get_x_double(traj->position);
+ double y = position_get_y_double(traj->position);
+ double a = position_get_a_rad_double(traj->position);
+ int32_t d_consign=0, a_consign=0;
+
+ /* These vectors contain target position of the robot in
+ * its own coordinates */
+ vect2_cart v2cart_pos;
+ vect2_pol v2pol_target;
+
+ /* step 1 : process new commands to quadramps */
+
+ switch (traj->state) {
+ case RUNNING_XY_START:
+ case RUNNING_XY_ANGLE:
+ case RUNNING_XY_ANGLE_OK:
+ case RUNNING_XY_F_START:
+ case RUNNING_XY_F_ANGLE:
+ case RUNNING_XY_F_ANGLE_OK:
+ case RUNNING_XY_B_START:
+ case RUNNING_XY_B_ANGLE:
+ case RUNNING_XY_B_ANGLE_OK:
+
+ /* process the command vector from current position to
+ * absolute target. */
+ v2cart_pos.x = traj->target.cart.x - x;
+ v2cart_pos.y = traj->target.cart.y - y;
+ vect2_cart2pol(&v2cart_pos, &v2pol_target);
+ v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a);
+
+ /* asked to go backwards */
+ if (traj->state >= RUNNING_XY_B_START &&
+ traj->state <= RUNNING_XY_B_ANGLE_OK ) {
+ v2pol_target.r = -v2pol_target.r;
+ v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta + M_PI);
+ }
+
+ /* if we don't need to go forward */
+ if (traj->state >= RUNNING_XY_START &&
+ traj->state <= RUNNING_XY_ANGLE_OK ) {
+ /* If the target is behind the robot, we need to go
+ * backwards. 0.52 instead of 0.5 because we prefer to
+ * go forward */
+ if ((v2pol_target.theta > 0.52*M_PI) ||
+ (v2pol_target.theta < -0.52*M_PI ) ) {
+ v2pol_target.r = -v2pol_target.r;
+ v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta + M_PI);
+ }
+ }
+
+ /* XXX circle */
+
+ /* If the robot is correctly oriented to start moving in distance */
+ /* here limit dist speed depending on v2pol_target.theta */
+ if (ABS(v2pol_target.theta) > traj->a_start_rad) // || ABS(v2pol_target.r) < traj->d_win)
+ set_quadramp_speed(traj, 0, traj->a_speed);
+ else {
+ coef = (traj->a_start_rad - ABS(v2pol_target.theta)) / traj->a_start_rad;
+ set_quadramp_speed(traj, traj->d_speed * coef, traj->a_speed);
+ }
+
+ d_consign = (int32_t)(v2pol_target.r * (traj->position->phys.distance_imp_per_mm));
+ d_consign += rs_get_distance(traj->robot);
+
+ /* angle consign */
+ /* XXX here we specify 2.2 instead of 2.0 to avoid oscillations */
+ a_consign = (int32_t)(v2pol_target.theta *
+ (traj->position->phys.distance_imp_per_mm) *
+ (traj->position->phys.track_mm) / 2.2);
+ a_consign += rs_get_angle(traj->robot);
+
+ break;
+
+ default:
+ /* hmmm quite odd, delete the event */
+ DEBUG(E_TRAJECTORY, "GNI ???");
+ delete_event(traj);
+ traj->state = READY;
+ }
+
+
+ /* step 2 : update state, or delete event if we reached the
+ * destination */
+
+ /* XXX if target is our pos !! */
+
+ switch (traj->state) {
+ case RUNNING_XY_START:
+ case RUNNING_XY_F_START:
+ case RUNNING_XY_B_START:
+ /* START -> ANGLE */
+ DEBUG(E_TRAJECTORY, "-> ANGLE");
+ traj->state ++;
+ break;
+
+ case RUNNING_XY_ANGLE:
+ case RUNNING_XY_F_ANGLE:
+ case RUNNING_XY_B_ANGLE: {
+ struct quadramp_filter *q_a;
+ q_a = traj->csm_angle->consign_filter_params;
+ /* if d_speed is not 0, we are in start_angle_win */
+ if (get_quadramp_distance_speed(traj)) {
+ if (is_robot_in_xy_window(traj, traj->d_win)) {
+ delete_event(traj);
+ }
+ /* ANGLE -> ANGLE_OK */
+ traj->state ++;
+ DEBUG(E_TRAJECTORY, "-> ANGLE_OK");
+ }
+ break;
+ }
+
+ case RUNNING_XY_ANGLE_OK:
+ case RUNNING_XY_F_ANGLE_OK:
+ case RUNNING_XY_B_ANGLE_OK:
+ /* If we reached the destination */
+ if (is_robot_in_xy_window(traj, traj->d_win)) {
+ delete_event(traj);
+ }
+ break;
+
+ default:
+ break;
+ }
+
+ /* step 3 : send the processed commands to cs */
+
+ EVT_DEBUG(E_TRAJECTORY,"EVENT XY d_cur=%" PRIi32 ", d_consign=%" PRIi32 ", d_speed=%" PRIi32 ", "
+ "a_cur=%" PRIi32 ", a_consign=%" PRIi32 ", a_speed=%" PRIi32,
+ rs_get_distance(traj->robot), d_consign, get_quadramp_distance_speed(traj),
+ rs_get_angle(traj->robot), a_consign, get_quadramp_angle_speed(traj));
+
+ cs_set_consign(traj->csm_angle, a_consign);
+ cs_set_consign(traj->csm_distance, d_consign);
+}
+
+/* trajectory event for circles */
+/* XXX static */
+void trajectory_manager_circle_event(struct trajectory *traj)
+{
+ double coef = 1.0;
+ double x = position_get_x_double(traj->position);
+ double y = position_get_y_double(traj->position);
+ double a = position_get_a_rad_double(traj->position);
+ int32_t d_consign = 0, a_consign = 0;
+ double angle_to_center_rad;
+ static int32_t d_prev, a_prev;
+ int32_t d_speed, a_speed;
+ int32_t d_pos, a_pos;
+
+ d_pos = rs_get_distance(traj->robot);
+ a_pos = rs_get_angle(traj->robot);
+ d_speed = d_pos - d_prev;
+ a_speed = a_pos - a_prev;
+ d_prev = d_pos;
+ a_prev = a_pos;
+
+ /* These vectors contain target position of the robot in
+ * its own coordinates */
+ vect2_cart v2cart_pos;
+ vect2_pol v2pol_target;
+
+ int32_t delta_d, delta_a;
+ double coef_deriv = traj->circle_coef;
+ double new_radius;
+ struct quadramp_filter * q_d, * q_a;
+
+ q_d = traj->csm_distance->consign_filter_params;
+ q_a = traj->csm_angle->consign_filter_params;
+ delta_a = a_speed;//q_a->previous_var;
+ delta_d = d_speed;//q_d->previous_var;
+
+ /* step 1 : process new commands to quadramps */
+
+ /* process the command vector from current position to the
+ * center of the circle. */
+ v2cart_pos.x = traj->target.circle.center.x - x;
+ v2cart_pos.y = traj->target.circle.center.y - y;
+ vect2_cart2pol(&v2cart_pos, &v2pol_target);
+ v2pol_target.theta = simple_modulo_2pi(v2pol_target.theta - a);
+
+ /* pas trop mal, mais oscille */
+ //new_radius = traj->target.circle.radius - delta_a * delta_d * coef_deriv;
+ if (v2pol_target.r > traj->target.circle.radius/2)
+ new_radius = traj->target.circle.radius - delta_a * delta_d * coef_deriv * traj->target.circle.radius / v2pol_target.r;
+ else
+ new_radius = traj->target.circle.radius - delta_a * delta_d * coef_deriv ;
+
+ /* oscille a mort */
+ //new_radius = traj->target.circle.radius - traj->target.circle.radius * delta_a * delta_a * coef_deriv;
+
+ /* ? */
+ //new_radius = traj->target.circle.radius - traj->target.circle.radius * delta_a * coef_deriv;
+
+
+ /* wanted direction of center of circle:
+ * if we are far, go in the center direction,
+ * if we are at radius, we want to see the center at 90°
+ * if we are nearer than radius, angle to center is > 90° */
+ if (v2pol_target.r > new_radius) {
+ angle_to_center_rad = new_radius / v2pol_target.r;
+ angle_to_center_rad *= (M_PI / 2);
+ }
+ else {
+ angle_to_center_rad = 1. - (v2pol_target.r /
+ (2 * new_radius));
+ angle_to_center_rad *= M_PI;
+ }
+
+ /* XXX check flags */
+ v2pol_target.theta -= angle_to_center_rad;
+
+ /* If the robot is correctly oriented to start moving in distance */
+ /* here limit dist speed depending on v2pol_target.theta */
+ if (ABS(v2pol_target.theta) > traj->a_start_rad)
+ set_quadramp_speed(traj, 0, traj->a_speed);
+ else {
+ coef = (traj->a_start_rad - ABS(v2pol_target.theta)) / traj->a_start_rad;
+ set_quadramp_speed(traj, traj->d_speed * coef, traj->a_speed);
+ }
+
+ /* XXX check flags */
+ d_consign = 40000 + rs_get_distance(traj->robot);
+
+ /* angle consign */
+ a_consign = (int32_t)(v2pol_target.theta *
+ (traj->position->phys.distance_imp_per_mm) *
+ (traj->position->phys.track_mm) / 2.0);
+ a_consign += rs_get_angle(traj->robot);
+
+ /* step 2 : update state, or delete event if we reached the
+ * destination */
+
+ /* output angle -> delete event */
+ if (a_consign >= traj->target.circle.dest_angle) {
+ a_consign = traj->target.circle.dest_angle;
+ delete_event(traj);
+ }
+
+ /* step 3 : send the processed commands to cs */
+
+ EVT_DEBUG(E_TRAJECTORY,"EVENT CIRCLE d_cur=%" PRIi32 ", d_consign=%" PRIi32
+ ", d_speed=%" PRIi32 ", a_cur=%" PRIi32 ", a_consign=%" PRIi32
+ ", a_speed=%" PRIi32 "radius = %f",
+ rs_get_distance(traj->robot), d_consign, get_quadramp_distance_speed(traj),
+ rs_get_angle(traj->robot), a_consign, get_quadramp_angle_speed(traj),
+ new_radius);
+
+ cs_set_consign(traj->csm_angle, a_consign);
+ cs_set_consign(traj->csm_distance, d_consign);
+}
+
+/* trajectory event */
+void trajectory_manager_event(void * param)
+{
+ struct trajectory *traj = (struct trajectory *)param;
+
+ switch (traj->state) {
+ case RUNNING_XY_START:
+ case RUNNING_XY_ANGLE:
+ case RUNNING_XY_ANGLE_OK:
+ case RUNNING_XY_F_START:
+ case RUNNING_XY_F_ANGLE:
+ case RUNNING_XY_F_ANGLE_OK:
+ case RUNNING_XY_B_START:
+ case RUNNING_XY_B_ANGLE:
+ case RUNNING_XY_B_ANGLE_OK:
+ trajectory_manager_xy_event(traj);
+ break;
+
+ case RUNNING_CIRCLE:
+ trajectory_manager_circle_event(traj);
+ break;
+
+ default:
+ break;
+ }
+}
+
+/*********** *CIRCLE */
+
+/* make the robot orbiting around (x,y) on a circle whose radius is
+ * radius_mm, and exit when relative destination angle is reached. The
+ * flags set if we go forward or backwards, and CW/CCW. */
+void trajectory_circle_rel(struct trajectory *traj,
+ double x, double y,
+ double radius_mm,
+ double rel_a_deg,
+ uint8_t flags)
+{
+ double dst_angle;
+
+ delete_event(traj);
+
+ traj->target.circle.center.x = x;
+ traj->target.circle.center.y = y;
+ traj->target.circle.radius = radius_mm;
+ traj->target.circle.flags = flags;
+
+ /* convert in steps */
+ dst_angle = RAD(rel_a_deg) *
+ (traj->position->phys.distance_imp_per_mm) *
+ (traj->position->phys.track_mm) / 2.0;
+
+ traj->target.circle.dest_angle = rs_get_angle(traj->robot);
+ traj->target.circle.dest_angle += dst_angle;
+
+ DEBUG(E_TRAJECTORY, "Circle rel (x,y)=%2.2f,%2.2f r=%2.2f flags=%x dst_angle=%"PRIi32"",
+ x, y, radius_mm, flags, traj->target.circle.dest_angle);
+
+ traj->state = RUNNING_CIRCLE;
+ trajectory_manager_event(traj);
+ schedule_event(traj);
+}
+
+/*
+ * Compute the fastest distance and angle speeds matching the radius
+ * from current traj_speed
+ */
+/* static */void circle_get_da_speed_from_radius(struct trajectory *traj,
+ double radius_mm,
+ double *speed_d,
+ double *speed_a)
+{
+ /* speed_d = coef * speed_a */
+ double coef;
+ double speed_d2, speed_a2;
+
+ coef = 2. * radius_mm / traj->position->phys.track_mm;
+
+ speed_d2 = traj->a_speed * coef;
+ if (speed_d2 < traj->d_speed) {
+ *speed_d = speed_d2;
+ *speed_a = traj->a_speed;
+ }
+ else {
+ speed_a2 = traj->d_speed / coef;
+ *speed_d = traj->d_speed;
+ *speed_a = speed_a2;
+ }
+}
+
+/* return the distance in millimeters that corresponds to an angle in
+ * degree and a radius in mm */
+/* static */double circle_get_dist_from_degrees(double radius_mm, double a_deg)
+{
+ double a_rad = RAD(a_deg);
+ return a_rad * radius_mm;
+}
+
+/*
+ * Start a circle of specified radius around the specified center
+ * (relative with d,a). The distance is specified in mm.
+ */
+void trajectory_circle(struct trajectory *traj,
+ double center_d_mm, double center_a_rad,
+ double radius_mm, double dist_mm)
+{
+/* double */
+
+/* DEBUG(E_TRAJECTORY, "CIRCLE to d=%f a_rad=%f", center_d_mm, */
+/* center_a_rad); */
+/* delete_event(traj); */
+/* traj->state = RUNNING_CIRCLE; */
+
+
+}
+
+/*
+ * Start a circle of specified radius around the specified center
+ * (absolute). The distance is specified in mm.
+ */
+void trajectory_circle_abs_dist_mm(struct trajectory *traj,
+ double x_rel_mm, double y_rel_mm,
+ double radius_mm, double dist_mm)
+{
+}
+
+/*
+ * Start a circle of specified radius around the specified center
+ * (absolute). The distance is specified in degrees.
+ */
+void trajectory_circle_abs_dist_deg(struct trajectory *traj,
+ double x_rel_mm, double y_rel_mm,
+ double radius_mm, double dist_degrees)
+{
+
+}
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ *
+ * This program 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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: trajectory_manager.c,v 1.4.4.17 2009-05-18 12:28:36 zer0 Exp $
+ *
+ */
+
+/* Trajectory Manager v3 - zer0 - for Eurobot 2010 */
+
+/**
+ * update angle and/or distance
+ * this function is not called directly by the user
+ * traj : pointer to the trajectory structure
+ * d_mm : distance in mm
+ * 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 state, uint8_t flags);
+
+/** go straight forward (d is in mm) */
+void trajectory_d_rel(struct trajectory *traj, double d_mm);
+
+/** update distance consign without changing angle consign */
+void trajectory_only_d_rel(struct trajectory *traj, double d_mm);
+
+/** turn by 'a' degrees */
+void trajectory_a_rel(struct trajectory *traj, double a_deg_rel);
+
+/** turn by 'a' degrees */
+void trajectory_a_abs(struct trajectory *traj, double a_deg_abs);
+
+/** turn the robot until the point x,y is in front of us */
+void trajectory_turnto_xy(struct trajectory *traj, double x_abs_mm, double y_abs_mm);
+
+
+/** turn the robot until the point x,y is behind us */
+void trajectory_turnto_xy_behind(struct trajectory *traj, double x_abs_mm, double y_abs_mm);
+
+
+/** update angle consign without changing distance consign */
+void trajectory_only_a_rel(struct trajectory *traj, double a_deg);
+
+/** update angle consign without changing distance consign */
+void trajectory_only_a_abs(struct trajectory *traj, double a_deg_abs);
+
+
+/** turn by 'a' degrees */
+void trajectory_d_a_rel(struct trajectory *traj, double d_mm, double a_deg);
+
+/** set relative angle and distance consign to 0 */
+void trajectory_stop(struct trajectory *traj);
+
+/** set relative angle and distance consign to 0, and break any
+ * deceleration ramp in quadramp filter */
+void trajectory_hardstop(struct trajectory *traj);
+
+
+/************ GOTO XY, USE EVENTS */
+
+/** goto a x,y point, using a trajectory event */
+void trajectory_goto_xy_abs(struct trajectory *traj, double x, double y);
+
+/** go forward to a x,y point, using a trajectory event */
+void trajectory_goto_forward_xy_abs(struct trajectory *traj, double x, double y);
+
+/** go backward to a x,y point, using a trajectory event */
+void trajectory_goto_backward_xy_abs(struct trajectory *traj, double x, double y);
+
+/** go forward to a d,a point, using a trajectory event */
+void trajectory_goto_d_a_rel(struct trajectory *traj, double d, double a);
+
+/** go forward to a x,y relative point, using a trajectory event */
+void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_mm, double y_rel_mm);
+
+/************ FUNCS FOR GETTING TRAJ STATE */
+
+/** return true if the position consign is equal to the filtered
+ * position consign (after quadramp filter), for angle and
+ * distance. */
+uint8_t trajectory_finished(struct trajectory *traj);
+
+/** return true if traj is nearly finished */
+uint8_t trajectory_in_window(struct trajectory *traj, double d_win, double a_win_rad);
+
+/*********** *TRAJECTORY EVENT FUNC */
+
+/** event called for xy trajectories */
+void trajectory_manager_xy_event(struct trajectory *traj);
+
+/* trajectory event for circles */
+void trajectory_manager_circle_event(struct trajectory *traj);
+
+/* trajectory event */
+void trajectory_manager_event(void * param);
+
+/*********** *CIRCLE */
+
+/* XXX TODO */
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ *
+ * This program 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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: trajectory_manager.c,v 1.4.4.17 2009-05-18 12:28:36 zer0 Exp $
+ *
+ */
+
+/* Trajectory Manager v3 - zer0 - for Eurobot 2010 */
+
+
+#include <string.h>
+#include <stdlib.h>
+#include <math.h>
+
+#include <aversive.h>
+#include <aversive/error.h>
+#include <scheduler.h>
+#include <vect2.h>
+
+#include <position_manager.h>
+#include <robot_system.h>
+#include <control_system_manager.h>
+#include <quadramp.h>
+
+#include <trajectory_manager.h>
+#include "trajectory_manager_utils.h"
+#include "trajectory_manager_core.h"
+
+/** set speed consign in quadramp filter */
+void set_quadramp_speed(struct trajectory *traj, int16_t d_speed, int16_t a_speed)
+{
+ struct quadramp_filter * q_d, * q_a;
+ q_d = traj->csm_distance->consign_filter_params;
+ q_a = traj->csm_angle->consign_filter_params;
+ quadramp_set_1st_order_vars(q_d, ABS(d_speed), ABS(d_speed));
+ quadramp_set_1st_order_vars(q_a, ABS(a_speed), ABS(a_speed));
+}
+
+/** get angle speed consign in quadramp filter */
+uint32_t get_quadramp_angle_speed(struct trajectory *traj)
+{
+ struct quadramp_filter *q_a;
+ q_a = traj->csm_angle->consign_filter_params;
+ return q_a->var_1st_ord_pos;
+}
+
+/** get distance speed consign in quadramp filter */
+uint32_t get_quadramp_distance_speed(struct trajectory *traj)
+{
+ struct quadramp_filter *q_d;
+ q_d = traj->csm_distance->consign_filter_params;
+ return q_d->var_1st_ord_pos;
+}
+
+/** remove event if any */
+void delete_event(struct trajectory *traj)
+{
+ set_quadramp_speed(traj, traj->d_speed, traj->a_speed);
+ if ( traj->scheduler_task != -1) {
+ DEBUG(E_TRAJECTORY, "Delete event");
+ scheduler_del_event(traj->scheduler_task);
+ traj->scheduler_task = -1;
+ }
+}
+
+/** schedule the trajectory event */
+void schedule_event(struct trajectory *traj)
+{
+ if ( traj->scheduler_task != -1) {
+ DEBUG(E_TRAJECTORY, "Schedule event, already scheduled");
+ }
+ else {
+ traj->scheduler_task =
+ scheduler_add_periodical_event_priority(&trajectory_manager_event,
+ (void*)traj,
+ TRAJ_EVT_PERIOD, 30);
+ }
+}
+
+/** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */
+double simple_modulo_2pi(double a)
+{
+ if (a < -M_PI) {
+ a += M_2PI;
+ }
+ else if (a > M_PI) {
+ a -= M_2PI;
+ }
+ return a;
+}
+
+/** do a modulo 2.pi -> [-Pi,+Pi] */
+double modulo_2pi(double a)
+{
+ double res = a - (((int32_t) (a/M_2PI)) * M_2PI);
+ return simple_modulo_2pi(res);
+}
+
+/** near the target (dist) ? */
+uint8_t is_robot_in_dist_window(struct trajectory *traj, double d_win)
+{
+ 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);
+}
+
+/** near the target (dist in x,y) ? */
+uint8_t is_robot_in_xy_window(struct trajectory *traj, double d_win)
+{
+ double x1 = traj->target.cart.x;
+ double y1 = traj->target.cart.y;
+ double x2 = position_get_x_double(traj->position);
+ double y2 = position_get_y_double(traj->position);
+ return ( sqrt ((x2-x1) * (x2-x1) + (y2-y1) * (y2-y1)) < d_win );
+}
+
+/** near the angle target in radian ? Only valid if
+ * traj->target.pol.angle is set (i.e. an angle command, not an xy
+ * command) */
+uint8_t is_robot_in_angle_window(struct trajectory *traj, double 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;
+}
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ *
+ * This program 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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: trajectory_manager.c,v 1.4.4.17 2009-05-18 12:28:36 zer0 Exp $
+ *
+ */
+
+/* Trajectory Manager v3 - zer0 - for Eurobot 2010 */
+
+#define M_2PI (2*M_PI)
+
+#define DEG(x) ((x) * (180.0 / M_PI))
+#define RAD(x) ((x) * (M_PI / 180.0))
+
+/* 25 ms */
+#define TRAJ_EVT_PERIOD (25000UL/SCHEDULER_UNIT)
+
+/** set speed consign in quadramp filter */
+void set_quadramp_speed(struct trajectory *traj, int16_t d_speed, int16_t a_speed);
+
+/** get angle speed consign in quadramp filter */
+uint32_t get_quadramp_angle_speed(struct trajectory *traj);
+
+/** get distance speed consign in quadramp filter */
+uint32_t get_quadramp_distance_speed(struct trajectory *traj);
+
+/** remove event if any */
+void delete_event(struct trajectory *traj);
+
+/** schedule the trajectory event */
+void schedule_event(struct trajectory *traj);
+
+/** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */
+double simple_modulo_2pi(double a);
+
+/** do a modulo 2.pi -> [-Pi,+Pi] */
+double modulo_2pi(double a);
+
+/** near the target (dist) ? */
+uint8_t is_robot_in_dist_window(struct trajectory *traj, double d_win);
+
+/** near the target (dist in x,y) ? */
+uint8_t is_robot_in_xy_window(struct trajectory *traj, double d_win);
+
+/** near the angle target in radian ? Only valid if
+ * traj->target.pol.angle is set (i.e. an angle command, not an xy
+ * command) */
+uint8_t is_robot_in_angle_window(struct trajectory *traj, double a_win_rad);
#include "parse.h"
+#ifdef HOST_VERSION
+#define pgm_read_pgmptr(x) ((void *)(*(x)))
+#else
+#define pgm_read_pgmptr(x) (void *)pgm_read_word(x)
+#endif
+
//#define CMDLINE_DEBUG
//#define debug_printf printf
#define debug_printf(args...) do {} while(0)
int8_t n = 0;
struct token_hdr token_hdr;
- token_p = (parse_pgm_token_hdr_t *)pgm_read_word(&inst->tokens[token_num]);
+ token_p = (parse_pgm_token_hdr_t *)pgm_read_pgmptr(&inst->tokens[token_num]);
if (token_p)
memcpy_P(&token_hdr, token_p, sizeof(token_hdr));
buf += n;
token_num ++;
- token_p = (parse_pgm_token_hdr_t *)pgm_read_word(&inst->tokens[token_num]);
+ token_p = (parse_pgm_token_hdr_t *)pgm_read_pgmptr(&inst->tokens[token_num]);
if (token_p)
memcpy_P(&token_hdr, token_p, sizeof(token_hdr));
}
#endif
/* parse it !! */
- inst = (parse_pgm_inst_t *)pgm_read_word(ctx+inst_num);
+ inst = (parse_pgm_inst_t *)pgm_read_pgmptr(ctx+inst_num);
while (inst) {
debug_printf("INST\n");
}
inst_num ++;
- inst = (parse_pgm_inst_t *)pgm_read_word(ctx+inst_num);
+ inst = (parse_pgm_inst_t *)pgm_read_pgmptr(ctx+inst_num);
}
/* call func */
nb_completable = 0;
nb_non_completable = 0;
- inst = (parse_pgm_inst_t *)pgm_read_word(ctx+inst_num);
+ inst = (parse_pgm_inst_t *)pgm_read_pgmptr(ctx+inst_num);
while (inst) {
/* parse the first tokens of the inst */
if (nb_token && match_inst(inst, buf, nb_token, NULL))
goto next;
debug_printf("instruction match \n");
- token_p = (parse_pgm_token_hdr_t *) pgm_read_word(&inst->tokens[nb_token]);
+ token_p = (parse_pgm_token_hdr_t *) pgm_read_pgmptr(&inst->tokens[nb_token]);
if (token_p)
memcpy_P(&token_hdr, token_p, sizeof(token_hdr));
}
next:
inst_num ++;
- inst = (parse_pgm_inst_t *)pgm_read_word(ctx+inst_num);
+ inst = (parse_pgm_inst_t *)pgm_read_pgmptr(ctx+inst_num);
}
debug_printf("total choices %d for this completion\n", nb_completable);
debug_printf("Multiple choice STATE=%d\n", *state);
inst_num = 0;
- inst = (parse_pgm_inst_t *)pgm_read_word(ctx+inst_num);
+ inst = (parse_pgm_inst_t *)pgm_read_pgmptr(ctx+inst_num);
while (inst) {
/* we need to redo it */
- inst = (parse_pgm_inst_t *)pgm_read_word(ctx+inst_num);
+ inst = (parse_pgm_inst_t *)pgm_read_pgmptr(ctx+inst_num);
if (nb_token && match_inst(inst, buf, nb_token, NULL))
goto next2;
- token_p = (parse_pgm_token_hdr_t *)pgm_read_word(&inst->tokens[nb_token]);
+ token_p = (parse_pgm_token_hdr_t *)pgm_read_pgmptr(&inst->tokens[nb_token]);
if (token_p)
memcpy_P(&token_hdr, token_p, sizeof(token_hdr));
(*state)++;
if (token_p && token_hdr.ops->get_help) {
token_hdr.ops->get_help(token_p, tmpbuf, sizeof(tmpbuf));
- help_str = (prog_char *) pgm_read_word(&inst->help_str);
+ help_str = (prog_char *) pgm_read_pgmptr(&inst->help_str);
if (help_str)
- snprintf_P(dst, size, PSTR("[%s]: %S"), tmpbuf, help_str);
+ snprintf_P(dst, size, PSTR("[%s]: "PGMS_FMT""), tmpbuf, help_str);
else
snprintf_P(dst, size, PSTR("[%s]: No help"), tmpbuf);
}
l=snprintf(dst, size, "%s", tmpbuf);
if (l>=0 && token_hdr.ops->get_help) {
token_hdr.ops->get_help(token_p, tmpbuf, sizeof(tmpbuf));
- help_str = (prog_char *) pgm_read_word(&inst->help_str);
+ help_str = (prog_char *) pgm_read_pgmptr(&inst->help_str);
if (help_str)
- snprintf_P(dst+l, size-l, PSTR("[%s]: %S"), tmpbuf, help_str);
+ snprintf_P(dst+l, size-l, PSTR("[%s]: "PGMS_FMT), tmpbuf, help_str);
else
snprintf_P(dst+l, size-l, PSTR("[%s]: No help"), tmpbuf);
}
-
+
return 1;
}
}
next2:
inst_num ++;
- inst = (parse_pgm_inst_t *)pgm_read_word(ctx+inst_num);
+ inst = (parse_pgm_inst_t *)pgm_read_pgmptr(ctx+inst_num);
}
return 0;
}
#
-# Automatically generated by make menuconfig: don't edit
+# Automatically generated make config: don't edit
#
#
#
# Base modules
#
+
+#
+# Enable math library in generation options to see all modules
+#
CONFIG_MODULE_CIRBUF=y
# CONFIG_MODULE_CIRBUF_LARGE is not set
CONFIG_MODULE_FIXED_POINT=y
#
# Communication modules
#
+
+#
+# uart needs circular buffer, mf2 client may need scheduler
+#
CONFIG_MODULE_UART=y
# CONFIG_MODULE_UART_9BITS is not set
CONFIG_MODULE_UART_CREATE_CONFIG=y
CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG=y
#
-# Robot specific modules
+# Robot specific modules (fixed point lib may be needed)
#
CONFIG_MODULE_ROBOT_SYSTEM=y
+# CONFIG_MODULE_ROBOT_SYSTEM_USE_F64 is not set
# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set
CONFIG_MODULE_POSITION_MANAGER=y
CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE=y
# Control system modules
#
CONFIG_MODULE_CONTROL_SYSTEM_MANAGER=y
+
+#
+# Filters
+#
CONFIG_MODULE_PID=y
# CONFIG_MODULE_PID_CREATE_CONFIG is not set
# CONFIG_MODULE_RAMP is not set
#
# 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
#
# 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=y
CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG=y
CONFIG_MODULE_ERROR=y
extern parse_pgm_inst_t cmd_traj_speed_show;
extern parse_pgm_inst_t cmd_trajectory;
extern parse_pgm_inst_t cmd_trajectory_show;
+extern parse_pgm_inst_t cmd_circle_coef;
+extern parse_pgm_inst_t cmd_circle_coef_show;
extern parse_pgm_inst_t cmd_rs_gains;
extern parse_pgm_inst_t cmd_rs_gains_show;
extern parse_pgm_inst_t cmd_track;
extern parse_pgm_inst_t cmd_goto1;
extern parse_pgm_inst_t cmd_goto2;
extern parse_pgm_inst_t cmd_goto3;
+extern parse_pgm_inst_t cmd_goto4;
extern parse_pgm_inst_t cmd_position;
extern parse_pgm_inst_t cmd_position_set;
extern parse_pgm_inst_t cmd_strat_infos;
(parse_pgm_inst_t *)&cmd_traj_speed_show,
(parse_pgm_inst_t *)&cmd_trajectory,
(parse_pgm_inst_t *)&cmd_trajectory_show,
+ (parse_pgm_inst_t *)&cmd_circle_coef,
+ (parse_pgm_inst_t *)&cmd_circle_coef_show,
(parse_pgm_inst_t *)&cmd_rs_gains,
(parse_pgm_inst_t *)&cmd_rs_gains_show,
(parse_pgm_inst_t *)&cmd_track,
(parse_pgm_inst_t *)&cmd_pt_list_show,
(parse_pgm_inst_t *)&cmd_goto1,
(parse_pgm_inst_t *)&cmd_goto2,
+ (parse_pgm_inst_t *)&cmd_goto4,
(parse_pgm_inst_t *)&cmd_position,
(parse_pgm_inst_t *)&cmd_position_set,
(parse_pgm_inst_t *)&cmd_strat_infos,
#endif
/**********************************************************/
-/* Test */
+/* Circle_Radius */
-/* this structure is filled when cmd_test is parsed successfully */
-struct cmd_test_result {
+/* this structure is filled when cmd_circle_radius is parsed successfully */
+struct cmd_circle_radius_result {
fixed_string_t arg0;
int32_t radius;
};
double radius_mm,
double *speed_d,
double *speed_a);
-/* function called when cmd_test is parsed successfully */
-static void cmd_test_parsed(void *parsed_result, void *data)
+/* function called when cmd_circle_radius is parsed successfully */
+static void cmd_circle_radius_parsed(void *parsed_result, void *data)
{
- struct cmd_test_result *res = parsed_result;
+ struct cmd_circle_radius_result *res = parsed_result;
double d,a;
strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
circle_get_da_speed_from_radius(&mainboard.traj, res->radius, &d, &a);
printf_P(PSTR("d=%2.2f a=%2.2f\r\n"), d, a);
}
+prog_char str_circle_radius_arg0[] = "circle_radius";
+parse_pgm_token_string_t cmd_circle_radius_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_circle_radius_result, arg0, str_circle_radius_arg0);
+parse_pgm_token_num_t cmd_circle_radius_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_circle_radius_result, radius, INT32);
+
+prog_char help_circle_radius[] = "Circle_Radius function";
+parse_pgm_inst_t cmd_circle_radius = {
+ .f = cmd_circle_radius_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_circle_radius,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_circle_radius_arg0,
+ (prog_void *)&cmd_circle_radius_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Test */
+
+/* this structure is filled when cmd_test is parsed successfully */
+struct cmd_test_result {
+ fixed_string_t arg0;
+ uint32_t mask;
+};
+
+extern uint32_t encmask;
+
+/* function called when cmd_test is parsed successfully */
+static void cmd_test_parsed(void *parsed_result, void *data)
+{
+ struct cmd_test_result *res = parsed_result;
+ encmask = res->mask;
+}
+
prog_char str_test_arg0[] = "test";
parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
-parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, radius, INT32);
+parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, mask, UINT32);
prog_char help_test[] = "Test function";
parse_pgm_inst_t cmd_test = {
},
};
+/**********************************************************/
+/* circle coef configuration */
+
+/* this structure is filled when cmd_circle_coef is parsed successfully */
+struct cmd_circle_coef_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ float circle_coef;
+};
+
+
+/* function called when cmd_circle_coef is parsed successfully */
+static void cmd_circle_coef_parsed(void *parsed_result, void *data)
+{
+ struct cmd_circle_coef_result *res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("set"))) {
+ trajectory_set_circle_coef(&mainboard.traj, res->circle_coef);
+ }
+
+ printf_P(PSTR("circle_coef set %2.2f\r\n"), mainboard.traj.circle_coef);
+}
+
+prog_char str_circle_coef_arg0[] = "circle_coef";
+parse_pgm_token_string_t cmd_circle_coef_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg0, str_circle_coef_arg0);
+prog_char str_circle_coef_arg1[] = "set";
+parse_pgm_token_string_t cmd_circle_coef_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_arg1);
+parse_pgm_token_num_t cmd_circle_coef_val = TOKEN_NUM_INITIALIZER(struct cmd_circle_coef_result, circle_coef, FLOAT);
+
+prog_char help_circle_coef[] = "Set circle coef";
+parse_pgm_inst_t cmd_circle_coef = {
+ .f = cmd_circle_coef_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_circle_coef,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_circle_coef_arg0,
+ (prog_void *)&cmd_circle_coef_arg1,
+ (prog_void *)&cmd_circle_coef_val,
+ NULL,
+ },
+};
+
+/* show */
+
+prog_char str_circle_coef_show_arg[] = "show";
+parse_pgm_token_string_t cmd_circle_coef_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_show_arg);
+
+prog_char help_circle_coef_show[] = "Show circle coef";
+parse_pgm_inst_t cmd_circle_coef_show = {
+ .f = cmd_circle_coef_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_circle_coef_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_circle_coef_arg0,
+ (prog_void *)&cmd_circle_coef_show_arg,
+ NULL,
+ },
+};
+
/**********************************************************/
/* trajectory window configuration */
RIGHT_ENCODER, res->right); //en augmentant on tourne à droite
}
printf_P(PSTR("rs_gains set "));
- f64_print(mainboard.rs.left_ext_gain);
+ //f64_print(mainboard.rs.left_ext_gain);
printf_P(PSTR(" "));
- f64_print(mainboard.rs.right_ext_gain);
+ //f64_print(mainboard.rs.right_ext_gain);
printf_P(PSTR("\r\n"));
}
int32_t arg2;
int32_t arg3;
int32_t arg4;
+ int32_t arg5;
};
/* function called when cmd_goto is parsed successfully */
else if (!strcmp_P(res->arg1, PSTR("da_rel"))) {
trajectory_d_a_rel(&mainboard.traj, res->arg2, res->arg3);
}
+ else if (!strcmp_P(res->arg1, PSTR("circle_rel"))) {
+ trajectory_circle_rel(&mainboard.traj, res->arg2, res->arg3,
+ res->arg4, res->arg5, 0);
+ return; /* XXX */
+ }
t1 = time_get_us2();
while ((err = test_traj_end(0xFF)) == 0) {
t2 = time_get_us2();
},
};
+prog_char str_goto_arg1_c[] = "circle_rel";
+parse_pgm_token_string_t cmd_goto_arg1_c = TOKEN_STRING_INITIALIZER(struct cmd_goto_result, arg1, str_goto_arg1_c);
+parse_pgm_token_num_t cmd_goto_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg4, INT32);
+parse_pgm_token_num_t cmd_goto_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_goto_result, arg5, INT32);
+
+/* 4 params */
+prog_char help_goto4[] = "Do a circle: (x,y, radius, angle)";
+parse_pgm_inst_t cmd_goto4 = {
+ .f = cmd_goto_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_goto4,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_goto_arg0,
+ (prog_void *)&cmd_goto_arg1_c,
+ (prog_void *)&cmd_goto_arg2,
+ (prog_void *)&cmd_goto_arg3,
+ (prog_void *)&cmd_goto_arg4,
+ (prog_void *)&cmd_goto_arg5,
+ NULL,
+ },
+};
+
/**********************************************************/
/* Position tests */
cs_get_out(cs));
}
+uint32_t encmask = 0xFFFFFFFF;
+int32_t my_encoders_get_value(void *enc)
+{
+ int32_t tmp = encoders_spi_get_value(enc);
+ return tmp & encmask;
+}
+
void dump_pid(const char *name, struct pid_filter *pid)
{
printf_P(PSTR("%s P=% .8ld I=% .8ld D=% .8ld out=% .8ld\r\n"),
rs_set_left_pwm(&mainboard.rs, pwm_set_and_save, LEFT_PWM);
rs_set_right_pwm(&mainboard.rs, pwm_set_and_save, RIGHT_PWM);
/* increase gain to decrease dist, increase left and it will turn more left */
- rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value,
+ rs_set_left_ext_encoder(&mainboard.rs, my_encoders_get_value,
LEFT_ENCODER, IMP_COEF * 1.0015);
- rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value,
+ rs_set_right_ext_encoder(&mainboard.rs, my_encoders_get_value,
RIGHT_ENCODER, IMP_COEF * -1.006);
/* rs will use external encoders */
rs_set_flags(&mainboard.rs, RS_USE_EXT);
return a;
}
-/** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */
-double simple_modulo_2pi(double a)
-{
- if (a < -M_PI) {
- a += M_2PI;
- }
- else if (a > M_PI) {
- a -= M_2PI;
- }
- return a;
-}
-
/* return the distance to a point in the area */
int16_t angle_abs_to_rel(int16_t a_abs)
{
int16_t distance_between(int16_t x1, int16_t y1, int16_t x2, int16_t y2);
int16_t distance_from_robot(int16_t x, int16_t y);
int16_t simple_modulo_360(int16_t a);
-double simple_modulo_2pi(double a);
int16_t angle_abs_to_rel(int16_t a_abs);
void rel_da_to_abs_xy(double d_rel, double a_rel_rad, double *x_abs, double *y_abs);
double norm(double x, double y);
import shlex
import time
import math
+import re
import warnings
warnings.filterwarnings("ignore","tempnam",RuntimeWarning, __name__)
time.sleep(1)
self.ser.write("pwm s3(3C) 250\n")
+ def do_circle(self, arg):
+ arg = re.sub(" *", " ", arg)
+ arg = arg.strip()
+ arg = arg.split(" ")
+ if len(arg)!=4:
+ print "radius coef dspeed aspeed"
+ return
+ r = int(arg[0])
+ c = float(arg[1])
+ dspeed = int(arg[2])
+ aspeed = int(arg[3])
+
+ self.ser.write("event cs on\n")
+ time.sleep(0.3)
+
+ self.ser.write("traj_speed distance %d\n"%dspeed)
+ time.sleep(0.3)
+ self.ser.write("traj_speed angle %d\n"%aspeed)
+ time.sleep(0.3)
+
+ self.ser.write("goto d_rel 300\n")
+ time.sleep(3)
+
+ self.ser.write("position reset\n")
+ time.sleep(0.3)
+ self.ser.write("circle_coef set %f\n"%c)
+ time.sleep(0.3)
+
+ self.ser.write("goto circle_rel 700 460 %d 360\n"%r)
+ self.ser.flushInput()
+ time.sleep(0.3)
+
+
if __name__ == "__main__":
try:
import readline,atexit
#
-# Automatically generated by make menuconfig: don't edit
+# Automatically generated make config: don't edit
#
#
#
# Base modules
#
+
+#
+# Enable math library in generation options to see all modules
+#
CONFIG_MODULE_CIRBUF=y
# CONFIG_MODULE_CIRBUF_LARGE is not set
CONFIG_MODULE_FIXED_POINT=y
#
# Communication modules
#
+
+#
+# uart needs circular buffer, mf2 client may need scheduler
+#
CONFIG_MODULE_UART=y
# CONFIG_MODULE_UART_9BITS is not set
CONFIG_MODULE_UART_CREATE_CONFIG=y
CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG=y
#
-# Robot specific modules
+# Robot specific modules (fixed point lib may be needed)
#
CONFIG_MODULE_ROBOT_SYSTEM=y
+# CONFIG_MODULE_ROBOT_SYSTEM_USE_F64 is not set
# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set
CONFIG_MODULE_POSITION_MANAGER=y
CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE=y
# Control system modules
#
CONFIG_MODULE_CONTROL_SYSTEM_MANAGER=y
+
+#
+# Filters
+#
CONFIG_MODULE_PID=y
# CONFIG_MODULE_PID_CREATE_CONFIG is not set
# CONFIG_MODULE_RAMP is not set
#
# 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
#
# 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=y
CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG=y
CONFIG_MODULE_ERROR=y
extern parse_pgm_inst_t cmd_traj_speed_show;
extern parse_pgm_inst_t cmd_trajectory;
extern parse_pgm_inst_t cmd_trajectory_show;
+extern parse_pgm_inst_t cmd_circle_coef;
+extern parse_pgm_inst_t cmd_circle_coef_show;
extern parse_pgm_inst_t cmd_rs_gains;
extern parse_pgm_inst_t cmd_rs_gains_show;
extern parse_pgm_inst_t cmd_track;
(parse_pgm_inst_t *)&cmd_traj_speed_show,
(parse_pgm_inst_t *)&cmd_trajectory,
(parse_pgm_inst_t *)&cmd_trajectory_show,
+ (parse_pgm_inst_t *)&cmd_circle_coef,
+ (parse_pgm_inst_t *)&cmd_circle_coef_show,
(parse_pgm_inst_t *)&cmd_rs_gains,
(parse_pgm_inst_t *)&cmd_rs_gains_show,
(parse_pgm_inst_t *)&cmd_track,
#include <aversive/pgmspace.h>
#include <aversive/wait.h>
#include <aversive/error.h>
-#include <avr/eeprom.h>
+#include <aversive/eeprom.h>
#include <ax12.h>
#include <uart.h>
},
};
+/**********************************************************/
+/* circle coef configuration */
+
+/* this structure is filled when cmd_circle_coef is parsed successfully */
+struct cmd_circle_coef_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ float circle_coef;
+};
+
+
+/* function called when cmd_circle_coef is parsed successfully */
+static void cmd_circle_coef_parsed(void *parsed_result, void *data)
+{
+ struct cmd_circle_coef_result *res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("set"))) {
+ trajectory_set_circle_coef(&mainboard.traj, res->circle_coef);
+ }
+
+ printf_P(PSTR("circle_coef %2.2f\r\n"), mainboard.traj.circle_coef);
+}
+
+prog_char str_circle_coef_arg0[] = "circle_coef";
+parse_pgm_token_string_t cmd_circle_coef_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg0, str_circle_coef_arg0);
+prog_char str_circle_coef_arg1[] = "set";
+parse_pgm_token_string_t cmd_circle_coef_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_arg1);
+parse_pgm_token_num_t cmd_circle_coef_val = TOKEN_NUM_INITIALIZER(struct cmd_circle_coef_result, circle_coef, FLOAT);
+
+prog_char help_circle_coef[] = "Set circle coef";
+parse_pgm_inst_t cmd_circle_coef = {
+ .f = cmd_circle_coef_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_circle_coef,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_circle_coef_arg0,
+ (prog_void *)&cmd_circle_coef_arg1,
+ (prog_void *)&cmd_circle_coef_val,
+ NULL,
+ },
+};
+
+/* show */
+
+prog_char str_circle_coef_show_arg[] = "show";
+parse_pgm_token_string_t cmd_circle_coef_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_circle_coef_result, arg1, str_circle_coef_show_arg);
+
+prog_char help_circle_coef_show[] = "Show circle coef";
+parse_pgm_inst_t cmd_circle_coef_show = {
+ .f = cmd_circle_coef_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_circle_coef_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_circle_coef_arg0,
+ (prog_void *)&cmd_circle_coef_show_arg,
+ NULL,
+ },
+};
+
/**********************************************************/
/* trajectory window configuration */
#include <aversive/pgmspace.h>
#include <aversive/wait.h>
#include <aversive/error.h>
-#include <avr/eeprom.h>
+#include <aversive/eeprom.h>
#include <ax12.h>
#include <uart.h>
--- /dev/null
+#
+# Automatically generated by make menuconfig: don't edit
+#
+
+#
+# Hardware
+#
+# CONFIG_MCU_AT90S2313 is not set
+# CONFIG_MCU_AT90S2323 is not set
+# CONFIG_MCU_AT90S3333 is not set
+# CONFIG_MCU_AT90S2343 is not set
+# CONFIG_MCU_ATTINY22 is not set
+# CONFIG_MCU_ATTINY26 is not set
+# CONFIG_MCU_AT90S4414 is not set
+# CONFIG_MCU_AT90S4433 is not set
+# CONFIG_MCU_AT90S4434 is not set
+# CONFIG_MCU_AT90S8515 is not set
+# CONFIG_MCU_AT90S8534 is not set
+# CONFIG_MCU_AT90S8535 is not set
+# CONFIG_MCU_AT86RF401 is not set
+# CONFIG_MCU_ATMEGA103 is not set
+# CONFIG_MCU_ATMEGA603 is not set
+# CONFIG_MCU_AT43USB320 is not set
+# CONFIG_MCU_AT43USB355 is not set
+# CONFIG_MCU_AT76C711 is not set
+# CONFIG_MCU_ATMEGA8 is not set
+# CONFIG_MCU_ATMEGA48 is not set
+# CONFIG_MCU_ATMEGA88 is not set
+# CONFIG_MCU_ATMEGA8515 is not set
+# CONFIG_MCU_ATMEGA8535 is not set
+# CONFIG_MCU_ATTINY13 is not set
+# CONFIG_MCU_ATTINY2313 is not set
+# CONFIG_MCU_ATMEGA16 is not set
+# CONFIG_MCU_ATMEGA161 is not set
+# CONFIG_MCU_ATMEGA162 is not set
+# CONFIG_MCU_ATMEGA163 is not set
+# CONFIG_MCU_ATMEGA165 is not set
+# CONFIG_MCU_ATMEGA168 is not set
+# CONFIG_MCU_ATMEGA169 is not set
+# CONFIG_MCU_ATMEGA32 is not set
+# CONFIG_MCU_ATMEGA323 is not set
+# CONFIG_MCU_ATMEGA325 is not set
+# CONFIG_MCU_ATMEGA3250 is not set
+# CONFIG_MCU_ATMEGA64 is not set
+# CONFIG_MCU_ATMEGA645 is not set
+# CONFIG_MCU_ATMEGA6450 is not set
+CONFIG_MCU_ATMEGA128=y
+# CONFIG_MCU_ATMEGA1281 is not set
+# CONFIG_MCU_AT90CAN128 is not set
+# CONFIG_MCU_AT94K is not set
+# CONFIG_MCU_AT90S1200 is not set
+# CONFIG_MCU_ATMEGA2560 is not set
+# CONFIG_MCU_ATMEGA256 is not set
+CONFIG_QUARTZ=16000000
+
+#
+# Generation options
+#
+# CONFIG_OPTM_0 is not set
+# CONFIG_OPTM_1 is not set
+# CONFIG_OPTM_2 is not set
+# CONFIG_OPTM_3 is not set
+CONFIG_OPTM_S=y
+CONFIG_MATH_LIB=y
+# CONFIG_FDEVOPEN_COMPAT is not set
+# CONFIG_NO_PRINTF is not set
+# CONFIG_MINIMAL_PRINTF is not set
+CONFIG_STANDARD_PRINTF=y
+# CONFIG_ADVANCED_PRINTF is not set
+# CONFIG_FORMAT_IHEX is not set
+# CONFIG_FORMAT_SREC is not set
+CONFIG_FORMAT_BINARY=y
+
+#
+# Base modules
+#
+# CONFIG_MODULE_CIRBUF is not set
+# CONFIG_MODULE_CIRBUF_LARGE is not set
+# CONFIG_MODULE_FIXED_POINT is not set
+# CONFIG_MODULE_VECT2 is not set
+CONFIG_MODULE_GEOMETRY=y
+# CONFIG_MODULE_SCHEDULER is not set
+# CONFIG_MODULE_SCHEDULER_STATS is not set
+# CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set
+# CONFIG_MODULE_SCHEDULER_USE_TIMERS is not set
+CONFIG_MODULE_SCHEDULER_TIMER0=y
+# CONFIG_MODULE_SCHEDULER_MANUAL is not set
+# CONFIG_MODULE_TIME is not set
+# CONFIG_MODULE_TIME_CREATE_CONFIG is not set
+# CONFIG_MODULE_TIME_EXT is not set
+# CONFIG_MODULE_TIME_EXT_CREATE_CONFIG is not set
+
+#
+# Communication modules
+#
+# CONFIG_MODULE_UART is not set
+# CONFIG_MODULE_UART_9BITS is not set
+# CONFIG_MODULE_UART_CREATE_CONFIG is not set
+# CONFIG_MODULE_SPI is not set
+# CONFIG_MODULE_SPI_CREATE_CONFIG is not set
+# CONFIG_MODULE_I2C is not set
+# CONFIG_MODULE_I2C_MASTER is not set
+# CONFIG_MODULE_I2C_MULTIMASTER is not set
+# CONFIG_MODULE_I2C_CREATE_CONFIG is not set
+# CONFIG_MODULE_MF2_CLIENT is not set
+# CONFIG_MODULE_MF2_CLIENT_USE_SCHEDULER is not set
+# CONFIG_MODULE_MF2_CLIENT_CREATE_CONFIG is not set
+# CONFIG_MODULE_MF2_SERVER is not set
+# CONFIG_MODULE_MF2_SERVER_CREATE_CONFIG is not set
+
+#
+# Hardware modules
+#
+# CONFIG_MODULE_TIMER is not set
+# CONFIG_MODULE_TIMER_CREATE_CONFIG is not set
+# CONFIG_MODULE_TIMER_DYNAMIC is not set
+# CONFIG_MODULE_PWM is not set
+# CONFIG_MODULE_PWM_CREATE_CONFIG is not set
+# CONFIG_MODULE_PWM_NG is not set
+# CONFIG_MODULE_ADC is not set
+# CONFIG_MODULE_ADC_CREATE_CONFIG is not set
+
+#
+# IHM modules
+#
+# CONFIG_MODULE_MENU is not set
+# CONFIG_MODULE_VT100 is not set
+# CONFIG_MODULE_RDLINE is not set
+# CONFIG_MODULE_RDLINE_CREATE_CONFIG is not set
+# CONFIG_MODULE_RDLINE_KILL_BUF is not set
+# CONFIG_MODULE_RDLINE_HISTORY is not set
+# CONFIG_MODULE_PARSE is not set
+# CONFIG_MODULE_PARSE_NO_FLOAT is not set
+
+#
+# External devices modules
+#
+# CONFIG_MODULE_LCD is not set
+# CONFIG_MODULE_LCD_CREATE_CONFIG is not set
+# CONFIG_MODULE_MULTISERVO is not set
+# CONFIG_MODULE_MULTISERVO_CREATE_CONFIG is not set
+# CONFIG_MODULE_AX12 is not set
+# CONFIG_MODULE_AX12_CREATE_CONFIG is not set
+
+#
+# Brushless motor drivers (you should enable pwm modules to see all)
+#
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_CREATE_CONFIG is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE_CREATE_CONFIG is not set
+
+#
+# Encoders (you need comm/spi for encoders_spi)
+#
+# CONFIG_MODULE_ENCODERS_MICROB is not set
+# CONFIG_MODULE_ENCODERS_MICROB_CREATE_CONFIG is not set
+# CONFIG_MODULE_ENCODERS_EIRBOT is not set
+# CONFIG_MODULE_ENCODERS_EIRBOT_CREATE_CONFIG is not set
+# CONFIG_MODULE_ENCODERS_SPI is not set
+# CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG is not set
+
+#
+# Robot specific modules
+#
+# CONFIG_MODULE_ROBOT_SYSTEM is not set
+# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set
+# CONFIG_MODULE_POSITION_MANAGER is not set
+# CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE is not set
+# CONFIG_MODULE_TRAJECTORY_MANAGER is not set
+# CONFIG_MODULE_BLOCKING_DETECTION_MANAGER is not set
+# CONFIG_MODULE_OBSTACLE_AVOIDANCE is not set
+# CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG is not set
+
+#
+# Control system modules
+#
+# CONFIG_MODULE_CONTROL_SYSTEM_MANAGER is not set
+# CONFIG_MODULE_PID is not set
+# CONFIG_MODULE_PID_CREATE_CONFIG is not set
+# CONFIG_MODULE_RAMP is not set
+# CONFIG_MODULE_QUADRAMP is not set
+# CONFIG_MODULE_QUADRAMP_DERIVATE is not set
+# CONFIG_MODULE_BIQUAD is not set
+
+#
+# Radio devices
+#
+# CONFIG_MODULE_CC2420 is not set
+# CONFIG_MODULE_CC2420_CREATE_CONFIG is not set
+
+#
+# Crypto modules
+#
+# CONFIG_MODULE_AES is not set
+# CONFIG_MODULE_AES_CTR is not set
+# CONFIG_MODULE_MD5 is not set
+# CONFIG_MODULE_MD5_HMAC is not set
+# CONFIG_MODULE_RC4 is not set
+
+#
+# Encodings modules
+#
+# CONFIG_MODULE_BASE64 is not set
+# CONFIG_MODULE_HAMMING is not set
+
+#
+# Debug modules
+#
+# CONFIG_MODULE_DIAGNOSTIC is not set
+# CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG is not set
+CONFIG_MODULE_ERROR=y
+CONFIG_MODULE_ERROR_CREATE_CONFIG=y
+
+#
+# Programmer options
+#
+CONFIG_AVRDUDE=y
+# CONFIG_AVARICE is not set
+
+#
+# Avrdude
+#
+# CONFIG_AVRDUDE_PROG_FUTURELEC is not set
+# CONFIG_AVRDUDE_PROG_ABCMINI is not set
+# CONFIG_AVRDUDE_PROG_PICOWEB is not set
+# CONFIG_AVRDUDE_PROG_SP12 is not set
+# CONFIG_AVRDUDE_PROG_ALF is not set
+# CONFIG_AVRDUDE_PROG_BASCOM is not set
+# CONFIG_AVRDUDE_PROG_DT006 is not set
+# CONFIG_AVRDUDE_PROG_PONY_STK200 is not set
+CONFIG_AVRDUDE_PROG_STK200=y
+# CONFIG_AVRDUDE_PROG_PAVR is not set
+# CONFIG_AVRDUDE_PROG_BUTTERFLY is not set
+# CONFIG_AVRDUDE_PROG_AVR910 is not set
+# CONFIG_AVRDUDE_PROG_STK500 is not set
+# CONFIG_AVRDUDE_PROG_AVRISP is not set
+# CONFIG_AVRDUDE_PROG_BSD is not set
+# CONFIG_AVRDUDE_PROG_DAPA is not set
+# CONFIG_AVRDUDE_PROG_JTAG1 is not set
+# CONFIG_AVRDUDE_PROG_AVR109 is not set
+CONFIG_AVRDUDE_PORT="/dev/parport0"
+CONFIG_AVRDUDE_BAUDRATE=19200
+
+#
+# Avarice
+#
+CONFIG_AVARICE_PORT="/dev/ttyS0"
+CONFIG_AVARICE_DEBUG_PORT=1234
+CONFIG_AVARICE_PROG_MKI=y
+# CONFIG_AVARICE_PROG_MKII is not set
+# CONFIG_AVRDUDE_CHECK_SIGNATURE is not set
--- /dev/null
+#
+# Automatically generated by make menuconfig: don't edit
+#
+
+#
+# Hardware
+#
+# CONFIG_MCU_AT90S2313 is not set
+# CONFIG_MCU_AT90S2323 is not set
+# CONFIG_MCU_AT90S3333 is not set
+# CONFIG_MCU_AT90S2343 is not set
+# CONFIG_MCU_ATTINY22 is not set
+# CONFIG_MCU_ATTINY26 is not set
+# CONFIG_MCU_AT90S4414 is not set
+# CONFIG_MCU_AT90S4433 is not set
+# CONFIG_MCU_AT90S4434 is not set
+# CONFIG_MCU_AT90S8515 is not set
+# CONFIG_MCU_AT90S8534 is not set
+# CONFIG_MCU_AT90S8535 is not set
+# CONFIG_MCU_AT86RF401 is not set
+# CONFIG_MCU_ATMEGA103 is not set
+# CONFIG_MCU_ATMEGA603 is not set
+# CONFIG_MCU_AT43USB320 is not set
+# CONFIG_MCU_AT43USB355 is not set
+# CONFIG_MCU_AT76C711 is not set
+# CONFIG_MCU_ATMEGA8 is not set
+# CONFIG_MCU_ATMEGA48 is not set
+# CONFIG_MCU_ATMEGA88 is not set
+# CONFIG_MCU_ATMEGA8515 is not set
+# CONFIG_MCU_ATMEGA8535 is not set
+# CONFIG_MCU_ATTINY13 is not set
+# CONFIG_MCU_ATTINY2313 is not set
+# CONFIG_MCU_ATMEGA16 is not set
+# CONFIG_MCU_ATMEGA161 is not set
+# CONFIG_MCU_ATMEGA162 is not set
+# CONFIG_MCU_ATMEGA163 is not set
+# CONFIG_MCU_ATMEGA165 is not set
+# CONFIG_MCU_ATMEGA168 is not set
+# CONFIG_MCU_ATMEGA169 is not set
+# CONFIG_MCU_ATMEGA32 is not set
+# CONFIG_MCU_ATMEGA323 is not set
+# CONFIG_MCU_ATMEGA325 is not set
+# CONFIG_MCU_ATMEGA3250 is not set
+# CONFIG_MCU_ATMEGA64 is not set
+# CONFIG_MCU_ATMEGA645 is not set
+# CONFIG_MCU_ATMEGA6450 is not set
+CONFIG_MCU_ATMEGA128=y
+# CONFIG_MCU_ATMEGA1281 is not set
+# CONFIG_MCU_AT90CAN128 is not set
+# CONFIG_MCU_AT94K is not set
+# CONFIG_MCU_AT90S1200 is not set
+# CONFIG_MCU_ATMEGA2560 is not set
+# CONFIG_MCU_ATMEGA256 is not set
+CONFIG_QUARTZ=16000000
+
+#
+# Generation options
+#
+# CONFIG_OPTM_0 is not set
+# CONFIG_OPTM_1 is not set
+# CONFIG_OPTM_2 is not set
+# CONFIG_OPTM_3 is not set
+CONFIG_OPTM_S=y
+CONFIG_MATH_LIB=y
+# CONFIG_FDEVOPEN_COMPAT is not set
+# CONFIG_NO_PRINTF is not set
+# CONFIG_MINIMAL_PRINTF is not set
+CONFIG_STANDARD_PRINTF=y
+# CONFIG_ADVANCED_PRINTF is not set
+CONFIG_FORMAT_IHEX=y
+# CONFIG_FORMAT_SREC is not set
+# CONFIG_FORMAT_BINARY is not set
+
+#
+# Base modules
+#
+# CONFIG_MODULE_CIRBUF is not set
+# CONFIG_MODULE_CIRBUF_LARGE is not set
+# CONFIG_MODULE_FIXED_POINT is not set
+# CONFIG_MODULE_VECT2 is not set
+CONFIG_MODULE_GEOMETRY=y
+# CONFIG_MODULE_SCHEDULER is not set
+# CONFIG_MODULE_SCHEDULER_STATS is not set
+# CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set
+# CONFIG_MODULE_SCHEDULER_USE_TIMERS is not set
+CONFIG_MODULE_SCHEDULER_TIMER0=y
+# CONFIG_MODULE_SCHEDULER_MANUAL is not set
+# CONFIG_MODULE_TIME is not set
+# CONFIG_MODULE_TIME_CREATE_CONFIG is not set
+# CONFIG_MODULE_TIME_EXT is not set
+# CONFIG_MODULE_TIME_EXT_CREATE_CONFIG is not set
+
+#
+# Communication modules
+#
+# CONFIG_MODULE_UART is not set
+# CONFIG_MODULE_UART_9BITS is not set
+# CONFIG_MODULE_UART_CREATE_CONFIG is not set
+# CONFIG_MODULE_SPI is not set
+# CONFIG_MODULE_SPI_CREATE_CONFIG is not set
+# CONFIG_MODULE_I2C is not set
+# CONFIG_MODULE_I2C_MASTER is not set
+# CONFIG_MODULE_I2C_MULTIMASTER is not set
+# CONFIG_MODULE_I2C_CREATE_CONFIG is not set
+# CONFIG_MODULE_MF2_CLIENT is not set
+# CONFIG_MODULE_MF2_CLIENT_USE_SCHEDULER is not set
+# CONFIG_MODULE_MF2_CLIENT_CREATE_CONFIG is not set
+# CONFIG_MODULE_MF2_SERVER is not set
+# CONFIG_MODULE_MF2_SERVER_CREATE_CONFIG is not set
+
+#
+# Hardware modules
+#
+# CONFIG_MODULE_TIMER is not set
+# CONFIG_MODULE_TIMER_CREATE_CONFIG is not set
+# CONFIG_MODULE_TIMER_DYNAMIC is not set
+# CONFIG_MODULE_PWM is not set
+# CONFIG_MODULE_PWM_CREATE_CONFIG is not set
+# CONFIG_MODULE_PWM_NG is not set
+# CONFIG_MODULE_ADC is not set
+# CONFIG_MODULE_ADC_CREATE_CONFIG is not set
+
+#
+# IHM modules
+#
+# CONFIG_MODULE_MENU is not set
+# CONFIG_MODULE_VT100 is not set
+# CONFIG_MODULE_RDLINE is not set
+# CONFIG_MODULE_RDLINE_CREATE_CONFIG is not set
+# CONFIG_MODULE_RDLINE_KILL_BUF is not set
+# CONFIG_MODULE_RDLINE_HISTORY is not set
+# CONFIG_MODULE_PARSE is not set
+# CONFIG_MODULE_PARSE_NO_FLOAT is not set
+
+#
+# External devices modules
+#
+# CONFIG_MODULE_LCD is not set
+# CONFIG_MODULE_LCD_CREATE_CONFIG is not set
+# CONFIG_MODULE_MULTISERVO is not set
+# CONFIG_MODULE_MULTISERVO_CREATE_CONFIG is not set
+# CONFIG_MODULE_AX12 is not set
+# CONFIG_MODULE_AX12_CREATE_CONFIG is not set
+
+#
+# Brushless motor drivers (you should enable pwm modules to see all)
+#
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_CREATE_CONFIG is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE is not set
+# CONFIG_MODULE_BRUSHLESS_3PHASE_DIGITAL_HALL_DOUBLE_CREATE_CONFIG is not set
+
+#
+# Encoders (you need comm/spi for encoders_spi)
+#
+# CONFIG_MODULE_ENCODERS_MICROB is not set
+# CONFIG_MODULE_ENCODERS_MICROB_CREATE_CONFIG is not set
+# CONFIG_MODULE_ENCODERS_EIRBOT is not set
+# CONFIG_MODULE_ENCODERS_EIRBOT_CREATE_CONFIG is not set
+# CONFIG_MODULE_ENCODERS_SPI is not set
+# CONFIG_MODULE_ENCODERS_SPI_CREATE_CONFIG is not set
+
+#
+# Robot specific modules
+#
+# CONFIG_MODULE_ROBOT_SYSTEM is not set
+# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set
+# CONFIG_MODULE_POSITION_MANAGER is not set
+# CONFIG_MODULE_COMPENSATE_CENTRIFUGAL_FORCE is not set
+# CONFIG_MODULE_TRAJECTORY_MANAGER is not set
+# CONFIG_MODULE_BLOCKING_DETECTION_MANAGER is not set
+# CONFIG_MODULE_OBSTACLE_AVOIDANCE is not set
+# CONFIG_MODULE_OBSTACLE_AVOIDANCE_CREATE_CONFIG is not set
+
+#
+# Control system modules
+#
+# CONFIG_MODULE_CONTROL_SYSTEM_MANAGER is not set
+# CONFIG_MODULE_PID is not set
+# CONFIG_MODULE_PID_CREATE_CONFIG is not set
+# CONFIG_MODULE_RAMP is not set
+# CONFIG_MODULE_QUADRAMP is not set
+# CONFIG_MODULE_QUADRAMP_DERIVATE is not set
+# CONFIG_MODULE_BIQUAD is not set
+
+#
+# Radio devices
+#
+# CONFIG_MODULE_CC2420 is not set
+# CONFIG_MODULE_CC2420_CREATE_CONFIG is not set
+
+#
+# Crypto modules
+#
+# CONFIG_MODULE_AES is not set
+# CONFIG_MODULE_AES_CTR is not set
+# CONFIG_MODULE_MD5 is not set
+# CONFIG_MODULE_MD5_HMAC is not set
+# CONFIG_MODULE_RC4 is not set
+
+#
+# Encodings modules
+#
+# CONFIG_MODULE_BASE64 is not set
+# CONFIG_MODULE_HAMMING is not set
+
+#
+# Debug modules
+#
+# CONFIG_MODULE_DIAGNOSTIC is not set
+# CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG is not set
+CONFIG_MODULE_ERROR=y
+CONFIG_MODULE_ERROR_CREATE_CONFIG=y
+
+#
+# Programmer options
+#
+CONFIG_AVRDUDE=y
+# CONFIG_AVARICE is not set
+
+#
+# Avrdude
+#
+# CONFIG_AVRDUDE_PROG_FUTURELEC is not set
+# CONFIG_AVRDUDE_PROG_ABCMINI is not set
+# CONFIG_AVRDUDE_PROG_PICOWEB is not set
+# CONFIG_AVRDUDE_PROG_SP12 is not set
+# CONFIG_AVRDUDE_PROG_ALF is not set
+# CONFIG_AVRDUDE_PROG_BASCOM is not set
+# CONFIG_AVRDUDE_PROG_DT006 is not set
+# CONFIG_AVRDUDE_PROG_PONY_STK200 is not set
+CONFIG_AVRDUDE_PROG_STK200=y
+# CONFIG_AVRDUDE_PROG_PAVR is not set
+# CONFIG_AVRDUDE_PROG_BUTTERFLY is not set
+# CONFIG_AVRDUDE_PROG_AVR910 is not set
+# CONFIG_AVRDUDE_PROG_STK500 is not set
+# CONFIG_AVRDUDE_PROG_AVRISP is not set
+# CONFIG_AVRDUDE_PROG_BSD is not set
+# CONFIG_AVRDUDE_PROG_DAPA is not set
+# CONFIG_AVRDUDE_PROG_JTAG1 is not set
+# CONFIG_AVRDUDE_PROG_AVR109 is not set
+CONFIG_AVRDUDE_PORT="/dev/parport0"
+CONFIG_AVRDUDE_BAUDRATE=19200
+
+#
+# Avarice
+#
+CONFIG_AVARICE_PORT="/dev/ttyS0"
+CONFIG_AVARICE_DEBUG_PORT=1234
+CONFIG_AVARICE_PROG_MKI=y
+# CONFIG_AVARICE_PROG_MKII is not set
+# CONFIG_AVRDUDE_CHECK_SIGNATURE is not set
--- /dev/null
+TARGET = main
+
+# repertoire des modules
+AVERSIVE_DIR =../..# VALUE, absolute or relative path : example ../.. #
+
+# List C source files here. (C dependencies are automatically generated.)
+SRC = $(TARGET).c
+
+# List Assembler source files here.
+# Make them always end in a capital .S. Files ending in a lowercase .s
+# will not be considered source files but generated files (assembler
+# output from the compiler), and will be deleted upon "make clean"!
+# Even though the DOS/Win* filesystem matches both .s and .S the same,
+# it will preserve the spelling of the filenames, and gcc itself does
+# care about how the name is spelled on its command-line.
+ASRC =
+
+########################################
+
+-include .aversive_conf
+include $(AVERSIVE_DIR)/mk/aversive_project.mk
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ *
+ * This program 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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: error_config.h,v 1.4.6.1 2006-11-26 21:06:03 zer0 Exp $
+ *
+ */
+
+#ifndef _ERROR_CONFIG_
+#define _ERROR_CONFIG_
+
+/** enable the dump of the comment */
+#define ERROR_DUMP_TEXTLOG
+
+/** enable the dump of filename and line number */
+#define ERROR_DUMP_FILE_LINE
+
+#endif
--- /dev/null
+import sys, re, math
+import numpy as np
+import matplotlib
+import matplotlib.path as mpath
+import matplotlib.patches as mpatches
+import matplotlib.pyplot as plt
+from matplotlib.patches import Arrow, Circle, Wedge, Polygon
+from matplotlib.collections import PatchCollection
+from numpy.random import randn
+import pylab
+import popen2, random
+
+Path = mpath.Path
+FLOAT = "([-+]?[0-9]*\.?[0-9]+)"
+INT = "([-+]?[0-9][0-9]*)"
+RANDOM_ERROR = 0.3 # deg
+beacons = [ (0.0, 1050.0), (3000.0, 0.0), (3000.0, 2100.0) ]
+
+def build_poly(ptlist):
+ polydata = []
+ polydata.append((Path.MOVETO, (ptlist[0])))
+ for pt in ptlist[1:]:
+ polydata.append((Path.LINETO, (pt)))
+ polydata.append((Path.CLOSEPOLY, (ptlist[0])))
+ codes, verts = zip(*polydata)
+ poly = mpath.Path(verts, codes)
+ x, y = zip(*poly.vertices)
+ return x,y
+
+def build_path(ptlist):
+ polydata = []
+ polydata.append((Path.MOVETO, (ptlist[0])))
+ for pt in ptlist[1:]:
+ polydata.append((Path.LINETO, (pt)))
+ codes, verts = zip(*polydata)
+ poly = mpath.Path(verts, codes)
+ x, y = zip(*poly.vertices)
+ return x,y
+
+def get_angle(ref, b):
+ """get angle from robot point of view (ref) of beacon 'b'"""
+ a = math.atan2(b[1]-ref[1], b[0]-ref[0])
+ ea = (math.pi/180.) * RANDOM_ERROR * random.random()
+ ea = random.choice([ea, -ea])
+ return a + ea, ea
+
+ alpha = math.atan2(a[1]-ref[1], a[0]-ref[0])
+ beta = math.atan2(b[1]-ref[1], b[0]-ref[0])
+ gamma = beta-alpha
+ if gamma < 0:
+ gamma = gamma + 2*math.pi
+ return gamma + error, error
+
+def dist(p1, p2):
+ return math.sqrt((p1[0]-p2[0])**2+(p1[1]-p2[1])**2)
+
+def graph(filename, real_x, real_y):
+
+ real_pt = (real_x, real_y)
+
+ # display beacons
+ patches = []
+ for b in beacons:
+ patches += [ Circle((b[0], b[1]), 40, alpha=0.4) ]
+
+ patches += [ Circle((real_x, real_y), 20, alpha=0.4, facecolor="red") ]
+
+ # process angles from robot position
+ a0,ea0 = get_angle((real_x, real_y), beacons[0])
+ a1,ea1 = get_angle((real_x, real_y), beacons[1])
+ a2,ea2 = get_angle((real_x, real_y), beacons[2])
+ text = "a0 = %2.2f (%+2.2f deg)\n"%(a0, ea0*(180./math.pi))
+ text += "a1 = %2.2f (%+2.2f deg)\n"%(a1, ea1*(180./math.pi))
+ text += "a2 = %2.2f (%+2.2f deg)\n"%(a2, ea2*(180./math.pi))
+
+ a01 = a1-a0
+ if a01 < 0:
+ a01 += 2*math.pi
+ a12 = a2-a1
+ if a12 < 0:
+ a12 += 2*math.pi
+ a20 = a0-a2
+ if a20 < 0:
+ a20 += 2*math.pi
+
+ cmd = "./main angle2pos %f %f %f"%(a01, a12, a20)
+ o,i = popen2.popen2(cmd)
+ i.close()
+ s = o.read(1000000)
+ o.close()
+
+ open(filename + ".txt", "w").write(s)
+
+ if len(s) == 1000000:
+ gloupix()
+
+ fig = plt.figure()
+ ax = fig.add_subplot(111)
+ ax.set_title("Erreur de position en mm lorsqu'on ajoute une erreur de mesure\n"
+ "d'angle aleatoire comprise entre - %1.1f et %1.1f deg"%(RANDOM_ERROR,
+ RANDOM_ERROR))
+
+ # area
+ x,y = build_poly([(0,0), (3000,0), (3000,2100), (0,2100)])
+ ax.plot(x, y, 'g-')
+
+ for l in s.split("\n"):
+ m = re.match("circle: x=%s y=%s r=%s"%(FLOAT, FLOAT, FLOAT), l)
+ if m:
+ x,y,r = (float(m.groups()[0]), float(m.groups()[1]), float(m.groups()[2]))
+ print x,y,r
+ patches += [ Circle((x, y), r, facecolor="none") ]
+ m = re.match("p%s: x=%s y=%s"%(INT, FLOAT, FLOAT), l)
+ if m:
+ n,x,y = (float(m.groups()[0]), float(m.groups()[1]), float(m.groups()[2]))
+ if (n == 0):
+ patches += [ Circle((x, y), 20, alpha=0.4, facecolor="yellow") ]
+ result_pt = (x, y)
+ text += l + "\n"
+
+ p = PatchCollection(patches, cmap=matplotlib.cm.jet, match_original=True)
+
+ # text area, far from the point
+ l = [(800., 1800.), (800., 500.), (1500., 1800.), (1500., 500.),
+ (2200., 1800.), (2200., 500.)]
+ l.sort(cmp=lambda p1,p2: (dist(p1,real_pt)<dist(p2,real_pt)) and 1 or -1)
+ x,y = l[0]
+ text += "real_pt: x=%2.2f, y=%2.2f\n"%(real_x, real_y)
+ text += "error = %2.2f mm"%(dist(real_pt, result_pt))
+ matplotlib.pyplot.text(x, y, text, size=8,
+ ha="center", va="center",
+ bbox = dict(boxstyle="round",
+ ec=(1., 0.5, 0.5),
+ fc=(1., 0.8, 0.8),
+ alpha=0.6,
+ ),
+ alpha=0.8
+ )
+ ax.add_collection(p)
+
+ ax.grid()
+ ax.set_xlim(-100, 3100)
+ ax.set_ylim(-100, 2200)
+ #ax.set_title('spline paths')
+ #plt.show()
+ fig.savefig(filename)
+
+def do_random_test():
+ random.seed(0)
+ for i in range(100):
+ x = random.randint(0, 3000)
+ y = random.randint(0, 2100)
+ graph("test%d.png"%i, x, y)
+
+def do_graph_2d(data, filename, title):
+ # Make plot with vertical (default) colorbar
+ fig = plt.figure()
+ ax = fig.add_subplot(111)
+
+ cax = ax.imshow(data)
+ ax.set_title(title)
+
+ # Add colorbar, make sure to specify tick locations to match desired ticklabels
+ cbar = fig.colorbar(cax, ticks=[0, 50])
+ cbar.ax.set_yticklabels(['0', '50 et plus'])# vertically oriented colorbar
+ fig.savefig(filename)
+
+def get_data(cmd, sat=0):
+ data = np.array([[0.]*210]*300)
+ oo,ii = popen2.popen2(cmd)
+ ii.close()
+ while True:
+ l = oo.readline()
+ if l == "":
+ break
+ try:
+ x,y,e = l[:-1].split(" ")
+ except:
+ print "Fail: %s"%(l)
+ continue
+ x = int(x)
+ y = int(y)
+ e = float(e)
+ if sat:
+ if e < sat:
+ e = 0
+ else:
+ e = sat
+ data[x,y] = e
+ oo.close()
+ return data
+
+def do_graph_2d_simple_error():
+ for i in range(4):
+ for j in ["0.1", "0.5", "1.0"]:
+ data = get_data("./main simple_error %d %s"%(i,j))
+ if i != 3:
+ title = 'Erreur de position en mm, pour une erreur\n'
+ title += 'de mesure de %s deg sur la balise %d'%(j,i)
+ else:
+ title = 'Erreur de position en mm, pour une erreur\n'
+ title += 'de mesure de %s deg sur les 3 balises'%(j)
+ do_graph_2d(data, "error_a%d_%s.png"%(i,j), title)
+
+def do_graph_2d_move_error():
+ i = 0
+ for period in [ 20, 40 ]:
+ for speed in [ 0.3, 0.7, 1. ]:
+ angle_deg = 0
+ while angle_deg < 360:
+ angle_rad = angle_deg * (math.pi/180.)
+ data = get_data("./main move_error %f %f %f"%(speed, period, angle_rad))
+ do_graph_2d(data, "error_move_error_%d.png"%(i),
+ 'Erreur de mesure si le robot se deplace a %2.2f m/s\n'
+ 'vers %d deg (periode tourelle = %d ms)'%(speed, angle_deg, period))
+ angle_deg += 45
+ i += 1
+ period = 20
+ speed = 1.
+ angle_deg = 45
+ angle_rad = angle_deg * (math.pi/180.)
+ data = get_data("./main move_error %f %f %f"%(speed, period, angle_rad), sat=20)
+ do_graph_2d(data, "error_move_error_%d.png"%(i),
+ "En rouge, l'erreur de mesure est > 2cm (pour un deplacement\n"
+ 'a %2.2f m/s vers %d deg et une periode tourelle = %d ms)'%(speed, angle_deg, period))
+
+#do_random_test()
+#do_graph_2d_simple_error()
+do_graph_2d_move_error()
--- /dev/null
+/*
+ * Copyright Droids Corporation (2009)
+ *
+ * This program 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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: f16.h,v 1.6.4.3 2008-05-10 15:06:26 zer0 Exp $
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <aversive.h>
+
+#include <vect_base.h>
+#include <lines.h>
+#include <circles.h>
+
+#define POS_ACCURACY 10.0 /* 1 cm accuracy max */
+#ifndef HOST_VERSION
+#define printf(args...) do {} while(0)
+#endif
+
+static int dprint = 0;
+#define dprintf(args...) if (dprint) printf(args)
+
+const point_t beacon0 = { 0, 1050 };
+const point_t beacon1 = { 3000, 0 };
+const point_t beacon2 = { 3000, 2100 };
+
+/* Fill the 2 circles pointer given as parameter, each of those cross
+ * both beacons b1 and b2. From any point of these circles (except b1
+ * and b2), we see b1 and b2 with the angle of a_rad (which must be
+ * positive). Return 0 on success.
+ *
+ * l
+ * <------------------------->
+ *
+ * b1 O b2
+ * +----------------------------+
+ * ,' \\ | /|'\
+ * / \ \ | ^ / | `
+ * / \ \ a___ | | d / | `.
+ * / \ \ / | v / | \
+ * | \ \ | / | |
+ * | \ \ | / | |
+ * | \ \|/ | |
+ * | \ * C | |
+ * | \ | .'
+ * | \ | |
+ * | \ | .'
+ * \ \ a____| /
+ * \ \ / | ,'
+ * ` \ | /
+ * '. \ | ,'
+ * '-. \ |_,'
+ * '-._ _,*'
+ * '`--......---' R (the robot)
+ *
+ */
+int8_t angle_to_circles(circle_t *c1, circle_t *c2,
+ const point_t *b1, const point_t *b2,
+ double a_rad)
+{
+ point_t O;
+ vect_t v;
+ float l, d;
+
+ /* reject negative or too small angles */
+ if (a_rad <= 0.01)
+ return -1;
+
+ /* get position of O */
+ O.x = (b1->x + b2->x) / 2;
+ O.y = (b1->y + b2->y) / 2;
+
+ /* get the length l */
+ v.x = b2->x - b1->x;
+ v.y = b2->y - b1->y;
+ l = vect_norm(&v);
+
+ /* distance from O to the center of the circle */
+ /* XXX div by 0 when pi */
+ d = l / (2 * tan(a_rad));
+
+ /* get the circle c1 */
+ vect_rot_trigo(&v);
+ vect_resize(&v, d);
+ if (c1) {
+ c1->x = O.x + v.x;
+ c1->y = O.y + v.y;
+ c1->r = norm(b1->x, b1->y, c1->x, c1->y);
+ }
+
+ /* get the circle c2 */
+ if (c2) {
+ c2->x = O.x - v.x;
+ c2->y = O.y - v.y;
+ c2->r = norm(b1->x, b1->y, c1->x, c1->y);
+ }
+
+ return 0;
+}
+
+/* get the position of the robot from the angle of the 3 beacons */
+int8_t angles_to_posxy(point_t *pos, double a01, double a12, double a20)
+{
+ circle_t c01, c12, c20;
+ point_t dummy_pt, p1, p2, p3;
+
+ dprintf("a01 = %2.2f\n", a01);
+ dprintf("a12 = %2.2f\n", a12);
+ dprintf("a20 = %2.2f\n", a20);
+
+ if (angle_to_circles(&c01, NULL, &beacon0, &beacon1, a01))
+ return -1;
+ dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c01.x, c01.y, c01.r);
+
+ if (angle_to_circles(&c12, NULL, &beacon1, &beacon2, a12))
+ return -1;
+ dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c12.x, c12.y, c12.r);
+
+ if (angle_to_circles(&c20, NULL, &beacon2, &beacon0, a20))
+ return -1;
+ dprintf("circle: x=%2.2f y=%2.2f r=%2.2f\n", c20.x, c20.y, c20.r);
+
+ if (circle_intersect(&c01, &c12, &p1, &dummy_pt) == 0)
+ return -1;
+ if (circle_intersect(&c12, &c20, &p2, &dummy_pt) == 0)
+ return -1;
+ if (circle_intersect(&c20, &c01, &dummy_pt, &p3) == 0)
+ return -1;
+
+ dprintf("p1: x=%2.2f y=%2.2f\n", p1.x, p1.y);
+ dprintf("p2: x=%2.2f y=%2.2f\n", p2.x, p2.y);
+ dprintf("p3: x=%2.2f y=%2.2f\n", p3.x, p3.y);
+
+ /* if (norm(p1.x, p1.y, p2.x, p2.y) > POS_ACCURACY || */
+ /* norm(p2.x, p2.y, p3.x, p3.y) > POS_ACCURACY || */
+ /* norm(p3.x, p3.y, p1.x, p1.y) > POS_ACCURACY) */
+ /* return -1; */
+
+ pos->x = (p1.x + p2.x + p3.x) / 3.0;
+ pos->y = (p1.y + p2.y + p3.y) / 3.0;
+
+ return 0;
+}
+
+/* get the angles of beacons from xy pos */
+int8_t posxy_to_angles(point_t pos, double *a01, double *a12,
+ double *a20, int err_num, float err_val)
+{
+ double a0, a1, a2;
+
+ a0 = atan2(beacon0.y-pos.y, beacon0.x-pos.x);
+ a1 = atan2(beacon1.y-pos.y, beacon1.x-pos.x);
+ a2 = atan2(beacon2.y-pos.y, beacon2.x-pos.x);
+
+ if (err_num == 0 || err_num == 3)
+ a0 += (err_val * M_PI/180.);
+ if (err_num == 1 || err_num == 3)
+ a1 += (err_val * M_PI/180.);
+ if (err_num == 2 || err_num == 3)
+ a2 += (err_val * M_PI/180.);
+
+ *a01 = a1-a0;
+ if (*a01 < 0)
+ *a01 += M_PI*2;
+ *a12 = a2-a1;
+ if (*a12 < 0)
+ *a12 += M_PI*2;
+ *a20 = a0-a2;
+ if (*a20 < 0)
+ *a20 += M_PI*2;
+
+ return 0;
+}
+
+int8_t process_move_error(double x, double y, double speed,
+ double period, double angle, double *err)
+{
+ double a01, a12, a20;
+ point_t pos, tmp;
+ double a0, a1, a2;
+ vect_t u,v;
+ point_t pos2, pos3;
+
+ pos.x = x;
+ pos.y = y;
+
+ /* from start to destination */
+ v.x = cos(angle) * speed * period;
+ v.y = sin(angle) * speed * period;
+
+ /* first process real pos */
+ posxy_to_angles(pos, &a01, &a12, &a20, -1, 0);
+
+ /* vector covered during measure of a0 and a1 */
+ u.x = v.x * a01 / (2*M_PI);
+ u.y = v.y * a01 / (2*M_PI);
+ pos2.x = pos.x + u.x;
+ pos2.y = pos.y + u.y;
+
+ /* vector covered during measure of a1 and a2 */
+ u.x = v.x * a12 / (2*M_PI);
+ u.y = v.y * a12 / (2*M_PI);
+ pos3.x = pos2.x + u.x;
+ pos3.y = pos2.y + u.y;
+
+ dprintf("p0: x=%2.2f y=%2.2f\n", pos.x, pos.y);
+ dprintf("p1: x=%2.2f y=%2.2f\n", pos2.x, pos2.y);
+ dprintf("p2: x=%2.2f y=%2.2f\n", pos3.x, pos3.y);
+
+ a0 = atan2(beacon0.y-pos.y, beacon0.x-pos.x);
+ a1 = atan2(beacon1.y-pos2.y, beacon1.x-pos2.x);
+ a2 = atan2(beacon2.y-pos3.y, beacon2.x-pos3.x);
+
+ a01 = a1-a0;
+ if (a01 < 0)
+ a01 += M_PI*2;
+ a12 = a2-a1;
+ if (a12 < 0)
+ a12 += M_PI*2;
+ a20 = a0-a2;
+ if (a20 < 0)
+ a20 += M_PI*2;
+
+ if (angles_to_posxy(&tmp, a01, a12, a20))
+ return -1;
+ *err = pt_norm(&tmp, &pos);
+ if (*err > 50.) /* saturate error to 5cm */
+ *err = 50.;
+ return 0;
+}
+
+/* whole process is around 3ms on atmega128 at 16Mhz */
+int main(int argc, char **argv)
+{
+ double a01, a12, a20;
+ point_t pos, tmp;
+ const char *mode = "nothing";
+
+#ifdef HOST_VERSION
+ if (argc < 2) {
+ printf("bad args\n");
+ return -1;
+ }
+ mode = argv[1];
+#else
+ mode = "angle2pos";
+ argc = 5;
+ a01 = 1.65;
+ a12 = 2.12;
+ a20 = 2.53;
+#endif
+
+ if (argc == 5 && strcmp(mode, "angle2pos") == 0) {
+#ifdef HOST_VERSION
+ dprint = 1;
+ a01 = atof(argv[2]);
+ a12 = atof(argv[3]);
+ a20 = atof(argv[4]);
+#endif
+ if (angles_to_posxy(&pos, a01, a12, a20) < 0)
+ return -1;
+ printf("p0: x=%2.2f y=%2.2f\n", pos.x, pos.y);
+ return 0;
+ }
+
+ if (argc == 4 && strcmp(mode, "simple_error") == 0) {
+ int x, y;
+ int err_num;
+ double err_val_deg;
+ double err;
+
+ err_num = atof(argv[2]); /* which beacon sees an error */
+ err_val_deg = atof(argv[3]); /* how many degrees of error */
+
+ for (x=0; x<300; x++) {
+ for (y=0; y<210; y++) {
+ pos.x = x*10;
+ pos.y = y*10;
+ posxy_to_angles(pos, &a01, &a12, &a20,
+ err_num, err_val_deg);
+ if (angles_to_posxy(&tmp, a01, a12, a20))
+ continue;
+ err = pt_norm(&tmp, &pos);
+ if (err > 50.) /* saturate error to 5cm */
+ err = 50.;
+ printf("%d %d %2.2f\n", x, y, err);
+ }
+ }
+ return 0;
+ }
+
+ if ((argc == 5 || argc == 7)
+ && strcmp(argv[1], "move_error") == 0) {
+ int x, y;
+ double angle, speed, period, err;
+
+ speed = atof(argv[2]); /* speed in m/s ( = mm/ms) */
+ period = atof(argv[3]); /* period of turret in ms */
+ angle = atof(argv[4]); /* direction of moving */
+ if (argc == 7) {
+ dprint = 1;
+ process_move_error(atof(argv[5]), atof(argv[6]),
+ speed, period, angle, &err);
+ printf("%2.2f %2.2f %2.2f\n", atof(argv[5]),
+ atof(argv[6]), err);
+ return 0;
+ }
+
+ for (x=0; x<300; x++) {
+ for (y=0; y<210; y++) {
+ pos.x = x*10;
+ pos.y = y*10;
+ if (process_move_error(pos.x, pos.y,
+ speed, period, angle,
+ &err) < 0)
+ continue;
+ printf("%d %d %2.2f\n", x, y, err);
+ }
+ }
+ return 0;
+ }
+
+ printf("bad args\n");
+ return -1;
+}
git.droids-corp.org / aversive.git / commitdiff