// Arduino robotkar met HC-SR04  utrasoon sensor (afstandsmeter} getest in Bieb Drachten, AANGEPAST VOOR THEOKAR NIEUW - ZIE #IF 1 - versie2

/* over de HC-SR04 ultrasoon sensor:

blauw = VCC=spanning,

Geel = Trigger komt op 12,

Groen = Echo komt op 13

Purper = Ground

*/

// Definieer pinnen voor de motor driver

#if 0

const int IN1 = 5;

const int IN2 = 6;

const int IN3 = 9;

const int IN4 = 10;

const int ENA = 3; // PWM

const int ENB = 11; // PWM

#endif

#if 1

// Definieer pinnen voor de motor driver - AANGEPAST VOOR THEO-KAR NIEUW

const int IN1 = 8;           //ORANJE

const int IN2 = 9;           //GEEL

const int IN3 = 10;          //BRUIN

const int IN4 = 11;          //BLAUW

const int ENA = 5; // PWM    //GROEN

const int ENB = 6; // PWM    //ZWART

#endif

// Definieer pinnen voor de ultrasone sensor

const int trigPin = 12;

const int echoPin = 13;

// Variabelen voor afstandsmeting

long duration;

int distance;

void setup() {

  // Motor pinnen als output

  pinMode(IN1, OUTPUT);

  pinMode(IN2, OUTPUT);

  pinMode(IN3, OUTPUT);

  pinMode(IN4, OUTPUT);

  pinMode(ENA, OUTPUT);

  pinMode(ENB, OUTPUT);

  // Ultrasone sensor pinnen

  pinMode(trigPin, OUTPUT);

  pinMode(echoPin, INPUT);

  // Start seriële communicatie voor debugging

  Serial.begin(9600);

}

void loop() {

  // Meet de afstand

  distance = measureDistance();

  Serial.print("Afstand: ");

  Serial.print(distance);

  Serial.println(" cm");

//distance = 30;

  // Als er een obstakel binnen 20 cm is, ga achteruit en draai

if (distance < 20)

{

    moveBackward();

    delay(500);

    turnRight();

    delay(500);

  } else {

    moveForward();

  }

  delay(100);

}

// Functie om afstand te meten met de HC-SR04

int measureDistance() {

  digitalWrite(trigPin, LOW);

  delayMicroseconds(2);

  digitalWrite(trigPin, HIGH);

  delayMicroseconds(10);

  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);

  return duration * 0.034 / 2;

}

// Functies voor beweging

void moveForward() {

    Serial.print("Vooruit ");

  digitalWrite(IN1, HIGH);

  digitalWrite(IN2, LOW);

  digitalWrite(IN3, HIGH);

  digitalWrite(IN4, LOW);

  analogWrite(ENA, 150); // Snelheid (0-255)

  analogWrite(ENB, 150);

}

void moveBackward() {

  Serial.print("achteruit ");

  digitalWrite(IN1, LOW);

  digitalWrite(IN2, HIGH);

  digitalWrite(IN3, LOW);

  digitalWrite(IN4, HIGH);

  analogWrite(ENA, 150);

  analogWrite(ENB, 150);

}

void turnRight() {

  Serial.print("rechts");

  digitalWrite(IN1, HIGH);

  digitalWrite(IN2, LOW);

  digitalWrite(IN3, LOW);

  digitalWrite(IN4, HIGH);

  analogWrite(ENA, 150);

  analogWrite(ENB, 150);

}

void turnLeft() {

  Serial.print("links ");

  digitalWrite(IN1, LOW);

  digitalWrite(IN2, HIGH);

  digitalWrite(IN3, HIGH);

  digitalWrite(IN4, LOW);

  analogWrite(ENA, 150);

  analogWrite(ENB, 150);

}

void stopMoving() {

  Serial.println("stop ");

  digitalWrite(IN1, LOW);

  digitalWrite(IN2, LOW);

  digitalWrite(IN3, LOW);

  digitalWrite(IN4, LOW);

  analogWrite(ENA, 0);

  analogWrite(ENB, 0);

}