Commit from zer0 (2008-04-01 13:47 CEST) ================ force logs when doing strat
aversive_projects microb2008/main/commands.c 1.19 aversive_projects microb2008/main/main.c 1.27 aversive_projects microb2008/main/main.h 1.17 ============================================ aversive_projects/microb2008/main/commands.c (1.18 -> 1.19) ============================================ @@ -1552,6 +1552,10 @@ static void cmd_start_parsed(void * parsed_result, void * data) { struct cmd_start_result *res = parsed_result; + uint8_t old_level = robot.log_level; + + robot.logs[NB_LOGS] = E_USER_STRAT; + robot.log_level = 5; if (!strcmp_P(res->color, PSTR("red"))) { robot.our_color = I2C_EXTENSION_COLOR_RED; @@ -1562,6 +1566,9 @@ i2c_set_color(I2C_EXTENSION_COLOR_BLUE); } start(); + + robot.logs[NB_LOGS] = 0; + robot.log_level = old_level; } prog_char str_start_arg0[] = "start"; ======================================== aversive_projects/microb2008/main/main.c (1.26 -> 1.27) ======================================== @@ -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.26 2008-03-31 22:22:52 zer0 Exp $ + * Revision : $Id: main.c,v 1.27 2008-04-01 11:47:19 zer0 Exp $ * */ @@ -215,10 +215,10 @@ if (robot.log_level < e->severity) return; - for (i=0; i<NB_LOGS; i++) + for (i=0; i<NB_LOGS+1; i++) if (robot.logs[i] == e->err_num) break; - if (i==NB_LOGS) + if (i==NB_LOGS+1) return; va_start(ap, e); ======================================== aversive_projects/microb2008/main/main.h (1.16 -> 1.17) ======================================== @@ -15,7 +15,7 @@ * along with this program; if not, write to the Free Software * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * - * Revision : $Id: main.h,v 1.16 2008-03-31 22:22:52 zer0 Exp $ + * Revision : $Id: main.h,v 1.17 2008-04-01 11:47:19 zer0 Exp $ * */ @@ -112,7 +112,7 @@ uint8_t extension_color; uint16_t extension_test_cpt; - uint8_t logs[NB_LOGS]; + uint8_t logs[NB_LOGS+1]; uint8_t log_level; uint8_t debug; Commit from zer0 (2008-04-01 15:50 CEST) ================ - little test trajs (square, hourglass) - auto_position on any color aversive_projects microb2008/main/commands.c 1.20 aversive_projects microb2008/main/strat.c 1.9 aversive_projects microb2008/main/strat.h 1.6 ============================================ aversive_projects/microb2008/main/commands.c (1.19 -> 1.20) ============================================ @@ -1186,6 +1186,42 @@ int32_t arg4; }; +static void auto_position(void) +{ + trajectory_set_speed(&robot.traj, 100, 100); + + trajectory_d_rel(&robot.traj, -20); + wait_traj_end(END_TRAJ|END_BLOCKING); + wait_ms(100); + + position_set(&robot.pos, 16, 0, 0); + + trajectory_d_rel(&robot.traj, 10); + wait_traj_end(END_TRAJ); + + trajectory_a_rel(&robot.traj, COL_ANGLE(90)); + wait_traj_end(END_TRAJ); + + trajectory_d_rel(&robot.traj, -20); + wait_traj_end(END_TRAJ|END_BLOCKING); + wait_ms(100); + + position_set(&robot.pos, position_get_x_s16(&robot.pos), + COL_COORD_Y(16), COL_ANGLE(90)); + + trajectory_d_rel(&robot.traj, 10); + wait_traj_end(END_TRAJ); + wait_ms(500); + + trajectory_a_abs(&robot.traj, COL_ANGLE(45)); + wait_traj_end(END_TRAJ); + wait_ms(500); + + trajectory_d_rel(&robot.traj, -4); + wait_traj_end(END_TRAJ); +} + + /* function called when cmd_position is parsed successfully */ static void cmd_position_parsed(void * parsed_result, void * data) { @@ -1198,6 +1234,16 @@ else if (!strcmp_P(res->arg1, PSTR("set"))) { position_set(&robot.pos, res->arg2, res->arg3, res->arg4); } + else if (!strcmp_P(res->arg1, PSTR("autoset_blue"))) { + robot.our_color = I2C_EXTENSION_COLOR_BLUE; + i2c_set_color(I2C_EXTENSION_COLOR_BLUE); + auto_position(); + } + else if (!strcmp_P(res->arg1, PSTR("autoset_red"))) { + robot.our_color = I2C_EXTENSION_COLOR_RED; + i2c_set_color(I2C_EXTENSION_COLOR_RED); + auto_position(); + } /* else it's just a "show" */ printf_P(PSTR("x=%.2f y=%.2f a=%.2f\r\n"), @@ -1208,7 +1254,7 @@ prog_char str_position_arg0[] = "position"; parse_pgm_token_string_t cmd_position_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg0, str_position_arg0); -prog_char str_position_arg1[] = "show#reset"; +prog_char str_position_arg1[] = "show#reset#autoset_blue#autoset_red"; parse_pgm_token_string_t cmd_position_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_position_result, arg1, str_position_arg1); prog_char help_position[] = "Show/reset (x,y,a) position"; @@ -1659,6 +1705,47 @@ fixed_string_t name; }; +void test_square(void) +{ + position_set(&robot.pos, 0, 0, 0); + + while(uart0_recv_nowait() == -1) { + trajectory_goto_forward_xy_abs(&robot.traj, 0, 60); + wait_traj_end(END_TRAJ|END_BLOCKING); + + trajectory_goto_forward_xy_abs(&robot.traj, 60, 60); + wait_traj_end(END_TRAJ|END_BLOCKING); + + trajectory_goto_forward_xy_abs(&robot.traj, 60, 0); + wait_traj_end(END_TRAJ|END_BLOCKING); + + trajectory_goto_forward_xy_abs(&robot.traj, 0, 0); + wait_traj_end(END_TRAJ|END_BLOCKING); + } +} + +void test_hourglass(void) +{ + position_set(&robot.pos, 0, 0, 0); + + while(uart0_recv_nowait() == -1) { + trajectory_goto_forward_xy_abs(&robot.traj, 60, 60); + wait_traj_end(END_TRAJ|END_BLOCKING); + + trajectory_goto_forward_xy_abs(&robot.traj, 0, 60); + wait_traj_end(END_TRAJ|END_BLOCKING); + + trajectory_goto_forward_xy_abs(&robot.traj, 60, 0); + wait_traj_end(END_TRAJ|END_BLOCKING); + + trajectory_goto_forward_xy_abs(&robot.traj, 0, 0); + wait_traj_end(END_TRAJ|END_BLOCKING); + } +} + + + + /* function called when cmd_substrat is parsed successfully */ static void cmd_substrat_parsed(void * parsed_result, void * data) { @@ -1670,14 +1757,17 @@ else if (!strcmp_P(res->name, PSTR("eject"))) { strat_eject_balls(); } - if (!strcmp_P(res->name, PSTR("position"))) { - strat_position(); + else if (!strcmp_P(res->name, PSTR("square"))) { + test_square(); + } + else if (!strcmp_P(res->name, PSTR("hourglass"))) { + test_hourglass(); } } prog_char str_substrat_arg0[] = "substrat"; parse_pgm_token_string_t cmd_substrat_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_substrat_result, arg0, str_substrat_arg0); -prog_char str_substrat_name[] = "shot#eject#position"; +prog_char str_substrat_name[] = "shot#eject#square#hourglass"; parse_pgm_token_string_t cmd_substrat_name = TOKEN_STRING_INITIALIZER(struct cmd_substrat_result, name, str_substrat_name); prog_char help_substrat[] = "Test sub-strats"; ========================================= aversive_projects/microb2008/main/strat.c (1.8 -> 1.9) ========================================= @@ -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: strat.c,v 1.8 2008-03-31 22:22:52 zer0 Exp $ + * Revision : $Id: strat.c,v 1.9 2008-04-01 13:50:09 zer0 Exp $ * */ @@ -82,40 +82,6 @@ robot.cs_events &= DO_TIMER; } -void strat_position(void) -{ - trajectory_set_speed(&robot.traj, 100, 100); - - trajectory_d_rel(&robot.traj, -20); - wait_traj_end(END_TRAJ|END_BLOCKING); - wait_ms(100); - - position_set(&robot.pos, 16, 0, 0); - - trajectory_d_rel(&robot.traj, 10); - wait_traj_end(END_TRAJ); - - trajectory_a_rel(&robot.traj, 90); - wait_traj_end(END_TRAJ); - - trajectory_d_rel(&robot.traj, -20); - wait_traj_end(END_TRAJ|END_BLOCKING); - wait_ms(100); - - position_set(&robot.pos, position_get_x_s16(&robot.pos), 16, 90); - - trajectory_d_rel(&robot.traj, 10); - wait_traj_end(END_TRAJ); - wait_ms(500); - - trajectory_a_abs(&robot.traj, 45); - wait_traj_end(END_TRAJ); - wait_ms(500); - - trajectory_d_rel(&robot.traj, -4); - wait_traj_end(END_TRAJ); -} - uint8_t strat_unblock(void) { uint8_t flags; ========================================= aversive_projects/microb2008/main/strat.h (1.5 -> 1.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: strat.h,v 1.5 2008-03-31 22:22:52 zer0 Exp $ + * Revision : $Id: strat.h,v 1.6 2008-04-01 13:50:09 zer0 Exp $ * */ @@ -98,7 +98,6 @@ void strat_uninit(void); uint8_t strat_main(void); -void strat_position(void); uint8_t strat_shot_balls(void); uint8_t strat_eject_balls(void); Commit from zer0 (2008-04-02 00:44 CEST) ================ - prepare cam - strat for h_disp - new command for catapult control (h_disp) - Add END_TIMER, END_INTR - Better error management in strats aversive_projects microb2008/common/i2c_commands.h 1.10 aversive_projects microb2008/extension/i2c_protocol.c 1.15 aversive_projects microb2008/extension/state.c 1.7 aversive_projects microb2008/extension/state.h 1.3 aversive_projects microb2008/main/cam.c 1.5 aversive_projects microb2008/main/cam.h 1.4 aversive_projects microb2008/main/commands.c 1.21 aversive_projects microb2008/main/i2c_protocol.c 1.15 aversive_projects microb2008/main/i2c_protocol.h 1.11 aversive_projects microb2008/main/main.c 1.28 aversive_projects microb2008/main/strat.c 1.10 aversive_projects microb2008/main/strat.h 1.7 aversive_projects microb2008/main/strat_base.c 1.11 aversive_projects microb2008/main/strat_base.h 1.9 ================================================== aversive_projects/microb2008/common/i2c_commands.h (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: i2c_commands.h,v 1.9 2008-03-24 23:29:12 zer0 Exp $ + * Revision : $Id: i2c_commands.h,v 1.10 2008-04-01 22:44:41 zer0 Exp $ * */ @@ -202,6 +202,14 @@ }; /****/ + +#define I2C_CMD_EXTENSION_CATA_OUT 0x0E + +struct i2c_cmd_extension_cata_out { + struct i2c_cmd_hdr hdr; +}; + +/****/ /* requests and their answers */ /****/ @@ -234,7 +242,8 @@ #define I2C_EXTENSION_STATE_PREPARE 3 #define I2C_EXTENSION_STATE_DROP_READY 4 #define I2C_EXTENSION_STATE_DROP 5 -#define I2C_EXTENSION_STATE_EXIT 6 +#define I2C_EXTENSION_STATE_CATA_OUT 6 +#define I2C_EXTENSION_STATE_EXIT 7 #define I2C_ANS_EXTENSION_STATUS 0x83 ===================================================== aversive_projects/microb2008/extension/i2c_protocol.c (1.14 -> 1.15) ===================================================== @@ -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: i2c_protocol.c,v 1.14 2008-03-29 18:53:12 zer0 Exp $ + * Revision : $Id: i2c_protocol.c,v 1.15 2008-04-01 22:44:41 zer0 Exp $ * */ @@ -266,6 +266,14 @@ break; } + case I2C_CMD_EXTENSION_CATA_OUT: + { + if (size != sizeof(struct i2c_cmd_extension_cata_out)) + goto error; + state_catapult_out(); + break; + } + case I2C_CMD_EXTENSION_CS_DEBUG: { if (size != sizeof(struct i2c_cmd_extension_cs_debug)) ============================================== aversive_projects/microb2008/extension/state.c (1.6 -> 1.7) ============================================== @@ -15,7 +15,7 @@ * along with this program; if not, write to the Free Software * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * - * Revision : $Id: state.c,v 1.6 2008-03-30 22:01:43 zer0 Exp $ + * Revision : $Id: state.c,v 1.7 2008-04-01 22:44:41 zer0 Exp $ * */ @@ -78,6 +78,16 @@ } /* set a new state, return 0 on success */ +int8_t state_catapult_out(void) +{ + if (state == I2C_EXTENSION_STATE_DROP_READY) { + catapult_goto(200, 10000); + state = I2C_EXTENSION_STATE_CATA_OUT; + } + return 0; +} + +/* set a new state, return 0 on success */ int8_t state_exit(void) { DEBUG(E_USER_ST_MACH, "%s", __FUNCTION__); @@ -264,18 +274,22 @@ { int8_t index; uint8_t flags; + uint8_t type; + int32_t destination; if (state != I2C_EXTENSION_STATE_PREPARE) return 0; DEBUG(E_USER_ST_MACH, "%s", __FUNCTION__); + type = prepared_type; + destination = prepared_dest; roller_off(); action_servo_set(SERVO_LEFT_ARM_NUM, SERVO_LEFT_ARM_POS_INSIDE); action_servo_set(SERVO_RIGHT_ARM_NUM, SERVO_RIGHT_ARM_POS_INSIDE); /* prepare barrel */ - index = barrel_prepare(prepared_type, prepared_dest); + index = barrel_prepare(type, destination); DEBUG(E_USER_ST_MACH, "%s: prepare room %d", __FUNCTION__, index); if (index == -1) { DEBUG(E_USER_ST_MACH, "%s: no such ball !", __FUNCTION__); @@ -283,9 +297,12 @@ return 0; } - while(barrel_get_idx(prepared_dest) != index) { + while(barrel_get_idx(destination) != index) { if (state != I2C_EXTENSION_STATE_PREPARE) return 0; + /* we received a new order */ + if (type != prepared_type || destination != prepared_type) + return 0; } wait_ms(150); /* a little wait to be sure that the barrel is @@ -326,6 +343,16 @@ return 0; } +static int8_t state_do_catapult_out(void) +{ + if (state != I2C_EXTENSION_STATE_CATA_OUT) + return 0; + + while(state == I2C_EXTENSION_STATE_CATA_OUT); + catapult_release(); + return 0; +} + uint8_t state_get(void) { return state; @@ -337,6 +364,7 @@ while (state_do_pickup()); while (state_do_harvest()); while (state_do_prepare()); + while(state_do_catapult_out()); while (state_do_drop()); } } ============================================== aversive_projects/microb2008/extension/state.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: state.h,v 1.2 2008-03-24 17:58:19 zer0 Exp $ + * Revision : $Id: state.h,v 1.3 2008-04-01 22:44:41 zer0 Exp $ * */ @@ -28,6 +28,7 @@ int8_t state_pickup(void); int8_t state_prepare(uint8_t type, int32_t destination); int8_t state_drop(uint32_t speed, int32_t pos); +int8_t state_catapult_out(void); int8_t state_exit(void); int8_t state_init(void); uint8_t state_get(void); ======================================= aversive_projects/microb2008/main/cam.c (1.4 -> 1.5) ======================================= @@ -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: cam.c,v 1.4 2008-03-31 22:22:52 zer0 Exp $ + * Revision : $Id: cam.c,v 1.5 2008-04-01 22:44:41 zer0 Exp $ * */ #include <avr/pgmspace.h> @@ -33,6 +33,7 @@ #include "cam.h" #define CAM_NB_BUG_MAX 4 +#define CAM_TIMEOUT 300 /* in ms, about 300 ms */ volatile static uint8_t cam_nb_balls=0; struct cam_ball cam_ball_tab[CAM_NB_BALLS]; @@ -66,6 +67,16 @@ uart1_register_rx_event(cam_recv); } +void cam_enable(void) +{ + cam_bug = 0; +} + +void cam_disable(void) +{ + cam_bug = CAM_NB_BUG_MAX; +} + /* callback fct */ static void cam_recv(char c) { @@ -135,7 +146,7 @@ cam_bug ++; printf_P(PSTR("No answer from camera\r\n")); } - return -1; + return 0; } void cam_dump_balls(void) @@ -143,7 +154,16 @@ uint8_t i; for (i=0 ; i<cam_nb_balls ; i++) { - printf_P(PSTR("flags=%x d=%d a=%d\r\n"), cam_ball_tab[i].flags, - cam_ball_tab[i].distance, cam_ball_tab[i].angle); + if (cam_ball_tab[i].flags == CAM_COLOR_BLUE) + printf_P(PSTR("blue ")); + else if (cam_ball_tab[i].flags == CAM_COLOR_RED) + printf_P(PSTR("red ")); + else if (cam_ball_tab[i].flags == CAM_COLOR_WHITE) + printf_P(PSTR("white ")); + else + printf_P(PSTR("? ")); + printf_P(PSTR("%d cm, %d deg\r\n"), + cam_ball_tab[i].distance, + cam_ball_tab[i].angle); } } ======================================= aversive_projects/microb2008/main/cam.h (1.3 -> 1.4) ======================================= @@ -16,13 +16,10 @@ * 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: cam.h,v 1.3 2008-03-31 22:22:52 zer0 Exp $ + * Revision : $Id: cam.h,v 1.4 2008-04-01 22:44:41 zer0 Exp $ * */ -#define CAM_TIMEOUT 300000L /* in us, about 300 ms */ -#define CAM_TIMEOUT_MS (CAM_TIMEOUT / 1000) - struct cam_ball { #define CAM_COLOR_BLUE 1 /* set if the ball color is blue */ #define CAM_COLOR_RED 2 /* set if the ball color is red */ @@ -41,5 +38,10 @@ extern struct cam_ball cam_ball_tab[CAM_NB_BALLS]; void cam_init(void); + +/* return the number of balls */ int8_t cam_get_balls(void); + void cam_dump_balls(void); +void cam_enable(void); +void cam_disable(void); ============================================ aversive_projects/microb2008/main/commands.c (1.20 -> 1.21) ============================================ @@ -439,7 +439,7 @@ else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) { while (1) { why = goto_and_avoid(pt_list[i].x, pt_list[i].y); - printf("zaedaze\r\n"); + printf("next point\r\n"); if (why != END_OBSTACLE) break; } @@ -927,6 +927,7 @@ { struct cmd_goto_result * res = parsed_result; + interrupt_traj_reset(); if (!strcmp_P(res->arg1, PSTR("a_rel"))) { trajectory_a_rel(&robot.traj, res->arg2); } @@ -962,6 +963,7 @@ //trajectory_goto_xya(&robot.traj, res->arg2, res->arg3, res->arg4); } wait_traj_end(0xFF); + trajectory_stop(&robot.traj); } prog_char str_goto_arg0[] = "goto"; @@ -1647,32 +1649,31 @@ static void cmd_cam_parsed(void * parsed_result, void * data) { struct cmd_cam_result *res = parsed_result; - int8_t n, i; + int8_t n; if (!strcmp_P(res->type, PSTR("init"))) { cam_init(); + return; + } + else if (!strcmp_P(res->type, PSTR("enable"))) { + cam_enable(); + return; + } + else if (!strcmp_P(res->type, PSTR("disable"))) { + cam_disable(); + return; } /* else it's a "dump" or a "harvest" */ n = cam_get_balls(); - for (i=0 ; i<n; i++) { - if (cam_ball_tab[i].flags == CAM_COLOR_BLUE) - printf_P(PSTR("blue ")); - else if (cam_ball_tab[i].flags == CAM_COLOR_RED) - printf_P(PSTR("red ")); - else if (cam_ball_tab[i].flags == CAM_COLOR_WHITE) - printf_P(PSTR("white ")); - else - printf_P(PSTR("? ")); - printf_P(PSTR("%d cm, %d deg\r\n"), - cam_ball_tab[i].distance, - cam_ball_tab[i].angle); - } + cam_dump_balls(); + if (!strcmp_P(res->type, PSTR("harvest"))) { if (n) { + interrupt_traj_reset(); trajectory_goto_d_a_rel(&robot.traj, - cam_ball_tab[i].distance, - cam_ball_tab[i].angle); + cam_ball_tab[0].distance, + cam_ball_tab[0].angle); wait_traj_end(0xFF); trajectory_stop(&robot.traj); } @@ -1681,10 +1682,10 @@ prog_char str_cam_arg0[] = "cam"; parse_pgm_token_string_t cmd_cam_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cam_result, arg0, str_cam_arg0); -prog_char str_cam_type[] = "init#dump"; +prog_char str_cam_type[] = "init#dump#harvest#enable#disable"; parse_pgm_token_string_t cmd_cam_type = TOKEN_STRING_INITIALIZER(struct cmd_cam_result, type, str_cam_type); -prog_char help_cam[] = "Cam the robot"; +prog_char help_cam[] = "Webcam tests"; parse_pgm_inst_t cmd_cam = { .f = cmd_cam_parsed, /* function to call */ .data = NULL, /* 2nd arg of func */ @@ -1711,16 +1712,16 @@ while(uart0_recv_nowait() == -1) { trajectory_goto_forward_xy_abs(&robot.traj, 0, 60); - wait_traj_end(END_TRAJ|END_BLOCKING); + wait_traj_end(0xFF); trajectory_goto_forward_xy_abs(&robot.traj, 60, 60); - wait_traj_end(END_TRAJ|END_BLOCKING); + wait_traj_end(0xFF); trajectory_goto_forward_xy_abs(&robot.traj, 60, 0); - wait_traj_end(END_TRAJ|END_BLOCKING); + wait_traj_end(0xFF); trajectory_goto_forward_xy_abs(&robot.traj, 0, 0); - wait_traj_end(END_TRAJ|END_BLOCKING); + wait_traj_end(0xFF); } } @@ -1730,16 +1731,16 @@ while(uart0_recv_nowait() == -1) { trajectory_goto_forward_xy_abs(&robot.traj, 60, 60); - wait_traj_end(END_TRAJ|END_BLOCKING); + wait_traj_end(0xFF); trajectory_goto_forward_xy_abs(&robot.traj, 0, 60); - wait_traj_end(END_TRAJ|END_BLOCKING); + wait_traj_end(0xFF); trajectory_goto_forward_xy_abs(&robot.traj, 60, 0); - wait_traj_end(END_TRAJ|END_BLOCKING); + wait_traj_end(0xFF); trajectory_goto_forward_xy_abs(&robot.traj, 0, 0); - wait_traj_end(END_TRAJ|END_BLOCKING); + wait_traj_end(0xFF); } } @@ -2583,7 +2584,7 @@ static void cmd_sensor_parsed(void *parsed_result, void *data) { printf_P(PSTR("Sensors state:\r\n" - " %lx\r\n"), robot.sensor); + " %8.8lx\r\n"), robot.sensor); } prog_char str_sensor_arg0[] = "sensor"; ================================================ aversive_projects/microb2008/main/i2c_protocol.c (1.14 -> 1.15) ================================================ @@ -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: i2c_protocol.c,v 1.14 2008-03-31 22:22:52 zer0 Exp $ + * Revision : $Id: i2c_protocol.c,v 1.15 2008-04-01 22:44:41 zer0 Exp $ * */ @@ -426,6 +426,13 @@ return i2c_send_command((uint8_t*)&buf, sizeof(buf)); } +int8_t i2c_catapult_out(void) +{ + struct i2c_cmd_extension_cata_out buf; + buf.hdr.cmd = I2C_CMD_EXTENSION_CATA_OUT; + return i2c_send_command((uint8_t*)&buf, sizeof(buf)); +} + int8_t i2c_cs_debug(uint8_t cmd, int32_t val) { struct i2c_cmd_extension_cs_debug buf; ================================================ aversive_projects/microb2008/main/i2c_protocol.h (1.10 -> 1.11) ================================================ @@ -15,7 +15,7 @@ * along with this program; if not, write to the Free Software * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * - * Revision : $Id: i2c_protocol.h,v 1.10 2008-03-31 22:22:52 zer0 Exp $ + * Revision : $Id: i2c_protocol.h,v 1.11 2008-04-01 22:44:41 zer0 Exp $ * */ @@ -42,6 +42,7 @@ int8_t i2c_pickup(void); int8_t i2c_prepare_ball(uint8_t ball_type, uint8_t drop_dst); int8_t i2c_drop(int32_t catapult_speed, uint32_t catapult_pos); +int8_t i2c_catapult_out(void); int8_t i2c_cs_debug(uint8_t cmd, int32_t val); int8_t i2c_set_color(uint8_t color); int8_t i2c_set_arm(uint8_t arm); ======================================== aversive_projects/microb2008/main/main.c (1.27 -> 1.28) ======================================== @@ -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.27 2008-04-01 11:47:19 zer0 Exp $ + * Revision : $Id: main.c,v 1.28 2008-04-01 22:44:41 zer0 Exp $ * */ @@ -157,7 +157,7 @@ position_get_a_deg_s16(&robot.pos)); } -#if 0 +#if 0 /* reactivate it before competition */ if(robot.cs_events & DO_TIMER) { uint8_t second; second = time_get_s(); @@ -235,7 +235,9 @@ static uint8_t i = 0; /* interrupt traj here XXX */ - + if (c == '\003') + interrupt_traj(); + if( (i == 0 && c == 'p') || (i == 1 && c == 'o') || (i == 2 && c == 'p') ) @@ -412,6 +414,9 @@ scheduler_add_periodical_event_priority(do_led_blink, NULL, 100000L / SCHEDULER_UNIT, LED_PRIO); + /* PWM */ + pwm_init(); + /* I2C */ wait_ms(50); i2c_protocol_init(); @@ -421,9 +426,6 @@ scheduler_add_periodical_event_priority(i2c_poll_slaves, NULL, 10000L / SCHEDULER_UNIT, I2C_POLL_PRIO); - /* PWM */ - pwm_init(); - /* ROBOT_SYSTEM */ rs_init(&robot.rs); rs_set_left_pwm(&robot.rs, pwm_set_and_save, LEFT_PWM); ========================================= aversive_projects/microb2008/main/strat.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: strat.c,v 1.9 2008-04-01 13:50:09 zer0 Exp $ + * Revision : $Id: strat.c,v 1.10 2008-04-01 22:44:41 zer0 Exp $ * */ @@ -40,6 +40,8 @@ #define DISP_MAX_TRIES 2 +#define TRAJ_STD_FLAGS (END_TRAJ|END_BLOCKING|END_OBSTACLE) + struct v_dispenser { int8_t count; uint8_t tries; @@ -58,6 +60,7 @@ }; static uint8_t h_disp_enabled; +static uint8_t last_color_std_cont = I2C_BALL_TYPE_COLORED; /* call it before each strat */ void strat_init(void) @@ -68,6 +71,7 @@ i2c_set_arm(I2C_EXTENSION_ARM_HARVEST); trajectory_set_speed(&robot.traj, 300, 300); time_reset(); + interrupt_traj_reset(); colored_disp.count = 5; white_disp.count = 5; h_disp_enabled = 0; @@ -79,15 +83,23 @@ { i2c_set_roller(0); i2c_set_arm(I2C_EXTENSION_ARM_INSIDE); + i2c_harvest(); robot.cs_events &= DO_TIMER; } -uint8_t strat_unblock(void) +/* XXX very pas terrible */ +static uint8_t strat_unblock(uint8_t err) { uint8_t flags; int32_t pwm_l, pwm_r; DEBUG(E_USER_STRAT, "%s", __FUNCTION__); + + if (err == END_BLOCKING) { + bd_reset(&robot.bd_l); + bd_reset(&robot.bd_r); + } + IRQ_LOCK(flags); pwm_l = robot.pwm_l; pwm_r = robot.pwm_r; @@ -116,42 +128,40 @@ DEBUG(E_USER_STRAT, "%s", __FUNCTION__); trajectory_goto_forward_xy_abs(&robot.traj, BALL7_X, BALL7_Y); - err = wait_traj_end(END_TRAJ|END_BLOCKING); - if (err == END_BLOCKING) - return strat_unblock(); + err = wait_traj_end(TRAJ_STD_FLAGS); + if (!traj_success(err)) + return err; trajectory_goto_forward_xy_abs(&robot.traj, BALL11_X, BALL11_Y); - err = wait_traj_end(END_TRAJ|END_BLOCKING); - if (err == END_BLOCKING) - return strat_unblock(); + err = wait_traj_end(TRAJ_STD_FLAGS); + if (!traj_success(err)) + return err; trajectory_goto_forward_xy_abs(&robot.traj, BALL9_X, BALL9_Y); - err = wait_traj_end(END_TRAJ|END_BLOCKING); - if (err == END_BLOCKING) - return strat_unblock(); + err = wait_traj_end(TRAJ_STD_FLAGS); + if (!traj_success(err)) + return err; trajectory_goto_forward_xy_abs(&robot.traj, BALL5_X, BALL5_Y); - err = wait_traj_end(END_TRAJ|END_BLOCKING); - if (err == END_BLOCKING) - return strat_unblock(); + err = wait_traj_end(TRAJ_STD_FLAGS); + if (!traj_success(err)) + return err; trajectory_goto_forward_xy_abs(&robot.traj, BALL3_X, BALL3_Y); - err = wait_traj_end(END_TRAJ|END_BLOCKING); - if (err == END_BLOCKING) - return strat_unblock(); + err = wait_traj_end(TRAJ_STD_FLAGS); + if (!traj_success(err)) + return err; trajectory_goto_forward_xy_abs(&robot.traj, BALL1_X, BALL1_Y + COL_SIGN(10)); - err = wait_traj_end(END_TRAJ|END_BLOCKING); - if (err == END_BLOCKING) - return strat_unblock(); + err = wait_traj_end(TRAJ_STD_FLAGS); + if (!traj_success(err)) + return err; trajectory_goto_forward_xy_abs(&robot.traj, BALL4_X, BALL4_Y); - err = wait_traj_end(END_TRAJ|END_BLOCKING); - if (err == END_BLOCKING) - return strat_unblock(); - - + err = wait_traj_end(TRAJ_STD_FLAGS); + if (!traj_success(err)) + return err; return END_TRAJ; } @@ -164,6 +174,8 @@ uint8_t initial_count; uint8_t prev_retrieved_balls = 0; uint8_t retrieved_balls = 0; + uint8_t err; + microseconds us; if (disp->tries >= DISP_MAX_TRIES || disp->count == 0) @@ -212,9 +224,9 @@ disp->count = 0; trajectory_d_rel(&robot.traj, -10); - wait_traj_end(END_TRAJ|END_BLOCKING); - - return END_TRAJ; + err = wait_traj_end(TRAJ_STD_FLAGS); + + return err; } /* */ @@ -231,20 +243,20 @@ DEBUG(E_USER_STRAT, "%s, %d balls remaining", __FUNCTION__, white_disp.count); - trajectory_goto_xy_abs(&robot.traj, PREPARE_V_WHI_DISP_X, V_WHI_DISP_Y); - err = wait_traj_end(END_TRAJ|END_BLOCKING); - if (err == END_BLOCKING) - return strat_unblock(); + trajectory_goto_forward_xy_abs(&robot.traj, PREPARE_V_WHI_DISP_X, V_WHI_DISP_Y); + err = wait_traj_end(TRAJ_STD_FLAGS); + if (!traj_success(err)) + return err; trajectory_a_abs(&robot.traj, V_WHI_DISP_A); - err = wait_traj_end(END_TRAJ|END_BLOCKING); - if (err == END_BLOCKING) - return strat_unblock(); + err = wait_traj_end(TRAJ_STD_FLAGS); + if (!traj_success(err)) + return err; trajectory_set_speed(&robot.traj, 100, 300); - trajectory_goto_xy_abs(&robot.traj, V_WHI_DISP_X, V_WHI_DISP_Y); - err = wait_traj_end(END_TRAJ|END_BLOCKING); + trajectory_goto_forward_xy_abs(&robot.traj, V_WHI_DISP_X, V_WHI_DISP_Y); + err = wait_traj_end(TRAJ_STD_FLAGS); DEBUG(E_USER_STRAT, "%s end=%d", __FUNCTION__, err); trajectory_d_rel(&robot.traj, -1); @@ -269,20 +281,20 @@ DEBUG(E_USER_STRAT, "%s, %d balls remaining", __FUNCTION__, colored_disp.count); - trajectory_goto_xy_abs(&robot.traj, V_COL_DISP_X, PREPARE_V_COL_DISP_Y); - err = wait_traj_end(END_TRAJ|END_BLOCKING); - if (err == END_BLOCKING) - return strat_unblock(); + trajectory_goto_forward_xy_abs(&robot.traj, V_COL_DISP_X, PREPARE_V_COL_DISP_Y); + err = wait_traj_end(TRAJ_STD_FLAGS); + if (!traj_success(err)) + return err; trajectory_a_abs(&robot.traj, V_COL_DISP_A); - err = wait_traj_end(END_TRAJ|END_BLOCKING); - if (err == END_BLOCKING) - return strat_unblock(); + err = wait_traj_end(TRAJ_STD_FLAGS); + if (!traj_success(err)) + return err; trajectory_set_speed(&robot.traj, 100, 300); - trajectory_goto_xy_abs(&robot.traj, V_COL_DISP_X, V_COL_DISP_Y); - err = wait_traj_end(END_TRAJ|END_BLOCKING); + trajectory_goto_forward_xy_abs(&robot.traj, V_COL_DISP_X, V_COL_DISP_Y); + err = wait_traj_end(TRAJ_STD_FLAGS); DEBUG(E_USER_STRAT, "%s end=%d", __FUNCTION__, err); trajectory_d_rel(&robot.traj, -1); @@ -298,7 +310,6 @@ { uint8_t err; - /* XXX improve it here, use force and time */ if (barrel_colored_count() == 0) return END_TRAJ; @@ -310,14 +321,14 @@ if (!can_shot()) { /* go to the center of area */ trajectory_goto_xy_abs(&robot.traj, CENTER_X, CENTER_Y); - while(test_traj_end(END_TRAJ|END_BLOCKING) == 0 && !can_shot()); + while(test_traj_end(TRAJ_STD_FLAGS) == 0 && !can_shot()); /* XXX process blocking */ } trajectory_turnto_xy_behind(&robot.traj, GOAL_X, GOAL_Y); - err = wait_traj_end(END_TRAJ|END_BLOCKING); - if (err == END_BLOCKING) - return strat_unblock(); + err = wait_traj_end(TRAJ_STD_FLAGS); + if (!traj_success(err) && err != END_TIMER) + return err; while (barrel_colored_count()) { @@ -349,35 +360,35 @@ else side = I2C_DST_DROP_RIGHT; - if (barrel_white_count() != 0) + if (barrel_white_count() != 0 && + last_color_std_cont == I2C_BALL_TYPE_COLORED) col = I2C_BALL_TYPE_WHITE; else col = I2C_BALL_TYPE_COLORED; i2c_prepare_ball(col, side); - /* XXX go forward ? and check blocking ... */ - trajectory_goto_xy_abs(&robot.traj, STD_CONT_X_MID, PREPARE_STD_CONT_Y); - err = wait_traj_end(END_TRAJ|END_BLOCKING); - if (err == END_BLOCKING) - return strat_unblock(); + trajectory_goto_forward_xy_abs(&robot.traj, STD_CONT_X_MID, PREPARE_STD_CONT_Y); + err = wait_traj_end(TRAJ_STD_FLAGS); + if (!traj_success(err) && err != END_TIMER) + return err; - trajectory_goto_xy_abs(&robot.traj, STD_CONT_X_MID, STD_CONT_Y); - wait_traj_end(END_TRAJ|END_BLOCKING); + trajectory_goto_forward_xy_abs(&robot.traj, STD_CONT_X_MID, STD_CONT_Y); + wait_traj_end(TRAJ_STD_FLAGS); trajectory_d_rel(&robot.traj, -5); - wait_traj_end(END_TRAJ|END_BLOCKING); + wait_traj_end(TRAJ_STD_FLAGS); trajectory_a_abs(&robot.traj, STD_CONT_A); - err = wait_traj_end(END_TRAJ|END_BLOCKING); - if (err == END_BLOCKING) - return strat_unblock(); - + err = wait_traj_end(TRAJ_STD_FLAGS); + if (!traj_success(err) && err != END_TIMER) + return err; - /* XXX do something better for order */ + /* try to order balls as much as possible */ while (col != I2C_BALL_TYPE_EMPTY) { while(extension_state() != I2C_EXTENSION_STATE_DROP_READY); i2c_drop(0, 0); while(extension_state() != I2C_EXTENSION_STATE_HARVEST); + last_color_std_cont = col; if (col == I2C_BALL_TYPE_COLORED) { if (barrel_white_count()) @@ -407,28 +418,9 @@ DEBUG(E_USER_STRAT, "%s, %d white and %d colored", __FUNCTION__, barrel_white_count(), barrel_colored_count()); - /* si il reste peu de temps, il faut mettre les points quel - * que soit ce qu'on a. - * - * si on est proche du conteneur standard, c'est plus malin - * d'aller faire des combinaisons. - * - * si on est proche d'un distributeur couleur plein, qu'on a - * (nb_col+nb_vide*2) >= 5 et qu'il reste plein de temps, (ça - * peut etre le cas apres le ramassage des balles statiques) - * alors c'est plus malin de ne rien faire, la suite de la - * strat videra le distributeur. - * - * si on a des balles de couleurs, qu'on est proche du dist - * couleur avec qqs balles alors on ne tire que les couleurs. - * - * si on a peu de balles et beaucoup de temps, on ne fait - * rien, on sera rappelé plus tard de toute façon. - * - */ - /* not much time left ! */ - if (time_get_s() > MATCH_TIME - 10) { + if (time_get_s() > MATCH_TIME - 15) { + /* XXX try to shot white balls ? */ if (barrel_white_count() != 0) { DEBUG(E_USER_STRAT, "%s not much time, eject balls", __FUNCTION__); return strat_eject_balls(); @@ -445,42 +437,111 @@ return strat_eject_balls(); } -#if 0 /* if we are close of col disp, */ if (distance_from_robot(V_COL_DISP_X, V_COL_DISP_Y) < 60) { - if (barrel_empty_count()*2 + barrel_colored_count() >= 5) { - + /* if there is some balls in container, just shot our + * colored balls */ + if ((barrel_empty_count() + barrel_colored_count()) > colored_disp.count) { + DEBUG(E_USER_STRAT, "%s we are close of colored disp, shot", __FUNCTION__); + return strat_shot_balls(); } } -#endif - /* XXX currently very bof */ - if (barrel_white_count() < 3) - return strat_shot_balls(); - else + /* if we have lots of balls */ + if (barrel_white_count() + barrel_colored_count() >= 4) { + + /* if we don't have lots of white */ + if (barrel_white_count() <= 1) + return strat_shot_balls(); + + /* else eject */ return strat_eject_balls(); + } + + /* else do nothing... we will be called later */ + return END_TRAJ; } /* free balls contained in horizontal dispenser */ static uint8_t strat_enable_h_disp(void) { - /* XXX check that we have at least a free room in barrel, - * because Hdisp is enabled with the catapult */ + uint16_t hdisp1_dist, hdisp2_dist; + uint8_t err; if (h_disp_enabled) return END_TRAJ; + /* we must have a free room in barrel because Hdisp is enabled + * with the catapult */ + if (barrel_colored_count() + barrel_white_count() >= 5) + return END_TRAJ; + DEBUG(E_USER_STRAT, "%s", __FUNCTION__); i2c_prepare_ball(I2C_BALL_TYPE_EMPTY, I2C_DST_DROP_SHOT); - /* goto... */ - /* check status */ + hdisp1_dist = distance_from_robot(H_DISP1_X, H_DISP_Y); + hdisp2_dist = distance_from_robot(H_DISP2_X, H_DISP_Y); + + if (hdisp1_dist < hdisp2_dist) { + DEBUG(E_USER_STRAT, "%s go to Hdisp 1", __FUNCTION__); - i2c_drop(CATAPULT_SPEED_H_DISP, CATAPULT_POS_H_DISP); + trajectory_goto_forward_xy_abs(&robot.traj, H_DISP1_X, + PREPARE_H_DISP_Y); + err = wait_traj_end(TRAJ_STD_FLAGS); + if (!traj_success(err)) + goto err; + + trajectory_a_abs(&robot.traj, H_DISP_A); + err = wait_traj_end(TRAJ_STD_FLAGS); + if (!traj_success(err)) + goto err; + + trajectory_set_speed(&robot.traj, 100, 300); + i2c_catapult_out(); + /* backwards */ + trajectory_goto_xy_abs(&robot.traj, V_WHI_DISP_X, V_WHI_DISP_Y); + err = wait_traj_end(TRAJ_STD_FLAGS); + if (!traj_success(err)) + goto err_restore_params; + + } + else { + DEBUG(E_USER_STRAT, "%s go to Hdisp 2", __FUNCTION__); + + trajectory_goto_forward_xy_abs(&robot.traj, H_DISP2_X, + PREPARE_H_DISP_Y); + err = wait_traj_end(TRAJ_STD_FLAGS); + if (!traj_success(err)) + goto err; + + trajectory_a_abs(&robot.traj, H_DISP_A); + err = wait_traj_end(TRAJ_STD_FLAGS); + if (!traj_success(err)) + goto err; + + trajectory_set_speed(&robot.traj, 100, 300); + i2c_catapult_out(); + + /* backwards */ + trajectory_goto_xy_abs(&robot.traj, V_WHI_DISP_X, V_WHI_DISP_Y); + err = wait_traj_end(TRAJ_STD_FLAGS); + if (!traj_success(err)) + goto err_restore_params; + } + + i2c_harvest(); + trajectory_set_speed(&robot.traj, 300, 300); h_disp_enabled = 1; + return END_TRAJ; + + err_restore_params: + i2c_harvest(); + trajectory_set_speed(&robot.traj, 300, 300); + err: + return err; } /* search balls with cam and get them */ @@ -520,16 +581,33 @@ return END_TRAJ; } + +#define HANDLE_ERROR(err) \ +if (err == END_INTR) \ + goto out; \ +if (err == END_TIMER) \ + goto timer; \ +if (err != END_TRAJ) { \ + DEBUG(E_USER_STRAT, "error %d line %d", err, __LINE__); \ + strat_unblock(err); \ +} + + uint8_t strat_main(void) { uint8_t err; err = strat_get_static_balls(); + HANDLE_ERROR(err); err = strat_drop_balls(); + HANDLE_ERROR(err); while(1) { err = strat_get_colored_v_disp(); - strat_drop_balls(); + HANDLE_ERROR(err); + + err = strat_drop_balls(); + HANDLE_ERROR(err); /* si on a une chance s'y retourner sans encombres, on retente */ if (err == END_TRAJ && colored_disp.count != 0 && @@ -537,7 +615,10 @@ continue; err = strat_get_white_v_disp(); - strat_drop_balls(); + HANDLE_ERROR(err); + + err = strat_drop_balls(); + HANDLE_ERROR(err); if (white_disp.count != 0 && white_disp.tries < DISP_MAX_TRIES) @@ -546,15 +627,28 @@ /* faut-il le faire tout le temps ? ca peut dependre * de la config ? */ err = strat_enable_h_disp(); + HANDLE_ERROR(err); err = strat_get_balls_cam(); if (err == END_ERROR) { err = strat_get_balls(); } + /* no else here */ + HANDLE_ERROR(err); + err = strat_drop_balls(); + HANDLE_ERROR(err); err = strat_get_balls_borders(); + HANDLE_ERROR(err); + err = strat_drop_balls(); + HANDLE_ERROR(err); + + timer: + strat_drop_balls(); } + + out: return (0); } ========================================= aversive_projects/microb2008/main/strat.h (1.6 -> 1.7) ========================================= @@ -15,7 +15,7 @@ * along with this program; if not, write to the Free Software * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * - * Revision : $Id: strat.h,v 1.6 2008-04-01 13:50:09 zer0 Exp $ + * Revision : $Id: strat.h,v 1.7 2008-04-01 22:44:41 zer0 Exp $ * */ @@ -54,6 +54,12 @@ #define STD_CONT_Y COL_COORD_Y(200) #define STD_CONT_A COL_ANGLE(0) +#define H_DISP1_X 125 +#define H_DISP2_X 175 +#define PREPARE_H_DISP_Y COL_COORD_Y(35) +#define H_DISP_Y COL_COORD_Y(15) +#define H_DISP_A COL_ANGLE(90) + /* from left to right, and from up to down */ /* * +--------^----------------^--------+ ============================================== aversive_projects/microb2008/main/strat_base.c (1.10 -> 1.11) ============================================== @@ -15,7 +15,7 @@ * along with this program; if not, write to the Free Software * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA * - * Revision : $Id: strat_base.c,v 1.10 2008-03-31 22:22:52 zer0 Exp $ + * Revision : $Id: strat_base.c,v 1.11 2008-04-01 22:44:41 zer0 Exp $ * */ #include <stdio.h> @@ -45,9 +45,9 @@ "END_BLOCKING", "END_NEAR", "END_OBSTACLE", - "", - "", - "", + "END_ERROR", + "END_INTR", + "END_TIMER", "END_UNKNOWN", }; @@ -136,6 +136,26 @@ position_get_y_s16(&robot.pos), x, y); } +void rel_da_to_abs_xy(double d_rel, double a_rel_deg, + double *x_abs, double *y_abs) +{ + double x = position_get_x_double(&robot.pos); + double y = position_get_y_double(&robot.pos); + double a = position_get_a_rad_double(&robot.pos); + + *x_abs = x + d_rel*cos(a+a_rel_deg); + *y_abs = y + d_rel*sin(a+a_rel_deg); +} + +void rel_xy_to_abs_xy(double x_rel, double y_rel, + double *x_abs, double *y_abs) +{ + double d_rel, a_rel; + d_rel = sqrt(x_rel*x_rel + y_rel*y_rel); + a_rel = atan2(y_rel, x_rel) * 180. / M_PI; + rel_da_to_abs_xy(d_rel, a_rel, x_abs, y_abs); +} + int8_t can_shot(void) { double posx = position_get_x_double(&robot.pos); @@ -353,21 +373,40 @@ wait_ms(500); } #endif - strat_init(); strat_main(); strat_uninit(); } + void interrupt_traj(void) { traj_intr = 1; } +void interrupt_traj_reset(void) +{ + traj_intr = 0; +} + uint8_t test_traj_end(uint8_t why) { + static uint16_t prev_timer = 0; + uint16_t cur_timer; + + /* trigger an event every 5 seconds at the end of the match if + * we have balls in the barrel */ + cur_timer = time_get_s(); + if (cur_timer >= MATCH_TIME - 15) { + if ((cur_timer % 5 == 3) && (cur_timer != prev_timer)) { + prev_timer = cur_timer; + if (barrel_colored_count() || barrel_white_count()) + return END_TIMER; + } + } + if ((why & END_INTR) && traj_intr) { - traj_intr = 0; + interrupt_traj_reset(); return END_INTR; } @@ -385,6 +424,9 @@ if( (why & END_OBSTACLE) && sensor_obstacle() ) { double x,y,a; + + /* XXX check that the obstacle is in the area !! */ + /* disable sensor during some time, and set oponent * pos */ sensor_obstacle_disable(); @@ -412,3 +454,10 @@ } } } + +uint8_t traj_success(uint8_t err) +{ + if (err == END_TRAJ && err == END_NEAR) + return 1; + return 0; +} ============================================== aversive_projects/microb2008/main/strat_base.h (1.8 -> 1.9) ============================================== @@ -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: strat_base.h,v 1.8 2008-03-31 22:22:52 zer0 Exp $ + * Revision : $Id: strat_base.h,v 1.9 2008-04-01 22:44:41 zer0 Exp $ * */ @@ -30,7 +30,9 @@ #define END_NEAR 4 /* we are near destination */ #define END_OBSTACLE 8 /* There is an obstacle in front of us */ #define END_ERROR 16 /* Cannot do the command */ -#define END_INTR 128 /* interrupted by user */ +#define END_INTR 32 /* interrupted by user */ +#define END_TIMER 64 /* we don't a lot of time */ +#define END_UNKNOWN 128 /* */ #define BLUE I2C_EXTENSION_COLOR_BLUE #define RED I2C_EXTENSION_COLOR_RED @@ -61,13 +63,22 @@ /* return 1 if position of the robot is ok for shot */ int8_t can_shot(void); +/* relative to absolute position */ +void rel_xy_to_abs_xy(double x_rel, double y_rel, + double *x_abs, double *y_abs); +void rel_da_to_abs_xy(double d_rel, double a_rel_deg, + double *x_abs, double *y_abs); + + uint8_t get_color(void); uint8_t get_opponent_color(void); void start(void); void interrupt_traj(void); +void interrupt_traj_reset(void); uint8_t test_traj_end(uint8_t why); uint8_t wait_traj_end(uint8_t why); +uint8_t traj_success(uint8_t err); uint8_t barrel_full(void); uint8_t barrel_colored_count(void); Commit from zer0 on branch b_zer0 (2008-04-02 00:45 CEST) ================================= change a divisor... not tested aversive modules/devices/robot/trajectory_manager/trajectory_manager.c 1.4.4.10 ====================================================================== aversive/modules/devices/robot/trajectory_manager/trajectory_manager.c (1.4.4.9 -> 1.4.4.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: trajectory_manager.c,v 1.4.4.9 2008-03-31 22:16:50 zer0 Exp $ + * Revision : $Id: trajectory_manager.c,v 1.4.4.10 2008-04-01 22:45:09 zer0 Exp $ * */ @@ -532,10 +532,10 @@ d_consign += rs_get_distance(traj->robot); /* angle consign */ - /* XXX here we specify 2.2 instead of 2.0 to avoid oscillations */ + /* XXX here we specify 1.1 instead of 1.0 to avoid oscillations */ a_consign = (int32_t)(v2pol_target.theta * (traj->position->phys.distance_imp_per_cm)* - (traj->position->phys.track_cm) / 2.2); + (traj->position->phys.track_cm) / 1.1); a_consign += rs_get_angle(traj->robot); break;
_______________________________________________ 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