X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_corn.c;h=1f5e1d347c4868af3093b634cb242ef842498480;hp=3694bcdab01e70367ac9c2ac3a52358e7ed776d6;hb=876789a372da2e93a816428649e511c2ad9e40be;hpb=4e7801883ed4076cb14b63a0571467747894c0f8 diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c index 3694bcd..1f5e1d3 100644 --- a/projects/microb2010/mainboard/strat_corn.c +++ b/projects/microb2010/mainboard/strat_corn.c @@ -27,6 +27,7 @@ #include #include +#include #include #include @@ -61,14 +62,17 @@ #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" -#if 0 +static volatile uint8_t clitoid_slow = 0; + /* return 1 if there is a corn near, and fill the index ptr */ -uint8_t corn_is_near(int8_t *corn_idx, uint8_t side) +int8_t corn_is_near(uint8_t *corn_idx, uint8_t side) { + /* XXX to be checked */ #define SENSOR_CORN_DIST 225 #define SENSOR_CORN_ANGLE 90 double x = position_get_x_double(&mainboard.pos); @@ -90,48 +94,155 @@ uint8_t corn_is_near(int8_t *corn_idx, uint8_t side) y_corn_int = y_corn; wp = xycoord_to_corn_idx(&x_corn_int, &y_corn_int); - if (wp->corn.idx < 0) + if (wp == NULL) return 0; + *corn_idx = wp->corn.idx; return 1; } -#endif -/* - * - send the correct commands to the spickles - * - return 1 if we need to stop (cobboard is stucked) -*/ -static uint8_t handle_spickles(void) +/* fill 2 points that are on the line (num, dir) */ +static void num2line(struct line_2pts *l, uint8_t num, uint8_t dir) { - return 0; -#if 0 - int8_t corn_idx; + float n = num; - if (!corn_is_near(&corn_idx, I2C_LEFT_SIDE)) - i2c_cobboard_mode_deploy(I2C_LEFT_SIDE); - else { - if (corn_table[corn_idx] == TYPE_WHITE_CORN) - i2c_cobboard_mode_harvest(I2C_LEFT_SIDE); + 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 - i2c_cobboard_mode_pack(I2C_LEFT_SIDE); + return 0; } -/* printf("%d %d\n", corn_idx, corn_table[corn_idx]); */ -/* time_wait_ms(100); */ - if (!corn_is_near(&corn_idx, I2C_RIGHT_SIDE)) - i2c_cobboard_mode_deploy(I2C_RIGHT_SIDE); - else { - if (corn_table[corn_idx] == TYPE_WHITE_CORN) - i2c_cobboard_mode_harvest(I2C_RIGHT_SIDE); + /* 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 - i2c_cobboard_mode_pack(I2C_RIGHT_SIDE); + return 0; } + /* go fast */ return 0; -#endif } -uint8_t line2line(uint8_t dir1, uint8_t num1, - uint8_t dir2, uint8_t num2) +/* + * handle speed before clitoid (on the line), depending on strat_db. + * return true if clitoid started + */ +#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; +} + +/* 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; @@ -139,16 +250,16 @@ uint8_t line2line(uint8_t dir1, uint8_t num1, 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); - printf_P(PSTR("A2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"), - l1.p1.x, l1.p1.y, l1.p2.x, l1.p2.y); - printf_P(PSTR("B2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"), - l2.p1.x, l2.p1.y, l2.p2.x, l2.p2.y); + 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); + DEBUG(E_USER_STRAT, "line2: (%2.2f, %2.2f) -> (%2.2f, %2.2f)", + l2.p1.x, l2.p1.y, l2.p2.x, l2.p2.y); /* convert to line eq and find intersection */ pts2line(&l1.p1, &l1.p2, &ll1); @@ -161,15 +272,32 @@ uint8_t line2line(uint8_t dir1, uint8_t num1, line2_a_rad = atan2(l2.p2.y - l2.p1.y, l2.p2.x - l2.p1.x); diff_a_deg = DEG(line2_a_rad - line1_a_rad); + if (diff_a_deg < -180) { + diff_a_deg += 360; + } + else if (diff_a_deg > 180) { + diff_a_deg -= 360; + } diff_a_deg_abs = fabs(diff_a_deg); +/* 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 = 200; - if (diff_a_deg > 0) - beta_deg = 40; - else - beta_deg = -40; + radius = 150; + if (diff_a_deg > 0) { + beta_deg = 0; + *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.) { radius = 100; if (diff_a_deg > 0) @@ -177,78 +305,82 @@ uint8_t line2line(uint8_t dir1, uint8_t num1, else beta_deg = -40; } + /* hard turn, 120 deg */ else { - radius = 120; + radius = 75; if (diff_a_deg > 0) - beta_deg = 60; + beta_deg = 0; else - beta_deg = -60; - } - - 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)); - 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); - } + beta_deg = 0; } - err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); - return err; + 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; } -void num2line(struct line_2pts *l, uint8_t dir, uint8_t num) +/* 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) { - float n = num; + int8_t ret; + uint8_t err, pack_spickles; - switch (dir) { + reprocess: + ret = strat_calc_clitoid(num1, dir1, num2, dir2, &pack_spickles); + if (ret < 0) + DEBUG(E_USER_STRAT, "clitoid failed"); - 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; + /* XXX what to do if cobboard is stucked */ + + /* 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 (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); } + + DEBUG(E_USER_STRAT, "clitoid finished"); + + /* XXX 600 -> cste */ + /* XXX does not work, do better */ +/* if (err == 0 && d_speed < 600 && */ +/* mainboard.traj.state == RUNNING_CLITOID_LINE) */ +/* strat_set_speed(600, SPEED_ANGLE_FAST); */ + + strat_rpack60 = 0; + strat_lpack60 = 0; + return err; } +