# error not supported
#endif
- //eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_BALLBOARD);
+ eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_BALLBOARD);
/* check eeprom to avoid to run the bad program */
if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) !=
EEPROM_MAGIC_BALLBOARD) {
*
*/
+/* was sensorboard in 2009 */
+
#define LED_TOGGLE(port, bit) do { \
if (port & _BV(bit)) \
port &= ~_BV(bit); \
# error not supported
#endif
- //eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_COBBOARD);
+ eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_COBBOARD);
/* check eeprom to avoid to run the bad program */
if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) !=
EEPROM_MAGIC_COBBOARD) {
*
*/
+/* was mainboard in 2009 */
+
#define LED_TOGGLE(port, bit) do { \
if (port & _BV(bit)) \
port &= ~_BV(bit); \
};
/* pullup mapping:
- * CAP 1,5,6,7,8
+ * CAP 1,5,6,7,8 (notation elec)
*/
static struct sensor_filter sensor_filter[SENSOR_MAX] = {
[S_COB_INSIDE_L] = { 5, 0, 4, 1, 0, 1 }, /* 0 */
[S_COB_INSIDE_R] = { 5, 0, 4, 1, 0, 0 }, /* 2 */
[S_CAP4] = { 1, 0, 0, 1, 0, 0 }, /* 3 */
[S_LCOB] = { 1, 0, 0, 1, 0, 1 }, /* 4 */
- [S_LEFT] = { 5, 0, 4, 1, 0, 0 }, /* 5 */ /////// not used
- [S_RIGHT] = { 5, 0, 4, 1, 0, 1 }, /* 6 */ /////// not used
+ [S_CAP6] = { 5, 0, 4, 1, 0, 0 }, /* 5 */
+ [S_CAP7] = { 5, 0, 4, 1, 0, 1 }, /* 6 */
[S_RCOB] = { 1, 0, 0, 1, 0, 1 }, /* 7 */
[S_RESERVED1] = { 10, 0, 3, 7, 0, 0 }, /* 8 */
[S_RESERVED2] = { 10, 0, 3, 7, 0, 0 }, /* 9 */
#define S_COB_INSIDE_R 2
#define S_CAP4 3
#define S_LCOB 4
-#define S_LEFT 5
-#define S_RIGHT 6
+#define S_CAP6 5
+#define S_CAP7 6
#define S_RCOB 7
#define S_RESERVED1 8
#define S_RESERVED2 9
&cobboard.right_spickle,
},
.pos_deployed = {
- 200, /* left */
- 200, /* right */
+ 7000, // 200, /* left */
+ 7000, // 200, /* right */
},
.pos_mid = {
25000, /* left */
#define I2C_COB_BLACK 0
#define I2C_COB_WHITE 1
+#define I2C_COB_UNKNOWN 2
+#define I2C_COB_REMOVED 3
struct i2c_cmd_hdr {
uint8_t cmd;
SRC += commands_cs.c commands_mainboard.c commands_traj.c commands.c
SRC += i2c_protocol.c sensor.c actuator.c cs.c ax12_user.c
SRC += strat_utils.c strat_base.c strat.c strat_corn.c
+SRC += strat_db.c
ifeq ($(H),1)
SRC += robotsim.c
endif
extern parse_pgm_inst_t cmd_goto3;
extern parse_pgm_inst_t cmd_position;
extern parse_pgm_inst_t cmd_position_set;
-extern parse_pgm_inst_t cmd_strat_infos;
+extern parse_pgm_inst_t cmd_strat_db;
extern parse_pgm_inst_t cmd_strat_conf;
extern parse_pgm_inst_t cmd_strat_conf2;
extern parse_pgm_inst_t cmd_strat_conf3;
(parse_pgm_inst_t *)&cmd_goto2,
(parse_pgm_inst_t *)&cmd_position,
(parse_pgm_inst_t *)&cmd_position_set,
- (parse_pgm_inst_t *)&cmd_strat_infos,
+ (parse_pgm_inst_t *)&cmd_strat_db,
(parse_pgm_inst_t *)&cmd_strat_conf,
(parse_pgm_inst_t *)&cmd_strat_conf2,
(parse_pgm_inst_t *)&cmd_strat_conf3,
#include "strat.h"
#include "strat_utils.h"
#include "strat_base.h"
+#include "strat_db.h"
#include "strat_corn.h"
#include "i2c_protocol.h"
#include "actuator.h"
gen.logs[NB_LOGS] = E_USER_STRAT;
if (!strcmp_P(res->debug, PSTR("debug"))) {
- strat_infos.dump_enabled = 1;
+ strat_db.dump_enabled = 1;
gen.log_level = 5;
}
else {
- strat_infos.dump_enabled = 0;
+ strat_db.dump_enabled = 0;
gen.log_level = 0;
}
int32_t dist;
};
-#if 0
-static void reverse_line(struct line_2pts *l)
-{
- point_t tmp;
-
- tmp.x = l->p1.x;
- tmp.y = l->p1.y;
- l->p1.x = l->p2.x;
- l->p1.y = l->p2.y;
- l->p2.x = tmp.x;
- l->p2.y = tmp.y;
-}
-#endif
-
/* function called when cmd_test is parsed successfully */
static void cmd_test_parsed(void *parsed_result, void *data)
{
- uint8_t err;
-
-#ifdef HOST_VERSION
- strat_reset_pos(298.48, COLOR_Y(309.21),
- COLOR_A(70.02));
- mainboard.angle.on = 1;
- mainboard.distance.on = 1;
- strat_set_speed(250, SPEED_ANGLE_FAST);
-#endif
- init_corn_table(0, 0);
- time_wait_ms(100);
-
- err = line2line(LINE_UP, 0, LINE_R_DOWN, 2);
- err = line2line(LINE_R_DOWN, 2, LINE_R_UP, 2);
- err = line2line(LINE_R_UP, 2, LINE_UP, 5);
- trajectory_hardstop(&mainboard.traj);
-
- /* ball ejection */
- trajectory_a_abs(&mainboard.traj, COLOR_A(90));
- i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT);
- time_wait_ms(2000);
-
- /* half turn */
- trajectory_goto_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847));
- err = wait_traj_end(END_INTR|END_TRAJ);
- i2c_cobboard_mode_pack(I2C_LEFT_SIDE);
- i2c_cobboard_mode_pack(I2C_RIGHT_SIDE);
- trajectory_a_rel(&mainboard.traj, COLOR_A(180));
- err = wait_traj_end(END_INTR|END_TRAJ);
-
- /* cob ejection */
- trajectory_d_rel(&mainboard.traj, -100);
- err = wait_traj_end(END_INTR|END_TRAJ);
- i2c_cobboard_mode_eject();
- time_wait_ms(2000);
}
prog_char str_test_arg0[] = "test";
#include "strat_utils.h"
#include "strat_base.h"
#include "strat.h"
+#include "strat_db.h"
#include "../common/i2c_commands.h"
#include "i2c_protocol.h"
/**********************************************************/
/* strat configuration */
-/* this structure is filled when cmd_strat_infos is parsed successfully */
-struct cmd_strat_infos_result {
+/* this structure is filled when cmd_strat_db is parsed successfully */
+struct cmd_strat_db_result {
fixed_string_t arg0;
fixed_string_t arg1;
};
-/* function called when cmd_strat_infos is parsed successfully */
-static void cmd_strat_infos_parsed(void *parsed_result, void *data)
+/* function called when cmd_strat_db is parsed successfully */
+static void cmd_strat_db_parsed(void *parsed_result, void *data)
{
- struct cmd_strat_infos_result *res = parsed_result;
+ struct cmd_strat_db_result *res = parsed_result;
if (!strcmp_P(res->arg1, PSTR("reset"))) {
- strat_reset_infos();
+ strat_db_init();
}
- strat_infos.dump_enabled = 1;
- strat_dump_infos(__FUNCTION__);
+ strat_db.dump_enabled = 1;
+ strat_db_dump(__FUNCTION__);
}
-prog_char str_strat_infos_arg0[] = "strat_infos";
-parse_pgm_token_string_t cmd_strat_infos_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg0, str_strat_infos_arg0);
-prog_char str_strat_infos_arg1[] = "show#reset";
-parse_pgm_token_string_t cmd_strat_infos_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_infos_result, arg1, str_strat_infos_arg1);
+prog_char str_strat_db_arg0[] = "strat_db";
+parse_pgm_token_string_t cmd_strat_db_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_db_result, arg0, str_strat_db_arg0);
+prog_char str_strat_db_arg1[] = "show#reset";
+parse_pgm_token_string_t cmd_strat_db_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_db_result, arg1, str_strat_db_arg1);
-prog_char help_strat_infos[] = "reset/show strat_infos";
-parse_pgm_inst_t cmd_strat_infos = {
- .f = cmd_strat_infos_parsed, /* function to call */
+prog_char help_strat_db[] = "reset/show strat_db";
+parse_pgm_inst_t cmd_strat_db = {
+ .f = cmd_strat_db_parsed, /* function to call */
.data = NULL, /* 2nd arg of func */
- .help_str = help_strat_infos,
+ .help_str = help_strat_db,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_strat_infos_arg0,
- (prog_void *)&cmd_strat_infos_arg1,
+ (prog_void *)&cmd_strat_db_arg0,
+ (prog_void *)&cmd_strat_db_arg1,
NULL,
},
};
static void cmd_strat_conf_parsed(void *parsed_result, void *data)
{
// struct cmd_strat_conf_result *res = parsed_result;
-
- strat_infos.dump_enabled = 1;
- strat_dump_conf();
+ strat_conf.dump_enabled = 1;
+ strat_conf_dump(__FUNCTION__);
}
prog_char str_strat_conf_arg0[] = "strat_conf";
#endif
if (on)
- strat_infos.conf.flags |= bit;
+ strat_conf.flags |= bit;
else
- strat_infos.conf.flags &= (~bit);
+ strat_conf.flags &= (~bit);
- strat_infos.dump_enabled = 1;
- strat_dump_conf();
+ strat_conf.dump_enabled = 1;
+ strat_conf_dump(__FUNCTION__);
}
prog_char str_strat_conf2_arg0[] = "strat_conf";
if (!strcmp_P(res->arg1, PSTR("scan_opponent_min_time"))) {
if (res->arg2 > 90)
res->arg2 = 90;
- strat_infos.conf.scan_opp_min_time = res->arg2;
+ strat_conf.scan_opp_min_time = res->arg2;
}
else if (!strcmp_P(res->arg1, PSTR("delay_between_opponent_scan"))) {
if (res->arg2 > 90)
res->arg2 = 90;
- strat_infos.conf.delay_between_opp_scan = res->arg2;
+ strat_conf.delay_between_opp_scan = res->arg2;
}
else if (!strcmp_P(res->arg1, PSTR("scan_our_min_time"))) {
if (res->arg2 > 90)
res->arg2 = 90;
- strat_infos.conf.scan_our_min_time = res->arg2;
+ strat_conf.scan_our_min_time = res->arg2;
}
else if (!strcmp_P(res->arg1, PSTR("delay_between_our_scan"))) {
if (res->arg2 > 90)
res->arg2 = 90;
- strat_infos.conf.delay_between_our_scan = res->arg2;
+ strat_conf.delay_between_our_scan = res->arg2;
}
else if (!strcmp_P(res->arg1, PSTR("wait_opponent"))) {
- strat_infos.conf.wait_opponent = res->arg2;
+ strat_conf.wait_opponent = res->arg2;
}
else if (!strcmp_P(res->arg1, PSTR("lintel_min_time"))) {
- strat_infos.conf.lintel_min_time = res->arg2;
+ strat_conf.lintel_min_time = res->arg2;
}
#endif
- strat_infos.dump_enabled = 1;
- strat_dump_conf();
+ strat_conf.dump_enabled = 1;
+ strat_conf_dump(__FUNCTION__);
}
prog_char str_strat_conf3_arg0[] = "strat_conf";
#include "actuator.h"
#include "cs.h"
#include "strat_base.h"
+#include "strat_db.h"
#include "i2c_protocol.h"
/* 0 means "programmed"
/* strat */
gen.logs[0] = E_USER_STRAT;
gen.log_level = 5;
- strat_reset_infos();
+ strat_db_init();
/* strat-related event */
scheduler_add_periodical_event_priority(strat_event, NULL,
*
*/
+/* was mechboard in 2009 */
+
#define LED_TOGGLE(port, bit) do { \
if (port & _BV(bit)) \
port &= ~_BV(bit); \
#define I2C_POLL_PRIO 20
#define EEPROM_TIME_PRIO 10
-#define CS_PERIOD 5000L /* in microsecond */
+#define CS_PERIOD 5000L /* in microsecond */
#define CS_HZ (1000000. / CS_PERIOD)
#define NB_LOGS 4
static struct sensor_filter sensor_filter[SENSOR_MAX] = {
[S_CAP1] = { 1, 0, 0, 1, 0, 0 }, /* 0 */
[S_CAP2] = { 1, 0, 0, 1, 0, 0 }, /* 1 */
- [S_COLUMN_LEFT] = { 1, 0, 0, 1, 0, 1 }, /* 2 */
- [S_COLUMN_RIGHT] = { 1, 0, 0, 1, 0, 1 }, /* 3 */
+ [S_LFRONT] = { 1, 0, 0, 1, 0, 1 }, /* 2 */
+ [S_RFRONT] = { 1, 0, 0, 1, 0, 1 }, /* 3 */
[S_START_SWITCH] = { 10, 0, 3, 7, 0, 0 }, /* 4 */
[S_DISP_LEFT] = { 1, 0, 0, 1, 0, 1 }, /* 5 */
[S_RCOB_WHITE] = { 1, 0, 0, 1, 0, 0 }, /* 6 */
/* synchronize with sensor.c */
#define S_CAP1 0
#define S_CAP2 1
-#define S_COLUMN_RIGHT 2
-#define S_COLUMN_LEFT 3
+#define S_LFRONT 2
+#define S_RFRONT 3
#define S_START_SWITCH 4
#define S_DISP_LEFT 5
#define S_RCOB_WHITE 6
#include "i2c_protocol.h"
#include "main.h"
#include "strat.h"
+#include "strat_db.h"
#include "strat_base.h"
#include "strat_corn.h"
#include "strat_utils.h"
#define COL_DISP_MARGIN 400 /* stop 40 cm in front of dispenser */
#define COL_SCAN_PRE_MARGIN 250
-struct strat_infos strat_infos = {
- /* conf */
- .conf = {
- .flags = 0,
- },
- /* status */
- .status = {
- .flags = 0,
- },
-};
+struct strat_conf strat_conf;
/*************************************************************/
/*************************************************************/
-void strat_set_bounding_box(void)
-{
- if (get_color() == I2C_COLOR_YELLOW) {
- strat_infos.area_bbox.x1 = 300;
- strat_infos.area_bbox.y1 = 200;
- strat_infos.area_bbox.x2 = 2720; /* needed for c1 */
- strat_infos.area_bbox.y2 = 1800;
- }
- else {
- strat_infos.area_bbox.x1 = 200;
- strat_infos.area_bbox.y1 = 300;
- strat_infos.area_bbox.x2 = 2720; /* needed for c1 */
- strat_infos.area_bbox.y2 = 1900;
- }
-
- polygon_set_boundingbox(strat_infos.area_bbox.x1,
- strat_infos.area_bbox.y1,
- strat_infos.area_bbox.x2,
- strat_infos.area_bbox.y2);
-}
-
/* called before each strat, and before the start switch */
void strat_preinit(void)
{
DO_POS | DO_BD | DO_POWER;
//i2c_cobboard_mode_init();
- strat_dump_conf();
- strat_dump_infos(__FUNCTION__);
+ strat_conf_dump(__FUNCTION__);
+ strat_db_dump(__FUNCTION__);
}
-void strat_dump_conf(void)
+void strat_conf_dump(const char *caller)
{
- if (!strat_infos.dump_enabled)
+ if (!strat_conf.dump_enabled)
return;
printf_P(PSTR("-- conf --\r\n"));
}
-/* display current information about the state of the game */
-void strat_dump_infos(const char *caller)
-{
- if (!strat_infos.dump_enabled)
- return;
-
- printf_P(PSTR("%s() dump strat infos:\r\n"), caller);
-}
-
-/* init current area state before a match. Dump update user conf
- * here */
-void strat_reset_infos(void)
-{
- init_corn_table(-1, -1);
-}
-
/* call it just before launching the strat */
void strat_init(void)
{
/* XXX init rollers, .. */
- strat_reset_infos();
+ strat_db_init();
/* we consider that the color is correctly set */
/* call it after each strat */
void strat_exit(void)
{
+#ifndef HOST_VERSION
uint8_t flags;
+#endif
mainboard.flags &= ~(DO_TIMER);
strat_hardstop();
time_reset();
- wait_ms(1000);
+ wait_ms(100);
+#ifndef HOST_VERSION
IRQ_LOCK(flags);
mainboard.flags &= ~(DO_CS);
+ IRQ_UNLOCK(flags);
pwm_ng_set(LEFT_PWM, 0);
pwm_ng_set(RIGHT_PWM, 0);
- IRQ_UNLOCK(flags);
+#endif
}
/* called periodically */
uint8_t err;
strat_set_speed(250, SPEED_ANGLE_FAST);
- //init_corn_table(0, 0);
+ l1:
err = line2line(LINE_UP, 0, LINE_R_DOWN, 2);
+ if (!TRAJ_SUCCESS(err)) {
+ trajectory_hardstop(&mainboard.traj);
+ time_wait_ms(2000);
+ goto l1;
+ }
+
+ l2:
err = line2line(LINE_R_DOWN, 2, LINE_R_UP, 2);
+ if (!TRAJ_SUCCESS(err)) {
+ trajectory_hardstop(&mainboard.traj);
+ time_wait_ms(2000);
+ goto l2;
+ }
+
+ l3:
err = line2line(LINE_R_UP, 2, LINE_UP, 5);
+ if (!TRAJ_SUCCESS(err)) {
+ trajectory_hardstop(&mainboard.traj);
+ time_wait_ms(2000);
+ goto l3;
+ }
+
trajectory_hardstop(&mainboard.traj);
/* ball ejection */
i2c_cobboard_mode_eject();
time_wait_ms(2000);
+ trajectory_hardstop(&mainboard.traj);
return END_TRAJ;
}
#define CORNER_X 3000
#define CORNER_Y COLOR_Y(2100)
+/* XXX these offset are not related to corn, but to waypoints */
+#define OFFSET_CORN_X 150
+#define OFFSET_CORN_Y 222
+#define STEP_CORN_X 225
+#define STEP_CORN_Y 250
+
+#define CORN_NB 18
+#define TOMATO_NB 14
+
+#define WAYPOINTS_NBX 13
+#define WAYPOINTS_NBY 8
+
/*
- *
+ * Corn position and lines
*
* vertical lines
* O 1 2 3 4 5
* 2100 +-----|-----|-----|-----|-----|-----|-----+
- * | o o o |
- * | o o o o | diag
- * | o o o | lines
- * 0/ o o o o \0
- * y | o o |
- * 1/ o o \1
- * | |
- * 2/------ ------\2
+ * | c5 c9 c14 |
+ * | c2 c7 c11 c17 | diag
+ * | c4 c8 c13 | lines
+ * 0/ c1 c6 c10 c16 \0
+ * y | c3 c12 |
+ * 1/ c0 c15 \1
+ * |-----+ +-----|
+ * 2/ | | \2
* | | | |
+ * 0 +-----+-----------------------------+-----+
+ * 0 x 3000
+ *
+ * Ball (tomato) and i,j coords (for waypoints)
+ *
+ * 2100 +--0--1--2--3--4--5--6--7--8--9-10-11-12--+
+ * 7 t5 t9 |
+ * 6 t3 t7 t11 |
+ * 5 t1 t4 t8 t13 |
+ * 4 t2 t6 t10 |
+ * y 3 t0 t12 |
+ * 2 |
+ * 1-----+ +-----|
+ * 0 | | |
* | | | |
* 0 +-----+-----------------------------+-----+
* 0 x 3000
+ *
*/
/* useful traj flags */
#define SPEED_ANGLE_VERY_SLOW 400.
/* strat infos structures */
-
-struct strat_bbox {
- int32_t x1;
- int32_t y1;
- int32_t x2;
- int32_t y2;
-};
-
struct strat_conf {
-#define STRAT_CONF_XXX 0x01
- uint8_t flags;
-};
+ uint8_t dump_enabled;
-struct strat_status {
-#define STRAT_STATUS_LHARVEST 0x01
-#define STRAT_STATUS_RHARVEST 0x02
+#define STRAT_CONF_XXX 0x01
uint8_t flags;
};
-/* all infos related to strat */
-struct strat_infos {
- uint8_t dump_enabled;
- struct strat_conf conf;
- struct strat_bbox area_bbox;
- struct strat_status status;
-};
-extern struct strat_infos strat_infos;
+extern struct strat_conf strat_conf;
/* in strat.c */
-void strat_dump_infos(const char *caller); /* show current known state
- of area */
-void strat_dump_conf(void);
+void strat_conf_dump(const char *caller);
void strat_reset_infos(void); /* reset current known state */
void strat_preinit(void);
void strat_init(void);
/*
- * Copyright Droids Corporation, Microb Technology (2009)
+ * Copyright Droids, Microb Technology (2010)
*
* 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
* 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.8 2009-11-08 17:24:33 zer0 Exp $
+ * Revision : $Id: strat.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $
*
+ * Olivier MATZ <zer0@droids-corp.org>
*/
+
#include <stdio.h>
#include <stdlib.h>
-#include <string.h>
+#include <stdint.h>
#include <math.h>
+#include <aversive.h>
#include <aversive/pgmspace.h>
-#include <aversive/wait.h>
-#include <aversive/error.h>
#include <ax12.h>
#include <uart.h>
#include <quadramp.h>
#include <control_system_manager.h>
#include <trajectory_manager.h>
+#include <trajectory_manager_utils.h>
+#include <trajectory_manager_core.h>
#include <vect_base.h>
#include <lines.h>
#include <polygon.h>
#include <robot_system.h>
#include <position_manager.h>
+#include <diagnostic.h>
+
#include <rdline.h>
#include <parse.h>
#include "../common/i2c_commands.h"
-
+#include "i2c_protocol.h"
#include "main.h"
+#include "strat.h"
+#include "strat_base.h"
+#include "strat_corn.h"
+#include "strat_utils.h"
+#include "sensor.h"
+#include "actuator.h"
+
+/* XXX TODO
+static
+const
+change x,y -> i,j to avoid confusion with coords
+could be optimized in mem space: it is not needed to store the x,y coord,
+ we can process it from idx. however it will be less optimized for speed
+
+*/
+
+#define OFFSET_CORN_X 150
+#define OFFSET_CORN_Y 222
+#define STEP_CORN_X 225
+#define STEP_CORN_Y 250
+
+#define CORN_NB 18
+
+#define WAYPOINTS_NBX 13
+#define WAYPOINTS_NBY 8
+
+/* enum is better */
+#define TYPE_WAYPOINT 0
+#define TYPE_DANGEROUS 1
+#define TYPE_WHITE_CORN 2
+#define TYPE_BLACK_CORN 3
+#define TYPE_OBSTACLE 4
+#define TYPE_UNKNOWN 5
+
+/* XXX enum possible ? else just rename */
+#define START 0
+#define UP 1
+#define UP_RIGHT 2
+#define DOWN_RIGHT 3
+#define DOWN 4
+#define DOWN_LEFT 5
+#define UP_LEFT 6
+#define END 7
+
+struct djpoint {
+ uint16_t weight;
+ struct djpoint *parent;
+
+ uint8_t type:3;
+ uint8_t parent_pos:3;
+ uint8_t updated:1;
+ uint8_t todo:1;
+};
+
+uint8_t corn_table[CORN_NB];
+
+static struct djpoint djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY];
+
+/* return index from neigh pointer */
+#define PT2IDX(neigh) ( ((void *)(neigh)-(void *)(&djpoints)) / sizeof(*neigh) )
+
+void dump(void)
+{
+ int8_t i, j;
+ struct djpoint *pt;
+
+ printf_P(PSTR(" "));
+ for (i=0; i<WAYPOINTS_NBX; i++) {
+ printf_P(PSTR(" %2d "), i);
+ }
+ printf_P(PSTR("\r\n"));
+
+ for (j=WAYPOINTS_NBY*2-1; j>=0; j--) {
+ printf_P(PSTR("%3d "), j/2);
+
+ if ((j&1) == 0)
+ printf_P(PSTR(" "));
+
+ for (i=0; i<WAYPOINTS_NBX; i++) {
+ pt = &djpoints[i][j/2];
+
+ if (((i+j) & 1) == 0)
+ continue;
+
+ if (pt->type == TYPE_OBSTACLE)
+ printf_P(PSTR(" X "));
+ else if (pt->type == TYPE_DANGEROUS)
+ printf_P(PSTR(" D "));
+ else if (pt->type == TYPE_WHITE_CORN)
+ printf_P(PSTR(" W "));
+ else if (pt->type == TYPE_BLACK_CORN)
+ printf_P(PSTR(" B "));
+ else if (pt->type == TYPE_WAYPOINT)
+ printf_P(PSTR(" %5d "), pt->weight);
+ else
+ printf_P(PSTR(" ? "));
+ }
+ printf_P(PSTR("\r\n"));
+ }
+}
+
+static inline uint8_t opposite_position(uint8_t pos)
+{
+ pos += 3;
+ if (pos > UP_LEFT)
+ pos -= 6;
+ return pos;
+}
+
+/* return coord of the entry in the table from the pointer */
+static void djpoint2ij(struct djpoint *pt, int8_t *x, int8_t *y)
+{
+ int8_t idx = PT2IDX(pt);
+ *x = idx / WAYPOINTS_NBY;
+ *y = idx % WAYPOINTS_NBY;
+}
+
+/* get the neighbour of the point at specified position */
+static struct djpoint *get_neigh(struct djpoint *pt,
+ uint8_t position)
+{
+ int8_t i,j;
+ struct djpoint *neigh;
+
+ djpoint2ij(pt, &i, &j);
+
+ switch (position) {
+ case UP:
+ j++;
+ break;
+ case UP_RIGHT:
+ if (!(i & 1)) j++;
+ i++;
+ break;
+ case DOWN_RIGHT:
+ if (i & 1) j--;
+ i++;
+ break;
+ case DOWN:
+ j--;
+ break;
+ case DOWN_LEFT:
+ if (i & 1) j--;
+ i--;
+ break;
+ case UP_LEFT:
+ if (!(i & 1)) j++;
+ i--;
+ break;
+ default:
+ return NULL;
+ }
+ if (i < 0 || j < 0 || i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY)
+ return NULL;
+
+ neigh = &djpoints[i][j];
+
+ if (neigh->type != TYPE_WAYPOINT)
+ return NULL;
+
+ return neigh;
+}
+
+static struct djpoint *get_next_neigh(struct djpoint *pt, uint8_t *position)
+{
+ struct djpoint *neigh = NULL;
+ uint8_t pos = *position + 1;
+
+ for (pos = *position + 1; pos < END; pos++) {
+ neigh = get_neigh(pt, pos);
+ if (neigh != NULL)
+ break;
+ }
+
+ *position = pos;
+ return neigh;
+}
+
+/* browse all points */
+#define POINT_FOREACH(cur) \
+ for (cur = &djpoints[0][0]; \
+ cur < &djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY]; \
+ cur ++)
+
+/* XXX comment */
+#define NEIGH_FOREACH(neigh, pos, point) \
+ for (pos = START, neigh = get_next_neigh(point, &pos); \
+ neigh; \
+ neigh = get_next_neigh(point, &pos))
+
+int dijkstra_init(void)
+{
+ return 0;
+}
+
+static uint16_t dist(struct djpoint *p1, struct djpoint *p2)
+{
+ double vx, vy;
+ vx = p2->pos.x - p1->pos.x;
+ vy = p2->pos.y - p1->pos.y;
+ return sqrt(vx * vx + vy * vy);
+}
+
+void dijkstra_process_neighs(struct djpoint *pt)
+{
+ struct djpoint *neigh;
+ uint8_t pos, parent_pos;
+ uint16_t weight;
+ int8_t i,j;
+
+ djpoint2ij(pt, &i, &j);
+ printf_P(PSTR("at %d %d:\r\n"), i, j);
+
+ NEIGH_FOREACH(neigh, pos, pt) {
+ weight = pt->weight + dist(pt, neigh);
+ parent_pos = opposite_position(pos);
+
+ /* bonus if we keep the same direction */
+ if (parent_pos == pt->parent_pos ||
+ pt->parent_pos == END)
+ weight = weight - 1;
+
+ printf_P(PSTR(" pos=%d: ppos=%d opos=%d nw=%d w=%d\r\n"), pos,
+ pt->parent_pos, parent_pos,
+ neigh->weight, weight);
+ if (neigh->weight == 0 || weight < neigh->weight) {
+ djpoint2ij(neigh, &i, &j);
+ //printf_P(PSTR(" %d,%d updated\r\n"), i, j);
+ neigh->weight = weight;
+ neigh->parent_pos = parent_pos;
+ neigh->updated = 1;
+ }
+ }
+}
+
+int dijkstra(struct djpoint *start)
+{
+ struct djpoint *cur;
+ uint8_t todolist = 1;
+
+ start->todo = 1;
+
+ while (todolist) {
+ printf_P(PSTR("\r\n"));
+ dump();
+ /* process all neighbours of todo list */
+ POINT_FOREACH(cur) {
+ if (!cur->todo)
+ continue;
+ dijkstra_process_neighs(cur);
+ cur->todo = 0;
+ }
+
+ /* convert updated list in todo list */
+ todolist = 0;
+ POINT_FOREACH(cur) {
+ if (!cur->updated)
+ continue;
+ todolist = 1;
+ cur->todo = 1;
+ cur->updated = 0;
+ }
+ }
+ return 0; /* XXX */
+}
+
+/* init waypoints position */
+void init_djpoints(void)
+{
+ int8_t i, j;
+ struct djpoint *pt;
+
+ for (i=0; i<WAYPOINTS_NBX; i++) {
+
+ for (j=0; j<WAYPOINTS_NBY; j++) {
+ pt = &djpoints[i][j];
+
+ pt->type = TYPE_WAYPOINT;
+ pt->parent_pos = END;
+ pt->updated = 0;
+ pt->todo = 0;
+ }
+ }
+}
+
+/* update the type and weight of waypoints, before starting
+ * dijkstra */
+void update_djpoints(void)
+{
+ int8_t i, j, c;
+ struct djpoint *pt;
+
+ for (i=0; i<WAYPOINTS_NBX; i++) {
+
+ for (j=0; j<WAYPOINTS_NBY; j++) {
+ pt = &djpoints[i][j];
+
+ pt->weight = 0;
+
+ /* corn */
+ c = ijcoord_to_corn_idx(i, j);
+ if (c >= 0) {
+ pt->type = corn_table[c];
+ continue;
+ }
+ /* too close of border */
+ if ((i & 1) == 1 && j == (WAYPOINTS_NBY-1)) {
+ pt->type = TYPE_OBSTACLE;
+ continue;
+ }
+ /* hill */
+ if (i >= 2 && i < (WAYPOINTS_NBX-2) && j < 2) {
+ pt->type = TYPE_OBSTACLE;
+ continue;
+ }
+ /* dangerous points */
+ if (i == 0 || i == (WAYPOINTS_NBX-1)) {
+ pt->type = TYPE_DANGEROUS;
+ continue;
+ }
+ if ( (i&1) == 0 && j == (WAYPOINTS_NBY-1)) {
+ pt->type = TYPE_DANGEROUS;
+ continue;
+ }
+ pt->type = TYPE_WAYPOINT;
+ }
+ }
+}
+
+int get_path(struct djpoint *start, struct djpoint *end)
+{
+ struct djpoint *cur;
+ uint8_t prev_direction = 0;
+ int8_t idx;
+ int16_t x, y;
+
+ for (cur = start;
+ cur != NULL && cur->parent_pos != END && cur != end;
+ cur = get_neigh(cur, cur->parent_pos)) {
+ if (prev_direction != cur->parent_pos) {
+ idx = PT2IDX(cur);
+ corn_idx_to_coordxy(idx, &x, &y);
+ printf_P(PSTR("%d %d (%d)\r\n"),
+ x, y, cur->parent_pos);
+ }
+ prev_direction = cur->parent_pos;
+ }
+ idx = PT2IDX(end);
+ corn_idx_to_coordxy(idx, &x, &y);
+ printf_P(PSTR("%d %d\r\n"), x, y);
+ return 0; /* XXX */
+}
/* start the strat */
void strat_start(void)
{
- int8_t i;
uint8_t err;
strat_preinit();
+#ifndef HOST_VERSION
/* if start sw not plugged */
if (sensor_get(S_START_SWITCH)) {
+ int8_t i;
+
printf_P(PSTR("No start switch, press a key or plug it\r\n"));
/* while start sw not plugged */
/* while start sw plugged */
while (!sensor_get(S_START_SWITCH));
}
+#endif
strat_init();
err = strat_main();
+ printf("coucou\n");
NOTICE(E_USER_STRAT, "Finished !! returned %s", get_err(err));
strat_exit();
}
/* return true if we have to brake due to an obstacle */
uint8_t strat_obstacle(void)
{
+#if 0
int16_t x_rel, y_rel;
int16_t opp_x, opp_y, opp_d, opp_a;
opponent_obstacle.y = opp_y;
opponent_obstacle.d = opp_d;
opponent_obstacle.a = opp_a;
+#else /* belgium cup only */
+ int16_t x_rel, y_rel;
+ int16_t opp_d, opp_a;
+ double opp_x, opp_y;
+
+#ifdef HOST_VERSION
+ if (time_get_s() >= 12 && time_get_s() <= 30)
+ return 1;
+#endif
+ if (!sensor_get(S_RCOB_WHITE))
+ return 0;
+
+ opp_a = 0;
+ opp_d = 300;
+
+ rel_da_to_abs_xy(opp_d, RAD(opp_a), &opp_x, &opp_y);
+ if (!is_in_area(opp_x, opp_y, 250))
+ return 0;
+#endif
/* sensor are temporarily disabled */
if (sensor_obstacle_is_disabled())
#include "i2c_protocol.h"
#include "main.h"
#include "strat.h"
+#include "strat_db.h"
#include "strat_base.h"
#include "strat_corn.h"
#include "strat_utils.h"
#include "sensor.h"
#include "actuator.h"
-/* XXX TODO
-static
-const
-change x,y -> i,j to avoid confusion with coords
-could be optimized in mem space: it is not needed to store the x,y coord,
- we can process it from idx. however it will be less optimized for speed
-
-*/
-
-#define OFFSET_CORN_X 150
-#define OFFSET_CORN_Y 222
-#define STEP_CORN_X 225
-#define STEP_CORN_Y 250
-
-#define CORN_NB 18
-
-#define WAYPOINTS_NBX 13
-#define WAYPOINTS_NBY 8
-
-/* enum is better */
-#define TYPE_WAYPOINT 0
-#define TYPE_DANGEROUS 1
-#define TYPE_WHITE_CORN 2
-#define TYPE_BLACK_CORN 3
-#define TYPE_OBSTACLE 4
-#define TYPE_UNKNOWN 5
-
-/* XXX enum possible ? else just rename */
-#define START 0
-#define UP 1
-#define UP_RIGHT 2
-#define DOWN_RIGHT 3
-#define DOWN 4
-#define DOWN_LEFT 5
-#define UP_LEFT 6
-#define END 7
-
-struct point {
- int32_t x;
- int32_t y;
-};
-
-struct djpoint {
- struct point pos;
- uint16_t weight;
- struct djpoint *parent;
-
- uint8_t type:3;
- uint8_t parent_pos:3;
- uint8_t updated:1;
- uint8_t todo:1;
-};
-
-uint8_t corn_table[CORN_NB];
-
-const uint8_t corn_coord_i[CORN_NB] = {
- 0, 0, 0, 2, 2, 2, 4, 4, 6,
- 6, 8, 8, 10, 10, 10, 12, 12, 12,
-};
-
-const uint8_t corn_coord_j[CORN_NB] = {
- 2, 4, 6, 3, 5, 7, 4, 6, 5,
- 7, 4, 6, 3, 5, 7, 2, 4, 6,
-};
-
-static struct djpoint djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY];
-
-/* table to find the symetric idx */
-uint8_t corn_sym[] = {
- 15, 16, 17, 12, 13, 14, 10, 11, 8, 9, 6, 7, 3, 4, 5, 0, 1, 2
-};
-
-uint8_t corn_side_confs[9][2] = {
- { 1, 4 },
- { 0, 4 },
- { 2, 4 },
- { 2, 3 },
- { 0, 3 },
- { 1, 3 },
- { 1, 6 },
- { 0, 6 },
- { 2, 6 },
-};
-uint8_t corn_center_confs[4][2] = {
- { 5, 8 },
- { 7, 8 },
- { 5, 9 },
- { 7, 8 },
-};
-
-
-/* return index from neigh pointer */
-#define PT2IDX(neigh) ( ((void *)(neigh)-(void *)(&djpoints)) / sizeof(*neigh) )
-
-void dump(void)
-{
- int8_t i, j;
- struct djpoint *pt;
-
- printf_P(PSTR(" "));
- for (i=0; i<WAYPOINTS_NBX; i++) {
- printf_P(PSTR(" %2d "), i);
- }
- printf_P(PSTR("\r\n"));
-
- for (j=WAYPOINTS_NBY*2-1; j>=0; j--) {
- printf_P(PSTR("%3d "), j/2);
-
- if ((j&1) == 0)
- printf_P(PSTR(" "));
-
- for (i=0; i<WAYPOINTS_NBX; i++) {
- pt = &djpoints[i][j/2];
-
- if (((i+j) & 1) == 0)
- continue;
-
- if (pt->type == TYPE_OBSTACLE)
- printf_P(PSTR(" X "));
- else if (pt->type == TYPE_DANGEROUS)
- printf_P(PSTR(" D "));
- else if (pt->type == TYPE_WHITE_CORN)
- printf_P(PSTR(" W "));
- else if (pt->type == TYPE_BLACK_CORN)
- printf_P(PSTR(" B "));
- else if (pt->type == TYPE_WAYPOINT)
- printf_P(PSTR(" %5d "), pt->weight);
- else
- printf_P(PSTR(" ? "));
- }
- printf_P(PSTR("\r\n"));
- }
-}
-
-static inline uint8_t opposite_position(uint8_t pos)
-{
- pos += 3;
- if (pos > UP_LEFT)
- pos -= 6;
- return pos;
-}
-
-/* return coord of the entry in the table from the pointer */
-static void djpoint2ij(struct djpoint *pt, int8_t *x, int8_t *y)
-{
- int8_t idx = PT2IDX(pt);
- *x = idx / WAYPOINTS_NBY;
- *y = idx % WAYPOINTS_NBY;
-}
-
-/* get the neighbour of the point at specified position */
-static struct djpoint *get_neigh(struct djpoint *pt,
- uint8_t position)
-{
- int8_t i,j;
- struct djpoint *neigh;
-
- djpoint2ij(pt, &i, &j);
-
- switch (position) {
- case UP:
- j++;
- break;
- case UP_RIGHT:
- if (!(i & 1)) j++;
- i++;
- break;
- case DOWN_RIGHT:
- if (i & 1) j--;
- i++;
- break;
- case DOWN:
- j--;
- break;
- case DOWN_LEFT:
- if (i & 1) j--;
- i--;
- break;
- case UP_LEFT:
- if (!(i & 1)) j++;
- i--;
- break;
- default:
- return NULL;
- }
- if (i < 0 || j < 0 || i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY)
- return NULL;
-
- neigh = &djpoints[i][j];
-
- if (neigh->type != TYPE_WAYPOINT)
- return NULL;
-
- return neigh;
-}
-
-static struct djpoint *get_next_neigh(struct djpoint *pt, uint8_t *position)
-{
- struct djpoint *neigh = NULL;
- uint8_t pos = *position + 1;
-
- for (pos = *position + 1; pos < END; pos++) {
- neigh = get_neigh(pt, pos);
- if (neigh != NULL)
- break;
- }
-
- *position = pos;
- return neigh;
-}
-
-/* browse all points */
-#define POINT_FOREACH(cur) \
- for (cur = &djpoints[0][0]; \
- cur < &djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY]; \
- cur ++)
-
-/* XXX comment */
-#define NEIGH_FOREACH(neigh, pos, point) \
- for (pos = START, neigh = get_next_neigh(point, &pos); \
- neigh; \
- neigh = get_next_neigh(point, &pos))
-
-int dijkstra_init(void)
-{
- return 0;
-}
-
-static uint16_t dist(struct djpoint *p1, struct djpoint *p2)
-{
- double vx, vy;
- vx = p2->pos.x - p1->pos.x;
- vy = p2->pos.y - p1->pos.y;
- return sqrt(vx * vx + vy * vy);
-}
-
-void dijkstra_process_neighs(struct djpoint *pt)
-{
- struct djpoint *neigh;
- uint8_t pos, parent_pos;
- uint16_t weight;
- int8_t i,j;
-
- djpoint2ij(pt, &i, &j);
- printf_P(PSTR("at %d %d:\r\n"), i, j);
-
- NEIGH_FOREACH(neigh, pos, pt) {
- weight = pt->weight + dist(pt, neigh);
- parent_pos = opposite_position(pos);
-
- /* bonus if we keep the same direction */
- if (parent_pos == pt->parent_pos ||
- pt->parent_pos == END)
- weight = weight - 1;
-
- printf_P(PSTR(" pos=%d: ppos=%d opos=%d nw=%d w=%d\r\n"), pos,
- pt->parent_pos, parent_pos,
- neigh->weight, weight);
- if (neigh->weight == 0 || weight < neigh->weight) {
- djpoint2ij(neigh, &i, &j);
- //printf_P(PSTR(" %d,%d updated\r\n"), i, j);
- neigh->weight = weight;
- neigh->parent_pos = parent_pos;
- neigh->updated = 1;
- }
- }
-}
-
-int dijkstra(struct djpoint *start)
-{
- struct djpoint *cur;
- uint8_t todolist = 1;
-
- start->todo = 1;
-
- while (todolist) {
- printf_P(PSTR("\r\n"));
- dump();
- /* process all neighbours of todo list */
- POINT_FOREACH(cur) {
- if (!cur->todo)
- continue;
- dijkstra_process_neighs(cur);
- cur->todo = 0;
- }
-
- /* convert updated list in todo list */
- todolist = 0;
- POINT_FOREACH(cur) {
- if (!cur->updated)
- continue;
- todolist = 1;
- cur->todo = 1;
- cur->updated = 0;
- }
- }
- return 0; /* XXX */
-}
-
-int8_t ijcoord_to_corn_idx(uint8_t i, uint8_t j)
-{
- uint8_t n;
- for (n = 0; n < CORN_NB; n ++) {
- if (i == corn_coord_i[n] &&
- j == corn_coord_j[n])
- return n;
- }
- return -1;
-}
-
-int8_t corn_idx_to_coordij(uint8_t idx, uint8_t *i, uint8_t *j)
-{
- if (idx >= CORN_NB)
- return -1;
- *i = corn_coord_i[idx];
- *j = corn_coord_j[idx];
- return 0;
-}
-
-/* return the index of the closest corn at these coordinates. If the
- * corn is really too far (~20cm), return -1. The x and y pointer are
- * updated with the real position of the corn */
-int8_t xycoord_to_corn_idx(int16_t *x, int16_t *y)
-{
- uint8_t idx = -1, n, i, j;
- int16_t d, x_corn, y_corn;
- int16_t x_corn_min = 0, y_corn_min = 0;
- int16_t d_min = 0;
-
- for (n = 0; n < CORN_NB; n ++) {
- corn_idx_to_coordij(n, &i, &j);
- x_corn = (OFFSET_CORN_X + i*STEP_CORN_X);
- y_corn = (OFFSET_CORN_Y + j*STEP_CORN_Y);
- d = xy_norm(x_corn, y_corn, *x, *y);
- if (d < 200 && (d_min == 0 || d < d_min)) {
- d_min = d;
- idx = n;
- x_corn_min = x_corn;
- y_corn_min = y_corn;
- }
- }
- if (d_min == 0)
- return -1;
-
- *x = x_corn_min;
- *y = y_corn_min;
-
- return idx;
-}
-
-int8_t corn_get_sym(int8_t i)
-{
- if (i >= CORN_NB)
- return -1;
- return corn_sym[i];
-}
-
-void init_corn_table(int8_t conf_side, int8_t conf_center)
-{
- int8_t sym, i;
-
- /* before match */
- if (conf_side == -1) {
- for (i=0; i<CORN_NB; i++)
- corn_table[i] = TYPE_UNKNOWN;
- return;
- }
-
- /* for test */
- printf_P(PSTR("confs = %d, %d\r\n"), conf_side, conf_center);
- for (i=0; i<CORN_NB; i++) {
- if (i == corn_side_confs[conf_side][0] ||
- i == corn_side_confs[conf_side][1]) {
- corn_table[i] = TYPE_BLACK_CORN;
- continue;
- }
- sym = corn_get_sym(i);
- if (sym == corn_side_confs[conf_side][0] ||
- sym == corn_side_confs[conf_side][1]) {
- corn_table[i] = TYPE_BLACK_CORN;
- continue;
- }
- if (i == corn_center_confs[conf_center][0] ||
- i == corn_center_confs[conf_center][1]) {
- corn_table[i] = TYPE_BLACK_CORN;
- continue;
- }
- sym = corn_get_sym(i);
- if (sym == corn_center_confs[conf_center][0] ||
- sym == corn_center_confs[conf_center][1]) {
- corn_table[i] = TYPE_BLACK_CORN;
- continue;
- }
- corn_table[i] = TYPE_WHITE_CORN;
- }
-}
-
-/* init waypoints position */
-void init_waypoints(void)
-{
- int8_t i, j;
- int32_t x, y;
- struct djpoint *pt;
-
- x = OFFSET_CORN_X;
- for (i=0; i<WAYPOINTS_NBX; i++) {
-
- if (i & 1)
- y = OFFSET_CORN_Y - STEP_CORN_Y/2;
- else
- y = OFFSET_CORN_Y;
-
- for (j=0; j<WAYPOINTS_NBY; j++) {
- pt = &djpoints[i][j];
-
- pt->pos.x = x;
- pt->pos.y = y;
-
- pt->type = TYPE_WAYPOINT;
- pt->parent_pos = END;
- pt->updated = 0;
- pt->todo = 0;
-
- y += STEP_CORN_Y;
- }
-
- x += STEP_CORN_X;
- }
-}
-
-/* update the type and weight of waypoints, before starting
- * dijkstra */
-void update_waypoints(void)
-{
- int8_t i, j, c;
- struct djpoint *pt;
-
- for (i=0; i<WAYPOINTS_NBX; i++) {
-
- for (j=0; j<WAYPOINTS_NBY; j++) {
- pt = &djpoints[i][j];
-
- pt->weight = 0;
-
- /* corn */
- c = ijcoord_to_corn_idx(i, j);
- if (c >= 0) {
- pt->type = corn_table[c];
- continue;
- }
- /* too close of border */
- if ((i & 1) == 1 && j == (WAYPOINTS_NBY-1)) {
- pt->type = TYPE_OBSTACLE;
- continue;
- }
- /* hill */
- if (i >= 2 && i < (WAYPOINTS_NBX-2) && j < 2) {
- pt->type = TYPE_OBSTACLE;
- continue;
- }
- /* dangerous points */
- if (i == 0 || i == (WAYPOINTS_NBX-1)) {
- pt->type = TYPE_DANGEROUS;
- continue;
- }
- if ( (i&1) == 0 && j == (WAYPOINTS_NBY-1)) {
- pt->type = TYPE_DANGEROUS;
- continue;
- }
- pt->type = TYPE_WAYPOINT;
- }
- }
-}
-
-int get_path(struct djpoint *start, struct djpoint *end)
-{
- struct djpoint *cur;
- uint8_t prev_direction = 0;
-
- for (cur=start;
- cur != NULL && cur->parent_pos != END && cur != end;
- cur = get_neigh(cur, cur->parent_pos)) {
- if (prev_direction != cur->parent_pos) {
- printf_P(PSTR("%d %d (%d)\r\n"),
- cur->pos.x, cur->pos.y,
- cur->parent_pos);
- }
- prev_direction = cur->parent_pos;
- }
- printf_P(PSTR("%d %d\r\n"), end->pos.x, end->pos.y);
-
- return 0; /* XXX */
-}
-
+#if 0
/* return 1 if there is a corn near, and fill the index ptr */
-static uint8_t corn_is_near(int8_t *corn_idx, uint8_t side)
+uint8_t corn_is_near(int8_t *corn_idx, uint8_t side)
{
#define SENSOR_CORN_DIST 225
#define SENSOR_CORN_ANGLE 90
double a_rad = position_get_a_rad_double(&mainboard.pos);
double x_corn, y_corn;
int16_t x_corn_int, y_corn_int;
+ struct waypoint_db *wp;
if (side == I2C_LEFT_SIDE) {
x_corn = x + cos(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
x_corn_int = x_corn;
y_corn_int = y_corn;
- *corn_idx = xycoord_to_corn_idx(&x_corn_int, &y_corn_int);
- if (*corn_idx < 0)
+ wp = xycoord_to_corn_idx(&x_corn_int, &y_corn_int);
+ if (wp->corn.idx < 0)
return 0;
return 1;
}
+#endif
/*
* - send the correct commands to the spickles
*/
static uint8_t handle_spickles(void)
{
- int8_t corn_idx;
-
return 0;
+#if 0
+ int8_t corn_idx;
if (!corn_is_near(&corn_idx, I2C_LEFT_SIDE))
i2c_cobboard_mode_deploy(I2C_LEFT_SIDE);
}
return 0;
+#endif
}
uint8_t line2line(uint8_t dir1, uint8_t num1,
line1_a_deg, 150., diff_a_deg, beta_deg,
radius, xy_norm(l1.p1.x, l1.p1.y,
p.x, p.y));
- err = 0;
- while (err == 0) {
- err = WAIT_COND_OR_TRAJ_END(handle_spickles(), 0xFF);
- if (err == 0) {
- /* cobboard is stucked */
- trajectory_hardstop(&mainboard.traj);
- return err; /* XXX do something */
+ if (0) {
+ err = 0;
+ while (err == 0) {
+ err = WAIT_COND_OR_TRAJ_END(handle_spickles(), 0xFF);
+ if (err == 0) {
+ /* cobboard is stucked */
+ trajectory_hardstop(&mainboard.traj);
+ return err; /* XXX do something */
+ }
+ err = test_traj_end(0xFF);
}
- err = test_traj_end(0xFF);
}
+
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
return err;
}
break;
}
}
-
-
-#if 0
-int main(void)
-{
- struct djpoint *start;
- struct djpoint *end;
-
- start = &djpoints[1][1];
- end = &djpoints[12][1];
-
- init_corn_table(0, 0);
- init_waypoints();
- update_waypoints();
-
- dijkstra(end);
- dump();
-
- get_path(start, end);
-
- return 0;
-}
-#endif
* Olivier MATZ <zer0@droids-corp.org>
*/
-#define CORN_NB 18
-
-/* enum is better */
-#define TYPE_WAYPOINT 0
-#define TYPE_DANGEROUS 1
-#define TYPE_WHITE_CORN 2
-#define TYPE_BLACK_CORN 3
-#define TYPE_OBSTACLE 4
-
#define LINE_UP 0
#define LINE_DOWN 1
#define LINE_R_UP 2
point_t p2;
};
-extern uint8_t corn_table[CORN_NB];
-
-int8_t ijcoord_to_corn_idx(uint8_t i, uint8_t j);
-int8_t xycoord_to_corn_idx(int16_t *x, int16_t *y);
-void init_corn_table(int8_t conf_side, int8_t conf_center);
+void num2line(struct line_2pts *l, uint8_t dir, uint8_t num);
uint8_t line2line(uint8_t dir1, uint8_t num1,
uint8_t dir2, uint8_t num2);
-void num2line(struct line_2pts *l, uint8_t dir, uint8_t num);
git.droids-corp.org / aversive.git / commitdiff