summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorJérémy Dufour2008-05-03 11:15:31 +0200
committerJérémy Dufour2008-05-03 11:15:31 +0200
commit1bcff7dbcbb284f5564ac708292b23fd447395a9 (patch)
tree2cc5f47226bd7155f9ed575d1a76593b6f14a802 /digital
parentd803fd8ba61dba29c904ee1fccfab2b7177834d4 (diff)
* digital/io/src
* top FSM - change the position where to drop balls at the gutter (to prevent the order of dropped balls to change) ; - change the order of balls taken at the distributor: first two samples with three ice, then three samples with two ice. * get samples FSM - manage the case where the arm passed the noted position in the state where it should not (because the arm was blocked) ; - add some debug messages. * gutter FSM - reduce the time spent to drop the balls at the gutter. * move FSM - update the algorithm to avoid obstacle: when a obstacle is seen, first way, then if it is not present anymore continue, otherwise, try to find an alternative path; if none found, wait again. * chrono - near end of the match time increased to 83s.
Diffstat (limited to 'digital')
-rw-r--r--digital/io/src/chrono.c2
-rw-r--r--digital/io/src/getsamples.fsm4
-rw-r--r--digital/io/src/getsamples_cb.c39
-rw-r--r--digital/io/src/gutter_cb.c2
-rw-r--r--digital/io/src/main.c2
-rw-r--r--digital/io/src/move.c2
-rw-r--r--digital/io/src/move.fsm14
-rw-r--r--digital/io/src/move_cb.c43
-rw-r--r--digital/io/src/playground.h2
-rw-r--r--digital/io/src/top_cb.c23
10 files changed, 104 insertions, 29 deletions
diff --git a/digital/io/src/chrono.c b/digital/io/src/chrono.c
index 051f8670..552232a3 100644
--- a/digital/io/src/chrono.c
+++ b/digital/io/src/chrono.c
@@ -60,7 +60,7 @@
* CHRONO_OVERFLOW_MAX * seconds / 90
*/
#define CHRONO_OVERFLOW_COUNT_NEAR_END_MATCH \
- ((uint8_t) (CHRONO_OVERFLOW_MAX * 65 / 90))
+ ((uint8_t) (CHRONO_OVERFLOW_MAX * 83 / 90))
/**
* Match is finished.
diff --git a/digital/io/src/getsamples.fsm b/digital/io/src/getsamples.fsm
index 0079d458..175a1824 100644
--- a/digital/io/src/getsamples.fsm
+++ b/digital/io/src/getsamples.fsm
@@ -78,6 +78,8 @@ MOVE_AWAY_FROM_DISTRIBUTOR:
bot_move_succeed -> CLOSE_INPUT_HOLE
close input hole
setup a close timeout
+ arm_pass_noted_position -> .
+ prepare the classification of the taken sample
CLOSE_INPUT_HOLE:
arm_move_succeed -> IDLE
@@ -85,3 +87,5 @@ CLOSE_INPUT_HOLE:
wait_finished -> IDLE
timed out, give up
tell the top FSM we have finished
+ arm_pass_noted_position -> .
+ prepare the classification of the taken sample
diff --git a/digital/io/src/getsamples_cb.c b/digital/io/src/getsamples_cb.c
index 1ba50ac1..af7b6ced 100644
--- a/digital/io/src/getsamples_cb.c
+++ b/digital/io/src/getsamples_cb.c
@@ -35,6 +35,8 @@
#include "main.h" /* main_post_event_for_top_fsm */
#include "chrono.h"
+#include "modules/proto/proto.h"
+
#include "io.h"
/**
@@ -92,6 +94,7 @@ getsamples_count_samples (void)
count++;
return count;
}
+
/*
* MOVE_BACKWARD_FROM_DISTRIBUTOR =bot_move_succeed=>
* => TAKE_SAMPLES
@@ -109,6 +112,7 @@ getsamples__MOVE_BACKWARD_FROM_DISTRIBUTOR__bot_move_succeed (void)
BOT_ARM_SPEED);
/* Post an event for the top FSM to be waked up later */
main_getsamples_wait_cycle = GET_SAMPLES_ARM_TIMEOUT;
+ proto_send1b ('T', 0);
return getsamples_next (MOVE_BACKWARD_FROM_DISTRIBUTOR, bot_move_succeed);
}
@@ -122,6 +126,7 @@ getsamples__FACE_DISTRIBUTOR__bot_move_succeed (void)
{
/* Move the arm to open the input hole to be able to take some samples */
asserv_move_arm (-BOT_ARM_MIN_TO_OPEN, BOT_ARM_SPEED);
+ proto_send0 ('O');
return getsamples_next (FACE_DISTRIBUTOR, bot_move_succeed);
}
@@ -135,6 +140,7 @@ getsamples__OPEN_INPUT_HOLE__arm_move_succeed (void)
{
/* Approach the distributor */
asserv_go_to_distributor ();
+ proto_send0 ('A');
return getsamples_next (OPEN_INPUT_HOLE, arm_move_succeed);
}
@@ -149,10 +155,24 @@ getsamples__CLOSE_INPUT_HOLE__wait_finished (void)
{
/* Give up, tell the top FSM we have finished */
main_post_event_for_top_fsm = TOP_EVENT_get_samples_fsm_finished;
+ proto_send0 ('I');
return getsamples_next (CLOSE_INPUT_HOLE, wait_finished);
}
/*
+ * CLOSE_INPUT_HOLE =arm_pass_noted_position=>
+ * => CLOSE_INPUT_HOLE
+ * prepare the classification of the taken sample
+ */
+fsm_branch_t
+getsamples__CLOSE_INPUT_HOLE__arm_pass_noted_position (void)
+{
+ /* Prepare the classification of the taken sample */
+ getsamples_configure_classifier ();
+ return getsamples_next (CLOSE_INPUT_HOLE, arm_pass_noted_position);
+}
+
+/*
* CLOSE_INPUT_HOLE =arm_move_succeed=>
* => IDLE
* tell the top FSM we have finished
@@ -176,6 +196,7 @@ getsamples__TAKE_SAMPLES__wait_finished (void)
{
/* Go backward */
asserv_move_linearly (-PG_DISTANCE_DISTRIBUTOR);
+ proto_send1b ('M', 1);
return getsamples_next (TAKE_SAMPLES, wait_finished);
}
@@ -202,12 +223,14 @@ getsamples__TAKE_SAMPLES__arm_pass_noted_position (void)
/* Post an event for the top FSM to be waked up later */
main_getsamples_wait_cycle = GET_SAMPLES_ARM_TIMEOUT;
/* Continue to take sample */
+ proto_send1b ('T', 1);
return getsamples_next_branch (TAKE_SAMPLES, arm_pass_noted_position, more);
}
else
{
/* Go backward */
asserv_move_linearly (-PG_DISTANCE_DISTRIBUTOR);
+ proto_send1b ('M', 0);
return getsamples_next_branch (TAKE_SAMPLES, arm_pass_noted_position, no_more);
}
}
@@ -222,10 +245,24 @@ getsamples__IDLE__start (void)
{
/* Face the distributor */
asserv_goto_angle (getsamples_data_.approach_angle);
+ proto_send0 ('F');
return getsamples_next (IDLE, start);
}
/*
+ * 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);
+}
+
+/*
* MOVE_AWAY_FROM_DISTRIBUTOR =bot_move_succeed=>
* => CLOSE_INPUT_HOLE
* close input hole
@@ -238,6 +275,7 @@ getsamples__MOVE_AWAY_FROM_DISTRIBUTOR__bot_move_succeed (void)
asserv_move_arm (BOT_ARM_MIN_TO_OPEN + BOT_ARM_THIRD_ROUND, BOT_ARM_SPEED);
/* Post an event for the top FSM to be waked up later */
main_getsamples_wait_cycle = GET_SAMPLES_ARM_TIMEOUT;
+ proto_send0 ('C');
return getsamples_next (MOVE_AWAY_FROM_DISTRIBUTOR, bot_move_succeed);
}
@@ -251,5 +289,6 @@ getsamples__APPROACH_DISTRIBUTOR__bot_move_succeed (void)
{
/* Move a little bit backward from the distributor */
asserv_move_linearly (-GET_SAMPLES_MOVE_BACKWARD_DISTANCE);
+ proto_send0 ('m');
return getsamples_next (APPROACH_DISTRIBUTOR, bot_move_succeed);
}
diff --git a/digital/io/src/gutter_cb.c b/digital/io/src/gutter_cb.c
index 5abf7bcb..8d11a63a 100644
--- a/digital/io/src/gutter_cb.c
+++ b/digital/io/src/gutter_cb.c
@@ -40,7 +40,7 @@ uint16_t gutter_wait_cycle_;
* Count of cycles to wait before we estimate all the balls have been dropped
* into the gutter. A cycle normally last 4.4ms.
*/
-#define GUTTER_WAIT_FOR_BALLS_TO_DROP 1126
+#define GUTTER_WAIT_FOR_BALLS_TO_DROP 300
/*
* ROTATE_REAR_SIDE_TO_GUTTER =bot_move_succeed=>
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index 3c045368..844e0d1d 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -78,6 +78,7 @@ uint16_t main_sharp_ignore_event;
/**
* Flag for homologation, to disable the path finding and always stop in front
* of an obstacle and wait.
+ * Do not touch anymore ! XXX FIXME TODO
*/
uint8_t main_always_stop_for_obstacle = 1;
@@ -623,6 +624,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Omo-logo-ation flag, to prevent avoiding obstacle and stop
* instead.
* - 1b: state of the flag (0 to disable, 1 to enable).
+ * Do not touch anymore ! XXX FIXME TODO
*/
main_always_stop_for_obstacle = args[0];
}
diff --git a/digital/io/src/move.c b/digital/io/src/move.c
index 005a8fbc..c30e5ace 100644
--- a/digital/io/src/move.c
+++ b/digital/io/src/move.c
@@ -48,6 +48,8 @@ move_start (uint16_t position_x, uint16_t position_y, uint8_t
move_data.final.x = position_x;
move_data.final.y = position_y;
move_data.backward_movement_allowed = backward_movement_allowed;
+ /* XXX */
+ main_always_stop_for_obstacle = 1;
/* Reset move FSM flags */
main_sharp_ignore_event = 0;
main_move_wait_cycle = 0;
diff --git a/digital/io/src/move.fsm b/digital/io/src/move.fsm
index 317f25d8..c08d0454 100644
--- a/digital/io/src/move.fsm
+++ b/digital/io/src/move.fsm
@@ -91,9 +91,13 @@ MOVING_TO_INTERMEDIATE_POSITION:
move backward to turn freely
WAIT_FOR_CLEAR_PATH:
- wait_finished: obstacle -> .
- check for obstacle using stored moving direction
- post an event for the top FSM to be waked up later
wait_finished: no_obstacle -> MOVING_TO_FINAL_POSITION
- check for obstacle using stored moving direction
- try to go the final position
+ ask the asserv to go to the final position
+ wait_finished: obstacle_and_intermediate_path_found -> MOVING_TO_INTERMEDIATE_POSITION
+ compute the obstacle position
+ get next intermediate position from path module
+ go to next intermediate position
+ wait_finished: obstacle_and_no_intermediate_path_found -> .
+ compute the obstacle position
+ get next intermediate position from path module failed
+ post an event for the top FSM to be waked up later
diff --git a/digital/io/src/move_cb.c b/digital/io/src/move_cb.c
index 0de7ea41..e85a4ef7 100644
--- a/digital/io/src/move_cb.c
+++ b/digital/io/src/move_cb.c
@@ -68,7 +68,7 @@
/**
* The generic validity time (in term of number of cyles).
*/
-#define MOVE_OBSTACLE_VALIDITY (4 * 225)
+#define MOVE_OBSTACLE_VALIDITY (6 * 225)
/**
* Cycles count to ignore sharp event in the main loop.
@@ -79,7 +79,7 @@
* Number of cycles to wait before trying to read the sharps values again when
* we are stopped.
*/
-#define MOVE_WAIT_TIME_FOR_POOLING_SHARP (50)
+#define MOVE_WAIT_TIME_FOR_POOLING_SHARP (MOVE_MAIN_IGNORE_SHARP_EVENT)
/**
* A detection offset for the sharps.
@@ -223,11 +223,15 @@ move_after_moving_backward (void)
/*
* WAIT_FOR_CLEAR_PATH =wait_finished=>
* no_obstacle => MOVING_TO_FINAL_POSITION
- * check for obstacle using stored moving direction
- * try to go the final position
- * obstacle => WAIT_FOR_CLEAR_PATH
- * check for obstacle using stored moving direction
+ * ask the asserv to go to the final position
+ * obstacle_and_no_intermediate_path_found => WAIT_FOR_CLEAR_PATH
+ * compute the obstacle position
+ * get next intermediate position from path module failed
* post an event for the top FSM to be waked up later
+ * obstacle_and_intermediate_path_found => MOVING_TO_INTERMEDIATE_POSITION
+ * compute the obstacle position
+ * get next intermediate position from path module
+ * go to next intermediate position
*/
fsm_branch_t
move__WAIT_FOR_CLEAR_PATH__wait_finished (void)
@@ -243,9 +247,30 @@ move__WAIT_FOR_CLEAR_PATH__wait_finished (void)
}
else
{
- /* 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);
+ /* Enable path finding */
+ main_always_stop_for_obstacle = 0;
+ /* Get next position */
+ if (path_get_next (&move_data.intermediate.x, &move_data.intermediate.y))
+ {
+ /* Ignore sharps */
+ main_sharp_ignore_event = MOVE_MAIN_IGNORE_SHARP_EVENT;
+ /* Go to the next intermediate position */
+ if (move_data.backward_movement_allowed)
+ asserv_goto_back (move_data.intermediate.x, move_data.intermediate.y);
+ else
+ asserv_goto (move_data.intermediate.x, move_data.intermediate.y);
+ return move_next_branch (WAIT_FOR_CLEAR_PATH, wait_finished, obstacle_and_intermediate_path_found);
+ }
+ 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 */
+ 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);
+ }
}
}
diff --git a/digital/io/src/playground.h b/digital/io/src/playground.h
index ae97784e..6659d32b 100644
--- a/digital/io/src/playground.h
+++ b/digital/io/src/playground.h
@@ -110,7 +110,7 @@
/**
* The position of the gutter.
*/
-#define PG_GUTTER_X (PG_X_VALUE_COMPUTING (2250))
+#define PG_GUTTER_X (PG_X_VALUE_COMPUTING (2600))
#define PG_GUTTER_Y (300)
#define PG_GUTTER_A (90 * BOT_ANGLE_DEGREE)
diff --git a/digital/io/src/top_cb.c b/digital/io/src/top_cb.c
index 0cccdf38..2710543d 100644
--- a/digital/io/src/top_cb.c
+++ b/digital/io/src/top_cb.c
@@ -144,7 +144,7 @@ top__GO_TO_ADVERSE_ICE_DISTRIBUTOR__move_fsm_finished (void)
/* Start the get samples FSM with the correct angle to get only two
* samples. The problem is this should depend on the time we have until
* the end of match */
- uint8_t bitfield = _BV (out_right_box) | _BV (middle_right_box);
+ uint8_t bitfield = 0x3E;
getsamples_start (PG_DISTRIBUTOR_ICE_ADVERSE_A, bitfield);
return top_next (GO_TO_ADVERSE_ICE_DISTRIBUTOR, move_fsm_finished);
}
@@ -161,14 +161,14 @@ top__GO_TO_OUR_ICE_DISTRIBUTOR__move_fsm_finished (void)
uint8_t bitfield = 0;
if (top_fsm_loop_count_ == 0)
{
- /* First time we try to get our ice, let's took two */
- bitfield = _BV (middle_left_box) | _BV (middle_right_box);
+ /* First time we try to get our ice, let's took three */
+ bitfield = _BV (out_left_box) | _BV (middle_box) | _BV
+ (out_right_box);
}
else
{
- /* Second time we try to get our ice, let's took only three */
- bitfield = _BV (out_left_box) | _BV (middle_box) | _BV
- (out_right_box);
+ /* 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);
return top_next (GO_TO_OUR_ICE_DISTRIBUTOR, move_fsm_finished);
@@ -186,15 +186,14 @@ top__GO_TO_SAMPLE_DISTRIBUTOR__move_fsm_finished (void)
uint8_t bitfield = 0;
if (top_fsm_loop_count_ == 0)
{
- /* First time we try to get our samples, let's took three of them */
- bitfield = _BV (out_left_box) | _BV (middle_box) | _BV
- (out_right_box);
+ /* First time we try to get our samples, let's took only two of them */
+ bitfield = _BV (middle_left_box) | _BV (middle_right_box);
}
else
{
- /* Second time we try to get our samples, let's took only two of them
- */
- bitfield = _BV (middle_left_box) | _BV (middle_right_box);
+ /* Second time we try to get our samples, let's took three of them */
+ bitfield = _BV (out_left_box) | _BV (middle_box) | _BV
+ (out_right_box);
}
getsamples_start (PG_DISTRIBUTOR_SAMPLE_OUR_A, bitfield);
return top_next (GO_TO_SAMPLE_DISTRIBUTOR, move_fsm_finished);