From 876789a372da2e93a816428649e511c2ad9e40be Mon Sep 17 00:00:00 2001 From: zer0 Date: Mon, 3 May 2010 23:21:16 +0200 Subject: [PATCH] go fast between corn cobs --- projects/microb2010/common/i2c_commands.h | 1 + projects/microb2010/mainboard/strat.c | 18 +- projects/microb2010/mainboard/strat.h | 4 + projects/microb2010/mainboard/strat_avoid.c | 70 ++++-- projects/microb2010/mainboard/strat_avoid.h | 12 + projects/microb2010/mainboard/strat_corn.c | 254 +++++++++++++++----- projects/microb2010/mainboard/strat_corn.h | 9 +- projects/microb2010/mainboard/strat_db.c | 11 +- 8 files changed, 279 insertions(+), 100 deletions(-) diff --git a/projects/microb2010/common/i2c_commands.h b/projects/microb2010/common/i2c_commands.h index d64ed8b..bbebd32 100644 --- a/projects/microb2010/common/i2c_commands.h +++ b/projects/microb2010/common/i2c_commands.h @@ -32,6 +32,7 @@ #define I2C_RIGHT_SIDE 1 #define I2C_AUTO_SIDE 2 #define I2C_CENTER_SIDE 3 +#define I2C_NO_SIDE 4 #define I2C_COLOR_YELLOW 0 #define I2C_COLOR_BLUE 1 diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index 1585734..ced55e3 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -188,8 +188,6 @@ void strat_event(void *dummy) cobboard.cob_count = 5; if (time_get_s() == 16) cobboard.cob_count = 0; - if (time_get_s() == 25) - cobboard.cob_count = 5; #endif /* detect cob on left side */ @@ -280,7 +278,7 @@ static uint8_t strat_eject(void) TRAJ_FLAGS_NO_NEAR); if (err == 0) { want_pack = 1; - strat_set_speed(600, SPEED_ANGLE_SLOW); + strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW); err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); } @@ -328,14 +326,11 @@ static uint8_t strat_beginning(void) TRAJ_FLAGS_STD); strat_set_acc(ACC_DIST, ACC_ANGLE); - strat_set_speed(250, SPEED_ANGLE_SLOW); + strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW); l1: 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(0, LINE_UP, 2, LINE_R_DOWN); + err = line2line(0, LINE_UP, 2, LINE_R_DOWN, TRAJ_FLAGS_NO_NEAR); if (!TRAJ_SUCCESS(err)) { strat_hardstop(); time_wait_ms(2000); @@ -344,10 +339,7 @@ static uint8_t strat_beginning(void) l2: 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(2, LINE_R_DOWN, 2, LINE_R_UP); + err = line2line(2, LINE_R_DOWN, 2, LINE_R_UP, TRAJ_FLAGS_NO_NEAR); if (!TRAJ_SUCCESS(err)) { strat_hardstop(); time_wait_ms(2000); @@ -357,7 +349,7 @@ static uint8_t strat_beginning(void) strat_eject(); while (1) { - strat_set_speed(250, SPEED_ANGLE_SLOW); + strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW); strat_harvest_circuit(); strat_eject(); } diff --git a/projects/microb2010/mainboard/strat.h b/projects/microb2010/mainboard/strat.h index 94433c1..0930706 100644 --- a/projects/microb2010/mainboard/strat.h +++ b/projects/microb2010/mainboard/strat.h @@ -142,6 +142,10 @@ #define SPEED_DIST_VERY_SLOW 400. #define SPEED_ANGLE_VERY_SLOW 400. +#define SPEED_CLITOID_SLOW 250. +#define SPEED_CLITOID_FAST 500. + + /* strat infos structures */ struct strat_conf { uint8_t dump_enabled; diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c index ff5dac3..36bc163 100644 --- a/projects/microb2010/mainboard/strat_avoid.c +++ b/projects/microb2010/mainboard/strat_avoid.c @@ -130,6 +130,7 @@ static inline uint8_t opposite_position(uint8_t pos) return pos; } +#if 0 static uint8_t cc; uint8_t xget_cob_count(void) { @@ -147,35 +148,38 @@ uint8_t xtime_get_s(void) { return ts; } +#else +#define xget_cob_count() get_cob_count() +#define xget_ball_count() get_ball_count() +#define xtime_get_s() time_get_s() +#endif - -/* get the neighbour of the point at specified position, return -1 if +/* get the neighbour of the point at specified dir, 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) +int8_t wp_get_neigh(uint8_t i, uint8_t j, uint8_t *ni, uint8_t *nj, + uint8_t dir) { - switch (position) { + switch (dir) { case LINE_UP: j++; break; case LINE_R_UP: - if (!(i & 1)) j++; + if ((i & 1)) j++; i++; break; case LINE_R_DOWN: - if (i & 1) j--; + if (!(i & 1)) j--; i++; break; case LINE_DOWN: j--; break; case LINE_L_DOWN: - if (i & 1) j--; + if (!(i & 1)) j--; i--; break; case LINE_L_UP: - if (!(i & 1)) j++; + if ((i & 1)) j++; i--; break; default: @@ -248,6 +252,40 @@ static uint8_t get_dir(uint8_t prev_i, uint8_t prev_j, return 0xFF; } +/* return true if a waypoint belongs to a line */ +uint8_t wp_belongs_to_line(uint8_t i, uint8_t j, uint8_t linenum, uint8_t dir) +{ + uint8_t ln; + ln = get_line_num(i, j, dir); + if (ln == linenum) + return 1; + return 0; +} + +/* count the number of non-black corns which are neighbors of + * specified cob */ +uint8_t corn_count_neigh(uint8_t i, uint8_t j) +{ + uint8_t dir, n = 0; + uint8_t ni, nj; + + for (dir = LINE_UP; dir <= LINE_R_DOWN; dir++) { + if (wp_get_neigh(i, j, &ni, &nj, dir) < 0) + continue; + + //printf("i,j=%d,%d dir=%d, ni,nj=%d,%d\r\n", + // i, j, dir, ni, nj); + + /* is there a corn cob ? */ + if (strat_db.wp_table[ni][nj].type == WP_TYPE_CORN && + strat_db.wp_table[ni][nj].present && + strat_db.wp_table[ni][nj].corn.color != I2C_COB_BLACK) + n ++; + } + + return n; +} + int8_t get_path(const struct wp_coord *circuit, uint8_t starti, uint8_t startj, uint8_t faceA, @@ -421,7 +459,7 @@ int8_t browse_one_circuit(const struct wp_coord *circuit, /* 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) + if (wp_get_neigh(i, j, &ni, &nj, pos) < 0) continue; /* is there a corn cob ? */ @@ -522,16 +560,14 @@ uint8_t strat_harvest_circuit(void) 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); + err = line2line(prev_linenum, prev_dir, linenum, dir, + TRAJ_FLAGS_NO_NEAR); if (!TRAJ_SUCCESS(err)) { strat_hardstop(); time_wait_ms(2000); @@ -547,6 +583,10 @@ uint8_t strat_harvest_circuit(void) void test_strat_avoid(void) { + + //corn_count_neigh(1, 3); + + #if 0 uint8_t i, j; const struct wp_coord *selected_circuit; diff --git a/projects/microb2010/mainboard/strat_avoid.h b/projects/microb2010/mainboard/strat_avoid.h index 4afe032..d7ac4ac 100644 --- a/projects/microb2010/mainboard/strat_avoid.h +++ b/projects/microb2010/mainboard/strat_avoid.h @@ -42,6 +42,18 @@ int8_t browse_circuits(uint8_t i, uint8_t j, /* harvest on the best circuit */ uint8_t strat_harvest_circuit(void); +/* get the neighbour of the point at specified dir, return -1 if + * there is no neighbor */ +int8_t wp_get_neigh(uint8_t i, uint8_t j, uint8_t *ni, uint8_t *nj, + uint8_t dir); + +/* count the number of non-black corns which are neighbors of + * specified cob */ +uint8_t corn_count_neigh(uint8_t i, uint8_t j); + +/* return true if a waypoint belongs to a line */ +uint8_t wp_belongs_to_line(uint8_t i, uint8_t j, uint8_t linenum, uint8_t dir); + void test_strat_avoid(void); #endif diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c index 3ec6a2c..1f5e1d3 100644 --- a/projects/microb2010/mainboard/strat_corn.c +++ b/projects/microb2010/mainboard/strat_corn.c @@ -62,10 +62,13 @@ #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" +static volatile uint8_t clitoid_slow = 0; + /* return 1 if there is a corn near, and fill the index ptr */ int8_t corn_is_near(uint8_t *corn_idx, uint8_t side) { @@ -97,17 +100,149 @@ int8_t corn_is_near(uint8_t *corn_idx, uint8_t side) return 1; } +/* fill 2 points that are on the line (num, dir) */ +static void num2line(struct line_2pts *l, uint8_t num, uint8_t dir) +{ + float n = num; + + switch (dir) { + + case LINE_UP: + l->p1.x = n * 450 + 375; + l->p1.y = COLOR_Y(0); + l->p2.x = n * 450 + 375; + l->p2.y = COLOR_Y(2100); + break; + case LINE_DOWN: + l->p1.x = n * 450 + 375; + l->p1.y = COLOR_Y(2100); + l->p2.x = n * 450 + 375; + l->p2.y = COLOR_Y(0); + break; + case LINE_R_UP: + l->p1.x = 150; + l->p1.y = COLOR_Y(-n * 500 + 1472); + l->p2.x = 2850; + l->p2.y = COLOR_Y((-n + 4) * 500 + 972); + break; + case LINE_L_DOWN: + l->p1.x = 2850; + l->p1.y = COLOR_Y((-n + 4) * 500 + 972); + l->p2.x = 150; + l->p2.y = COLOR_Y(-n * 500 + 1472); + break; + case LINE_L_UP: + l->p1.x = 2850; + l->p1.y = COLOR_Y(-n * 500 + 1472); + l->p2.x = 150; + l->p2.y = COLOR_Y((-n + 4) * 500 + 972); + break; + case LINE_R_DOWN: + l->p1.x = 150; + l->p1.y = COLOR_Y((-n + 4) * 500 + 972); + l->p2.x = 2850; + l->p2.y = COLOR_Y(-n * 500 + 1472); + break; + default: + break; + } +} + +/* return true if we must go slow */ +static uint8_t clitoid_select_speed(uint8_t num1, uint8_t dir1, + uint8_t num2, uint8_t dir2) +{ + int16_t x, y; + uint8_t i, j; + uint8_t i2, i3, j2, j3; /* next wp */ + + x = position_get_x_s16(&mainboard.pos); + y = position_get_y_s16(&mainboard.pos); + + if (get_cob_count() >= 5) + return 0; /* fast */ + + if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) { + DEBUG(E_USER_STRAT, "%s(): cannot find waypoint at %d,%d", + __FUNCTION__, x, y); + return 1; + } + +/* if (time_get_s() > 39) */ +/* DEBUG(E_USER_STRAT, "i,j = (%d %d), count=%d", i, j, corn_count_neigh(i, j)); */ + + if (corn_count_neigh(i, j) == 2) + return 1; + + if (wp_belongs_to_line(i, j, num2, dir2)) + return 0; + + /* get next point */ + if (wp_get_neigh(i, j, &i2, &j2, dir1) < 0) { + DEBUG(E_USER_STRAT, "%s(): cannot get neigh1", + __FUNCTION__); + return 1; + } + + /* if (i2, j2) belongs to next line, check corn in opposition */ + if (wp_belongs_to_line(i2, j2, num2, dir2)) { + if (corn_count_neigh(i2, j2) > 0) + return 1; + else + return 0; + } + + /* get next point */ + if (wp_get_neigh(i2, j2, &i3, &j3, dir1) < 0) { + DEBUG(E_USER_STRAT, "%s(): cannot get neigh2", + __FUNCTION__); + return 1; + } + + /* if (i3, j3) belongs to next line, check corn in opposition */ + if (wp_belongs_to_line(i3, j3, num2, dir2)) { + if (corn_count_neigh(i2, j2) > 0 || + corn_count_neigh(i3, j3) > 0) + return 1; + else + return 0; + } + + /* go fast */ + return 0; +} + /* + * handle speed before clitoid (on the line), depending on strat_db. * return true if clitoid started */ -static uint8_t clitoid_started(void) +#define NORETURN_DIST 300 +static uint8_t speedify_clitoid(uint8_t num1, uint8_t dir1, + uint8_t num2, uint8_t dir2) { + uint8_t slow; + double turnx, turny; + + slow = clitoid_select_speed(num1, dir1, num2, dir2); + if (slow != clitoid_slow) { + turnx = mainboard.traj.target.line.turn_pt.x; + turny = mainboard.traj.target.line.turn_pt.y; + if (distance_from_robot(turnx, turny) > NORETURN_DIST) { + clitoid_slow = slow; + return 1; + } + } + return trajectory_get_state(&mainboard.traj) == RUNNING_CLITOID_CURVE; } -/* XXX passer les flags de traj */ -uint8_t line2line(uint8_t num1, uint8_t dir1, - uint8_t num2, uint8_t dir2) +/* process the clitoid parameters, return 0 on success or -1 if + * clitoid cannot be executed. pack_spickles is set to I2C_LEFT_SIDE, + * I2C_RIGHT_SIDE or I2C_NO_SIDE to tell if we need to pack a specific + * spickle. */ +static int8_t strat_calc_clitoid(uint8_t num1, uint8_t dir1, + uint8_t num2, uint8_t dir2, + uint8_t *pack_spickles) { double line1_a_rad, line1_a_deg, line2_a_rad; double diff_a_deg, diff_a_deg_abs, beta_deg; @@ -115,12 +250,11 @@ uint8_t line2line(uint8_t num1, uint8_t dir1, struct line_2pts l1, l2; line_t ll1, ll2; point_t p; - uint8_t err; int8_t ret; /* convert to 2 points */ - num2line(&l1, dir1, num1); - num2line(&l2, dir2, num2); + num2line(&l1, num1, dir1); + num2line(&l2, num2, dir2); DEBUG(E_USER_STRAT, "line1: (%2.2f, %2.2f) -> (%2.2f, %2.2f)", l1.p1.x, l1.p1.y, l1.p2.x, l1.p2.y); @@ -149,13 +283,19 @@ uint8_t line2line(uint8_t num1, uint8_t dir1, /* printf_P(PSTR("diff_a_deg=%2.2f\r\n"), diff_a_deg_abs); */ /* printf_P(PSTR("inter=%2.2f,%2.2f\r\n"), p.x, p.y); */ + *pack_spickles = I2C_NO_SIDE; + /* small angle, 60 deg */ if (diff_a_deg_abs < 70.) { radius = 150; - if (diff_a_deg > 0) + if (diff_a_deg > 0) { beta_deg = 0; - else + *pack_spickles = I2C_RIGHT_SIDE; + } + else { beta_deg = 0; + *pack_spickles = I2C_RIGHT_SIDE; + } } /* double 90 deg for half turn -- not used */ else if (diff_a_deg_abs < 100.) { @@ -174,30 +314,62 @@ uint8_t line2line(uint8_t num1, uint8_t dir1, beta_deg = 0; } + clitoid_slow = clitoid_select_speed(num1, dir1, num2, dir2); + if (clitoid_slow) { + DEBUG(E_USER_STRAT, "slow clito\n"); + strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW); + } + else { + DEBUG(E_USER_STRAT, "fast clito\n"); + strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW); + } + /* XXX check return value !! */ ret = trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y, line1_a_deg, 150., diff_a_deg, beta_deg, radius, xy_norm(l1.p1.x, l1.p1.y, p.x, p.y)); + return ret; +} + +/* go from line num1,dir1 to line num2,dir2. Uses trjectory flags + * specified as argument and return END_xxx condition */ +uint8_t line2line(uint8_t num1, uint8_t dir1, uint8_t num2, + uint8_t dir2, uint8_t flags) +{ + int8_t ret; + uint8_t err, pack_spickles; + + reprocess: + ret = strat_calc_clitoid(num1, dir1, num2, dir2, &pack_spickles); if (ret < 0) DEBUG(E_USER_STRAT, "clitoid failed"); /* XXX what to do if cobboard is stucked */ - err = WAIT_COND_OR_TRAJ_END(clitoid_started(), 0xFF); - DEBUG(E_USER_STRAT, "clitoid started err=%d diff_a_deg_abs=%2.2f diff_a_deg=%2.2f", - err, diff_a_deg_abs, diff_a_deg); + /* wait beginning of clitoid or changing of speed */ + err = WAIT_COND_OR_TRAJ_END(speedify_clitoid(num1, dir1, + num2, dir2), + flags); + + /* the speed has to change */ + if (trajectory_get_state(&mainboard.traj) != RUNNING_CLITOID_CURVE) + goto reprocess; + + DEBUG(E_USER_STRAT, "clitoid started err=%d pack_spickles=%d", + err, pack_spickles); /* when clitoid starts and angle is 60 deg, pack external * spickle */ - if (diff_a_deg_abs < 70. && err == 0) { - if (diff_a_deg > 0) - strat_rpack60 = 1; - else + if (err == 0) { + if (pack_spickles == I2C_LEFT_SIDE) strat_lpack60 = 1; + else if (pack_spickles == I2C_RIGHT_SIDE) + strat_rpack60 = 1; + + /* wait end of clitoid */ + err = wait_traj_end(flags); } - if (err == 0) - err = wait_traj_end(0xFF); DEBUG(E_USER_STRAT, "clitoid finished"); @@ -212,49 +384,3 @@ uint8_t line2line(uint8_t num1, uint8_t dir1, return err; } -void num2line(struct line_2pts *l, uint8_t dir, uint8_t num) -{ - float n = num; - - switch (dir) { - - case LINE_UP: - l->p1.x = n * 450 + 375; - l->p1.y = COLOR_Y(0); - l->p2.x = n * 450 + 375; - l->p2.y = COLOR_Y(2100); - break; - case LINE_DOWN: - l->p1.x = n * 450 + 375; - l->p1.y = COLOR_Y(2100); - l->p2.x = n * 450 + 375; - l->p2.y = COLOR_Y(0); - break; - case LINE_R_UP: - l->p1.x = 150; - l->p1.y = COLOR_Y(-n * 500 + 1472); - l->p2.x = 2850; - l->p2.y = COLOR_Y((-n + 4) * 500 + 972); - break; - case LINE_L_DOWN: - l->p1.x = 2850; - l->p1.y = COLOR_Y((-n + 4) * 500 + 972); - l->p2.x = 150; - l->p2.y = COLOR_Y(-n * 500 + 1472); - break; - case LINE_L_UP: - l->p1.x = 2850; - l->p1.y = COLOR_Y(-n * 500 + 1472); - l->p2.x = 150; - l->p2.y = COLOR_Y((-n + 4) * 500 + 972); - break; - case LINE_R_DOWN: - l->p1.x = 150; - l->p1.y = COLOR_Y((-n + 4) * 500 + 972); - l->p2.x = 2850; - l->p2.y = COLOR_Y(-n * 500 + 1472); - break; - default: - break; - } -} diff --git a/projects/microb2010/mainboard/strat_corn.h b/projects/microb2010/mainboard/strat_corn.h index 8b8342f..e84b83d 100644 --- a/projects/microb2010/mainboard/strat_corn.h +++ b/projects/microb2010/mainboard/strat_corn.h @@ -32,9 +32,10 @@ struct line_2pts { point_t p2; }; +/* there is a corn near */ int8_t corn_is_near(uint8_t *corn_idx, uint8_t side); -void num2line(struct line_2pts *l, uint8_t dir, uint8_t num); - -uint8_t line2line(uint8_t num1, uint8_t dir1, - uint8_t num2, uint8_t dir2); +/* go from line num1,dir1 to line num2,dir2. Uses trjectory flags + * specified as argument and return END_xxx condition */ +uint8_t line2line(uint8_t num1, uint8_t dir1, uint8_t num2, + uint8_t dir2, uint8_t flags); diff --git a/projects/microb2010/mainboard/strat_db.c b/projects/microb2010/mainboard/strat_db.c index d2835fe..f21dd57 100644 --- a/projects/microb2010/mainboard/strat_db.c +++ b/projects/microb2010/mainboard/strat_db.c @@ -173,10 +173,14 @@ int8_t xycoord_to_ijcoord(int16_t *xp, int16_t *yp, uint8_t *ip, uint8_t *jp) if ((i & 1) == 1) { j = y / STEP_CORN_Y; } + else if ((i & 3) == 0) { + j = y / (STEP_CORN_Y*2); + j = j*2 + 1; + } else { - y += (STEP_CORN_Y/2); - y /= (STEP_CORN_Y*2); - j = (y * 2) + 1; + y += (STEP_CORN_Y); + j = y / (STEP_CORN_Y*2); + j = j*2; } if (ijcoord_to_xycoord(i, j, &x, &y) < 0) @@ -194,7 +198,6 @@ int8_t xycoord_to_ijcoord(int16_t *xp, int16_t *yp, uint8_t *ip, uint8_t *jp) return 0; } - /******** CORN */ /* return the index of a corn given its i,j coords. */ -- 2.39.5