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
|
/*
* 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_catch_pillar.c
* @brief sucht nach einer Dose und fängt sie ein
*
* @author Benjamin Benz (bbe@heise.de)
* @date 08.12.06
*/
#include "bot-logic/bot-logik.h"
#ifdef BEHAVIOUR_CATCH_PILLAR_AVAILABLE
#include <math.h>
#define MAX_PILLAR_DISTANCE 350
#define START 0
#define LEFT_FOUND 1
#define TURN_MIDDLE 2
#define GO 3
static uint8 pillar_state=START;
/*!
* Fange eine Dose ein
* @param *data der Verhaltensdatensatz
*/
void bot_catch_pillar_behaviour(Behaviour_t *data){
static float angle;
switch (pillar_state){
case START:
if (sensDistL < MAX_PILLAR_DISTANCE){ // sieht der linke Sensor schon was?
angle=heading;
pillar_state=LEFT_FOUND;
} else
speedWishLeft=-BOT_SPEED_SLOW;
speedWishRight=+BOT_SPEED_SLOW;
//bot_turn(data,5); // Ein Stueck drehen
break;
case LEFT_FOUND:
if (sensDistR < MAX_PILLAR_DISTANCE){ // sieht der rechte Sensor schon was?
angle= heading- angle;
if (angle < 0)
angle+=360;
angle= heading - angle/2;
pillar_state=TURN_MIDDLE;
// bot_turn(data,-angle/2);
}else
speedWishLeft=-BOT_SPEED_SLOW;
speedWishRight=+BOT_SPEED_SLOW;
// bot_turn(data,5); // Eins Stueck drehen
break;
case TURN_MIDDLE:
if (fabs(heading - angle) > 2){
speedWishLeft=+BOT_SPEED_SLOW;
speedWishRight=-BOT_SPEED_SLOW;
} else {
bot_servo(data,SERVO1,DOOR_OPEN); // Klappe auf
pillar_state=GO;
}
break;
case GO:
if (sensTrans ==0){
speedWishLeft=+BOT_SPEED_SLOW;
speedWishRight=+BOT_SPEED_SLOW;
} else {
bot_servo(data,SERVO1,DOOR_CLOSE);
pillar_state++;
}
break;
default:
pillar_state=START;
return_from_behaviour(data);
break;
}
}
/*!
* Fange ein Objekt ein
* @param caller Der obligatorische Verhaltensdatensatz des Aufrufers
*/
void bot_catch_pillar(Behaviour_t * caller){
pillar_state=0;
// Zielwerte speichern
switch_to_behaviour(caller,bot_catch_pillar_behaviour,OVERRIDE);
}
#endif
|
