141 lines
3.8 KiB
C
141 lines
3.8 KiB
C
/*
|
|
* 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_gotoxy.c
|
|
* @brief Bot faehrt eine Position an
|
|
*
|
|
* @author Benjamin Benz (bbe@heise.de)
|
|
* @date 03.11.06
|
|
*/
|
|
|
|
#include "bot-logic/bot-logik.h"
|
|
|
|
#ifdef BEHAVIOUR_GOTOXY_AVAILABLE
|
|
#include <math.h>
|
|
|
|
|
|
|
|
|
|
/* Parameter fuer bot_gotoxy_behaviour-Verhalten */
|
|
float target_x; /*!< Zielkoordinate X */
|
|
float target_y; /*!< Zielkoordinate Y */
|
|
float initialDiffX; /*!< Anfangsdifferenz in X-Richtung */
|
|
float initialDiffY; /*!< Anfangsdifferenz in Y-Richtung */
|
|
|
|
|
|
/*!
|
|
* Auslagerung der Berechnung der benoetigten Drehung aus dem gotoxy_behaviour
|
|
* @param xDiff
|
|
* @param yDiff
|
|
*/
|
|
int16 bot_gotoxy_calc_turn(float xDiff, float yDiff){
|
|
float newHeading;
|
|
if(fabs(yDiff)>0.001 && fabs(xDiff)>0.001){
|
|
newHeading=atan(yDiff/xDiff)*180/(M_PI);
|
|
if (xDiff<0)
|
|
newHeading+=180;
|
|
} else{
|
|
if(fabs(xDiff)<=0.001)
|
|
newHeading=(yDiff>0)?90:(-90);
|
|
else
|
|
newHeading=(xDiff>0)?0:(180);
|
|
}
|
|
|
|
int16 toTurn=(int16)newHeading-heading;
|
|
if (toTurn > 180) toTurn-=360;
|
|
if (toTurn < -180) toTurn+=360;
|
|
|
|
return toTurn;
|
|
}
|
|
|
|
/*!
|
|
* Das Verhalten faehrt von der aktuellen Position zur angegebenen Position (x/y)
|
|
* @param *data der Verhaltensdatensatz
|
|
* Verbessert von Thomas Noll, Jens Schoemann, Ben Horst (Philipps-Universitaet Marburg)
|
|
*/
|
|
void bot_gotoxy_behaviour(Behaviour_t *data){
|
|
#define INITIAL_TURN 0
|
|
#define GOTO_LOOP 1
|
|
#define CORRECT_HEAD 2
|
|
#define REACHED_POS 3
|
|
static int16 speedLeft=BOT_SPEED_FOLLOW;
|
|
static int16 speedRight=BOT_SPEED_FOLLOW;
|
|
static int8 gotoState=INITIAL_TURN;
|
|
/* aus aktueller Position und Ziel neuen Zielwinkel berechnen */
|
|
float xDiff=target_x-x_pos;
|
|
float yDiff=target_y-y_pos;
|
|
|
|
if(xDiff*initialDiffX <0 || yDiff*initialDiffY <0){ // Hier kann noch verbessert werden
|
|
gotoState=REACHED_POS; // z.B. Abfragen statt *-Operation
|
|
speedWishLeft=BOT_SPEED_STOP; // bzw. neue Drehung statt Stehenbleiben
|
|
speedWishRight=BOT_SPEED_STOP;
|
|
}
|
|
|
|
|
|
switch(gotoState) {
|
|
case INITIAL_TURN:
|
|
gotoState=GOTO_LOOP;
|
|
bot_turn(data,bot_gotoxy_calc_turn(xDiff,yDiff));
|
|
break;
|
|
|
|
case GOTO_LOOP:
|
|
/* Position erreicht? */
|
|
if ((xDiff)<10 || (yDiff)<10) {
|
|
gotoState=CORRECT_HEAD;
|
|
bot_turn(data,bot_gotoxy_calc_turn(xDiff,yDiff));
|
|
break;
|
|
}
|
|
speedWishLeft=speedLeft;
|
|
speedWishRight=speedRight;
|
|
break;
|
|
|
|
case CORRECT_HEAD:
|
|
/* Position erreicht? */
|
|
if ((xDiff)<3 && (yDiff)<3) {
|
|
gotoState=REACHED_POS;
|
|
speedWishLeft=BOT_SPEED_STOP;
|
|
speedWishRight=BOT_SPEED_STOP;
|
|
break;
|
|
}
|
|
speedWishLeft=BOT_SPEED_SLOW;
|
|
speedWishRight=BOT_SPEED_SLOW;
|
|
break;
|
|
|
|
case REACHED_POS:
|
|
gotoState=INITIAL_TURN;
|
|
return_from_behaviour(data);
|
|
break;
|
|
}
|
|
|
|
}
|
|
|
|
/*!
|
|
* Botenfunktion: Das Verhalten faehrt von der aktuellen Position zur angegebenen Position (x/y)
|
|
* @param caller Aufrufendes Verhalten
|
|
* @param x X-Ordinate an die der Bot fahren soll
|
|
* @param y Y-Ordinate an die der Bot fahren soll
|
|
*/
|
|
void bot_gotoxy(Behaviour_t *caller, float x, float y){
|
|
target_x=x;
|
|
target_y=y;
|
|
initialDiffX=x-x_pos;
|
|
initialDiffY=y-y_pos;
|
|
switch_to_behaviour(caller, bot_gotoxy_behaviour, NOOVERRIDE);
|
|
}
|
|
#endif
|