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
|
/*
* 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_drive_distance.c
* @brief Bot faehrt ein Stueck
*
* @author Benjamin Benz (bbe@heise.de)
* @date 03.11.06
*/
#include "bot-logic/bot-logik.h"
#include "log.h"
#ifdef BEHAVIOUR_DRIVE_DISTANCE_AVAILABLE
/* Parameter fuer das bot_drive_distance_behaviour() */
int16 drive_distance_target; /*!< Zu fahrende Distanz bzw. angepeilter Stand der Radencoder sensEncL bzw. sensEncR */
int8 drive_distance_curve; /*!< Kruemmung der zu fahrenden Strecke. */
int16 drive_distance_speed; /*!< Angepeilte Geschwindigkeit. */
/*!
* laesst den Bot in eine Richtung fahren.
* Es handelt sich hierbei nicht im eigentlichen Sinn um ein Verhalten, sondern ist nur eine Abstraktion der Motorkontrollen.
* @param curve Gibt an, ob der Bot eine Kurve fahren soll. Werte von -127 (So scharf wie moeglich links) ueber 0 (gerade aus) bis 127 (so scharf wie moeglich rechts)
* @param speed Gibt an, wie schnell der Bot fahren soll. */
void bot_drive(int8 curve, int16 speed){
// Wenn etwas ausgewichen wurde, bricht das Verhalten hier ab, sonst wuerde es evtl. die Handlungsanweisungen von bot_avoid_harm() stoeren.
//if(bot_avoid_harm()) return;
if(curve < 0) {
speedWishLeft = speed * (1.0 + 2.0*((float)curve/127));
speedWishRight = speed;
} else if (curve > 0) {
speedWishRight = speed * (1.0 - 2.0*((float)curve/127));
speedWishLeft = speed;
} else {
speedWishLeft = speed;
speedWishRight = speed;
}
}
/*!
* Das Verhalten laesst den Bot eine vorher festgelegte Strecke fahren.
* @param *data der Verhaltensdatensatz
* @see bot_drive_distance()
*/
void bot_drive_distance_behaviour(Behaviour_t* data){
int16 *encoder;
int16 to_drive;
if (drive_distance_curve > 0){
// Es handelt sich um eine Rechtskurve, daher wird mit dem linken Encoder gerechnet
encoder = (int16*)&sensEncL;
} else {
encoder = (int16*)&sensEncR;
}
to_drive = drive_distance_target - *encoder;
if(drive_distance_speed < 0) to_drive = -to_drive;
if(to_drive <= 0){
return_from_behaviour(data);
} else {
if((drive_distance_speed > BOT_SPEED_SLOW || drive_distance_speed < -BOT_SPEED_SLOW) && to_drive < (0.1 * ENCODER_MARKS)) bot_drive(drive_distance_curve, drive_distance_speed / 2);
else bot_drive(drive_distance_curve, drive_distance_speed);
}
}
/*!
* Das Verhalten laesst den Bot eine vorher festgelegte Strecke fahren. Dabei legt die Geschwindigkeit fest, ob der Bot vorwaerts oder rueckwaerts fahren soll.
* @param curve Gibt an, ob der Bot eine Kurve fahren soll. Werte von -127 (So scharf wie moeglich links) ueber 0 (gerade aus) bis 127 (so scharf wie moeglich rechts)
* @param speed Gibt an, wie schnell der Bot fahren soll. Negative Werte lassen den Bot rueckwaerts fahren.
* @param cm Gibt an, wie weit der Bot fahren soll. In cm :-) Die Strecke muss positiv sein, die Fahrtrichtung wird ueber speed geregelt.
*/
void bot_drive_distance(Behaviour_t* caller,int8 curve, int16 speed, int16 cm){
// LOG_DEBUG(("curve= %d, speed= %d, cm =%d",curve, speed, cm));
int32 tmp = cm;
tmp*= 10 * ENCODER_MARKS;
tmp /= WHEEL_PERIMETER;
int16 marks_to_drive = (int16) tmp;
int16 *encoder;
drive_distance_curve = curve;
drive_distance_speed = speed;
if (curve > 0){
// Es handelt sich um eine Rechtskurve, daher wird mit dem linken Encoder gerechnet
encoder = (int16*)&sensEncL;
} else {
encoder = (int16*)&sensEncR;
}
if(speed < 0){
// Es soll rueckwaerts gefahren werden. Der Zielwert ist also kleiner als der aktuelle Encoder-Stand.
drive_distance_target = *encoder - marks_to_drive;
} else {
drive_distance_target = *encoder + marks_to_drive;
}
switch_to_behaviour(caller, bot_drive_distance_behaviour,NOOVERRIDE);
}
#endif
|
