bono 4

#define ENA 5
#define ENB 6
#define IN1 7
#define IN2 8
#define IN3 9
#define IN4 11
#define LED 13

#include <Servo.h>  //servo library
Servo myservo;      // create servo object to control servo

bool state = LOW;
char getstr;
int speedy = 150;

int Echo = A4;
int Trig = A5;
long dauer = 0; //funduino.de Nr.10
long entfernung = 0; //funduino.de Nr.10
int rightDistance = 0, leftDistance = 0, middleDistance = 0;
int currentDirection = 0; //0 = stop, 1 = forward, 2 = reverse, 3 = left, 4 = right
bool distForwardOk = false;

void forward()
{
  digitalWrite(ENA,HIGH);
  digitalWrite(ENB,HIGH);
  digitalWrite(IN1,HIGH);
  digitalWrite(IN2,LOW);
  digitalWrite(IN3,LOW);
  digitalWrite(IN4,HIGH);
  currentDirection = 1;
  Serial.println("Forward");
}

void back()
{
  digitalWrite(ENA,HIGH);
  digitalWrite(ENB,HIGH);
  digitalWrite(IN1,LOW);
  digitalWrite(IN2,HIGH);
  digitalWrite(IN3,HIGH);
  digitalWrite(IN4,LOW);
  currentDirection = 2;
  Serial.println("Back");
}

void left()
{
  analogWrite(ENA,speedy);
  analogWrite(ENB,speedy);
  digitalWrite(IN1,LOW);
  digitalWrite(IN2,HIGH);
  digitalWrite(IN3,LOW);
  digitalWrite(IN4,HIGH);
  currentDirection = 3;
  Serial.println("Left");
}

void right()
{
  analogWrite(ENA,speedy);
  analogWrite(ENB,speedy);
  digitalWrite(IN1,HIGH);
  digitalWrite(IN2,LOW);
  digitalWrite(IN3,HIGH);
  digitalWrite(IN4,LOW);
  currentDirection = 4;
  Serial.println("Right");
}

void stop()
{
  digitalWrite(ENA,LOW);
  digitalWrite(ENB,LOW);
  currentDirection = 0;
  Serial.println("Stop!");
}

void stateChange()
{
  state = !state;
  digitalWrite(LED, state);
  Serial.println("Light");
}

void speedDown()
{
  speedy = (speedy-50);

  if (speedy <=100)
    speedy = 100;
}

void speedUp()
{
  speedy = (speedy+50);

  if (speedy >=200)
    speedy = 200;
}

  //Ultrasonic distance measurement Sub function
int Distance_test() {
  digitalWrite(Trig, LOW);
  delayMicroseconds(2);
  digitalWrite(Trig, HIGH);
  delayMicroseconds(20);
  digitalWrite(Trig, LOW);
  float Fdistance = pulseIn(Echo, HIGH);
  Fdistance= Fdistance / 58;
  return (int)Fdistance;
}

void setup() {
  Serial.begin(9600);
  pinMode(LED, OUTPUT);
  pinMode(IN1,OUTPUT);
  pinMode(IN2,OUTPUT);
  pinMode(IN3,OUTPUT);
  pinMode(IN4,OUTPUT);
  pinMode(ENA,OUTPUT);
  pinMode(ENB,OUTPUT);
  pinMode(Trig, OUTPUT); // Trigger-Pin ist ein Ausgang
  pinMode(Echo, INPUT); // Echo-Pin ist ein Eingang
  stop();
}

void loop()
{

    digitalWrite(Trig, LOW);
    delay(5);
    digitalWrite(Trig, HIGH);
    delay(10);
    digitalWrite(Trig, LOW);
    dauer = pulseIn(Echo, HIGH);
    entfernung = (dauer/2) * 0.03432;
    distForwardOk = false;

    if (entfernung >= 1000 || entfernung <= 0)
    {
        Serial.println("Kein Messwert");
    }
    else
    {
        Serial.print(entfernung);
        Serial.println(" cm");

        if (entfernung >= 80)
        {
            distForwardOk = true;
        }
    }


    getstr = Serial.read();

    switch(getstr)
    {
        case 'a':
            stateChange();
            break;
        case 'f':
            if (distForwardOk)
            {
                forward();
            }
            break;

        case 'b':
            back();
            break;

        case 'l':
            left();
            break;

        case 'r':
            right();
            break;

        case 's':
            stop();
            break;

        default:
            break;

    }


    if (currentDirection == 1 &&
        !distForwardOk)
    {
        stop();
    }

    delay(100);
}