// Arduino robotkar met HC-SR04  utrasoon sensor (afstandsmeter} getest in Bieb Drachten, werkt prima.

/* 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

const int IN1 = 6;

const int IN2 = 5;

const int IN3 = 4;

const int IN4 = 3;

const int ENA = 7; // PWM

const int ENB = 2; // PWM

#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);

}