From 5ba03e9744a8916730d9b8462cbfa6512d380ac6 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 14 May 2011 16:49:48 +0200 Subject: digital/io-hub: add drop --- digital/ai/tools/test_simu_control_robospierre.py | 21 ++++++- digital/io-hub/src/robospierre/clamp.c | 73 +++++++++++++++++++++++ digital/io-hub/src/robospierre/clamp.h | 9 +++ digital/io-hub/src/robospierre/logistic.c | 11 ++++ digital/io-hub/src/robospierre/logistic.h | 5 ++ digital/io-hub/src/robospierre/main.c | 8 +++ digital/io-hub/tools/io_hub/io_hub.py | 10 ++++ 7 files changed, 136 insertions(+), 1 deletion(-) diff --git a/digital/ai/tools/test_simu_control_robospierre.py b/digital/ai/tools/test_simu_control_robospierre.py index 88fbc5f3..acd3ca7c 100644 --- a/digital/ai/tools/test_simu_control_robospierre.py +++ b/digital/ai/tools/test_simu_control_robospierre.py @@ -82,12 +82,21 @@ class TestSimuControl (TestSimu): text = 'Move element', padx = 0, pady = 0, command = self.clamp_move_element_command) self.clamp_element_move_button.pack () + self.drop_var = IntVar () + self.drop_button = Checkbutton (self.control_frame, text = 'Drop', + indicatoron = False, + variable = self.drop_var, command = self.drop_command) + self.drop_button.pack () + self.backward_var = IntVar () + self.backward_button = Checkbutton (self.control_frame, + text = 'Backward', variable = self.backward_var) + self.backward_button.pack () self.table_view.bind ('<1>', self.move) self.table_view.bind ('<3>', self.orient) def move (self, ev): pos = self.table_view.screen_coord ((ev.x, ev.y)) - self.asserv.goto (pos[0], pos[1]) + self.asserv.goto (pos[0], pos[1], self.backward_var.get ()) def orient (self, ev): x, y = self.table_view.screen_coord ((ev.x, ev.y)) @@ -129,6 +138,16 @@ class TestSimuControl (TestSimu): for i in (0, 1, 3, 4): self.io.pwm_set_timed (i, pwm, 255, 0) + def drop_command (self): + if self.drop_var.get (): + if self.backward_var.get (): + order = 'drop_backward' + else: + order = 'drop_forward' + else: + order = 'drop_clear' + self.io.drop (order) + def change_color (self, *dummy): pass diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 227579ff..6c063979 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -60,6 +60,10 @@ FSM_STATES ( CLAMP_TAKING_DOOR_CLOSING, /* Moving elements around. */ CLAMP_MOVING_ELEMENT, + /* Droping a tower. */ + CLAMP_DROPING_DOOR_OPENING, + /* Droping a tower, waiting for robot to advance. */ + CLAMP_DROPING_WAITING_ROBOT, /* Waiting movement order. */ CLAMP_MOVE_IDLE, @@ -83,6 +87,13 @@ FSM_EVENTS ( start, /* New element inside bottom slot. */ clamp_new_element, + /* Order to drop elements. */ + clamp_drop, + /* Sent once drop is done, but robot should advance to completely + * free the dropped tower. */ + clamp_drop_waiting, + /* Received when top FSM made the robot advance after a drop. */ + clamp_drop_clear, /* Order to move the clamp. */ clamp_move, /* Clamp movement success. */ @@ -110,6 +121,8 @@ struct clamp_t uint8_t pos_new; /** New element kind. */ uint8_t new_element; + /** Drop direction, drop on the other side. */ + uint8_t drop_direction; }; /** Global context. */ @@ -189,6 +202,25 @@ clamp_new_element (uint8_t pos, uint8_t element) FSM_HANDLE (AI, clamp_new_element); } +uint8_t +clamp_drop (uint8_t drop_direction) +{ + if (FSM_CAN_HANDLE (AI, clamp_drop)) + { + ctx.drop_direction = drop_direction; + FSM_HANDLE (AI, clamp_drop); + return 1; + } + else + return 0; +} + +void +clamp_drop_clear (void) +{ + FSM_HANDLE (AI, clamp_drop_clear); +} + uint8_t clamp_handle_event (void) { @@ -310,6 +342,16 @@ FSM_TRANS (CLAMP_IDLE, clamp_new_element, CLAMP_TAKING_DOOR_CLOSING) return FSM_NEXT (CLAMP_IDLE, clamp_new_element); } +FSM_TRANS (CLAMP_IDLE, clamp_drop, CLAMP_DROPING_DOOR_OPENING) +{ + /* If going forward, drop at back. */ + uint8_t bay = ctx.drop_direction == DIRECTION_FORWARD + ? CLAMP_SLOT_BACK_BOTTOM : CLAMP_SLOT_FRONT_BOTTOM; + pwm_set_timed (clamp_slot_door[bay + 0], BOT_PWM_DOOR_OPEN); + pwm_set_timed (clamp_slot_door[bay + 2], BOT_PWM_DOOR_OPEN); + return FSM_NEXT (CLAMP_IDLE, clamp_drop); +} + FSM_TRANS_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME, move_element, CLAMP_MOVING_ELEMENT, move_to_idle, CLAMP_GOING_IDLE, @@ -355,6 +397,37 @@ FSM_TRANS (CLAMP_MOVING_ELEMENT, clamp_move_success, done); } +FSM_TRANS_TIMEOUT (CLAMP_DROPING_DOOR_OPENING, BOT_PWM_CLAMP_OPEN_TIME, + CLAMP_DROPING_WAITING_ROBOT) +{ + fsm_queue_post_event (FSM_EVENT (AI, clamp_drop_waiting)); + return FSM_NEXT_TIMEOUT (CLAMP_DROPING_DOOR_OPENING); +} + +FSM_TRANS (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, + move_element, CLAMP_MOVING_ELEMENT, + move_to_idle, CLAMP_GOING_IDLE, + done, CLAMP_IDLE) +{ + logistic_drop (ctx.drop_direction); + if (logistic_global.moving_from != CLAMP_SLOT_NB) + { + clamp_move_element (logistic_global.moving_from, + logistic_global.moving_to); + return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, + move_element); + } + else if (logistic_global.clamp_pos_idle != ctx.pos_current) + { + clamp_move (logistic_global.clamp_pos_idle); + return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, + move_to_idle); + } + else + return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, + done); +} + /* CLAMP_MOVE FSM */ FSM_TRANS (CLAMP_MOVE_IDLE, clamp_move, diff --git a/digital/io-hub/src/robospierre/clamp.h b/digital/io-hub/src/robospierre/clamp.h index bb15b62e..a59e91ae 100644 --- a/digital/io-hub/src/robospierre/clamp.h +++ b/digital/io-hub/src/robospierre/clamp.h @@ -68,6 +68,15 @@ clamp_move (uint8_t pos); void clamp_move_element (uint8_t from, uint8_t to); +/** Drop an element tower. Return 0 if not currently possible. If + * drop_direction is forward, drop at the back. */ +uint8_t +clamp_drop (uint8_t drop_direction); + +/** Signal robot advanced, and drop is finished. */ +void +clamp_drop_clear (void); + /** Examine sensors to generate new events, return non zero if an event was * generated. */ uint8_t diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index 534f1c46..945bd583 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -210,3 +210,14 @@ logistic_element_move_done (void) logistic_decision (); } +void +logistic_drop (uint8_t direction) +{ + uint8_t bay = direction == DIRECTION_FORWARD + ? CLAMP_SLOT_BACK_BOTTOM : CLAMP_SLOT_FRONT_BOTTOM; + uint8_t i; + for (i = bay; i < bay + 3; i++) + ctx.slots[i] = 0; + logistic_decision (); +} + diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h index 75b49d53..c6aa4159 100644 --- a/digital/io-hub/src/robospierre/logistic.h +++ b/digital/io-hub/src/robospierre/logistic.h @@ -65,4 +65,9 @@ logistic_element_new (uint8_t pos, uint8_t element); void logistic_element_move_done (void); +/** To be called when elements have been dropped on the opposite side of + * direction. */ +void +logistic_drop (uint8_t direction); + #endif /* logistic_h */ diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index ffb05a99..938703b6 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -222,6 +222,14 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) 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; + case c ('d', 1): + /* Drop elements. + * - 1b: 00: drop clear, 01: drop forward, 02: drop backward. */ + if (args[0] == 0x00) + clamp_drop_clear (); + else + clamp_drop (args[0]); + 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 1b25102c..d6ae8618 100644 --- a/digital/io-hub/tools/io_hub/io_hub.py +++ b/digital/io-hub/tools/io_hub/io_hub.py @@ -50,6 +50,16 @@ class Proto: def clamp_move_element (self, from_, to): self.proto.send ('c', 'BB', from_, to) + def drop (self, order): + if order == 'drop_clear': + self.proto.send ('d', 'B', 0x00) + elif order == 'drop_forward': + self.proto.send ('d', 'B', 0x01) + elif order == 'drop_backward': + self.proto.send ('d', 'B', 0x02) + else: + raise ValueError + def close (self): self.reset () self.proto.wait (lambda: True) -- cgit v1.2.3