diff options
Diffstat (limited to 'source/ct-Bot/bot-logic/behaviour_scan.c')
| -rw-r--r-- | source/ct-Bot/bot-logic/behaviour_scan.c | 144 |
1 files changed, 144 insertions, 0 deletions
diff --git a/source/ct-Bot/bot-logic/behaviour_scan.c b/source/ct-Bot/bot-logic/behaviour_scan.c new file mode 100644 index 0000000..6f6869c --- /dev/null +++ b/source/ct-Bot/bot-logic/behaviour_scan.c @@ -0,0 +1,144 @@ +/* + * c't-Bot + * + * 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. + * + */ + +/*! @file behaviour_scan.c + * @brief Scannt die Umgebung und traegt sie in die Karte ein + * + * @author Benjamin Benz (bbe@heise.de) + * @date 03.11.06 +*/ + +#include "bot-logic/bot-logik.h" +#include "map.h" +#include <math.h> + +#ifdef BEHAVIOUR_SCAN_AVAILABLE +/*! + * Der Roboter faehrt einen Vollkreis und scannt dabei die Umgebung + * @param *data der Verhaltensdatensatz + */ +void bot_scan_onthefly_behaviour(Behaviour_t *data){ + #define ONTHEFLY_DIST_RESOLUTION 0.02 /*!< Alle wieviel gefahrene Strecke [m] soll die Karte aktualisiert werden. Achtung er prueft x und y getrennt, daher ist die tatsaechlich zurueckgelegte Strecke im worst case sqrt(2)*ONTHEFLY_DIST_RESOLUTION */ + #define ONTHEFLY_ANGLE_RESOLUTION 10 /*!< Alle wieviel Gerad Drehung [m] soll die Karte aktualisiert werden */ + + + + #ifdef MAP_AVAILABLE + static float last_x, last_y, last_head; + + float diff_x = fabs(x_pos-last_x); + float diff_y = fabs(y_pos-last_y); + + // Wenn der Bot faehrt, aktualisieren wir alles + if ((diff_x > ONTHEFLY_DIST_RESOLUTION) ||( diff_y > ONTHEFLY_DIST_RESOLUTION)){ + update_map_location(x_pos,y_pos); + update_map(x_pos,y_pos,heading,sensDistL,sensDistR); + + last_x=x_pos; + last_y=y_pos; + last_head=heading; + return; + } + + float diff_head = fabs(last_head-heading); + // Wenn der bot nur dreht, aktualisieren wir nur die Blickstrahlen + if ( diff_head > ONTHEFLY_ANGLE_RESOLUTION ){ + update_map(x_pos,y_pos,heading,sensDistL,sensDistR); + last_head=heading; + } + /* if ((diff_x*diff_x + diff_y*diff_y > ONTHEFLY_DIST_RESOLUTION)||fabs(last_head-heading) > ONTHEFLY_ANGLE_RESOLUTION ){ + last_x=x_pos; + last_y=y_pos; + last_head=heading; + print_map(); + } + */ + #endif +} + + +#define BOT_SCAN_STATE_START 0 +uint8 bot_scan_state = BOT_SCAN_STATE_START; /*!< Zustandsvariable fuer bot_scan_behaviour */ + +/*! + * Der Roboter faehrt einen Vollkreis und scannt dabei die Umgebung + * @param *data der Verhaltensdatensatz + */ +void bot_scan_behaviour(Behaviour_t *data){ + #define BOT_SCAN_STATE_SCAN 1 + + #define ANGLE_RESOLUTION 5 /*!< Aufloesung fuer den Scan in Grad */ + +// static uint16 bot_scan_start_angle; /*!< Winkel, bei dem mit dem Scan begonnen wurde */ + static float turned; /*!< Winkel um den bereits gedreht wurde */ + + static float last_scan_angle; /*!< Winkel bei dem zuletzt gescannt wurde */ + + float diff; + + switch (bot_scan_state){ + case BOT_SCAN_STATE_START: + + turned=0; + last_scan_angle=heading-ANGLE_RESOLUTION; + bot_scan_state=BOT_SCAN_STATE_SCAN; + break; + case BOT_SCAN_STATE_SCAN: + diff = heading - last_scan_angle; + if (diff < -180) + diff+=360; + if (diff*1.15 >= ANGLE_RESOLUTION){ + turned+= diff; + last_scan_angle=heading; + + #ifdef MAP_AVAILABLE + // Eigentlicher Scan hier + update_map(x_pos,y_pos,heading,sensDistL,sensDistR); + //////////// + #endif + + } + + if (turned >= 360-ANGLE_RESOLUTION) // Ende erreicht + bot_scan_state++; + break; + default: + bot_scan_state = BOT_SCAN_STATE_START; + #ifdef MAP_AVAILABLE + print_map(); + #endif + return_from_behaviour(data); + break; + } +} + +/*! + * Der Roboter faehrt einen Vollkreis und scannt dabei die Umgebung + * @param Der aufrufer + */ +void bot_scan(Behaviour_t* caller){ + + bot_scan_state = BOT_SCAN_STATE_START; + bot_turn(caller,360); + switch_to_behaviour(0, bot_scan_behaviour,OVERRIDE); + +// update_map(x_pos,y_pos,heading,sensDistL,sensDistR); +// print_map(); +} +#endif |