From 2277c9a7aa41c347ba04fca8e202c77b43618802 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 20 May 2011 08:27:02 +0200 Subject: digital/{ai,io-hub}: add init FSM --- digital/ai/src/fsm/init.c | 188 +++++++++++++++++++++++++++++ digital/ai/tools/robospierre.py | 5 +- digital/io-hub/src/robospierre/Makefile | 2 +- digital/io-hub/src/robospierre/bot.h | 10 ++ digital/io-hub/src/robospierre/clamp.c | 6 +- digital/io-hub/src/robospierre/init_defs.h | 50 ++++++++ digital/io-hub/src/robospierre/main.c | 14 ++- 7 files changed, 265 insertions(+), 10 deletions(-) create mode 100644 digital/ai/src/fsm/init.c create mode 100644 digital/io-hub/src/robospierre/init_defs.h (limited to 'digital') diff --git a/digital/ai/src/fsm/init.c b/digital/ai/src/fsm/init.c new file mode 100644 index 00000000..1f933ce1 --- /dev/null +++ b/digital/ai/src/fsm/init.c @@ -0,0 +1,188 @@ +/* init.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 "asserv.h" +#include "contact.h" +#include "chrono.h" + +#define FSM_NAME AI +#include "fsm.h" +#include "fsm_queue.h" + +#include "bot.h" +#include "init_defs.h" + +/* + * Initialise robot position with a calibration procedure. + */ + +FSM_STATES ( + /* Initial state, waiting for jack. */ + INIT_START, + /* After first jack insertion, waiting for removal to initialise actuators. */ + INIT_WAITING_FIRST_JACK_OUT, + /* Initialising actuators, then waiting for second jack insertion. */ + INIT_INITIALISING_ACTUATORS, + /* After second jack insertion, waiting the operator remove its + * hand. */ + INIT_WAITING_HANDS_OUT, + /* Finding the first wall. */ + INIT_FINDING_FIRST_WALL, + /* Going away from the wall. */ + INIT_GOING_AWAY_FIRST_WALL, + /* Turning to face the other wall. */ + INIT_FACING_SECOND_WALL, + /* Waiting after rotation for robot to stabilize. */ + INIT_WAITING_AFTER_FACING_SECOND_WALL, + /* Finding the second wall. */ + INIT_FINDING_SECOND_WALL, + /* Going away from the wall. */ + INIT_GOING_AWAY_SECOND_WALL, +#ifdef INIT_START_POSITION_ANGLE + /* Facing the start position. */ + INIT_FACING_START_POSITION, +#endif + /* Going to start position. */ + INIT_GOING_TO_START_POSITION, + /* Waiting for the round start (waiting for the jack). */ + INIT_WAITING_SECOND_JACK_OUT, + /* Initialisation finished, nothing else to do. */ + INIT_FINISHED) + +FSM_EVENTS ( + /* XXX: temporarily here. */ + robot_move_success, + robot_move_failure, + /* Jack is inserted. */ + jack_inserted, + /* Jack is removed. */ + jack_removed, + /* Sent to initialise actuators. */ + init_actuators, + /* Sent to start round. */ + init_start_round) + +FSM_START_WITH (INIT_START) + +FSM_TRANS (INIT_START, jack_inserted, INIT_WAITING_FIRST_JACK_OUT) +{ + return FSM_NEXT (INIT_START, jack_inserted); +} + +FSM_TRANS (INIT_WAITING_FIRST_JACK_OUT, jack_removed, + INIT_INITIALISING_ACTUATORS) +{ + fsm_queue_post_event (FSM_EVENT (AI, init_actuators)); + return FSM_NEXT (INIT_WAITING_FIRST_JACK_OUT, jack_removed); +} + +FSM_TRANS (INIT_INITIALISING_ACTUATORS, jack_inserted, INIT_WAITING_HANDS_OUT) +{ + team_color = contact_get_color (); + return FSM_NEXT (INIT_INITIALISING_ACTUATORS, jack_inserted); +} + +FSM_TRANS_TIMEOUT (INIT_WAITING_HANDS_OUT, 225, INIT_FINDING_FIRST_WALL) +{ + asserv_set_speed (BOT_SPEED_INIT); + asserv_push_the_wall (INIT_FIRST_WALL_PUSH); + return FSM_NEXT_TIMEOUT (INIT_WAITING_HANDS_OUT); +} + +FSM_TRANS (INIT_FINDING_FIRST_WALL, robot_move_success, + INIT_GOING_AWAY_FIRST_WALL) +{ + asserv_move_linearly (INIT_FIRST_WALL_AWAY); + return FSM_NEXT (INIT_FINDING_FIRST_WALL, robot_move_success); +} + +FSM_TRANS (INIT_GOING_AWAY_FIRST_WALL, robot_move_success, + INIT_FACING_SECOND_WALL) +{ + asserv_goto_angle (INIT_SECOND_WALL_ANGLE); + return FSM_NEXT (INIT_GOING_AWAY_FIRST_WALL, robot_move_success); +} + +FSM_TRANS (INIT_FACING_SECOND_WALL, robot_move_success, + INIT_WAITING_AFTER_FACING_SECOND_WALL) +{ + return FSM_NEXT (INIT_FACING_SECOND_WALL, robot_move_success); +} + +FSM_TRANS_TIMEOUT (INIT_WAITING_AFTER_FACING_SECOND_WALL, 225 / 2, + INIT_FINDING_SECOND_WALL) +{ + asserv_push_the_wall (INIT_SECOND_WALL_PUSH); + return FSM_NEXT_TIMEOUT (INIT_WAITING_AFTER_FACING_SECOND_WALL); +} + +FSM_TRANS (INIT_FINDING_SECOND_WALL, robot_move_success, + INIT_GOING_AWAY_SECOND_WALL) +{ + asserv_move_linearly (INIT_SECOND_WALL_AWAY); + return FSM_NEXT (INIT_FINDING_SECOND_WALL, robot_move_success); +} + +#ifdef INIT_START_POSITION_ANGLE +FSM_TRANS (INIT_GOING_AWAY_SECOND_WALL, robot_move_success, + INIT_FACING_START_POSITION) +{ + asserv_goto_angle (INIT_START_POSITION_ANGLE); + return FSM_NEXT (INIT_GOING_AWAY_SECOND_WALL, robot_move_success); +} + +FSM_TRANS (INIT_FACING_START_POSITION, robot_move_success, + INIT_GOING_TO_START_POSITION) +{ + asserv_goto_xya (INIT_START_POSITION); + return FSM_NEXT (INIT_FACING_START_POSITION, robot_move_success); +} + +#else + +FSM_TRANS (INIT_GOING_AWAY_SECOND_WALL, robot_move_success, + INIT_GOING_TO_START_POSITION) +{ + asserv_goto_xya (INIT_START_POSITION); + return FSM_NEXT (INIT_GOING_AWAY_SECOND_WALL, robot_move_success); +} + +#endif + +FSM_TRANS (INIT_GOING_TO_START_POSITION, robot_move_success, + INIT_WAITING_SECOND_JACK_OUT) +{ + asserv_set_speed (BOT_SPEED_NORMAL); + return FSM_NEXT (INIT_GOING_TO_START_POSITION, robot_move_success); +} + +FSM_TRANS (INIT_WAITING_SECOND_JACK_OUT, jack_removed, INIT_FINISHED) +{ + chrono_init (); + fsm_queue_post_event (FSM_EVENT (AI, init_start_round)); + return FSM_NEXT (INIT_WAITING_SECOND_JACK_OUT, jack_removed); +} + diff --git a/digital/ai/tools/robospierre.py b/digital/ai/tools/robospierre.py index 8d5db276..e8384f6c 100644 --- a/digital/ai/tools/robospierre.py +++ b/digital/ai/tools/robospierre.py @@ -34,7 +34,8 @@ class Robot: self.io = io_hub.Proto (PopenIO (io_hub_cmd), proto_time, **io_hub.init.host['robospierre']) self.robot_start_pos = { - False: (700, 2100 - 250, math.radians (-270)), - True: (3000 - 700, 2100 - 250, math.radians (-270)) + # In real life, better place the robot in green zone. + False: (300, 2100 - 200, math.radians (180)), + True: (3000 - 300, 2100 - 200, math.radians (0)) } diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile index 20e9dee0..d69918f5 100644 --- a/digital/io-hub/src/robospierre/Makefile +++ b/digital/io-hub/src/robospierre/Makefile @@ -5,7 +5,7 @@ PROGS = io_hub # Sources to compile. io_hub_SOURCES = main.c \ clamp.c logistic.c \ - fsm.host.c fsm_AI_gen.avr.c fsm_queue.c \ + init.c fsm.host.c fsm_AI_gen.avr.c fsm_queue.c \ pwm.avr.c pwm.host.c \ contact.avr.c contact.host.c \ twi_master.c asserv.c mimot.c \ diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index e014321e..a986c015 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -34,6 +34,16 @@ # define BOT_SCALE 0.0415178942124 #endif +/** Distance between the front contact point and the robot center. */ +#define BOT_FRONT_CONTACT_DIST_MM 150 +/** Angle error at the front contact point. */ +#define BOT_FRONT_CONTACT_ANGLE_ERROR_DEG 0 + +/** Speed used for initialisation. */ +#define BOT_SPEED_INIT 0x10, 0x10, 0x10, 0x10 +/** Normal cruise speed. */ +#define BOT_SPEED_NORMAL 0x40, 0x40, 0x20, 0x20 + #ifdef HOST # define BOT_CLAMP_SLOT_FRONT_BOTTOM_ELEVATION_STEP 0 diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 6c063979..583cadb5 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -83,8 +83,6 @@ FSM_STATES ( CLAMP_MOVE_DST_CLAMP_OPENING) FSM_EVENTS ( - /* Here for the moment, to be moved later. */ - start, /* New element inside bottom slot. */ clamp_new_element, /* Order to drop elements. */ @@ -325,10 +323,10 @@ clamp_route (void) /* CLAMP FSM */ -FSM_TRANS (CLAMP_START, start, CLAMP_GOING_IDLE) +FSM_TRANS (CLAMP_START, init_actuators, CLAMP_GOING_IDLE) { clamp_move (CLAMP_SLOT_FRONT_MIDDLE); - return FSM_NEXT (CLAMP_START, start); + return FSM_NEXT (CLAMP_START, init_actuators); } FSM_TRANS (CLAMP_GOING_IDLE, clamp_move_success, CLAMP_IDLE) diff --git a/digital/io-hub/src/robospierre/init_defs.h b/digital/io-hub/src/robospierre/init_defs.h new file mode 100644 index 00000000..6ce6eaae --- /dev/null +++ b/digital/io-hub/src/robospierre/init_defs.h @@ -0,0 +1,50 @@ +#ifndef init_defs_h +#define init_defs_h +/* init_defs.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. + * + * }}} */ + +#include "playground.h" +#include "bot.h" + +/** Parameters to push the first wall. */ +#define INIT_FIRST_WALL_PUSH \ + 0, PG_X (BOT_FRONT_CONTACT_DIST_MM), 200, \ + PG_A_DEG (180 + BOT_FRONT_CONTACT_ANGLE_ERROR_DEG) +/** Parameters to go away from the first wall. */ +#define INIT_FIRST_WALL_AWAY -500 +/** Parameter to face the second wall. */ +#define INIT_SECOND_WALL_ANGLE PG_A_DEG (90) +/** Parameters to push the second wall. */ +#define INIT_SECOND_WALL_PUSH \ + 0, -1, PG_Y (PG_LENGTH - BOT_FRONT_CONTACT_DIST_MM), -1 +/** Parameters to go away from the second wall. */ +#define INIT_SECOND_WALL_AWAY -(200 - BOT_FRONT_CONTACT_DIST_MM) +/** Parameter to face the start position. */ +#define INIT_START_POSITION_ANGLE PG_A_DEG (0) +/** Start position. */ +#define INIT_START_POSITION \ + PG_X (200), PG_Y (PG_LENGTH - 200), PG_A_DEG (0), ASSERV_BACKWARD + +#endif /* init_defs_h */ diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 97d5bd40..f1b4d8de 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -110,9 +110,15 @@ main_event_to_fsm (void) /* Update FSM timeouts. */ FSM_HANDLE_TIMEOUT_E (AI); /* Motor status. */ - asserv_status_e mimot_motor0_status, mimot_motor1_status; + asserv_status_e robot_move_status, mimot_motor0_status, + mimot_motor1_status; + robot_move_status = asserv_move_cmd_status (); mimot_motor0_status = mimot_motor0_cmd_status (); mimot_motor1_status = mimot_motor1_cmd_status (); + if (robot_move_status == success) + FSM_HANDLE_E (AI, robot_move_success); + else if (robot_move_status == failure) + FSM_HANDLE_E (AI, robot_move_failure); if (mimot_motor0_status == success && mimot_motor1_status == success) FSM_HANDLE_E (AI, clamp_elevation_rotation_success); @@ -123,9 +129,11 @@ main_event_to_fsm (void) /* Clamp specific events. */ if (clamp_handle_event ()) return; - /* Jack, XXX to be changed! */ + /* Jack. */ if (!contact_get_jack ()) - FSM_HANDLE_E (AI, start); + FSM_HANDLE_E (AI, jack_inserted); + else + FSM_HANDLE_E (AI, jack_removed); /* Events from the event queue. */ if (fsm_queue_poll ()) { -- cgit v1.2.3