return pos;
}
+#if 0
static uint8_t cc;
uint8_t xget_cob_count(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:
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,
/* 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 ? */
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);
void test_strat_avoid(void)
{
+
+ //corn_count_neigh(1, 3);
+
+
#if 0
uint8_t i, j;
const struct wp_coord *selected_circuit;
#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)
{
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;
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);
/* 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.) {
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");
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;
- }
-}
git.droids-corp.org / aversive.git / commitdiff