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
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
|
/*
* 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_turn.c
* @brief Drehe den Bot
*
* @author Benjamin Benz (bbe@heise.de)
* @date 03.11.06
*/
#include "bot-logic/bot-logik.h"
#ifdef BEHAVIOUR_TURN_AVAILABLE
#ifdef MCU
#include <avr/eeprom.h>
#endif
#include <stdlib.h>
#include <math.h>
/* Parameter fuer das bot_turn_behaviour() */
#ifndef MEASURE_MOUSE_AVAILABLE
int16 turn_targetR; /*!< Zu drehender Winkel bzw. angepeilter Stand des Radencoders sensEncR */
int16 turn_targetL; /*!< Zu drehender Winkel bzw. angepeilter Stand des Radencoders sensEncL */
#else
int8 angle_correct=0; /*!< Drehabschnitt 0=0-15Grad, 1=16-45 Grad, 2= >45 Grad */
int16 to_turn; /*!< Wieviel Grad sind noch zu drehen? */
#ifdef MCU
uint8 __attribute__ ((section (".eeprom"))) err15=1; /*!< Fehler bei Drehungen unter 15 Grad */
uint8 __attribute__ ((section (".eeprom"))) err45=2; /*!< Fehler bei Drehungen zwischen 15 und 45 Grad */
uint8 __attribute__ ((section (".eeprom"))) err_big=4; /*!< Fehler bei groesseren Drehungen */
#else
uint8 err15=0;
uint8 err45=0;
uint8 err_big=0;
#endif
#endif
int8 turn_direction; /*!< Richtung der Drehung */
/* Hilfsfunktion zur Berechnung einer Winkeldifferenz */
inline int16 calc_turned_angle(int8 direction, int16 angle1, int16 angle2) {
int16 diff_angle=0;
if (direction>0){
// Drehung in mathematisch positivem Sinn
if (angle1>angle2) {
// Es gab einen Ueberlauf
diff_angle=360-angle1+angle2;
} else {
diff_angle=angle2-angle1;
}
} else {
// Drehung in mathematisch negativem Sinn
if (angle1<angle2) {
// Es gab einen Ueberlauf
diff_angle=angle1+360-angle2;
} else {
diff_angle=angle1-angle2;
}
}
return diff_angle;
}
#ifdef MEASURE_MOUSE_AVAILABLE
/*!
* Das Verhalten laesst den Bot eine Punktdrehung durchfuehren.
+ * Drehen findet in drei Schritten statt. Die Drehung wird dabei
+ * bei Winkeln > 15 Grad zunaechst mit hoeherer Geschwindigkeit ausgefuehrt. Bei kleineren
+ * Winkeln oder wenn nur noch 15 Grad zu drehen sind, nur noch mit geringer Geschwindigkeit
* @param *data der Verhaltensdatensatz
* @see bot_turn()
*/
void bot_turn_behaviour(Behaviour_t *data){
// Zustaende fuer das bot_turn_behaviour-Verhalten
#define NORMAL_TURN 0
#define STOP_TURN 1
#define FULL_STOP 2
static int8 turnState=NORMAL_TURN;
static int16 old_heading=-1;
static int16 head_count=0;
uint8 e15;
uint8 e45;
uint8 ebig;
// seit dem letzten mal gedrehte Grad
int16 turned=0;
// aktueller Winkel als int16
int16 akt_heading=(int16)heading_mou;
// erster Aufruf? -> alter Winkel==neuer Winkel
if (old_heading==-1) old_heading=akt_heading;
// berechnen, wieviel Grad seit dem letzten Aufruf gedreht wurden
turned=calc_turned_angle(turn_direction,old_heading,akt_heading);
if (turned > 300) turned -= 360; // hier ging etwas schief
// aktueller Winkel wird alter Winkel
old_heading=akt_heading;
// aktuelle Drehung von zu drehendem Winkel abziehen
to_turn-=turned;
switch(turnState) {
case NORMAL_TURN:
// Solange drehen, bis Drehwinkel erreicht ist
// oder gar zu weit gedreht wurde
if (to_turn<1) {
/* Nachlauf abwarten */
speedWishLeft=BOT_SPEED_STOP;
speedWishRight=BOT_SPEED_STOP;
turnState=STOP_TURN;
break;
}
speedWishLeft = (turn_direction > 0) ? -BOT_SPEED_NORMAL : BOT_SPEED_NORMAL; speedWishLeft = (turn_direction > 0) ? -BOT_SPEED_SLOW : BOT_SPEED_SLOW;
speedWishRight = (turn_direction > 0) ? BOT_SPEED_NORMAL : -BOT_SPEED_NORMAL; speedWishRight = (turn_direction > 0) ? BOT_SPEED_SLOW : -BOT_SPEED_SLOW;
break;
case STOP_TURN:
// Abwarten, bis Nachlauf beendet
if (akt_heading!=old_heading){
head_count=0;
speedWishLeft=BOT_SPEED_STOP;
speedWishRight=BOT_SPEED_STOP;
break;
}
if (head_count<10) {
head_count++;
speedWishLeft=BOT_SPEED_STOP;
speedWishRight=BOT_SPEED_STOP;
break;
}
#ifdef MCU
e15=eeprom_read_byte(&err15);
e45=eeprom_read_byte(&err45);
ebig=eeprom_read_byte(&err_big);
#else
e15=err15;
e45=err45;
ebig=err_big;
#endif
// Neue Abweichung mit alter vergleichen und ggfs neu bestimmen
switch(angle_correct) {
case 0:
if (abs(to_turn)-e15>1) {
e15=(int8)(abs(to_turn)+e15)/2;
#ifdef MCU
eeprom_write_byte(&err15,e15);
#else
err15=e15;
#endif
}
break;
case 1:
if (abs(to_turn)-e45>1) {
e45=(int8)(abs(to_turn)+e45)/2;
#ifdef MCU
eeprom_write_byte(&err45,e45);
#else
err45=e45;
#endif
}
break;
case 2:
if (abs(to_turn)-ebig>1) {
ebig=(int8)(abs(to_turn)+ebig)/2;
#ifdef MCU
eeprom_write_byte(&err_big,ebig);
#else
err_big=ebig;
#endif
}
break;
}
// ok, verhalten beenden
speedWishLeft=BOT_SPEED_STOP;
speedWishRight=BOT_SPEED_STOP;
turnState=NORMAL_TURN;
old_heading=-1;
return_from_behaviour(data);
break;
}
}
void bot_turn(Behaviour_t *caller, int16 degrees)
{
// Richtungsgerechte Umrechnung in den Zielwinkel
if(degrees < 0) turn_direction = -1;
else turn_direction = 1;
to_turn=abs(degrees);
#ifdef MCU
if (eeprom_read_byte(&err15)==255 && eeprom_read_byte(&err45)==255 && eeprom_read_byte(&err_big)==255) {
eeprom_write_byte(&err15,1);
eeprom_write_byte(&err45,2);
eeprom_write_byte(&err_big,4);
}
if (to_turn>45) {
to_turn-=eeprom_read_byte(&err_big);
angle_correct=2;
} else if (to_turn<=45 && to_turn>15) {
to_turn-=eeprom_read_byte(&err45);
angle_correct=1;
} else {
to_turn-=eeprom_read_byte(&err15);
angle_correct=0;
}
#else
if (to_turn>45) {
to_turn-=err_big;
angle_correct=2;
} else if (to_turn<=45 && to_turn>15) {
to_turn-=err45;
angle_correct=1;
} else {
to_turn-=err15;
angle_correct=0;
}
#endif
switch_to_behaviour(caller, bot_turn_behaviour,NOOVERRIDE);
}
#else
/*!
* Das Verhalten laesst den Bot eine Punktdrehung durchfuehren.
* @see bot_turn()
* */
void bot_turn_behaviour(Behaviour_t* data){
/* Drehen findet in vier Schritten statt. Die Drehung wird dabei
* bei Winkeln > 90 Grad zunaechst mit maximaler Geschwindigkeit ausgefuehrt. Bei kleineren
* Winkeln oder wenn nur noch 90 Grad zu drehen sind, nur noch mit normaler Geschwindigkeit
*/
/* Zustaende fuer das bot_turn_behaviour-Verhalten */
#define NORMAL_TURN 0
#define SHORT_REVERSE 1
#define CORRECT_POSITION 2
#define FULL_STOP 3
static int8 turnState=NORMAL_TURN;
/* zu drehende Schritte in die korrekte Drehrichtung korrigieren */
int16 to_turnR = turn_direction*(turn_targetR - sensEncR);
int16 to_turnL = -turn_direction*(turn_targetL - sensEncL);
switch(turnState) {
case NORMAL_TURN:
/* Solange drehen, bis beide Encoder nur noch zwei oder weniger Schritte zu fahren haben */
if (to_turnL <= 2 && to_turnR<=2){
/* nur noch 2 Schritte oder weniger, abbremsen einleiten */
turnState=SHORT_REVERSE;
break;
}
/* Bis 90 Grad kann mit maximaler Geschwindigkeit gefahren werden, danach auf Normal reduzieren */
/* Geschwindigkeit fuer beide Raeder getrennt ermitteln */
if(abs(to_turnL) < ANGLE_CONSTANT*0.25) {
speedWishLeft = (turn_direction > 0) ? -BOT_SPEED_MEDIUM : BOT_SPEED_MEDIUM;
} else {
speedWishLeft = (turn_direction > 0) ? -BOT_SPEED_NORMAL : BOT_SPEED_NORMAL;
}
if(abs(to_turnR) < ANGLE_CONSTANT*0.25) {
speedWishRight = (turn_direction > 0) ? BOT_SPEED_MEDIUM : -BOT_SPEED_MEDIUM;
} else {
speedWishRight = (turn_direction > 0) ? BOT_SPEED_NORMAL : -BOT_SPEED_NORMAL;
}
break;
case SHORT_REVERSE:
/* Ganz kurz durch umpolen anbremsen */
speedWishLeft = (turn_direction > 0) ? BOT_SPEED_SLOW : -BOT_SPEED_SLOW;
speedWishRight = (turn_direction > 0) ? -BOT_SPEED_SLOW : BOT_SPEED_SLOW;
turnState=CORRECT_POSITION;
break;
case CORRECT_POSITION:
/* Evtl. etwas zuruecksetzen, falls wir zu weit gefahren sind */
if (to_turnR<0) {
/* rechts zu weit gefahren..langsam zurueck */
speedWishRight = (turn_direction > 0) ? -BOT_SPEED_SLOW : BOT_SPEED_SLOW;
} else if (to_turnR>0) {
/* rechts noch nicht weit genug...langsam vor */
speedWishRight = (turn_direction > 0) ? BOT_SPEED_SLOW : -BOT_SPEED_SLOW;
} else {
/* Endposition erreicht, rechtes Rad anhalten */
speedWishRight = BOT_SPEED_STOP;
}
if (to_turnL<0) {
/* links zu weit gefahren..langsam zurueck */
speedWishLeft = (turn_direction > 0) ? BOT_SPEED_SLOW : -BOT_SPEED_SLOW;
} else if (to_turnL>0) {
/* links noch nicht weit genug...langsam vor */
speedWishLeft = (turn_direction > 0) ? -BOT_SPEED_SLOW : BOT_SPEED_SLOW;
} else {
/* Endposition erreicht, linkes Rad anhalten */
speedWishLeft = BOT_SPEED_STOP;
}
if (speedWishLeft == BOT_SPEED_STOP && speedWishRight == BOT_SPEED_STOP) {
/* beide Raeder haben nun wirklich die Endposition erreicht, daher anhalten */
turnState=FULL_STOP;
}
break;
default:
/* ist gleichzeitig FULL_STOP, da gleiche Aktion
* Stoppen, State zuruecksetzen und Verhalten beenden */
speedWishLeft = BOT_SPEED_STOP;
speedWishRight = BOT_SPEED_STOP;
turnState=NORMAL_TURN;
return_from_behaviour(data);
break;
}
}
/*!
* Dreht den Bot im mathematisch positiven Sinn.
* @param degrees Grad, um die der Bot gedreht wird. Negative Zahlen drehen im (mathematisch negativen) Uhrzeigersinn.
* Die Aufloesung betraegt rund 3 Grad
*/
void bot_turn(Behaviour_t* caller,int16 degrees){
/* Umrechnung von Grad in Encoder-Markierungen.
* Hinweis: Eigentlich muessten der Umfang von Bot und Rad verwendet werden. Die Rechnung wird
* allerdings viel einfacher, wenn man Pi auskuerzt.
* Ist degrees negativ, ist die Drehung negativ und der rechte Encoder muss kleiner werden.
*/
if(degrees < 0) turn_direction = -1;
else turn_direction = 1;
/* Anzahl zu fahrender Encoderschritte berechnen */
turn_targetR=(degrees*ANGLE_CONSTANT)/360;
/* linkes Rad dreht entgegengesetzt, daher negativer Wert */
turn_targetL=-turn_targetR;
/* aktuellen Sensorwert zu zu drehenden Encoderschritten addieren */
turn_targetR+=sensEncR;
turn_targetL+=sensEncL;
switch_to_behaviour(caller, bot_turn_behaviour,OVERRIDE);
}
#endif
#endif
|
