summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src/top_cb.c
blob: 6c2b3f05f5fd40818c0d4ba44cd365e6d433a6c1 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
/* top_cb.c - top FSM callbacks. */
/*  {{{
 *
 * Copyright (C) 2008 Jérémy Dufour
 *
 * APBTeam:
 *        Web: http://apbteam.org/
 *      Email: team AT apbteam DOT org
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 *
 * }}} */
#include "common.h"
#include "fsm.h"
#include "top_cb.h"

#include "move.h"	/* move FSM */
#include "playground.h"	/* PG_* */
#include "asserv.h"	/* asserv_* */
#include "chrono.h"	/* chrono_init */
/* AVR include, non HOST */
#ifndef HOST
# include "switch.h"	/* switch_get_color */
#endif /* HOST */
#include "simu.host.h"

#include "getsamples.h"	/* getsamples_* */
#include "gutter.h"	/* gutter_start */

/**
 * When we need to tell the main loop we want to be alerted when the last
 * command sent to the asserv board has been acknowledged.
 */
extern uint8_t top_waiting_for_settings_ack_;

/*
 * DROP_OFF_BALLS_TO_GUTTER =gutter_fsm_finished=>
 *  => MOVE_AWAY_FROM_GUTTER_BORDER
 *   we have finished to drop off all the balls, let's move away from the gutter
 *   to move freely
 */
fsm_branch_t
top__DROP_OFF_BALLS_TO_GUTTER__gutter_fsm_finished (void)
{
    /* Get current position */
    asserv_position_t position;
    asserv_get_position (&position);
    /* Move away from the gutter */
    move_start (position.x, position.y - BOT_MIN_DISTANCE_TURN_FREE);
    return top_next (DROP_OFF_BALLS_TO_GUTTER, gutter_fsm_finished);
}

/*
 * WAIT_JACK_OUT =jack_removed_from_bot=>
 *  => CONFIGURE_ASSERV
 *   the match start, start the chronometer
 *   we should also initialize all the subsystems of IO (get our color, ...)
 *   set the settings of the asserv board (especially the position)
 */
fsm_branch_t
top__WAIT_JACK_OUT__jack_removed_from_bot (void)
{
    /* Set-up our color */
    bot_color = switch_get_color ();
    /* Start the chronometer */
    chrono_init ();
    /* Reset the position of the bot */
    asserv_set_position (PG_X_START, PG_Y_START, PG_A_START);
    /* Tell the main loop we need to be aware when the asserv acknowledge our
     * settings command */
    top_waiting_for_settings_ack_ = 1;
    return top_next (WAIT_JACK_OUT, jack_removed_from_bot);
}

/*
 * MOVE_AWAY_FROM_START_BORDER =move_fsm_finished=>
 *  => GO_TO_SAMPLE_DISTRIBUTOR
 *   order the bot to move to our samples distributors with the move FSM
 */
fsm_branch_t
top__MOVE_AWAY_FROM_START_BORDER__move_fsm_finished (void)
{
    /* Start the move FSM to our samples distributor */
    move_start (PG_DISTRIBUTOR_SAMPLE_OUR_X, PG_DISTRIBUTOR_SAMPLE_OUR_Y);
    return top_next (MOVE_AWAY_FROM_START_BORDER, move_fsm_finished);
}

/*
 * GET_SAMPLES_FROM_SAMPLES_DISTRIBUTOR =get_samples_fsm_finished=>
 *  => GO_TO_OUR_ICE_DISTRIBUTOR
 *   we have finished to get our samples, let's go to our ice distributor with
 *   the move FSM
 */
fsm_branch_t
top__GET_SAMPLES_FROM_SAMPLES_DISTRIBUTOR__get_samples_fsm_finished (void)
{
    /* Start the move FSM to our ice distributor */
    move_start (PG_DISTRIBUTOR_ICE_OUR_X, PG_DISTRIBUTOR_ICE_OUR_Y);
    return top_next (GET_SAMPLES_FROM_SAMPLES_DISTRIBUTOR, get_samples_fsm_finished);
}

/*
 * GO_TO_ADVERSE_ICE_DISTRIBUTOR =move_fsm_finished=>
 *  => GET_ICE_FROM_ADVERSE_ICE_DISTRIBUTOR
 *   we are now in front of the adverse ice distributor, launch the get samples
 *   FSM
 */
fsm_branch_t
top__GO_TO_ADVERSE_ICE_DISTRIBUTOR__move_fsm_finished (void)
{
    /* Start the get samples FSM with the correct angle */
    /* TODO: where to put the ice?! */
    getsamples_start (PG_DISTRIBUTOR_ICE_ADVERSE_A, 0);
    return top_next (GO_TO_ADVERSE_ICE_DISTRIBUTOR, move_fsm_finished);
}

/*
 * GO_TO_OUR_ICE_DISTRIBUTOR =move_fsm_finished=>
 *  => GET_ICE_FROM_OUR_ICE_DISTRIBUTOR
 *   we are now in front of our ice distributor, launch the get samples FSM
 */
fsm_branch_t
top__GO_TO_OUR_ICE_DISTRIBUTOR__move_fsm_finished (void)
{
    /* Start the get samples FSM with the correct angle */
    /* TODO: where to put the ice?! */
    getsamples_start (PG_DISTRIBUTOR_ICE_OUR_A, 0);
    return top_next (GO_TO_OUR_ICE_DISTRIBUTOR, move_fsm_finished);
}

