2 * Copyright Droids, Microb Technology (2010)
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with this program; if not, write to the Free Software
16 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 * Revision : $Id: strat.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $
20 * Olivier MATZ <zer0@droids-corp.org>
30 #include <aversive/pgmspace.h>
35 #include <clock_time.h>
40 #include <control_system_manager.h>
41 #include <trajectory_manager.h>
42 #include <trajectory_manager_utils.h>
43 #include <trajectory_manager_core.h>
44 #include <vect_base.h>
47 #include <obstacle_avoidance.h>
48 #include <blocking_detection_manager.h>
49 #include <robot_system.h>
50 #include <position_manager.h>
52 #include <diagnostic.h>
57 #include "../common/i2c_commands.h"
58 #include "i2c_protocol.h"
61 #include "strat_base.h"
62 #include "strat_corn.h"
63 #include "strat_utils.h"
70 change x,y -> i,j to avoid confusion with coords
71 could be optimized in mem space: it is not needed to store the x,y coord,
72 we can process it from idx. however it will be less optimized for speed
76 #define OFFSET_CORN_X 150
77 #define OFFSET_CORN_Y 222
78 #define STEP_CORN_X 225
79 #define STEP_CORN_Y 250
83 #define WAYPOINTS_NBX 13
84 #define WAYPOINTS_NBY 8
87 #define TYPE_WAYPOINT 0
88 #define TYPE_DANGEROUS 1
89 #define TYPE_WHITE_CORN 2
90 #define TYPE_BLACK_CORN 3
91 #define TYPE_OBSTACLE 4
92 #define TYPE_UNKNOWN 5
94 /* XXX enum possible ? else just rename */
112 struct djpoint *parent;
115 uint8_t parent_pos:3;
120 uint8_t corn_table[CORN_NB];
122 const uint8_t corn_coord_i[CORN_NB] = {
123 0, 0, 0, 2, 2, 2, 4, 4, 6,
124 6, 8, 8, 10, 10, 10, 12, 12, 12,
127 const uint8_t corn_coord_j[CORN_NB] = {
128 2, 4, 6, 3, 5, 7, 4, 6, 5,
129 7, 4, 6, 3, 5, 7, 2, 4, 6,
132 static struct djpoint djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY];
134 /* table to find the symetric idx */
135 uint8_t corn_sym[] = {
136 15, 16, 17, 12, 13, 14, 10, 11, 8, 9, 6, 7, 3, 4, 5, 0, 1, 2
139 uint8_t corn_side_confs[9][2] = {
150 uint8_t corn_center_confs[4][2] = {
158 /* return index from neigh pointer */
159 #define PT2IDX(neigh) ( ((void *)(neigh)-(void *)(&djpoints)) / sizeof(*neigh) )
167 for (i=0; i<WAYPOINTS_NBX; i++) {
168 printf_P(PSTR(" %2d "), i);
170 printf_P(PSTR("\r\n"));
172 for (j=WAYPOINTS_NBY*2-1; j>=0; j--) {
173 printf_P(PSTR("%3d "), j/2);
178 for (i=0; i<WAYPOINTS_NBX; i++) {
179 pt = &djpoints[i][j/2];
181 if (((i+j) & 1) == 0)
184 if (pt->type == TYPE_OBSTACLE)
185 printf_P(PSTR(" X "));
186 else if (pt->type == TYPE_DANGEROUS)
187 printf_P(PSTR(" D "));
188 else if (pt->type == TYPE_WHITE_CORN)
189 printf_P(PSTR(" W "));
190 else if (pt->type == TYPE_BLACK_CORN)
191 printf_P(PSTR(" B "));
192 else if (pt->type == TYPE_WAYPOINT)
193 printf_P(PSTR(" %5d "), pt->weight);
195 printf_P(PSTR(" ? "));
197 printf_P(PSTR("\r\n"));
201 static inline uint8_t opposite_position(uint8_t pos)
209 /* return coord of the entry in the table from the pointer */
210 static void djpoint2ij(struct djpoint *pt, int8_t *x, int8_t *y)
212 int8_t idx = PT2IDX(pt);
213 *x = idx / WAYPOINTS_NBY;
214 *y = idx % WAYPOINTS_NBY;
217 /* get the neighbour of the point at specified position */
218 static struct djpoint *get_neigh(struct djpoint *pt,
222 struct djpoint *neigh;
224 djpoint2ij(pt, &i, &j);
252 if (i < 0 || j < 0 || i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY)
255 neigh = &djpoints[i][j];
257 if (neigh->type != TYPE_WAYPOINT)
263 static struct djpoint *get_next_neigh(struct djpoint *pt, uint8_t *position)
265 struct djpoint *neigh = NULL;
266 uint8_t pos = *position + 1;
268 for (pos = *position + 1; pos < END; pos++) {
269 neigh = get_neigh(pt, pos);
278 /* browse all points */
279 #define POINT_FOREACH(cur) \
280 for (cur = &djpoints[0][0]; \
281 cur < &djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY]; \
285 #define NEIGH_FOREACH(neigh, pos, point) \
286 for (pos = START, neigh = get_next_neigh(point, &pos); \
288 neigh = get_next_neigh(point, &pos))
290 int dijkstra_init(void)
295 static uint16_t dist(struct djpoint *p1, struct djpoint *p2)
298 vx = p2->pos.x - p1->pos.x;
299 vy = p2->pos.y - p1->pos.y;
300 return sqrt(vx * vx + vy * vy);
303 void dijkstra_process_neighs(struct djpoint *pt)
305 struct djpoint *neigh;
306 uint8_t pos, parent_pos;
310 djpoint2ij(pt, &i, &j);
311 printf_P(PSTR("at %d %d:\r\n"), i, j);
313 NEIGH_FOREACH(neigh, pos, pt) {
314 weight = pt->weight + dist(pt, neigh);
315 parent_pos = opposite_position(pos);
317 /* bonus if we keep the same direction */
318 if (parent_pos == pt->parent_pos ||
319 pt->parent_pos == END)
322 printf_P(PSTR(" pos=%d: ppos=%d opos=%d nw=%d w=%d\r\n"), pos,
323 pt->parent_pos, parent_pos,
324 neigh->weight, weight);
325 if (neigh->weight == 0 || weight < neigh->weight) {
326 djpoint2ij(neigh, &i, &j);
327 //printf_P(PSTR(" %d,%d updated\r\n"), i, j);
328 neigh->weight = weight;
329 neigh->parent_pos = parent_pos;
335 int dijkstra(struct djpoint *start)
338 uint8_t todolist = 1;
343 printf_P(PSTR("\r\n"));
345 /* process all neighbours of todo list */
349 dijkstra_process_neighs(cur);
353 /* convert updated list in todo list */
366 int8_t ijcoord_to_corn_idx(uint8_t i, uint8_t j)
369 for (n = 0; n < CORN_NB; n ++) {
370 if (i == corn_coord_i[n] &&
371 j == corn_coord_j[n])
377 int8_t corn_idx_to_coordij(uint8_t idx, uint8_t *i, uint8_t *j)
381 *i = corn_coord_i[idx];
382 *j = corn_coord_j[idx];
386 /* return the index of the closest corn at these coordinates. If the
387 * corn is really too far (~20cm), return -1. The x and y pointer are
388 * updated with the real position of the corn */
389 int8_t xycoord_to_corn_idx(int16_t *x, int16_t *y)
391 uint8_t idx = -1, n, i, j;
392 int16_t d, x_corn, y_corn;
393 int16_t x_corn_min = 0, y_corn_min = 0;
396 for (n = 0; n < CORN_NB; n ++) {
397 corn_idx_to_coordij(n, &i, &j);
398 x_corn = (OFFSET_CORN_X + i*STEP_CORN_X);
399 y_corn = (OFFSET_CORN_Y + j*STEP_CORN_Y);
400 d = xy_norm(x_corn, y_corn, *x, *y);
401 if (d < 200 && (d_min == 0 || d < d_min)) {
417 int8_t corn_get_sym(int8_t i)
424 void init_corn_table(int8_t conf_side, int8_t conf_center)
429 if (conf_side == -1) {
430 for (i=0; i<CORN_NB; i++)
431 corn_table[i] = TYPE_UNKNOWN;
436 printf_P(PSTR("confs = %d, %d\r\n"), conf_side, conf_center);
437 for (i=0; i<CORN_NB; i++) {
438 if (i == corn_side_confs[conf_side][0] ||
439 i == corn_side_confs[conf_side][1]) {
440 corn_table[i] = TYPE_BLACK_CORN;
443 sym = corn_get_sym(i);
444 if (sym == corn_side_confs[conf_side][0] ||
445 sym == corn_side_confs[conf_side][1]) {
446 corn_table[i] = TYPE_BLACK_CORN;
449 if (i == corn_center_confs[conf_center][0] ||
450 i == corn_center_confs[conf_center][1]) {
451 corn_table[i] = TYPE_BLACK_CORN;
454 sym = corn_get_sym(i);
455 if (sym == corn_center_confs[conf_center][0] ||
456 sym == corn_center_confs[conf_center][1]) {
457 corn_table[i] = TYPE_BLACK_CORN;
460 corn_table[i] = TYPE_WHITE_CORN;
464 /* init waypoints position */
465 void init_waypoints(void)
472 for (i=0; i<WAYPOINTS_NBX; i++) {
475 y = OFFSET_CORN_Y - STEP_CORN_Y/2;
479 for (j=0; j<WAYPOINTS_NBY; j++) {
480 pt = &djpoints[i][j];
485 pt->type = TYPE_WAYPOINT;
486 pt->parent_pos = END;
497 /* update the type and weight of waypoints, before starting
499 void update_waypoints(void)
504 for (i=0; i<WAYPOINTS_NBX; i++) {
506 for (j=0; j<WAYPOINTS_NBY; j++) {
507 pt = &djpoints[i][j];
512 c = ijcoord_to_corn_idx(i, j);
514 pt->type = corn_table[c];
517 /* too close of border */
518 if ((i & 1) == 1 && j == (WAYPOINTS_NBY-1)) {
519 pt->type = TYPE_OBSTACLE;
523 if (i >= 2 && i < (WAYPOINTS_NBX-2) && j < 2) {
524 pt->type = TYPE_OBSTACLE;
527 /* dangerous points */
528 if (i == 0 || i == (WAYPOINTS_NBX-1)) {
529 pt->type = TYPE_DANGEROUS;
532 if ( (i&1) == 0 && j == (WAYPOINTS_NBY-1)) {
533 pt->type = TYPE_DANGEROUS;
536 pt->type = TYPE_WAYPOINT;
541 int get_path(struct djpoint *start, struct djpoint *end)
544 uint8_t prev_direction = 0;
547 cur != NULL && cur->parent_pos != END && cur != end;
548 cur = get_neigh(cur, cur->parent_pos)) {
549 if (prev_direction != cur->parent_pos) {
550 printf_P(PSTR("%d %d (%d)\r\n"),
551 cur->pos.x, cur->pos.y,
554 prev_direction = cur->parent_pos;
556 printf_P(PSTR("%d %d\r\n"), end->pos.x, end->pos.y);
561 /* return 1 if there is a corn near, and fill the index ptr */
562 static uint8_t corn_is_near(int8_t *corn_idx, uint8_t side)
564 #define SENSOR_CORN_DIST 225
565 #define SENSOR_CORN_ANGLE 90
566 double x = position_get_x_double(&mainboard.pos);
567 double y = position_get_y_double(&mainboard.pos);
568 double a_rad = position_get_a_rad_double(&mainboard.pos);
569 double x_corn, y_corn;
570 int16_t x_corn_int, y_corn_int;
572 if (side == I2C_LEFT_SIDE) {
573 x_corn = x + cos(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
574 y_corn = y + sin(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
577 x_corn = x + cos(a_rad + RAD(-SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
578 y_corn = y + sin(a_rad + RAD(-SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
583 *corn_idx = xycoord_to_corn_idx(&x_corn_int, &y_corn_int);
590 * - send the correct commands to the spickles
591 * - return 1 if we need to stop (cobboard is stucked)
593 static uint8_t handle_spickles(void)
599 if (!corn_is_near(&corn_idx, I2C_LEFT_SIDE))
600 i2c_cobboard_mode_deploy(I2C_LEFT_SIDE);
602 if (corn_table[corn_idx] == TYPE_WHITE_CORN)
603 i2c_cobboard_mode_harvest(I2C_LEFT_SIDE);
605 i2c_cobboard_mode_pack(I2C_LEFT_SIDE);
607 /* printf("%d %d\n", corn_idx, corn_table[corn_idx]); */
608 /* time_wait_ms(100); */
610 if (!corn_is_near(&corn_idx, I2C_RIGHT_SIDE))
611 i2c_cobboard_mode_deploy(I2C_RIGHT_SIDE);
613 if (corn_table[corn_idx] == TYPE_WHITE_CORN)
614 i2c_cobboard_mode_harvest(I2C_RIGHT_SIDE);
616 i2c_cobboard_mode_pack(I2C_RIGHT_SIDE);
622 uint8_t line2line(uint8_t dir1, uint8_t num1,
623 uint8_t dir2, uint8_t num2)
625 double line1_a_rad, line1_a_deg, line2_a_rad;
626 double diff_a_deg, diff_a_deg_abs, beta_deg;
628 struct line_2pts l1, l2;
633 /* convert to 2 points */
634 num2line(&l1, dir1, num1);
635 num2line(&l2, dir2, num2);
637 printf_P(PSTR("A2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"),
638 l1.p1.x, l1.p1.y, l1.p2.x, l1.p2.y);
639 printf_P(PSTR("B2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"),
640 l2.p1.x, l2.p1.y, l2.p2.x, l2.p2.y);
642 /* convert to line eq and find intersection */
643 pts2line(&l1.p1, &l1.p2, &ll1);
644 pts2line(&l2.p1, &l2.p2, &ll2);
645 intersect_line(&ll1, &ll2, &p);
647 line1_a_rad = atan2(l1.p2.y - l1.p1.y,
649 line1_a_deg = DEG(line1_a_rad);
650 line2_a_rad = atan2(l2.p2.y - l2.p1.y,
652 diff_a_deg = DEG(line2_a_rad - line1_a_rad);
653 diff_a_deg_abs = fabs(diff_a_deg);
655 if (diff_a_deg_abs < 70.) {
662 else if (diff_a_deg_abs < 100.) {
677 trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y,
678 line1_a_deg, 150., diff_a_deg, beta_deg,
679 radius, xy_norm(l1.p1.x, l1.p1.y,
683 err = WAIT_COND_OR_TRAJ_END(handle_spickles(), 0xFF);
685 /* cobboard is stucked */
686 trajectory_hardstop(&mainboard.traj);
687 return err; /* XXX do something */
689 err = test_traj_end(0xFF);
694 void num2line(struct line_2pts *l, uint8_t dir, uint8_t num)
701 l->p1.x = n * 450 + 375;
702 l->p1.y = COLOR_Y(0);
703 l->p2.x = n * 450 + 375;
704 l->p2.y = COLOR_Y(2100);
707 l->p1.x = n * 450 + 375;
708 l->p1.y = COLOR_Y(2100);
709 l->p2.x = n * 450 + 375;
710 l->p2.y = COLOR_Y(0);
714 l->p1.y = COLOR_Y(-n * 500 + 1472);
716 l->p2.y = COLOR_Y((-n + 4) * 500 + 972);
720 l->p1.y = COLOR_Y((-n + 4) * 500 + 972);
722 l->p2.y = COLOR_Y(-n * 500 + 1472);
726 l->p1.y = COLOR_Y(-n * 500 + 1472);
728 l->p2.y = COLOR_Y((-n + 4) * 500 + 972);
732 l->p1.y = COLOR_Y((-n + 4) * 500 + 972);
734 l->p2.y = COLOR_Y(-n * 500 + 1472);
745 struct djpoint *start;
748 start = &djpoints[1][1];
749 end = &djpoints[12][1];
751 init_corn_table(0, 0);
758 get_path(start, end);