summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src/gutter_cb.c
blob: 5abf7bcb054bd4a3032b88fd7dd1f98664520577 (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
/* 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 1126

/*
 * 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);
}

/*
 * 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_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
 */
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);
}