Commit from zer0 on branch b_zer0 (2007-12-04 10:28 CET)
=================================
don't erase eeprom when programming
aversive mk/aversive_project.mk 1.32.4.8
===============================
aversive/mk/aversive_project.mk (1.32.4.7 -> 1.32.4.8)
===============================
@@ -128,7 +128,7 @@
#AVRDUDE_FLAGS += -v -v
-AVARICE_WRITE_FLASH = --erase --program --file $(TARGET).$(FORMAT_EXTENSION)
+AVARICE_WRITE_FLASH = --program --file $(TARGET).$(FORMAT_EXTENSION)
#AVARICE_WRITE_EEPROM = XXX
export AVARICE_FLAGS = -P $(MCU) --jtag $(AVARICE_PORT) --$(AVARICE_PROGRAMMER)
Commit from zer0 on branch b_zer0 (2007-12-04 10:29 CET)
=================================
pid maxmiums are uint32_t
aversive modules/devices/control_system/filters/pid/pid.c 1.5.4.3
aversive modules/devices/control_system/filters/pid/pid.h 1.4.4.4
=========================================================
aversive/modules/devices/control_system/filters/pid/pid.c (1.5.4.2 -> 1.5.4.3)
=========================================================
@@ -15,7 +15,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: pid.c,v 1.5.4.2 2007-05-12 16:42:39 zer0 Exp $
+ * Revision : $Id: pid.c,v 1.5.4.3 2007-12-04 09:29:25 zer0 Exp $
*
*/
@@ -77,17 +77,17 @@
}
-int16_t pid_get_max_in(struct pid_filter *p)
+int32_t pid_get_max_in(struct pid_filter *p)
{
return (p->max_in);
}
-int16_t pid_get_max_I(struct pid_filter *p)
+int32_t pid_get_max_I(struct pid_filter *p)
{
return (p->max_I);
}
-int16_t pid_get_max_out(struct pid_filter *p)
+int32_t pid_get_max_out(struct pid_filter *p)
{
return (p->max_out);
}
=========================================================
aversive/modules/devices/control_system/filters/pid/pid.h (1.4.4.3 -> 1.4.4.4)
=========================================================
@@ -15,7 +15,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: pid.h,v 1.4.4.3 2007-05-23 17:18:13 zer0 Exp $
+ * Revision : $Id: pid.h,v 1.4.4.4 2007-12-04 09:29:25 zer0 Exp $
*
*/
@@ -57,9 +57,9 @@
int16_t pid_get_gain_P(struct pid_filter *p);
int16_t pid_get_gain_I(struct pid_filter *p);
int16_t pid_get_gain_D(struct pid_filter *p);
-int16_t pid_get_max_in(struct pid_filter *p);
-int16_t pid_get_max_I(struct pid_filter *p);
-int16_t pid_get_max_out(struct pid_filter *p);
+int32_t pid_get_max_in(struct pid_filter *p);
+int32_t pid_get_max_I(struct pid_filter *p);
+int32_t pid_get_max_out(struct pid_filter *p);
uint8_t pid_get_out_shift(struct pid_filter *p);
int32_t pid_get_value_I(struct pid_filter *p);
Commit from zer0 on branch b_zer0 (2007-12-04 10:30 CET)
=================================
fix bug in turn_to_xy()
update interface in .h
aversive modules/devices/robot/trajectory_manager/trajectory_manager.c
1.4.4.6
aversive modules/devices/robot/trajectory_manager/trajectory_manager.h
1.4.4.4
======================================================================
aversive/modules/devices/robot/trajectory_manager/trajectory_manager.c
(1.4.4.5 -> 1.4.4.6)
======================================================================
@@ -15,7 +15,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: trajectory_manager.c,v 1.4.4.5 2007-06-07 12:22:52 zer0
Exp $
+ * Revision : $Id: trajectory_manager.c,v 1.4.4.6 2007-12-04 09:30:58 zer0
Exp $
*
*/
@@ -203,7 +203,13 @@
void __trajectory_goto_d_a_rel(struct trajectory* traj, double d_cm, double
a_rad, uint8_t flags)
{
int32_t a_consign, d_consign;
- DEBUG(E_TRAJECTORY, "Goto DA/RS rel");
+ /* XXX only used for debug */
+ double posx = position_get_x_double(traj->position);
+ double posy = position_get_y_double(traj->position);
+ double posa = position_get_a_rad_double(traj->position);
+
+ DEBUG(E_TRAJECTORY, "Goto DA/RS rel to d=%f a_rad=%f", d_cm, a_rad);
+ DEBUG(E_TRAJECTORY, "current pos = x=%f, y=%f, a_rad=%f", posx, posy,
posa);
delete_event(traj);
set_quadramp_speed(traj, traj->d_speed, traj->a_speed);
if (flags & UPDATE_A) {
@@ -255,9 +261,11 @@
{
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);
traj->state = RUNNING_A;
- __trajectory_goto_d_a_rel(traj, 0, atan2(y_abs_cm - posy, x_abs_cm -
posx),
+ DEBUG(E_TRAJECTORY, "Goto Turn To xy %f %f", x_abs_cm, y_abs_cm);
+ __trajectory_goto_d_a_rel(traj, 0, simple_modulo_2pi(atan2(y_abs_cm -
posy, x_abs_cm - posx) - posa),
UPDATE_A | UPDATE_D | RESET_D);
}
@@ -340,7 +348,7 @@
set_quadramp_speed(traj, traj->d_speed, traj->a_speed);
delete_event(traj);
p.r = d;
- p.theta = (a * M_PI / 180.0) +
position_get_a_rad_double(traj->position);;
+ p.theta = (a * M_PI / 180.0) +
position_get_a_rad_double(traj->position);
vect2_pol2cart(&p, &traj->target.cart);
traj->target.cart.x += x;
traj->target.cart.y += y;
======================================================================
aversive/modules/devices/robot/trajectory_manager/trajectory_manager.h
(1.4.4.3 -> 1.4.4.4)
======================================================================
@@ -15,7 +15,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: trajectory_manager.h,v 1.4.4.3 2007-06-01 09:37:22 zer0
Exp $
+ * Revision : $Id: trajectory_manager.h,v 1.4.4.4 2007-12-04 09:30:58 zer0
Exp $
*
*/
@@ -66,70 +66,63 @@
void trajectory_init(struct trajectory * traj);
/** structure initialization */
-void trajectory_set_cs(struct trajectory * traj, struct cs * cs_d, struct cs *
cs_a);
+void trajectory_set_cs(struct trajectory * traj, struct cs * cs_d,
+ struct cs * 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) ;
-/** initialisation of target windows */
-void trajectory_set_windows(struct trajectory * traj, double d_win, double
a_win_deg);
-
-/** go to (x,y,a) */
-void trajectory_goto_xya( struct trajectory * traj, double x, double y, double
a );
-
-/** go to (x,y) */
-void trajectory_goto_xy( struct trajectory * traj, double x, double y );
+/** set speed consign */
+void trajectory_set_speed(struct trajectory * traj, int16_t d_speed, int16_t
a_speed);
-/** go to (x,y,a) */
-void trajectory_goto_forward_xya( struct trajectory * traj, double x, double
y, double a );
-
-/** go to (x,y) */
-void trajectory_goto_forward_xy( struct trajectory * traj, double x, double y
);
-
-void trajectory_turnto_xy( struct trajectory* traj, double tx, double ty );
+/** set windows for trajectory */
+void trajectory_set_windows(struct trajectory * traj, double d_win, double
a_win_deg);
-/** go to (a) */
-void trajectory_goto_a( struct trajectory * traj, double a );
+/** go straight forward (d is in cm) */
+void trajectory_d_rel(struct trajectory * traj, double d_cm);
-/* set speed */
-void trajectory_set_speed( struct trajectory * traj, int16_t speed_d, int16_t
speed_a);
+/** update distance consign without changing angle consign */
+void trajectory_only_d_rel(struct trajectory * traj, double d_cm);
-/** go to (d,a) relative */
-/* ?? useful ?? */
-void trajectory_goto_rs_d_a_rel( struct trajectory * traj, double d, double a
);
+/** turn by 'a' degrees */
+void trajectory_a_rel(struct trajectory * traj, double a_deg);
-/** go to (d,a) relative */
-void trajectory_goto_d_a_rel( struct trajectory * traj, double d, double a );
+/** turn the robot until the point x,y is in front of us */
+void trajectory_turnto_xy(struct trajectory* traj, double x_abs_cm, double
y_abs_cm);
-/** go to (x,y) relative */
-void trajectory_goto_xy_rel( struct trajectory* traj, double tx, double ty );
+/** update angle consign without changing distance consign */
+void trajectory_only_a_rel(struct trajectory * traj, double a_deg);
-/** go to (a) relative */
-void trajectory_goto_a_rel( struct trajectory * traj, double a );
+/** turn by 'a' degrees */
+void trajectory_d_a_rel(struct trajectory * traj, double d_cm, double a_deg);
-/* speed control system.... beta test */
-void trajectory_speed_d_a( struct trajectory* traj, int16_t speed_d, int16_t
speed_a, int32_t d );
+/** set relative angle and distance consign to 0 */
+void trajectory_stop(struct trajectory * traj);
-/** only change d */
-void trajectory_only_d( struct trajectory * traj, double d );
+/** set relative angle and distance consign to 0, and break any
+ * deceleration ramp in quadramp filter */
+void trajectory_hardstop(struct trajectory * traj);
-/** only change a */
-void trajectory_only_a( struct trajectory * traj, double a );
+/** goto a x,y point, using a trajectory event */
+void trajectory_goto_xy_abs(struct trajectory * traj, double x, double y);
-/** stop robot */
-void trajectory_stop( struct trajectory * 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);
-/** stop robot, but better */
-void trajectory_hardstop( struct trajectory * 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);
-/** go to (d) relative */
-void trajectory_straight( struct trajectory * traj, double d );
+/** go forward to a x,y relative point, using a trajectory event */
+void trajectory_goto_xy_rel(struct trajectory* traj, double x_rel_cm, double
y_rel_cm);
-/* return 1 if trajectory is finished */
-uint8_t trajectory_finished( struct trajectory* traj);
+/** 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 1 if trajectory is "nearly" (hum) finished */
-uint8_t trajectory_nearly_finished(struct trajectory* traj);
+/** return true if traj is nearly finished */
+uint8_t trajectory_in_window(struct trajectory* traj);
#endif //TRAJECTORY_MANAGER
Commit from zer0 on branch b_zer0 (2007-12-04 10:31 CET)
=================================
update defaults values in rdline.h, but it will go in a config file in
the future.
aversive modules/ihm/rdline/rdline.h 1.1.2.4
====================================
aversive/modules/ihm/rdline/rdline.h (1.1.2.3 -> 1.1.2.4)
====================================
@@ -16,7 +16,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: rdline.h,v 1.1.2.3 2007-11-21 21:54:39 zer0 Exp $
+ * Revision : $Id: rdline.h,v 1.1.2.4 2007-12-04 09:31:45 zer0 Exp $
*
*
*/
@@ -77,11 +77,11 @@
#define vt100_word_right "\033\146"
/* configuration */
-#define RDLINE_BUF_SIZE 32
+#define RDLINE_BUF_SIZE 64
#define RDLINE_PROMPT_SIZE 16
-#define RDLINE_VT100_BUF_SIZE 16
+#define RDLINE_VT100_BUF_SIZE 8
#define RDLINE_HISTORY_BUF_SIZE 128
-#define RDLINE_HISTORY_MAX_LINE 32
+#define RDLINE_HISTORY_MAX_LINE 64
enum vt100_parser_state {
VT100_INIT,
Commit from zer0 (2007-12-04 10:38 CET)
================
Start control system debugging.
- Add profiling ability in code
- Control system debugging can be activated in live
- Add a python script to display control system curves
- New commands to set/get gains
- Start a function to store configuration in eeprom
- Use new trajectory manager
aversive_projects microb2008/main/.config 1.9
aversive_projects microb2008/main/Makefile 1.4
aversive_projects microb2008/main/commands.c 1.5
+ aversive_projects microb2008/main/eeprom.c 1.1
+ aversive_projects microb2008/main/eeprom.h 1.1
aversive_projects microb2008/main/main.c 1.10
aversive_projects microb2008/main/main.h 1.5
+ aversive_projects microb2008/main/parse_cs_debug.py 1.1
aversive_projects microb2008/main/pwm_config.h 1.3
aversive_projects microb2008/main/scheduler_config.h 1.3
aversive_projects microb2008/main/time_config.h 1.2
aversive_projects microb2008/main/uart_config.h 1.2
=========================================
aversive_projects/microb2008/main/.config (1.8 -> 1.9)
=========================================
@@ -208,7 +208,7 @@
# CONFIG_AVRDUDE_PROG_BSD is not set
# CONFIG_AVRDUDE_PROG_DAPA is not set
# CONFIG_AVRDUDE_PROG_JTAG1 is not set
-CONFIG_AVRDUDE_PORT="/dev/parport0"
+CONFIG_AVRDUDE_PORT="/dev/.static/dev/parport0"
#
# Avarice
==========================================
aversive_projects/microb2008/main/Makefile (1.3 -> 1.4)
==========================================
@@ -7,7 +7,7 @@
# List C source files here. (C dependencies are automatically generated.)
SRC = $(TARGET).c cam.c test.c action.c sensor.c i2c_protocol.c \
-strat_base.c strat.c commands.c
+strat_base.c strat.c commands.c eeprom.c
# List Assembler source files here.
# Make them always end in a capital .S. Files ending in a lowercase .s
============================================
aversive_projects/microb2008/main/commands.c (1.4 -> 1.5)
============================================
@@ -8,11 +8,14 @@
#include <pwm.h>
#include <encoders_microb.h>
#include <uart.h>
+#include <scheduler.h>
#include <parse.h>
#include <parse_num.h>
#include <parse_string.h>
+#include <trajectory_manager.h>
#include "main.h"
+#include "eeprom.h"
//#include "test.h"
#include "sensor.h"
@@ -206,6 +209,147 @@
};
+/**********************************************************/
+/* Speeds for control system */
+
+/* this structure is filled when cmd_speed is parsed successfully */
+struct cmd_speed_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint16_t s;
+};
+
+/* function called when cmd_speed is parsed successfully */
+static void cmd_speed_parsed(void * parsed_result, void * data)
+{
+ struct cmd_speed_result * res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("angle"))) {
+ trajectory_set_speed(&robot.traj, robot.traj.d_speed, res->s);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("distance"))) {
+ trajectory_set_speed(&robot.traj, res->s, robot.traj.a_speed);
+ }
+ /* else it is a "show" */
+
+ printf_P(PSTR("angle %u, distance %u\n"),
+ robot.traj.a_speed,
+ robot.traj.d_speed);
+}
+
+prog_char str_speed_arg0[] = "speed";
+parse_pgm_token_string_t cmd_speed_arg0 = TOKEN_STRING_INITIALIZER(struct
cmd_speed_result, arg0, str_speed_arg0);
+prog_char str_speed_arg1[] = "angle#distance";
+parse_pgm_token_string_t cmd_speed_arg1 = TOKEN_STRING_INITIALIZER(struct
cmd_speed_result, arg1, str_speed_arg1);
+parse_pgm_token_num_t cmd_speed_s = TOKEN_NUM_INITIALIZER(struct
cmd_speed_result, s, UINT16);
+
+prog_char help_speed[] = "Set speed values for trajectory manager";
+parse_pgm_inst_t cmd_speed = {
+ .f = cmd_speed_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_speed,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_speed_arg0,
+ (prog_void *)&cmd_speed_arg1,
+ (prog_void *)&cmd_speed_s,
+ NULL,
+ },
+};
+
+/* show */
+
+prog_char str_speed_show_arg[] = "show";
+parse_pgm_token_string_t cmd_speed_show_arg = TOKEN_STRING_INITIALIZER(struct
cmd_speed_result, arg1, str_speed_show_arg);
+
+prog_char help_speed_show[] = "Show speed values for trajectory manager";
+parse_pgm_inst_t cmd_speed_show = {
+ .f = cmd_speed_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_speed_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_speed_arg0,
+ (prog_void *)&cmd_speed_show_arg,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Maximums for control system */
+
+/* this structure is filled when cmd_maximum is parsed successfully */
+struct cmd_maximum_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint32_t in;
+ uint32_t i;
+ uint32_t out;
+};
+
+/* function called when cmd_maximum is parsed successfully */
+static void cmd_maximum_parsed(void * parsed_result, void * data)
+{
+ struct cmd_maximum_result * res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("angle"))) {
+ pid_set_maximums(&robot.pid_a, res->in, res->i, res->out);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("distance"))) {
+ pid_set_maximums(&robot.pid_d, res->in, res->i, res->out);
+ }
+ /* else it is a "show" */
+
+ printf_P(PSTR("angle %lu %lu %lu\n"),
+ pid_get_max_in(&robot.pid_a),
+ pid_get_max_I(&robot.pid_a),
+ pid_get_max_out(&robot.pid_a));
+ printf_P(PSTR("distance %lu %lu %lu\n"),
+ pid_get_max_in(&robot.pid_d),
+ pid_get_max_I(&robot.pid_d),
+ pid_get_max_out(&robot.pid_d));
+}
+
+prog_char str_maximum_arg0[] = "maximum";
+parse_pgm_token_string_t cmd_maximum_arg0 = TOKEN_STRING_INITIALIZER(struct
cmd_maximum_result, arg0, str_maximum_arg0);
+prog_char str_maximum_arg1[] = "angle#distance";
+parse_pgm_token_string_t cmd_maximum_arg1 = TOKEN_STRING_INITIALIZER(struct
cmd_maximum_result, arg1, str_maximum_arg1);
+parse_pgm_token_num_t cmd_maximum_in = TOKEN_NUM_INITIALIZER(struct
cmd_maximum_result, in, UINT32);
+parse_pgm_token_num_t cmd_maximum_i = TOKEN_NUM_INITIALIZER(struct
cmd_maximum_result, i, UINT32);
+parse_pgm_token_num_t cmd_maximum_out = TOKEN_NUM_INITIALIZER(struct
cmd_maximum_result, out, UINT32);
+
+prog_char help_maximum[] = "Set maximum values for PID (in, I, out)";
+parse_pgm_inst_t cmd_maximum = {
+ .f = cmd_maximum_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_maximum,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_maximum_arg0,
+ (prog_void *)&cmd_maximum_arg1,
+ (prog_void *)&cmd_maximum_in,
+ (prog_void *)&cmd_maximum_i,
+ (prog_void *)&cmd_maximum_out,
+ NULL,
+ },
+};
+
+/* show */
+
+prog_char str_maximum_show_arg[] = "show";
+parse_pgm_token_string_t cmd_maximum_show_arg =
TOKEN_STRING_INITIALIZER(struct cmd_maximum_result, arg1, str_maximum_show_arg);
+
+prog_char help_maximum_show[] = "Show maximum values for PID";
+parse_pgm_inst_t cmd_maximum_show = {
+ .f = cmd_maximum_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_maximum_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_maximum_arg0,
+ (prog_void *)&cmd_maximum_show_arg,
+ NULL,
+ },
+};
+
+
/**********************************************************/
@@ -311,11 +455,12 @@
{
struct cmd_consign_result * res = parsed_result;
-#ifdef DEBUG_ASSERV
- wait_ms(100);
- printf(PSTR("\nasserv debug start\n"));
- to_dump = 0;
-#endif /* DEBUG_ASSERV */
+ if (robot.debug & DEBUG_CS) {
+ wait_ms(100);
+ printf(PSTR("\npress a key\ncs debug start\n"));
+ while(uart0_recv_nowait() == -1);
+ to_dump = 0;
+ }
if (!strcmp_P(res->arg1, PSTR("angle"))) {
cs_set_consign(&robot.cs_a, res->arg2);
}
@@ -326,10 +471,11 @@
cs_set_consign(&robot.cs_d, res->arg2);
cs_set_consign(&robot.cs_a, res->arg3);
}
-#ifdef DEBUG_ASSERV
- to_dump = 2;
- printf(PSTR("\nasserv debug end\n"));
-#endif /* DEBUG_ASSERV */
+ if (robot.debug & DEBUG_CS) {
+ while(uart0_recv_nowait() == -1);
+ to_dump = 2;
+ printf(PSTR("cs debug end\n"));
+ }
}
prog_char str_consign_arg0[] = "consign";
@@ -392,31 +538,36 @@
struct cmd_goto_result * res = parsed_result;
if (!strcmp_P(res->arg1, PSTR("a_rel"))) {
- trajectory_goto_a_rel(&robot.traj, res->arg2);
+ trajectory_a_rel(&robot.traj, res->arg2);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("d_rel"))) {
+ trajectory_d_rel(&robot.traj, res->arg2);
}
else if (!strcmp_P(res->arg1, PSTR("a_abs"))) {
- trajectory_goto_a(&robot.traj, res->arg2);
+ // trajectory_goto_a(&robot.traj, res->arg2);
+ printf_P(PSTR("not implemented\n"));
+ }
+ else if (!strcmp_P(res->arg1, PSTR("a_to_xy"))) {
+ trajectory_turnto_xy(&robot.traj, res->arg2, res->arg3);
}
else if (!strcmp_P(res->arg1, PSTR("xy_rel"))) {
trajectory_goto_xy_rel(&robot.traj, res->arg2, res->arg3);
}
else if (!strcmp_P(res->arg1, PSTR("xy_abs"))) {
- trajectory_goto_xy(&robot.traj, res->arg2, res->arg3);
+ trajectory_goto_xy_abs(&robot.traj, res->arg2, res->arg3);
}
else if (!strcmp_P(res->arg1, PSTR("da_rel"))) {
- trajectory_goto_d_a_rel(&robot.traj, res->arg2, res->arg3);
- }
- else if (!strcmp_P(res->arg1, PSTR("da_rs_rel"))) {
- trajectory_goto_rs_d_a_rel(&robot.traj, res->arg2, res->arg3);
+ trajectory_d_a_rel(&robot.traj, res->arg2, res->arg3);
}
else if (!strcmp_P(res->arg1, PSTR("xya_abs"))) {
- trajectory_goto_xya(&robot.traj, res->arg2, res->arg3,
res->arg4);
+ printf_P(PSTR("not implemented\n"));
+ //trajectory_goto_xya(&robot.traj, res->arg2, res->arg3,
res->arg4);
}
}
prog_char str_goto_arg0[] = "goto";
parse_pgm_token_string_t cmd_goto_arg0 = TOKEN_STRING_INITIALIZER(struct
cmd_goto_result, arg0, str_goto_arg0);
-prog_char str_goto_arg1_a[] = "a_rel#a_abs";
+prog_char str_goto_arg1_a[] = "d_rel#a_rel#a_abs";
parse_pgm_token_string_t cmd_goto_arg1_a = TOKEN_STRING_INITIALIZER(struct
cmd_goto_result, arg1, str_goto_arg1_a);
parse_pgm_token_num_t cmd_goto_arg2 = TOKEN_NUM_INITIALIZER(struct
cmd_goto_result, arg2, INT32);
@@ -434,7 +585,7 @@
},
};
-prog_char str_goto_arg1_b[] = "xy_rel#xy_abs#da_rel#da_rs_rel";
+prog_char str_goto_arg1_b[] = "xy_rel#xy_abs#da_rel#a_to_xy";
parse_pgm_token_string_t cmd_goto_arg1_b = TOKEN_STRING_INITIALIZER(struct
cmd_goto_result, arg1, str_goto_arg1_b);
parse_pgm_token_num_t cmd_goto_arg3 = TOKEN_NUM_INITIALIZER(struct
cmd_goto_result, arg3, INT32);
@@ -637,6 +788,64 @@
/**********************************************************/
+/* Save */
+
+/* this structure is filled when cmd_save is parsed successfully */
+struct cmd_save_result {
+ fixed_string_t arg0;
+};
+
+/* function called when cmd_save is parsed successfully */
+static void cmd_save_parsed(void * parsed_result, void * data)
+{
+ save_conf();
+}
+
+prog_char str_save_arg0[] = "save";
+parse_pgm_token_string_t cmd_save_arg0 = TOKEN_STRING_INITIALIZER(struct
cmd_save_result, arg0, str_save_arg0);
+
+prog_char help_save[] = "Save the configuration";
+parse_pgm_inst_t cmd_save = {
+ .f = cmd_save_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_save,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_save_arg0,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Load */
+
+/* this structure is filled when cmd_load is parsed successfully */
+struct cmd_load_result {
+ fixed_string_t arg0;
+};
+
+/* function called when cmd_load is parsed successfully */
+static void cmd_load_parsed(void * parsed_result, void * data)
+{
+ load_conf();
+}
+
+prog_char str_load_arg0[] = "load";
+parse_pgm_token_string_t cmd_load_arg0 = TOKEN_STRING_INITIALIZER(struct
cmd_load_result, arg0, str_load_arg0);
+
+prog_char help_load[] = "Load the configuration";
+parse_pgm_inst_t cmd_load = {
+ .f = cmd_load_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_load,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_load_arg0,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
/* Start */
/* this structure is filled when cmd_start is parsed successfully */
@@ -714,6 +923,90 @@
/**********************************************************/
+/* Debug */
+
+/* used for cs debugging */
+volatile uint8_t to_dump = 2;
+struct cs_debug cs_debug_a;
+struct cs_debug cs_debug_d;
+int8_t debug_cs_evt = -1;
+
+/* used for cs debugging */
+void debug_cs(void * dummy)
+{
+ uint8_t i;
+ uint8_t flags;
+
+ IRQ_LOCK(flags);
+ if (to_dump == 1) {
+ IRQ_UNLOCK(flags);
+ for (i=0; i< sizeof(struct cs_debug); i++) {
+ uart0_send( ((char *)&cs_debug_a)[i] );
+ }
+ for (i=0; i< sizeof(struct cs_debug); i++) {
+ uart0_send( ((char *)&cs_debug_d)[i] );
+ }
+ to_dump = 0;
+ }
+ else {
+ IRQ_UNLOCK(flags);
+ }
+}
+
+
+
+/* this structure is filled when cmd_debug is parsed successfully */
+struct cmd_debug_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ fixed_string_t arg2;
+};
+
+/* function called when cmd_debug is parsed successfully */
+static void cmd_debug_parsed(void * parsed_result, void * data)
+{
+ struct cmd_debug_result *res = (struct cmd_debug_result *)
parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("cs"))) {
+ if (!strcmp_P(res->arg2, PSTR("on"))) {
+ debug_cs_evt =
scheduler_add_periodical_event_priority(debug_cs, NULL,
+
5000L / SCHEDULER_UNIT, CS_PRIO-1);
+ robot.debug |= DEBUG_CS;
+ }
+ else if (!strcmp_P(res->arg2, PSTR("off"))) {
+ robot.debug &= (~DEBUG_CS);
+ scheduler_del_event(debug_cs_evt);
+ debug_cs_evt = -1;
+ }
+ else { /* show */
+ printf_P(PSTR("cs is %S\n"),
+ robot.debug & DEBUG_CS ? PSTR("off") :
PSTR("on"));
+ }
+ }
+}
+
+prog_char str_debug_arg0[] = "debug";
+parse_pgm_token_string_t cmd_debug_arg0 = TOKEN_STRING_INITIALIZER(struct
cmd_debug_result, arg0, str_debug_arg0);
+prog_char str_debug_arg1[] = "cs";
+parse_pgm_token_string_t cmd_debug_arg1 = TOKEN_STRING_INITIALIZER(struct
cmd_debug_result, arg1, str_debug_arg1);
+prog_char str_debug_arg2[] = "on#off#show";
+parse_pgm_token_string_t cmd_debug_arg2 = TOKEN_STRING_INITIALIZER(struct
cmd_debug_result, arg2, str_debug_arg2);
+
+
+prog_char help_debug[] = "Enable/disable debug";
+parse_pgm_inst_t cmd_debug = {
+ .f = cmd_debug_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_debug,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_debug_arg0,
+ (prog_void *)&cmd_debug_arg1,
+ (prog_void *)&cmd_debug_arg2,
+ NULL,
+ },
+};
+
+/**********************************************************/
/* Interact */
/* this structure is filled when cmd_interact is parsed successfully */
@@ -855,7 +1148,8 @@
case 'u':
if(robot.cs_events & DO_CS) {
- trajectory_speed_d_a(&robot.traj, 1000, 0, 0);
+ //trajectory_set_speed(&robot.traj, 1000, 0, 0);
+ printf_P(PSTR("not implemented\n"));
}
else {
pwm_set(LEFT_PWM, 800);
@@ -864,7 +1158,8 @@
break;
case 'h':
if(robot.cs_events & DO_CS) {
- trajectory_speed_d_a(&robot.traj, 0, 500, 0);
+ //trajectory_set_speed(&robot.traj, 0, 500, 0);
+ printf_P(PSTR("not implemented\n"));
}
else {
pwm_set(LEFT_PWM, -800);
@@ -873,7 +1168,8 @@
break;
case 'j':
if(robot.cs_events & DO_CS) {
- trajectory_speed_d_a(&robot.traj, -1000, 0, 0);
+ //trajectory_set_speed(&robot.traj, -1000, 0,
0);
+ printf_P(PSTR("not implemented\n"));
}
else {
pwm_set(LEFT_PWM, -800);
@@ -882,7 +1178,8 @@
break;
case 'k':
if(robot.cs_events & DO_CS) {
- trajectory_speed_d_a(&robot.traj, 0, -500, 0);
+ //trajectory_set_speed(&robot.traj, 0, -500, 0);
+ printf_P(PSTR("not implemented\n"));
}
else {
pwm_set(LEFT_PWM, 800);
@@ -945,18 +1242,25 @@
(parse_pgm_inst_t *)&cmd_pwm,
(parse_pgm_inst_t *)&cmd_gain,
(parse_pgm_inst_t *)&cmd_gain_show,
+ (parse_pgm_inst_t *)&cmd_maximum,
+ (parse_pgm_inst_t *)&cmd_maximum_show,
(parse_pgm_inst_t *)&cmd_quadramp,
(parse_pgm_inst_t *)&cmd_quadramp_show,
(parse_pgm_inst_t *)&cmd_encoders,
(parse_pgm_inst_t *)&cmd_position,
(parse_pgm_inst_t *)&cmd_reset,
+ (parse_pgm_inst_t *)&cmd_save,
+ (parse_pgm_inst_t *)&cmd_load,
(parse_pgm_inst_t *)&cmd_interact,
(parse_pgm_inst_t *)&cmd_log,
+ (parse_pgm_inst_t *)&cmd_debug,
(parse_pgm_inst_t *)&cmd_start,
(parse_pgm_inst_t *)&cmd_consign,
(parse_pgm_inst_t *)&cmd_consign_all,
(parse_pgm_inst_t *)&cmd_goto1,
(parse_pgm_inst_t *)&cmd_goto2,
(parse_pgm_inst_t *)&cmd_goto3,
+ (parse_pgm_inst_t *)&cmd_speed,
+ (parse_pgm_inst_t *)&cmd_speed_show,
NULL,
};
==========================================
aversive_projects/microb2008/main/eeprom.c (1.1)
==========================================
@@ -0,0 +1,93 @@
+/*
+ * 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: eeprom.c,v 1.1 2007-12-04 09:38:52 zer0 Exp $
+ *
+ */
+
+#include <avr/eeprom.h>
+
+#include "eeprom.h"
+#include "main.h"
+
+
+void load_conf(void)
+{
+ struct robot_conf robot_conf;
+
+ eeprom_read_block(&robot_conf, (uint8_t *)0,
+ sizeof(struct robot_conf));
+
+ robot.pid_a.gain_P = robot_conf.pid_a_gain_P;
+ robot.pid_a.gain_I = robot_conf.pid_a_gain_I;
+ robot.pid_a.gain_D = robot_conf.pid_a_gain_D;
+ robot.pid_a.max_in = robot_conf.pid_a_max_in;
+ robot.pid_a.max_I = robot_conf.pid_a_max_I;
+ robot.pid_a.max_out = robot_conf.pid_a_max_out;
+
+ robot.pid_d.gain_P = robot_conf.pid_d_gain_P;
+ robot.pid_d.gain_I = robot_conf.pid_d_gain_I;
+ robot.pid_d.gain_D = robot_conf.pid_d_gain_D;
+ robot.pid_d.max_in = robot_conf.pid_d_max_in;
+ robot.pid_d.max_I = robot_conf.pid_d_max_I;
+ robot.pid_d.max_out = robot_conf.pid_d_max_out;
+
+ robot.qr_a.var_2nd_ord_pos = robot_conf.qa_var_2nd_ord_pos;
+ robot.qr_a.var_2nd_ord_neg = robot_conf.qa_var_2nd_ord_neg;
+ robot.qr_a.var_1st_ord_pos = robot_conf.qa_var_1st_ord_pos;
+ robot.qr_a.var_1st_ord_neg = robot_conf.qa_var_1st_ord_neg;
+
+ robot.qr_d.var_2nd_ord_pos = robot_conf.qd_var_2nd_ord_pos;
+ robot.qr_d.var_2nd_ord_neg = robot_conf.qd_var_2nd_ord_neg;
+ robot.qr_d.var_1st_ord_pos = robot_conf.qd_var_1st_ord_pos;
+ robot.qr_d.var_1st_ord_neg = robot_conf.qd_var_1st_ord_neg;
+}
+
+void save_conf(void)
+{
+ struct robot_conf robot_conf;
+
+ robot_conf.pid_a_gain_P = robot.pid_a.gain_P;
+ robot_conf.pid_a_gain_I = robot.pid_a.gain_I;
+ robot_conf.pid_a_gain_D = robot.pid_a.gain_D;
+ robot_conf.pid_a_max_in = robot.pid_a.max_in;
+ robot_conf.pid_a_max_I = robot.pid_a.max_I;
+ robot_conf.pid_a_max_out = robot.pid_a.max_out;
+
+ robot_conf.pid_d_gain_P = robot.pid_d.gain_P;
+ robot_conf.pid_d_gain_I = robot.pid_d.gain_I;
+ robot_conf.pid_d_gain_D = robot.pid_d.gain_D;
+ robot_conf.pid_d_max_in = robot.pid_d.max_in;
+ robot_conf.pid_d_max_I = robot.pid_d.max_I;
+ robot_conf.pid_d_max_out = robot.pid_d.max_out;
+
+ robot_conf.qa_var_2nd_ord_pos = robot.qr_a.var_2nd_ord_pos;
+ robot_conf.qa_var_2nd_ord_neg = robot.qr_a.var_2nd_ord_neg;
+ robot_conf.qa_var_1st_ord_pos = robot.qr_a.var_1st_ord_pos;
+ robot_conf.qa_var_1st_ord_neg = robot.qr_a.var_1st_ord_neg;
+
+ robot_conf.qd_var_2nd_ord_pos = robot.qr_d.var_2nd_ord_pos;
+ robot_conf.qd_var_2nd_ord_neg = robot.qr_d.var_2nd_ord_neg;
+ robot_conf.qd_var_1st_ord_pos = robot.qr_d.var_1st_ord_pos;
+ robot_conf.qd_var_1st_ord_neg = robot.qr_d.var_1st_ord_neg;
+
+ eeprom_write_block(&robot_conf, (uint8_t *)0,
+ sizeof(struct robot_conf));
+
+
+}
+
==========================================
aversive_projects/microb2008/main/eeprom.h (1.1)
==========================================
@@ -0,0 +1,54 @@
+/*
+ * 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: eeprom.h,v 1.1 2007-12-04 09:38:52 zer0 Exp $
+ *
+ */
+
+#ifndef _MICROB_EEPROM_H_
+#define _MICROB_EEPROM_H_
+
+struct robot_conf {
+ int16_t pid_a_gain_P;
+ int16_t pid_a_gain_I;
+ int16_t pid_a_gain_D;
+ int32_t pid_a_max_in;
+ int32_t pid_a_max_I;
+ int32_t pid_a_max_out;
+
+ int16_t pid_d_gain_P;
+ int16_t pid_d_gain_I;
+ int16_t pid_d_gain_D;
+ int32_t pid_d_max_in;
+ int32_t pid_d_max_I;
+ int32_t pid_d_max_out;
+
+ uint32_t qa_var_2nd_ord_pos;
+ uint32_t qa_var_2nd_ord_neg;
+ uint32_t qa_var_1st_ord_pos;
+ uint32_t qa_var_1st_ord_neg;
+
+ uint32_t qd_var_2nd_ord_pos;
+ uint32_t qd_var_2nd_ord_neg;
+ uint32_t qd_var_1st_ord_pos;
+ uint32_t qd_var_1st_ord_neg;
+};
+
+void load_conf(void);
+void save_conf(void);
+
+#endif
========================================
aversive_projects/microb2008/main/main.c (1.9 -> 1.10)
========================================
@@ -15,7 +15,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: main.c,v 1.9 2007-11-27 23:16:53 zer0 Exp $
+ * Revision : $Id: main.c,v 1.10 2007-12-04 09:38:52 zer0 Exp $
*
*/
@@ -23,6 +23,7 @@
#include <stdio.h>
#include <string.h>
#include <math.h>
+#include <avr/eeprom.h>
#include <aversive/pgmspace.h>
#include <aversive/error.h>
@@ -74,53 +75,14 @@
#endif
}
-#ifdef DEBUG_ASSERV
-volatile uint8_t to_dump = 2;
-struct cs_debug {
- uint32_t cpt;
- int32_t consign_value;
- int32_t encoders;
- int32_t error_value;
- int32_t i;
- int32_t d;
- int32_t out_value;
-};
-
-struct cs_debug cs_debug_a;
-struct cs_debug cs_debug_d;
-
-void debug_asserv(void * dummy)
-{
- uint8_t i;
- uint8_t flags;
-
- IRQ_LOCK(flags);
- if (to_dump == 1) {
- IRQ_UNLOCK(flags);
- for (i=0; i< sizeof(struct cs_debug); i++) {
- uart0_send( ((char *)&cs_debug_a)[i] );
- }
- for (i=0; i< sizeof(struct cs_debug); i++) {
- uart0_send( ((char *)&cs_debug_d)[i] );
- }
- to_dump = 0;
- }
- else {
- IRQ_UNLOCK(flags);
- }
-}
-
-#endif /* DEBUG_ASSERV */
-
-
/* called every 5 ms */
void do_cs(void * dummy) {
static uint8_t cpt = 0;
uint8_t second;
-#ifdef DEBUG_ASSERV
- static uint32_t debug_asserv_cpt = 0;
+
+ /* used for cs debugging */
+ static uint32_t debug_cs_cpt = 0;
uint8_t flags;
-#endif /* DEBUG_ASSERV */
if(robot.cs_events & DO_RS) {
rs_update(&robot.rs);
@@ -129,36 +91,35 @@
cs_manage(&robot.cs_a);
cs_manage(&robot.cs_d);
-#ifdef DEBUG_ASSERV
- debug_asserv_cpt++;
-
- IRQ_LOCK(flags);
- if (to_dump == 0) {
- to_dump = 1;
- IRQ_UNLOCK(flags);
+ if (robot.debug & DEBUG_CS) {
+ debug_cs_cpt++;
- cs_debug_d.cpt = debug_asserv_cpt;
- cs_debug_d.consign_value =
robot.cs_d.filtered_consign_value;
- cs_debug_d.encoders = rs_get_distance(&robot.rs);
- cs_debug_d.error_value = robot.cs_d.error_value;
- cs_debug_d.out_value = robot.cs_d.out_value;
- cs_debug_d.d = robot.pid_d.prev_D;
- cs_debug_d.i = robot.pid_d.integral;
-
- cs_debug_a.cpt = debug_asserv_cpt;
- cs_debug_a.consign_value =
robot.cs_a.filtered_consign_value;
- cs_debug_a.encoders = rs_get_angle(&robot.rs);
- cs_debug_a.error_value = robot.cs_a.error_value;
- cs_debug_a.out_value = robot.cs_a.out_value;
- cs_debug_a.d = robot.pid_a.prev_D;
- cs_debug_a.i = robot.pid_a.integral;
- }
- else {
- IRQ_UNLOCK(flags);
+ IRQ_LOCK(flags);
+ if (to_dump == 0) {
+ to_dump = 1;
+ IRQ_UNLOCK(flags);
+
+ cs_debug_d.cpt = debug_cs_cpt;
+ cs_debug_d.consign_value =
robot.cs_d.filtered_consign_value;
+ cs_debug_d.encoders =
rs_get_distance(&robot.rs);
+ cs_debug_d.error_value = robot.cs_d.error_value;
+ cs_debug_d.out_value = robot.cs_d.out_value;
+ cs_debug_d.d = robot.pid_d.prev_D;
+ cs_debug_d.i = robot.pid_d.integral;
+
+ cs_debug_a.cpt = debug_cs_cpt;
+ cs_debug_a.consign_value =
robot.cs_a.filtered_consign_value;
+ cs_debug_a.encoders = rs_get_angle(&robot.rs);
+ cs_debug_a.error_value = robot.cs_a.error_value;
+ cs_debug_a.out_value = robot.cs_a.out_value;
+ cs_debug_a.d = robot.pid_a.prev_D;
+ cs_debug_a.i = robot.pid_a.integral;
+ }
+ else {
+ IRQ_UNLOCK(flags);
+ }
}
-#endif /* DEBUG_ASSERV */
-
}
if((cpt % 4 == 0) && robot.cs_events & DO_POS) {
@@ -212,7 +173,7 @@
encoders_microb_manage(NULL);
encoder_running = 0;
- if (cpt%4)
+ if ((cpt%4) == 0)
scheduler_interrupt();
}
@@ -285,12 +246,106 @@
}
+#define PROFILE_TIME 10
+volatile int a = 0;
+
+void __attribute__ ((noinline)) dump_reg(uint16_t pc)
+{
+ static volatile uint8_t cpt = PROFILE_TIME;
+
+ if (cpt == 1) {
+ OCR2 = 0x80 + (rand()&0x7F);
+
+ TCCR2 = 2;
+ TCNT2 = 0;
+ }
+ else if (cpt == 0) {
+ OCR2 = 0;
+ TCCR2 = 4;
+ TCNT2 = 0;
+ cpt = PROFILE_TIME;
+ printf("%.4x\n", pc);
+ }
+ cpt--;
+}
+
+
+void SIG_OUTPUT_COMPARE2(void) __attribute__ ((signal , naked, __INTR_ATTRS));
+
+void SIG_OUTPUT_COMPARE2(void)
+{
+ asm volatile("push r1" "\n\t"
+ "push __tmp_reg__" "\n\t"
+
+ /* save sreg */
+ "in __tmp_reg__,__SREG__" "\n\t"
+ "push __tmp_reg__" "\n\t"
+ "eor r1, r1" "\n\t"
+
+ /* save used regs (see avr-gcc doc about used regs) */
+ "push r18" "\n\t"
+ "push r19" "\n\t"
+ "push r20" "\n\t"
+ "push r21" "\n\t"
+ "push r22" "\n\t"
+ "push r23" "\n\t"
+ "push r24" "\n\t"
+ "push r25" "\n\t"
+ "push r26" "\n\t"
+ "push r27" "\n\t"
+ "push r30" "\n\t"
+ "push r31" "\n\t"
+
+ /* load sp in r30/r31 */
+ "in r30, __SP_L__" "\n\t"
+ "in r31, __SP_H__" "\n\t"
+
+ /* point to saved PC */
+ "subi r30, lo8(-16)" "\n\t"
+ "sbci r31, hi8(-16)" "\n\t"
+
+ /* load Program Counter into r24-r25 */
+ "ldd r25, Z+0" "\n\t"
+ "ldd r24, Z+1" "\n\t"
+
+ /* call dump_reg, params are in r24-25 */
+ "call dump_reg" "\n\t"
+
+ /* restore regs */
+ "pop r31" "\n\t"
+ "pop r30" "\n\t"
+ "pop r27" "\n\t"
+ "pop r26" "\n\t"
+ "pop r25" "\n\t"
+ "pop r24" "\n\t"
+ "pop r23" "\n\t"
+ "pop r22" "\n\t"
+ "pop r21" "\n\t"
+ "pop r20" "\n\t"
+ "pop r19" "\n\t"
+ "pop r18" "\n\t"
+
+ /* sreg */
+ "pop __tmp_reg__" "\n\t"
+ "out __SREG__, __tmp_reg__" "\n\t"
+
+ /* tmp reg */
+ "pop __tmp_reg__" "\n\t"
+ "pop r1" "\n\t"
+
+ "reti" "\n\t"
+ :
+ :
+ );
+}
+
/*** main */
int main(void) {
int c;
uint8_t i=0;
+ const char * history;
/* init struct robot */
memset(&robot, 0, sizeof(struct robot));
@@ -308,6 +363,7 @@
/* UART */
uart_init();
fdevopen(uart0_dev_send, uart0_dev_recv);
+
printf_P(PSTR("salut\n"));
uart0_register_rx_event(emergency);
@@ -318,6 +374,9 @@
error_register_notice(mylog);
error_register_debug(mylog);
+ /* ENCODERS */
+ encoders_microb_init();
+
/* TIMER */
timer_init();
timer0_register_OV_intr(main_timer_interrupt);
@@ -326,6 +385,7 @@
scheduler_init();
scheduler_add_periodical_event_priority(do_led_blink, NULL,
500000L / SCHEDULER_UNIT,
LED_PRIO);
+
/* I2C */
/* i2c_protocol_init(); */
/* i2c_init(I2C_MODE_MASTER, I2C_MAIN_ADDR); */
@@ -347,15 +407,9 @@
/* wait_ms(500); */
/* } */
-
/* PWM */
pwm_init();
- /* ENCODERS */
- encoders_microb_init();
- scheduler_add_periodical_event_priority(encoders_microb_manage, NULL,
- 1, ENCODERS_PRIO);
-
/* ROBOT_SYSTEM */
rs_init(&robot.rs);
rs_set_left_pwm(&robot.rs, pwm_set, LEFT_PWM);
@@ -383,12 +437,12 @@
/* CS DISTANCE */
pid_init(&robot.pid_d);
- pid_set_gains(&robot.pid_d, 350, 8, 0/* 5000 */);
+ pid_set_gains(&robot.pid_d, 1000, 30, 20000);
pid_set_maximums(&robot.pid_d, 0, 50000, 4095);
pid_set_out_shift(&robot.pid_d, 10);
quadramp_init(&robot.qr_d);
- quadramp_set_1st_order_vars(&robot.qr_d, 1000, 1000); /* set speed */
+ quadramp_set_1st_order_vars(&robot.qr_d, 2000, 2000); /* set speed */
quadramp_set_2nd_order_vars(&robot.qr_d, 15, 15); /* set accel */
cs_init(&robot.cs_d);
@@ -400,7 +454,7 @@
/* CS ANGLE */
pid_init(&robot.pid_a);
- pid_set_gains(&robot.pid_a, 500, 6, 20000);
+ pid_set_gains(&robot.pid_a, 1000, 15, 20000);
pid_set_maximums(&robot.pid_a, 0, 100000, 4095);
pid_set_out_shift(&robot.pid_a, 10);
@@ -419,6 +473,7 @@
trajectory_init(&robot.traj);
trajectory_set_cs(&robot.traj, &robot.cs_d, &robot.cs_a);
trajectory_set_robot_params(&robot.traj, &robot.rs, &robot.pos);
+ trajectory_set_speed(&robot.traj, 2000, 2000);
/* distance window, angle window */
trajectory_set_windows(&robot.traj, 5.0, 5.0);
@@ -438,11 +493,13 @@
scheduler_add_periodical_event_priority(sensor_update, NULL,
50000L / SCHEDULER_UNIT,
SENSORS_PRIO);
-#ifdef DEBUG_ASSERV
- wait_ms(1000);
- scheduler_add_periodical_event_priority(debug_asserv, NULL,
- 5000L / SCHEDULER_UNIT,
CS_PRIO-1);
-#endif /* DEBUG_ASSERV */
+ /* it uses timer 2 */
+#ifdef PROFILE
+ OCR2 = 0;
+ TCNT2 = 0;
+ TCCR2 = 4;
+ sbi(TIMSK, OCIE2);
+#endif
sei();
@@ -488,6 +545,7 @@
rdline_init(&rdl, write_char, valid_buffer, complete_buffer);
+ wait_ms(100);
printf_P("\n");
snprintf(prompt, sizeof(prompt), "microb > ");
rdline_newline(&rdl, prompt);
@@ -498,7 +556,9 @@
int8_t ret;
ret = rdline_char_in(&rdl, c);
if (ret != 2 && ret != 0) {
- rdline_add_history(&rdl,
rdline_get_buffer(&rdl));
+ history = rdline_get_buffer(&rdl);
+ if (strlen(history)>1)
+ rdline_add_history(&rdl, history);
rdline_newline(&rdl, prompt);
}
}
========================================
aversive_projects/microb2008/main/main.h (1.4 -> 1.5)
========================================
@@ -15,7 +15,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: main.h,v 1.4 2007-11-27 23:16:53 zer0 Exp $
+ * Revision : $Id: main.h,v 1.5 2007-12-04 09:38:52 zer0 Exp $
*
*/
@@ -30,13 +30,11 @@
#include <trajectory_manager.h>
+//#define DEBUG_ASSERV
+//#define PROFILE
+
+
/* to remove when we will have the new trajectory manager XXX */
-#define trajectory_goto_xy(args...) do { } while(0)
-#define trajectory_goto_a(args...) do { } while(0)
-#define trajectory_goto_xya(args...) do { } while(0)
-#define trajectory_goto_rs_d_a_rel(args...) do { } while(0)
-#define trajectory_goto_a_rel(args...) do { } while(0)
-#define trajectory_speed_d_a(args...) do { } while(0)
#define trajectory_goto_forward_xy(args...) do { } while(0)
#define trajectory_nearly_finished(args...) (0)
@@ -79,6 +77,9 @@
#define SENSORS_PRIO 100
#define I2C_POLL_PRIO 20
+/* debug flags */
+#define DEBUG_CS 1
+
struct robot {
int8_t cs_events;
@@ -97,12 +98,31 @@
uint8_t log_level;
uint8_t log_type;
+ uint8_t debug;
uint8_t blocking; /* XXX ? */
};
extern struct robot robot;
+/* cs debugging */
+
+struct cs_debug {
+ uint32_t cpt;
+ int32_t consign_value;
+ int32_t encoders;
+ int32_t error_value;
+ int32_t i;
+ int32_t d;
+ int32_t out_value;
+};
+
+extern volatile uint8_t to_dump;
+extern struct cs_debug cs_debug_a;
+extern struct cs_debug cs_debug_d;
+
+
+
void do_cs(void * dummy);
#define LED5BIT 2
===================================================
aversive_projects/microb2008/main/parse_cs_debug.py (1.1)
===================================================
@@ -0,0 +1,133 @@
+#!/usr/bin/python
+
+import sys
+import struct
+import Gnuplot
+import re
+
+if len(sys.argv) != 2:
+ print "Bad arguments"
+ sys.exit(1)
+
+filename = sys.argv[1]
+
+#
+# read data from file
+#
+f = open(filename)
+while True:
+ line = f.readline()
+ if line == "":
+ print "No start found"
+ sys.exit(1)
+
+ if line == "cs debug start\n":
+ break
+print "detect start ok"
+data_angle = []
+data_distance = []
+pattern = 'cs debug end\n'
+
+to_read = "<Iiiiiii"
+to_read_size = struct.calcsize(to_read)
+angle_distance = False
+while True:
+
+ line = f.read(to_read_size)
+ print repr(line)
+ if line == "":
+ print "No end found"
+ sys.exit(1)
+
+ if line[:len(pattern)] == pattern:
+ break
+ angle_distance = not angle_distance
+ if angle_distance:
+ data_angle.append(list(struct.unpack(to_read, line)))
+ else:
+ data_distance.append(list(struct.unpack(to_read, line)))
+
+
+f.close()
+
+angle_cpt = []
+angle_consign_value = []
+angle_encoders = []
+angle_error_values = []
+angle_i = []
+angle_d = []
+angle_out_value = []
+angle_all = [angle_cpt,
+ angle_consign_value,
+ angle_encoders,
+ angle_error_values,
+ angle_i,
+ angle_d,
+ angle_out_value]
+
+distance_cpt = []
+distance_consign_value = []
+distance_encoders = []
+distance_error_values = []
+distance_i = []
+distance_d = []
+distance_out_value = []
+distance_all = [distance_cpt,
+ distance_consign_value,
+ distance_encoders,
+ distance_error_values,
+ distance_i,
+ distance_d,
+ distance_out_value]
+
+
+for y in xrange(len(angle_all)):
+ angle_all[y]+=map(lambda x:x[y], data_angle)
+for y in xrange(len(distance_all)):
+ distance_all[y]+=map(lambda x:x[y], data_distance)
+
+
+def assoc_line(a):
+ global angle_cpt
+ return map(lambda x,y:[x,y], angle_cpt, a),
+
+g = Gnuplot.Gnuplot()
+g('set data style lines')
+
+h = Gnuplot.Gnuplot()
+h('set data style lines')
+
+#g.set_label(["1", "2", "3", "4", "5"])
+g.title("Angle")
+g.plot(Gnuplot.Data(assoc_line(angle_consign_value), title="cons"),
+ Gnuplot.Data(assoc_line(angle_encoders), title="encoders"),
+ Gnuplot.Data(assoc_line(angle_error_values), title="error"),
+ Gnuplot.Data(assoc_line(angle_i), title="i"),
+ Gnuplot.Data(assoc_line(angle_d), title="d"),
+ Gnuplot.Data(assoc_line(angle_out_value), title="out"))
+
+h.title("Distance")
+h.plot(Gnuplot.Data(assoc_line(distance_consign_value), title="cons"),
+ Gnuplot.Data(assoc_line(distance_encoders), title="encoders"),
+ Gnuplot.Data(assoc_line(distance_error_values), title="error"),
+ Gnuplot.Data(assoc_line(distance_i), title="i"),
+ Gnuplot.Data(assoc_line(distance_d), title="d"),
+ Gnuplot.Data(assoc_line(distance_out_value), title="out"))
+
+while(1):
+ pass
+#
+# parse data, two structs like below (one for angle, one
+# for distance)
+#
+# struct cs_debug {
+# uint32_t cpt;
+# int32_t consign_value;
+# int32_t encoders;
+# int32_t error_value;
+# int32_t i;
+# int32_t d;
+# int32_t out_value;
+# };
+
+
==============================================
aversive_projects/microb2008/main/pwm_config.h (1.2 -> 1.3)
==============================================
@@ -15,7 +15,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: pwm_config.h,v 1.2 2007-11-15 11:14:54 zer0 Exp $
+ * Revision : $Id: pwm_config.h,v 1.3 2007-12-04 09:38:52 zer0 Exp $
*
*/
@@ -32,7 +32,7 @@
#define PWM1A_ENABLED
#define PWM1B_ENABLED
#define PWM3C_ENABLED
-#define PWM2_ENABLED
+//#define PWM2_ENABLED
/** max value for PWM entry, default 12 bits > 4095 */
#define PWM_SIGNIFICANT_BITS 12
====================================================
aversive_projects/microb2008/main/scheduler_config.h (1.2 -> 1.3)
====================================================
@@ -15,7 +15,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: scheduler_config.h,v 1.2 2007-11-27 23:16:53 zer0 Exp $
+ * Revision : $Id: scheduler_config.h,v 1.3 2007-12-04 09:38:52 zer0 Exp $
*
*/
@@ -28,8 +28,8 @@
#define SCHEDULER_NB_MAX_EVENT 7
-#define SCHEDULER_UNIT_FLOAT 1000.0
-#define SCHEDULER_UNIT 1000L
+#define SCHEDULER_UNIT_FLOAT 512.0
+#define SCHEDULER_UNIT 512L
/** number of allowed imbricated scheduler interrupts. The maximum
* should be SCHEDULER_NB_MAX_EVENT since we never need to imbricate
===============================================
aversive_projects/microb2008/main/time_config.h (1.1 -> 1.2)
===============================================
@@ -15,9 +15,9 @@
* 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: time_config.h,v 1.1 2007-06-28 22:13:04 zer0 Exp $
+ * Revision : $Id: time_config.h,v 1.2 2007-12-04 09:38:52 zer0 Exp $
*
*/
/** precision of the time processor, in us */
-#define TIME_PRECISION 1000l
+#define TIME_PRECISION 100000l
===============================================
aversive_projects/microb2008/main/uart_config.h (1.1 -> 1.2)
===============================================
@@ -15,7 +15,7 @@
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
- * Revision : $Id: uart_config.h,v 1.1 2007-06-28 22:13:04 zer0 Exp $
+ * Revision : $Id: uart_config.h,v 1.2 2007-12-04 09:38:52 zer0 Exp $
*
*/
@@ -48,7 +48,7 @@
#define UART0_USE_DOUBLE_SPEED 0
//#define UART0_USE_DOUBLE_SPEED 1
-#define UART0_RX_FIFO_SIZE 4
+#define UART0_RX_FIFO_SIZE 32
#define UART0_TX_FIFO_SIZE 16
//#define UART0_NBITS 5
//#define UART0_NBITS 6
_______________________________________________
Avr-list mailing list
Avr-list@droids-corp.org
CVSWEB : http://cvsweb.droids-corp.org/cgi-bin/viewcvs.cgi/aversive
WIKI : http://wiki.droids-corp.org/index.php/Aversive
DOXYGEN : http://zer0.droids-corp.org/doxygen_aversive/html/
BUGZILLA : http://bugzilla.droids-corp.org
COMMIT LOGS : http://zer0.droids-corp.org/aversive_commitlog
