From 5c8d716b96d32609d18af5219c541201209e586d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 29 May 2011 19:19:28 +0200 Subject: digital/io-hub: handle element detection on table --- digital/io-hub/src/robospierre/pawn_sensor.h | 33 ++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) create mode 100644 digital/io-hub/src/robospierre/pawn_sensor.h (limited to 'digital/io-hub/src/robospierre/pawn_sensor.h') diff --git a/digital/io-hub/src/robospierre/pawn_sensor.h b/digital/io-hub/src/robospierre/pawn_sensor.h new file mode 100644 index 00000000..14ce2d62 --- /dev/null +++ b/digital/io-hub/src/robospierre/pawn_sensor.h @@ -0,0 +1,33 @@ +#ifndef pawn_sensor_h +#define pawn_sensor_h +/* pawn_sensor.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. + * + * }}} */ + +/** Update sensor state and return the element type if an element is ready to + * be taken. */ +uint8_t +pawn_sensor_get (uint8_t direction); + +#endif /* pawn_sensor_h */ -- cgit v1.2.3 From d5ca3805ed0eb4829b491352a31165b9fd58b15e Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 08:24:48 +0200 Subject: digital/io-hub: handle bumpers --- digital/ai/src/twi_master/mimot.c | 6 +++ digital/ai/src/twi_master/mimot.h | 4 ++ digital/io-hub/src/robospierre/main.c | 4 +- digital/io-hub/src/robospierre/pawn_sensor.c | 77 ++++++++++++++++++++++++++++ digital/io-hub/src/robospierre/pawn_sensor.h | 8 +++ digital/io-hub/src/robospierre/top.c | 28 ++++++++-- digital/mimot/src/dirty/simu.host.c | 2 +- 7 files changed, 123 insertions(+), 6 deletions(-) (limited to 'digital/io-hub/src/robospierre/pawn_sensor.h') diff --git a/digital/ai/src/twi_master/mimot.c b/digital/ai/src/twi_master/mimot.c index 6159e6c9..8df84538 100644 --- a/digital/ai/src/twi_master/mimot.c +++ b/digital/ai/src/twi_master/mimot.c @@ -97,6 +97,12 @@ mimot_motor1_cmd_status (void) return none; } +uint8_t +mimot_get_input (void) +{ + return mimot_status.input_port; +} + uint16_t mimot_get_motor0_position (void) { diff --git a/digital/ai/src/twi_master/mimot.h b/digital/ai/src/twi_master/mimot.h index c59c1608..c9de7090 100644 --- a/digital/ai/src/twi_master/mimot.h +++ b/digital/ai/src/twi_master/mimot.h @@ -56,6 +56,10 @@ mimot_motor0_cmd_status (void); asserv_status_e mimot_motor1_cmd_status (void); +/** Return input port state. */ +uint8_t +mimot_get_input (void); + /** Get motor0 position in steps. */ uint16_t mimot_get_motor0_position (void); diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 1e12c427..9ae644e6 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -41,6 +41,7 @@ #include "pwm.h" #include "contact.h" #include "codebar.h" +#include "pawn_sensor.h" #include "radar.h" #define FSM_NAME AI @@ -207,6 +208,7 @@ main_loop (void) /* Update IO modules. */ pwm_update (); contact_update (); + pawn_sensor_update (); if (usdist_update ()) { position_t robot_pos; @@ -234,7 +236,7 @@ main_loop (void) } if (main_stats_contact_ && !--main_stats_contact_cpt_) { - proto_send1d ('P', contact_all ()); + proto_send1d ('P', contact_all () | (uint32_t) mimot_get_input () << 24); main_stats_contact_cpt_ = main_stats_contact_; } if (main_stats_codebar_ && !--main_stats_codebar_cpt_) diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c index ccb5eb5b..5ca18759 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.c +++ b/digital/io-hub/src/robospierre/pawn_sensor.c @@ -23,6 +23,7 @@ * * }}} */ #include "common.h" +#include "defs.h" #include "pawn_sensor.h" #include "asserv.h" @@ -33,6 +34,12 @@ #include "bot.h" #include "playground.h" #include "codebar.h" +#include "mimot.h" +#include "main.h" + +#define FSM_NAME AI +#include "fsm.h" +#include "fsm_queue.h" #include "modules/utils/utils.h" #include "modules/math/geometry/distance.h" @@ -49,8 +56,18 @@ struct pawn_sensor_t vect_t active_position; }; +/** Pawn sensor general context. */ +struct pawn_sensor_general_t +{ + /** Last bumped element position. */ + vect_t last_bumped; + /** Bumper triggered, wait until the next one. */ + uint16_t bump_wait; +}; + /** Global contexts. */ struct pawn_sensor_t pawn_sensor_front, pawn_sensor_back; +struct pawn_sensor_general_t pawn_sensor_global; static uint8_t pawn_sensor_get_type (uint8_t direction) @@ -132,3 +149,63 @@ pawn_sensor_get (uint8_t direction) return 0; } +void +pawn_sensor_bumper (uint8_t bumped, uint16_t dx, uint16_t dy) +{ + uint8_t i; + if (bumped) + { + /* Compute pawn position. */ + position_t robot_pos; + asserv_get_position (&robot_pos); + vect_t bumped_pawn; + bumped_pawn.x = dx; + bumped_pawn.y = dy; + vect_rotate_uf016 (&bumped_pawn, robot_pos.a); + vect_translate (&bumped_pawn, &robot_pos.v); + /* Do not count if out of the table. */ + if (bumped_pawn.x < 400 + BOT_ELEMENT_RADIUS + && bumped_pawn.x >= PG_WIDTH - 400 - BOT_ELEMENT_RADIUS + && bumped_pawn.y < BOT_ELEMENT_RADIUS + && bumped_pawn.y >= PG_WIDTH - BOT_ELEMENT_RADIUS) + return; + /* Do not count the opponent as a pawn. */ + for (i = 0; i < main_obstacles_nb; i++) + { + uint16_t dist = distance_point_point (&bumped_pawn, + &main_obstacles_pos[i]); + if (dist < 300 + BOT_ELEMENT_RADIUS) + return; + } + /* OK, take it. */ + pawn_sensor_global.last_bumped = bumped_pawn; + fsm_queue_post_event (FSM_EVENT (AI, top_bumper)); + pawn_sensor_global.bump_wait = 3 * 250; + } +} + +void +pawn_sensor_update (void) +{ +#define BUMPER_FRONT_LEFT _BV (6) +#define BUMPER_FRONT_RIGHT _BV (7) +#define BUMPER_BACK_RIGHT _BV (5) +#define BUMPER_BACK_LEFT _BV (4) + if (pawn_sensor_global.bump_wait) + pawn_sensor_global.bump_wait--; + else + { + uint8_t bumpers = mimot_get_input (); + pawn_sensor_bumper (!(bumpers & BUMPER_FRONT_LEFT), 120, 265); + pawn_sensor_bumper (!(bumpers & BUMPER_FRONT_RIGHT), 120, -265); + pawn_sensor_bumper (!(bumpers & BUMPER_BACK_RIGHT), -120, -265); + pawn_sensor_bumper (!(bumpers & BUMPER_BACK_LEFT), -120, 265); + } +} + +vect_t +pawn_sensor_get_last_bumped (void) +{ + return pawn_sensor_global.last_bumped; +} + diff --git a/digital/io-hub/src/robospierre/pawn_sensor.h b/digital/io-hub/src/robospierre/pawn_sensor.h index 14ce2d62..04ea1a2f 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.h +++ b/digital/io-hub/src/robospierre/pawn_sensor.h @@ -30,4 +30,12 @@ uint8_t pawn_sensor_get (uint8_t direction); +/** Update pawn sensors. */ +void +pawn_sensor_update (void); + +/** Return last bumped pawn. */ +vect_t +pawn_sensor_get_last_bumped (void); + #endif /* pawn_sensor_h */ diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 9cdefc3e..52bb05ae 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -33,6 +33,7 @@ #include "logistic.h" #include "move.h" #include "chrono.h" +#include "pawn_sensor.h" /* * Here is the top FSM. This FSM is suppose to give life to the robot with an @@ -70,6 +71,10 @@ FSM_STATES ( /* Dropping, clearing so that doors can be closed. */ TOP_DROP_CLEARING) +FSM_EVENTS ( + /* Bumpers have seen something. */ + top_bumper) + FSM_START_WITH (TOP_START) /** Top context. */ @@ -127,6 +132,15 @@ top_prepare_level (void) return 1; } +static void +top_go_this_element (vect_t pos, int16_t shorten) +{ + ctx.go_to_element_direction = logistic_global.collect_direction; + uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD + ? 0 : ASSERV_BACKWARD; + move_start_noangle (pos, backward, shorten); +} + static uint8_t top_go_element (void) { @@ -142,10 +156,7 @@ top_go_element (void) logistic_global.prepare = top_prepare_level (); } vect_t element_pos = element_get_pos (ctx.target_element_id); - ctx.go_to_element_direction = logistic_global.collect_direction; - uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD - ? 0 : ASSERV_BACKWARD; - move_start_noangle (element_pos, backward, 0); + top_go_this_element (element_pos, 0); return 1; } @@ -312,6 +323,15 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, clamp_working, TOP_WAITING_CLAMP) return FSM_NEXT (TOP_GOING_TO_ELEMENT, clamp_working); } +FSM_TRANS (TOP_GOING_TO_ELEMENT, top_bumper, TOP_GOING_TO_ELEMENT) +{ + if (!ctx.broken) + logistic_global.prepare = top_prepare_level (); + move_stop (); + top_go_this_element (pawn_sensor_get_last_bumped (), BOT_ELEMENT_RADIUS - 50); + return FSM_NEXT (TOP_GOING_TO_ELEMENT, top_bumper); +} + FSM_TRANS (TOP_WAITING_CLAMP, clamp_done, drop, TOP_GOING_TO_DROP, element, TOP_GOING_TO_ELEMENT) diff --git a/digital/mimot/src/dirty/simu.host.c b/digital/mimot/src/dirty/simu.host.c index 8afd73ca..d27ffe5f 100644 --- a/digital/mimot/src/dirty/simu.host.c +++ b/digital/mimot/src/dirty/simu.host.c @@ -156,7 +156,7 @@ simu_sensor_update_marcel (void) void simu_sensor_update_robospierre (void) { - PINC = 0; + PINC = 0xf0; if (simu_aux_model[0].th < 120.0 * 5.0 / 6.0 * simu_aux_model[0].m.i_G) PINC |= IO_BV (CONTACT_AUX0_ZERO_IO); } -- cgit v1.2.3 From 618872c7705a7f71afe77cd13b694a07660ebcb8 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 4 Jun 2011 02:48:28 +0200 Subject: digital/io-hub: conditionaly enable bumpers --- digital/io-hub/src/robospierre/pawn_sensor.c | 27 +++++++++++++++++++-------- digital/io-hub/src/robospierre/pawn_sensor.h | 4 ++++ digital/io-hub/src/robospierre/top.c | 7 +++++++ 3 files changed, 30 insertions(+), 8 deletions(-) (limited to 'digital/io-hub/src/robospierre/pawn_sensor.h') diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c index 5ca18759..cc9e6900 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.c +++ b/digital/io-hub/src/robospierre/pawn_sensor.c @@ -59,6 +59,8 @@ struct pawn_sensor_t /** Pawn sensor general context. */ struct pawn_sensor_general_t { + /** Activate bumpers. */ + uint8_t bumper_enabled; /** Last bumped element position. */ vect_t last_bumped; /** Bumper triggered, wait until the next one. */ @@ -191,18 +193,27 @@ pawn_sensor_update (void) #define BUMPER_FRONT_RIGHT _BV (7) #define BUMPER_BACK_RIGHT _BV (5) #define BUMPER_BACK_LEFT _BV (4) - if (pawn_sensor_global.bump_wait) - pawn_sensor_global.bump_wait--; - else + if (pawn_sensor_global.bumper_enabled) { - uint8_t bumpers = mimot_get_input (); - pawn_sensor_bumper (!(bumpers & BUMPER_FRONT_LEFT), 120, 265); - pawn_sensor_bumper (!(bumpers & BUMPER_FRONT_RIGHT), 120, -265); - pawn_sensor_bumper (!(bumpers & BUMPER_BACK_RIGHT), -120, -265); - pawn_sensor_bumper (!(bumpers & BUMPER_BACK_LEFT), -120, 265); + if (pawn_sensor_global.bump_wait) + pawn_sensor_global.bump_wait--; + else + { + uint8_t bumpers = mimot_get_input (); + pawn_sensor_bumper (!(bumpers & BUMPER_FRONT_LEFT), 120, 265); + pawn_sensor_bumper (!(bumpers & BUMPER_FRONT_RIGHT), 120, -265); + pawn_sensor_bumper (!(bumpers & BUMPER_BACK_RIGHT), -120, -265); + pawn_sensor_bumper (!(bumpers & BUMPER_BACK_LEFT), -120, 265); + } } } +void +pawn_sensor_bumper_enable (uint8_t enabled) +{ + pawn_sensor_global.bumper_enabled = enabled; +} + vect_t pawn_sensor_get_last_bumped (void) { diff --git a/digital/io-hub/src/robospierre/pawn_sensor.h b/digital/io-hub/src/robospierre/pawn_sensor.h index 04ea1a2f..8d6d2651 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.h +++ b/digital/io-hub/src/robospierre/pawn_sensor.h @@ -34,6 +34,10 @@ pawn_sensor_get (uint8_t direction); void pawn_sensor_update (void); +/** Enable bumpers. */ +void +pawn_sensor_bumper_enable (uint8_t enabled); + /** Return last bumped pawn. */ vect_t pawn_sensor_get_last_bumped (void); diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 7e3f4413..f4afaa17 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -153,9 +153,15 @@ top_go_element (void) if (!ctx.broken) { if (e.attr & ELEMENT_GREEN) + { logistic_global.prepare = 0; + pawn_sensor_bumper_enable (0); + } else + { logistic_global.prepare = top_prepare_level (); + pawn_sensor_bumper_enable (1); + } } vect_t element_pos = element_get_pos (ctx.target_element_id); top_go_this_element (element_pos, 0); @@ -172,6 +178,7 @@ top_go_drop (void) drop_pos.v = element_get_pos (ctx.target_element_id); if (!ctx.broken) logistic_global.prepare = top_prepare_level (); + pawn_sensor_bumper_enable (0); uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD ? 0 : ASSERV_BACKWARD; /* Go above or below the drop point. */ -- cgit v1.2.3 From 0b9ae480b928a24664bfd30edcd17d760db593d1 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 25 Jun 2011 14:43:16 +0200 Subject: digital/io-hub/src/robospierre: disable pawn sensors when tower dropped --- digital/io-hub/src/robospierre/pawn_sensor.c | 6 ++++++ digital/io-hub/src/robospierre/pawn_sensor.h | 4 ++++ digital/io-hub/src/robospierre/top.c | 2 ++ 3 files changed, 12 insertions(+) (limited to 'digital/io-hub/src/robospierre/pawn_sensor.h') diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c index cc9e6900..faeee879 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.c +++ b/digital/io-hub/src/robospierre/pawn_sensor.c @@ -214,6 +214,12 @@ pawn_sensor_bumper_enable (uint8_t enabled) pawn_sensor_global.bumper_enabled = enabled; } +void +pawn_sensor_bumper_wait (uint16_t wait) +{ + pawn_sensor_global.bump_wait = wait; +} + vect_t pawn_sensor_get_last_bumped (void) { diff --git a/digital/io-hub/src/robospierre/pawn_sensor.h b/digital/io-hub/src/robospierre/pawn_sensor.h index 8d6d2651..9c76f77a 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.h +++ b/digital/io-hub/src/robospierre/pawn_sensor.h @@ -38,6 +38,10 @@ pawn_sensor_update (void); void pawn_sensor_bumper_enable (uint8_t enabled); +/** Temporarily disable bumpers. */ +void +pawn_sensor_bumper_wait (uint16_t wait); + /** Return last bumped pawn. */ vect_t pawn_sensor_get_last_bumped (void); diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 7732fb50..3d39f60a 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -434,6 +434,7 @@ FSM_TRANS (TOP_DROP_CLEARING, robot_move_success, element, TOP_GOING_TO_ELEMENT) { clamp_drop_clear (); + pawn_sensor_bumper_wait (3 * 250); switch (top_decision ()) { default: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_success, drop); @@ -446,6 +447,7 @@ FSM_TRANS (TOP_DROP_CLEARING, robot_move_failure, element, TOP_GOING_TO_ELEMENT) { clamp_drop_clear (); + pawn_sensor_bumper_wait (3 * 250); switch (top_decision ()) { default: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_failure, drop); -- cgit v1.2.3 From a42a9f908fc0cad8760ecda5670a6b1322d56a83 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 25 Jun 2011 16:39:27 +0200 Subject: Revert "digital/io-hub/src/robospierre: disable pawn sensors when tower dropped" This reverts commit 0b9ae480b928a24664bfd30edcd17d760db593d1. --- digital/io-hub/src/robospierre/pawn_sensor.c | 6 ------ digital/io-hub/src/robospierre/pawn_sensor.h | 4 ---- digital/io-hub/src/robospierre/top.c | 2 -- 3 files changed, 12 deletions(-) (limited to 'digital/io-hub/src/robospierre/pawn_sensor.h') diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c index faeee879..cc9e6900 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.c +++ b/digital/io-hub/src/robospierre/pawn_sensor.c @@ -214,12 +214,6 @@ pawn_sensor_bumper_enable (uint8_t enabled) pawn_sensor_global.bumper_enabled = enabled; } -void -pawn_sensor_bumper_wait (uint16_t wait) -{ - pawn_sensor_global.bump_wait = wait; -} - vect_t pawn_sensor_get_last_bumped (void) { diff --git a/digital/io-hub/src/robospierre/pawn_sensor.h b/digital/io-hub/src/robospierre/pawn_sensor.h index 9c76f77a..8d6d2651 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.h +++ b/digital/io-hub/src/robospierre/pawn_sensor.h @@ -38,10 +38,6 @@ pawn_sensor_update (void); void pawn_sensor_bumper_enable (uint8_t enabled); -/** Temporarily disable bumpers. */ -void -pawn_sensor_bumper_wait (uint16_t wait); - /** Return last bumped pawn. */ vect_t pawn_sensor_get_last_bumped (void); diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 3d39f60a..7732fb50 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -434,7 +434,6 @@ FSM_TRANS (TOP_DROP_CLEARING, robot_move_success, element, TOP_GOING_TO_ELEMENT) { clamp_drop_clear (); - pawn_sensor_bumper_wait (3 * 250); switch (top_decision ()) { default: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_success, drop); @@ -447,7 +446,6 @@ FSM_TRANS (TOP_DROP_CLEARING, robot_move_failure, element, TOP_GOING_TO_ELEMENT) { clamp_drop_clear (); - pawn_sensor_bumper_wait (3 * 250); switch (top_decision ()) { default: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_failure, drop); -- cgit v1.2.3