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
[email protected]
CVSWEB : http://cvsweb.droids-corp.org/cgi-bin/viewcvs.cgi/aversive
WIKI : http://wiki.droids-corp.org/index.php/Aversive
DOXYGEN : http://zer0.droids-corp.org/doxygen_aversive/html/
BUGZILLA : http://bugzilla.droids-corp.org
COMMIT LOGS : http://zer0.droids-corp.org/aversive_commitlog
