20100416
[aversive.git] / projects / microb2010 / cobboard / state.c
1 /*
2  *  Copyright Droids Corporation (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: state.c,v 1.5 2009-11-08 17:25:00 zer0 Exp $
19  *
20  */
21
22 #include <math.h>
23 #include <string.h>
24
25 #include <aversive.h>
26 #include <aversive/wait.h>
27 #include <aversive/error.h>
28
29 #include <ax12.h>
30 #include <uart.h>
31 #include <spi.h>
32 #include <encoders_spi.h>
33 #include <pwm_ng.h>
34 #include <timer.h>
35 #include <scheduler.h>
36 #include <clock_time.h>
37
38 #include <pid.h>
39 #include <quadramp.h>
40 #include <control_system_manager.h>
41 #include <blocking_detection_manager.h>
42
43 #include <rdline.h>
44 #include <vt100.h>
45
46 #include "../common/i2c_commands.h"
47 #include "main.h"
48 #include "cmdline.h"
49 #include "sensor.h"
50 #include "actuator.h"
51 #include "spickle.h"
52 #include "shovel.h"
53 #include "state.h"
54
55 #define STMCH_DEBUG(args...) DEBUG(E_USER_ST_MACH, args)
56 #define STMCH_NOTICE(args...) NOTICE(E_USER_ST_MACH, args)
57 #define STMCH_ERROR(args...) ERROR(E_USER_ST_MACH, args)
58
59 static struct vt100 local_vt100;
60 static volatile uint8_t state_mode;
61 static volatile uint8_t state_status;
62 static uint8_t cob_count;
63
64 /* short aliases */
65 #define L_DEPLOY(mode)   (!!((mode) & I2C_COBBOARD_MODE_L_DEPLOY))
66 #define R_DEPLOY(mode)   (!!((mode) & I2C_COBBOARD_MODE_R_DEPLOY))
67 #define DEPLOY(side, mode) ((side) == I2C_LEFT_SIDE ? L_DEPLOY(mode) : R_DEPLOY(mode))
68 #define L_HARVEST(mode)  (!!((mode) & I2C_COBBOARD_MODE_L_HARVEST))
69 #define R_HARVEST(mode)  (!!((mode) & I2C_COBBOARD_MODE_R_HARVEST))
70 #define HARVEST(side, mode) ((side) == I2C_LEFT_SIDE ? L_HARVEST(mode) : R_HARVEST(mode))
71 #define EJECT(mode)      (!!((mode) & I2C_COBBOARD_MODE_EJECT))
72 #define INIT(mode)       (!!((mode) & I2C_COBBOARD_MODE_INIT))
73
74 uint8_t state_debug = 0;
75
76 uint8_t state_get_cob_count(void)
77 {
78         return cob_count;
79 }
80
81 static void state_debug_wait_key_pressed(void)
82 {
83         if (!state_debug)
84                 return;
85         printf_P(PSTR("press a key\r\n"));
86         while(!cmdline_keypressed());
87 }
88
89 /* return true if the cob is correctly inside */
90 static uint8_t state_cob_inside(void)
91 {
92         return sensor_get(S_COB_INSIDE_L) &&
93                 sensor_get(S_COB_INSIDE_R);
94 }
95
96 /* return true if there is no cob correctly inside */
97 static uint8_t state_no_cob_inside(void)
98 {
99         return !sensor_get(S_COB_INSIDE_L) &&
100                 !sensor_get(S_COB_INSIDE_R);
101 }
102
103 /* pack/deploy spickles depending on mode */
104 static void spickle_prepare(uint8_t side)
105 {
106         if (cob_count >= 5)
107                 spickle_pack(side);
108         else if (DEPLOY(side, state_mode) && !HARVEST(side, state_mode))
109                 spickle_mid(side);
110         else if (DEPLOY(side, state_mode) && HARVEST(side, state_mode))
111                 spickle_deploy(side);
112         else
113                 spickle_pack(side);
114 }
115
116 /* set a new state, return 0 on success */
117 int8_t state_set_mode(uint8_t mode)
118 {
119         state_mode = mode;
120
121         /* preempt current action if not busy */
122         if (state_status != I2C_COBBOARD_STATUS_LBUSY)
123                 spickle_prepare(I2C_LEFT_SIDE);
124         if (state_status != I2C_COBBOARD_STATUS_RBUSY)
125                 spickle_prepare(I2C_RIGHT_SIDE);
126
127 /*      STMCH_DEBUG("%s(): l_deploy=%d l_harvest=%d " */
128 /*                  "r_deploy=%d r_harvest=%d eject=%d", */
129 /*                  __FUNCTION__, L_DEPLOY(mode), L_HARVEST(mode), */
130 /*                  R_DEPLOY(mode), R_HARVEST(mode), EJECT(mode)); */
131
132         return 0;
133 }
134
135 /* check that state is the one in parameter and that state did not
136  * changed */
137 static uint8_t state_want_exit(void)
138 {
139         int16_t c;
140
141         /* force quit when CTRL-C is typed */
142         c = cmdline_getchar();
143         if (c == -1)
144                 return 0;
145         if (vt100_parser(&local_vt100, c) == KEY_CTRL_C)
146                 return 1;
147         printf_P(PSTR("CTRL-C\r\n"));
148         return 0;
149 }
150
151 uint8_t state_get_mode(void)
152 {
153         return state_mode;
154 }
155
156 uint8_t state_get_status(void)
157 {
158         return state_status;
159 }
160
161 /* harvest cobs from area */
162 static void state_do_harvest(uint8_t side)
163 {
164         if (side == I2C_LEFT_SIDE)
165                 state_status = I2C_COBBOARD_STATUS_LBUSY;
166         else
167                 state_status = I2C_COBBOARD_STATUS_RBUSY;
168
169         /* if there is no cob, return */
170         if (cob_falling_edge(side) == 0)
171                 return;
172
173         STMCH_DEBUG("start");
174
175         /* eat the cob */
176         spickle_pack(side);
177
178         time_wait_ms(250);
179         cobroller_on(side);
180
181         /* check that cob is correctly in place */
182         if (WAIT_COND_OR_TIMEOUT(state_cob_inside(), 750) == 0) {
183                 if (state_no_cob_inside()) {
184                         STMCH_DEBUG("no cob");
185                         return;
186                 }
187                 STMCH_DEBUG("bad cob state");
188
189                 /* while cob is not correctly placed try to extract
190                  * it as much as we can */
191                 while (state_cob_inside() == 0 &&
192                        state_no_cob_inside() == 0) {
193                         uint8_t cpt = 0;
194                         if (cpt == 0)
195                                 cobroller_reverse(side);
196                         else if (cpt == 1)
197                                 cobroller_off(side);
198                         else if (cpt == 2)
199                                 cobroller_on(side);
200                         cpt ++;
201                         cpt %= 3;
202                         shovel_mid();
203                         time_wait_ms(250);
204                         shovel_down();
205                         time_wait_ms(250);
206                 }
207
208                 STMCH_DEBUG("cob removed");
209                 if (state_no_cob_inside()) {
210                         STMCH_DEBUG("no cob");
211                         return;
212                 }
213         }
214
215         /* cob is inside, switch off roller */
216         cobroller_off(side);
217         cob_count ++;
218
219         /* last cob, nothing to do */
220         if (cob_count == 5)
221                 return;
222
223         /* redeploy the spickle */
224         spickle_deploy(side);
225         state_debug_wait_key_pressed();
226
227         /* let the loaded cobs go */
228         servo_door_block();
229         servo_carry_open();
230         time_wait_ms(100);
231         state_debug_wait_key_pressed();
232
233         /* store it */
234         shovel_up();
235
236         while (WAIT_COND_OR_TIMEOUT(shovel_is_up(), 400) == 0) {
237                 STMCH_DEBUG("shovel blocked");
238                 shovel_down();
239                 time_wait_ms(250);
240                 shovel_up();
241         }
242
243         state_debug_wait_key_pressed();
244
245         /* close the carry servos */
246         servo_carry_close();
247         servo_door_close();
248         time_wait_ms(200);
249         state_debug_wait_key_pressed();
250
251         shovel_down();
252
253         while (WAIT_COND_OR_TIMEOUT(shovel_is_down(), 400) == 0) {
254                 STMCH_DEBUG("shovel blocked");
255                 shovel_up();
256                 time_wait_ms(250);
257                 shovel_down();
258         }
259
260         STMCH_DEBUG("end");
261 }
262
263 /* eject cobs */
264 static void state_do_eject(void)
265 {
266         state_status = I2C_COBBOARD_STATUS_EJECT;
267         cob_count = 0;
268         shovel_mid();
269         servo_carry_open();
270         servo_door_open();
271         time_wait_ms(2000);
272         shovel_down();
273         servo_door_close();
274         servo_carry_close();
275 }
276
277 /* main state machine */
278 void state_machine(void)
279 {
280         while (state_want_exit() == 0) {
281
282                 state_status = I2C_COBBOARD_STATUS_READY;
283
284                 /* init */
285                 if (INIT(state_mode)) {
286                         state_mode &= (~I2C_COBBOARD_MODE_INIT);
287                         state_init();
288                 }
289
290                 /* pack/deply spickles, enable/disable roller */
291                 cobroller_off(I2C_LEFT_SIDE);
292                 cobroller_off(I2C_RIGHT_SIDE);
293                 spickle_prepare(I2C_LEFT_SIDE);
294                 spickle_prepare(I2C_RIGHT_SIDE);
295
296                 /* harvest */
297                 if (cob_count < 5) {
298                         if (L_DEPLOY(state_mode) && L_HARVEST(state_mode))
299                                 state_do_harvest(I2C_LEFT_SIDE);
300                         if (R_DEPLOY(state_mode) && R_HARVEST(state_mode))
301                                 state_do_harvest(I2C_RIGHT_SIDE);
302                 }
303
304                 /* eject */
305                 if (EJECT(state_mode)) {
306                         state_mode &= (~I2C_COBBOARD_MODE_EJECT);
307                         state_do_eject();
308                 }
309         }
310         state_status = I2C_COBBOARD_STATUS_READY;
311 }
312
313 void state_init(void)
314 {
315         vt100_init(&local_vt100);
316         shovel_down();
317         servo_door_close();
318         servo_carry_close();
319         spickle_pack(I2C_LEFT_SIDE);
320         spickle_pack(I2C_RIGHT_SIDE);
321         state_mode = 0;
322         cob_count = 0;
323         state_status = I2C_COBBOARD_STATUS_READY;
324 }