Fahrzeugsteuerung

 /* Motosteuerung eines Roboters */

int geschwindigkeit = 150;        /* Standardgewindigkeit für die Motoren  */
float faktor = 0.95;             /* Ausgleichsfaktor für die Motoren.  Muss experimentell ermittelt werden.*/
/* !!!!!!!!  Achtung   Achtung  Bei Linkskurven wird der Kurvenfaktor 2x eingesetzt
Wahrscheinlich reagieren die Motoren bzw. das Fahrgestell beim Vorwärtsgang anders als bei Rückwärtsgang :-((*/
float kurvenfaktor = 1.2;      /* Faktor für den Kurvenradius  */
const int El = 6;          /*   PWM - Ausgang zur Steuerung des Motors 1 = links  */
const int Ml = 7;          /*  Laufrichtung von Motor 1 = links  einstellen HIGH = vorwärts, LOW = rückwärts  */
const int Er = 5;          /*  Analog für Motor 2  = rechts  */
const int Mr= 4;          /*  Analog für Motor 2  = rechts  */
 
void setup()
  {
  Serial.begin(9600);
  pinMode(El, OUTPUT);
  pinMode(Ml, OUTPUT);
  pinMode(Er, OUTPUT);
  pinMode(Mr, OUTPUT);
  }

void halt()
{
    int l = 0;
    int r = 0;
    Serial.print(l);  Serial.print("   ");  Serial.print(r);  Serial.println("  halt");
    digitalWrite(Ml,HIGH);
    digitalWrite(Mr, HIGH);
    analogWrite(El, l); //PWM Speed Control
    analogWrite(Er, r); //PWM Speed Control  
}
 
void geradeausvorwaerts(int g)
{
    int l = floor(g*faktor);
    int r = g;
    Serial.print(l);  Serial.print("   ");  Serial.print(r);  Serial.println("  geradeausvorwaerts");
    digitalWrite(Ml,HIGH);
    digitalWrite(Mr, HIGH);
    analogWrite(El, l); //PWM Speed Control
    analogWrite(Er, r); //PWM Speed Control
}
 
void geradeausrueckwaerts(int g)
{
    int l = floor(g*faktor);
    int r = g;
    Serial.print(l);  Serial.print("   ");  Serial.print(r);  Serial.println("  geradeausrueckwaerts");
    digitalWrite(Ml,LOW);
    digitalWrite(Mr, LOW);
    analogWrite(El, l); //PWM Speed Control
    analogWrite(Er, r); //PWM Speed Control
}
 
void rechtsdrehen(int g)
{
    int l = floor(g*faktor);
    int r = g;
    Serial.print(l);  Serial.print("   ");  Serial.print(r);  Serial.println("  rechtsdrehen");
    digitalWrite(Ml,HIGH);
    digitalWrite(Mr, LOW);
    analogWrite(El, l); //PWM Speed Control
    analogWrite(Er, r); //PWM Speed Control
}
 
void linksdrehen(int g)
{
    int l = floor(g*faktor);
    int r = g;
    Serial.print(l);  Serial.print("   ");  Serial.print(r);  Serial.println("  linksdrehen");
    digitalWrite(Ml, LOW);
    digitalWrite(Mr, HIGH);
    analogWrite(El, l); //PWM Speed Control
    analogWrite(Er, r); //PWM Speed Control
}
 
void rechtskurvevorwaerts(int g)
{
    int l = floor(g*faktor*kurvenfaktor);
    int r = floor(g/kurvenfaktor);
    Serial.print(l);  Serial.print("   ");  Serial.print(r);  Serial.println("  rechtskurvevorwaerts");
    digitalWrite(Ml,HIGH);
    digitalWrite(Mr, HIGH);
    analogWrite(El, l); //PWM Speed Control
    analogWrite(Er, r); //PWM Speed Control
}
 
void linkskurvevorwaerts(int g)
{
    int l = floor(g*faktor/kurvenfaktor/kurvenfaktor);
    int r = floor(g*kurvenfaktor*kurvenfaktor);
    Serial.print(l);  Serial.print("   ");  Serial.print(r);  Serial.println("  linkskurvevorwaerts");
    digitalWrite(Ml,HIGH);
    digitalWrite(Mr, HIGH);
    analogWrite(El, l); //PWM Speed Control
    analogWrite(Er, r); //PWM Speed Control
}
 
void rechtskurverueckwaerts(int g)
{
    int l = floor(g*faktor*kurvenfaktor);
    int r = floor(g/kurvenfaktor);
    Serial.print(l);  Serial.print("   ");  Serial.print(r);  Serial.println("  rechtskurverueckwaerts");
    digitalWrite(Ml,LOW);
    digitalWrite(Mr, LOW);
    analogWrite(El, l); //PWM Speed Control
    analogWrite(Er, r); //PWM Speed Control
}
 
void linkskurverueckwaerts(int g)
{
    int l = floor(g*faktor/kurvenfaktor/kurvenfaktor);
    int r = floor(g*kurvenfaktor*kurvenfaktor);
    Serial.print(l);  Serial.print("   ");  Serial.print(r);  Serial.println("  linkskurverueckwaerts");
    digitalWrite(Ml,LOW);
    digitalWrite(Mr, LOW);
    analogWrite(El, l); //PWM Speed Control
    analogWrite(Er, r); //PWM Speed Control
}

void loop()
{
   geradeausvorwaerts(geschwindigkeit);
   delay(5000);
   halt();
   delay(1000);
   geradeausrueckwaerts(geschwindigkeit);
   delay(3000);
   halt();
   delay(1000);
   rechtsdrehen(geschwindigkeit);
   delay(3000);
   halt();
   delay(1000);
   linksdrehen(geschwindigkeit);
   delay(3000);
   halt();
   delay(1000);
   rechtskurvevorwaerts(geschwindigkeit);
   delay(3000);
   halt();
   delay(1000);
   linkskurvevorwaerts(geschwindigkeit);
   delay(3000);
   halt();
   delay(1000);
   rechtskurverueckwaerts(geschwindigkeit);
   delay(3000);
   halt();
   delay(1000);
   linkskurverueckwaerts(geschwindigkeit);
   delay(3000);
   halt();
   delay(1000);
}