summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src/gutter_cb.c
blob: 96f5c6054e1d188aba3e4aeb1d74809e5487605c (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
/* gutter_cb.c - gutter FSM callbacks. */
/*  {{{
 *
 * Copyright (C) 2008 Nélio Laranjeiro
 *
 * 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 "gutter_cb.h"

#include "asserv.h"	/* asserv_go_to_the_wall */
#include "trap.h"	/* trap_* */
#include "playground.h"	/* PG_GUTTER_A */
#include "main.h"	/* main_post_event_for_top_fsm */

/**
 * Gutter private data to wait a certain number of cycles.
 */
uint16_t gutter_wait_cycle_;

/**
 * Count of cycles to wait before we estimate all the balls have been dropped
 * into the gutter. A cycle normally last 4.4ms.
 */
#define GUTTER_WAIT_FOR_BALLS_TO_DROP 300

/**
 * Time to wait before trying the go to the wall again.
 */
#define GUTTER_WAIT_BEFORE_TRY_AGAIN (225)

/*
 * ROTATE_REAR_SIDE_TO_GUTTER =bot_move_succeed=>
 *  => GO_TO_THE_GUTTER_WALL
 *   make the bot reversing against the gutter
 */
fsm_branch_t
gutter__ROTATE_REAR_SIDE_TO_GUTTER__bot_move_succeed (void)
{
    /* Make the bot reversing against the gutter */
    asserv_go_to_the_wall ();
    return gutter_next (ROTATE_REAR_SIDE_TO_GUTTER, bot_move_succeed);
}

/*
 * WAIT_AND_TRY_AGAIN =wait_finished=>
 *  => GO_TO_THE_GUTTER_WALL
 *   try the fuck the wall again
 */
fsm_branch_t
gutter__WAIT_AND_TRY_AGAIN__wait_finished (void)
{
    /* Make the bot reversing against the gutter */
    asserv_go_to_the_wall ();
    return gutter_next (WAIT_AND_TRY_AGAIN, wait_finished);
}

/*
 * IDLE =start=>
 *  => ROTATE_REAR_SIDE_TO_GUTTER
 *   put the bot back to the gutter
 */
fsm_branch_t
gutter__IDLE__start (void)
{
    /* Initialize internal data */
    gutter_wait_cycle_ = 0;
    /* Put the bot back to the gutter */
    asserv_goto_angle (PG_GUTTER_A);
    return gutter_next (IDLE, start);
}

/*
 * GO_TO_THE_GUTTER_WALL =bot_move_failed=>
 *  => WAIT_AND_TRY_AGAIN
 *   ask the top FSM to wake us up in a few times
 */
fsm_branch_t
gutter__GO_TO_THE_GUTTER_WALL__bot_move_failed (void)
{
    main_getsamples_wait_cycle = GUTTER_WAIT_BEFORE_TRY_AGAIN;
    return gutter_next (GO_TO_THE_GUTTER_WALL, bot_move_failed);
}

/*
 * GO_TO_THE_GUTTER_WALL =bot_move_succeed=>
 *  => DROP_BALLS
 *   open the collector to drop the balls
 *   wait for a while
 */
fsm_branch_t
gutter__GO_TO_THE_GUTTER_WALL__bot_move_succeed (void)
{
    /* Open the rear panel */
    trap_open_rear_panel ();
    /* Wait for GUTTER_WAIT_FOR_BALLS_TO_DROP before being calling back by the
     * main loop */
    gutter_wait_cycle_ = GUTTER_WAIT_FOR_BALLS_TO_DROP;
    return gutter_next (GO_TO_THE_GUTTER_WALL, bot_move_succeed);
}

/*
 * DROP_BALLS =wait_finished=>
 *  => IDLE
 *   close the rear panel
 *   tell the top FSM we have finished
 */
fsm_branch_t
gutter__DROP_BALLS__wait_finished (void)
{
    /* Close the rear panel */
    trap_close_rear_panel ();
    /* Tell the top FSM we have finished */
    main_post_event_for_top_fsm = TOP_EVENT_gutter_fsm_finished;
    return gutter_next (DROP_BALLS, wait_finished);
}