Bluetooth Steuerung mit Ultraschall

#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;

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

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

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

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

void stop(){
  digitalWrite(ENA,LOW);
  digitalWrite(ENB,LOW);
  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;
    if (entfernung >= 1000 || entfernung <= 0)
      {
      Serial.println("Kein Messwert");
      }
      else
      {
      Serial.print(entfernung);
      Serial.println(" cm");
      }
           if (entfernung >= 80){
           getstr = Serial.read();
           switch(getstr){
                  case 'a': stateChange(); break;
                  case 'f': forward(); break;
                  case 'b': back(); break;
                  case 'l': left(); break;
                  case 'r': right(); break;
                  case 's': stop(); break;
                  default:  break;
              }
           }
          else if (entfernung < 80){
            getstr = Serial.read();
            switch(getstr){
                  case 'a': stateChange(); break;
                  case 'b': back(); break;
                  case 'l': left(); break;
                  case 'r': right(); break;
                  case 's': stop(); break;
                  default:  break;
            }
          }
          else {
                stop();
              }
            delay(100);