summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
Diffstat (limited to 'digital')
-rw-r--r--digital/io/src/move.c41
-rw-r--r--digital/io/src/move.h14
-rw-r--r--digital/io/src/move_cb.c60
3 files changed, 59 insertions, 56 deletions
diff --git a/digital/io/src/move.c b/digital/io/src/move.c
index 1f8addfc..b239253f 100644
--- a/digital/io/src/move.c
+++ b/digital/io/src/move.c
@@ -77,3 +77,44 @@ move_can_go_on_left_or_right (asserv_position_t current_pos,
}
}
+/** Go to the right.
+ */
+void
+move_go_to_right (void)
+{
+ asserv_position_t pos;
+ asserv_position_t new_pos;
+
+ asserv_get_position (&pos);
+ new_pos.x = pos.x - MOVE_BORDER_LEVEL;
+ new_pos.y = pos.y;
+ new_pos.a = pos.a;
+
+ if (move_can_go_on_left_or_right (pos, new_pos))
+ {
+ asserv_goto (new_pos.x, new_pos.y);
+ }
+}
+
+/** Go to the left.
+ */
+void
+move_go_to_left (void)
+{
+ asserv_position_t pos;
+ asserv_position_t new_pos;
+
+ asserv_get_position (&pos);
+ new_pos.x = pos.x + MOVE_BORDER_LEVEL;
+ new_pos.y = pos.y;
+ new_pos.a = pos.a;
+
+ if (move_can_go_on_left_or_right (pos, new_pos))
+ {
+ asserv_goto (new_pos.x, new_pos.y);
+ }
+
+ return move_next (MOVE_ON_LEFT, failed);
+}
+
+
diff --git a/digital/io/src/move.h b/digital/io/src/move.h
index 99997bc3..69a2975e 100644
--- a/digital/io/src/move.h
+++ b/digital/io/src/move.h
@@ -27,6 +27,10 @@
#include "asserv.h"
+/* if the left border is under 500 mm of the actual position do not go there. */
+#define MOVE_BORDER_LEVEL 200
+
+
/** move FSM associated data. */
struct move_data_t
{
@@ -54,4 +58,14 @@ uint8_t
move_can_go_on_left_or_right (asserv_position_t current_pos,
asserv_position_t new_pos);
+/** Go to the right.
+ */
+void
+move_go_to_right (void);
+
+/** Go to the left.
+ */
+void
+move_go_to_left (void);
+
#endif /* move_h */
diff --git a/digital/io/src/move_cb.c b/digital/io/src/move_cb.c
index 13571d29..fb32ff32 100644
--- a/digital/io/src/move_cb.c
+++ b/digital/io/src/move_cb.c
@@ -11,10 +11,6 @@
#include "move.h"
#include "asserv.h"
-
-/* if the left border is under 500 mm of the actual position do not go there. */
-#define MOVE_BORDER_LEVEL 200
-
/*
* IDLE =start=>
* => DESIRED_POSITION
@@ -35,19 +31,7 @@ move__IDLE__start (void)
fsm_branch_t
move__MOVE_ON_RIGHT__failed (void)
{
- asserv_position_t pos;
- asserv_position_t new_pos;
-
- asserv_get_position (&pos);
- new_pos.x = pos.x - MOVE_BORDER_LEVEL;
- new_pos.y = pos.y;
- new_pos.a = pos.a;
-
- if (move_can_go_on_left_or_right (pos, new_pos))
- {
- asserv_goto (new_pos.x, new_pos.y);
- }
-
+ move_go_to_right ();
return move_next (MOVE_ON_RIGHT, failed);
}
@@ -71,19 +55,7 @@ move__MOVE_ON_RIGHT__reached (void)
fsm_branch_t
move__MOVE_ON_RIGHT__blocked (void)
{
- asserv_position_t pos;
- asserv_position_t new_pos;
-
- asserv_get_position (&pos);
- new_pos.x = pos.x - MOVE_BORDER_LEVEL;
- new_pos.y = pos.y;
- new_pos.a = pos.a;
-
- if (move_can_go_on_left_or_right (pos, new_pos))
- {
- asserv_goto (new_pos.x, new_pos.y);
- }
-
+ move_go_to_right ();
return move_next (MOVE_ON_RIGHT, blocked);
}
@@ -168,19 +140,7 @@ move__DESIRED_POSITION__blocked (void)
fsm_branch_t
move__MOVE_ON_LEFT__failed (void)
{
- asserv_position_t pos;
- asserv_position_t new_pos;
-
- asserv_get_position (&pos);
- new_pos.x = pos.x + MOVE_BORDER_LEVEL;
- new_pos.y = pos.y;
- new_pos.a = pos.a;
- if (move_can_go_on_left_or_right (pos, new_pos))
- {
- // call the correct function to go to the right.
- asserv_goto (new_pos.x, new_pos.y);
- }
-
+ move_go_to_left ();
return move_next (MOVE_ON_LEFT, failed);
}
@@ -204,19 +164,7 @@ move__MOVE_ON_LEFT__reached (void)
fsm_branch_t
move__MOVE_ON_LEFT__blocked (void)
{
- asserv_position_t pos;
- asserv_position_t new_pos;
-
- asserv_get_position (&pos);
- new_pos.x = pos.x + MOVE_BORDER_LEVEL;
- new_pos.y = pos.y;
- new_pos.a = pos.a;
- if (move_can_go_on_left_or_right (pos, new_pos))
- {
- // call the correct function to go to the right.
- asserv_goto (new_pos.x, new_pos.y);
- }
-
+ move_go_to_left ();
return move_next (MOVE_ON_LEFT, blocked);
}