summaryrefslogtreecommitdiff
path: root/digital/io/src/move.c
diff options
context:
space:
mode:
authorNélio Laranjeiro2008-04-08 00:02:53 +0200
committerNélio Laranjeiro2008-04-08 00:02:53 +0200
commitb9d646e41511eb5443faad66b8fc1c7323b2e738 (patch)
tree61daf69e2f92986683c87b3fcd761b616ecde6cc /digital/io/src/move.c
parent8877e2dc061b6f98a12445e66222bef9c155d7bc (diff)
Tested the move FSM machine.
Diffstat (limited to 'digital/io/src/move.c')
-rw-r--r--digital/io/src/move.c37
1 files changed, 37 insertions, 0 deletions
diff --git a/digital/io/src/move.c b/digital/io/src/move.c
index 211c53ac..b4155589 100644
--- a/digital/io/src/move.c
+++ b/digital/io/src/move.c
@@ -23,9 +23,18 @@
*
* }}} */
#include "common.h"
+#include "asserv.h"
#include "move.h"
#include "fsm.h"
+#define MOVE_BORDER_END 500
+
+#define MOVE_ANGLE_0 0x0
+#define MOVE_ANGLE_90 0x4000
+#define MOVE_ANGLE_180 0x8000
+#define MOVE_ANGLE_270 0xC000
+
+
struct move_data_t move_data;
/** Start a move FSM. */
@@ -40,3 +49,31 @@ move_start (uint32_t position_x, uint32_t position_y)
fsm_handle_event (&move_fsm, MOVE_EVENT_ok);
}
+/** Verify if the position desired is in the table use when the robot tries to
+ * reach a point and a obstacle is in front of it.
+ * \param pos the robot's current position on the table.
+ * \param new_pos the position desired by the user.
+ * \return true if the it can reach the position.
+ */
+uint8_t
+move_can_go_on_left_or_right (asserv_position_t current_pos,
+ asserv_position_t new_pos)
+{
+ // Go on right.
+ if (new_pos.x > current_pos.x)
+ {
+ if (new_pos.x - current_pos.x < 3000 - MOVE_BORDER_END)
+ return 0x1;
+ else
+ return 0x0;
+ }
+ // go on left.
+ else
+ {
+ if (current_pos.x - new_pos.x < MOVE_BORDER_END)
+ return 0x1;
+ else
+ return 0x0;
+ }
+}
+