summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src/move_cb.c
blob: 12e10c351a1a02a117deb2091962ab32d76d1f39 (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
/*
 * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT!
 *
 * Skeleton for move callbacks implementation.
 *
 * 
 */
#include "common.h"
#include "fsm.h"
#include "move_cb.h"
#include "move.h"
#include "asserv.h"

/*
 * IDLE =start=>
 *  => DESIRED_POSITION
 *   Tries to reach a position provided by the user. If the position desired can not be reached, it all try to move on the right or the left.
 */
fsm_branch_t
move__IDLE__start (void)
{
    asserv_goto (move_data.position_x, move_data.position_y);
    return move_next (IDLE, start);
}

/*
 * MOVE_ON_RIGHT =blocked=>
 *  => MOVE_ON_LEFT
 *   The robot will try to go on the left because the previous movement block on the left.
 */
fsm_branch_t
move__MOVE_ON_RIGHT__blocked (void)
{
    move_go_to_left (); 
    return move_next (MOVE_ON_RIGHT, blocked);
}

/*
 * MOVE_ON_RIGHT =reached=>
 *  => DESIRED_POSITION
 *   The position has been reached. It will now try to reach the position provided by the user.
 */
fsm_branch_t
move__MOVE_ON_RIGHT__reached (void)
{
    asserv_goto (move_data.position_x, move_data.position_y);
    return move_next (MOVE_ON_RIGHT, reached);
}

/*
 * DESIRED_POSITION =blocked=>
 * near_right_border => MOVE_ON_LEFT
 *   The robot will compute the position from the border and will take the decision to go on the left because the right border is too near.
 * near_left_border => MOVE_ON_RIGHT
 *   The robot will compute the position from the border and will take the decision to go on the right because the left border is too near.
 */
fsm_branch_t
move__DESIRED_POSITION__blocked (void)
{
    asserv_position_t pos;
    asserv_position_t new_pos;

    asserv_get_position (&pos);
    new_pos = pos;
    new_pos.x += MOVE_BORDER_LEVEL ;

    if (move_can_go_on_left_or_right (pos, new_pos))
      {
	asserv_goto (new_pos.x, new_pos.y);
	return move_next_branch (DESIRED_POSITION, blocked, near_left_border);
      }
    else
      {
	new_pos.x = pos.x + MOVE_BORDER_LEVEL;
	// replace this by the correct function.
	asserv_goto (new_pos.x, new_pos.y);
	return move_next_branch (DESIRED_POSITION, blocked, near_right_border);
      }
}

/*
 * DESIRED_POSITION =reached=>
 *  => IDLE
 *   The position provided by the user has been reached, the FSM can stop.
 */
fsm_branch_t
move__DESIRED_POSITION__reached (void)
{
    fsm_handle_event (&top_fsm, TOP_EVENT_move_fsm_finished);
    return move_next (DESIRED_POSITION, reached);
}

/*
 * MOVE_ON_LEFT =blocked=>
 *  => MOVE_ON_RIGHT
 *   The robot will try to go on the left because the previous movement block on the right.
 */
fsm_branch_t
move__MOVE_ON_LEFT__blocked (void)
{
    move_go_to_right();
    return move_next (MOVE_ON_LEFT, blocked);
}

/*
 * MOVE_ON_LEFT =reached=>
 *  => DESIRED_POSITION
 *   The position has been reached. It will now try to reach the position provided by the user.
 */
fsm_branch_t
move__MOVE_ON_LEFT__reached (void)
{
    asserv_goto (move_data.position_x, move_data.position_y);
    return move_next (MOVE_ON_LEFT, reached);
}