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 --- digital/io-hub/src/robospierre/clamp.c | 169 ++++++++++++++++++++++++++++++--- 1 file changed, 155 insertions(+), 14 deletions(-) (limited to 'digital/io-hub/src/robospierre/clamp.c') diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 2f1b2471..5d9533fa 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -29,15 +29,28 @@ #include "fsm.h" #include "mimot.h" +#include "pwm.h" #include "bot.h" FSM_INIT FSM_STATES ( - /* Wait order. */ - CLAMP_IDLE, + /* Waiting movement order. */ + CLAMP_MOVE_IDLE, /* Moving to a final or intermediary position. */ - CLAMP_ROUTING) + CLAMP_MOVE_ROUTING, + /* Moving to source slot. */ + CLAMP_MOVE_SRC_ROUTING, + /* Closing the clamp once arrived at source. */ + CLAMP_MOVE_SRC_CLAMP_CLOSING, + /* Opening door once clamp closed. */ + CLAMP_MOVE_SRC_DOOR_OPENDING, + /* Moving to destination slot. */ + CLAMP_MOVE_DST_ROUTING, + /* Closing door once arrived at destination. */ + CLAMP_MOVE_DST_DOOR_CLOSING, + /* Opening the clamp once door closed. */ + CLAMP_MOVE_DST_CLAMP_OPENING) FSM_EVENTS ( /* Order to move the clamp. */ @@ -49,15 +62,17 @@ FSM_EVENTS ( /* Rotation motor failure. */ clamp_rotation_failure) -FSM_START_WITH (CLAMP_IDLE) +FSM_START_WITH (CLAMP_MOVE_IDLE) /** Clamp context. */ struct clamp_t { - /* Current position. */ + /** Current position. */ uint8_t pos_current; - /* Requested position. */ + /** Requested position. */ uint8_t pos_request; + /** Element moving destination. */ + uint8_t moving_to; }; /** Global context. */ @@ -88,16 +103,36 @@ static const uint16_t clamp_pos[][2] = { BOT_CLAMP_BAY_SIDE_ROTATION_STEP }, }; +/** Slot doors. */ +static const uint8_t clamp_slot_door[] = { + BOT_PWM_DOOR_FRONT_BOTTOM, + 0xff, + BOT_PWM_DOOR_FRONT_TOP, + BOT_PWM_DOOR_BACK_BOTTOM, + 0xff, + BOT_PWM_DOOR_BACK_TOP, + 0xff +}; + void clamp_move (uint8_t pos) { if (pos != ctx.pos_current) { ctx.pos_request = pos; + ctx.moving_to = CLAMP_POS_NB; FSM_HANDLE (AI, clamp_move); } } +void +clamp_move_element (uint8_t from, uint8_t to) +{ + ctx.pos_request = from; + ctx.moving_to = to; + FSM_HANDLE (AI, clamp_move); +} + /** Find next position and start motors. */ static void clamp_route (void) @@ -157,26 +192,132 @@ clamp_route (void) ctx.pos_current = pos_new; } -FSM_TRANS (CLAMP_IDLE, clamp_move, CLAMP_ROUTING) +FSM_TRANS (CLAMP_MOVE_IDLE, clamp_move, + move, CLAMP_MOVE_ROUTING, + move_element, CLAMP_MOVE_SRC_ROUTING, + move_element_here, CLAMP_MOVE_SRC_CLAMP_CLOSING) { - clamp_route (); - return FSM_NEXT (CLAMP_IDLE, clamp_move); + if (ctx.moving_to == CLAMP_POS_NB) + { + clamp_route (); + return FSM_NEXT (CLAMP_MOVE_IDLE, clamp_move, move); + } + else + { + if (ctx.pos_current != ctx.pos_request) + { + clamp_route (); + return FSM_NEXT (CLAMP_MOVE_IDLE, clamp_move, move_element); + } + else + { + pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_CLOSE); + return FSM_NEXT (CLAMP_MOVE_IDLE, clamp_move, move_element_here); + } + } } -FSM_TRANS (CLAMP_ROUTING, clamp_elevation_rotation_success, - done, CLAMP_IDLE, - next, CLAMP_ROUTING) +FSM_TRANS (CLAMP_MOVE_ROUTING, clamp_elevation_rotation_success, + done, CLAMP_MOVE_IDLE, + next, CLAMP_MOVE_ROUTING) { if (ctx.pos_current == ctx.pos_request) { - return FSM_NEXT (CLAMP_ROUTING, clamp_elevation_rotation_success, + return FSM_NEXT (CLAMP_MOVE_ROUTING, clamp_elevation_rotation_success, done); } else { clamp_route (); - return FSM_NEXT (CLAMP_ROUTING, clamp_elevation_rotation_success, + return FSM_NEXT (CLAMP_MOVE_ROUTING, clamp_elevation_rotation_success, next); } } +FSM_TRANS (CLAMP_MOVE_SRC_ROUTING, clamp_elevation_rotation_success, + done, CLAMP_MOVE_SRC_CLAMP_CLOSING, + next, CLAMP_MOVE_SRC_ROUTING) +{ + if (ctx.pos_current == ctx.pos_request) + { + pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_CLOSE); + return FSM_NEXT (CLAMP_MOVE_SRC_ROUTING, + clamp_elevation_rotation_success, done); + } + else + { + clamp_route (); + return FSM_NEXT (CLAMP_MOVE_SRC_ROUTING, + clamp_elevation_rotation_success, next); + } +} + +FSM_TRANS_TIMEOUT (CLAMP_MOVE_SRC_CLAMP_CLOSING, BOT_PWM_CLAMP_CLOSE_TIME, + open_door, CLAMP_MOVE_SRC_DOOR_OPENDING, + move, CLAMP_MOVE_DST_ROUTING) +{ + if (clamp_slot_door[ctx.pos_current] != 0xff) + { + pwm_set_timed (clamp_slot_door[ctx.pos_current], BOT_PWM_DOOR_OPEN); + return FSM_NEXT_TIMEOUT (CLAMP_MOVE_SRC_CLAMP_CLOSING, open_door); + } + else + { + ctx.pos_request = ctx.moving_to; + clamp_route (); + return FSM_NEXT_TIMEOUT (CLAMP_MOVE_SRC_CLAMP_CLOSING, move); + } +} + +FSM_TRANS_TIMEOUT (CLAMP_MOVE_SRC_DOOR_OPENDING, BOT_PWM_DOOR_OPEN_TIME, + CLAMP_MOVE_DST_ROUTING) +{ + ctx.pos_request = ctx.moving_to; + clamp_route (); + return FSM_NEXT_TIMEOUT (CLAMP_MOVE_SRC_DOOR_OPENDING); +} + +FSM_TRANS (CLAMP_MOVE_DST_ROUTING, clamp_elevation_rotation_success, + done_close_door, CLAMP_MOVE_DST_DOOR_CLOSING, + done_open_clamp, CLAMP_MOVE_DST_CLAMP_OPENING, + next, CLAMP_MOVE_DST_ROUTING) +{ + if (ctx.pos_current == ctx.pos_request) + { + if (clamp_slot_door[ctx.pos_current] != 0xff) + { + pwm_set_timed (clamp_slot_door[ctx.pos_current], + BOT_PWM_DOOR_CLOSE); + return FSM_NEXT (CLAMP_MOVE_DST_ROUTING, + clamp_elevation_rotation_success, + done_close_door); + } + else + { + pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_OPEN); + return FSM_NEXT (CLAMP_MOVE_DST_ROUTING, + clamp_elevation_rotation_success, + done_open_clamp); + } + } + else + { + clamp_route (); + return FSM_NEXT (CLAMP_MOVE_DST_ROUTING, + clamp_elevation_rotation_success, next); + } +} + +FSM_TRANS_TIMEOUT (CLAMP_MOVE_DST_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME, + CLAMP_MOVE_DST_CLAMP_OPENING) +{ + pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_OPEN); + return FSM_NEXT_TIMEOUT (CLAMP_MOVE_DST_DOOR_CLOSING); +} + +FSM_TRANS_TIMEOUT (CLAMP_MOVE_DST_CLAMP_OPENING, BOT_PWM_CLAMP_OPEN_TIME, + CLAMP_MOVE_IDLE) +{ + return FSM_NEXT_TIMEOUT (CLAMP_MOVE_DST_CLAMP_OPENING); +} + -- cgit v1.2.3