summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/guybrush/top.c
diff options
context:
space:
mode:
authorNicolas Schodet2012-05-14 13:27:27 +0200
committerNicolas Schodet2012-05-14 13:27:27 +0200
commit8af1e02a8e876ad592478e6f566aaede9f467af3 (patch)
tree3fd7ea09611bf6f7113a19e71b02acd281640703 /digital/io-hub/src/guybrush/top.c
parent0386b2edbd7974b31bf4b0536a625365d69a31f1 (diff)
digital/io-hub/src/guybrush: compute linear moves distances
Diffstat (limited to 'digital/io-hub/src/guybrush/top.c')
-rw-r--r--digital/io-hub/src/guybrush/top.c20
1 files changed, 16 insertions, 4 deletions
diff --git a/digital/io-hub/src/guybrush/top.c b/digital/io-hub/src/guybrush/top.c
index 0b763480..a1cc246d 100644
--- a/digital/io-hub/src/guybrush/top.c
+++ b/digital/io-hub/src/guybrush/top.c
@@ -23,6 +23,7 @@
*
* }}} */
#include "common.h"
+#include "modules/utils/utils.h"
#include "io.h"
#include "playground_2012.h"
@@ -106,6 +107,8 @@ struct top_t
{
/** Decision position. */
vect_t decision_pos;
+ /** Current distance to totem. */
+ int16_t totem_distance;
};
/** Global context. */
@@ -120,6 +123,8 @@ top_go_totem (void)
pos.a = pos.v.y > PG_LENGTH / 2 ? POSITION_A_DEG (-90)
: POSITION_A_DEG (90);
move_start (pos, 0);
+ top.totem_distance = UTILS_ABS (pos.v.y - PG_LENGTH / 2)
+ - PG_TOTEM_WIDTH_MM / 2;
}
/** Go push a bottle button. */
@@ -211,7 +216,9 @@ FSM_TRANS (TOP_TOTEM_GOING, move_success, TOP_TOTEM_CLEAN_STARTING)
FSM_TRANS (TOP_TOTEM_CLEAN_STARTING, clamps_ready, TOP_TOTEM_CLEAN_APPROACHING)
{
- asserv_move_linearly (PATH_GRID_CLEARANCE_MM - BOT_SIZE_FRONT - 130);
+ int16_t move = top.totem_distance - BOT_SIZE_LOWER_CLAMP_FRONT - 140;
+ top.totem_distance -= move;
+ asserv_move_linearly (move);
return FSM_NEXT (TOP_TOTEM_CLEAN_STARTING, clamps_ready);
}
@@ -225,7 +232,9 @@ FSM_TRANS (TOP_TOTEM_CLEAN_APPROACHING, robot_move_success,
FSM_TRANS (TOP_TOTEM_CLEAN_CATCH_WAITING, clamps_ready,
TOP_TOTEM_CLEAN_GOING_BACK)
{
- asserv_move_linearly (-100);
+ int16_t move = top.totem_distance - BOT_SIZE_LOWER_CLAMP_FRONT - 240;
+ top.totem_distance -= move;
+ asserv_move_linearly (move);
return FSM_NEXT (TOP_TOTEM_CLEAN_CATCH_WAITING, clamps_ready);
}
@@ -244,7 +253,9 @@ FSM_TRANS (TOP_TOTEM_CLEAN_LOADING, clamps_ready, TOP_TOTEM_CLAMP_DOWNING)
FSM_TRANS (TOP_TOTEM_CLAMP_DOWNING, clamps_ready, TOP_TOTEM_APPROACHING)
{
- asserv_move_linearly (200);
+ int16_t move = top.totem_distance - BOT_SIZE_FRONT - 30;
+ top.totem_distance -= move;
+ asserv_move_linearly (move);
return FSM_NEXT (TOP_TOTEM_CLAMP_DOWNING, clamps_ready);
}
@@ -331,7 +342,8 @@ FSM_TRANS (TOP_TOTEM_ERROR_GOING_BACK, move_failure, TOP_TOTEM_CLAMP_UPPING)
FSM_TRANS (TOP_BOTTLE_GOING, move_success, TOP_BOTTLE_APPROACHING)
{
- asserv_move_linearly (-(BOT_SIZE_RADIUS + 70 - BOT_SIZE_BACK - 22 - 30));
+ int16_t move = top.decision_pos.y - BOT_SIZE_BACK - 22 - 30;
+ asserv_move_linearly (-move);
return FSM_NEXT (TOP_BOTTLE_GOING, move_success);
}