summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
Diffstat (limited to 'digital')
-rw-r--r--digital/io/src/ai_loader_cb.c35
-rw-r--r--digital/io/src/bot.h9
-rw-r--r--digital/io/src/loader.fsm16
-rw-r--r--digital/io/src/main.c11
4 files changed, 66 insertions, 5 deletions
diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c
index 18cdc4cb..1d4f816e 100644
--- a/digital/io/src/ai_loader_cb.c
+++ b/digital/io/src/ai_loader_cb.c
@@ -26,6 +26,7 @@
#include "fsm.h"
#include "ai_cb.h"
#include "asserv.h"
+#include "mimot.h"
#include "bot.h"
/*
@@ -52,25 +53,53 @@ ai__LOADER_WAIT_JACK_IN__jack_inserted_into_bot (void)
* LOADER_WAIT_JACK_OUT =jack_removed_from_bot=>
* => LOADER_INIT_ELEVATOR_ZERO
* find elevator zero
+ * close clamp
*/
fsm_branch_t
ai__LOADER_WAIT_JACK_OUT__jack_removed_from_bot (void)
{
asserv_motor0_zero_position (-BOT_ELEVATOR_ZERO_SPEED);
+ mimot_move_motor0_absolute (BOT_CLAMP_STROKE_STEP / 4,
+ BOT_CLAMP_ZERO_SPEED);
+ mimot_move_motor1_absolute (BOT_CLAMP_STROKE_STEP / 4,
+ BOT_CLAMP_ZERO_SPEED);
return ai_next (LOADER_WAIT_JACK_OUT, jack_removed_from_bot);
}
/*
* LOADER_INIT_ELEVATOR_ZERO =elevator_succeed=>
- * => LOADER_INIT_ELEVATOR_UP
- * move elevator up
+ * => LOADER_INIT_CLAMP_CLOSE
*/
fsm_branch_t
ai__LOADER_INIT_ELEVATOR_ZERO__elevator_succeed (void)
{
+ return ai_next (LOADER_INIT_ELEVATOR_ZERO, elevator_succeed);
+}
+
+/*
+ * LOADER_INIT_CLAMP_CLOSE =clamp_succeed=>
+ * => LOADER_INIT_CLAMP_ZERO
+ * move elevator up
+ * find clamp zero
+ */
+fsm_branch_t
+ai__LOADER_INIT_CLAMP_CLOSE__clamp_succeed (void)
+{
asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP,
BOT_ELEVATOR_SPEED);
- return ai_next (LOADER_INIT_ELEVATOR_ZERO, elevator_succeed);
+ mimot_motor0_zero_position (-BOT_CLAMP_ZERO_SPEED);
+ mimot_motor1_zero_position (-BOT_CLAMP_ZERO_SPEED);
+ return ai_next (LOADER_INIT_CLAMP_CLOSE, clamp_succeed);
+}
+
+/*
+ * LOADER_INIT_CLAMP_ZERO =clamp_succeed=>
+ * => LOADER_INIT_ELEVATOR_UP
+ */
+fsm_branch_t
+ai__LOADER_INIT_CLAMP_ZERO__clamp_succeed (void)
+{
+ return ai_next (LOADER_INIT_CLAMP_ZERO, clamp_succeed);
}
/*
diff --git a/digital/io/src/bot.h b/digital/io/src/bot.h
index fa8ad845..634d68e7 100644
--- a/digital/io/src/bot.h
+++ b/digital/io/src/bot.h
@@ -68,6 +68,15 @@
/** Elevator find zero speed. */
#define BOT_ELEVATOR_ZERO_SPEED 0x10
+/** Clamp stroke in steps. */
+#define BOT_CLAMP_STROKE_STEP 13500
+
+/** Clamp work speed. */
+#define BOT_CLAMP_SPEED 0x40
+
+/** Clamp find zero speed. */
+#define BOT_CLAMP_ZERO_SPEED 0x10
+
/**
* Definition of the colors.
*/
diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm
index c89e8931..554ff08a 100644
--- a/digital/io/src/loader.fsm
+++ b/digital/io/src/loader.fsm
@@ -11,6 +11,10 @@ States:
wait until the jack is removed to initialise robot
LOADER_INIT_ELEVATOR_ZERO
move the elevator down to find zero position
+ LOADER_INIT_CLAMP_CLOSE
+ test clamp closing
+ LOADER_INIT_CLAMP_ZERO
+ find clamp zero position
LOADER_INIT_ELEVATOR_UP
move the elevator up so that the robot can go to the wall
@@ -19,6 +23,8 @@ Events:
asserv success result
elevator_failed
asserv failure result
+ clamp_succeed
+ asserv success result, clamp movement cannot fail
LOADER_IDLE:
start -> LOADER_WAIT_JACK_IN
@@ -29,10 +35,18 @@ LOADER_WAIT_JACK_IN:
LOADER_WAIT_JACK_OUT:
jack_removed_from_bot -> LOADER_INIT_ELEVATOR_ZERO
find elevator zero
+ close clamp
LOADER_INIT_ELEVATOR_ZERO:
- elevator_succeed -> LOADER_INIT_ELEVATOR_UP
+ elevator_succeed -> LOADER_INIT_CLAMP_CLOSE
+
+LOADER_INIT_CLAMP_CLOSE:
+ clamp_succeed -> LOADER_INIT_CLAMP_ZERO
move elevator up
+ find clamp zero
+
+LOADER_INIT_CLAMP_ZERO:
+ clamp_succeed -> LOADER_INIT_ELEVATOR_UP
LOADER_INIT_ELEVATOR_UP:
elevator_succeed -> LOADER_IDLE
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index b0973d66..21bfdff8 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -41,6 +41,7 @@
#include "simu.host.h"
#include "asserv.h"
+#include "mimot.h"
#include "twi_master.h"
#include "eeprom.h" /* Parameters loaded/stored in the EEPROM */
#include "fsm.h" /* fsm_* */
@@ -126,12 +127,16 @@ main_event_to_fsm (void)
asserv_status_e
move_status = none,
motor0_status = none,
- motor1_status = none;
+ motor1_status = none,
+ motorm0_status = none,
+ motorm1_status = none;
/* Get status of move, motor0 and motor1. */
move_status = asserv_move_cmd_status ();
motor0_status = asserv_motor0_cmd_status ();
motor1_status = asserv_motor1_cmd_status ();
+ motorm0_status = mimot_motor0_cmd_status ();
+ motorm1_status = mimot_motor1_cmd_status ();
/* Check commands move status. */
if (move_status == success)
@@ -144,6 +149,9 @@ main_event_to_fsm (void)
else if (motor0_status == failure)
FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_elevator_failed);
+ if (motorm0_status == success && motorm1_status == success)
+ FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_clamp_succeed);
+
/* Jack */
if (switch_get_jack ())
FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_jack_removed_from_bot);
@@ -195,6 +203,7 @@ main_init (void)
utils_delay_ms (500);
/* Asserv communication */
asserv_init ();
+ mimot_init ();
/* TWI master. */
twi_master_init ();
/* Switch module */