diff options
Diffstat (limited to 'source/ct-Bot/motor.c')
| -rw-r--r-- | source/ct-Bot/motor.c | 268 |
1 files changed, 268 insertions, 0 deletions
diff --git a/source/ct-Bot/motor.c b/source/ct-Bot/motor.c new file mode 100644 index 0000000..ce62d97 --- /dev/null +++ b/source/ct-Bot/motor.c @@ -0,0 +1,268 @@ +/* + * c't-Sim - Robotersimulator fuer den 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 motor.c + * @brief High-Level-Routinen fuer die Motorsteuerung des c't-Bot + * @author Benjamin Benz (bbe@heise.de) + * @date 15.01.05 +*/ + +#include <stdlib.h> +#include "global.h" +#include "ct-Bot.h" +#include "motor.h" +#include "bot-local.h" +#include "motor-low.h" +#include "timer.h" +#include "sensor.h" +#include "display.h" + +int16 speed_l=0; /*!< Geschwindigkeit linker Motor */ +int16 speed_r=0; /*!< Geschwindigkeit rechter Motor */ + +direction_t direction; /*!< Drehrichtung der Motoren */ + +#define PWMMAX 255 /*!< Maximaler PWM-Wert */ +#define PWMMIN 0 /*!< Minimaler PWM-Wert */ + +#ifdef SPEED_CONTROL_AVAILABLE + int8 encoderTargetRateL=0; + int8 encoderTargetRateR=0; + + /*! + * @brief Drehzahlregelung fuer die Motoren des c't-Bots + * @author Benjamin Benz (bbe@heise.de + * @date 01.05.06 + * Getrennte Drehzahlregelung fuer linken und rechten Motor sorgt fuer konstante Drehzahl und somit annaehernd + * fuer Geradeauslauf + * Feintuning von Kp, Ki, Kd verbessert die Genauigkeit und Schnelligkeit der Regelung + * Querkopplung der Motoren verbessert Gleichlauf, beispielsweise x*(sensEncL - sensEncR) + * in jeden Regler einbauen + */ + void speed_control(void){ + int8 Kp=0; + int8 Ki=0; + + int16 StellwertL=motor_left; /*!< Stellwert links*/ + int16 StellwertR=motor_right; /*!< Stellwert rechts*/ + + static int16 lastEncoderL=0; /*!< vorhergehender Wert von sensEncL */ + static int8 lastErrL=0; /*!< letzter Drehzahlfehler links */ + static int8 last2ErrL=0; /*!< vorletzter Drehzahlfehler links */ + + static int16 lastEncoderR=0; /*!< vorhergehender Wert von sensEncR */ + static int8 lastErrR=0; /*!< letzter Drehzahlfehler rechts */ + static int8 last2ErrR=0; /*!< vorletzter Drehzahlfehler rechts */ + + int16 err=0; // aktuelle Abweichung vom Soll-Wert + int16 encoderRate=0; // IST-Wert [Encoder-ticks/Aufruf] + + + // Wir arbeiten mit verschiedenen PID-Parametern fuer verschiedene Geschwindigkeitsabschnitte + if (encoderTargetRateL <= PID_LOW_RATE){ + Kp=PID_LOW_Kp; + Ki=PID_LOW_Ki; + } else { + if (encoderTargetRateL >= PID_HIGH_RATE) { + Kp=PID_HIGH_Kp; + Ki=PID_HIGH_Ki; + } else { + Kp=(PID_HIGH_Kp+PID_LOW_Kp)/2; + Ki=(PID_HIGH_Ki+PID_LOW_Ki)/2; + } + } + + + + //Regler links + if (encoderTargetRateL == 0){ + StellwertL=0; + err=0; lastErrL = 0; + } else { + encoderRate = sensEncL-lastEncoderL; // aktuelle Ist-Wert berechnen [Encoder-ticks/aufruf] + lastEncoderL = sensEncL; // Anzahl der Encoderpulse merken fuer naechsten Aufruf merken + err = encoderTargetRateL - encoderRate; // Regelabweichung links + + // Stellwert Berechnen + StellwertL += (Kp * (err - lastErrL)) / 10; // P-Beitrag + StellwertL += (Ki * (err + lastErrL)/2 ) /10; // I-Beitrag + // StellwertL += Kd * (errL - 2 * lastErrL + last2ErrL); // D-Beitrag + + //berechneten Stellwert auf zulaessige Groesse begrenzen + if (StellwertL > PWMMAX) StellwertL = PWMMAX; + if (StellwertL < -PWMMAX) StellwertL = -PWMMAX; + + #ifdef DISPLAY_REGELUNG_AVAILABLE + if (display_screen==DISPLAY_REGELUNG_AVAILABLE){ + display_cursor(1,1); + display_printf("%03d/%03d ",encoderRate,encoderTargetRateL); + display_cursor(2,1); + display_printf("e =%03d ",err); + display_cursor(3,1); + display_printf("L =%04d ", StellwertL); + } + #endif + } + + last2ErrL = lastErrL; // alten N-2 Fehler merken + lastErrL = err; // alten N-1 Fehler merken + + + //Regler rechts + if (encoderTargetRateR == 0){ + StellwertR=0; + err=0; lastErrR=0; + } else { + encoderRate = sensEncR-lastEncoderR; // aktuelle Ist-Wert berechnen [Encoder-ticks/aufruf] + lastEncoderR = sensEncR; // Anzahl der Encoderpulse merken fuer naechsten Aufruf merken + err = encoderTargetRateR - encoderRate; // Regelabweichung links + + // Stellwert Berechnen + StellwertR += (Kp * (err - lastErrR))/10; // P-Beitrag + StellwertR += (Ki * (err + lastErrR)/2)/10; // I-Beitrag + // StellwertR += Kd * (err - 2 * lastErrR + last2ErrR); // D-Beitrag + + //berechneten Stellwert auf zulaessige Groesse begrenzen + if (StellwertR > PWMMAX) StellwertR = PWMMAX; + if (StellwertR < -PWMMAX) StellwertR = -PWMMAX; + + #ifdef DISPLAY_REGELUNG_AVAILABLE + if (display_screen==DISPLAY_REGELUNG_AVAILABLE){ + display_cursor(1,10); + display_printf("%03d/%03d ",encoderRate,encoderTargetRateR); + display_cursor(2,10); + display_printf("e =%03d ",err); + display_cursor(3,10); + display_printf("R =%04d ", StellwertR); + } + #endif + } + + last2ErrR = lastErrR; // alten N-2 Fehler merken + lastErrR = err; // alten N-1 Fehler merken + + #ifdef DISPLAY_REGELUNG_AVAILABLE + if (display_screen==DISPLAY_REGELUNG_AVAILABLE){ + display_cursor(4,1); + display_printf("Kp=%03d Ki=%03d", Kp, Ki); + } + #endif + + // Und nun den Wert setzen + bot_motor(StellwertL,StellwertR); + } +#endif + +/*! + * Direkter Zugriff auf den Motor + * @param left Geschwindigkeit fuer den linken Motor + * @param right Geschwindigkeit fuer den linken Motor + * Geschwindigkeit liegt zwischen -255 und +255. + * 0 bedeutet Stillstand, 255 volle Kraft voraus, -255 volle Kraft zurueck. + * Sinnvoll ist die Verwendung der Konstanten: BOT_SPEED_XXX, + * also z.B. motor_set(BOT_SPEED_LOW,-BOT_SPEED_LOW); + * fuer eine langsame Drehung +*/ +void motor_set(int16 left, int16 right){ + #ifdef SPEED_CONTROL_AVAILABLE + static int16 old_mot_ticks=0; + #endif + + if (left == BOT_SPEED_IGNORE) + left=BOT_SPEED_STOP; + + if (right == BOT_SPEED_IGNORE) + right=BOT_SPEED_STOP; + + + // Haben wir ueberhaupt etwas zu tun? + if ((speed_l == left) && (speed_r == right)){ + // Hier sitzt die eigentliche Regelung + #ifdef SPEED_CONTROL_AVAILABLE + register uint16 ticks = TIMER_GET_TICKCOUNT_16; + if (ticks-old_mot_ticks > MS_TO_TICKS((uint16)SPEED_CONTROL_INTERVAL)) { + speed_control(); + old_mot_ticks = TIMER_GET_TICKCOUNT_16; + } + #endif + return; // Keine Aenderung? Dann zuerueck + } + + if (abs(left) > BOT_SPEED_MAX) // Nicht schneller fahren als moeglich + speed_l = BOT_SPEED_MAX; + else if (left == 0) // Stop wird nicht veraendert + speed_l = BOT_SPEED_STOP; + else if (abs(left) < BOT_SPEED_SLOW) // Nicht langsamer als die + speed_l = BOT_SPEED_SLOW; // Motoren koennen + else // Sonst den Wunsch uebernehmen + speed_l = abs(left); + + if (abs(right) > BOT_SPEED_MAX)// Nicht schneller fahren als moeglich + speed_r = BOT_SPEED_MAX; + else if (abs(right) == 0) // Stop wird nicht veraendert + speed_r = BOT_SPEED_STOP; + else if (abs(right) < BOT_SPEED_SLOW) // Nicht langsamer als die + speed_r = BOT_SPEED_SLOW; // Motoren koennen + else // Sonst den Wunsch uebernehmen + speed_r = abs(right); + + if (left < 0 ) + speed_l=-speed_l; + + if (right < 0 ) + speed_r=-speed_r; + + #ifdef SPEED_CONTROL_AVAILABLE + // TODO Hier koennten wir die Motorkennlinie heranziehen um gute Einstiegswerte fuer die Regelung zu haben + // Evtl. sogar eine im EEPROM kalibrierbare Tabelle?? + encoderTargetRateL = left / SPEED_TO_ENCODER_RATE; // Geschwindigkeit [mm/s] umrechnen in [EncoderTicks/Aufruf] + encoderTargetRateR = right / SPEED_TO_ENCODER_RATE; // Geschwindigkeit [mm/s] umrechnen in [EncoderTicks/Aufruf] + #endif + + // Zuerst einmal eine lineare Kennlinie annehmen + bot_motor(speed_l/2,speed_r/2); + +} + +/*! + * Stellt die Servos + * Sinnvolle Werte liegen zwischen 8 und 16 + * @param servo Nummer des Servos + * @param servo Zielwert + */ +void servo_set(uint8 servo, uint8 pos){ + if ((servo== SERVO1) && (pos != SERVO_OFF)) { + if (pos< DOOR_CLOSE) + pos=DOOR_CLOSE; + if (pos> DOOR_OPEN) + pos=DOOR_OPEN; + } + servo_low(servo,pos); +} + +/*! + * Initialisiere den Motorkrams + */ +void motor_init(void){ + speed_l=0; + speed_r=0; + motor_low_init(); +} + + |