1401 distant ezda

//////////////////////////////////////////////
//     2019.07.16 woronin,  umkiedu@gmail.com
//     Проезд по датчику ультразвук - вдоль стены
//     Robot UMKI controller  K6_3
//     самостоятельно сделать реакцию на препятствие
//////////////////////////////////////////////
#define Trig A3 // трегер-излучатель на контакте А3 
#define Echo A4 // приемниик на А4
int motorR1=2; //контакт 2 определяет напр вращения правого мотора
int motorL1=4; //контакт 4 определяет напр вращ левого мотора
int mL_PWM=3; // контакт 3 определяет скорость вращения лев мотора от 0 до 255
int mR_PWM=5; // конт 5 - прав мотор
void setup() {
pinMode(Trig, OUTPUT); //инициируем как выход 
pinMode(Echo, INPUT); //инициируем как вход 
Serial.begin(9600); // задаем скорость общения.
// задаем параметры работы моторов L & R
pinMode(mL_PWM, OUTPUT);
pinMode(mL_PWM, OUTPUT);
pinMode(motorR1, OUTPUT);
pinMode(motorL1, OUTPUT);
delay(2000); 
}
unsigned int impulseTime=0; 
unsigned int distance_sm=0;
void loop() {
digitalWrite(motorR1, HIGH);
analogWrite(mR_PWM,200); //ШИМ =0 мотор стоит
digitalWrite(motorL1, HIGH);
analogWrite(mL_PWM, 200);
delay(500);
digitalWrite(Trig, HIGH); 
/* Подаем импульс на вход trig дальномера */
delayMicroseconds(10); // равный 10 микросекундам 
digitalWrite(Trig, LOW); // Отключаем 
impulseTime=pulseIn(Echo, HIGH); // Замеряем длину импульса 
distance_sm=impulseTime/58; // Пересчитываем в сантиметры 
Serial.println(distance_sm); // Выводим на порт 
if (distance_sm<30) // Если расстояние менее 20 сантиметром 
{ 
digitalWrite(motorR1,HIGH); //правый газ левый стоп, поворот налево
analogWrite(mR_PWM, 70); 
digitalWrite(motorL1, HIGH);
analogWrite(mL_PWM,200);
delay(500);
} 
else if (distance_sm>40)
{
digitalWrite(motorR1,HIGH); // правый стоп, левый газ, поворот на право
analogWrite(mR_PWM, 200); 
digitalWrite(motorL1, HIGH);
analogWrite(mL_PWM,70);
delay(500);
}
else
{
digitalWrite(motorR1,HIGH); //прямо
analogWrite(mR_PWM, 250); 
digitalWrite(motorL1, HIGH);
analogWrite(mL_PWM,250);
delay(500); 
}
delay(100);
}