361 lines
11 KiB
C
361 lines
11 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_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
|
|
|