ArduinoRobotNa17c

int maxDistanceBeforeTurn;
int lampa = 1;
int echoPin = 7;
int trigPin = 1;
int duration;
int lampa1 = 13;
int lampa2 = 8;
int lampa3 = 4;
int lampa4 = 3;
int lampa5 = 2;
int leftMotor = 12;
int rightMotor = 11;
int speed = 255;
float turnTime = 5;
float carWidth = 10;
float turningRadius = 20;
float angle = 3.1415/2;
int lampDelay = 20;

void setup(){
    Serial.begin(9600);
    pinMode(echoPin, INPUT);
    pinMode(trigPin, OUTPUT);
    pinMode(lampa1, OUTPUT);
    pinMode(lampa2, OUTPUT);
    pinMode(lampa3, OUTPUT);
    pinMode(lampa4, OUTPUT);
    pinMode(lampa5, OUTPUT);
    pinMode(leftMotor, OUTPUT);
    pinMode(rightMotor, OUTPUT);
}

void blink(int lampa){
    switch (lampa){
        case 1:
            digitalWrite(lampa1, HIGH);
            delay(lampDelay);
            digitalWrite(lampa1, LOW);
        case 2:
            digitalWrite(lampa2, HIGH);
            delay(lampDelay);
            digitalWrite(lampa2, LOW);
        case 3:
            digitalWrite(lampa3, HIGH);
            delay(lampDelay);
            digitalWrite(lampa3, LOW);
        case 4:
            digitalWrite(lampa4, HIGH);
            delay(lampDelay);
            digitalWrite(lampa4, LOW);
        case 5:
            digitalWrite(lampa5, HIGH);
            delay(lampDelay);
            digitalWrite(lampa5, LOW);
        case 6:
            digitalWrite(lampa4, HIGH);
            delay(lampDelay);
            digitalWrite(lampa4, LOW);
        case 7:
            digitalWrite(lampa3, HIGH);
            delay(lampDelay);
            digitalWrite(lampa3, LOW);
        case 8:
            digitalWrite(lampa2, HIGH);
            delay(lampDelay);
            digitalWrite(lampa2, LOW);
    }
}

void motorON(int speed){
    analogWrite(leftMotor, speed);
    analogWrite(rightMotor, speed);
}

void motorOFF(){
    analogWrite(leftMotor, 0);
    analogWrite(rightMotor, 0);
}

float calculateSpeed(float carWidth, float turningRadius, float angle){
    return (turningRadius*angle*255)/((turningRadius+carWidth)*angle);
}

void turnLeftAngle(){
    motorOFF();
    digitalWrite(rightMotor, 255);
    digitalWrite(leftMotor, calculateSpeed(carWidth, turningRadius, angle));
    delay(turnTime*1000);
    motorOFF();
}

float getDistance(){
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    duration = pulseIn(echoPin, HIGH);
    return duration / 58;
}

void loop(){
    motorON(speed);
    float distance = getDistance();
    Serial.println(distance);
    if (distance < maxDistanceBeforeTurn) turnLeftAngle();
    if (lampa != 8){
        blink(lampa);
        lampa++;
    } else if (lampa == 8){
        blink(lampa);
        lampa = 1;
    }
}