merge
[aversive.git] / projects / microb2009 / mainboard / strat_column_disp.c
1 /*  
2  *  Copyright Droids, Microb Technology (2009)
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_column_disp.c,v 1.5 2009-11-08 17:24:33 zer0 Exp $
19  *
20  *  Olivier MATZ <zer0@droids-corp.org> 
21  */
22
23 #include <stdio.h>
24 #include <stdlib.h>
25 #include <string.h>
26 #include <math.h>
27
28 #include <aversive/pgmspace.h>
29 #include <aversive/queue.h>
30 #include <aversive/wait.h>
31 #include <aversive/error.h>
32
33 #include <ax12.h>
34 #include <uart.h>
35 #include <pwm_ng.h>
36 #include <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 <vect_base.h>
44 #include <lines.h>
45 #include <polygon.h>
46 #include <obstacle_avoidance.h>
47 #include <blocking_detection_manager.h>
48 #include <robot_system.h>
49 #include <position_manager.h>
50
51 #include <rdline.h>
52 #include <parse.h>
53
54 #include "../common/i2c_commands.h"
55 #include "main.h"
56 #include "actuator.h"
57 #include "strat.h"
58 #include "strat_base.h"
59 #include "strat_avoid.h"
60 #include "strat_utils.h"
61 #include "sensor.h"
62 #include "i2c_protocol.h"
63
64 #define ERROUT(e) do {                          \
65                 err = e;                        \
66                 goto end;                       \
67         } while(0)
68
69 /* distance between the wheel axis and the IR sensor */
70 #define IR_SHIFT_DISTANCE_RIGHT 85
71 #define IR_SHIFT_DISTANCE_LEFT  95
72
73 /* return red or green sensor */
74 #define COLOR_IR_SENSOR(left)                                           \
75         ({                                                              \
76                 uint8_t __ret = 0;                                      \
77                 if (left)                                               \
78                         __ret = sensor_get(S_DISP_LEFT);                \
79                 else                                                    \
80                         __ret = sensor_get(S_DISP_RIGHT);               \
81                                                                         \
82                 __ret;                                                  \
83         })                                                              \
84
85 /* eject one col, some error codes are ignored here: we want to be
86  * sure that the column is correctly ejected. */
87 uint8_t strat_eject_col(int16_t eject_a, int16_t pickup_a)
88 {
89         uint8_t err;
90
91         strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
92         trajectory_d_rel(&mainboard.traj, -300);
93         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
94
95         i2c_mechboard_mode_eject();
96         time_wait_ms(600);
97         trajectory_a_abs(&mainboard.traj, eject_a);
98
99         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR);
100         i2c_mechboard_mode_clear();
101         time_wait_ms(1000);
102         trajectory_a_abs(&mainboard.traj, pickup_a);
103
104         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
105         return err;
106 }
107
108 /* get columns from dispenser. Must be called when the robot is in
109  * front of the dispenser. */
110 static uint8_t strat_pickup_col_disp(struct column_dispenser *disp)
111 {
112         uint16_t old_spdd, old_spda;
113         int16_t recalib_x, recalib_y;
114         int16_t eject_a, pickup_a;
115         uint8_t err, timeout = 0;
116         int8_t cols_count_before, cols_count_after, cols;
117         microseconds us;
118         uint8_t first_try = 1;
119         uint8_t pickup_mode = I2C_MECHBOARD_MODE_PICKUP;
120
121         /* XXX set lazy pickup mode */
122
123         DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
124
125         strat_get_speed(&old_spdd, &old_spda);
126
127         cols_count_before = get_column_count();
128         pickup_a = COLOR_A(disp->pickup_a);
129         eject_a = COLOR_A(disp->eject_a);
130         recalib_x = disp->recalib_x;
131         recalib_y = COLOR_Y(disp->recalib_y);
132
133         strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
134
135         /* turn to dispenser */
136         i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE);
137         trajectory_a_abs(&mainboard.traj, pickup_a);
138         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
139         if (!TRAJ_SUCCESS(err))
140                 ERROUT(err);
141
142         /* go forward until blocking, then go back ~30mm */
143
144         pickup_wheels_on();     
145  retry:
146         if (time_get_s() > 86) {
147                 DEBUG(E_USER_STRAT, "%s() too late...", __FUNCTION__);
148                 return END_TIMER;
149         }
150
151         if ((strat_infos.conf.flags & STRAT_CONF_BIG_3_TEMPLE) &&
152             strat_infos.col_in_boobs == 0 &&
153             strat_infos.lazy_pickup_done == 0) {
154                 DEBUG(E_USER_STRAT, "%s() mode lazy", __FUNCTION__);
155                 pickup_mode = I2C_MECHBOARD_MODE_LAZY_PICKUP;
156                 strat_infos.col_in_boobs = 1;
157                 strat_infos.lazy_pickup_done = 1;
158         }
159         else {
160                 pickup_mode = I2C_MECHBOARD_MODE_PICKUP;
161                 strat_infos.col_in_boobs = 0;
162         }
163
164         if (first_try)
165                 i2c_mechboard_mode_lazy_harvest();
166         else
167                 i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE);
168         first_try = 0;
169
170         strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
171         trajectory_d_rel(&mainboard.traj, 120);
172         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
173         strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
174
175         err = strat_calib(600, TRAJ_FLAGS_SMALL_DIST);
176
177         trajectory_d_rel(&mainboard.traj, -DIST_BACK_DISPENSER);
178         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
179         if (!TRAJ_SUCCESS(err))
180                 ERROUT(err);
181
182         if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) {
183                 strat_eject_col(eject_a, pickup_a);
184                 goto retry;
185         }
186
187         /* start to pickup with finger / arms */
188
189         DEBUG(E_USER_STRAT, "%s pickup now", __FUNCTION__);
190         
191         if (pickup_mode == I2C_MECHBOARD_MODE_PICKUP)
192                 i2c_mechboard_mode_pickup();
193         else
194                 i2c_mechboard_mode_lazy_pickup();
195         WAIT_COND_OR_TIMEOUT(get_mechboard_mode() == pickup_mode, 100);
196         us = time_get_us2();
197         cols = get_column_count();
198         while (get_mechboard_mode() == pickup_mode) {
199                 if (get_column_count() != cols) {
200                         cols = get_column_count();
201                         us = time_get_us2();
202                 }
203                 if ((get_column_count() - cols_count_before) >= disp->count) {
204                         DEBUG(E_USER_STRAT, "%s no more cols in disp", __FUNCTION__);
205                         break;
206                 }
207                 /* 1 second timeout */
208                 if (time_get_us2() - us > 1000000L) {
209                         DEBUG(E_USER_STRAT, "%s timeout", __FUNCTION__);
210                         timeout = 1;
211                         break;
212                 }
213         }
214
215         /* eject if we found a bad color column */
216         
217         if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) {
218                 strat_eject_col(eject_a, pickup_a);
219                 goto retry;
220         }
221
222         /* only recalib if it was not a timeout or if we got at least
223          * 2 cols. */
224         if (timeout == 0 || (get_column_count() - cols_count_before >= 2))
225                 strat_reset_pos(recalib_x, recalib_y, pickup_a);
226         else {
227                 /* else just update x or y depending on disp */
228                 if (disp == &strat_infos.c1)
229                         strat_reset_pos(recalib_x, DO_NOT_SET_POS,
230                                         DO_NOT_SET_POS);
231                 else
232                         strat_reset_pos(recalib_x, DO_NOT_SET_POS,
233                                         DO_NOT_SET_POS);
234         }
235
236         /* go back */
237         
238         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
239         trajectory_d_rel(&mainboard.traj, -300);
240         wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR);
241
242         /* update dispenser count */
243
244         cols_count_after = get_column_count();
245         cols = cols_count_after - cols_count_before;
246         if (cols > 0) {
247                 DEBUG(E_USER_STRAT, "%s we got %d cols", __FUNCTION__, cols);
248                 disp->count -= cols;
249                 if (disp->count < 0)
250                         disp->count = 0;
251         }
252
253         pickup_wheels_off();
254         if (pickup_mode == I2C_MECHBOARD_MODE_PICKUP)
255                 i2c_mechboard_mode_clear();
256         else
257                 disp->count -= 2;
258
259         ERROUT(END_TRAJ);
260
261 end:
262         strat_set_speed(old_spdd, old_spda);
263         return err;
264 }
265
266 /* 
267  * Go in front of a dispenser. It will update the dispenser if it is
268  * c2 or c3 if we detect that this dispenser does not exist.
269  */
270 uint8_t strat_goto_col_disp(struct column_dispenser **pdisp)
271 {
272         uint8_t err;
273         int16_t checkpoint_x, checkpoint_y;
274         int16_t scan_a;
275         uint16_t old_spdd, old_spda, scan_left;
276         int16_t pos1x, pos1y, pos2x, pos2y, pos, dist;
277         int16_t margin_col2, margin_col3;
278         struct column_dispenser *disp = *pdisp;
279
280         if (disp->count <= 0)
281                 return END_ERROR;
282
283         if (disp->last_try_time >= time_get_s())
284                 return END_ERROR;
285
286         disp->last_try_time = time_get_s();
287
288         strat_get_speed(&old_spdd, &old_spda);
289
290         i2c_mechboard_mode_prepare_pickup_next(I2C_AUTO_SIDE,
291                                                I2C_MECHBOARD_MODE_CLEAR);
292
293         /* set some useful variables */
294         checkpoint_x = disp->checkpoint_x;
295         checkpoint_y = COLOR_Y(disp->checkpoint_y);
296         scan_a = COLOR_A(disp->scan_a);
297         scan_left = COLOR_INVERT(disp->scan_left);
298
299         /* goto checkpoint */
300         DEBUG(E_USER_STRAT, "%s(): goto %s (%d,%d) scan_left=%d",
301               __FUNCTION__, disp->name, checkpoint_x,
302               checkpoint_y, scan_left);
303         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
304
305 #if 0
306         /* we have an intermediate checkpoint if we are on our
307          * side. If goto_and_avoid() returns END_ERROR, skip
308          * this checkpoint.  */
309         if (position_get_x_s16(&mainboard.pos) < 1500) {
310                 err = goto_and_avoid(1000, COLOR_Y(1500),
311                                      TRAJ_FLAGS_STD,
312                                      TRAJ_FLAGS_STD);
313                 if (!TRAJ_SUCCESS(err) && err != END_ERROR)
314                         ERROUT(err);
315         }
316 #endif
317         /* go to checkpoint near the dispenser */
318         err = goto_and_avoid(checkpoint_x, checkpoint_y,
319                              TRAJ_FLAGS_STD, TRAJ_FLAGS_NO_NEAR);
320         if (!TRAJ_SUCCESS(err))
321                 ERROUT(err);
322
323         /* turn to correct angle to prepare scanning */
324
325         trajectory_a_abs(&mainboard.traj, scan_a);
326         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
327         if (!TRAJ_SUCCESS(err))
328                 ERROUT(err);
329         
330         /* scan now */
331
332         DEBUG(E_USER_STRAT, "%s(): scanning dispenser", __FUNCTION__);
333
334         strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
335         trajectory_d_rel(&mainboard.traj, -1000);
336         err = WAIT_COND_OR_TRAJ_END(!COLOR_IR_SENSOR(scan_left),
337                                     TRAJ_FLAGS_NO_NEAR);
338         if (err) /* we should not reach end */
339                 ERROUT(END_ERROR);
340         pos1x = position_get_x_s16(&mainboard.pos);
341         pos1y = position_get_y_s16(&mainboard.pos);
342
343         err = WAIT_COND_OR_TRAJ_END(COLOR_IR_SENSOR(scan_left),
344                                     TRAJ_FLAGS_NO_NEAR);
345         if (err)
346                 ERROUT(END_ERROR);
347         pos2x = position_get_x_s16(&mainboard.pos);
348         pos2y = position_get_y_s16(&mainboard.pos);
349
350         dist = distance_between(pos1x, pos1y, pos2x, pos2y);
351         DEBUG(E_USER_STRAT, "%s(): scan done dist=%d", __FUNCTION__, dist);
352
353         if (scan_left)
354                 trajectory_d_rel(&mainboard.traj, -IR_SHIFT_DISTANCE_LEFT + dist/2);
355         else
356                 trajectory_d_rel(&mainboard.traj, -IR_SHIFT_DISTANCE_RIGHT + dist/2);
357         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
358
359         if (disp == &strat_infos.c1) 
360                 ERROUT(END_TRAJ);
361
362         /* mark c2 or c3 as empty... */
363         if (strat_infos.c2.count == 0 || strat_infos.c3.count == 0) 
364                 ERROUT(END_TRAJ);
365
366         pos = (pos2y + pos1y) / 2;
367         if (scan_a == 90) /* y is decreasing when scanning */
368                 pos -= 80;
369         else if (scan_a == -90) /* y is increasing when scanning */
370                 pos += 80;
371
372         margin_col2 = ABS(pos - COLOR_Y(strat_infos.c2.recalib_y));
373         margin_col3 = ABS(pos - COLOR_Y(strat_infos.c3.recalib_y));
374                 
375         if (margin_col3 > margin_col2) {
376                 DEBUG(E_USER_STRAT, "%s(): delete disp c3 (scan_pos=%d)", __FUNCTION__, pos);
377                 strat_infos.c3.count = 0;
378                 *pdisp = &strat_infos.c2;
379                 if (strat_infos.c3.last_try_time > strat_infos.c2.last_try_time)
380                         strat_infos.c2.last_try_time = strat_infos.c3.last_try_time;
381         }
382         else {
383                 DEBUG(E_USER_STRAT, "%s(): delete disp c2 (scan_pos=%d)", __FUNCTION__, pos);
384                 strat_infos.c2.count = 0;
385                 *pdisp = &strat_infos.c3;
386                 if (strat_infos.c2.last_try_time > strat_infos.c3.last_try_time)
387                         strat_infos.c3.last_try_time = strat_infos.c2.last_try_time;
388         }
389         ERROUT(END_TRAJ);
390
391 end:
392         strat_set_speed(old_spdd, old_spda);
393         return err;
394 }
395
396 /* return the best dispenser between the 2 */
397 static struct column_dispenser *
398 strat_disp_compare(struct column_dispenser *a,
399                    struct column_dispenser *b)
400 {
401         uint8_t want_cols = 4 - get_column_count();
402
403         DEBUG(E_USER_STRAT, "%s() want_cols=%d", __FUNCTION__, want_cols);
404
405         /* an empty dispenser is not valid */
406         if (a->count == 0)
407                 return b;
408         if (b->count == 0)
409                 return a;
410
411         /* try to do a round robin: this is not optimal, but at least
412          * we will try another dispenser when one fails. */
413         if (a->last_try_time < b->last_try_time) {
414                 return a;
415         }
416         if (b->last_try_time < a->last_try_time) {
417                 return b;
418         }
419
420         /* take the one with the most columns */
421         if (a->count >= want_cols && b->count < want_cols)
422                 return a;
423
424         /* take the one with the most columns */
425         if (b->count >= want_cols && a->count < want_cols)
426                 return b;
427
428         /* the closer is the better */
429         if (distance_from_robot(a->recalib_x, COLOR_Y(a->recalib_y)) <
430             distance_from_robot(b->recalib_x, COLOR_Y(b->recalib_y))) {
431                 return a;
432         }
433         return b;
434 }
435
436 /* choose the best dispenser */
437 static struct column_dispenser *strat_get_best_col_disp(void)
438 {
439         struct column_dispenser *disp;
440
441         DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
442
443         /* for the first call, use c3 */
444         if (strat_infos.c1.last_try_time == 0 &&
445             strat_infos.c2.last_try_time == 0 &&
446             strat_infos.c3.last_try_time == 0)
447                 return &strat_infos.c2; // XXX c3
448         
449         DEBUG(E_USER_STRAT, "%s(): compare values", __FUNCTION__);
450
451         /* else compare with standard conditions */
452         disp = strat_disp_compare(&strat_infos.c1, &strat_infos.c2);
453         disp = strat_disp_compare(disp, &strat_infos.c3);
454
455         if (disp->count == 0)
456                 return NULL;
457         
458         return disp;
459 }
460
461 /* choose the best dispenser, depending on disp count, distance,
462  * tries, ... and go pickup on it. */
463 uint8_t strat_pickup_columns(void)
464 {
465         struct column_dispenser *disp;
466         uint8_t err;
467
468         DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
469         disp = strat_get_best_col_disp();
470
471         if (disp == NULL) {
472                 DEBUG(E_USER_STRAT, "%s(): no col disp found", __FUNCTION__);
473                 return END_ERROR;
474         }
475
476         err = strat_goto_col_disp(&disp);
477         if (!TRAJ_SUCCESS(err))
478                 return err;
479
480         err = strat_pickup_col_disp(disp);
481         if (!TRAJ_SUCCESS(err))
482                 return err;
483
484         return END_TRAJ;
485 }