57c20bab93ee116e44341e98b198a6db6fc51447
[aversive.git] / strat_db.c
1 /*
2  *  Copyright Droids, Microb Technology (2010)
3  *
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.
8  *
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.
13  *
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
17  *
18  *  Revision : $Id: strat.c,v 1.6 2009-11-08 17:24:33 zer0 Exp $
19  *
20  *  Olivier MATZ <zer0@droids-corp.org>
21  */
22
23
24 #include <string.h>
25 #include <stdio.h>
26 #include <stdlib.h>
27 #include <stdint.h>
28 #include <math.h>
29
30 #include <aversive.h>
31 #include <aversive/pgmspace.h>
32
33 #include <ax12.h>
34 #include <uart.h>
35 #include <pwm_ng.h>
36 #include <clock_time.h>
37 #include <spi.h>
38
39 #include <pid.h>
40 #include <quadramp.h>
41 #include <control_system_manager.h>
42 #include <trajectory_manager.h>
43 #include <trajectory_manager_utils.h>
44 #include <trajectory_manager_core.h>
45 #include <vect_base.h>
46 #include <lines.h>
47 #include <polygon.h>
48 #include <obstacle_avoidance.h>
49 #include <blocking_detection_manager.h>
50 #include <robot_system.h>
51 #include <position_manager.h>
52
53 #include <diagnostic.h>
54
55 #include <rdline.h>
56 #include <parse.h>
57
58 #include "../common/i2c_commands.h"
59 #include "i2c_protocol.h"
60 #include "main.h"
61 #include "strat.h"
62 #include "strat_base.h"
63 #include "strat_corn.h"
64 #include "strat_db.h"
65 #include "strat_utils.h"
66 #include "sensor.h"
67 #include "actuator.h"
68
69 /* status of objects on area */
70 struct strat_db strat_db;
71
72 /* given an index, give the i coord */
73 static const uint8_t corn_coord_i[CORN_NB] = {
74         0, 0, 0, 2, 2, 2, 4, 4, 6,
75         6, 8, 8, 10, 10, 10, 12, 12, 12,
76 };
77
78 /* given an index, give the j coord */
79 static const uint8_t corn_coord_j[CORN_NB] = {
80         2, 4, 6, 3, 5, 7, 4, 6, 5,
81         7, 4, 6, 3, 5, 7, 2, 4, 6,
82 };
83
84 /* table to find the symetric idx */
85 static const uint8_t corn_sym[] = {
86         15, 16, 17, 12, 13, 14, 10, 11,
87         8, 9, 6, 7, 3, 4, 5, 0, 1, 2
88 };
89
90 #if 0 /* XXX maybe useless */
91 /* the 10 possible configurations for corn on the side */
92 static const uint8_t corn_side_confs[9][2] = {
93         { 1, 4 },
94         { 0, 4 },
95         { 2, 4 },
96         { 2, 3 },
97         { 0, 3 },
98         { 1, 3 },
99         { 1, 6 },
100         { 0, 6 },
101         { 2, 6 },
102 };
103
104 /* the 4 possible configurations for corn on center */
105 static const uint8_t corn_center_confs[4][2] = {
106         { 5, 8 },
107         { 7, 8 },
108         { 5, 9 },
109         { 7, 8 },
110 };
111 #endif
112
113 /* in these groups, only one black cob */
114 static const int8_t corn_group1[] = { 0, 1, 2, -1, };
115 static const int8_t corn_group2[] = { 3, 4, 6, -1, };
116 static const int8_t corn_group3[] = { 5, 7, -1, };
117 static const int8_t corn_group4[] = { 8, 9, -1, };
118 static const int8_t corn_group5[] = { 11, 14, -1, };
119 static const int8_t corn_group6[] = { 10, 12, 13, -1, };
120 static const int8_t corn_group7[] = { 15, 16, 17, -1, };
121
122 static const int8_t *corn_groups[] = {
123         corn_group1,
124         corn_group2,
125         corn_group3,
126         corn_group4,
127         corn_group5,
128         corn_group6,
129         corn_group7,
130         NULL,
131 };
132
133 /* given an index, give the i coord */
134 static const uint8_t tomato_coord_i[TOMATO_NB] = {
135         0, 0, 2, 2, 4, 4, 6, 6,
136         8, 8, 10, 10, 12, 12,
137 };
138
139 /* given an index, give the j coord */
140 static const uint8_t tomato_coord_j[TOMATO_NB] = {
141         3, 5, 4, 6, 5, 7, 4, 6, 5, 7, 4, 6, 3, 5,
142 };
143
144 /******** Generic waypoint */
145
146 /* return the xy coords of a waypoint given its ij coords. */
147 int8_t ijcoord_to_xycoord(uint8_t i, uint8_t j, int16_t *x, int16_t *y)
148 {
149         if (i >= WAYPOINTS_NBX && j >= WAYPOINTS_NBY)
150                 return -1;
151         *x = (OFFSET_CORN_X + i*STEP_CORN_X);
152         *y = (OFFSET_CORN_Y + j*STEP_CORN_Y);
153         return 0;
154 }
155
156 /******** CORN */
157
158 /* return the index of a corn given its i,j coords. */
159 int8_t ijcoord_to_corn_idx(uint8_t i, uint8_t j)
160 {
161         uint8_t n;
162         for (n = 0; n < CORN_NB; n ++) {
163                 if (i == corn_coord_i[n] &&
164                     j == corn_coord_j[n])
165                         return n;
166         }
167         return -1;
168 }
169
170 /* return the i,j coords of a corn given its index */
171 int8_t corn_idx_to_ijcoord(uint8_t idx, uint8_t *i, uint8_t *j)
172 {
173         if (idx >= CORN_NB)
174                 return -1;
175         *i = corn_coord_i[idx];
176         *j = corn_coord_j[idx];
177         return 0;
178 }
179
180 /* return the index of a corn given its x,y coords. */
181 int8_t corn_idx_to_xycoord(uint8_t idx, int16_t *x, int16_t *y)
182 {
183         uint8_t i, j;
184         if (corn_idx_to_ijcoord(idx, &i, &j) < 0)
185                 return -1;
186         if (ijcoord_to_xycoord(i, j, x, y) < 0)
187                 return -1;
188         return 0;
189 }
190
191 /* return the index of the closest corn at these coordinates. If the
192  * corn is really too far (~20cm), return NULL. The x and y pointer are
193  * updated with the real position of the corn */
194 struct waypoint_db *xycoord_to_corn_idx(int16_t *x, int16_t *y)
195 {
196         uint8_t idx = -1, n;
197         int16_t d, x_corn, y_corn;
198         int16_t x_corn_min = 0, y_corn_min = 0;
199         int16_t d_min = 0;
200
201         /* XXX does it work when we are blue ? */
202         for (n = 0; n < CORN_NB; n ++) {
203                 corn_idx_to_xycoord(n, &x_corn, &y_corn);
204                 d = xy_norm(x_corn, y_corn, *x, *y);
205                 /* XXX 200 -> constant */
206                 if (d < 200 && (d_min == 0 || d < d_min)) {
207                         d_min = d;
208                         idx = n;
209                         x_corn_min = x_corn;
210                         y_corn_min = y_corn;
211                 }
212         }
213         if (d_min == 0)
214                 return NULL;
215
216         *x = x_corn_min;
217         *y = y_corn_min;
218
219         return strat_db.corn_table[idx];
220 }
221
222 /* return true if 'idx' is in group */
223 static uint8_t is_in_group(const int8_t *group, uint8_t idx)
224 {
225         const int8_t *pidx;
226         for (pidx = group; *pidx != -1; pidx++) {
227                 if (*pidx == idx) {
228                         return 1;
229                 }
230         }
231         return 0;
232 }
233
234 /* return the number of cob of that color in the group */
235 static uint8_t count_in_group(const int8_t *group, uint8_t color)
236 {
237         const int8_t *pidx;
238         struct waypoint_db *wp;
239         uint8_t count = 0;
240
241         for (pidx = &group[0]; *pidx != -1; pidx++) {
242                 wp = strat_db.corn_table[*pidx];
243                 if (wp->corn.color == color)
244                         count ++;
245         }
246         return count;
247 }
248
249 /* set all unkown cobs to specified color */
250 static void set_unknown_in_group(const int8_t *group, uint8_t color)
251 {
252         const int8_t *pidx;
253         struct waypoint_db *wp;
254
255         for (pidx = &group[0]; *pidx != -1; pidx++) {
256                 wp = strat_db.corn_table[*pidx];
257                 if (wp->corn.color == I2C_COB_UNKNOWN)
258                         wp->corn.color = color;
259         }
260 }
261
262 /* depending on which cob is set (and its color), set the color of
263  * other cobs */
264 static void corn_deduct_other(uint8_t idx, uint8_t color)
265 {
266         const int8_t **pgroup;
267
268         for (pgroup = &corn_groups[0]; *pgroup; pgroup++) {
269                 if (!is_in_group(*pgroup, idx))
270                         continue;
271                 if (color == I2C_COB_BLACK) {
272                         set_unknown_in_group(*pgroup, I2C_COB_WHITE);
273                 }
274                 else if (color == I2C_COB_WHITE) {
275                         if (count_in_group(*pgroup, I2C_COB_UNKNOWN) == 1)
276                                 set_unknown_in_group(*pgroup, I2C_COB_BLACK);
277                 }
278         }
279 }
280
281 /* set color of a corn
282  * type is I2C_COB_BLACK, I2C_COB_WHITE, I2C_COB_UNKNOWN
283  * it will update the symetric corn if != UNKOWN
284  * it will also deduct color of some other cobs */
285 void corn_set_color(struct waypoint_db *wp, uint8_t color)
286 {
287         uint8_t symidx;
288
289         wp->corn.color = color;
290         if (color == I2C_COB_UNKNOWN)
291                 return;
292         corn_deduct_other(wp->corn.idx, color);
293         symidx = corn_get_sym_idx(wp->corn.idx);
294         strat_db.corn_table[symidx]->corn.color = color;
295         corn_deduct_other(symidx, color);
296 }
297
298
299 /* return the idx of the symetric corn */
300 int8_t corn_get_sym_idx(int8_t i)
301 {
302         if (i >= CORN_NB)
303                 return -1;
304         return corn_sym[i];
305 }
306
307 /*********** TOMATO */
308
309 /* return the index of a tomato given its i,j coords. */
310 int8_t ijcoord_to_tomato_idx(uint8_t i, uint8_t j)
311 {
312         uint8_t n;
313         for (n = 0; n < TOMATO_NB; n ++) {
314                 if (i == tomato_coord_i[n] &&
315                     j == tomato_coord_j[n])
316                         return n;
317         }
318         return -1;
319 }
320
321 /* return the i,j coords of a tomato given its index */
322 int8_t tomato_idx_to_ijcoord(uint8_t idx, uint8_t *i, uint8_t *j)
323 {
324         if (idx >= TOMATO_NB)
325                 return -1;
326         *i = tomato_coord_i[idx];
327         *j = tomato_coord_j[idx];
328         return 0;
329 }
330
331 /* return the index of a tomato given its x,y coords. */
332 int8_t tomato_idx_to_xycoord(uint8_t idx, int16_t *x, int16_t *y)
333 {
334         uint8_t i, j;
335         if (tomato_idx_to_ijcoord(idx, &i, &j) < 0)
336                 return -1;
337         if (ijcoord_to_xycoord(i, j, x, y) < 0)
338                 return -1;
339         return 0;
340 }
341
342 /* return the index of the closest tomato at these coordinates. If the
343  * tomato is really too far (~20cm), return NULL. The x and y pointer are
344  * updated with the real position of the tomato */
345 struct waypoint_db *xycoord_to_tomato_idx(int16_t *x, int16_t *y)
346 {
347         uint8_t idx = -1, n;
348         int16_t d, x_tomato, y_tomato;
349         int16_t x_tomato_min = 0, y_tomato_min = 0;
350         int16_t d_min = 0;
351
352         /* XXX does it work when we are blue ? */
353         for (n = 0; n < TOMATO_NB; n ++) {
354                 tomato_idx_to_xycoord(n, &x_tomato, &y_tomato);
355                 d = xy_norm(x_tomato, y_tomato, *x, *y);
356                 /* XXX 200 -> constant */
357                 if (d < 200 && (d_min == 0 || d < d_min)) {
358                         d_min = d;
359                         idx = n;
360                         x_tomato_min = x_tomato;
361                         y_tomato_min = y_tomato;
362                 }
363         }
364         if (d_min == 0)
365                 return NULL;
366
367         *x = x_tomato_min;
368         *y = y_tomato_min;
369
370         return strat_db.tomato_table[idx];
371 }
372
373 /*
374  * Init internal database. The initialization is done with UNKNOWN
375  * corn with all objects present
376  */
377 void strat_db_init(void)
378 {
379         struct waypoint_db *wp;
380         int8_t idx;
381         int8_t i, j;
382
383         memset(&strat_db, 0, sizeof(strat_db));
384
385         /* corn table */
386         for (i=0; i<CORN_NB; i++) {
387                 strat_db.corn_table[i] =
388                         &strat_db.wp_table[corn_coord_i[i]][corn_coord_j[i]];
389         }
390         /* tomato table */
391         for (i=0; i<TOMATO_NB; i++) {
392                 strat_db.tomato_table[i] =
393                         &strat_db.wp_table[tomato_coord_i[i]][tomato_coord_j[i]];
394         }
395
396         strat_db.our_oranges_count = 6;
397         strat_db.opp_oranges_count = 6;
398
399         for (i=0; i<WAYPOINTS_NBX; i++) {
400
401                 for (j=0; j<WAYPOINTS_NBY; j++) {
402                         wp = &strat_db.wp_table[i][j];
403
404                         /* default type */
405                         wp->type = WP_TYPE_WAYPOINT;
406
407                         /* mark dangerous points */
408                         if (i == 0 || i == (WAYPOINTS_NBX-1))
409                                 wp->dangerous = 1;
410                         if ((i&1) == 0 && j == (WAYPOINTS_NBY-1))
411                                 wp->dangerous = 1;
412
413                         /* too close of border, unreachable wp */
414                         if ((i & 1) == 1 && j == (WAYPOINTS_NBY-1)) {
415                                 wp->type = WP_TYPE_OBSTACLE;
416                                 continue;
417                         }
418
419                         /* hill */
420                         if (i >= 2 && i < (WAYPOINTS_NBX-2) && j < 2) {
421                                 wp->type = WP_TYPE_OBSTACLE;
422                                 continue;
423                         }
424
425                         /* corn */
426                         idx = ijcoord_to_corn_idx(i, j);
427                         if (idx >= 0) {
428                                 wp->type = WP_TYPE_CORN;
429                                 wp->present = 1;
430                                 wp->corn.idx = idx;
431                                 wp->corn.color = I2C_COB_UNKNOWN;
432                                 continue;
433                         }
434
435                         /* tomato */
436                         idx = ijcoord_to_tomato_idx(i, j);
437                         if (idx >= 0) {
438                                 wp->type = WP_TYPE_TOMATO;
439                                 wp->present = 1;
440                                 continue;
441                         }
442                 }
443         }
444 }
445
446 /* dump infos about area and objects */
447 void strat_db_dump(const char *caller)
448 {
449         uint8_t i;
450         struct waypoint_db *wp;
451
452         if (strat_db.dump_enabled == 0)
453                 return;
454
455         printf_P(PSTR("DB dump from <%s>\r\n"), caller);
456         for (i=0; i<CORN_NB; i++) {
457                 wp = strat_db.corn_table[i];
458                 printf_P(PSTR("corn%d: present=%d opp=%d "),
459                          i, wp->present, wp->opp_visited);
460                 if (wp->corn.color == I2C_COB_UNKNOWN)
461                         printf_P(PSTR("unknown"));
462                 else if (wp->corn.color == I2C_COB_BLACK)
463                         printf_P(PSTR("black"));
464                 else if (wp->corn.color == I2C_COB_WHITE)
465                         printf_P(PSTR("white"));
466                 printf_P(PSTR("\r\n"));
467         }
468
469         for (i=0; i<TOMATO_NB; i++) {
470                 wp = strat_db.tomato_table[i];
471                 printf_P(PSTR("tomato%d: present=%d opp=%d\r\n"),
472                          i, wp->present, wp->opp_visited);
473         }
474 }