summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorNicolas Schodet2011-05-12 23:42:26 +0200
committerNicolas Schodet2011-05-13 01:50:51 +0200
commita3fdbecac564a600c65568cac0e02fc72604567e (patch)
tree07dc894a2b64434b300a0fcffb3486e831a68f1b
parentf58d19eb2d786cb5446d511acd6839a03d791a44 (diff)
digital/{ai,io-hub}, host/simu/robots/robospierre: add doors and element move
-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
-rw-r--r--host/simu/robots/robospierre/model/bag.py6
-rw-r--r--host/simu/robots/robospierre/model/clamp.py28
-rw-r--r--host/simu/robots/robospierre/view/clamp.py10
9 files changed, 258 insertions, 28 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)
diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py
index 4d87192c..0a89d00c 100644
--- a/host/simu/robots/robospierre/model/bag.py
+++ b/host/simu/robots/robospierre/model/bag.py
@@ -37,10 +37,12 @@ class Bag:
self.contact = [ Switch (contact)
for contact in link_bag.io_hub.contact[2:] ]
self.position = Position (link_bag.asserv.position)
- self.clamping_motor = MotorBasic (link_bag.io_hub.pwm[0], scheduler,
+ self.clamping_motor = MotorBasic (link_bag.io_hub.pwm[2], scheduler,
2 * pi, 0, pi)
+ self.door_motors = [ MotorBasic (link_bag.io_hub.pwm[i], scheduler,
+ 2 * pi, 0, 0.5 * pi) for i in (0, 1, 3, 4) ]
self.clamp = Clamp (table, self.position, link_bag.mimot.aux[0],
- link_bag.mimot.aux[1], self.clamping_motor)
+ link_bag.mimot.aux[1], self.clamping_motor, self.door_motors)
self.distance_sensor = [
DistanceSensorSensopart (link_bag.io_hub.adc[0], scheduler, table,
(20, 20), pi * 10 / 180, (self.position, ), 2),
diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py
index ff27a7ea..9bb8bacc 100644
--- a/host/simu/robots/robospierre/model/clamp.py
+++ b/host/simu/robots/robospierre/model/clamp.py
@@ -29,12 +29,13 @@ from math import pi, cos, sin
class Slot:
"""Slot which can contain a pawn."""
- def __init__ (self, x, y, z, side):
+ def __init__ (self, x, y, z, side, door_motor):
self.x = x
self.y = y
self.z = z
self.side = side
self.pawn = None
+ self.door_motor = door_motor
class Clamp (Observable):
@@ -59,21 +60,30 @@ class Clamp (Observable):
SLOT_SIDE = 6
def __init__ (self, table, robot_position, elevation_motor,
- rotation_motor, clamping_motor):
+ rotation_motor, clamping_motor, door_motors):
Observable.__init__ (self)
self.table = table
self.robot_position = robot_position
self.elevation_motor = elevation_motor
self.rotation_motor = rotation_motor
self.clamping_motor = clamping_motor
+ self.door_motors = (door_motors[0], None, door_motors[1],
+ door_motors[2], None, door_motors[3], None)
self.slots = (
- Slot (self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 0),
- Slot (self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 0),
- Slot (self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 0),
- Slot (-self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 1),
- Slot (-self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 1),
- Slot (-self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 1),
- Slot (0, self.BAY_OFFSET, 2 * self.BAY_ZOFFSET, None))
+ Slot (self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 0,
+ door_motors[0]),
+ Slot (self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 0,
+ None),
+ Slot (self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 0,
+ door_motors[1]),
+ Slot (-self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 1,
+ door_motors[2]),
+ Slot (-self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 1,
+ None),
+ Slot (-self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 1,
+ door_motors[3]),
+ Slot (0, self.BAY_OFFSET, 2 * self.BAY_ZOFFSET, None,
+ None))
self.load = None
self.robot_position.register (self.__robot_position_notified)
self.elevation_motor.register (self.__elevation_notified)
diff --git a/host/simu/robots/robospierre/view/clamp.py b/host/simu/robots/robospierre/view/clamp.py
index bd5f6dab..130c3eea 100644
--- a/host/simu/robots/robospierre/view/clamp.py
+++ b/host/simu/robots/robospierre/view/clamp.py
@@ -133,6 +133,7 @@ class ClampSide (Drawable):
self.draw_pawn ((-slot.x, slot.z), slot.pawn)
# Draw clamp.
if self.model.rotation is not None:
+ self.trans_push ()
self.trans_translate ((0, self.model.elevation))
m = TransMatrix ()
m.rotate (pi + self.model.rotation)
@@ -169,4 +170,13 @@ class ClampSide (Drawable):
self.draw_pawn ((lcenter, 0), self.model.load)
self.draw_rectangle ((lbase, 10), (lcenter - 103, 40), **attr)
self.draw_rectangle ((rbase, 10), (rtip, 40), **attr)
+ self.trans_pop ()
+ # Draw doors.
+ for slot in self.model.slots:
+ if slot.door_motor is not None:
+ self.trans_push ()
+ self.trans_translate ((-slot.x, slot.z + 50))
+ self.trans_rotate (-0.5 * pi + slot.door_motor.angle)
+ self.draw_line ((0, 0), (40, 0))
+ self.trans_pop ()