summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorNicolas Schodet2010-05-11 22:39:29 +0200
committerNicolas Schodet2010-05-11 22:39:29 +0200
commit3462dbdfea97226ce1ddb27d23ccaca8d10b7e11 (patch)
tree259f179fdd4374e313541f6d2c9cd67f3f9f5813
parent196a0500cc747be402d910ffb36ea521f842168c (diff)
digital/{asserv,io}, host/simu/robots/marcel: add gate simulation
-rw-r--r--digital/asserv/src/asserv/models.host.c26
-rw-r--r--digital/io/tools/test_simu_control.py12
-rw-r--r--host/simu/robots/marcel/model/bag.py2
-rw-r--r--host/simu/robots/marcel/model/loader.py21
-rw-r--r--host/simu/robots/marcel/view/loader.py6
5 files changed, 63 insertions, 4 deletions
diff --git a/digital/asserv/src/asserv/models.host.c b/digital/asserv/src/asserv/models.host.c
index 9c16539a..f4d8ca5a 100644
--- a/digital/asserv/src/asserv/models.host.c
+++ b/digital/asserv/src/asserv/models.host.c
@@ -289,6 +289,28 @@ static const struct motor_def_t marcel_elevator_model =
0.0, +INFINITY,
};
+/* Marcel gate model, with a RE25CLL and a 1:10 ratio gearbox. */
+static const struct motor_def_t marcel_gate_model =
+{
+ /* Motor characteristics. */
+ 407 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */
+ 23.4 / 1000, /* Torque constant (N.m/A). */
+ 0, /* Bearing friction (N.m/(rad/s)). */
+ 2.18, /* Terminal resistance (Ohm). */
+ 0.24 / 1000, /* Terminal inductance (H). */
+ 24.0, /* Maximum voltage (V). */
+ /* WARNING: this motor uses a 12V motor on 24V power, PWM should be
+ * limited to half scale. */
+ /* Gearbox characteristics. */
+ 10, /* Gearbox ratio. */
+ 0.75, /* Gearbox efficiency. */
+ /* Load characteristics. */
+ 0.100 * 0.01 * 0.01,/* Load (kg.m^2). */
+ /* This is a pifometric estimation. */
+ /* Hardware limits. */
+ -INFINITY, +INFINITY,
+};
+
/* Marcel, APBTeam 2010. */
static const struct robot_t marcel_robot =
{
@@ -311,9 +333,9 @@ static const struct robot_t marcel_robot =
/** Distance between the encoders wheels (m). */
0.28,
/** Auxiliary motors, NULL if not present. */
- { &marcel_elevator_model, NULL },
+ { &marcel_elevator_model, &marcel_gate_model },
/** Number of steps for each auxiliary motor encoder. */
- { 256, 0 },
+ { 256, 250 },
/** Sensor update function. */
simu_sensor_update_marcel,
};
diff --git a/digital/io/tools/test_simu_control.py b/digital/io/tools/test_simu_control.py
index 49691fb7..1f51f1c2 100644
--- a/digital/io/tools/test_simu_control.py
+++ b/digital/io/tools/test_simu_control.py
@@ -46,6 +46,11 @@ class TestSimuControl (TestSimu):
text = 'Elevator', indicatoron = False,
variable = self.elevator_var, command = self.elevator_command)
self.elevator_button.pack ()
+ self.gate_var = IntVar ()
+ self.gate_button = Checkbutton (self.control_frame,
+ text = 'Gate', indicatoron = False,
+ variable = self.gate_var, command = self.gate_command)
+ self.gate_button.pack ()
self.loader_up_button = Button (self.control_frame,
text = 'Loader up', padx = 0, pady = 0,
command = self.loader_up_command)
@@ -83,6 +88,13 @@ class TestSimuControl (TestSimu):
pos = 0
self.asserv.goto_pos ('a0', pos)
+ def gate_command (self):
+ if self.gate_var.get ():
+ pos = 0x2081
+ else:
+ pos = 0
+ self.asserv.goto_pos ('a1', pos)
+
def loader_up_command (self):
self.io.loader ('u')
diff --git a/host/simu/robots/marcel/model/bag.py b/host/simu/robots/marcel/model/bag.py
index 04edd12d..000090d1 100644
--- a/host/simu/robots/marcel/model/bag.py
+++ b/host/simu/robots/marcel/model/bag.py
@@ -37,7 +37,7 @@ class Bag:
self.position = Position (link_bag.asserv.position)
self.loader = Loader (table, self.position, link_bag.mimot.aux[0],
link_bag.mimot.aux[1], link_bag.asserv.aux[0],
- link_bag.io.contact[0:2])
+ link_bag.asserv.aux[1], link_bag.io.contact[0:2])
self.distance_sensor = [
DistanceSensorSensopart (link_bag.io.adc[0], scheduler, table,
(30 - 20, 0), 0, (self.position, ), 2),
diff --git a/host/simu/robots/marcel/model/loader.py b/host/simu/robots/marcel/model/loader.py
index 44ff83bc..79292ecd 100644
--- a/host/simu/robots/marcel/model/loader.py
+++ b/host/simu/robots/marcel/model/loader.py
@@ -36,16 +36,19 @@ class Loader (Observable):
ELEVATOR_LINEAR_STROKE = 32
ELEVATOR_ROTATING_STROKE = 200 - 32
+ GATE_STROKE = 3.3284 * pi
+
FRONT_ZONE_X_MIN = 120 - 15
FRONT_ZONE_X_MAX = 120 + CLAMP_LENGTH - 15
def __init__ (self, table, robot_position, left_clamp_link, right_clamp_link,
- elevator_link, element_contacts):
+ elevator_link, gate_link, element_contacts):
Observable.__init__ (self)
self.table = table
self.robot_position = robot_position
self.clamp_link = (left_clamp_link, right_clamp_link)
self.elevator_link = elevator_link
+ self.gate_link = gate_link
self.element_contacts = element_contacts
self.element_contacts[1].state = True
self.element_contacts[1].notify ()
@@ -58,6 +61,7 @@ class Loader (Observable):
self.clamp_link[0].register (self.__left_clamp_notified)
self.clamp_link[1].register (self.__right_clamp_notified)
self.elevator_link.register (self.__elevator_notified)
+ self.gate_link.register (self.__gate_notified)
def __robot_position_notified (self):
c = self.element_contacts[0]
@@ -149,6 +153,21 @@ class Loader (Observable):
def __right_clamp_notified (self):
self.__clamp_notified (1)
+ def __gate_notified (self):
+ self.gate_angle = self.gate_link.angle
+ if self.gate_angle is not None and self.robot_position is not None:
+ # If gate is high, drop elements.
+ if self.load and self.gate_angle > self.GATE_STROKE / 2:
+ m = TransMatrix ()
+ m.rotate (self.robot_position.angle)
+ m.translate (self.robot_position.pos)
+ pos = m.apply ((-250, 0))
+ for e in self.load:
+ e.pos = pos
+ e.notify ()
+ self.load = [ ]
+ self.notify ()
+
def __get_front_elements (self):
"""Return a list of elements in front of the robot, between clamp."""
elements = [ ]
diff --git a/host/simu/robots/marcel/view/loader.py b/host/simu/robots/marcel/view/loader.py
index e640157b..4ad2ffce 100644
--- a/host/simu/robots/marcel/view/loader.py
+++ b/host/simu/robots/marcel/view/loader.py
@@ -59,6 +59,12 @@ class Loader (Drawable):
if x > 200:
x = 0
y += 40
+ if self.model.gate_angle is not None:
+ ratio = self.model.gate_angle / self.model.GATE_STROKE
+ # Draw gate.
+ for i in xrange (0, 4):
+ self.draw_line ((100, 125 + 5 * i),
+ (300, 25 + 25 * i + (75 - 10 * i) * ratio))
if self.model.elevator_height is not None:
self.trans_identity ()
self.trans_rotate (-self.model.elevator_angle)