summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src/top_cb.c
blob: c1624569a07139c4fe02ce5b13af59951685941b (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
/* 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_* */
/* AVR include, non HOST */
#ifndef HOST
# include "chrono.h"	/* chrono_init */
# include "switch.h"	/* switch_get_color */
#endif /* HOST */
#include "simu.host.h"

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

/*
 * DROP_OFF_BALLS_TO_GUTTER =gutter_fsm_finished=>
 *  => GO_TO_SAMPLE_DISTRIBUTOR
 *   we have finished to drop off all the balls, let's go to our sample
 *   ditributor to try the same strategy again
 *   reset internal data
 */
fsm_branch_t
top__DROP_OFF_BALLS_TO_GUTTER__gutter_fsm_finished (void)
{
    /* Start the move FSM */
    move_start (PG_DISTRIBUTOR_SAMPLE_OUR_X, PG_DISTRIBUTOR_SAMPLE_OUR_Y);
    return top_next (DROP_OFF_BALLS_TO_GUTTER, gutter_fsm_finished);
}

/*
 * WAIT_JACK_OUT =jack_removed_from_bot=>
 *  => GO_TO_SAMPLE_DISTRIBUTOR
 *   the match start, start the chronometer
 *   we should also initialize all the subsystems of IO (reset position, get our
 *   color, ...)
 *   order the bot to move to our samples distributors with the move FSM
 */
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_x_position (PG_X_START);
    asserv_set_y_position (PG_Y_START);
    asserv_set_angle_position (PG_A_START);
    /* Start the move FSM to our samples distributor */
    move_start (PG_DISTRIBUTOR_SAMPLE_OUR_X, PG_DISTRIBUTOR_SAMPLE_OUR_Y);
    return top_next (WAIT_JACK_OUT, jack_removed_from_bot);
}

/*
 * 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 */
    struct getsamples_data_t data;
    /* TODO: where to put the ice?! */
    data.sample_bitfield = 0;
    data.approach_angle = PG_DISTRIBUTOR_ICE_ADVERSE_A;
    getsamples_start (data);
    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 */
    struct getsamples_data_t data;
    /* TODO: where to put the ice?! */
    data.sample_bitfield = 0;
    data.approach_angle = PG_DISTRIBUTOR_ICE_OUR_A;
    getsamples_start (data);
    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 */
    struct getsamples_data_t data;
    /* TODO: where to put the samples?! */
    data.sample_bitfield = 0;
    data.approach_angle = PG_DISTRIBUTOR_SAMPLE_OUR_A;
    getsamples_start (data);
    return top_next (GO_TO_SAMPLE_DISTRIBUTOR, move_fsm_finished);
}

/*
 * IDLE =start=>
 *  => WAIT_JACK_IN
 *   tell the main loop we want to be informed when the jack is inserted into the
 *   bot
 */
fsm_branch_t
top__IDLE__start (void)
{
    /* TODO */
    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);
      }
}

/*
 * 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
 *   tell the main loop we want to be informed when the jack is removed from the
 *   bot
 */
fsm_branch_t
top__WAIT_JACK_IN__jack_inserted_into_bot (void)
{
    /* TODO */
    return top_next (WAIT_JACK_IN, jack_inserted_into_bot);
}