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/io-hub/src/robospierre/bot.h | 14 ++++++ digital/io-hub/src/robospierre/clamp.c | 92 ++++++++++++++++++++++++++++++++-- digital/io-hub/src/robospierre/main.c | 4 ++ 3 files changed, 107 insertions(+), 3 deletions(-) (limited to 'digital/io-hub/src/robospierre') 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. */ -- cgit v1.2.3