From a3fdbecac564a600c65568cac0e02fc72604567e Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 12 May 2011 23:42:26 +0200 Subject: digital/{ai,io-hub}, host/simu/robots/robospierre: add doors and element move --- host/simu/robots/robospierre/model/clamp.py | 28 +++++++++++++++++++--------- 1 file changed, 19 insertions(+), 9 deletions(-) (limited to 'host/simu/robots/robospierre/model/clamp.py') 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) -- cgit v1.2.3 From c8d4bd0e0f79dc8e49ad0074a1e67317e903c41f Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 14 May 2011 01:28:54 +0200 Subject: digital/io-hub, host/simu: add contacts --- digital/io-hub/src/robospierre/contact_defs.h | 18 +++++++++++--- digital/io-hub/src/robospierre/simu.host.c | 2 +- digital/io-hub/src/robospierre/simu.host.h | 2 +- digital/io-hub/tools/io_hub/mex.py | 2 +- host/simu/model/switch.py | 8 ++++-- host/simu/robots/robospierre/model/bag.py | 7 +++--- host/simu/robots/robospierre/model/clamp.py | 36 ++++++++++++++++----------- 7 files changed, 49 insertions(+), 26 deletions(-) (limited to 'host/simu/robots/robospierre/model/clamp.py') diff --git a/digital/io-hub/src/robospierre/contact_defs.h b/digital/io-hub/src/robospierre/contact_defs.h index 55b34354..1e04f6bf 100644 --- a/digital/io-hub/src/robospierre/contact_defs.h +++ b/digital/io-hub/src/robospierre/contact_defs.h @@ -27,11 +27,21 @@ #define CONTACT_COLOR A, 7 #define CONTACT_JACK F, 7 -#define CONTACT_EX1 E, 0 -#define CONTACT_EX2 E, 1 +#define CONTACT_FRONT_BOTTOM A, 4 +#define CONTACT_FRONT_MIDDLE F, 4 +#define CONTACT_BACK_BOTTOM A, 5 +#define CONTACT_BACK_MIDDLE F, 5 +#define CONTACT_FRONT_TOP A, 6 +#define CONTACT_BACK_TOP F, 6 +#define CONTACT_SIDE E, 7 #define CONTACT_LIST \ - CONTACT (CONTACT_EX1) \ - CONTACT (CONTACT_EX2) + CONTACT (CONTACT_FRONT_BOTTOM) \ + CONTACT (CONTACT_FRONT_MIDDLE) \ + CONTACT (CONTACT_FRONT_TOP) \ + CONTACT (CONTACT_BACK_BOTTOM) \ + CONTACT (CONTACT_BACK_MIDDLE) \ + CONTACT (CONTACT_BACK_TOP) \ + CONTACT (CONTACT_SIDE) #endif /* contact_defs_h */ diff --git a/digital/io-hub/src/robospierre/simu.host.c b/digital/io-hub/src/robospierre/simu.host.c index 8f4019aa..281a60d6 100644 --- a/digital/io-hub/src/robospierre/simu.host.c +++ b/digital/io-hub/src/robospierre/simu.host.c @@ -31,7 +31,7 @@ #include "io.h" /** AVR registers. */ -uint8_t PINE; +uint8_t PINA, PINE, PINF; /** Initialise simulation. */ void diff --git a/digital/io-hub/src/robospierre/simu.host.h b/digital/io-hub/src/robospierre/simu.host.h index 4b461e86..ed6a002d 100644 --- a/digital/io-hub/src/robospierre/simu.host.h +++ b/digital/io-hub/src/robospierre/simu.host.h @@ -27,7 +27,7 @@ #ifdef HOST -extern uint8_t PINE; +extern uint8_t PINA, PINE, PINF; #else /* !defined (HOST) */ diff --git a/digital/io-hub/tools/io_hub/mex.py b/digital/io-hub/tools/io_hub/mex.py index 7c8c0012..d9568197 100644 --- a/digital/io-hub/tools/io_hub/mex.py +++ b/digital/io-hub/tools/io_hub/mex.py @@ -31,7 +31,7 @@ ADC_NB = 8 PWM_NB = 6 PWM_VALUE_MAX = 1024 -CONTACT_NB = 4 +CONTACT_NB = 9 CONTACT_INIT = 0xffffffff class Mex: diff --git a/host/simu/model/switch.py b/host/simu/model/switch.py index 30cc1466..6a06a954 100644 --- a/host/simu/model/switch.py +++ b/host/simu/model/switch.py @@ -26,13 +26,17 @@ from utils.observable import Observable class Switch (Observable): - def __init__ (self, link): + def __init__ (self, link, invert = False): Observable.__init__ (self) self.link = link self.state = None + self.invert = invert self.register (self.__update) def __update (self): - self.link.state = self.state + if not self.invert: + self.link.state = self.state + else: + self.link.state = not self.state self.link.notify () diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py index 0a89d00c..5a6fa023 100644 --- a/host/simu/robots/robospierre/model/bag.py +++ b/host/simu/robots/robospierre/model/bag.py @@ -32,8 +32,8 @@ from math import pi class Bag: def __init__ (self, scheduler, table, link_bag): - self.color_switch = Switch (link_bag.io_hub.contact[0]) - self.jack = Switch (link_bag.io_hub.contact[1]) + self.color_switch = Switch (link_bag.io_hub.contact[0], invert = True) + self.jack = Switch (link_bag.io_hub.contact[1], invert = True) self.contact = [ Switch (contact) for contact in link_bag.io_hub.contact[2:] ] self.position = Position (link_bag.asserv.position) @@ -42,7 +42,8 @@ class Bag: 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, self.door_motors) + link_bag.mimot.aux[1], self.clamping_motor, self.door_motors, + self.contact[0:7]) 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 9bb8bacc..e2592aa8 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -29,13 +29,19 @@ from math import pi, cos, sin class Slot: """Slot which can contain a pawn.""" - def __init__ (self, x, y, z, side, door_motor): + def __init__ (self, x, y, z, side, door_motor, contact): self.x = x self.y = y self.z = z self.side = side - self.pawn = None self.door_motor = door_motor + self.contact = contact + self.set_pawn (None) + + def set_pawn (self, pawn): + self.pawn = pawn + self.contact.state = pawn is None + self.contact.notify () class Clamp (Observable): @@ -60,7 +66,7 @@ class Clamp (Observable): SLOT_SIDE = 6 def __init__ (self, table, robot_position, elevation_motor, - rotation_motor, clamping_motor, door_motors): + rotation_motor, clamping_motor, door_motors, slot_contacts): Observable.__init__ (self) self.table = table self.robot_position = robot_position @@ -71,19 +77,19 @@ class Clamp (Observable): door_motors[2], None, door_motors[3], None) self.slots = ( Slot (self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 0, - door_motors[0]), + door_motors[0], slot_contacts[0]), Slot (self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 0, - None), + None, slot_contacts[1]), Slot (self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 0, - door_motors[1]), + door_motors[1], slot_contacts[2]), Slot (-self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 1, - door_motors[2]), + door_motors[2], slot_contacts[3]), Slot (-self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 1, - None), + None, slot_contacts[4]), Slot (-self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 1, - door_motors[3]), + door_motors[3], slot_contacts[5]), Slot (0, self.BAY_OFFSET, 2 * self.BAY_ZOFFSET, None, - None)) + None, slot_contacts[6])) self.load = None self.robot_position.register (self.__robot_position_notified) self.elevation_motor.register (self.__elevation_notified) @@ -98,7 +104,7 @@ class Clamp (Observable): if slot.pawn is None: p = self.__get_floor_elements (slot.side) if p is not None: - slot.pawn = p + slot.set_pawn (p) p.pos = None p.notify () changed = True @@ -133,14 +139,16 @@ class Clamp (Observable): if self.clamping == 0 and self.load is None: # Load an element. slot = self.__get_clamp_slot () - if slot: - self.load, slot.pawn = slot.pawn, self.load + if slot and slot.pawn is not None: + self.load = slot.pawn + slot.set_pawn (None) elif self.clamping == self.CLAMPING_STROKE \ and self.load is not None: # Unload an element. slot = self.__get_clamp_slot () if slot and slot.pawn is None: - self.load, slot.pawn = slot.pawn, self.load + slot.set_pawn (self.load) + self.load = None self.notify () def __get_floor_elements (self, side): -- cgit v1.2.3 From 73f25d3310706c5d7a559f120805c54860b16795 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 15 May 2011 20:14:12 +0200 Subject: host/simu/robots/robospierre: add tower management --- host/simu/robots/robospierre/model/clamp.py | 60 ++++++++++++++++++++++++----- host/simu/robots/robospierre/view/clamp.py | 31 +++++++++------ host/simu/robots/robospierre/view/robot.py | 4 +- host/simu/view/table_eurobot2011.py | 32 +++++++++------ 4 files changed, 92 insertions(+), 35 deletions(-) (limited to 'host/simu/robots/robospierre/model/clamp.py') diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index e2592aa8..976e559f 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -24,6 +24,7 @@ """Robospierre clamp.""" from utils.observable import Observable from simu.utils.trans_matrix import TransMatrix +from simu.model.round_obstacle import RoundObstacle from math import pi, cos, sin class Slot: @@ -36,12 +37,7 @@ class Slot: self.side = side self.door_motor = door_motor self.contact = contact - self.set_pawn (None) - - def set_pawn (self, pawn): - self.pawn = pawn - self.contact.state = pawn is None - self.contact.notify () + self.pawn = None class Clamp (Observable): @@ -90,6 +86,14 @@ class Clamp (Observable): door_motors[3], slot_contacts[5]), Slot (0, self.BAY_OFFSET, 2 * self.BAY_ZOFFSET, None, None, slot_contacts[6])) + self.front_slots = ( + self.slots[self.SLOT_FRONT_BOTTOM], + self.slots[self.SLOT_FRONT_MIDDLE], + self.slots[self.SLOT_FRONT_TOP]) + self.back_slots = ( + self.slots[self.SLOT_BACK_BOTTOM], + self.slots[self.SLOT_BACK_MIDDLE], + self.slots[self.SLOT_BACK_TOP]) self.load = None self.robot_position.register (self.__robot_position_notified) self.elevation_motor.register (self.__elevation_notified) @@ -104,11 +108,12 @@ class Clamp (Observable): if slot.pawn is None: p = self.__get_floor_elements (slot.side) if p is not None: - slot.set_pawn (p) + slot.pawn = p p.pos = None p.notify () changed = True if changed: + self.update_contacts () self.notify () def __elevation_notified (self): @@ -141,16 +146,53 @@ class Clamp (Observable): slot = self.__get_clamp_slot () if slot and slot.pawn is not None: self.load = slot.pawn - slot.set_pawn (None) + slot.pawn = None elif self.clamping == self.CLAMPING_STROKE \ and self.load is not None: # Unload an element. slot = self.__get_clamp_slot () if slot and slot.pawn is None: - slot.set_pawn (self.load) + slot.pawn = self.load self.load = None + # Little resources saving hack: done here, all motors are notified + # at the same time. + self.check_tower () + self.update_contacts () self.notify () + def check_tower (self): + """Check whether several elements can make a tower.""" + for slots in (self.front_slots, self.back_slots): + if slots[0].pawn is not None and slots[1].pawn is not None: + assert slots[0].pawn.kind != 'tower' + tower = RoundObstacle (100, 1) + tower.kind = 'tower' + tower.tower = [ slots[0].pawn, slots[1].pawn ] + slots[0].pawn, slots[1].pawn = tower, None + if slots[0].pawn is not None and slots[0].pawn.kind == 'tower' \ + and slots[2].pawn and slots[2].door_motor.angle: + slots[0].pawn.tower.append (slots[2].pawn) + slots[2].pawn = None + + def update_contacts (self): + """Update pawn contacts.""" + for slots in (self.front_slots, self.back_slots): + slots[0].contact.state = not (slots[0].pawn is not None) + # A tower at level 0 is seen at level 1. + slots[1].contact.state = not ( + slots[1].pawn is not None + or (slots[0].pawn is not None + and slots[0].pawn.kind == 'tower')) + # This one is really high. + slots[2].contact.state = not (slots[2].pawn is not None) + slot_side = self.slots[self.SLOT_SIDE] + slot_side.contact.state = slot_side.pawn is None + clamp_slot = self.__get_clamp_slot () + if clamp_slot is not None: + clamp_slot.contact.state = False + for slot in self.slots: + slot.contact.notify () + def __get_floor_elements (self, side): """Return an elements in front (side = 0) or in back (side = 1) of the robot, on the floor.""" diff --git a/host/simu/robots/robospierre/view/clamp.py b/host/simu/robots/robospierre/view/clamp.py index 130c3eea..07a10929 100644 --- a/host/simu/robots/robospierre/view/clamp.py +++ b/host/simu/robots/robospierre/view/clamp.py @@ -68,7 +68,7 @@ class ClampTop (Drawable): self.trans_push () self.trans_scale (1 - slot.z / 1000.0) self.trans_translate ((slot.x, slot.y)) - draw_pawn (self, slot.pawn.radius, slot.pawn.kind) + draw_pawn (self, slot.pawn) self.trans_pop () # Draw clamp. if self.model.rotation is not None: @@ -82,7 +82,7 @@ class ClampTop (Drawable): if load is not None: self.trans_push () self.trans_translate ((150, 0)) - draw_pawn (self, load.radius, load.kind) + draw_pawn (self, load) self.trans_pop () # Mobile side. self.trans_rotate (-self.model.clamping / 43) @@ -107,16 +107,23 @@ class ClampSide (Drawable): if pawn is not None: self.trans_push () self.trans_translate (pos) - self.draw_rectangle ((-100, 0), (100, 50), fill = YELLOW) - if pawn.kind == 'king': - self.draw_polygon ((-50, 50), (-10, 170), (-50, 170), (-50, 190), - (-10, 190), (-10, 230), (10, 230), (10, 190), (50, 190), - (50, 170), (5, 170), (50, 50), fill = YELLOW, - outline = BLACK) - elif pawn.kind == 'queen': - self.draw_polygon ((-50, 50), (-10, 180), (10, 180), (50, 50), - fill = YELLOW, outline = BLACK) - self.draw_circle ((0, 180), 50, fill = YELLOW) + if pawn.kind == 'tower': + pawns = pawn.tower + else: + pawns = (pawn, ) + for p in pawns: + self.draw_rectangle ((-100, 0), (100, 50), fill = YELLOW) + if p.kind == 'king': + self.draw_polygon ((-50, 50), (-10, 170), (-50, 170), + (-50, 190), (-10, 190), (-10, 230), (10, 230), + (10, 190), (50, 190), (50, 170), (5, 170), + (50, 50), fill = YELLOW, + outline = BLACK) + elif p.kind == 'queen': + self.draw_polygon ((-50, 50), (-10, 180), (10, 180), + (50, 50), fill = YELLOW, outline = BLACK) + self.draw_circle ((0, 180), 50, fill = YELLOW) + self.trans_translate ((0, 50)) self.trans_pop () def draw (self): diff --git a/host/simu/robots/robospierre/view/robot.py b/host/simu/robots/robospierre/view/robot.py index c0f6624c..90624000 100644 --- a/host/simu/robots/robospierre/view/robot.py +++ b/host/simu/robots/robospierre/view/robot.py @@ -63,7 +63,7 @@ class Robot (simu.inter.drawable.Drawable): self.trans_push () self.trans_scale (1 - slot.z / 1000.0) self.trans_translate ((slot.x, slot.y)) - draw_pawn (self, slot.pawn.radius, slot.pawn.kind) + draw_pawn (self, slot.pawn) self.trans_pop () # Draw clamp. if self.clamp_model.rotation is not None: @@ -77,7 +77,7 @@ class Robot (simu.inter.drawable.Drawable): if load: self.trans_push () self.trans_translate ((150, 0)) - draw_pawn (self, load.radius, load.kind) + draw_pawn (self, load) self.trans_pop () # Mobile side. self.trans_rotate (- self.clamp_model.clamping / 47) diff --git a/host/simu/view/table_eurobot2011.py b/host/simu/view/table_eurobot2011.py index f0a65ce1..5725e5b8 100644 --- a/host/simu/view/table_eurobot2011.py +++ b/host/simu/view/table_eurobot2011.py @@ -31,17 +31,25 @@ GREEN = '#268126' BLACK = '#181818' YELLOW = '#cccc00' -def draw_pawn (d, radius, kind): - d.draw_circle ((0, 0), radius, fill = YELLOW) - if kind == 'king': - a = 0.1 * radius - b = 0.5 * radius - d.draw_line ((a, b), (a, a), (b, a), (b, -a), (a, -a), (a, -b), - (-a, -b), (-a, -a), (-b, -a), (-b, a), (-a, a), (-a, b), - (a, b)) - elif kind == 'queen': - d.draw_circle ((0, 0), 0.5 * radius) - d.draw_circle ((0, 0), 0.4 * radius) +def draw_pawn (d, pawn): + d.trans_push () + if pawn.kind == 'tower': + pawns = pawn.tower + else: + pawns = (pawn, ) + for p in pawns: + d.draw_circle ((0, 0), p.radius, fill = YELLOW) + if p.kind == 'king': + a = 0.1 * p.radius + b = 0.5 * p.radius + d.draw_line ((a, b), (a, a), (b, a), (b, -a), (a, -a), (a, -b), + (-a, -b), (-a, -a), (-b, -a), (-b, a), (-a, a), (-a, b), + (a, b)) + elif p.kind == 'queen': + d.draw_circle ((0, 0), 0.5 * p.radius) + d.draw_circle ((0, 0), 0.4 * p.radius) + d.trans_scale (0.95) + d.trans_pop () class Pawn (Drawable): @@ -59,7 +67,7 @@ class Pawn (Drawable): self.reset () if self.pos: self.trans_translate (self.pos) - draw_pawn (self, self.model.radius, self.model.kind) + draw_pawn (self, self.model) Drawable.draw (self) class Table (Drawable): -- cgit v1.2.3 From 201aa07cbd28c6be8c4003f126b1ae258f859c68 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 15 May 2011 22:44:20 +0200 Subject: host/simu: handle dropping tower on the table --- host/simu/model/table.py | 4 +- host/simu/model/table_eurobot2011.py | 6 +++ host/simu/robots/robospierre/model/clamp.py | 64 ++++++++++++++++++++++++----- host/simu/view/table_eurobot2011.py | 9 +++- 4 files changed, 70 insertions(+), 13 deletions(-) (limited to 'host/simu/robots/robospierre/model/clamp.py') diff --git a/host/simu/model/table.py b/host/simu/model/table.py index 41767ac4..d79f7758 100644 --- a/host/simu/model/table.py +++ b/host/simu/model/table.py @@ -22,6 +22,7 @@ # # }}} """Table model.""" +from utils.observable import Observable class Intersect: @@ -29,9 +30,10 @@ class Intersect: self.obstacle = obstacle self.distance = distance -class Table: +class Table (Observable): def __init__ (self): + Observable.__init__ (self) self.obstacles = [ ] def intersect (self, a, b, level = None, comp = None): diff --git a/host/simu/model/table_eurobot2011.py b/host/simu/model/table_eurobot2011.py index c9c1d193..f4bb63b7 100644 --- a/host/simu/model/table_eurobot2011.py +++ b/host/simu/model/table_eurobot2011.py @@ -81,3 +81,9 @@ class Table (simu.model.table.Table): self.pawns.append (pawn) # Add everything to obstacles. self.obstacles += self.pawns + + def add_pawn (self, pawn): + self.pawns.append (pawn) + self.obstacles.append (pawn) + self.notify () + diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index 976e559f..fd3dde7c 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -101,16 +101,31 @@ class Clamp (Observable): self.clamping_motor.register (self.__clamping_notified) def __robot_position_notified (self): + # Compute robot direction. + direction = self.__get_robot_direction () # Update bottom slots. changed = False for sloti in (self.SLOT_FRONT_BOTTOM, self.SLOT_BACK_BOTTOM): slot = self.slots[sloti] - if slot.pawn is None: - p = self.__get_floor_elements (slot.side) - if p is not None: - slot.pawn = p - p.pos = None - p.notify () + if direction == slot.side or direction is None: + # If pushing, can take new elements. + if slot.pawn is None: + p = self.__get_floor_elements (slot.side) + if p is not None: + slot.pawn = p + p.pos = None + p.notify () + changed = True + else: + # Else, can drop elements. + if slot.pawn is not None and slot.door_motor.angle: + m = TransMatrix () + m.translate (self.robot_position.pos) + m.rotate (self.robot_position.angle) + xoffset = (self.BAY_OFFSET, -self.BAY_OFFSET)[slot.side] + slot.pawn.pos = m.apply ((xoffset, 0)) + slot.pawn.notify () + slot.pawn = None changed = True if changed: self.update_contacts () @@ -169,6 +184,7 @@ class Clamp (Observable): tower.kind = 'tower' tower.tower = [ slots[0].pawn, slots[1].pawn ] slots[0].pawn, slots[1].pawn = tower, None + self.table.add_pawn (tower) if slots[0].pawn is not None and slots[0].pawn.kind == 'tower' \ and slots[2].pawn and slots[2].door_motor.angle: slots[0].pawn.tower.append (slots[2].pawn) @@ -199,10 +215,7 @@ class Clamp (Observable): if self.robot_position.pos is None: return None # Matrix to transform an obstacle position into robot coordinates. - m = TransMatrix () - m.rotate (-self.robot_position.angle) - m.translate ((-self.robot_position.pos[0], - -self.robot_position.pos[1])) + m = self.__get_robot_matrix () # Look up elements. xoffset = (self.BAY_OFFSET, -self.BAY_OFFSET)[side] xmargin = 20 @@ -230,3 +243,34 @@ class Clamp (Observable): return slot return None + def __get_robot_direction (self): + """Is robot going forward (0), backward (1), or something else + (None)?""" + if self.robot_position.pos is None: + return None + filt = 5 + if hasattr (self, 'old_robot_position'): + m = self.__get_robot_matrix () + oldrel = m.apply (self.old_robot_position) + if oldrel[0] < 0 and self.direction_counter < filt: + self.direction_counter += 1 + elif oldrel[0] > 0 and self.direction_counter > -filt: + self.direction_counter -= 1 + else: + self.direction_counter = 0 + self.old_robot_position = self.robot_position.pos + # Filter oscillations. + if self.direction_counter > 0: + return 0 + elif self.direction_counter < 0: + return 1 + else: + return None + + def __get_robot_matrix (self): + """Return robot transformation matrix.""" + m = TransMatrix () + m.rotate (-self.robot_position.angle) + m.translate ((-self.robot_position.pos[0], + -self.robot_position.pos[1])) + return m diff --git a/host/simu/view/table_eurobot2011.py b/host/simu/view/table_eurobot2011.py index 5725e5b8..b39f2834 100644 --- a/host/simu/view/table_eurobot2011.py +++ b/host/simu/view/table_eurobot2011.py @@ -76,8 +76,13 @@ class Table (Drawable): def __init__ (self, onto, model): Drawable.__init__ (self, onto) self.model = model - for e in model.pawns: - Pawn (self, e) + self.model.register (self.__notified) + self.__notified () + + def __notified (self): + for e in self.model.pawns: + if e not in self.children: + Pawn (self, e) def draw_both (self, primitive, *args, **kargs): """Draw a primitive on both sides.""" -- cgit v1.2.3 From 9b1ce3a050d9169f3bf77764063de0cff800190e Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 27 May 2011 07:58:36 +0200 Subject: digital/{ai,io-hub,mimot}, host/simu: change robospierre rotation motor --- digital/ai/tools/robospierre.py | 2 +- digital/ai/tools/test_simu_control_robospierre.py | 2 +- digital/io-hub/src/robospierre/bot.h | 18 ++++++------- digital/mimot/src/dirty/models.host.c | 32 +++++++++++++++++++++++ digital/mimot/tools/mimot/init.py | 2 +- host/simu/robots/robospierre/model/clamp.py | 2 +- 6 files changed, 45 insertions(+), 13 deletions(-) (limited to 'host/simu/robots/robospierre/model/clamp.py') diff --git a/digital/ai/tools/robospierre.py b/digital/ai/tools/robospierre.py index e8384f6c..6a44883e 100644 --- a/digital/ai/tools/robospierre.py +++ b/digital/ai/tools/robospierre.py @@ -25,7 +25,7 @@ class Robot: self.robot_view = simu.robots.robospierre.view.bag asserv_cmd = ('../../asserv/src/asserv/asserv.host', '-m9', 'robospierre') - mimot_cmd = ('../../mimot/src/dirty/dirty.host', '-m9', 'marcel') + mimot_cmd = ('../../mimot/src/dirty/dirty.host', '-m9', 'robospierre') io_hub_cmd = ('../../io-hub/src/robospierre/io_hub.host') self.asserv = asserv.Proto (PopenIO (asserv_cmd), proto_time, **asserv.init.host['robospierre']) diff --git a/digital/ai/tools/test_simu_control_robospierre.py b/digital/ai/tools/test_simu_control_robospierre.py index 0d52219e..1795e17f 100644 --- a/digital/ai/tools/test_simu_control_robospierre.py +++ b/digital/ai/tools/test_simu_control_robospierre.py @@ -30,7 +30,7 @@ class TestSimuControl (TestSimu): ELEVATION_STROKE = 0x3b0b - ROTATION_STROKE = 0x11c6 + ROTATION_STROKE = 0x233e def __init__ (self, robot_class): TestSimu.__init__ (self, robot_class) diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 1071bf7e..65f6adf6 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -67,9 +67,9 @@ # define BOT_CLAMP_SLOT_FRONT_BOTTOM_ROTATION_STEP 0 # define BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP 0 # define BOT_CLAMP_SLOT_FRONT_TOP_ROTATION_STEP 0 -# define BOT_CLAMP_SLOT_BACK_BOTTOM_ROTATION_STEP 0x11c6 -# define BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP 0x11c6 -# define BOT_CLAMP_SLOT_BACK_TOP_ROTATION_STEP 0x11c6 +# define BOT_CLAMP_SLOT_BACK_BOTTOM_ROTATION_STEP 0x233e +# define BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP 0x233e +# define BOT_CLAMP_SLOT_BACK_TOP_ROTATION_STEP 0x233e # define BOT_CLAMP_BAY_FRONT_ROTATION_STEP \ BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP @@ -96,23 +96,23 @@ # define BOT_CLAMP_SLOT_FRONT_BOTTOM_ROTATION_STEP 0 # define BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP 0 # define BOT_CLAMP_SLOT_FRONT_TOP_ROTATION_STEP 0 -# define BOT_CLAMP_SLOT_BACK_BOTTOM_ROTATION_STEP 0x10ce -# define BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP ((0x10ce + 0x10e2) / 2) -# define BOT_CLAMP_SLOT_BACK_TOP_ROTATION_STEP 0x10e2 +# define BOT_CLAMP_SLOT_BACK_BOTTOM_ROTATION_STEP 0x233e +# define BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP 0x233e +# define BOT_CLAMP_SLOT_BACK_TOP_ROTATION_STEP 0x233e # define BOT_CLAMP_BAY_FRONT_ROTATION_STEP \ BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP # define BOT_CLAMP_BAY_BACK_ROTATION_STEP \ BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP -# define BOT_CLAMP_BAY_SIDE_ROTATION_STEP 0x816 +# define BOT_CLAMP_BAY_SIDE_ROTATION_STEP 0x10de #define BOT_CLAMP_CLOSED_ROTATION_OFFSET -61 #endif /* !HOST */ #define BOT_CLAMP_ELEVATION_SPEED 0x60 -#define BOT_CLAMP_ROTATION_SPEED 0x30 -#define BOT_CLAMP_ROTATION_OFFSET_SPEED 2 +#define BOT_CLAMP_ROTATION_SPEED 0x60 +#define BOT_CLAMP_ROTATION_OFFSET_SPEED 1 #define BOT_PWM_CLAMP 2 #define BOT_PWM_DOOR_FRONT_BOTTOM 0 diff --git a/digital/mimot/src/dirty/models.host.c b/digital/mimot/src/dirty/models.host.c index a835298f..b1141748 100644 --- a/digital/mimot/src/dirty/models.host.c +++ b/digital/mimot/src/dirty/models.host.c @@ -52,6 +52,26 @@ static const struct motor_def_t marcel_clamp_f2342_model = -INFINITY, +INFINITY, }; +/* Robospierre rotation motor, AMAX32GHP with 1:16 gearbox model. */ +static const struct motor_def_t robospierre_rotation_amax32ghp_model = +{ + /* Motor characteristics. */ + 269 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */ + 25.44 / 1000, /* Torque constant (N.m/A). */ + 0, /* Bearing friction (N.m/(rad/s)). */ + 3.99, /* Terminal resistance (Ohm). */ + 0.24 / 1000, /* Terminal inductance (H). */ + 24.0, /* Maximum voltage (V). */ + /* Gearbox characteristics. */ + 16, /* Gearbox ratio. */ + 0.75, /* Gearbox efficiency. */ + /* Load characteristics. */ + 0.200 * 0.010 * 0.010, /* Load (kg.m^2). */ + /* This is a pifometric estimation. */ + /* Hardware limits. */ + -INFINITY, +INFINITY, +}; + /* Marcel, APBTeam 2010. */ static const struct robot_t marcel_robot = { @@ -63,6 +83,17 @@ static const struct robot_t marcel_robot = simu_sensor_update_marcel, }; +/* Robospierre, APBTeam 2011. */ +static const struct robot_t robospierre_robot = +{ + /** Auxiliary motors, NULL if not present. */ + { &marcel_clamp_f2342_model, &robospierre_rotation_amax32ghp_model }, + /** Number of steps for each auxiliary motor encoder. */ + { 256, 250 }, + /** Sensor update function. */ + NULL, +}; + /* Table of models. */ static const struct { @@ -70,6 +101,7 @@ static const struct const struct robot_t *robot; } models[] = { { "marcel", &marcel_robot }, + { "robospierre", &robospierre_robot }, { 0, 0 } }; diff --git a/digital/mimot/tools/mimot/init.py b/digital/mimot/tools/mimot/init.py index a05f8c44..d302e060 100644 --- a/digital/mimot/tools/mimot/init.py +++ b/digital/mimot/tools/mimot/init.py @@ -17,7 +17,7 @@ target_robospierre = dict ( a1a = 0.5, a1sm = 0x30, a1ss = 0x08, a1be = 256, a1bs = 0x18, a1bc = 5, E = 0x3ff, D = 0x1ff, - w = 0x01, + w = 0x03, ) target = { 'marcel': target_marcel, diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index fd3dde7c..c36d0d36 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -45,7 +45,7 @@ class Clamp (Observable): ELEVATION_MOTOR_STROKE = 120.0 * 5.0 / 6.0 ROTATION_STROKE = pi - ROTATION_MOTOR_STROKE = pi * 115.0 / 12.0 + ROTATION_MOTOR_STROKE = 2 * pi * 36.088 / 16 CLAMPING_STROKE = 10 CLAMPING_MOTOR_STROKE = pi -- cgit v1.2.3 From 4882fbefaeca9cc5292cec9a2f93d34f04e67b5c Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 28 May 2011 00:31:13 +0200 Subject: digital/io-hub: add clamp initialisation --- digital/ai/src/twi_master/mimot.c | 42 +++++++++++++ digital/ai/src/twi_master/mimot.h | 16 +++++ digital/io-hub/src/robospierre/bot.h | 14 +++++ digital/io-hub/src/robospierre/clamp.c | 92 ++++++++++++++++++++++++++++- digital/io-hub/src/robospierre/main.c | 4 ++ digital/mimot/src/dirty/twi_proto.c | 21 +++++++ host/simu/robots/robospierre/model/clamp.py | 2 + 7 files changed, 188 insertions(+), 3 deletions(-) (limited to 'host/simu/robots/robospierre/model/clamp.py') diff --git a/digital/ai/src/twi_master/mimot.c b/digital/ai/src/twi_master/mimot.c index af6ff5f9..c3b82823 100644 --- a/digital/ai/src/twi_master/mimot.c +++ b/digital/ai/src/twi_master/mimot.c @@ -117,6 +117,30 @@ mimot_reset (void) twi_master_send_buffer (1); } +void +mimot_set_motor0_position (uint16_t position) +{ + uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE); + buffer[0] = 'p'; + buffer[1] = 'Y'; + buffer[2] = 0; + buffer[3] = v16_to_v8 (position, 1); + buffer[4] = v16_to_v8 (position, 0); + twi_master_send_buffer (5); +} + +void +mimot_set_motor1_position (uint16_t position) +{ + uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE); + buffer[0] = 'p'; + buffer[1] = 'Y'; + buffer[2] = 1; + buffer[3] = v16_to_v8 (position, 1); + buffer[4] = v16_to_v8 (position, 0); + twi_master_send_buffer (5); +} + void mimot_move_motor0_absolute (uint16_t position, uint8_t speed) { @@ -181,3 +205,21 @@ mimot_motor1_clamp (int8_t speed, int16_t pwm) twi_master_send_buffer (5); } +void +mimot_motor0_free (void) +{ + uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE); + buffer[0] = 'w'; + buffer[1] = 0; + twi_master_send_buffer (2); +} + +void +mimot_motor1_free (void) +{ + uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE); + buffer[0] = 'w'; + buffer[1] = 1; + twi_master_send_buffer (2); +} + diff --git a/digital/ai/src/twi_master/mimot.h b/digital/ai/src/twi_master/mimot.h index 60811c10..97575b0d 100644 --- a/digital/ai/src/twi_master/mimot.h +++ b/digital/ai/src/twi_master/mimot.h @@ -68,6 +68,14 @@ mimot_get_motor1_position (void); void mimot_reset (void); +/** Set motor0 position in steps. */ +void +mimot_set_motor0_position (uint16_t position); + +/** Set motor1 position in steps. */ +void +mimot_set_motor1_position (uint16_t position); + /** Move motor0 to absolute position in steps. */ void mimot_move_motor0_absolute (uint16_t position, uint8_t speed); @@ -92,4 +100,12 @@ mimot_motor0_clamp (int8_t speed, int16_t pwm); void mimot_motor1_clamp (int8_t speed, int16_t pwm); +/** Free motor0. */ +void +mimot_motor0_free (void); + +/** Free motor1. */ +void +mimot_motor1_free (void); + #endif /* mimot_h */ diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 02b74da9..7a8065dd 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -63,6 +63,8 @@ # define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP (0x3b0b / 2 + 1000) # define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP (0x3b0b / 2 + 1000) # define BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP (0x3b0b / 2) +# define BOT_CLAMP_INIT_FRONT_SEEN_ELEVATION_STEP 0x18ca +# define BOT_CLAMP_INIT_BACK_SEEN_ELEVATION_STEP 0x18ca # define BOT_CLAMP_SLOT_FRONT_BOTTOM_ROTATION_STEP 0 # define BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP 0 @@ -93,6 +95,9 @@ # define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP 0x1da7 # define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP 0x1f03 # define BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP ((0x1da7 + 0x1f03) / 2) +// TODO: to be measured. +# define BOT_CLAMP_INIT_FRONT_SEEN_ELEVATION_STEP (0x1da7 / 2) +# define BOT_CLAMP_INIT_BACK_SEEN_ELEVATION_STEP (0x1f03 / 2) # define BOT_CLAMP_SLOT_FRONT_BOTTOM_ROTATION_STEP 0 # define BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP 0 @@ -112,12 +117,17 @@ #endif /* !HOST */ +#define BOT_CLAMP_INIT_ELEVATION_STEP \ + BOT_CLAMP_SLOT_FRONT_MIDDLE_ELEVATION_STEP + #define BOT_CLAMP_CLOSED_ROTATION_OFFSET(pos) \ (CLAMP_IS_SLOT_IN_FRONT_BAY (pos) \ ? BOT_CLAMP_CLOSED_FRONT_ROTATION_OFFSET \ : (CLAMP_IS_SLOT_IN_BACK_BAY (pos) \ ? BOT_CLAMP_CLOSED_BACK_ROTATION_OFFSET : 0)) +#define BOT_CLAMP_INIT_ELEVATION_SPEED 0x08 +#define BOT_CLAMP_INIT_ROTATION_SPEED -0x04 #define BOT_CLAMP_ELEVATION_SPEED 0x60 #define BOT_CLAMP_ROTATION_SPEED 0x60 #define BOT_CLAMP_ROTATION_OFFSET_SPEED 1 @@ -140,4 +150,8 @@ -0x3ff, 50, ((slot == CLAMP_SLOT_FRONT_BOTTOM \ || slot == CLAMP_SLOT_BACK_BOTTOM) ? -0x100 : -0x180) +#define BOT_PWM_CLAMP_INIT 0x1ff, 125, 0 +#define BOT_PWM_DOOR_INIT 0x1ff, 74, 0x55 +#define BOT_PWM_CLAMP_DOOR_INIT 125 + #endif /* bot_h */ diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 71cb2dfb..65a90a7a 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -35,6 +35,8 @@ #include "fsm.h" #include "fsm_queue.h" +#include "modules/proto/proto.h" + #include "logistic.h" /* @@ -50,8 +52,18 @@ FSM_INIT FSM_STATES ( - /* Initial state, to be complete with full initialisation. */ + /* Initial state. */ CLAMP_START, + /* Initialisation sequence: opening everything. */ + CLAMP_INIT_OPENING, + /* Initialisation sequence: going up until a middle pawn sensor + * see the clamp. */ + CLAMP_INIT_UPING_UNTIL_SEEN, + /* Initialisation sequence: going to the middle level. */ + CLAMP_INIT_GOING_MIDDLE, + /* Initialisation sequence: finding front right edge. */ + CLAMP_INIT_FINDING_EDGE, + /* Returning to idle position. */ CLAMP_GOING_IDLE, /* Waiting external events, clamp at middle level. */ @@ -85,6 +97,8 @@ FSM_STATES ( CLAMP_MOVE_DST_CLAMP_OPENING) FSM_EVENTS ( + /* During initialisation sequence, clamp seen by a sensor. */ + clamp_init_seen, /* New element inside bottom slot. */ clamp_new_element, /* Order to drop elements. */ @@ -100,8 +114,12 @@ FSM_EVENTS ( clamp_move_success, /* Elevation and elevation motor success. */ clamp_elevation_rotation_success, + /* Elevation motor success. */ + clamp_elevation_success, /* Elevation motor failure. */ clamp_elevation_failure, + /* Rotation motor success. */ + clamp_rotation_success, /* Rotation motor failure. */ clamp_rotation_failure) @@ -127,6 +145,10 @@ struct clamp_t uint8_t open; /** True if clamp position is controled. */ uint8_t controled; + /** Position of clamp when seen by sensor. */ + uint16_t init_seen_step; + /** Position to initialise at seen position. */ + uint16_t init_seen_init_step; }; /** Global context. */ @@ -280,6 +302,24 @@ clamp_handle_event (void) /* Go directly to next point. */ clamp_route (); } + /* Handle initialisation. */ + if (FSM_CAN_HANDLE (AI, clamp_init_seen)) + { + if (!IO_GET (CONTACT_FRONT_MIDDLE)) + { + ctx.init_seen_step = mimot_get_motor0_position (); + ctx.init_seen_init_step = BOT_CLAMP_INIT_FRONT_SEEN_ELEVATION_STEP; + FSM_HANDLE (AI, clamp_init_seen); + return 1; + } + if (!IO_GET (CONTACT_BACK_MIDDLE)) + { + ctx.init_seen_step = mimot_get_motor0_position (); + ctx.init_seen_init_step = BOT_CLAMP_INIT_BACK_SEEN_ELEVATION_STEP; + FSM_HANDLE (AI, clamp_init_seen); + return 1; + } + } return 0; } @@ -374,12 +414,58 @@ clamp_route (void) /* CLAMP FSM */ -FSM_TRANS (CLAMP_START, init_actuators, CLAMP_GOING_IDLE) +FSM_TRANS (CLAMP_START, init_actuators, CLAMP_INIT_OPENING) { - clamp_move (CLAMP_SLOT_FRONT_MIDDLE); + pwm_set_timed (BOT_PWM_DOOR_FRONT_BOTTOM, BOT_PWM_DOOR_INIT); + pwm_set_timed (BOT_PWM_DOOR_FRONT_TOP, BOT_PWM_DOOR_INIT); + pwm_set_timed (BOT_PWM_DOOR_BACK_BOTTOM, BOT_PWM_DOOR_INIT); + pwm_set_timed (BOT_PWM_DOOR_BACK_TOP, BOT_PWM_DOOR_INIT); + pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_INIT); return FSM_NEXT (CLAMP_START, init_actuators); } +FSM_TRANS_TIMEOUT (CLAMP_INIT_OPENING, BOT_PWM_CLAMP_DOOR_INIT, + CLAMP_INIT_UPING_UNTIL_SEEN) +{ + mimot_move_motor0_absolute (mimot_get_motor0_position () + + BOT_CLAMP_INIT_ELEVATION_STEP, + BOT_CLAMP_INIT_ELEVATION_SPEED); + return FSM_NEXT_TIMEOUT (CLAMP_INIT_OPENING); +} + +FSM_TRANS (CLAMP_INIT_UPING_UNTIL_SEEN, clamp_elevation_success, CLAMP_START) +{ + /* Dead end. */ + mimot_motor0_free (); + return FSM_NEXT (CLAMP_INIT_UPING_UNTIL_SEEN, clamp_elevation_success); +} + +FSM_TRANS (CLAMP_INIT_UPING_UNTIL_SEEN, clamp_init_seen, + CLAMP_INIT_GOING_MIDDLE) +{ + mimot_motor0_free (); + mimot_set_motor0_position (ctx.init_seen_init_step + + mimot_get_motor0_position () + - ctx.init_seen_step); + proto_send1w ('C', ctx.init_seen_step); + mimot_move_motor0_absolute (BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP, + BOT_CLAMP_ELEVATION_SPEED); + return FSM_NEXT (CLAMP_INIT_UPING_UNTIL_SEEN, clamp_init_seen); +} + +FSM_TRANS (CLAMP_INIT_GOING_MIDDLE, clamp_elevation_success, + CLAMP_INIT_FINDING_EDGE) +{ + mimot_motor1_zero_position (BOT_CLAMP_INIT_ROTATION_SPEED); + return FSM_NEXT (CLAMP_INIT_GOING_MIDDLE, clamp_elevation_success); +} + +FSM_TRANS (CLAMP_INIT_FINDING_EDGE, clamp_rotation_success, CLAMP_GOING_IDLE) +{ + clamp_move (CLAMP_SLOT_FRONT_MIDDLE); + return FSM_NEXT (CLAMP_INIT_FINDING_EDGE, clamp_rotation_success); +} + FSM_TRANS (CLAMP_GOING_IDLE, clamp_move_success, CLAMP_IDLE) { return FSM_NEXT (CLAMP_GOING_IDLE, clamp_move_success); diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index b1e85b72..7c773761 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -139,8 +139,12 @@ main_event_to_fsm (void) if (mimot_motor0_status == success && mimot_motor1_status == success) FSM_HANDLE_E (AI, clamp_elevation_rotation_success); + if (mimot_motor0_status == success) + FSM_HANDLE_E (AI, clamp_elevation_success); else if (mimot_motor0_status == failure) FSM_HANDLE_E (AI, clamp_elevation_failure); + if (mimot_motor1_status == success) + FSM_HANDLE_E (AI, clamp_rotation_success); else if (mimot_motor1_status == failure) FSM_HANDLE_E (AI, clamp_rotation_failure); /* Clamp specific events. */ diff --git a/digital/mimot/src/dirty/twi_proto.c b/digital/mimot/src/dirty/twi_proto.c index 3048c594..bda255ef 100644 --- a/digital/mimot/src/dirty/twi_proto.c +++ b/digital/mimot/src/dirty/twi_proto.c @@ -150,6 +150,18 @@ twi_proto_callback (u8 *buf, u8 size) else buf[0] = 0; break; + case c ('w', 1): + /* Free motor. + * - b: aux index. */ + if (buf[2] < AC_ASSERV_AUX_NB) + { + pos_reset (&pos_aux[buf[2]]); + state_aux[buf[2]].mode = MODE_PWM; + pwm_set (&pwm_aux[buf[2]], 0); + } + else + buf[0] = 0; + break; case c ('p', x): /* Set parameters. */ if (twi_proto_params (&buf[2], size - 2) != 0) @@ -173,6 +185,15 @@ twi_proto_params (u8 *buf, u8 size) size--; switch (*buf++) { + case 'Y': + /* Set current aux position. + * - b: aux index. + * - w: position. */ + if (buf[0] >= AC_ASSERV_AUX_NB || size < 3) + return 1; + aux[buf[0]].pos = v8_to_v16 (buf[1], buf[2]); + eat = 3; + break; default: return 1; } diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index c36d0d36..a3476a46 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -69,6 +69,8 @@ class Clamp (Observable): self.elevation_motor = elevation_motor self.rotation_motor = rotation_motor self.clamping_motor = clamping_motor + self.rotation_motor.limits.min = 0 + self.rotation_motor.limits.notify () self.door_motors = (door_motors[0], None, door_motors[1], door_motors[2], None, door_motors[3], None) self.slots = ( -- cgit v1.2.3 From 5c8d716b96d32609d18af5219c541201209e586d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 29 May 2011 19:19:28 +0200 Subject: digital/io-hub: handle element detection on table --- digital/io-hub/src/robospierre/Makefile | 2 +- digital/io-hub/src/robospierre/bot.h | 7 ++ digital/io-hub/src/robospierre/clamp.c | 12 +-- digital/io-hub/src/robospierre/pawn_sensor.c | 113 +++++++++++++++++++++++++++ digital/io-hub/src/robospierre/pawn_sensor.h | 33 ++++++++ host/simu/robots/robospierre/model/clamp.py | 2 +- 6 files changed, 161 insertions(+), 8 deletions(-) create mode 100644 digital/io-hub/src/robospierre/pawn_sensor.c create mode 100644 digital/io-hub/src/robospierre/pawn_sensor.h (limited to 'host/simu/robots/robospierre/model/clamp.py') diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile index e4b87124..a9bb2283 100644 --- a/digital/io-hub/src/robospierre/Makefile +++ b/digital/io-hub/src/robospierre/Makefile @@ -5,7 +5,7 @@ PROGS = io_hub HOST_PROGS = test_element # Sources to compile. io_hub_SOURCES = main.c \ - clamp.c logistic.c element.c \ + clamp.c logistic.c element.c pawn_sensor.c \ radar_defs.c radar.c path.c move.c \ init.c fsm.host.c fsm_AI_gen.avr.c fsm_queue.c \ pwm.avr.c pwm.host.c \ diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 890780b3..7e773032 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -46,6 +46,13 @@ /** Angle error at the front contact point. */ #define BOT_FRONT_CONTACT_ANGLE_ERROR_DEG 0 +/** Distance from robot center to front pawn detection threshold. */ +#define BOT_PAWN_FRONT_DETECTION_THRESHOLD_MM 190 +/** Distance from robot center to back pawn detection threshold. */ +#define BOT_PAWN_BACK_DETECTION_THRESHOLD_MM -190 +/** Distance from robot center to an element near enough to be taken. */ +#define BOT_PAWN_TAKING_DISTANCE_MM 150 + /** Speed used for initialisation. */ #define BOT_SPEED_INIT 0x10, 0x10, 0x10, 0x10 /** Normal cruise speed. */ diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 4c0fc9a7..bae3591d 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -38,6 +38,7 @@ #include "modules/proto/proto.h" #include "logistic.h" +#include "pawn_sensor.h" /* * There is two FSM in this file. @@ -275,16 +276,15 @@ clamp_handle_event (void) { if (FSM_CAN_HANDLE (AI, clamp_new_element)) { - /* XXX: temporary hack. */ - uint8_t element_type = contact_get_color () ? ELEMENT_PAWN : ELEMENT_KING; - if (!IO_GET (CONTACT_FRONT_BOTTOM) - && !logistic_global.slots[CLAMP_SLOT_FRONT_BOTTOM]) + uint8_t element_type; + element_type = pawn_sensor_get (DIRECTION_FORWARD); + if (element_type) { clamp_new_element (CLAMP_SLOT_FRONT_BOTTOM, element_type); return 1; } - if (!IO_GET (CONTACT_BACK_BOTTOM) - && !logistic_global.slots[CLAMP_SLOT_BACK_BOTTOM]) + element_type = pawn_sensor_get (DIRECTION_BACKWARD); + if (element_type) { clamp_new_element (CLAMP_SLOT_BACK_BOTTOM, element_type); return 1; diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c new file mode 100644 index 00000000..65f0afe4 --- /dev/null +++ b/digital/io-hub/src/robospierre/pawn_sensor.c @@ -0,0 +1,113 @@ +/* pawn_sensor.c */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "pawn_sensor.h" + +#include "asserv.h" +#include "contact.h" +#include "logistic.h" +#include "element.h" +#include "clamp.h" +#include "bot.h" + +#include "modules/math/geometry/distance.h" + +/* Handle pawn sensors. When a pawn is detected, it can not be taken + * directly, but only once it is inside the robot. */ + +/** Pawn sensor context. */ +struct pawn_sensor_t +{ + /** Is there something in front of the sensor? */ + uint8_t active; + /** If active, supposed position of element. */ + vect_t active_position; +}; + +/** Global contexts. */ +struct pawn_sensor_t pawn_sensor_front, pawn_sensor_back; + +static uint8_t +pawn_sensor_get_type (uint8_t direction) +{ + uint8_t element_type = contact_get_color () ? ELEMENT_PAWN : ELEMENT_KING; + return element_type; +} + +uint8_t +pawn_sensor_get (uint8_t direction) +{ + struct pawn_sensor_t *ctx; + uint8_t contact_value, slot; + int16_t dist; + /* Check direction. */ + if (direction == DIRECTION_FORWARD) + { + ctx = &pawn_sensor_front; + contact_value = !IO_GET (CONTACT_FRONT_BOTTOM); + slot = CLAMP_SLOT_FRONT_BOTTOM; + dist = BOT_PAWN_FRONT_DETECTION_THRESHOLD_MM; + } + else + { + ctx = &pawn_sensor_back; + contact_value = !IO_GET (CONTACT_BACK_BOTTOM); + slot = CLAMP_SLOT_BACK_BOTTOM; + dist = BOT_PAWN_BACK_DETECTION_THRESHOLD_MM; + } + /* Handle contact. */ + if (contact_value) + { + if (!logistic_global.slots[slot] + && logistic_global.moving_to != slot) + { + position_t robot_position; + asserv_get_position (&robot_position); + if (ctx->active) + { + int32_t d = distance_point_point (&ctx->active_position, + &robot_position.v); + if (d < BOT_PAWN_TAKING_DISTANCE_MM) + { + ctx->active = 0; + return pawn_sensor_get_type (direction); + } + } + else + { + ctx->active = 1; + vect_from_polar_uf016 (&ctx->active_position, dist, + robot_position.a); + vect_translate (&ctx->active_position, &robot_position.v); + } + } + } + else + { + ctx->active = 0; + } + return 0; +} + diff --git a/digital/io-hub/src/robospierre/pawn_sensor.h b/digital/io-hub/src/robospierre/pawn_sensor.h new file mode 100644 index 00000000..14ce2d62 --- /dev/null +++ b/digital/io-hub/src/robospierre/pawn_sensor.h @@ -0,0 +1,33 @@ +#ifndef pawn_sensor_h +#define pawn_sensor_h +/* pawn_sensor.h */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +/** Update sensor state and return the element type if an element is ready to + * be taken. */ +uint8_t +pawn_sensor_get (uint8_t direction); + +#endif /* pawn_sensor_h */ diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index a3476a46..0af4ccf3 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -220,7 +220,7 @@ class Clamp (Observable): m = self.__get_robot_matrix () # Look up elements. xoffset = (self.BAY_OFFSET, -self.BAY_OFFSET)[side] - xmargin = 20 + xmargin = 40 ymargin = 50 for o in self.table.obstacles: if o.level == 1 and o.pos is not None: -- cgit v1.2.3 From 51b917f7994d6d53fde11263ba4da3470fd2a6c4 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 1 Jun 2011 15:26:22 +0200 Subject: digital/io-hub, host/simu: add codebar --- digital/io-hub/src/robospierre/Makefile | 1 + digital/io-hub/src/robospierre/codebar.avr.c | 64 +++++++++++++++++++++++++++ digital/io-hub/src/robospierre/codebar.h | 36 +++++++++++++++ digital/io-hub/src/robospierre/codebar.host.c | 56 +++++++++++++++++++++++ digital/io-hub/src/robospierre/main.c | 15 +++++++ digital/io-hub/src/robospierre/pawn_sensor.c | 5 ++- digital/io-hub/tools/io_hub/mex.py | 43 ++++++++++++++++++ host/simu/robots/robospierre/model/bag.py | 2 +- host/simu/robots/robospierre/model/clamp.py | 15 +++++-- 9 files changed, 231 insertions(+), 6 deletions(-) create mode 100644 digital/io-hub/src/robospierre/codebar.avr.c create mode 100644 digital/io-hub/src/robospierre/codebar.h create mode 100644 digital/io-hub/src/robospierre/codebar.host.c (limited to 'host/simu/robots/robospierre/model/clamp.py') diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile index 714541dc..88e6c79e 100644 --- a/digital/io-hub/src/robospierre/Makefile +++ b/digital/io-hub/src/robospierre/Makefile @@ -6,6 +6,7 @@ HOST_PROGS = test_element # Sources to compile. io_hub_SOURCES = main.c top.c \ clamp.c logistic.c element.c pawn_sensor.c \ + codebar.avr.c codebar.host.c \ radar_defs.c radar.c path.c move.c \ init.c fsm.host.c fsm_AI_gen.avr.c fsm_queue.c \ pwm.avr.c pwm.host.c \ diff --git a/digital/io-hub/src/robospierre/codebar.avr.c b/digital/io-hub/src/robospierre/codebar.avr.c new file mode 100644 index 00000000..d2609012 --- /dev/null +++ b/digital/io-hub/src/robospierre/codebar.avr.c @@ -0,0 +1,64 @@ +/* codebar.avr.c */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "codebar.h" + +#include "defs.h" + +#include "modules/twi/twi.h" +#include "modules/utils/utils.h" +#include "modules/utils/crc.h" +#include "modules/utils/byte.h" + +#define CODEBAR_ADDRESS 0x20 +#define CODEBAR_STATUS_LENGTH 7 + +void +codebar_init (void) +{ +} + +uint8_t +codebar_get (uint8_t direction) +{ + uint8_t buffer[CODEBAR_STATUS_LENGTH]; + /* Read status. */ + twi_master_recv (CODEBAR_ADDRESS, buffer, sizeof (buffer)); + uint8_t ret = twi_master_wait (); + if (ret != CODEBAR_STATUS_LENGTH) + return 0; + uint8_t crc = crc_compute (buffer + 1, CODEBAR_STATUS_LENGTH - 1); + if (crc != buffer[0]) + return 0; + /* Get data. */ + uint8_t offset = direction == DIRECTION_FORWARD ? 1 : 4; + uint16_t age = v8_to_v16 (buffer[offset], buffer[offset + 1]); + uint16_t type = buffer[offset + 2]; + if (age > 225) + return 0; + else + return type; +} + diff --git a/digital/io-hub/src/robospierre/codebar.h b/digital/io-hub/src/robospierre/codebar.h new file mode 100644 index 00000000..d334f131 --- /dev/null +++ b/digital/io-hub/src/robospierre/codebar.h @@ -0,0 +1,36 @@ +#ifndef codebar_h +#define codebar_h +/* codebar.h */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +/** Initialise module. */ +void +codebar_init (void); + +/** Get element type on the specified direction. */ +uint8_t +codebar_get (uint8_t direction); + +#endif /* codebar_h */ diff --git a/digital/io-hub/src/robospierre/codebar.host.c b/digital/io-hub/src/robospierre/codebar.host.c new file mode 100644 index 00000000..68ced300 --- /dev/null +++ b/digital/io-hub/src/robospierre/codebar.host.c @@ -0,0 +1,56 @@ +/* codebar.host.c */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "codebar.h" +#include "defs.h" + +#include "modules/host/host.h" +#include "modules/host/mex.h" + +uint8_t codebar_front, codebar_back; + +static void +codebar_handle (void *user, mex_msg_t *msg) +{ + mex_msg_pop (msg, "BB", &codebar_front, &codebar_back); +} + +void +codebar_init (void) +{ + const char *mex_instance = host_get_instance ("io-hub0", 0); + uint8_t mtype = mex_node_reservef ("%s:codebar", mex_instance); + mex_node_register (mtype, codebar_handle, 0); +} + +uint8_t +codebar_get (uint8_t direction) +{ + if (direction == DIRECTION_FORWARD) + return codebar_front; + else + return codebar_back; +} + diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 79c0fe2a..42a2809a 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -40,6 +40,7 @@ #include "pwm.h" #include "contact.h" +#include "codebar.h" #include "radar.h" #define FSM_NAME AI @@ -77,6 +78,9 @@ static uint8_t main_stats_asserv_, main_stats_asserv_cpt_; /** Contact stats counters. */ static uint8_t main_stats_contact_, main_stats_contact_cpt_; +/** Codebar stats counters. */ +static uint8_t main_stats_codebar_, main_stats_codebar_cpt_; + /** US sensors stats counters. */ static uint8_t main_stats_usdist_, main_stats_usdist_cpt_; @@ -103,6 +107,7 @@ main_init (void) /* IO modules. */ pwm_init (); contact_init (); + codebar_init (); usdist_init (); /* AI modules. */ clamp_init (); @@ -223,6 +228,12 @@ main_loop (void) proto_send1d ('P', contact_all ()); main_stats_contact_cpt_ = main_stats_contact_; } + if (main_stats_codebar_ && !--main_stats_codebar_cpt_) + { + proto_send2b ('B', codebar_get (DIRECTION_FORWARD), + codebar_get (DIRECTION_BACKWARD)); + main_stats_codebar_cpt_ = main_stats_codebar_; + } if (main_stats_usdist_ && !--main_stats_usdist_cpt_) { proto_send4w ('U', usdist_mm[0], usdist_mm[1], usdist_mm[2], @@ -331,6 +342,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Contact stats. */ main_stats_contact_ = main_stats_contact_cpt_ = args[0]; break; + case c ('B', 1): + /* Codebar stats. */ + main_stats_codebar_ = main_stats_codebar_cpt_ = args[0]; + break; case c ('U', 1): /* US sensors stats. */ main_stats_usdist_ = main_stats_usdist_cpt_ = args[0]; diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c index be6f3c87..cb4cfe4a 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.c +++ b/digital/io-hub/src/robospierre/pawn_sensor.c @@ -31,6 +31,7 @@ #include "element.h" #include "clamp.h" #include "bot.h" +#include "codebar.h" #include "modules/utils/utils.h" #include "modules/math/geometry/distance.h" @@ -53,7 +54,9 @@ struct pawn_sensor_t pawn_sensor_front, pawn_sensor_back; static uint8_t pawn_sensor_get_type (uint8_t direction) { - uint8_t element_type = IO_GET (CONTACT_STRAT) ? ELEMENT_PAWN : ELEMENT_KING; + uint8_t element_type = codebar_get (direction); + if (!element_type) + element_type = ELEMENT_PAWN; return element_type; } diff --git a/digital/io-hub/tools/io_hub/mex.py b/digital/io-hub/tools/io_hub/mex.py index 8d758382..9b72ccde 100644 --- a/digital/io-hub/tools/io_hub/mex.py +++ b/digital/io-hub/tools/io_hub/mex.py @@ -121,6 +121,46 @@ class Mex: m.push ('L', self.contacts) self.node.send (m) + class Codebar (Observable): + """Codebar stub. + + - element_type: 'queen', 'king', or anything else. + + """ + + def __init__ (self, pack, index): + Observable.__init__ (self) + self.pack = pack + self.index = index + self.element_type = None + self.register (self.__notified) + + def __notified (self): + self.pack.set (self.index, self.element_type) + + class Pack: + """Handle emission of several codebar for one message.""" + + def __init__ (self, node, instance): + self.node = node + self.codebars = [0, 0] + self.mtype = node.reserve (instance + ':codebar') + + def set (self, index, element_type): + if element_type == 'queen': + self.codebars[index] = 4 + elif element_type == 'king': + self.codebars[index] = 8 + else: + self.codebars[index] = 0 + self.__send () + + def __send (self): + m = simu.mex.msg.Msg (self.mtype) + for c in self.codebars: + m.push ('b', c) + self.node.send (m) + class Path (Observable): """Path finding algorithm report. @@ -167,6 +207,9 @@ class Mex: self.__contact_pack = self.Contact.Pack (node, instance) self.contact = tuple (self.Contact (self.__contact_pack, i) for i in range (CONTACT_NB)) + self.__codebar_pack = self.Codebar.Pack (node, instance) + self.codebar = tuple (self.Codebar (self.__codebar_pack, i) + for i in (0, 1)) self.path = self.Path (node, instance) self.pos_report = self.PosReport (node, instance) diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py index 284e5bb8..b9d3bc3f 100644 --- a/host/simu/robots/robospierre/model/bag.py +++ b/host/simu/robots/robospierre/model/bag.py @@ -44,7 +44,7 @@ class Bag: 8 * 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, self.door_motors, - self.contact[0:7]) + self.contact[0:7], link_bag.io_hub.codebar) 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 0af4ccf3..dbcab5f2 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -30,13 +30,14 @@ from math import pi, cos, sin class Slot: """Slot which can contain a pawn.""" - def __init__ (self, x, y, z, side, door_motor, contact): + def __init__ (self, x, y, z, side, door_motor, contact, codebar = None): self.x = x self.y = y self.z = z self.side = side self.door_motor = door_motor self.contact = contact + self.codebar = codebar self.pawn = None class Clamp (Observable): @@ -62,7 +63,8 @@ class Clamp (Observable): SLOT_SIDE = 6 def __init__ (self, table, robot_position, elevation_motor, - rotation_motor, clamping_motor, door_motors, slot_contacts): + rotation_motor, clamping_motor, door_motors, slot_contacts, + codebars): Observable.__init__ (self) self.table = table self.robot_position = robot_position @@ -75,13 +77,13 @@ class Clamp (Observable): door_motors[2], None, door_motors[3], None) self.slots = ( Slot (self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 0, - door_motors[0], slot_contacts[0]), + door_motors[0], slot_contacts[0], codebars[0]), Slot (self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 0, None, slot_contacts[1]), Slot (self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 0, door_motors[1], slot_contacts[2]), Slot (-self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 1, - door_motors[2], slot_contacts[3]), + door_motors[2], slot_contacts[3], codebars[1]), Slot (-self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 1, None, slot_contacts[4]), Slot (-self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 1, @@ -203,6 +205,11 @@ class Clamp (Observable): and slots[0].pawn.kind == 'tower')) # This one is really high. slots[2].contact.state = not (slots[2].pawn is not None) + if slots[0].pawn: + slots[0].codebar.element_type = slots[0].pawn.kind + else: + slots[0].codebar.element_type = None + slots[0].codebar.notify () slot_side = self.slots[self.SLOT_SIDE] slot_side.contact.state = slot_side.pawn is None clamp_slot = self.__get_clamp_slot () -- cgit v1.2.3 From 4eb705e639457a07e6d5e9423ba81f8cede1b979 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 04:37:35 +0200 Subject: digital/io-hub: handle element change to head --- digital/io-hub/src/robospierre/clamp.c | 39 +++++++++++++++++++++++++++-- digital/io-hub/src/robospierre/logistic.c | 8 ++++++ digital/io-hub/src/robospierre/logistic.h | 4 +++ host/simu/robots/robospierre/model/clamp.py | 6 ++--- 4 files changed, 52 insertions(+), 5 deletions(-) (limited to 'host/simu/robots/robospierre/model/clamp.py') diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index e05f79d7..6b48c56a 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -373,11 +373,17 @@ clamp_route (void) } else if (pos_current == CLAMP_BAY_FRONT_LEAVE) { - pos_new = CLAMP_BAY_FRONT_LEAVING; + if (CLAMP_IS_SLOT_IN_FRONT_BAY (pos_request)) + pos_new = pos_request; + else + pos_new = CLAMP_BAY_FRONT_LEAVING; } else if (pos_current == CLAMP_BAY_BACK_LEAVE) { - pos_new = CLAMP_BAY_BACK_LEAVING; + if (CLAMP_IS_SLOT_IN_BACK_BAY (pos_request)) + pos_new = pos_request; + else + pos_new = CLAMP_BAY_BACK_LEAVING; } else if (pos_current == CLAMP_BAY_FRONT_LEAVING) { @@ -417,6 +423,34 @@ clamp_route (void) ctx.pos_current = pos_new; } +/* When lifting an element, we can discover it is actually a head. In this + * case, change destination. */ +void +clamp_change (void) +{ + uint8_t from = logistic_global.moving_from; + if (logistic_global.slots[from] == ELEMENT_PAWN) + { + /* Look head contact. */ + uint8_t contact_head = 0; + if (from == CLAMP_SLOT_FRONT_BOTTOM) + contact_head = !IO_GET (CONTACT_FRONT_TOP); + else if (from == CLAMP_SLOT_BACK_BOTTOM) + contact_head = !IO_GET (CONTACT_BACK_TOP); + /* Change? */ + if (contact_head) + { + logistic_element_change (from, ELEMENT_KING); + if (logistic_global.moving_from != from) + /* Cancel move. */ + ctx.pos_request = ctx.moving_to = from; + else + /* Change move. */ + ctx.pos_request = ctx.moving_to = logistic_global.moving_to; + } + } +} + static void clamp_blocked (void) { @@ -861,6 +895,7 @@ FSM_TRANS (CLAMP_MOVE_DST_ROUTING, clamp_elevation_rotation_success, done_open_clamp, CLAMP_MOVE_DST_CLAMP_OPENING, next, CLAMP_MOVE_DST_ROUTING) { + clamp_change (); if (ctx.pos_current == ctx.pos_request) { if (clamp_slot_door[ctx.pos_current] != 0xff) diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index dea6e0a5..f59f2da4 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -533,6 +533,14 @@ logistic_element_new (uint8_t pos, uint8_t element_type) logistic_decision (); } +void +logistic_element_change (uint8_t pos, uint8_t element_type) +{ + assert (pos < CLAMP_SLOT_NB); + ctx.slots[pos] = element_type; + logistic_decision (); +} + void logistic_element_move_done (void) { diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h index 6acb681e..4a06dcb7 100644 --- a/digital/io-hub/src/robospierre/logistic.h +++ b/digital/io-hub/src/robospierre/logistic.h @@ -115,6 +115,10 @@ logistic_update (void); void logistic_element_new (uint8_t pos, uint8_t element_type); +/** Oh la la, the pawn was not a pawn, it's a head. */ +void +logistic_element_change (uint8_t pos, uint8_t element_type); + /** To be called when a element movement is done. */ void logistic_element_move_done (void); diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index dbcab5f2..c199259e 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -203,8 +203,7 @@ class Clamp (Observable): slots[1].pawn is not None or (slots[0].pawn is not None and slots[0].pawn.kind == 'tower')) - # This one is really high. - slots[2].contact.state = not (slots[2].pawn is not None) + slots[2].contact.state = True if slots[0].pawn: slots[0].codebar.element_type = slots[0].pawn.kind else: @@ -213,7 +212,8 @@ class Clamp (Observable): slot_side = self.slots[self.SLOT_SIDE] slot_side.contact.state = slot_side.pawn is None clamp_slot = self.__get_clamp_slot () - if clamp_slot is not None: + if clamp_slot is not None and clamp_slot != self.SLOT_FRONT_TOP \ + and clamp_slot != self.SLOT_BACK_TOP: clamp_slot.contact.state = False for slot in self.slots: slot.contact.notify () -- cgit v1.2.3 From f9f6fbde971e8a7d4698e197b09a0ea2dd8a8526 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 05:22:43 +0200 Subject: host/simu/robots/robospierre: fix level 2 drop --- host/simu/robots/robospierre/model/clamp.py | 3 +++ 1 file changed, 3 insertions(+) (limited to 'host/simu/robots/robospierre/model/clamp.py') diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index c199259e..d32edf25 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -193,6 +193,9 @@ class Clamp (Observable): and slots[2].pawn and slots[2].door_motor.angle: slots[0].pawn.tower.append (slots[2].pawn) slots[2].pawn = None + if slots[0].pawn is None and slots[1].pawn is None \ + and slots[2].pawn and slots[2].door_motor.angle: + slots[0].pawn, slots[2].pawn = slots[2].pawn, None def update_contacts (self): """Update pawn contacts.""" -- cgit v1.2.3 From 53483bc33fd092d70be211fcaba1db5372e1706d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 16:35:00 +0200 Subject: digital/io-hub: fix bug in head detection workaround --- digital/io-hub/src/robospierre/clamp.c | 25 +++++++++++++++++++++---- digital/io-hub/src/robospierre/logistic.c | 2 +- host/simu/robots/robospierre/model/clamp.py | 2 +- 3 files changed, 23 insertions(+), 6 deletions(-) (limited to 'host/simu/robots/robospierre/model/clamp.py') diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index f91af6a0..a74c643c 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -161,6 +161,8 @@ struct clamp_t uint8_t open; /** True if clamp position is controled. */ uint8_t controled; + /** Contact state at start of move, for head check. */ + uint8_t contact_head_before_move; }; /** Global context. */ @@ -212,6 +214,9 @@ clamp_openclose (uint8_t open); static void clamp_route (void); +static void +clamp_head_check_prepare (uint8_t from); + void clamp_init (void) { @@ -243,6 +248,7 @@ clamp_move_element (uint8_t from, uint8_t to) assert (from != to); ctx.pos_request = from; ctx.moving_to = to; + clamp_head_check_prepare (from); FSM_HANDLE (AI, clamp_move); } @@ -435,11 +441,11 @@ clamp_taken_pawn (uint8_t element_type) /* When lifting an element, we can discover it is actually a head. In this * case, change destination. */ -void -clamp_change (void) +static void +clamp_head_check (void) { uint8_t from = logistic_global.moving_from; - if (logistic_global.slots[from] == ELEMENT_PAWN) + if (!ctx.contact_head_before_move && logistic_global.slots[from] == ELEMENT_PAWN) { /* Look head contact. */ uint8_t contact_head = 0; @@ -462,6 +468,17 @@ clamp_change (void) } } +/** Prepare head check, if contact is set yet, this is not a new head. */ +static void +clamp_head_check_prepare (uint8_t from) +{ + ctx.contact_head_before_move = 0; + if (CLAMP_IS_SLOT_IN_FRONT_BAY (from)) + ctx.contact_head_before_move = !IO_GET (CONTACT_FRONT_TOP); + else if (CLAMP_IS_SLOT_IN_BACK_BAY (from)) + ctx.contact_head_before_move = !IO_GET (CONTACT_BACK_TOP); +} + static void clamp_blocked (void) { @@ -909,7 +926,7 @@ FSM_TRANS (CLAMP_MOVE_DST_ROUTING, clamp_elevation_rotation_success, done_open_clamp, CLAMP_MOVE_DST_CLAMP_OPENING, next, CLAMP_MOVE_DST_ROUTING) { - clamp_change (); + clamp_head_check (); if (ctx.pos_current == ctx.pos_request) { if (clamp_slot_door[ctx.pos_current] != 0xff) diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index f59f2da4..f9b13427 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -544,7 +544,7 @@ logistic_element_change (uint8_t pos, uint8_t element_type) void logistic_element_move_done (void) { - assert (!ctx.slots[ctx.moving_to]); + assert (!ctx.slots[ctx.moving_to] || ctx.moving_to == ctx.moving_from); ctx.slots[ctx.moving_to] = ctx.slots[ctx.moving_from]; ctx.slots[ctx.moving_from] = 0; ctx.moving_from = ctx.moving_to = CLAMP_SLOT_NB; diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index d32edf25..4bbe988f 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -206,7 +206,7 @@ class Clamp (Observable): slots[1].pawn is not None or (slots[0].pawn is not None and slots[0].pawn.kind == 'tower')) - slots[2].contact.state = True + slots[2].contact.state = not (slots[2] is not None) if slots[0].pawn: slots[0].codebar.element_type = slots[0].pawn.kind else: -- cgit v1.2.3