summaryrefslogtreecommitdiff
path: root/host
diff options
context:
space:
mode:
authorNicolas Schodet2011-05-12 23:42:26 +0200
committerNicolas Schodet2011-05-13 01:50:51 +0200
commita3fdbecac564a600c65568cac0e02fc72604567e (patch)
tree07dc894a2b64434b300a0fcffb3486e831a68f1b /host
parentf58d19eb2d786cb5446d511acd6839a03d791a44 (diff)
digital/{ai,io-hub}, host/simu/robots/robospierre: add doors and element move
Diffstat (limited to 'host')
-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
3 files changed, 33 insertions, 11 deletions
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 ()