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(-) 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