#include "strat_base.h"
#include "strat_corn.h"
#include "strat_utils.h"
+#include "strat_avoid.h"
#include "sensor.h"
#include "actuator.h"
mainboard.flags = DO_ENCODERS | DO_CS | DO_RS |
DO_POS | DO_BD | DO_POWER;
- //i2c_cobboard_mode_init();
+ strat_db_init();
strat_conf_dump(__FUNCTION__);
strat_db_dump(__FUNCTION__);
}
/* we consider that the color is correctly set */
strat_running = 1;
- strat_db_init();
strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
time_reset();
interrupt_traj_reset();
if (strat_running == 0)
return;
+ /* read sensors from ballboard */
IRQ_LOCK(flags);
lcob = ballboard.lcob;
ballboard.lcob = I2C_COB_NONE;
ballboard.rcob = I2C_COB_NONE;
IRQ_UNLOCK(flags);
+/* if (lcob != I2C_COB_NONE) */
+/* DEBUG(E_USER_STRAT, "XXX lcob %s", */
+/* lcob == I2C_COB_WHITE ? "white" : "black"); */
+/* if (rcob != I2C_COB_NONE) */
+/* DEBUG(E_USER_STRAT, "XXX rcob %s", */
+/* rcob == I2C_COB_WHITE ? "white" : "black"); */
/* XXX take opponent position into account */
#ifdef HOST_VERSION
}
/* control the cobboard mode for left spickle */
- if (get_cob_count() >= 5 || want_pack) {
- if (strat_db.corn_table[lidx]->corn.color != I2C_COB_WHITE &&
- strat_db.corn_table[lidx]->corn.color != I2C_COB_BLACK)
- i2c_cobboard_pack(I2C_LEFT_SIDE);
+ if (lcob_near) {
+ if (get_cob_count() >= 5 || want_pack) {
+ if (strat_db.corn_table[lidx]->corn.color != I2C_COB_WHITE &&
+ strat_db.corn_table[lidx]->corn.color != I2C_COB_BLACK)
+ i2c_cobboard_pack(I2C_LEFT_SIDE);
+ }
+ else {
+ if (strat_db.corn_table[lidx]->corn.color == I2C_COB_WHITE)
+ i2c_cobboard_autoharvest(I2C_LEFT_SIDE);
+ else if (strat_db.corn_table[lidx]->corn.color == I2C_COB_BLACK)
+ i2c_cobboard_deploy_nomove(I2C_LEFT_SIDE);
+ else
+ i2c_cobboard_deploy(I2C_LEFT_SIDE);
+ }
}
else {
- if (strat_db.corn_table[lidx]->corn.color == I2C_COB_WHITE)
- i2c_cobboard_autoharvest(I2C_LEFT_SIDE);
- else if (strat_db.corn_table[lidx]->corn.color == I2C_COB_BLACK)
- i2c_cobboard_deploy_nomove(I2C_LEFT_SIDE);
+ if (get_cob_count() >= 5 || want_pack)
+ i2c_cobboard_pack(I2C_LEFT_SIDE);
else
i2c_cobboard_deploy(I2C_LEFT_SIDE);
}
/* control the cobboard mode for right spickle */
- if (get_cob_count() >= 5 || want_pack) {
- if (strat_db.corn_table[lidx]->corn.color != I2C_COB_WHITE &&
- strat_db.corn_table[lidx]->corn.color != I2C_COB_BLACK)
- i2c_cobboard_pack(I2C_RIGHT_SIDE);
+ if (rcob_near) {
+ if (get_cob_count() >= 5 || want_pack) {
+ if (strat_db.corn_table[ridx]->corn.color != I2C_COB_WHITE &&
+ strat_db.corn_table[ridx]->corn.color != I2C_COB_BLACK)
+ i2c_cobboard_pack(I2C_RIGHT_SIDE);
+ }
+ else {
+ if (strat_db.corn_table[ridx]->corn.color == I2C_COB_WHITE)
+ i2c_cobboard_autoharvest(I2C_RIGHT_SIDE);
+ else if (strat_db.corn_table[ridx]->corn.color == I2C_COB_BLACK)
+ i2c_cobboard_deploy_nomove(I2C_RIGHT_SIDE);
+ else
+ i2c_cobboard_deploy(I2C_RIGHT_SIDE);
+ }
}
else {
- if (strat_db.corn_table[lidx]->corn.color == I2C_COB_WHITE)
- i2c_cobboard_autoharvest(I2C_RIGHT_SIDE);
- else if (strat_db.corn_table[lidx]->corn.color == I2C_COB_BLACK)
- i2c_cobboard_deploy_nomove(I2C_RIGHT_SIDE);
+ if (get_cob_count() >= 5 || want_pack)
+ i2c_cobboard_pack(I2C_RIGHT_SIDE);
else
i2c_cobboard_deploy(I2C_RIGHT_SIDE);
}
//XXX return vals
strat_set_speed(600, SPEED_ANGLE_SLOW);
+ want_pack = 1;
trajectory_goto_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847));
err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+
+ /* ball ejection */
+ i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT);
trajectory_a_abs(&mainboard.traj, COLOR_A(70));
err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
strat_hardstop();
-
- /* ball ejection */
- trajectory_a_abs(&mainboard.traj, COLOR_A(90));
- i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT);
time_wait_ms(2000);
+
/* half turn */
+ strat_event_disable();
i2c_cobboard_pack(I2C_LEFT_SIDE);
i2c_cobboard_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);
+ trajectory_d_rel(&mainboard.traj, -70);
err = wait_traj_end(END_INTR|END_TRAJ);
- strat_running = ;
+
i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT);
+ strat_db_dump(__FUNCTION__);
time_wait_ms(2000);
+ strat_event_enable();
+ want_pack = 0;
return 0;
}
#if 1
l1:
- DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
+ DEBUG(E_USER_STRAT, "%s():%d count=%d", __FUNCTION__, __LINE__, get_cob_count());
if (get_cob_count() >= 5)
strat_set_speed(600, SPEED_ANGLE_FAST);
- err = line2line(LINE_UP, 0, LINE_R_DOWN, 2);
+ err = line2line(0, LINE_UP, 2, LINE_R_DOWN);
if (!TRAJ_SUCCESS(err)) {
strat_hardstop();
time_wait_ms(2000);
}
l2:
- DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
+ DEBUG(E_USER_STRAT, "%s():%d count=%d", __FUNCTION__, __LINE__, get_cob_count());
if (get_cob_count() >= 5)
strat_set_speed(600, SPEED_ANGLE_FAST);
- err = line2line(LINE_R_DOWN, 2, LINE_R_UP, 2);
+ err = line2line(2, LINE_R_DOWN, 2, LINE_R_UP);
if (!TRAJ_SUCCESS(err)) {
strat_hardstop();
time_wait_ms(2000);
}
#else
- strat_set_speed(600, SPEED_ANGLE_FAST);
- err = line2line(LINE_UP, 0, LINE_R_DOWN, 3);
- err = line2line(LINE_R_DOWN, 3, LINE_R_UP, 2);
- err = line2line(LINE_R_UP, 2, LINE_R_DOWN, 2);
- err = line2line(LINE_R_DOWN, 2, LINE_R_UP, 3);
- err = line2line(LINE_R_UP, 3, LINE_UP, 5);
- err = line2line(LINE_UP, 5, LINE_L_DOWN, 2);
- err = line2line(LINE_L_DOWN, 2, LINE_L_UP, 1);
- err = line2line(LINE_L_UP, 1, LINE_L_DOWN, 1);
- err = line2line(LINE_L_DOWN, 1, LINE_DOWN, 0);
+/* strat_set_speed(600, SPEED_ANGLE_FAST); */
+/* err = line2line(0, LINE_UP, 3, LINE_R_DOWN); */
+/* err = line2line(3, LINE_R_DOWN, 2, LINE_R_UP); */
+/* err = line2line(2, LINE_R_UP, 2, LINE_R_DOWN); */
+/* err = line2line(2, LINE_R_DOWN, 3, LINE_R_UP); */
+/* err = line2line(3, LINE_R_UP, 5, LINE_UP); */
+/* err = line2line(5, LINE_UP, 2, LINE_L_DOWN); */
+/* err = line2line(2, LINE_L_DOWN, 1, LINE_L_UP); */
+/* err = line2line(1, LINE_L_UP, 1, LINE_L_DOWN); */
+/* err = line2line(1, LINE_L_DOWN, 0, LINE_DOWN); */
+
+ strat_set_speed(300, SPEED_ANGLE_FAST);
+ err = line2line(0, LINE_UP, 0, LINE_R_UP);
+ err = line2line(0, LINE_R_UP, 1, LINE_DOWN);
+ err = line2line(1, LINE_DOWN, 1, LINE_L_DOWN);
+ err = line2line(1, LINE_L_DOWN, 0, LINE_DOWN);
wait_ms(500);
strat_hardstop();
return END_TRAJ;
strat_set_speed(250, SPEED_ANGLE_FAST);
+#if 0
l4:
- DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
+ DEBUG(E_USER_STRAT, "%s():%d count=%d", __FUNCTION__, __LINE__, get_cob_count());
if (get_cob_count() >= 5)
strat_set_speed(600, SPEED_ANGLE_FAST);
- err = line2line(LINE_DOWN, 5, LINE_L_UP, 2);
+ err = line2line(5, LINE_DOWN, 2, LINE_L_UP);
if (!TRAJ_SUCCESS(err)) {
strat_hardstop();
time_wait_ms(2000);
}
l5:
- DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
+ DEBUG(E_USER_STRAT, "%s():%d count=%d", __FUNCTION__, __LINE__, get_cob_count());
if (get_cob_count() >= 5)
strat_set_speed(600, SPEED_ANGLE_FAST);
- err = line2line(LINE_L_UP, 2, LINE_R_UP, 2);
+ err = line2line(2, LINE_L_UP, 2, LINE_R_UP);
if (!TRAJ_SUCCESS(err)) {
strat_hardstop();
time_wait_ms(2000);
goto l5;
}
- DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
+ DEBUG(E_USER_STRAT, "%s():%d count=%d", __FUNCTION__, __LINE__, get_cob_count());
if (get_cob_count() >= 5)
strat_set_speed(600, SPEED_ANGLE_FAST);
+#else
+ strat_harvest_circuit();
+#endif
WAIT_COND_OR_TRAJ_END(distance_from_robot(2625, COLOR_Y(1847)) < 100,
TRAJ_FLAGS_STD);
#include <aversive.h>
#include <aversive/pgmspace.h>
+#include <aversive/error.h>
#include <ax12.h>
#include <uart.h>
#include "strat_db.h"
#include "strat_base.h"
#include "strat_corn.h"
+#include "strat_avoid.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
-
-*/
-
-/* 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 parent_pos:3;
- uint8_t updated:1;
- uint8_t todo:1;
- uint8_t reserved:3;
-};
-
-/* database for dijkstra */
-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;
- struct waypoint_db *wp;
-
- 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];
- wp = &strat_db.wp_table[i][j/2];
+const struct wp_coord circuit1[] = {
+ { .i = 11, .j = 6, .end = 0, },
+ { .i = 10, .j = 6, .end = 0, },
+ { .i = 9, .j = 5, .end = 0, },
+ { .i = 8, .j = 5, .end = 0, },
+ { .i = 7, .j = 4, .end = 0, },
+ { .i = 6, .j = 4, .end = 0, },
+ { .i = 5, .j = 4, .end = 0, },
+ { .i = 4, .j = 5, .end = 0, },
+ { .i = 3, .j = 5, .end = 0, },
+ { .i = 2, .j = 6, .end = 0, },
+ { .i = 1, .j = 6, .end = 0, },
+ { .i = 1, .j = 5, .end = 0, },
+ { .i = 1, .j = 4, .end = 0, },
+ { .i = 1, .j = 3, .end = 0, },
+ { .i = 1, .j = 2, .end = 0, },
+ { .i = 1, .j = 1, .end = 0, },
+ { .i = 2, .j = 2, .end = 0, },
+ { .i = 3, .j = 2, .end = 0, },
+ { .i = 4, .j = 3, .end = 0, },
+ { .i = 5, .j = 3, .end = 0, },
+ { .i = 6, .j = 4, .end = 0, },
+ { .i = 7, .j = 3, .end = 0, },
+ { .i = 8, .j = 3, .end = 0, },
+ { .i = 9, .j = 2, .end = 0, },
+ { .i = 10, .j = 2, .end = 0, },
+ { .i = 11, .j = 1, .end = 0, },
+ { .i = 11, .j = 2, .end = 0, },
+ { .i = 11, .j = 3, .end = 0, },
+ { .i = 11, .j = 4, .end = 0, },
+ { .i = 11, .j = 5, .end = 0, },
+ { .i = 11, .j = 6, .end = 1, },
+};
- if (((i+j) & 1) == 0)
- continue;
+const struct wp_coord circuit2[] = {
+ { .i = 11, .j = 6, .end = 0, },
+ { .i = 10, .j = 6, .end = 0, },
+ { .i = 9, .j = 5, .end = 0, },
+ { .i = 9, .j = 4, .end = 0, },
+ { .i = 9, .j = 3, .end = 0, },
+ { .i = 10, .j = 4, .end = 0, },
+ { .i = 11, .j = 4, .end = 0, },
+ { .i = 11, .j = 5, .end = 0, },
+ { .i = 11, .j = 6, .end = 1, },
+};
- if (wp->type == WP_TYPE_OBSTACLE)
- printf_P(PSTR(" X "));
- else if (wp->dangerous)
- printf_P(PSTR(" D "));
- else if (wp->type == WP_TYPE_CORN &&
- wp->corn.color == I2C_COB_WHITE)
- printf_P(PSTR(" W "));
- else if (wp->type == WP_TYPE_CORN &&
- wp->corn.color == I2C_COB_BLACK)
- printf_P(PSTR(" B "));
- else if (wp->type == WP_TYPE_CORN &&
- wp->corn.color == I2C_COB_UNKNOWN)
- printf_P(PSTR(" U "));
- else if (wp->type == WP_TYPE_WAYPOINT ||
- wp->type == WP_TYPE_TOMATO)
- printf_P(PSTR(" %5d "), pt->weight);
- else
- printf_P(PSTR(" ? "));
- }
- printf_P(PSTR("\r\n"));
- }
-}
+/* list of all possible circuits */
+const struct wp_coord *circuits[] = {
+ circuit1,
+ circuit2,
+ NULL,
+};
+/* symetric neighbor position */
static inline uint8_t opposite_position(uint8_t pos)
{
pos += 3;
- if (pos > UP_LEFT)
+ if (pos > LINE_L_UP)
pos -= 6;
return pos;
}
-/* is point reachable by the robot ? */
-static uint8_t is_reachable(uint8_t i, uint8_t j)
+static uint8_t cc;
+uint8_t xget_cob_count(void)
{
- struct waypoint_db *wp;
-
- wp = &strat_db.wp_table[i][j];
- if (wp->type == WP_TYPE_WAYPOINT)
- return 1;
- if (wp->type == WP_TYPE_TOMATO)
- return 1;
- if (wp->type == WP_TYPE_CORN &&
- wp->present == 0)
- return 1;
- return 0;
+ return cc;
}
-/* return coord of the entry in the table from the pointer */
-static void djpoint2ij(struct djpoint *pt, uint8_t *i, uint8_t *j)
+static uint8_t bc;
+uint8_t xget_ball_count(void)
{
- int8_t idx = PT2IDX(pt);
- *i = idx / WAYPOINTS_NBY;
- *j = idx % WAYPOINTS_NBY;
+ return bc;
}
-/* get the neighbour of the point at specified position */
-static struct djpoint *get_neigh(struct djpoint *pt,
- uint8_t position)
+static uint32_t ts;
+uint8_t xtime_get_s(void)
{
- uint8_t i,j;
- struct djpoint *neigh;
+ return ts;
+}
- djpoint2ij(pt, &i, &j);
+/* get the neighbour of the point at specified position, return -1 if
+ * there is no neighbor */
+static int8_t get_neigh(uint8_t i, uint8_t j,
+ uint8_t *ni, uint8_t *nj,
+ uint8_t position)
+{
switch (position) {
- case UP:
+ case LINE_UP:
j++;
break;
- case UP_RIGHT:
+ case LINE_R_UP:
if (!(i & 1)) j++;
i++;
break;
- case DOWN_RIGHT:
+ case LINE_R_DOWN:
if (i & 1) j--;
i++;
break;
- case DOWN:
+ case LINE_DOWN:
j--;
break;
- case DOWN_LEFT:
+ case LINE_L_DOWN:
if (i & 1) j--;
i--;
break;
- case UP_LEFT:
+ case LINE_L_UP:
if (!(i & 1)) j++;
i--;
break;
default:
- return NULL;
+ return -1;
}
if (i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY)
- return NULL;
+ return -1;
- if (is_reachable(i, j) == 0)
- return NULL;
+ *ni = i;
+ *nj = j;
+ return 0;
+}
- neigh = &djpoints[i][j];
- return neigh;
+static uint8_t get_line_num(int8_t i, int8_t j, uint8_t dir)
+{
+ switch (dir) {
+ case LINE_UP:
+ case LINE_DOWN:
+ return i/2;
+ case LINE_R_UP:
+ case LINE_L_DOWN:
+ i &= 0xfe;
+ j -= i/2;
+ return (5-j)/2;
+ case LINE_R_DOWN:
+ case LINE_L_UP:
+ i &= 0xfe;
+ j += i/2;
+ return (11-j)/2;
+ default:
+ return -1;
+ }
}
-static struct djpoint *get_next_neigh(struct djpoint *pt, uint8_t *position)
+static uint8_t get_dir(uint8_t prev_i, uint8_t prev_j,
+ uint8_t i, uint8_t j)
{
- struct djpoint *neigh = NULL;
- uint8_t pos = *position + 1;
+ int8_t diff_i, diff_j;
+
+ diff_i = i - prev_i;
+ diff_j = j - prev_j;
+
+ if (diff_i == 0 && diff_j == 1)
+ return LINE_UP;
+ if (diff_i == 0 && diff_j == -1)
+ return LINE_DOWN;
+
+ if ((prev_i & 1) == 0) {
+ if (diff_i == 1 && diff_j == 0)
+ return LINE_R_UP;
+ if (diff_i == 1 && diff_j == -1)
+ return LINE_R_DOWN;
+ if (diff_i == -1 && diff_j == 0)
+ return LINE_L_UP;
+ if (diff_i == -1 && diff_j == -1)
+ return LINE_L_DOWN;
+ }
+ else {
+ if (diff_i == 1 && diff_j == 1)
+ return LINE_R_UP;
+ if (diff_i == 1 && diff_j == 0)
+ return LINE_R_DOWN;
+ if (diff_i == -1 && diff_j == 1)
+ return LINE_L_UP;
+ if (diff_i == -1 && diff_j == 0)
+ return LINE_L_DOWN;
+ }
- for (pos = *position + 1; pos < END; pos++) {
- neigh = get_neigh(pt, pos);
- if (neigh != NULL)
+ /* invalid value */
+ return 0xFF;
+}
+
+
+int8_t get_path(const struct wp_coord *circuit,
+ uint8_t starti, uint8_t startj, uint8_t faceA,
+ struct wp_line *circuit_wpline)
+{
+ const struct wp_coord *curcircuit;
+ uint8_t prev_i, prev_j;
+ uint8_t dir, prev_dir = 0xFF;
+ uint8_t found = 0, i = 0, j = 0;
+ uint8_t linenum;
+ int8_t step = faceA ? 1 : -1;
+ int8_t skipfirst=0;
+ int8_t path_len = 0;
+
+ printf("face: %d\n", faceA);
+ if ( !faceA && circuit->i == 11 && circuit->j == 6)
+ skipfirst=1;
+
+ /* check that the point is present in the circuit */
+ for (curcircuit = circuit + skipfirst; curcircuit->end == 0; curcircuit ++) {
+ if (curcircuit->i == starti && curcircuit->j == startj) {
+ found = 1;
break;
+ }
}
- *position = pos;
- return neigh;
-}
+ if ( !faceA && curcircuit->i == 11 && curcircuit->j == 6)
+ found = 1;
+ if (found == 0)
+ return -1;
-/* browse all points */
-#define POINT_FOREACH(cur) \
- for (cur = &djpoints[0][0]; \
- cur < &djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY]; \
- cur ++)
+ /* XXX len must be >= 1 */
+ /* XXX start = 11,6 */
-/* XXX comment */
-#define NEIGH_FOREACH(neigh, pos, point) \
- for (pos = START, neigh = get_next_neigh(point, &pos); \
- neigh; \
- neigh = get_next_neigh(point, &pos))
+ prev_i = starti;
+ prev_j = startj;
-int dijkstra_init(void)
-{
- return 0;
+ curcircuit = curcircuit;
+ while (1) {
+ if (faceA && curcircuit->end)
+ break;
+ else if (!faceA && curcircuit == circuit)
+ break;
+ i = curcircuit->i;
+ j = curcircuit->j;
+
+ dir = get_dir(prev_i, prev_j, i, j);
+
+ if (prev_dir != dir) {
+ linenum = get_line_num(prev_i, prev_j, dir);
+ /* printf_P(PSTR("COIN %d, %d, dir=%d linenum=%d\r\n"), */
+ /* prev_i, prev_j, dir, linenum); */
+ circuit_wpline[path_len].line_num = linenum;
+ circuit_wpline[path_len].dir = dir;
+ path_len++;
+ }
+ prev_dir = dir;
+ prev_i = i;
+ prev_j = j;
+ curcircuit += step;
+ }
+
+/* printf_P(PSTR("COIN %d, %d\r\n"), curcircuit->i, curcircuit->j); */
+
+ return path_len; /* XXX */
}
-/* return distance between p1 and p2 */
-static uint16_t dist(struct djpoint *p1, struct djpoint *p2)
+int16_t get_score(uint32_t wcorn_retrieved, uint32_t ucorn_retrieved,
+ uint16_t tomato_retrieved, uint8_t len)
{
- int16_t x1, y1, x2, y2;
- double vx, vy;
- uint8_t i, j;
+ int16_t score = 0;
+ uint8_t i;
+ uint32_t mask = 1;
+ uint8_t n;
+
+ /* score with corn */
+ n = xget_cob_count() * 2;
+ for (i = 0; i<CORN_NB; i++) {
+ if (n >= 10)
+ break;
+ if (wcorn_retrieved & mask) {
+ score += 250;
+ n += 2;
+ }
+ if (n >= 10)
+ break;
+ if (ucorn_retrieved & mask) {
+ score += 125;
+ n += 1;
+ }
+ mask <<= 1UL;
+ }
- djpoint2ij(p1, &i, &j);
- ijcoord_to_xycoord(i, j, &x1, &y1);
+ printf("get score: cob %d \n", n);
+ /* score with tomato */
+ n = xget_ball_count();
+ mask = 1;
+ for (i = 0; i<TOMATO_NB; i++) {
+ if (n >= 4)
+ break;
+ if (tomato_retrieved & mask) {
+ score += 150;
+ n += 1;
+ }
+ mask <<= 1UL;
+ }
- djpoint2ij(p2, &i, &j);
- ijcoord_to_xycoord(i, j, &x2, &y2);
+ printf("get score: ball %d \n", n);
+ /* malus for long circuits */
+ score -= (len * 20);
+
+ /* double malus for long circuits if we don't have much
+ * time */
+#define WP_SPEED 1
+ if (len * WP_SPEED > (MATCH_TIME - xtime_get_s())) {
+ uint16_t extra;
+ extra = (len * WP_SPEED) - (MATCH_TIME - xtime_get_s());
+ score -= (200 * extra);
+ }
- vx = x2 - x1;
- vy = y2 - y1;
- return sqrt(vx * vx + vy * vy);
+ /* XXX use direction of robot */
+
+ return score;
}
-void dijkstra_process_neighs(struct djpoint *pt)
+/* i,j: starting position */
+int8_t browse_one_circuit(const struct wp_coord *circuit,
+ uint8_t starti, uint8_t startj,
+ int16_t *scoreA, int16_t *scoreB)
{
- struct djpoint *neigh;
- uint8_t pos, parent_pos;
- uint16_t weight;
- uint8_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;
+ const struct wp_coord *curcircuit;
+ uint32_t wcorn_retrieved = 0; /* bit mask */
+ uint32_t ucorn_retrieved = 0; /* bit mask */
+ uint16_t tomato_retrieved = 0; /* bit mask */
+ uint8_t len = 0, found = 0, i, j;
+ uint8_t ni = 0, nj = 0, pos, color;
+
+ /* check that the point is present in the circuit */
+ for (curcircuit = circuit; curcircuit->end == 0; curcircuit ++) {
+ if (curcircuit->i == starti && curcircuit->j == startj) {
+ found = 1;
+ break;
}
}
-}
-int dijkstra(struct djpoint *start)
-{
- struct djpoint *cur;
- uint8_t todolist = 1;
+ if (found == 0)
+ return -1;
- start->todo = 1;
+ for (curcircuit = circuit; curcircuit->end == 0; curcircuit ++, len ++) {
+ i = curcircuit->i;
+ j = curcircuit->j;
- 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;
+/* printf("cur:%d,%d %x %x %x %d\n", i, j, */
+/* wcorn_retrieved, ucorn_retrieved, tomato_retrieved, len); */
+
+ /* face A completed */
+ if (i == starti && j == startj) {
+ *scoreA = get_score(wcorn_retrieved, ucorn_retrieved,
+ tomato_retrieved, len);
+ wcorn_retrieved = 0; /* bit mask */
+ ucorn_retrieved = 0; /* bit mask */
+ tomato_retrieved = 0; /* bit mask */
+ len = 0;
+ }
+
+ /* is there a tomato ? */
+ if (strat_db.wp_table[i][j].type == WP_TYPE_TOMATO &&
+ strat_db.wp_table[i][j].present) {
+ tomato_retrieved |= (1UL << strat_db.wp_table[i][j].tomato.idx);
}
- /* convert updated list in todo list */
- todolist = 0;
- POINT_FOREACH(cur) {
- if (!cur->updated)
+ /* browse all neighbours to see if there is cobs */
+ for (pos = LINE_UP; pos <= LINE_R_DOWN; pos++) {
+ if (get_neigh(i, j, &ni, &nj, pos) < 0)
continue;
- todolist = 1;
- cur->todo = 1;
- cur->updated = 0;
+
+ /* is there a corn cob ? */
+ if (strat_db.wp_table[ni][nj].type == WP_TYPE_CORN &&
+ strat_db.wp_table[ni][nj].present) {
+ color = strat_db.wp_table[ni][nj].corn.color;
+ if (color == I2C_COB_WHITE)
+ wcorn_retrieved |= (1UL << strat_db.wp_table[ni][nj].corn.idx);
+ else if (color == I2C_COB_UNKNOWN)
+ ucorn_retrieved |= (1UL << strat_db.wp_table[ni][nj].corn.idx);
+ }
}
- }
- return 0; /* XXX */
-}
+ };
-/* init waypoints position */
-void init_djpoints(void)
-{
- int8_t i, j;
- struct djpoint *pt;
+ *scoreB = get_score(wcorn_retrieved, ucorn_retrieved,
+ tomato_retrieved, len);
+ if (circuit->i == starti && circuit->j == startj)
+ *scoreA = *scoreB;
- for (i=0; i<WAYPOINTS_NBX; i++) {
+ return 0;
+}
- for (j=0; j<WAYPOINTS_NBY; j++) {
- pt = &djpoints[i][j];
- pt->parent_pos = END;
- pt->updated = 0;
- pt->todo = 0;
- pt->weight = 0;
+/* i,j starting position */
+int8_t browse_circuits(uint8_t i, uint8_t j,
+ const struct wp_coord **selected_circuit,
+ int8_t *selected_face)
+{
+ const struct wp_coord **circuit;
+ int16_t scoreA, scoreB;
+ int16_t selected_score = 0x8000; /* ~-int_max */
+ int8_t found = -1;
+
+ *selected_face = 0;
+ *selected_circuit = circuits[0] ;
+ for (circuit = &circuits[0]; *circuit; circuit++) {
+ if (browse_one_circuit(*circuit, i, j, &scoreA, &scoreB) < 0)
+ continue;
+ found = 0;
+ printf_P(PSTR("Circuit: %d %d\r\n"), scoreA, scoreB);
+ if (scoreA > selected_score) {
+ *selected_circuit = *circuit;
+ selected_score = scoreA;
+ *selected_face = 0;
}
+ if (scoreB > selected_score) {
+ *selected_circuit = *circuit;
+ selected_score = scoreB;
+ *selected_face = 1;
+ }
+ }
+ return found;
+}
+
+static void dump_circuit_wp(struct wp_line *circuit_wpline, int8_t len)
+{
+ int8_t i;
+ if (len <= 0)
+ return;
+ for (i = 0; i < len; i ++) {
+ printf(PSTR("linenum %d dir %d\r\n"), circuit_wpline[i].line_num,
+ circuit_wpline[i].dir);
}
+
}
-int get_path(struct djpoint *start, struct djpoint *end)
+uint8_t strat_harvest_circuit(void)
{
- struct djpoint *cur;
- uint8_t prev_direction = 0;
- int8_t idx;
+ const struct wp_coord *selected_circuit;
+ int8_t selected_face;
+ struct wp_line circuit_wpline[MAX_CIRCUIT_WPLINE];
+ int8_t len;
+ uint8_t i, j, idx;
int16_t x, y;
+ uint8_t linenum, prev_linenum;
+ uint8_t dir, prev_dir;
+ uint8_t err;
+
+ x = position_get_x_s16(&mainboard.pos);
+ y = position_get_y_s16(&mainboard.pos);
- 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_xycoord(idx, &x, &y);
- printf_P(PSTR("%d %d (%d)\r\n"),
- x, y, cur->parent_pos);
+ if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) {
+ DEBUG(E_USER_STRAT, "%s(): cannot find waypoint at %d,%d",
+ __FUNCTION__, x, y);
+ return END_ERROR;
+ }
+
+ browse_circuits(i, j, &selected_circuit, &selected_face);
+ len = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+ if (len < 0) {
+ DEBUG(E_USER_STRAT, "%s(): cannot find a path",
+ __FUNCTION__);
+ return END_ERROR;
+ }
+
+ dump_circuit_wp(circuit_wpline, len);
+
+ prev_linenum = circuit_wpline[0].line_num;
+ prev_dir = circuit_wpline[0].dir;
+ for (idx = 1; idx < len; idx ++) {
+ retry:
+ if (get_cob_count() >= 5)
+ strat_set_speed(600, SPEED_ANGLE_FAST);
+
+ linenum = circuit_wpline[idx].line_num;
+ dir = circuit_wpline[idx].dir;
+
+ /* XXX basic opponent management */
+ DEBUG(E_USER_STRAT, "%s(): line %d dir %d -> line %d dir %d",
+ __FUNCTION__, prev_linenum, prev_dir, linenum, dir);
+ err = line2line(prev_linenum, prev_dir, linenum, dir);
+ if (!TRAJ_SUCCESS(err)) {
+ strat_hardstop();
+ time_wait_ms(2000);
+ goto retry;
}
- prev_direction = cur->parent_pos;
+
+ prev_linenum = linenum;
+ prev_dir = dir;
}
- idx = PT2IDX(end);
- corn_idx_to_xycoord(idx, &x, &y);
- printf_P(PSTR("%d %d\r\n"), x, y);
- return 0; /* XXX */
+ return END_TRAJ; // XXX
+}
+
+void test_strat_avoid(void)
+{
+ uint8_t i, j;
+ const struct wp_coord *selected_circuit;
+ int8_t selected_face;
+ struct wp_line circuit_wpline[MAX_CIRCUIT_WPLINE];
+ int8_t ret;
+
+ i = 1; j = 1;
+ printf("========= i=%d, j=%d\r\n", i, j);
+
+ ts = 0; bc = 0; cc = 0;
+ printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+ browse_circuits(i, j, &selected_circuit, &selected_face);
+ ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+ dump_circuit_wp(circuit_wpline, ret);
+
+ ts = 0; bc = 3; cc = 0;
+ printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+ browse_circuits(i, j, &selected_circuit, &selected_face);
+ ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+ dump_circuit_wp(circuit_wpline, ret);
+
+ ts = 0; bc = 4; cc = 0;
+ printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+ browse_circuits(i, j, &selected_circuit, &selected_face);
+ ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+ dump_circuit_wp(circuit_wpline, ret);
+
+ ts = 0; bc = 3; cc = 5;
+ printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+ browse_circuits(i, j, &selected_circuit, &selected_face);
+ ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+ dump_circuit_wp(circuit_wpline, ret);
+
+ ts = 0; bc = 4; cc = 5;
+ printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+ browse_circuits(i, j, &selected_circuit, &selected_face);
+ ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+ dump_circuit_wp(circuit_wpline, ret);
+
+ ts = 80; bc = 0; cc = 0;
+ printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+ browse_circuits(i, j, &selected_circuit, &selected_face);
+ ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+ dump_circuit_wp(circuit_wpline, ret);
+
+ i = 4; j = 3;
+ printf("========= i=%d, j=%d\r\n", i, j);
+
+ ts = 0; bc = 0; cc = 0;
+ printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+ browse_circuits(i, j, &selected_circuit, &selected_face);
+ ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+ dump_circuit_wp(circuit_wpline, ret);
+
+ ts = 0; bc = 3; cc = 0;
+ printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+ browse_circuits(i, j, &selected_circuit, &selected_face);
+ ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+ dump_circuit_wp(circuit_wpline, ret);
+
+ ts = 80; bc = 0; cc = 0;
+ printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+ browse_circuits(i, j, &selected_circuit, &selected_face);
+ ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+ dump_circuit_wp(circuit_wpline, ret);
+
+ i = 11; j = 6;
+ printf("========= i=%d, j=%d\r\n", i, j);
+
+ ts = 0; bc = 0; cc = 0;
+ printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+ browse_circuits(i, j, &selected_circuit, &selected_face);
+ ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+ dump_circuit_wp(circuit_wpline, ret);
+
+ ts = 0; bc = 3; cc = 0;
+ printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+ browse_circuits(i, j, &selected_circuit, &selected_face);
+ ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+ dump_circuit_wp(circuit_wpline, ret);
+
+ ts = 80; bc = 0; cc = 0;
+ printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+ browse_circuits(i, j, &selected_circuit, &selected_face);
+ ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
+ dump_circuit_wp(circuit_wpline, ret);
+
}
git.droids-corp.org / aversive.git / commitdiff