Ülesanne 7.2 Iseliikuv auto

const int trigPin = 3;
const int echoPin = 2;
const int trigPin2 = 9; 
const int echoPin2 = 7; 
float dist_check1, dist_check2;
long duration, distance;
int dist_result;
 
//MOTORS VARIABLES
const int mot1f = 6;
const int mot1b = 5;
const int mot2f = 11;
const int mot2b = 10;
int mot_speed_right = 75;
int mot_speed_left = 130;
int k = 0; //BRAKE
 
//LOGICS VARIABLES
const int dist_stop = 20;
const int max_range = 1200;
const int min_range = 20;
int closerSensor = 0;
int errorLED = 13;
 
//FUNCTIONS
int ping() { //CHECK DISTANCE FUNCTION 
    int sumDistances1 = 0, sumDistances2 = 0;
    for(int i = 0; i < 3; i++) {
        digitalWrite(trigPin, 0);
        delayMicroseconds(2);
        digitalWrite(trigPin, 1);
        delayMicroseconds(10);
        digitalWrite(trigPin, 0);
        duration = pulseIn(echoPin, 1);
        distance = duration / 58;
        sumDistances1 += distance;
        
        digitalWrite(trigPin2, 0);
        delayMicroseconds(2);
        digitalWrite(trigPin2, 1);
        delayMicroseconds(10);
        digitalWrite(trigPin2, 0);
        duration = pulseIn(echoPin2, 1);
        distance = duration / 58;
        sumDistances2 += distance;
    }
    float avgDistance1 = sumDistances1 / 3;
    float avgDistance2 = sumDistances2 / 3;
    
   
    int closerSensor = (avgDistance1 < avgDistance2)? 1 : 2;
    dist_result = avgDistance1 + avgDistance2; 

    Serial.print("Average Distance 1: ");
    Serial.println(avgDistance1);
    Serial.print("Average Distance 2: ");
    Serial.println(avgDistance2);
    Serial.print("Total Distance Result: ");
    Serial.println(dist_result);
    
    return dist_result;
}
 
//INITIALIZATION
void setup() {
    pinMode(trigPin, OUTPUT);
    pinMode(echoPin, INPUT);
    pinMode(trigPin2, OUTPUT);
    pinMode(echoPin2, INPUT); 
    pinMode(errorLED, OUTPUT);
    Serial.begin(9600); // Initialize serial communication at 9600 baud rate
    delay(3000); // Delay for 3 seconds before starting
}
 
//BASIC PROGRAM CYCLE
void loop() {
    int result1 = ping(); // Check distance for sensor 1
    int result2 = ping(); // Check distance for sensor 2
    
    // Define the threshold for detecting obstacles

    // Check if either sensor detects an obstacle within the threshold distance
    if (result1 <= dist_stop || result2 <= dist_stop) {
        // Turn right if sensor 1 detects an obstacle
        if (result1 <= dist_stop && result2 > dist_stop) {
          motors_back();
          delay(500);
          motors_stop();
          delay(1000);
          motors_right();
          delay(150);
          motors_stop();
          delay(1000); 
        }
        // Turn left if sensor 2 detects an obstacle
        else if (result2 <= dist_stop && result1 > dist_stop) {
          motors_back();
          delay(500);
          motors_stop();
          delay(1000);
          motors_left();
          delay(150);
          motors_stop();
          delay(1000); 
        }
        // Turn back if both sensors detect an obstacle
        else {
            motors_back();
            delay(500);
            motors_stop();
            delay(1000);
            motors_left();
            delay(500);
            motors_stop();
            delay(1000);
        }
    } else { // If no obstacles detected, move forward
        motors_forward();
        delay(100);
    }
}
 
//MOTOR CONTROL FUNCTIONS
void motors_forward() {
    analogWrite(mot1f, mot_speed_left);
    analogWrite(mot2f, mot_speed_right);
    digitalWrite(mot1b, 0);
    digitalWrite(mot2b, 0);
}
 
void motors_back() {
    digitalWrite(mot1f, 0);
    digitalWrite(mot2f, 0);
    analogWrite(mot1b, mot_speed_left);
    analogWrite(mot2b, mot_speed_right);
}
 
void motors_stop() {
    digitalWrite(mot1f, 1);
    digitalWrite(mot2f, 1);
    digitalWrite(mot1b, 1);
    digitalWrite(mot2b, 1);
}
 
void motors_left() {
    analogWrite(mot1f, mot_speed_left);
    digitalWrite(mot2f, 0);
    digitalWrite(mot1b, 0);
    analogWrite(mot2b, mot_speed_right);
}
 
void motors_right() {
    digitalWrite(mot1f, 0);
    analogWrite(mot2f, mot_speed_right);
    analogWrite(mot1b, mot_speed_left);
    digitalWrite(mot2b, 0);
}
 
void motors_foward_left() {
    k = mot_speed_right * 0.8;
    analogWrite(mot1f, mot_speed_left);
    analogWrite(mot2f, k);
    digitalWrite(mot1b, 0);
    digitalWrite(mot2b, 0);
}
 
void motors_foward_right() {
    k = mot_speed_left * 0.8;
    analogWrite(mot1f, k);
    analogWrite(mot2f, mot_speed_right);
    analogWrite(mot1b, 0);
    analogWrite(mot2b, 0);
}
 
void motors_back_left() {
    k = mot_speed_right * 0.8;
    digitalWrite(mot1f, 0);
    digitalWrite(mot2f, 0);
    analogWrite(mot1b, k);
    analogWrite(mot2b, mot_speed_left);
}
 
void motors_back_right() {
    k = mot_speed_left * 0.8;
    digitalWrite(mot1f, 0);
    digitalWrite(mot2f, 0);
    analogWrite(mot1b, mot_speed_right);
    analogWrite(mot2b, k);
}