high level strat
[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, lspickle, rspickle;
61 static volatile uint8_t state_status;
62 static uint8_t cob_count;
63
64 /* short aliases */
65 #define HARVEST(mode)    (!!((mode) & I2C_COBBOARD_MODE_HARVEST))
66 #define EJECT(mode)      (!!((mode) & I2C_COBBOARD_MODE_EJECT))
67 #define INIT(mode)       (!!((mode) & I2C_COBBOARD_MODE_INIT))
68
69 uint8_t state_debug = 0;
70
71 uint8_t state_get_cob_count(void)
72 {
73         return cob_count;
74 }
75
76 static void state_debug_wait_key_pressed(void)
77 {
78         if (!state_debug)
79                 return;
80         printf_P(PSTR("press a key\r\n"));
81         while(!cmdline_keypressed());
82 }
83
84 /* return true if the cob is correctly inside */
85 static uint8_t state_cob_inside(void)
86 {
87         return sensor_get(S_COB_INSIDE_L) &&
88                 sensor_get(S_COB_INSIDE_R);
89 }
90
91 /* return true if there is no cob correctly inside */
92 static uint8_t state_no_cob_inside(void)
93 {
94         return !sensor_get(S_COB_INSIDE_L) &&
95                 !sensor_get(S_COB_INSIDE_R);
96 }
97
98 static uint8_t is_deployed(uint8_t side)
99 {
100         if (side == I2C_LEFT_SIDE)
101                 return lspickle & I2C_COBBOARD_SPK_DEPLOY;
102         else
103                 return rspickle & I2C_COBBOARD_SPK_DEPLOY;
104 }
105
106 static uint8_t is_autoharvest(uint8_t side)
107 {
108         if (side == I2C_LEFT_SIDE)
109                 return lspickle & I2C_COBBOARD_SPK_AUTOHARVEST;
110         else
111                 return rspickle & I2C_COBBOARD_SPK_AUTOHARVEST;
112 }
113
114 /* pack/deploy spickles depending on mode */
115 static void spickle_prepare(uint8_t side)
116 {
117         if (cob_count >= 5)
118                 spickle_pack(side);
119         else if (is_deployed(side) && !is_autoharvest(side))
120                 spickle_mid(side);
121         else if (is_deployed(side) && is_autoharvest(side))
122                 spickle_deploy(side);
123         else
124                 spickle_pack(side);
125 }
126
127 void state_set_spickle(uint8_t side, uint8_t flags)
128 {
129         if (side == I2C_LEFT_SIDE) {
130                 /* preempt current action if not busy */
131                 if (lspickle != 0 && flags == 0 &&
132                     state_status != I2C_COBBOARD_STATUS_LBUSY)
133                         spickle_prepare(I2C_LEFT_SIDE);
134                 lspickle = flags;
135         }
136         else {
137                 /* preempt current action if not busy */
138                 if (rspickle != 0 && flags == 0 &&
139                     state_status != I2C_COBBOARD_STATUS_RBUSY)
140                         spickle_prepare(I2C_RIGHT_SIDE);
141                 rspickle = flags;
142         }
143 }
144
145 /* set a new state, return 0 on success */
146 int8_t state_set_mode(uint8_t mode)
147 {
148         state_mode = mode;
149
150 /*      STMCH_DEBUG("%s(): l_deploy=%d l_harvest=%d " */
151 /*                  "r_deploy=%d r_harvest=%d eject=%d", */
152 /*                  __FUNCTION__, L_DEPLOY(mode), L_HARVEST(mode), */
153 /*                  R_DEPLOY(mode), R_HARVEST(mode), EJECT(mode)); */
154
155         return 0;
156 }
157
158 /* check that state is the one in parameter and that state did not
159  * changed */
160 static uint8_t state_want_exit(void)
161 {
162         int16_t c;
163
164         /* force quit when CTRL-C is typed */
165         c = cmdline_getchar();
166         if (c == -1)
167                 return 0;
168         if (vt100_parser(&local_vt100, c) == KEY_CTRL_C)
169                 return 1;
170         printf_P(PSTR("CTRL-C\r\n"));
171         return 0;
172 }
173
174 uint8_t state_get_mode(void)
175 {
176         return state_mode;
177 }
178
179 uint8_t state_get_status(void)
180 {
181         return state_status;
182 }
183
184 /* harvest cobs from area */
185 static void state_do_harvest(uint8_t side)
186 {
187         if (side == I2C_LEFT_SIDE)
188                 state_status = I2C_COBBOARD_STATUS_LBUSY;
189         else
190                 state_status = I2C_COBBOARD_STATUS_RBUSY;
191
192         /* if there is no cob, return */
193         if (cob_falling_edge(side) == 0)
194                 return;
195
196         STMCH_DEBUG("start");
197
198         /* eat the cob */
199         spickle_pack(side);
200
201         time_wait_ms(250);
202         cobroller_on(side);
203
204         /* check that cob is correctly in place */
205         if (WAIT_COND_OR_TIMEOUT(state_cob_inside(), 750) == 0) {
206                 if (state_no_cob_inside()) {
207                         STMCH_DEBUG("no cob");
208                         return;
209                 }
210                 STMCH_DEBUG("bad cob state");
211
212                 /* while cob is not correctly placed try to extract
213                  * it as much as we can */
214                 while (state_cob_inside() == 0 &&
215                        state_no_cob_inside() == 0) {
216                         uint8_t cpt = 0;
217                         if (cpt == 0)
218                                 cobroller_reverse(side);
219                         else if (cpt == 1)
220                                 cobroller_off(side);
221                         else if (cpt == 2)
222                                 cobroller_on(side);
223                         cpt ++;
224                         cpt %= 3;
225                         shovel_mid();
226                         time_wait_ms(250);
227                         shovel_down();
228                         time_wait_ms(250);
229                 }
230
231                 STMCH_DEBUG("cob removed");
232                 if (state_no_cob_inside()) {
233                         STMCH_DEBUG("no cob");
234                         return;
235                 }
236         }
237
238         /* cob is inside, switch off roller */
239         cobroller_off(side);
240         cob_count ++;
241
242         /* last cob, nothing to do */
243         if (cob_count == 5)
244                 return;
245
246         /* redeploy the spickle */
247         spickle_deploy(side);
248         state_debug_wait_key_pressed();
249
250         /* let the loaded cobs go */
251         servo_door_block();
252         servo_carry_open();
253         time_wait_ms(100);
254         state_debug_wait_key_pressed();
255
256         /* store it */
257         shovel_up();
258
259         while (WAIT_COND_OR_TIMEOUT(shovel_is_up(), 600) == 0) {
260                 STMCH_DEBUG("shovel blocked");
261                 shovel_down();
262                 time_wait_ms(250);
263                 shovel_up();
264         }
265
266         state_debug_wait_key_pressed();
267
268         /* close the carry servos */
269         servo_carry_close();
270         servo_door_close();
271         time_wait_ms(200);
272         state_debug_wait_key_pressed();
273
274         shovel_down();
275
276         while (WAIT_COND_OR_TIMEOUT(shovel_is_down(), 400) == 0) {
277                 STMCH_DEBUG("shovel blocked");
278                 shovel_up();
279                 time_wait_ms(250);
280                 shovel_down();
281         }
282
283         STMCH_DEBUG("end");
284 }
285
286 /* eject cobs */
287 static void state_do_eject(void)
288 {
289         state_status = I2C_COBBOARD_STATUS_EJECT;
290         cob_count = 0;
291         shovel_mid();
292         servo_carry_open();
293         servo_door_open();
294         time_wait_ms(2000);
295         shovel_down();
296         servo_door_close();
297         servo_carry_close();
298 }
299
300 /* main state machine */
301 void state_machine(void)
302 {
303         while (state_want_exit() == 0) {
304
305                 state_status = I2C_COBBOARD_STATUS_READY;
306
307                 /* init */
308                 if (INIT(state_mode)) {
309                         state_mode &= (~I2C_COBBOARD_MODE_INIT);
310                         state_init();
311                 }
312
313                 /* pack/deply spickles, enable/disable roller */
314                 cobroller_off(I2C_LEFT_SIDE);
315                 cobroller_off(I2C_RIGHT_SIDE);
316                 spickle_prepare(I2C_LEFT_SIDE);
317                 spickle_prepare(I2C_RIGHT_SIDE);
318
319                 /* harvest */
320                 if (cob_count < 5) {
321                         if ((lspickle & I2C_COBBOARD_SPK_DEPLOY) &&
322                             (lspickle & I2C_COBBOARD_SPK_DEPLOY))
323                                 state_do_harvest(I2C_LEFT_SIDE);
324                         if ((rspickle & I2C_COBBOARD_SPK_DEPLOY) &&
325                             (rspickle & I2C_COBBOARD_SPK_DEPLOY))
326                                 state_do_harvest(I2C_RIGHT_SIDE);
327                 }
328
329                 /* eject */
330                 if (EJECT(state_mode)) {
331                         state_mode &= (~I2C_COBBOARD_MODE_EJECT);
332                         state_do_eject();
333                 }
334         }
335         state_status = I2C_COBBOARD_STATUS_OFF;
336 }
337
338 void state_init(void)
339 {
340         vt100_init(&local_vt100);
341         shovel_down();
342         servo_door_close();
343         servo_carry_close();
344         spickle_pack(I2C_LEFT_SIDE);
345         spickle_pack(I2C_RIGHT_SIDE);
346         state_mode = 0;
347         cob_count = 0;
348         state_status = I2C_COBBOARD_STATUS_OFF;
349 }