1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
|
/*
* 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
|