/*
 * GO_TO_SAMPLE_DISTRIBUTOR =move_fsm_finished=>
 *  => GET_SAMPLES_FROM_SAMPLES_DISTRIBUTOR
 *   we are now in front of our samples distributor, launch the get samples FSM
 */
fsm_branch_t
top__GO_TO_SAMPLE_DISTRIBUTOR__move_fsm_finished (void)
{
    /* Start the get samples FSM with the correct angle */
    /* TODO: where to put the samples?! */
    getsamples_start (PG_DISTRIBUTOR_SAMPLE_OUR_A, 0);
    return top_next (GO_TO_SAMPLE_DISTRIBUTOR, move_fsm_finished);
}

/*
 * IDLE =start=>
 *  => WAIT_JACK_IN
 *   wait for the jack to be inserted into the bot
 */
fsm_branch_t
top__IDLE__start (void)
{
    return top_next (IDLE, start);
}

/*
 * GO_TO_GUTTER =move_fsm_finished=>
 *  => DROP_OFF_BALLS_TO_GUTTER
 *   we are now at the gutter, let's drop all ours balls into it with the gutter
 *   FSM
 */
fsm_branch_t
top__GO_TO_GUTTER__move_fsm_finished (void)
{
    /* Start the gutter FSM */
    gutter_start ();
    return top_next (GO_TO_GUTTER, move_fsm_finished);
}

/*
 * GET_ICE_FROM_OUR_ICE_DISTRIBUTOR =get_samples_fsm_finished=>
 * not full => GO_TO_ADVERSE_ICE_DISTRIBUTOR
 *   we have finished to get ice from our distributor and we have some space
 *   left, let's go the adverse ice distributor with the move FSM
 * full => GO_TO_GUTTER
 *   we have finished to get ice from our distributor and we have no more space
 *   left, let's go the gutter with the move FSM
 */
fsm_branch_t
top__GET_ICE_FROM_OUR_ICE_DISTRIBUTOR__get_samples_fsm_finished (void)
{
    /* TODO: how to detect we have no more space? */
    if (0)
      {
	/* Start the move FSM to the adverse ice distributor */
	move_start (PG_DISTRIBUTOR_ICE_ADVERSE_X, PG_DISTRIBUTOR_ICE_ADVERSE_Y);
	return top_next_branch (GET_ICE_FROM_OUR_ICE_DISTRIBUTOR, get_samples_fsm_finished, not_full);
      }
    else
      {
	/* Start the move FSM to go to the gutter */
	move_start (PG_GUTTER_X, PG_GUTTER_Y);
	return top_next_branch (GET_ICE_FROM_OUR_ICE_DISTRIBUTOR, get_samples_fsm_finished, full);
      }
}

/*
 * MOVE_AWAY_FROM_GUTTER_BORDER =move_fsm_finished=>
 *  => GO_TO_SAMPLE_DISTRIBUTOR
 *   go to our sample ditributor to try the same strategy again
 *   reset internal data
 */
fsm_branch_t
top__MOVE_AWAY_FROM_GUTTER_BORDER__move_fsm_finished (void)
{
    /* Start the move FSM */
    move_start (PG_DISTRIBUTOR_SAMPLE_OUR_X, PG_DISTRIBUTOR_SAMPLE_OUR_Y);
    return top_next (MOVE_AWAY_FROM_GUTTER_BORDER, move_fsm_finished);
}

/*
 * GET_ICE_FROM_ADVERSE_ICE_DISTRIBUTOR =get_samples_fsm_finished=>
 *  => GO_TO_GUTTER
 *   we have finished to get ice. Even if we are not full, let's go to the gutter
 *   with the move FSM
 */
fsm_branch_t
top__GET_ICE_FROM_ADVERSE_ICE_DISTRIBUTOR__get_samples_fsm_finished (void)
{
    /* Start the move FSM to go to the gutter */
    move_start (PG_GUTTER_X, PG_GUTTER_Y);
    return top_next (GET_ICE_FROM_ADVERSE_ICE_DISTRIBUTOR, get_samples_fsm_finished);
}

/*
 * WAIT_JACK_IN =jack_inserted_into_bot=>
 *  => WAIT_JACK_OUT
 *   wait for the jack to be removed from the bot
 */
fsm_branch_t
top__WAIT_JACK_IN__jack_inserted_into_bot (void)
{
    return top_next (WAIT_JACK_IN, jack_inserted_into_bot);
}

/*
 * CONFIGURE_ASSERV =settings_acknowledged=>
 *  => MOVE_AWAY_FROM_START_BORDER
 *   move the bot away from the border to be able to turn freely
 */
fsm_branch_t
top__CONFIGURE_ASSERV__settings_acknowledged (void)
{
    /* Clear the flag for the setting acknowleged */
    top_waiting_for_settings_ack_ = 0;
    /* Start the move FSM to move the the bot away from the border */
    move_start (PG_X_START, PG_Y_START - BOT_MIN_DISTANCE_TURN_FREE);
    return top_next (CONFIGURE_ASSERV, settings_acknowledged);
}