summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2011-05-31 15:07:25 +0200
committerNicolas Schodet2011-05-31 15:17:37 +0200
commit32a21a69bb7adb44c4fc91d025ad8f0ccdaf45a1 (patch)
tree03aac5ed95b95902279b7604e4dc13697104942f /digital
parent7110e5fc8d629f10a85a4355bf34e5d022845f1c (diff)
digital/io-hub: remove special case for green elements
Diffstat (limited to 'digital')
-rw-r--r--digital/io-hub/src/robospierre/bot.h7
-rw-r--r--digital/io-hub/src/robospierre/element.c13
-rw-r--r--digital/io-hub/src/robospierre/element.h5
-rw-r--r--digital/io-hub/src/robospierre/test_element.c4
-rw-r--r--digital/io-hub/src/robospierre/top.c85
5 files changed, 21 insertions, 93 deletions
diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h
index 8e32a317..de32ebb2 100644
--- a/digital/io-hub/src/robospierre/bot.h
+++ b/digital/io-hub/src/robospierre/bot.h
@@ -58,10 +58,9 @@
/** Distance from border to position in front of a green element. */
#define BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM 600
-/** Distance to go to capture a green element. */
-#define BOT_GREEN_ELEMENT_MOVE_DISTANCE_MM \
- (BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM - BOT_ELEMENT_RADIUS \
- - BOT_SIZE_FRONT - 20)
+/** Distance from border to go to capture a green element. */
+#define BOT_GREEN_ELEMENT_DISTANCE_MM \
+ (BOT_ELEMENT_RADIUS + BOT_SIZE_FRONT + 20)
/** Speed used for initialisation. */
#ifdef HOST
diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c
index bc76a2b0..71f754e0 100644
--- a/digital/io-hub/src/robospierre/element.c
+++ b/digital/io-hub/src/robospierre/element.c
@@ -706,24 +706,21 @@ element_opposed (uint8_t element_id)
return op;
}
-position_t
+vect_t
element_get_pos (uint8_t element_id)
{
element_t e = element_get (element_id);
- position_t pos;
- pos.v = e.pos;
- pos.a = 0xffff;
+ vect_t pos;
+ pos = e.pos;
if (e.attr == (ELEMENT_GREEN |ELEMENT_RIGHT))
{
/* To the right. */
- pos.a = 0x0000;
- pos.v.x = PG_WIDTH - BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM;
+ pos.x = PG_WIDTH - BOT_GREEN_ELEMENT_DISTANCE_MM;
}
if (e.attr == (ELEMENT_GREEN |ELEMENT_LEFT))
{
/* To the left. */
- pos.a = 0x8000;
- pos.v.x = BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM;
+ pos.x = BOT_GREEN_ELEMENT_DISTANCE_MM;
}
return pos;
}
diff --git a/digital/io-hub/src/robospierre/element.h b/digital/io-hub/src/robospierre/element.h
index 01e8f22e..3ac1f9ba 100644
--- a/digital/io-hub/src/robospierre/element.h
+++ b/digital/io-hub/src/robospierre/element.h
@@ -167,11 +167,8 @@ element_nearest_element_id (position_t robot_pos);
/**
Give the position where the robot need to be placed to get an element.
- The position correspond to the emplacement of the element with an angle of
- 0xffff. If the element is in the green zone, the returned position include
- the angle the robot need to set before moving to the element.
*/
-position_t
+vect_t
element_get_pos (uint8_t element_id);
#endif /* element_h */
diff --git a/digital/io-hub/src/robospierre/test_element.c b/digital/io-hub/src/robospierre/test_element.c
index 2de92361..7e62a492 100644
--- a/digital/io-hub/src/robospierre/test_element.c
+++ b/digital/io-hub/src/robospierre/test_element.c
@@ -328,8 +328,8 @@ int main ()
case 'g':
printf ("element id: ");
scanf ("%i", &x);
- position_t pos = element_get_pos (x);
- printf ("x: %u y: %u a: %u\n", pos.v.x, pos.v.y, pos.a);
+ vect_t pos = element_get_pos (x);
+ printf ("x: %u y: %u\n", pos.x, pos.y);
break;
case 'q':
exit = 1;
diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c
index 5c10a0f3..a3ad4c72 100644
--- a/digital/io-hub/src/robospierre/top.c
+++ b/digital/io-hub/src/robospierre/top.c
@@ -49,11 +49,7 @@ FSM_STATES (
TOP_GOING_OUT2,
TOP_GOING_TO_DROP,
- TOP_GOING_TO_ELEMENT,
- TOP_GOING_TO_GREEN_POS,
- TOP_GOING_TO_GREEN_ELEMENT,
- TOP_TAKING_GREEN_ELEMENT,
- TOP_GOING_FROM_GREEN_ELEMENT)
+ TOP_GOING_TO_ELEMENT)
FSM_START_WITH (TOP_START)
@@ -62,8 +58,6 @@ struct top_t
{
/** Target element. */
uint8_t target_element_id;
- /** Green element move distance. */
- int16_t green_move_distance_mm;
};
/** Global context. */
@@ -89,27 +83,11 @@ top_go_element (void)
position_t robot_pos;
asserv_get_position (&robot_pos);
ctx.target_element_id = element_best (robot_pos);
- position_t element_pos = element_get_pos (ctx.target_element_id);
+ vect_t element_pos = element_get_pos (ctx.target_element_id);
uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD
? 0 : ASSERV_BACKWARD;
- if (element_pos.a != 0xffff)
- {
- /* Green zone element. */
- ctx.green_move_distance_mm = BOT_GREEN_ELEMENT_MOVE_DISTANCE_MM;
- if (backward)
- {
- element_pos.a += 0x8000;
- ctx.green_move_distance_mm = -ctx.green_move_distance_mm;
- }
- move_start (element_pos, backward);
- return 2;
- }
- else
- {
- /* Regular element. */
- move_start_noangle (element_pos.v, backward, 0);
- return 1;
- }
+ move_start_noangle (element_pos, backward, 0);
+ return 1;
}
static uint8_t
@@ -118,10 +96,10 @@ top_go_drop (void)
position_t robot_pos;
asserv_get_position (&robot_pos);
uint8_t drop_pos_id = 37;
- position_t drop_pos = element_get_pos (drop_pos_id);
+ vect_t drop_pos = element_get_pos (drop_pos_id);
uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD
? 0 : ASSERV_BACKWARD;
- move_start_noangle (drop_pos.v, backward, 0);
+ move_start_noangle (drop_pos, backward, 0);
return 0;
}
@@ -137,79 +115,36 @@ top_decision (void)
FSM_TRANS (TOP_GOING_OUT2, robot_move_success,
drop, TOP_GOING_TO_DROP,
- element, TOP_GOING_TO_ELEMENT,
- green_element, TOP_GOING_TO_GREEN_POS)
+ element, TOP_GOING_TO_ELEMENT)
{
switch (top_decision ())
{
default: return FSM_NEXT (TOP_GOING_OUT2, robot_move_success, drop);
case 1: return FSM_NEXT (TOP_GOING_OUT2, robot_move_success, element);
- case 2: return FSM_NEXT (TOP_GOING_OUT2, robot_move_success,
- green_element);
}
}
FSM_TRANS (TOP_GOING_TO_DROP, move_success,
drop, TOP_GOING_TO_DROP,
- element, TOP_GOING_TO_ELEMENT,
- green_element, TOP_GOING_TO_GREEN_POS)
+ element, TOP_GOING_TO_ELEMENT)
{
clamp_drop (logistic_global.collect_direction);
switch (top_decision ())
{
default: return FSM_NEXT (TOP_GOING_TO_DROP, move_success, drop);
case 1: return FSM_NEXT (TOP_GOING_TO_DROP, move_success, element);
- case 2: return FSM_NEXT (TOP_GOING_TO_DROP, move_success,
- green_element);
}
}
FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success,
drop, TOP_GOING_TO_DROP,
- element, TOP_GOING_TO_ELEMENT,
- green_element, TOP_GOING_TO_GREEN_POS)
+ element, TOP_GOING_TO_ELEMENT)
{
+ element_taken (ctx.target_element_id, ELEMENT_PAWN);
switch (top_decision ())
{
default: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, drop);
case 1: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, element);
- case 2: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success,
- green_element);
- }
-}
-
-FSM_TRANS (TOP_GOING_TO_GREEN_POS, move_success, TOP_GOING_TO_GREEN_ELEMENT)
-{
- asserv_move_linearly (ctx.green_move_distance_mm);
- return FSM_NEXT (TOP_GOING_TO_GREEN_POS, move_success);
-}
-
-FSM_TRANS (TOP_GOING_TO_GREEN_ELEMENT, robot_move_success,
- TOP_TAKING_GREEN_ELEMENT)
-{
- element_taken (ctx.target_element_id, ELEMENT_PAWN);
- return FSM_NEXT (TOP_GOING_TO_GREEN_ELEMENT, robot_move_success);
-}
-
-FSM_TRANS_TIMEOUT (TOP_TAKING_GREEN_ELEMENT, 250, TOP_GOING_FROM_GREEN_ELEMENT)
-{
- asserv_move_linearly (-ctx.green_move_distance_mm);
- return FSM_NEXT_TIMEOUT (TOP_TAKING_GREEN_ELEMENT);
-}
-
-FSM_TRANS (TOP_GOING_FROM_GREEN_ELEMENT, robot_move_success,
- drop, TOP_GOING_TO_DROP,
- element, TOP_GOING_TO_ELEMENT,
- green_element, TOP_GOING_TO_GREEN_POS)
-{
- switch (top_decision ())
- {
- default: return FSM_NEXT (TOP_GOING_FROM_GREEN_ELEMENT,
- robot_move_success, drop);
- case 1: return FSM_NEXT (TOP_GOING_FROM_GREEN_ELEMENT,
- robot_move_success, element);
- case 2: return FSM_NEXT (TOP_GOING_FROM_GREEN_ELEMENT,
- robot_move_success, green_element);
}
}