merge
[aversive.git] / projects / microb2009 / mainboard / strat_lintel.c
1 /*  
2  *  Copyright Droids, Microb Technology (2008)
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_lintel.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 "strat.h"
57 #include "strat_base.h"
58 #include "strat_avoid.h"
59 #include "strat_utils.h"
60 #include "sensor.h"
61 #include "i2c_protocol.h"
62
63 #define ERROUT(e) do {                          \
64                 err = e;                        \
65                 goto end;                       \
66         } while(0)
67
68 #define X_PRE_MARGIN 20
69 #define X_POST_MARGIN 10
70
71 /*
72  * goto lintel disp. Return END_TRAJ if success or if there is nothing
73  * to do. Return END_ERROR if dest cannot be reached, else, it may
74  * return END_OBSTACLE or END_BLOCKING.
75  */
76 uint8_t strat_goto_lintel_disp(struct lintel_dispenser *disp)
77 {
78         uint8_t err, first_try = 1, right_ok, left_ok;
79         uint16_t old_spdd, old_spda;
80         int16_t left_cur, right_cur, a;
81         
82         if (disp->count == 0)
83                 return END_ERROR;
84
85         if (get_lintel_count() == 2)
86                 return END_ERROR;
87
88         if (disp->last_try_time >= time_get_s())
89                 return END_ERROR;
90
91         disp->last_try_time = time_get_s();
92         
93         strat_get_speed(&old_spdd, &old_spda);
94         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
95
96         DEBUG(E_USER_STRAT, "%s(): goto %s", __FUNCTION__, disp->name);
97         i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE);
98
99         err = goto_and_avoid_backward(disp->x, COLOR_Y(400),
100                                       TRAJ_FLAGS_STD, TRAJ_FLAGS_NO_NEAR);
101         if (!TRAJ_SUCCESS(err))
102                 ERROUT(err);
103         
104         trajectory_a_abs(&mainboard.traj, COLOR_A(-90));
105         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
106         if (!TRAJ_SUCCESS(err))
107                 ERROUT(err);
108         
109         if (time_get_s() > 86) {
110                 DEBUG(E_USER_STRAT, "%s() too late...", __FUNCTION__);
111                 return END_TIMER;
112         }
113
114         i2c_mechboard_mode_prepare_get_lintel();
115  retry:
116         strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
117         err = strat_calib(500, TRAJ_FLAGS_SMALL_DIST);
118         if (err == END_BLOCKING) {
119                 a = position_get_a_deg_s16(&mainboard.pos);
120                 /* only reset pos if angle is not too different */
121                 if (ABS(a - COLOR_A(-90)) < 5)
122                         strat_reset_pos(DO_NOT_SET_POS,
123                                         COLOR_Y(ROBOT_LENGTH/2),
124                                         COLOR_A(-90));
125         }
126         else if (!TRAJ_SUCCESS(err))
127                 ERROUT(err);
128         
129         i2c_mechboard_mode_get_lintel();
130         time_wait_ms(500);
131
132         left_cur = sensor_get_adc(ADC_CSENSE3);
133         left_ok = (left_cur > I2C_MECHBOARD_CURRENT_COLUMN);
134         right_cur = mechboard.pump_right1_current;
135         right_ok = (right_cur > I2C_MECHBOARD_CURRENT_COLUMN);
136
137         DEBUG(E_USER_STRAT, "%s left_ok=%d (%d), right_ok=%d (%d)", __FUNCTION__,
138               left_ok, left_cur, right_ok, right_cur);
139         if (first_try) {
140                 if (!right_ok && !left_ok) {
141                         i2c_mechboard_mode_prepare_get_lintel();
142                         time_wait_ms(300);
143                 }
144                 /* XXX recalib x ? */
145                 else if (right_ok && !left_ok) {
146                         i2c_mechboard_mode_prepare_get_lintel();
147                         time_wait_ms(300);
148                         strat_set_speed(500, 500);
149                         trajectory_d_a_rel(&mainboard.traj, -200, 30);
150                         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
151                         trajectory_d_a_rel(&mainboard.traj, 190, -30);
152                         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
153                         first_try = 0;
154                         goto retry;
155                 }
156                 else if (!right_ok && left_ok) {
157                         i2c_mechboard_mode_prepare_get_lintel();
158                         time_wait_ms(300);
159                         strat_set_speed(500, 500);
160                         trajectory_d_a_rel(&mainboard.traj, -200, -30);
161                         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
162                         trajectory_d_a_rel(&mainboard.traj, 190, 30);
163                         err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
164                         first_try = 0;
165                         goto retry;
166                 }
167                 /* else, lintel is ok */
168                 else {
169                         strat_infos.taken_lintel ++;
170                         i2c_mechboard_mode_put_lintel();
171                 }
172         }
173         else {
174                 if (right_ok && left_ok) {
175                         /* lintel is ok */
176                         strat_infos.taken_lintel ++;
177                         i2c_mechboard_mode_put_lintel();
178                 }
179                 else {
180                         i2c_mechboard_mode_prepare_get_lintel();
181                         time_wait_ms(300);
182                 }
183         }
184         disp->count--;
185
186         strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
187         trajectory_d_rel(&mainboard.traj, -250);
188         err = wait_traj_end(TRAJ_FLAGS_STD);
189         
190         ERROUT(err);
191
192 end:
193         strat_set_speed(old_spdd, old_spda);
194         return err;
195 }
196
197 /* go pickup lintels on dispensers. Return END_TRAJ on success or if
198
199  * there is nothing to do, else return the error status. */
200 uint8_t strat_pickup_lintels(void)
201 {
202         uint8_t err;
203
204         if (get_column_count() != 0)
205                 return END_ERROR;
206
207         if (strat_infos.l1.count == 0 && strat_infos.l2.count == 0)
208                 return END_TRAJ;
209
210         /* skip if it's too early */
211         if (time_get_s() < strat_infos.conf.lintel_min_time)
212                 return END_TRAJ;
213
214         /* skip next lintel if we want only one */
215         if (strat_infos.conf.flags & STRAT_CONF_TAKE_ONE_LINTEL) {
216                 if (strat_infos.taken_lintel)
217                         return END_TRAJ;
218         }
219         
220         /* don't take lintel now if we already have one and if there
221          * is not much time */
222         if (get_lintel_count() && time_get_s() > 75)
223                 return END_TRAJ;
224
225         /* take lintel 1 */
226         err = strat_goto_lintel_disp(&strat_infos.l1);
227         if (!TRAJ_SUCCESS(err) && err != END_ERROR)
228                 return err;
229
230         /* skip next lintel if we want only one */
231         if (strat_infos.conf.flags & STRAT_CONF_TAKE_ONE_LINTEL) {
232                 if (strat_infos.taken_lintel)
233                         return END_TRAJ;
234         }
235         
236         /* don't take lintel now if we already have one and if there
237          * is not much time */
238         if (get_lintel_count() && time_get_s() > 75)
239                 return END_TRAJ;
240
241         /* take lintel 2 */
242         err = strat_goto_lintel_disp(&strat_infos.l2);
243         if (!TRAJ_SUCCESS(err) && err != END_ERROR)
244                 return err;
245
246         return END_TRAJ;
247 }