From 3462dbdfea97226ce1ddb27d23ccaca8d10b7e11 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 11 May 2010 22:39:29 +0200 Subject: digital/{asserv,io}, host/simu/robots/marcel: add gate simulation --- digital/asserv/src/asserv/models.host.c | 26 ++++++++++++++++++++++++-- digital/io/tools/test_simu_control.py | 12 ++++++++++++ host/simu/robots/marcel/model/bag.py | 2 +- host/simu/robots/marcel/model/loader.py | 21 ++++++++++++++++++++- host/simu/robots/marcel/view/loader.py | 6 ++++++ 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) -- cgit v1.2.3