From: zer0 Date: Sun, 2 May 2010 22:43:48 +0000 (+0200) Subject: circuit strat, first version X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=commitdiff_plain;h=17aadc4c8c3e60c2b5e6bbba91c8542849addbd7;hp=f1b54a67a3317295704f25e2a802e5f86646f4d5 circuit strat, first version --- diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index fc51e8e..52b6b92 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -870,6 +870,8 @@ static void cmd_ballboard_show_parsed(void * parsed_result, void * data) printf_P(PSTR("mode = %x\r\n"), ballboard.mode); printf_P(PSTR("status = %x\r\n"), ballboard.status); printf_P(PSTR("ball_count = %d\r\n"), ballboard.ball_count); + printf_P(PSTR("lcob = %d\r\n"), ballboard.lcob); + printf_P(PSTR("rcob = %d\r\n"), ballboard.rcob); #endif } diff --git a/projects/microb2010/mainboard/main.c b/projects/microb2010/mainboard/main.c index b6a12c8..ce76583 100755 --- a/projects/microb2010/mainboard/main.c +++ b/projects/microb2010/mainboard/main.c @@ -68,8 +68,10 @@ #include "cs.h" #include "strat_base.h" #include "strat_db.h" +#include "strat_avoid.h" #include "i2c_protocol.h" + /* 0 means "programmed" * ---- with 16 Mhz quartz * CKSEL 3-0 : 0111 @@ -297,6 +299,7 @@ int main(void) gen.logs[0] = E_USER_STRAT; gen.log_level = 5; strat_db_init(); + test_strat_avoid(); /* strat-related event */ scheduler_add_periodical_event_priority(strat_event, NULL, diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index 5ae3797..7ea7b88 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -61,6 +61,7 @@ #include "strat_base.h" #include "strat_corn.h" #include "strat_utils.h" +#include "strat_avoid.h" #include "sensor.h" #include "actuator.h" @@ -85,7 +86,7 @@ void strat_preinit(void) 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__); } @@ -119,7 +120,6 @@ void strat_init(void) /* 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(); @@ -165,6 +165,7 @@ void strat_event(void *dummy) if (strat_running == 0) return; + /* read sensors from ballboard */ IRQ_LOCK(flags); lcob = ballboard.lcob; ballboard.lcob = I2C_COB_NONE; @@ -172,6 +173,12 @@ void strat_event(void *dummy) 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 @@ -202,31 +209,47 @@ void strat_event(void *dummy) } /* 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); } @@ -248,32 +271,37 @@ static uint8_t strat_eject(void) //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; } @@ -301,11 +329,11 @@ static uint8_t strat_beginning(void) #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); @@ -313,11 +341,11 @@ static uint8_t strat_beginning(void) } 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); @@ -325,16 +353,22 @@ static uint8_t strat_beginning(void) } #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; @@ -344,12 +378,13 @@ static uint8_t strat_beginning(void) 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); @@ -357,20 +392,23 @@ static uint8_t strat_beginning(void) } 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); diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c index 87bcf9f..cd54e6c 100644 --- a/projects/microb2010/mainboard/strat_avoid.c +++ b/projects/microb2010/mainboard/strat_avoid.c @@ -28,6 +28,7 @@ #include #include +#include #include #include @@ -61,322 +62,576 @@ #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=0; j--) { - printf_P(PSTR("%3d "), j/2); - - if ((j&1) == 0) - printf_P(PSTR(" ")); - - for (i=0; itype == 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= 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= 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; iparent_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); + } diff --git a/projects/microb2010/mainboard/strat_avoid.h b/projects/microb2010/mainboard/strat_avoid.h new file mode 100644 index 0000000..4afe032 --- /dev/null +++ b/projects/microb2010/mainboard/strat_avoid.h @@ -0,0 +1,47 @@ +/* + * 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 + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * 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.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $ + * + * Olivier MATZ + */ + +#ifndef _STRAT_AVOID_H_ +#define _STRAT_AVOID_H_ +#define MAX_CIRCUIT_WPLINE 15 + +struct wp_coord { + uint8_t i:4; + uint8_t j:3; + uint8_t end:1; +}; + +struct wp_line { + uint8_t line_num:4; + uint8_t dir:4; +}; + +int8_t browse_circuits(uint8_t i, uint8_t j, + const struct wp_coord **selected_circuit, + int8_t *selected_face); + +/* harvest on the best circuit */ +uint8_t strat_harvest_circuit(void); + +void test_strat_avoid(void); + +#endif diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c index f8cafbc..8f8503f 100644 --- a/projects/microb2010/mainboard/strat_corn.c +++ b/projects/microb2010/mainboard/strat_corn.c @@ -131,8 +131,8 @@ static uint8_t handle_spickles(void) #endif } -uint8_t line2line(uint8_t dir1, uint8_t num1, - uint8_t dir2, uint8_t num2) +uint8_t line2line(uint8_t num1, uint8_t dir1, + uint8_t num2, uint8_t dir2) { double line1_a_rad, line1_a_deg, line2_a_rad; double diff_a_deg, diff_a_deg_abs, beta_deg; @@ -142,6 +142,7 @@ uint8_t line2line(uint8_t dir1, uint8_t num1, point_t p; uint8_t err; uint16_t a_speed, d_speed; + int8_t ret; /* convert to 2 points */ num2line(&l1, dir1, num1); @@ -197,10 +198,13 @@ uint8_t line2line(uint8_t dir1, uint8_t num1, } /* XXX check return value !! */ - 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)); + 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)); + if (ret < 0) + DEBUG(E_USER_STRAT, "clitoid failed"); + /* disabled */ if (0) { err = 0; diff --git a/projects/microb2010/mainboard/strat_corn.h b/projects/microb2010/mainboard/strat_corn.h index ac806d3..8b8342f 100644 --- a/projects/microb2010/mainboard/strat_corn.h +++ b/projects/microb2010/mainboard/strat_corn.h @@ -36,5 +36,5 @@ 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 dir1, uint8_t num1, - uint8_t dir2, uint8_t num2); +uint8_t line2line(uint8_t num1, uint8_t dir1, + uint8_t num2, uint8_t dir2); diff --git a/projects/microb2010/mainboard/strat_db.c b/projects/microb2010/mainboard/strat_db.c index 2d38967..d2835fe 100644 --- a/projects/microb2010/mainboard/strat_db.c +++ b/projects/microb2010/mainboard/strat_db.c @@ -153,6 +153,48 @@ int8_t ijcoord_to_xycoord(uint8_t i, uint8_t j, int16_t *x, int16_t *y) return 0; } +/* return the nearest waypoint that is not a corn: xp and yp contains + * the input and output, and ip, jp are only outputs. return 0 on + * success. */ +int8_t xycoord_to_ijcoord(int16_t *xp, int16_t *yp, uint8_t *ip, uint8_t *jp) +{ + int16_t x, y; + uint8_t i, j; + + x = *xp; + y = *yp; + + x -= OFFSET_CORN_X; + x += (STEP_CORN_X/2); + i = x / STEP_CORN_X; + + y = COLOR_Y(y); + y -= OFFSET_CORN_Y; + if ((i & 1) == 1) { + j = y / STEP_CORN_Y; + } + else { + y += (STEP_CORN_Y/2); + y /= (STEP_CORN_Y*2); + j = (y * 2) + 1; + } + + if (ijcoord_to_xycoord(i, j, &x, &y) < 0) + return -1; + + if (strat_db.wp_table[i][j].type != WP_TYPE_WAYPOINT && + strat_db.wp_table[i][j].type != WP_TYPE_TOMATO) + return -1; + + *xp = x; + *yp = y; + *ip = i; + *jp = j; + + return 0; +} + + /******** CORN */ /* return the index of a corn given its i,j coords. */ @@ -192,31 +234,44 @@ int8_t corn_idx_to_xycoord(uint8_t idx, int16_t *x, int16_t *y) /* return the index of the closest corn at these coordinates. If the * corn is really too far (~20cm), return NULL. The x and y pointer are * updated with the real position of the corn */ -struct waypoint_db *xycoord_to_corn_idx(int16_t *x, int16_t *y) +struct waypoint_db *xycoord_to_corn_idx(int16_t *xp, int16_t *yp) { - uint8_t idx = -1, n; - int16_t d, x_corn, y_corn; - int16_t x_corn_min = 0, y_corn_min = 0; - int16_t d_min = 0; + int16_t x, y; + uint8_t i, j; + double d; - /* XXX does it work when we are blue ? */ - for (n = 0; n < CORN_NB; n ++) { - corn_idx_to_xycoord(n, &x_corn, &y_corn); - d = xy_norm(x_corn, y_corn, *x, *y); - if (d < CORN_MARGIN && (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) + x = *xp; + y = *yp; + + x -= OFFSET_CORN_X; + x += STEP_CORN_X; + x /= (STEP_CORN_X*2); + + y = COLOR_Y(y); + y -= OFFSET_CORN_Y; + y += STEP_CORN_Y; + if ((x & 1) == 1) + y -= STEP_CORN_Y; + y /= (STEP_CORN_Y*2); + + i = (x * 2); + j = (y * 2) + (x & 1); + + if (ijcoord_to_xycoord(i, j, &x, &y) < 0) + return NULL; + + if (strat_db.wp_table[i][j].type != WP_TYPE_CORN) + return NULL; + + d = xy_norm(*xp, *yp, x, y); + + if (d > CORN_MARGIN) return NULL; - *x = x_corn_min; - *y = y_corn_min; + *xp = x; + *yp = y; - return strat_db.corn_table[idx]; + return &strat_db.wp_table[i][j]; } /* return true if 'idx' is in group */ @@ -286,11 +341,11 @@ void corn_set_color(struct waypoint_db *wp, uint8_t color) { uint8_t symidx; + if (wp->corn.color != I2C_COB_UNKNOWN) + return; wp->corn.color = color; if (color == I2C_COB_UNKNOWN) return; - if (wp->corn.color != I2C_COB_UNKNOWN) - return; corn_deduct_other(wp->corn.idx, color); symidx = corn_get_sym_idx(wp->corn.idx); strat_db.corn_table[symidx]->corn.color = color; @@ -382,7 +437,7 @@ void strat_db_init(void) int8_t idx; int8_t i, j; - memset(&strat_db, 0, sizeof(strat_db)); + memset(&strat_db.wp_table, 0, sizeof(strat_db.wp_table)); /* corn table */ for (i=0; idangerous = 1; - if ((i&1) == 0 && j == (WAYPOINTS_NBY-1)) + if ((i & 1) == 0 && j == (WAYPOINTS_NBY-1)) wp->dangerous = 1; - /* too close of border, unreachable wp */ + /* on border, unreachable wp */ if ((i & 1) == 1 && j == (WAYPOINTS_NBY-1)) { wp->type = WP_TYPE_OBSTACLE; continue; @@ -437,8 +492,10 @@ void strat_db_init(void) /* tomato */ idx = ijcoord_to_tomato_idx(i, j); if (idx >= 0) { + printf("%d %d\n", i, j); wp->type = WP_TYPE_TOMATO; wp->present = 1; + wp->tomato.idx = idx; continue; } } diff --git a/projects/microb2010/mainboard/strat_db.h b/projects/microb2010/mainboard/strat_db.h index 1b5314f..b770b43 100644 --- a/projects/microb2010/mainboard/strat_db.h +++ b/projects/microb2010/mainboard/strat_db.h @@ -33,7 +33,7 @@ struct corn_db { }; struct tomato_db { - /* nothing for now */ + uint8_t idx; }; /* structure describing the status of a waypoint */ @@ -87,6 +87,11 @@ struct strat_db { /* global structure storing the database */ extern struct strat_db strat_db; +/* return the nearest waypoint that is not a corn: xp and yp contains + * the input and output, and ip, jp are only outputs. return 0 on + * success. */ +int8_t xycoord_to_ijcoord(int16_t *xp, int16_t *yp, uint8_t *ip, uint8_t *jp); + /* convert i,j coords to x,y coords */ int8_t ijcoord_to_xycoord(uint8_t i, uint8_t j, int16_t *x, int16_t *y); diff --git a/projects/microb2010/tests/oa/graph.py b/projects/microb2010/tests/oa/graph.py index 49d3649..48e9cc3 100644 --- a/projects/microb2010/tests/oa/graph.py +++ b/projects/microb2010/tests/oa/graph.py @@ -166,6 +166,40 @@ def build_path(ptlist): x, y = zip(*poly.vertices) return x,y +def nearest_corn(x, y): + OFFX=150 + OFFY=222 + STEPX=450 + STEPY=500 + + x -= OFFX + x += STEPX/2 + x /= STEPX + + y -= OFFY + y += STEPY/2 + if (x & 1) == 1: + y -= STEPY/2 + y /= STEPY + + i = (x * 2) + j = (y * 2) + (x & 1) + if i >= WAYPOINTS_NBX: + return None + if j >= WAYPOINTS_NBY: + return None + + if (i & 1) == 0: + y = OFFSET_CORN_Y + else: + y = OFFSET_CORN_Y + STEP_CORN_Y/2 + y += (j * STEP_CORN_Y) + + x = OFFSET_CORN_X + (i * STEP_CORN_X) + + return x, y + + def build_area(ax): # area x,y = build_poly([(0,0), (3000,0), (3000,2100), (0,2100)]) @@ -180,6 +214,8 @@ def build_area(ax): x,y = build_poly([(740,0), (740,500), (2260,500), (2260,0)]) ax.plot(x, y, 'g--') + """ + # courbes a 2 balles x,y = build_path([(375,0), (375,2050)]) ax.plot(x, y, 'r-') @@ -202,6 +238,21 @@ def build_area(ax): alpha=0.5, facecolor=(1.,0.,0.)) ax.add_collection(p) + """ + + """ + #courbes a 2 balles 50 + for xx in range(0, 3000, 100): + print xx + for yy in range(0, 2100, 100): + pouet = nearest_corn(xx, yy) + if pouet == None: + continue + xxx, yyy = pouet + x,y = build_path([(xx,yy), (xxx,yyy)]) + ax.plot(x, y, 'r-') + """ + # limit #x,y = build_poly([(250,250), (2750,250), (2750,1850), (250,1850)]) #ax.plot(x, y, 'g--')