From d385a9eacb2e8580239f101f96356f7ed216dd97 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 8 May 2011 13:02:53 +0200 Subject: digital/io-hub: add clamp moving --- digital/io-hub/src/robospierre/Makefile | 13 ++- digital/io-hub/src/robospierre/bot.h | 39 +++++++ digital/io-hub/src/robospierre/clamp.c | 182 ++++++++++++++++++++++++++++++++ digital/io-hub/src/robospierre/clamp.h | 57 ++++++++++ digital/io-hub/src/robospierre/main.c | 45 ++++++++ 5 files changed, 335 insertions(+), 1 deletion(-) create mode 100644 digital/io-hub/src/robospierre/clamp.c create mode 100644 digital/io-hub/src/robospierre/clamp.h (limited to 'digital/io-hub/src/robospierre') diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile index a95d7586..21411631 100644 --- a/digital/io-hub/src/robospierre/Makefile +++ b/digital/io-hub/src/robospierre/Makefile @@ -4,6 +4,8 @@ BASE = ../../../avr PROGS = io_hub # Sources to compile. io_hub_SOURCES = main.c \ + clamp.c \ + fsm.host.c fsm_AI_gen.avr.c \ pwm.avr.c pwm.host.c \ contact.avr.c contact.host.c \ twi_master.c asserv.c mimot.c \ @@ -11,7 +13,7 @@ io_hub_SOURCES = main.c \ # Modules needed for IO. MODULES = proto uart twi utils \ math/fixed math/geometry -AI_MODULES = twi_master common utils +AI_MODULES = twi_master common utils fsm # Configuration file. CONFIGFILE = avrconfig.h AVR_MCU = at90usb1287 @@ -25,4 +27,13 @@ vpath %.c $(AI_MODULES:%=../../../ai/src/%) vpath %.h $(AI_MODULES:%=../../../ai/src/%) INCLUDES += -I. -I../common $(AI_MODULES:%=-I../../../ai/src/%) +EXTRA_CLEAN_FILES = fsm_AI_gen.h fsm_AI_gen.avr.c + include $(BASE)/make/Makefile.gen + +# FSM generation. +obj/main.avr.o: fsm_AI_gen.h +fsm_AI_gen.avr.c: fsm_AI_gen.h +fsm_AI_gen.h: io_hub.host + ./$< --gen + mv fsm_AI_gen.c fsm_AI_gen.avr.c diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index fa65ca07..9043a62c 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -34,4 +34,43 @@ # define BOT_SCALE 0.0415178942124 #endif +#ifdef HOST + +# define BOT_CLAMP_SLOT_FRONT_BOTTOM_ELEVATION_STEP 0 +# define BOT_CLAMP_SLOT_FRONT_MIDDLE_ELEVATION_STEP (0x3b0b / 2) +# define BOT_CLAMP_SLOT_FRONT_TOP_ELEVATION_STEP 0x3b0b +# define BOT_CLAMP_SLOT_BACK_BOTTOM_ELEVATION_STEP 0 +# define BOT_CLAMP_SLOT_BACK_MIDDLE_ELEVATION_STEP (0x3b0b / 2) +# define BOT_CLAMP_SLOT_BACK_TOP_ELEVATION_STEP 0x3b0b +# define BOT_CLAMP_SLOT_SIDE_ELEVATION_STEP 0x3b0b +# define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP (0x3b0b / 2) +# define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP (0x3b0b / 2) +# define BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP (0x3b0b / 2) + +# define BOT_CLAMP_BAY_FRONT_ROTATION_STEP 0 +# define BOT_CLAMP_BAY_BACK_ROTATION_STEP 0x11c6 +# define BOT_CLAMP_BAY_SIDE_ROTATION_STEP (0x11c6 / 2) + +#else /* !HOST */ + +# define BOT_CLAMP_SLOT_FRONT_BOTTOM_ELEVATION_STEP 0 +# define BOT_CLAMP_SLOT_FRONT_MIDDLE_ELEVATION_STEP 0x1d83 +# define BOT_CLAMP_SLOT_FRONT_TOP_ELEVATION_STEP 0x3288 +# define BOT_CLAMP_SLOT_BACK_BOTTOM_ELEVATION_STEP 0 +# define BOT_CLAMP_SLOT_BACK_MIDDLE_ELEVATION_STEP 0x1d83 +# define BOT_CLAMP_SLOT_BACK_TOP_ELEVATION_STEP 0x3288 +# define BOT_CLAMP_SLOT_SIDE_ELEVATION_STEP 0x3288 +# define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP 0x1d83 +# define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP 0x1d83 +# define BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP 0x1d83 + +# define BOT_CLAMP_BAY_FRONT_ROTATION_STEP 0 +# define BOT_CLAMP_BAY_BACK_ROTATION_STEP 0x10e2 +# define BOT_CLAMP_BAY_SIDE_ROTATION_STEP 0x820 + +#endif /* !HOST */ + +#define BOT_CLAMP_ELEVATION_SPEED 0x60 +#define BOT_CLAMP_ROTATION_SPEED 0x30 + #endif /* bot_h */ diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c new file mode 100644 index 00000000..2f1b2471 --- /dev/null +++ b/digital/io-hub/src/robospierre/clamp.c @@ -0,0 +1,182 @@ +/* clamp.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 "clamp.h" + +#define FSM_NAME AI +#include "fsm.h" + +#include "mimot.h" +#include "bot.h" + +FSM_INIT + +FSM_STATES ( + /* Wait order. */ + CLAMP_IDLE, + /* Moving to a final or intermediary position. */ + CLAMP_ROUTING) + +FSM_EVENTS ( + /* Order to move the clamp. */ + clamp_move, + /* Elevation and elevation motor success. */ + clamp_elevation_rotation_success, + /* Elevation motor failure. */ + clamp_elevation_failure, + /* Rotation motor failure. */ + clamp_rotation_failure) + +FSM_START_WITH (CLAMP_IDLE) + +/** Clamp context. */ +struct clamp_t +{ + /* Current position. */ + uint8_t pos_current; + /* Requested position. */ + uint8_t pos_request; +}; + +/** Global context. */ +struct clamp_t clamp_global; +#define ctx clamp_global + +/** Clamp positions. */ +static const uint16_t clamp_pos[][2] = { + { BOT_CLAMP_SLOT_FRONT_BOTTOM_ELEVATION_STEP, + BOT_CLAMP_BAY_FRONT_ROTATION_STEP }, + { BOT_CLAMP_SLOT_FRONT_MIDDLE_ELEVATION_STEP, + BOT_CLAMP_BAY_FRONT_ROTATION_STEP }, + { BOT_CLAMP_SLOT_FRONT_TOP_ELEVATION_STEP, + BOT_CLAMP_BAY_FRONT_ROTATION_STEP }, + { BOT_CLAMP_SLOT_BACK_BOTTOM_ELEVATION_STEP, + BOT_CLAMP_BAY_BACK_ROTATION_STEP }, + { BOT_CLAMP_SLOT_BACK_MIDDLE_ELEVATION_STEP, + BOT_CLAMP_BAY_BACK_ROTATION_STEP }, + { BOT_CLAMP_SLOT_BACK_TOP_ELEVATION_STEP, + BOT_CLAMP_BAY_BACK_ROTATION_STEP }, + { BOT_CLAMP_SLOT_SIDE_ELEVATION_STEP, + BOT_CLAMP_BAY_SIDE_ROTATION_STEP }, + { BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP, + BOT_CLAMP_BAY_FRONT_ROTATION_STEP }, + { BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP, + BOT_CLAMP_BAY_BACK_ROTATION_STEP }, + { BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP, + BOT_CLAMP_BAY_SIDE_ROTATION_STEP }, +}; + +void +clamp_move (uint8_t pos) +{ + if (pos != ctx.pos_current) + { + ctx.pos_request = pos; + FSM_HANDLE (AI, clamp_move); + } +} + +/** Find next position and start motors. */ +static void +clamp_route (void) +{ + uint8_t pos_new; + uint8_t pos_current = ctx.pos_current; + uint8_t pos_request = ctx.pos_request; + /* Compute new position. */ + if (CLAMP_IS_SLOT_IN_FRONT_BAY (pos_current)) + { + if (!CLAMP_IS_SLOT_IN_FRONT_BAY (pos_request)) + pos_new = CLAMP_BAY_FRONT_LEAVE; + else + pos_new = pos_request; + } + else if (CLAMP_IS_SLOT_IN_BACK_BAY (pos_current)) + { + if (!CLAMP_IS_SLOT_IN_BACK_BAY (pos_request)) + pos_new = CLAMP_BAY_BACK_LEAVE; + else + pos_new = pos_request; + } + else if (pos_current == CLAMP_SLOT_SIDE) + { + pos_new = CLAMP_BAY_SIDE_ENTER_LEAVE; + } + else if (pos_current == CLAMP_BAY_FRONT_LEAVE) + { + if (pos_request == CLAMP_SLOT_SIDE) + pos_new = CLAMP_BAY_SIDE_ENTER_LEAVE; + else + pos_new = CLAMP_SLOT_BACK_MIDDLE; + } + else if (pos_current == CLAMP_BAY_BACK_LEAVE) + { + if (pos_request == CLAMP_SLOT_SIDE) + pos_new = CLAMP_BAY_SIDE_ENTER_LEAVE; + else + pos_new = CLAMP_SLOT_FRONT_MIDDLE; + } + else + { + assert (pos_current == CLAMP_BAY_SIDE_ENTER_LEAVE); + if (pos_request == CLAMP_SLOT_SIDE) + pos_new = pos_request; + else if (CLAMP_IS_SLOT_IN_FRONT_BAY (pos_request)) + pos_new = CLAMP_SLOT_FRONT_MIDDLE; + else + pos_new = CLAMP_SLOT_BACK_MIDDLE; + } + /* Run motors. */ + mimot_move_motor0_absolute (clamp_pos[pos_new][0], + BOT_CLAMP_ELEVATION_SPEED); + mimot_move_motor1_absolute (clamp_pos[pos_new][1], + BOT_CLAMP_ROTATION_SPEED); + /* Remember new position. */ + ctx.pos_current = pos_new; +} + +FSM_TRANS (CLAMP_IDLE, clamp_move, CLAMP_ROUTING) +{ + clamp_route (); + return FSM_NEXT (CLAMP_IDLE, clamp_move); +} + +FSM_TRANS (CLAMP_ROUTING, clamp_elevation_rotation_success, + done, CLAMP_IDLE, + next, CLAMP_ROUTING) +{ + if (ctx.pos_current == ctx.pos_request) + { + return FSM_NEXT (CLAMP_ROUTING, clamp_elevation_rotation_success, + done); + } + else + { + clamp_route (); + return FSM_NEXT (CLAMP_ROUTING, clamp_elevation_rotation_success, + next); + } +} + diff --git a/digital/io-hub/src/robospierre/clamp.h b/digital/io-hub/src/robospierre/clamp.h new file mode 100644 index 00000000..104e3cb8 --- /dev/null +++ b/digital/io-hub/src/robospierre/clamp.h @@ -0,0 +1,57 @@ +#ifndef clamp_h +#define clamp_h +/* clamp.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. + * + * }}} */ + +enum { + /** Slot positions. */ + CLAMP_SLOT_FRONT_BOTTOM, + CLAMP_SLOT_FRONT_MIDDLE, + CLAMP_SLOT_FRONT_TOP, + CLAMP_SLOT_BACK_BOTTOM, + CLAMP_SLOT_BACK_MIDDLE, + CLAMP_SLOT_BACK_TOP, + CLAMP_SLOT_SIDE, + /** Leave the front bay, ready to enter side tunnel. */ + CLAMP_BAY_FRONT_LEAVE, + /** Leave the back bay, ready to enter side tunnel. */ + CLAMP_BAY_BACK_LEAVE, + /* Enter the side bay. Position on the side, above wheels. */ + CLAMP_BAY_SIDE_ENTER_LEAVE, +}; + +/** Is slot in front bay? */ +#define CLAMP_IS_SLOT_IN_FRONT_BAY(slot) \ + ((slot) <= CLAMP_SLOT_FRONT_TOP) + +/** Is slot in back bay? */ +#define CLAMP_IS_SLOT_IN_BACK_BAY(slot) \ + ((slot) >= CLAMP_SLOT_BACK_BOTTOM && (slot) <= CLAMP_SLOT_BACK_TOP) + +/** Move clamp to given position. */ +void +clamp_move (uint8_t pos); + +#endif /* clamp_h */ diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 980ab138..3a32a1ae 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -39,6 +39,14 @@ #include "pwm.h" #include "contact.h" +#define FSM_NAME AI +#include "fsm.h" +#ifdef HOST +# include +#endif + +#include "clamp.h" + #include "io.h" /** Our color. */ @@ -76,6 +84,27 @@ main_init (void) void main_event_to_fsm (void) { + /* If an event is handled, stop generating any other event, because a + * transition may have invalidated the current robot state. */ +#define FSM_HANDLE_E(fsm, event) \ + do { if (FSM_HANDLE (fsm, event)) return; } while (0) +#define FSM_HANDLE_VAR_E(fsm, event) \ + do { if (FSM_HANDLE_VAR (fsm, event)) return; } while (0) +#define FSM_HANDLE_TIMEOUT_E(fsm) \ + do { if (FSM_HANDLE_TIMEOUT (fsm)) return; } while (0) + /* Update FSM timeouts. */ + //FSM_HANDLE_TIMEOUT_E (AI); + /* Motor status. */ + asserv_status_e mimot_motor0_status, mimot_motor1_status; + mimot_motor0_status = mimot_motor0_cmd_status (); + mimot_motor1_status = mimot_motor1_cmd_status (); + if (mimot_motor0_status == success + && mimot_motor1_status == success) + FSM_HANDLE_E (AI, clamp_elevation_rotation_success); + else if (mimot_motor0_status == failure) + FSM_HANDLE_E (AI, clamp_elevation_failure); + else if (mimot_motor1_status == failure) + FSM_HANDLE_E (AI, clamp_rotation_failure); } /** Main (and infinite) loop. */ @@ -150,6 +179,11 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) v8_to_v16 (args[3], args[4]), v8_to_v16 (args[5], args[6])); break; + case c ('c', 1): + /* Move clamp. + * - 1b: position. */ + clamp_move (args[0]); + break; /* Stats commands. * - b: interval between stats. */ case c ('A', 1): @@ -173,6 +207,17 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) int main (int argc, char **argv) { +#ifdef HOST + /* Produce AVR's FSM headers. */ + int i; + if (argc > 1) + for (i = 1; i < argc; i++) + if (strcmp (argv[i], "--gen") == 0) + { + FSM_GENERATE (AVR, 0); + return 0; + } +#endif avr_init (argc, argv); main_init (); main_loop (); -- cgit v1.2.3