summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorJérémy Dufour2008-04-14 12:04:01 +0200
committerJérémy Dufour2008-04-14 12:04:01 +0200
commit011fd3e4e4d252d02df156a7679c6156cdc72c6a (patch)
treed766f5ca92fc3ed04d54e8b0a9d307864dcf912f /digital
parent72a35dab91098de10cbabf72f2f8cdf45186a2ea (diff)
* digital/io/src
* asserv - add an goto_angle (not implemented yet). * get sample FSM - the get sample FSM will not call any other FSM (like the move). Instead, it assumes we are already at the right place, we just ensure the angle is correct ; - update the first state in consequences ; - add an angle as parameters (maybe we can remove the X/Y). * general defines - add define to ease the opening of the input hole.
Diffstat (limited to 'digital')
-rw-r--r--digital/io/src/asserv.c10
-rw-r--r--digital/io/src/asserv.h8
-rw-r--r--digital/io/src/getsamples.c1
-rw-r--r--digital/io/src/getsamples.fsm19
-rw-r--r--digital/io/src/getsamples.h4
-rw-r--r--digital/io/src/getsamples_cb.c35
-rw-r--r--digital/io/src/giboulee.h7
-rw-r--r--digital/io/src/test/testgetsamples/main.c35
8 files changed, 82 insertions, 37 deletions
diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c
index d7d383c4..0b801a13 100644
--- a/digital/io/src/asserv.c
+++ b/digital/io/src/asserv.c
@@ -350,6 +350,16 @@ asserv_move_angularly (int16_t angle)
asserv_twi_send_command ('a', 2);
}
+/* Move the bot to a specific angle. */
+void
+asserv_goto_angle (int16_t angle)
+{
+ /* Put angle as parameter */
+ asserv_twi_buffer_param[0] = v16_to_v8 (angle, 1);
+ asserv_twi_buffer_param[1] = v16_to_v8 (angle, 0);
+ /* TODO */
+}
+
/* Go to the wall (moving backward). */
void
asserv_go_to_the_wall (void)
diff --git a/digital/io/src/asserv.h b/digital/io/src/asserv.h
index e6bc06c4..46448a59 100644
--- a/digital/io/src/asserv.h
+++ b/digital/io/src/asserv.h
@@ -177,6 +177,14 @@ void
asserv_move_angularly (int16_t angle);
/**
+ * Move the bot to a specific angle.
+ * The angle is absolute and not a difference with the current one.
+ * @param a the absolute angle in degrees
+ */
+void
+asserv_goto_angle (int16_t angle);
+
+/**
* Go to the wall (moving backward).
* Move class command.
*/
diff --git a/digital/io/src/getsamples.c b/digital/io/src/getsamples.c
index 9feed1eb..e68857a0 100644
--- a/digital/io/src/getsamples.c
+++ b/digital/io/src/getsamples.c
@@ -41,6 +41,7 @@ getsamples_start (struct getsamples_data_t data)
/* Set parameters */
getsamples_data.distributor_x = data.distributor_x;
getsamples_data.distributor_y = data.distributor_y;
+ getsamples_data.distributor_angle = data.distributor_angle;
getsamples_data.sample_bitfield = data.sample_bitfield;
getsamples_data.event = data.event;
diff --git a/digital/io/src/getsamples.fsm b/digital/io/src/getsamples.fsm
index 4c833bc0..1fdfaa98 100644
--- a/digital/io/src/getsamples.fsm
+++ b/digital/io/src/getsamples.fsm
@@ -1,12 +1,18 @@
# Get samples FSM
+# A general remark on this FSM: it is not its responsibility to put the bot in
+# front of the distributor, it the responsibility of the top FSM using the
+# move FSM.
+# For example, the top FSM will order the bot to move in front of the
+# distributor using the move FSM. Then, when it is arrived, it will the get
+# sample FSM that will ensure we well placed (in term of angle for example).
get_samples
Get samples from the distributor
States:
IDLE
waiting for the beginning of the get samples FSM
- GO_IN_FRONT_OF_DISTRIBUTOR
- move the bot in front of a distributor (not in contact with the distributor)
+ FACE_DISTRIBUTOR
+ face the distributor
OPEN_INPUT_HOLE
move the arm to let the samples enter into the bot by the input hole
APPROACH_DISTRIBUTOR
@@ -21,6 +27,8 @@ States:
Events:
start
initialize the FSM
+# For the moment, this event is currently not managed.
+# TODO: manage it
bot_move_failed
the bot movement failed (blocked by something for example)
bot_move_succeed
@@ -31,10 +39,10 @@ Events:
the arm has just passed the 'noted' position
IDLE:
- start -> GO_IN_FRONT_OF_DISTRIBUTOR
- start going in front of the desired distributor
+ start -> FACE_DISTRIBUTOR
+ do a goto angle to make the bot facing the distributor
-GO_IN_FRONT_OF_DISTRIBUTOR:
+FACE_DISTRIBUTOR:
bot_move_succeed -> OPEN_INPUT_HOLE
move the arm to open the input hole
@@ -57,7 +65,6 @@ MOVE_AWAY_FROM_DISTRIBUTOR:
bot_move_succeed -> CLOSE_INPUT_HOLE
close input hole
-
CLOSE_INPUT_HOLE:
arm_move_succeed -> IDLE
tell the top FSM we have finished
diff --git a/digital/io/src/getsamples.h b/digital/io/src/getsamples.h
index c033665d..f7dd06a1 100644
--- a/digital/io/src/getsamples.h
+++ b/digital/io/src/getsamples.h
@@ -39,6 +39,10 @@ struct getsamples_data_t
*/
uint32_t distributor_y;
/**
+ * The angle to approach the distributor.
+ */
+ int16_t distributor_angle;
+ /**
* Bit field to indicate where to put the sample.
* If bit 0 is set to 1, a sample will be put into the out_right_box. If
* set to 0, the out_right_box will not be used to store a sample.
diff --git a/digital/io/src/getsamples_cb.c b/digital/io/src/getsamples_cb.c
index 0de105e4..5e963ce6 100644
--- a/digital/io/src/getsamples_cb.c
+++ b/digital/io/src/getsamples_cb.c
@@ -34,6 +34,19 @@
#include "playground.h" /* PG_* */
/*
+ * FACE_DISTRIBUTOR =bot_move_succeed=>
+ * => OPEN_INPUT_HOLE
+ * move the arm to open the input hole
+ */
+fsm_branch_t
+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);
+ return getsamples_next (FACE_DISTRIBUTOR, bot_move_succeed);
+}
+
+/*
* OPEN_INPUT_HOLE =arm_move_succeed=>
* => APPROACH_DISTRIBUTOR
* start approaching the distributor now
@@ -60,19 +73,6 @@ getsamples__CLOSE_INPUT_HOLE__arm_move_succeed (void)
}
/*
- * GO_IN_FRONT_OF_DISTRIBUTOR =bot_move_succeed=>
- * => OPEN_INPUT_HOLE
- * move the arm to open the input hole
- */
-fsm_branch_t
-getsamples__GO_IN_FRONT_OF_DISTRIBUTOR__bot_move_succeed (void)
-{
- /* Move the arm to open the input hole to be able to take some samples */
- asserv_close_input_hole ();
- return getsamples_next (GO_IN_FRONT_OF_DISTRIBUTOR, bot_move_succeed);
-}
-
-/*
* TAKE_SAMPLES =arm_pass_noted_position=>
* no_more => MOVE_AWAY_FROM_DISTRIBUTOR
* go backward
@@ -102,15 +102,14 @@ getsamples__TAKE_SAMPLES__arm_pass_noted_position (void)
/*
* IDLE =start=>
- * => GO_IN_FRONT_OF_DISTRIBUTOR
- * start going in front of the desired distributor
+ * => FACE_DISTRIBUTOR
+ * do a goto angle to make the bot facing the distributor
*/
fsm_branch_t
getsamples__IDLE__start (void)
{
- /* Move to the desired distributor */
- move_start (getsamples_data.distributor_x,
- getsamples_data.distributor_y);
+ /* Face the distributor */
+ asserv_goto_angle (getsamples_data.distributor_angle);
return getsamples_next (IDLE, start);
}
diff --git a/digital/io/src/giboulee.h b/digital/io/src/giboulee.h
index 45744f99..bda60c8f 100644
--- a/digital/io/src/giboulee.h
+++ b/digital/io/src/giboulee.h
@@ -48,6 +48,13 @@
#define BOT_ARM_THIRD_ROUND BOT_ARM_STEP_ROUND / 3
/**
+ * The number of steps to do to open the input hole when the arm is closing
+ * it.
+ * For the moment, 15% of a complete turn
+ */
+#define BOT_ARM_MIN_TO_OPEN (BOT_ARM_STEP_ROUND / 100 * 15)
+
+/**
* How to compute a angle for giboulee?
* One degree is 65536 / 360
*/
diff --git a/digital/io/src/test/testgetsamples/main.c b/digital/io/src/test/testgetsamples/main.c
index 76f8627f..90eeabb3 100644
--- a/digital/io/src/test/testgetsamples/main.c
+++ b/digital/io/src/test/testgetsamples/main.c
@@ -47,8 +47,8 @@ getsamples_print_test (fsm_t *getsamples)
case GETSAMPLES_STATE_IDLE:
printf ("IDLE");
break;
- case GETSAMPLES_STATE_GO_IN_FRONT_OF_DISTRIBUTOR:
- printf ("GO_IN_FRONT_OF_DISTRIBUTOR");
+ case GETSAMPLES_STATE_FACE_DISTRIBUTOR:
+ printf ("FACE_DISTRIBUTOR");
break;
case GETSAMPLES_STATE_OPEN_INPUT_HOLE:
printf ("OPEN_INPUT_HOLE");
@@ -82,6 +82,7 @@ main (void)
/* Go to our distributor */
data.distributor_x = PG_DISTRIBUTOR_SAMPLE_OUR_X;
data.distributor_y = PG_DISTRIBUTOR_SAMPLE_OUR_Y;
+ data.distributor_angle = PG_DISTRIBUTOR_SAMPLE_OUR_A;
data.sample_bitfield = 0;
/* We want to put the sample into the 0, 2 and 4 box */
data.sample_bitfield |= _BV(0);
@@ -98,9 +99,10 @@ main (void)
getsamples_print_test (&getsamples_fsm);
/* The move to the front of the distributor failed */
- fsm_handle_event (&getsamples_fsm,
- GETSAMPLES_EVENT_bot_move_failed);
- getsamples_print_test (&getsamples_fsm);
+ /* TODO: manage it! */
+// fsm_handle_event (&getsamples_fsm,
+// GETSAMPLES_EVENT_bot_move_failed);
+// getsamples_print_test (&getsamples_fsm);
/* We are in front of the distributor */
fsm_handle_event (&getsamples_fsm,
@@ -143,6 +145,9 @@ main (void)
return 0;
}
+
+static uint16_t asserv_arm_position = 0;
+
/* Define functions for debug */
void
trap_setup_path_to_box (uint8_t box_id)
@@ -151,21 +156,25 @@ trap_setup_path_to_box (uint8_t box_id)
}
void
-asserv_close_input_hole (void)
+asserv_move_linearly (int32_t distance)
{
- printf ("[asserv] Put the arm in front of the input hole.\n");
+ printf ("[asserv] Make the bot move linearly of %d mm.\n", distance);
}
void
-asserv_move_linearly (int32_t distance)
+asserv_move_arm (uint16_t position, uint8_t speed)
{
- printf ("[asserv] Make the bot move linearly of %d mm.\n", distance);
+ asserv_arm_position += position;
+ printf ("[asserv] Move arm at %d (speed: %d).\n",
+ asserv_arm_position, speed);
}
void
-asserv_move_arm (uint16_t position, uint8_t speed)
+asserv_close_input_hole (void)
{
- printf ("[asserv] Move arm at %d (speed: %d).\n", position, speed);
+ printf ("[asserv] Put the arm in front of the input hole.\n");
+ asserv_move_arm (asserv_arm_position %
+ BOT_ARM_THIRD_ROUND, BOT_ARM_SPEED);
}
void
@@ -187,9 +196,9 @@ asserv_goto (uint32_t x, uint32_t y)
}
void
-move_start (uint32_t x, uint32_t y)
+asserv_goto_angle (int16_t angle)
{
- printf ("[FSM:move] Move the bot to (%d; %d).\n", x, y);
+ printf ("[asserv] Move the bot to face %X.\n", angle);
}
void