summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
authorJérémy Dufour2008-05-03 12:43:19 +0200
committerJérémy Dufour2008-05-03 12:43:19 +0200
commit23bce6a75a17b9cd9d3f35a251baa1b0635697a4 (patch)
treecc53e275464e9e86cf6fe705497d0212fea960f1 /digital
parent1bcff7dbcbb284f5564ac708292b23fd447395a9 (diff)
* digital/io/src
- manage the event where the bot can be blocked (when moving) in the different FSM, by waiting and trying again ; - ensure we update the classifier (with the servos motor) even when the arm move in a state where it should not.
Diffstat (limited to 'digital')
-rw-r--r--digital/io/src/getsamples.c4
-rw-r--r--digital/io/src/getsamples.fsm11
-rw-r--r--digital/io/src/getsamples.h4
-rw-r--r--digital/io/src/getsamples_cb.c112
-rw-r--r--digital/io/src/gutter.fsm9
-rw-r--r--digital/io/src/gutter_cb.c31
-rw-r--r--digital/io/src/main.c2
-rw-r--r--digital/io/src/move_cb.c16
-rw-r--r--digital/io/src/top_cb.c6
9 files changed, 169 insertions, 26 deletions
diff --git a/digital/io/src/getsamples.c b/digital/io/src/getsamples.c
index ead49ecc..c1a7dc60 100644
--- a/digital/io/src/getsamples.c
+++ b/digital/io/src/getsamples.c
@@ -35,11 +35,13 @@ struct getsamples_data_t getsamples_data_;
/* Start the get samples FSM. */
void
-getsamples_start (int16_t approach_angle, uint8_t sample_bitfield)
+getsamples_start (int16_t approach_angle, uint8_t sample_bitfield, uint8_t
+ direction)
{
/* Set parameters */
getsamples_data_.approach_angle = approach_angle;
getsamples_data_.sample_bitfield = sample_bitfield;
+ getsamples_data_.direction = direction;
/* Remove unhandled traps */
getsamples_data_.sample_bitfield &= (_BV (trap_count) - 1) & ~_BV (garbage);
diff --git a/digital/io/src/getsamples.fsm b/digital/io/src/getsamples.fsm
index 175a1824..4b54c5f8 100644
--- a/digital/io/src/getsamples.fsm
+++ b/digital/io/src/getsamples.fsm
@@ -25,6 +25,8 @@ States:
put the arm back in front of the input hole to prevent sample to enter
MOVE_BACKWARD_FROM_DISTRIBUTOR
move a little bit backward from the distributor
+ WAIT_AND_TRY_AGAIN
+ wait and try again
Events:
start
@@ -69,8 +71,10 @@ TAKE_SAMPLES:
take a new one
reset the timeout
arm_pass_noted_position: no_more -> MOVE_AWAY_FROM_DISTRIBUTOR
+ store current position
go backward
wait_finished -> MOVE_AWAY_FROM_DISTRIBUTOR
+ store current position
timed out, give up
go backward
@@ -78,6 +82,8 @@ MOVE_AWAY_FROM_DISTRIBUTOR:
bot_move_succeed -> CLOSE_INPUT_HOLE
close input hole
setup a close timeout
+ bot_move_failed -> WAIT_AND_TRY_AGAIN
+ ask to be wake up in a certain time
arm_pass_noted_position -> .
prepare the classification of the taken sample
@@ -89,3 +95,8 @@ CLOSE_INPUT_HOLE:
tell the top FSM we have finished
arm_pass_noted_position -> .
prepare the classification of the taken sample
+
+WAIT_AND_TRY_AGAIN:
+ wait_finished -> MOVE_AWAY_FROM_DISTRIBUTOR
+ compute remaining distance (protection agains 0 & 300)
+ try to move away again
diff --git a/digital/io/src/getsamples.h b/digital/io/src/getsamples.h
index e927397e..2adf0cb4 100644
--- a/digital/io/src/getsamples.h
+++ b/digital/io/src/getsamples.h
@@ -47,6 +47,7 @@ struct getsamples_data_t
uint8_t sample_bitfield;
/** Position of the arm where we want to be awken. */
uint16_t arm_noted_position;
+ uint8_t direction;
};
@@ -60,6 +61,7 @@ struct getsamples_data_t
* used to know the number of samples to collect from the distributor.
*/
void
-getsamples_start (int16_t approach_angle, uint8_t sample_bitfield);
+getsamples_start (int16_t approach_angle, uint8_t sample_bitfield, uint8_t
+ direction);
#endif /* getsamples_h */
diff --git a/digital/io/src/getsamples_cb.c b/digital/io/src/getsamples_cb.c
index af7b6ced..d42f05bd 100644
--- a/digital/io/src/getsamples_cb.c
+++ b/digital/io/src/getsamples_cb.c
@@ -36,9 +36,13 @@
#include "chrono.h"
#include "modules/proto/proto.h"
+#include "modules/utils/utils.h"
#include "io.h"
+
+uint16_t originate_position_;
+
/**
* The distance to go backward from the distributor.
*/
@@ -50,6 +54,11 @@
#define GET_SAMPLES_ARM_TIMEOUT (5 * 225)
/**
+ * Move back timeout.
+ */
+#define GET_SAMPLES_MOVE_AWAY_TIMEOUT (225)
+
+/**
* 'Private' get samples data used internaly by the FSM.
*/
extern struct getsamples_data_t getsamples_data_;
@@ -160,6 +169,19 @@ getsamples__CLOSE_INPUT_HOLE__wait_finished (void)
}
/*
+ * CLOSE_INPUT_HOLE =arm_move_succeed=>
+ * => IDLE
+ * tell the top FSM we have finished
+ */
+fsm_branch_t
+getsamples__CLOSE_INPUT_HOLE__arm_move_succeed (void)
+{
+ /* Tell the top FSM we have finished */
+ main_post_event_for_top_fsm = TOP_EVENT_get_samples_fsm_finished;
+ return getsamples_next (CLOSE_INPUT_HOLE, arm_move_succeed);
+}
+
+/*
* CLOSE_INPUT_HOLE =arm_pass_noted_position=>
* => CLOSE_INPUT_HOLE
* prepare the classification of the taken sample
@@ -173,27 +195,62 @@ getsamples__CLOSE_INPUT_HOLE__arm_pass_noted_position (void)
}
/*
- * CLOSE_INPUT_HOLE =arm_move_succeed=>
- * => IDLE
- * tell the top FSM we have finished
+ * WAIT_AND_TRY_AGAIN =wait_finished=>
+ * => MOVE_AWAY_FROM_DISTRIBUTOR
+ * compute remaining distance (protection agains 0 & 300)
+ * try to move away again
*/
fsm_branch_t
-getsamples__CLOSE_INPUT_HOLE__arm_move_succeed (void)
+getsamples__WAIT_AND_TRY_AGAIN__wait_finished (void)
{
- /* Tell the top FSM we have finished */
- main_post_event_for_top_fsm = TOP_EVENT_get_samples_fsm_finished;
- return getsamples_next (CLOSE_INPUT_HOLE, arm_move_succeed);
+ /* Get position */
+ asserv_position_t position;
+ asserv_get_position (&position);
+
+ uint16_t remaining_distance;
+
+ if (getsamples_data_.direction == 0)
+ {
+ /* Horizontal */
+ remaining_distance = UTILS_ABS (position.x - originate_position_);
+ }
+ else
+ {
+ /* Vertical */
+ remaining_distance = UTILS_ABS (position.y - originate_position_);
+ }
+ /* Compute real remaining */
+ remaining_distance = PG_DISTANCE_DISTRIBUTOR - remaining_distance;
+ /* Bound */
+ UTILS_BOUND (remaining_distance, 1, PG_DISTANCE_DISTRIBUTOR);
+ /* Move away */
+ asserv_move_linearly (-remaining_distance);
+ return getsamples_next (WAIT_AND_TRY_AGAIN, wait_finished);
}
/*
* TAKE_SAMPLES =wait_finished=>
* => MOVE_AWAY_FROM_DISTRIBUTOR
+ * store current position
* timed out, give up
* go backward
*/
fsm_branch_t
getsamples__TAKE_SAMPLES__wait_finished (void)
{
+ /* Get position */
+ asserv_position_t position;
+ asserv_get_position (&position);
+ if (getsamples_data_.direction == 0)
+ {
+ /* Horizontal */
+ originate_position_ = position.x;
+ }
+ else
+ {
+ /* Vertical */
+ originate_position_ = position.y;
+ }
/* Go backward */
asserv_move_linearly (-PG_DISTANCE_DISTRIBUTOR);
proto_send1b ('M', 1);
@@ -203,6 +260,7 @@ getsamples__TAKE_SAMPLES__wait_finished (void)
/*
* TAKE_SAMPLES =arm_pass_noted_position=>
* no_more => MOVE_AWAY_FROM_DISTRIBUTOR
+ * store current position
* go backward
* more => TAKE_SAMPLES
* prepare the classification of the taken sample
@@ -228,6 +286,19 @@ getsamples__TAKE_SAMPLES__arm_pass_noted_position (void)
}
else
{
+ /* Get position */
+ asserv_position_t position;
+ asserv_get_position (&position);
+ if (getsamples_data_.direction == 0)
+ {
+ /* Horizontal */
+ originate_position_ = position.x;
+ }
+ else
+ {
+ /* Vertical */
+ originate_position_ = position.y;
+ }
/* Go backward */
asserv_move_linearly (-PG_DISTANCE_DISTRIBUTOR);
proto_send1b ('M', 0);
@@ -250,16 +321,16 @@ getsamples__IDLE__start (void)
}
/*
- * MOVE_AWAY_FROM_DISTRIBUTOR =arm_pass_noted_position=>
- * => MOVE_AWAY_FROM_DISTRIBUTOR
- * prepare the classification of the taken sample
+ * MOVE_AWAY_FROM_DISTRIBUTOR =bot_move_failed=>
+ * => WAIT_AND_TRY_AGAIN
+ * ask to be wake up in a certain time
*/
fsm_branch_t
-getsamples__MOVE_AWAY_FROM_DISTRIBUTOR__arm_pass_noted_position (void)
+getsamples__MOVE_AWAY_FROM_DISTRIBUTOR__bot_move_failed (void)
{
- /* Prepare the classification of the taken sample */
- getsamples_configure_classifier ();
- return getsamples_next (MOVE_AWAY_FROM_DISTRIBUTOR, arm_pass_noted_position);
+ /* Post an event for the top FSM to be waked up later */
+ main_getsamples_wait_cycle = GET_SAMPLES_MOVE_AWAY_TIMEOUT;
+ return getsamples_next (MOVE_AWAY_FROM_DISTRIBUTOR, bot_move_failed);
}
/*
@@ -280,6 +351,19 @@ getsamples__MOVE_AWAY_FROM_DISTRIBUTOR__bot_move_succeed (void)
}
/*
+ * MOVE_AWAY_FROM_DISTRIBUTOR =arm_pass_noted_position=>
+ * => MOVE_AWAY_FROM_DISTRIBUTOR
+ * prepare the classification of the taken sample
+ */
+fsm_branch_t
+getsamples__MOVE_AWAY_FROM_DISTRIBUTOR__arm_pass_noted_position (void)
+{
+ /* Prepare the classification of the taken sample */
+ getsamples_configure_classifier ();
+ return getsamples_next (MOVE_AWAY_FROM_DISTRIBUTOR, arm_pass_noted_position);
+}
+
+/*
* APPROACH_DISTRIBUTOR =bot_move_succeed=>
* => MOVE_BACKWARD_FROM_DISTRIBUTOR
* move a little bit backward from the distributor
diff --git a/digital/io/src/gutter.fsm b/digital/io/src/gutter.fsm
index 966ee8f0..97163cc0 100644
--- a/digital/io/src/gutter.fsm
+++ b/digital/io/src/gutter.fsm
@@ -11,6 +11,8 @@ States:
make the bot reversing against the gutter
DROP_BALLS
open the collector to let the balls fall into the gutter
+ WAIT_AND_TRY_AGAIN
+ ask the top FSM to wake us up in a few times
Events:
start
@@ -38,9 +40,14 @@ GO_TO_THE_GUTTER_WALL:
# preventing from fucking the wall
# The best way is probably to abort this FSM with an error, tell the upper one
# (top) we need to try at another place (the gutter is quite long).
-# bot_move_failed
+ bot_move_failed -> WAIT_AND_TRY_AGAIN
+ ask the top FSM to wake us up in a few times
DROP_BALLS:
wait_finished -> IDLE
close the rear panel
tell the top FSM we have finished
+
+WAIT_AND_TRY_AGAIN:
+ wait_finished -> GO_TO_THE_GUTTER_WALL
+ try the fuck the wall again
diff --git a/digital/io/src/gutter_cb.c b/digital/io/src/gutter_cb.c
index 8d11a63a..96f5c605 100644
--- a/digital/io/src/gutter_cb.c
+++ b/digital/io/src/gutter_cb.c
@@ -42,6 +42,11 @@ uint16_t gutter_wait_cycle_;
*/
#define GUTTER_WAIT_FOR_BALLS_TO_DROP 300
+/**
+ * Time to wait before trying the go to the wall again.
+ */
+#define GUTTER_WAIT_BEFORE_TRY_AGAIN (225)
+
/*
* ROTATE_REAR_SIDE_TO_GUTTER =bot_move_succeed=>
* => GO_TO_THE_GUTTER_WALL
@@ -56,6 +61,19 @@ gutter__ROTATE_REAR_SIDE_TO_GUTTER__bot_move_succeed (void)
}
/*
+ * WAIT_AND_TRY_AGAIN =wait_finished=>
+ * => GO_TO_THE_GUTTER_WALL
+ * try the fuck the wall again
+ */
+fsm_branch_t
+gutter__WAIT_AND_TRY_AGAIN__wait_finished (void)
+{
+ /* Make the bot reversing against the gutter */
+ asserv_go_to_the_wall ();
+ return gutter_next (WAIT_AND_TRY_AGAIN, wait_finished);
+}
+
+/*
* IDLE =start=>
* => ROTATE_REAR_SIDE_TO_GUTTER
* put the bot back to the gutter
@@ -71,6 +89,18 @@ gutter__IDLE__start (void)
}
/*
+ * GO_TO_THE_GUTTER_WALL =bot_move_failed=>
+ * => WAIT_AND_TRY_AGAIN
+ * ask the top FSM to wake us up in a few times
+ */
+fsm_branch_t
+gutter__GO_TO_THE_GUTTER_WALL__bot_move_failed (void)
+{
+ main_getsamples_wait_cycle = GUTTER_WAIT_BEFORE_TRY_AGAIN;
+ return gutter_next (GO_TO_THE_GUTTER_WALL, bot_move_failed);
+}
+
+/*
* GO_TO_THE_GUTTER_WALL =bot_move_succeed=>
* => DROP_BALLS
* open the collector to drop the balls
@@ -91,6 +121,7 @@ gutter__GO_TO_THE_GUTTER_WALL__bot_move_succeed (void)
* DROP_BALLS =wait_finished=>
* => IDLE
* close the rear panel
+ * tell the top FSM we have finished
*/
fsm_branch_t
gutter__DROP_BALLS__wait_finished (void)
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index 844e0d1d..a24e7e14 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -518,7 +518,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - 1b: the approach angle to face the distributor ;
* - 1b: how many and where to put collected samples ;
*/
- getsamples_start (args[0] << 8, args[1]);
+ getsamples_start (args[0] << 8, args[1], 0);
break;
case c ('A', 1):
diff --git a/digital/io/src/move_cb.c b/digital/io/src/move_cb.c
index e85a4ef7..c55bfa30 100644
--- a/digital/io/src/move_cb.c
+++ b/digital/io/src/move_cb.c
@@ -157,7 +157,7 @@ move_compute_obstacle_position (asserv_position_t cur,
/* Convert the angle */
uint32_t angle = cur.a;
/* Change angle when going backward */
- if (asserv_get_moving_direction () == 2)
+ if (0)
angle += 0x8000;
angle = angle << 8;
DPRINTF ("We are at (%d ; %d ; %x)\n", cur.x, cur.y, cur.a);
@@ -236,7 +236,8 @@ move_after_moving_backward (void)
fsm_branch_t
move__WAIT_FOR_CLEAR_PATH__wait_finished (void)
{
- if (!sharp_path_obstrued (move_data.cached_moving_direction))
+/// if (!sharp_path_obstrued (move_data.cached_moving_direction))
+ if (!sharp_path_obstrued (1))
{
/* Try to go to the final position */
if (move_data.backward_movement_allowed)
@@ -264,12 +265,12 @@ move__WAIT_FOR_CLEAR_PATH__wait_finished (void)
else
{
/* Store current moving direction */
- move_data.cached_moving_direction = asserv_get_moving_direction ();
+ move_data.cached_moving_direction = asserv_get_moving_direction ();
/* Stop the bot */
asserv_stop_motor ();
/* Post an event for the top FSM to be waked up later */
main_move_wait_cycle = MOVE_WAIT_TIME_FOR_POOLING_SHARP;
- return move_next_branch (WAIT_FOR_CLEAR_PATH, wait_finished, obstacle_and_intermediate_path_found);
+ return move_next_branch (WAIT_FOR_CLEAR_PATH, wait_finished, obstacle_and_no_intermediate_path_found);
}
}
}
@@ -411,6 +412,8 @@ move__MOVING_BACKWARD__bot_move_failed (void)
}
else
{
+ /* Store current moving direction */
+ move_data.cached_moving_direction = 2;
/* Stop the bot */
asserv_stop_motor ();
/* Post an event for the top FSM to be waked up later */
@@ -438,6 +441,8 @@ move__MOVING_BACKWARD__bot_move_succeed (void)
}
else
{
+ /* Store current moving direction */
+ move_data.cached_moving_direction = asserv_get_moving_direction ();
/* Stop the bot */
asserv_stop_motor ();
/* Post an event for the top FSM to be waked up later */
@@ -459,7 +464,8 @@ move__MOVING_TO_FINAL_POSITION__bot_move_failed (void)
/* Compute obstacle position */
move_obstacle_here ();
/* Store current moving direction */
- move_data.cached_moving_direction = asserv_get_moving_direction ();
+ move_data.cached_moving_direction = move_data.backward_movement_allowed ?
+ 2 : 1;
/* Move backward to turn freely */
asserv_move_linearly (-PG_MOVE_DISTANCE);
return move_next (MOVING_TO_FINAL_POSITION, bot_move_failed);
diff --git a/digital/io/src/top_cb.c b/digital/io/src/top_cb.c
index 2710543d..9383d887 100644
--- a/digital/io/src/top_cb.c
+++ b/digital/io/src/top_cb.c
@@ -145,7 +145,7 @@ top__GO_TO_ADVERSE_ICE_DISTRIBUTOR__move_fsm_finished (void)
* samples. The problem is this should depend on the time we have until
* the end of match */
uint8_t bitfield = 0x3E;
- getsamples_start (PG_DISTRIBUTOR_ICE_ADVERSE_A, bitfield);
+ getsamples_start (PG_DISTRIBUTOR_ICE_ADVERSE_A, bitfield, 0);
return top_next (GO_TO_ADVERSE_ICE_DISTRIBUTOR, move_fsm_finished);
}
@@ -170,7 +170,7 @@ top__GO_TO_OUR_ICE_DISTRIBUTOR__move_fsm_finished (void)
/* Second time we try to get our ice, let's took only two */
bitfield = _BV (middle_left_box) | _BV (middle_right_box);
}
- getsamples_start (PG_DISTRIBUTOR_ICE_OUR_A, bitfield);
+ getsamples_start (PG_DISTRIBUTOR_ICE_OUR_A, bitfield, 0);
return top_next (GO_TO_OUR_ICE_DISTRIBUTOR, move_fsm_finished);
}
@@ -195,7 +195,7 @@ top__GO_TO_SAMPLE_DISTRIBUTOR__move_fsm_finished (void)
bitfield = _BV (out_left_box) | _BV (middle_box) | _BV
(out_right_box);
}
- getsamples_start (PG_DISTRIBUTOR_SAMPLE_OUR_A, bitfield);
+ getsamples_start (PG_DISTRIBUTOR_SAMPLE_OUR_A, bitfield, 1);
return top_next (GO_TO_SAMPLE_DISTRIBUTOR, move_fsm_finished);
}