diff options
Diffstat (limited to 'source/ct-Bot/bot-logic/behaviour_drive_distance.c')
| -rw-r--r-- | source/ct-Bot/bot-logic/behaviour_drive_distance.c | 120 |
1 files changed, 120 insertions, 0 deletions
diff --git a/source/ct-Bot/bot-logic/behaviour_drive_distance.c b/source/ct-Bot/bot-logic/behaviour_drive_distance.c new file mode 100644 index 0000000..bb29e66 --- /dev/null +++ b/source/ct-Bot/bot-logic/behaviour_drive_distance.c @@ -0,0 +1,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 |