Hello there, I'm building this BT RC car with obsticle avoidance, everything works kind of fine but i have this annoying problem. When ping sensor sense a obsticle it should stop and reverse the car for 1,5sec, everything is fine with stoping the motor so i cannot push it foward anymore while in this zone Distance<minDistance, the problem starts when it wants to reverse, it looks like it get just a half of current as speed would be like 100 if we count from 0-255, but if I also press reverse on my android it gets to normal speed, so i decided that it shares the same level of state. So my question would be how can I give priority, let's say for Ping, while it goes backward or something like that 
Other question would be, if ping sometimes sense false ranges for example : if range is 100cm sometimes it drops to 70cm what can be wrong about it? Could it be a lack of voltage ? Or It's software? Thank you for you answers I leave my code here:
#include <Servo.h>
#include <NewPing.h>
//L293
const int motorA1 = 5; // Pin 2
const int motorA2 = 6; // Pin 7
const int motorB1 = 10; // Pin 10
const int motorB2 = 9; // Pin 14
//Ultra garsas
const int trig = A1; //paduoda garsa
const int echo = A2; //priima garsa
const int maxDistance = 120; //cm
int minDistance = 50; //cm
int revDistance = 55; //cm
unsigned int pingSpeed = 50; // frequency of ping
unsigned long pingTimer;
unsigned int Distance;
int reverse = LOW;
NewPing sonar(trig, echo, maxDistance); // NewPing setup of pins and maximum distance.
//LEDAI
const int lights = 13;
const int lightsb = 12;
const int lightsrev = A4;
//Bluetooth
const int BTState = 2;
//Servo
Servo myservo; // create servo object to control a servo
const int servopin = A3;
int rotation = 1;
//Baterijos likutis
//const float maxBattery = 9.0;// Change value to your max battery voltage level!
//int perVolt; // Percentage variable
//float voltage = 0.0; // Read battery voltage
//int level;
// Use it to make a delay... without delay() function!
unsigned long previousMillis = 0; // -1000*10=-10sec. to read the first value. If you use 0 then you will take the first value after 10sec. ///long previousMillis = -1000 * 10;
const long interval = 1500; // interval at which to read battery voltage, change it if you want! (10*1000=10sec) ////long interval = 1000 * 10;
unsigned long currentMillis = 0; //unsigned long currentMillis;unsigned int pingSpeed = 50; // frequency of ping
//Papildomos nuorodos i koda
int i = 0;
int j = 0;
int state;
int vSpeed = 200; // Default speed, from 0 to 255
int RSpeed = 0;
int data;
void setup() {
pinMode(motorA1, OUTPUT);
pinMode(motorA2, OUTPUT);
pinMode(motorB1, OUTPUT);
pinMode(motorB2, OUTPUT);
pinMode(lights, OUTPUT);
pinMode(BTState, INPUT);
pinMode(lightsb, OUTPUT);
pinMode(lightsrev, OUTPUT);
// Serial for US and BT
Serial.begin(9600);
pingTimer = millis(); // Start now.
//Servo
myservo.attach(servopin); // attaches the servo on pin to the servo object
myservo.write(90); // align the servo ahead
}
void loop() {
//Isjungti variklius jei dingsta rysys
if (digitalRead(BTState) == LOW) {
state = 'S';
}
//BT duomenu saugojimas (siuntimas gavimas) jei daugiau nei 0 bitu
if (Serial.available() > 0) {
state = Serial.read();
}
//4 greiciu reguliatorius Values must be from 0 to 255 (PWM)
if (state == '0') {
vSpeed = 0;
}
else if (state == '1') {
vSpeed = 100;
}
else if (state == '2') {
vSpeed = 180;
}
else if (state == '3') {
vSpeed = 200;
}
else if (state == '4') {
vSpeed = 255;
}
/***********************Fowardi****************************/
//Raide F= Foward
if (state == 'F') {
analogWrite(motorA1, vSpeed); analogWrite(motorA2, 0);
analogWrite(motorB1, 0); analogWrite(motorB2, 0);
myservo.write(90);
}
/**********************Forward Left************************/
//Raide G= Left Foward
else if (state == 'G') {
analogWrite(motorA1, vSpeed); analogWrite(motorA2, 0);
analogWrite(motorB1, 255); analogWrite(motorB2, 0);
}
/**********************Forward Right************************/
//Raide I= Right Foward
else if (state == 'I') {
analogWrite(motorA1, vSpeed); analogWrite(motorA2, 0);
analogWrite(motorB1, 0); analogWrite(motorB2, 255);
}
/***********************Backward****************************/
//Raide B= Backward
else if (state == 'B') {
analogWrite(motorA1, 0); analogWrite(motorA2, 255);
analogWrite(motorB1, 0); analogWrite(motorB2, 0);
}
/**********************Backward Left************************/
//Raide H= Backward Left
else if (state == 'H') {
analogWrite(motorA1, 0); analogWrite(motorA2, 255);
analogWrite(motorB1, 255); analogWrite(motorB2, 0);
}
/**********************Backward Right************************/
//Raide J= Backward Right
else if (state == 'J') {
analogWrite(motorA1, 0); analogWrite(motorA2, 255 );
analogWrite(motorB1, 0); analogWrite(motorB2, 255);
}
/***************************Left*****************************/
//Raide L= Left
else if (state == 'L') {
analogWrite(motorA1, 0); analogWrite(motorA2, 0);
analogWrite(motorB1, 255); analogWrite(motorB2, 0);
myservo.write(70);
}
/***************************Right*****************************/
//Raide R= right
else if (state == 'R') {
analogWrite(motorA1, 0); analogWrite(motorA2, 0);
analogWrite(motorB1, 0); analogWrite(motorB2, 255);
myservo.write(110);
}
/************************Lights*****************************/
//Raide W= Lights
else if (state == 'W') {
if (i == 0) {
digitalWrite(lights, HIGH);
i = 1;
}
else if (i == 1) {
digitalWrite(lights, LOW);
i = 0;
}
state = 'w';
}
/************************Galines????*****************************/
//Raide U= Lights
else if (state == 'U') {
if (i == 0) {
digitalWrite(lightsb, HIGH);
i = 1;
}
else if (i == 1) {
digitalWrite(lightsb, LOW);
i = 0;
}
state = 'u';
}
/***********************Ultra garsas*****************************/
//UltraSonicMillisPing
// Notice how there's no delays in this sketch to allow you to do other processing in-line while doing distance pings.
if (millis() >= pingTimer) { // pingSpeed milliseconds since last ping, do another ping.
pingTimer += pingSpeed; // Set the next ping time.
sonar.ping_timer(echoCheck); // Send out the ping, calls "echoCheck" function every 24uS where you can check the ping status.
Serial.print("Ping: ");
Serial.print(Distance); // Ping returned, uS result in ping_result, convert to cm with US_ROUNDTRIP_CM.
Serial.println("cm");
}
/*************************************************/
//Raide X Us valdymas
if (Distance < minDistance)
vSpeed = 0;
else if (Distance > minDistance)
vSpeed = 200;
unsigned long currentMillis = millis();
if (Distance <= minDistance)
if (currentMillis - previousMillis >= interval) {
previousMillis = currentMillis;
if (reverse == LOW) {
reverse = HIGH;
}
else {
reverse = LOW;
}
}
analogWrite(motorA2, reverse);
digitalWrite(lightsrev, reverse);
}
//sviesos sensorius
void echoCheck() { // Timer2 interrupt calls this function every 24uS where you can check the ping status.
// Don't do anything here!
if (sonar.check_timer()) { // This is how you check to see if the ping was received.
// Here's where you can add code.
Serial.print("Ping: ");
Serial.print(sonar.ping_result / US_ROUNDTRIP_CM); // Ping returned, uS result in ping_result, convert to cm with US_ROUNDTRIP_CM.
Serial.println("cm");
Distance = sonar.ping_result / US_ROUNDTRIP_CM;
}
// Don't do anything here!
}