summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2011-05-12 23:42:26 +0200
committerNicolas Schodet2011-05-13 01:50:51 +0200
commita3fdbecac564a600c65568cac0e02fc72604567e (patch)
tree07dc894a2b64434b300a0fcffb3486e831a68f1b /digital
parentf58d19eb2d786cb5446d511acd6839a03d791a44 (diff)
digital/{ai,io-hub}, host/simu/robots/robospierre: add doors and element move
Diffstat (limited to 'digital')
-rw-r--r--digital/ai/tools/test_simu_control_robospierre.py29
-rw-r--r--digital/io-hub/src/robospierre/bot.h16
-rw-r--r--digital/io-hub/src/robospierre/clamp.c169
-rw-r--r--digital/io-hub/src/robospierre/clamp.h8
-rw-r--r--digital/io-hub/src/robospierre/main.c17
-rw-r--r--digital/io-hub/tools/io_hub/io_hub.py3
6 files changed, 225 insertions, 17 deletions
diff --git a/digital/ai/tools/test_simu_control_robospierre.py b/digital/ai/tools/test_simu_control_robospierre.py
index b1cccf4b..88fbc5f3 100644
--- a/digital/ai/tools/test_simu_control_robospierre.py
+++ b/digital/ai/tools/test_simu_control_robospierre.py
@@ -46,6 +46,12 @@ class TestSimuControl (TestSimu):
indicatoron = False,
variable = self.clamp_var, command = self.clamp_command)
self.clamp_button.pack ()
+ self.doors_var = IntVar ()
+ self.doors_var.set (1)
+ self.doors_button = Checkbutton (self.control_frame, text = 'Doors',
+ indicatoron = False,
+ variable = self.doors_var, command = self.doors_command)
+ self.doors_button.pack ()
self.elevation_up_button = Button (self.control_frame,
text = 'Elevation up', padx = 0, pady = 0,
command = self.elevation_up_command)
@@ -69,6 +75,13 @@ class TestSimuControl (TestSimu):
text = 'Move clamp', padx = 0, pady = 0,
command = self.clamp_move_command)
self.clamp_move_button.pack ()
+ self.clamp_to_scale = Scale (self.control_frame, orient = HORIZONTAL,
+ from_ = 0, to = 6)
+ self.clamp_to_scale.pack ()
+ self.clamp_element_move_button = Button (self.control_frame,
+ text = 'Move element', padx = 0, pady = 0,
+ command = self.clamp_move_element_command)
+ self.clamp_element_move_button.pack ()
self.table_view.bind ('<1>', self.move)
self.table_view.bind ('<3>', self.orient)
@@ -85,9 +98,9 @@ class TestSimuControl (TestSimu):
def clamp_command (self):
if self.clamp_var.get ():
- self.io.pwm_set_timed (0, -0x3ff, 255, 0)
+ self.io.pwm_set_timed (2, -0x3ff, 255, 0)
else:
- self.io.pwm_set_timed (0, 0x3ff, 255, 0)
+ self.io.pwm_set_timed (2, 0x3ff, 255, 0)
def elevation_up_command (self):
self.mimot.speed_pos ('a0', self.ELEVATION_STROKE / 2)
@@ -104,6 +117,18 @@ class TestSimuControl (TestSimu):
def clamp_move_command (self):
self.io.clamp_move (self.clamp_pos_scale.get ())
+ def clamp_move_element_command (self):
+ self.io.clamp_move_element (self.clamp_pos_scale.get (),
+ self.clamp_to_scale.get ())
+
+ def doors_command (self):
+ if self.doors_var.get ():
+ pwm = -0x3ff
+ else:
+ pwm = 0x3ff
+ for i in (0, 1, 3, 4):
+ self.io.pwm_set_timed (i, pwm, 255, 0)
+
def change_color (self, *dummy):
pass
diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h
index 9043a62c..5e795b0f 100644
--- a/digital/io-hub/src/robospierre/bot.h
+++ b/digital/io-hub/src/robospierre/bot.h
@@ -73,4 +73,20 @@
#define BOT_CLAMP_ELEVATION_SPEED 0x60
#define BOT_CLAMP_ROTATION_SPEED 0x30
+#define BOT_PWM_CLAMP 2
+#define BOT_PWM_DOOR_FRONT_BOTTOM 0
+#define BOT_PWM_DOOR_FRONT_TOP 1
+#define BOT_PWM_DOOR_BACK_BOTTOM 3
+#define BOT_PWM_DOOR_BACK_TOP 4
+
+#define BOT_PWM_CLAMP_OPEN_TIME 225
+#define BOT_PWM_CLAMP_OPEN 0x3ff, 225, 0
+#define BOT_PWM_CLAMP_CLOSE_TIME 225
+#define BOT_PWM_CLAMP_CLOSE -0x3ff, 225, 0
+
+#define BOT_PWM_DOOR_OPEN_TIME 225
+#define BOT_PWM_DOOR_OPEN 0x3ff, 225, 0
+#define BOT_PWM_DOOR_CLOSE_TIME 225
+#define BOT_PWM_DOOR_CLOSE -0x3ff, 225, 0
+
#endif /* bot_h */
diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c
index 2f1b2471..5d9533fa 100644
--- a/digital/io-hub/src/robospierre/clamp.c
+++ b/digital/io-hub/src/robospierre/clamp.c
@@ -29,15 +29,28 @@
#include "fsm.h"
#include "mimot.h"
+#include "pwm.h"
#include "bot.h"
FSM_INIT
FSM_STATES (
- /* Wait order. */
- CLAMP_IDLE,
+ /* Waiting movement order. */
+ CLAMP_MOVE_IDLE,
/* Moving to a final or intermediary position. */
- CLAMP_ROUTING)
+ CLAMP_MOVE_ROUTING,
+ /* Moving to source slot. */
+ CLAMP_MOVE_SRC_ROUTING,
+ /* Closing the clamp once arrived at source. */
+ CLAMP_MOVE_SRC_CLAMP_CLOSING,
+ /* Opening door once clamp closed. */
+ CLAMP_MOVE_SRC_DOOR_OPENDING,
+ /* Moving to destination slot. */
+ CLAMP_MOVE_DST_ROUTING,
+ /* Closing door once arrived at destination. */
+ CLAMP_MOVE_DST_DOOR_CLOSING,
+ /* Opening the clamp once door closed. */
+ CLAMP_MOVE_DST_CLAMP_OPENING)
FSM_EVENTS (
/* Order to move the clamp. */
@@ -49,15 +62,17 @@ FSM_EVENTS (
/* Rotation motor failure. */
clamp_rotation_failure)
-FSM_START_WITH (CLAMP_IDLE)
+FSM_START_WITH (CLAMP_MOVE_IDLE)
/** Clamp context. */
struct clamp_t
{
- /* Current position. */
+ /** Current position. */
uint8_t pos_current;
- /* Requested position. */
+ /** Requested position. */
uint8_t pos_request;
+ /** Element moving destination. */
+ uint8_t moving_to;
};
/** Global context. */
@@ -88,16 +103,36 @@ static const uint16_t clamp_pos[][2] = {
BOT_CLAMP_BAY_SIDE_ROTATION_STEP },
};
+/** Slot doors. */
+static const uint8_t clamp_slot_door[] = {
+ BOT_PWM_DOOR_FRONT_BOTTOM,
+ 0xff,
+ BOT_PWM_DOOR_FRONT_TOP,
+ BOT_PWM_DOOR_BACK_BOTTOM,
+ 0xff,
+ BOT_PWM_DOOR_BACK_TOP,
+ 0xff
+};
+
void
clamp_move (uint8_t pos)
{
if (pos != ctx.pos_current)
{
ctx.pos_request = pos;
+ ctx.moving_to = CLAMP_POS_NB;
FSM_HANDLE (AI, clamp_move);
}
}
+void
+clamp_move_element (uint8_t from, uint8_t to)
+{
+ ctx.pos_request = from;
+ ctx.moving_to = to;
+ FSM_HANDLE (AI, clamp_move);
+}
+
/** Find next position and start motors. */
static void
clamp_route (void)
@@ -157,26 +192,132 @@ clamp_route (void)
ctx.pos_current = pos_new;
}
-FSM_TRANS (CLAMP_IDLE, clamp_move, CLAMP_ROUTING)
+FSM_TRANS (CLAMP_MOVE_IDLE, clamp_move,
+ move, CLAMP_MOVE_ROUTING,
+ move_element, CLAMP_MOVE_SRC_ROUTING,
+ move_element_here, CLAMP_MOVE_SRC_CLAMP_CLOSING)
{
- clamp_route ();
- return FSM_NEXT (CLAMP_IDLE, clamp_move);
+ if (ctx.moving_to == CLAMP_POS_NB)
+ {
+ clamp_route ();
+ return FSM_NEXT (CLAMP_MOVE_IDLE, clamp_move, move);
+ }
+ else
+ {
+ if (ctx.pos_current != ctx.pos_request)
+ {
+ clamp_route ();
+ return FSM_NEXT (CLAMP_MOVE_IDLE, clamp_move, move_element);
+ }
+ else
+ {
+ pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_CLOSE);
+ return FSM_NEXT (CLAMP_MOVE_IDLE, clamp_move, move_element_here);
+ }
+ }
}
-FSM_TRANS (CLAMP_ROUTING, clamp_elevation_rotation_success,
- done, CLAMP_IDLE,
- next, CLAMP_ROUTING)
+FSM_TRANS (CLAMP_MOVE_ROUTING, clamp_elevation_rotation_success,
+ done, CLAMP_MOVE_IDLE,
+ next, CLAMP_MOVE_ROUTING)
{
if (ctx.pos_current == ctx.pos_request)
{
- return FSM_NEXT (CLAMP_ROUTING, clamp_elevation_rotation_success,
+ return FSM_NEXT (CLAMP_MOVE_ROUTING, clamp_elevation_rotation_success,
done);
}
else
{
clamp_route ();
- return FSM_NEXT (CLAMP_ROUTING, clamp_elevation_rotation_success,
+ return FSM_NEXT (CLAMP_MOVE_ROUTING, clamp_elevation_rotation_success,
next);
}
}
+FSM_TRANS (CLAMP_MOVE_SRC_ROUTING, clamp_elevation_rotation_success,
+ done, CLAMP_MOVE_SRC_CLAMP_CLOSING,
+ next, CLAMP_MOVE_SRC_ROUTING)
+{
+ if (ctx.pos_current == ctx.pos_request)
+ {
+ pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_CLOSE);
+ return FSM_NEXT (CLAMP_MOVE_SRC_ROUTING,
+ clamp_elevation_rotation_success, done);
+ }
+ else
+ {
+ clamp_route ();
+ return FSM_NEXT (CLAMP_MOVE_SRC_ROUTING,
+ clamp_elevation_rotation_success, next);
+ }
+}
+
+FSM_TRANS_TIMEOUT (CLAMP_MOVE_SRC_CLAMP_CLOSING, BOT_PWM_CLAMP_CLOSE_TIME,
+ open_door, CLAMP_MOVE_SRC_DOOR_OPENDING,
+ move, CLAMP_MOVE_DST_ROUTING)
+{
+ if (clamp_slot_door[ctx.pos_current] != 0xff)
+ {
+ pwm_set_timed (clamp_slot_door[ctx.pos_current], BOT_PWM_DOOR_OPEN);
+ return FSM_NEXT_TIMEOUT (CLAMP_MOVE_SRC_CLAMP_CLOSING, open_door);
+ }
+ else
+ {
+ ctx.pos_request = ctx.moving_to;
+ clamp_route ();
+ return FSM_NEXT_TIMEOUT (CLAMP_MOVE_SRC_CLAMP_CLOSING, move);
+ }
+}
+
+FSM_TRANS_TIMEOUT (CLAMP_MOVE_SRC_DOOR_OPENDING, BOT_PWM_DOOR_OPEN_TIME,
+ CLAMP_MOVE_DST_ROUTING)
+{
+ ctx.pos_request = ctx.moving_to;
+ clamp_route ();
+ return FSM_NEXT_TIMEOUT (CLAMP_MOVE_SRC_DOOR_OPENDING);
+}
+
+FSM_TRANS (CLAMP_MOVE_DST_ROUTING, clamp_elevation_rotation_success,
+ done_close_door, CLAMP_MOVE_DST_DOOR_CLOSING,
+ done_open_clamp, CLAMP_MOVE_DST_CLAMP_OPENING,
+ next, CLAMP_MOVE_DST_ROUTING)
+{
+ if (ctx.pos_current == ctx.pos_request)
+ {
+ if (clamp_slot_door[ctx.pos_current] != 0xff)
+ {
+ pwm_set_timed (clamp_slot_door[ctx.pos_current],
+ BOT_PWM_DOOR_CLOSE);
+ return FSM_NEXT (CLAMP_MOVE_DST_ROUTING,
+ clamp_elevation_rotation_success,
+ done_close_door);
+ }
+ else
+ {
+ pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_OPEN);
+ return FSM_NEXT (CLAMP_MOVE_DST_ROUTING,
+ clamp_elevation_rotation_success,
+ done_open_clamp);
+ }
+ }
+ else
+ {
+ clamp_route ();
+ return FSM_NEXT (CLAMP_MOVE_DST_ROUTING,
+ clamp_elevation_rotation_success, next);
+ }
+}
+
+FSM_TRANS_TIMEOUT (CLAMP_MOVE_DST_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME,
+ CLAMP_MOVE_DST_CLAMP_OPENING)
+{
+ pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_OPEN);
+ return FSM_NEXT_TIMEOUT (CLAMP_MOVE_DST_DOOR_CLOSING);
+}
+
+FSM_TRANS_TIMEOUT (CLAMP_MOVE_DST_CLAMP_OPENING, BOT_PWM_CLAMP_OPEN_TIME,
+ CLAMP_MOVE_IDLE)
+{
+ return FSM_NEXT_TIMEOUT (CLAMP_MOVE_DST_CLAMP_OPENING);
+}
+
diff --git a/digital/io-hub/src/robospierre/clamp.h b/digital/io-hub/src/robospierre/clamp.h
index 104e3cb8..7b488f92 100644
--- a/digital/io-hub/src/robospierre/clamp.h
+++ b/digital/io-hub/src/robospierre/clamp.h
@@ -40,6 +40,10 @@ enum {
CLAMP_BAY_BACK_LEAVE,
/* Enter the side bay. Position on the side, above wheels. */
CLAMP_BAY_SIDE_ENTER_LEAVE,
+ /** Total number of position, including intermediary positions. */
+ CLAMP_POS_NB,
+ /** Number of slots. */
+ CLAMP_SLOT_NB = CLAMP_SLOT_SIDE + 1,
};
/** Is slot in front bay? */
@@ -54,4 +58,8 @@ enum {
void
clamp_move (uint8_t pos);
+/** Move element using clamp. */
+void
+clamp_move_element (uint8_t from, uint8_t to);
+
#endif /* clamp_h */
diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c
index 3a32a1ae..13bad26c 100644
--- a/digital/io-hub/src/robospierre/main.c
+++ b/digital/io-hub/src/robospierre/main.c
@@ -47,6 +47,8 @@
#include "clamp.h"
+#include "bot.h"
+
#include "io.h"
/** Our color. */
@@ -93,7 +95,7 @@ main_event_to_fsm (void)
#define FSM_HANDLE_TIMEOUT_E(fsm) \
do { if (FSM_HANDLE_TIMEOUT (fsm)) return; } while (0)
/* Update FSM timeouts. */
- //FSM_HANDLE_TIMEOUT_E (AI);
+ FSM_HANDLE_TIMEOUT_E (AI);
/* Motor status. */
asserv_status_e mimot_motor0_status, mimot_motor1_status;
mimot_motor0_status = mimot_motor0_cmd_status ();
@@ -184,6 +186,19 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - 1b: position. */
clamp_move (args[0]);
break;
+ case c ('c', 2):
+ /* Move element using clamp.
+ * - 1b: source.
+ * - 1b: destination. */
+ clamp_move_element (args[0], args[1]);
+ break;
+ case c ('d', 0):
+ /* Open all doors. */
+ pwm_set_timed (BOT_PWM_DOOR_FRONT_BOTTOM, BOT_PWM_DOOR_OPEN);
+ pwm_set_timed (BOT_PWM_DOOR_FRONT_TOP, BOT_PWM_DOOR_OPEN);
+ pwm_set_timed (BOT_PWM_DOOR_BACK_BOTTOM, BOT_PWM_DOOR_OPEN);
+ pwm_set_timed (BOT_PWM_DOOR_BACK_TOP, BOT_PWM_DOOR_OPEN);
+ break;
/* Stats commands.
* - b: interval between stats. */
case c ('A', 1):
diff --git a/digital/io-hub/tools/io_hub/io_hub.py b/digital/io-hub/tools/io_hub/io_hub.py
index a363e5f6..1b25102c 100644
--- a/digital/io-hub/tools/io_hub/io_hub.py
+++ b/digital/io-hub/tools/io_hub/io_hub.py
@@ -47,6 +47,9 @@ class Proto:
def clamp_move (self, pos):
self.proto.send ('c', 'B', pos)
+ def clamp_move_element (self, from_, to):
+ self.proto.send ('c', 'BB', from_, to)
+
def close (self):
self.reset ()
self.proto.wait (lambda: True)