Arduino robot code v3

arduino_wallE3.ino

/**
   Wall E robot car avoid objects
   @Version: 3.0.0
   @Since  22-nov-2016
   @Author : Luuk Verhoeven
*/
#include <Servo.h>
#include <NewPing.h>


#define SERVO_PIN 7

#define E1 10  // Enable Pin for motor 1
#define E2 11  // Enable Pin for motor 2

// RIGHT
#define I1 8  // Control pin 1 for motor 1
#define I2 9  // Control pin 2 for motor 1

// LEFT
#define I3 12  // Control pin 1 for motor 2
#define I4 13  // Control pin 2 for motor 2

#define MAX_SPEED   200

// Servo.
Servo servo1;
#define SERVO_TURN_TIME 1500 // Delay between servo turns
#define SERVO_CENTER 98
#define SERVO_LEFT 10
#define SERVO_RIGHT 180

// SR04 / Distance sensor.
#define TRIGGER_PIN  5
#define ECHO_PIN     6
#define MAX_DISTANCE 200
#define PING_MEDIAN 5

#define PING_SPEED 50 // Ping frequency (in milliseconds), fastest we should ping is about 35ms per sensor
unsigned long pingTimer1;
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

// Set default values.
int left = 0;
int center = 0;
int right = 0;
int cm = 0;

// For incoming serial data.
int incomingByte = 0;

void setup() {
  //  melody();

  // Log debug info to a serial port.
  Serial.begin(9600);

  pinMode(E1, OUTPUT);
  pinMode(E2, OUTPUT);

  pinMode(I1, OUTPUT);
  pinMode(I2, OUTPUT);
  pinMode(I3, OUTPUT);
  pinMode(I4, OUTPUT);

  servo1.attach(SERVO_PIN);

  // Set eyes to the front.
  servo1.write(SERVO_CENTER);


  // Sensor 1 fires after 100 (PING_SPEED) ms
  pingTimer1 = millis() + PING_SPEED;

  // Speaker.
  pinMode(2, OUTPUT);
  delay(2000);
  melodyStart();
}

void loop() {
  // serialMode();
  defaultMode();
}

void defaultMode() {

  if (millis() >= pingTimer1) {
    pingTimer1 += PING_SPEED; // Make sensor 1 fire again 100ms later (pingSpeed)

    Serial.println("Checking");

    // Check distance
    cm = sonar.ping_cm();
    Serial.println(cm);
  }

  Serial.println("Loop");
  Serial.println(cm);

  if ((cm <= 28) && (cm > 6)) {
    // Something is getting close.
    beep(100);
    beep(50);

    Serial.println("HALT -> Change direction");
    back();

    change_direction();

  } else {
    Serial.println("Forward");

    // forward
    forward();
  }

}


void serialMode() {

  if (Serial.available() > 0) {

    int incomingByte = Serial.read();   // for incoming serial data

    // say what you got:
    Serial.print("I received: ");
    Serial.println(incomingByte);

    switch (incomingByte) {

      case 49: // 1
        Serial.println("moveForward");
        forward();
        break;

      case 50: // 2
        Serial.println("moveBackward");
        back();
        break;

      case 51: // 3
        Serial.println("turnLeft");
        leftMove();
        break;

      case 52: // 4
        Serial.println("turnRight");
        rightMove();
        delay(500);
        break;

      case 53: // 5
        Serial.println("stoping");
        stoping();
        break;
    }

    stoping();
  }

  delay(100);
}

void melodyStart() {

  beep(50);
  beep(150);
  beep(50);
  beep(150);
  delay(500);
}

void beep(unsigned char delayms) {
  analogWrite(2, 255);      // Almost any value can be used except 0 and 255
  // experiment to get the best tone
  delay(delayms);          // wait for a delayms ms
  analogWrite(2, 0);       // 0 turns it off
  delay(delayms);          // wait for a delayms ms

  analogWrite(2, 255);
  delay(delayms);
  analogWrite(2, 0);
}

/**
   Stop
*/
void stoping() {
  digitalWrite(E1, LOW);
  digitalWrite(E2, LOW);
  digitalWrite(I3, LOW);
  digitalWrite(I4, LOW);
  digitalWrite(I1, LOW);
  digitalWrite(I2, LOW);
}

/**
   Move forward
*/
void forward() {

  Serial.println("forward()");

  analogWrite(E1, MAX_SPEED); // Run in half speed
  analogWrite(E2, MAX_SPEED); // Run in full speed

  digitalWrite(I3, LOW);
  digitalWrite(I4, HIGH);

  digitalWrite(I1, HIGH);
  digitalWrite(I2, LOW);
}

/**
   Move left
*/
void leftMove() {
  beep(1000);

  Serial.println("leftMove()");

  analogWrite(E1, MAX_SPEED); // Run in half speed
  analogWrite(E2, MAX_SPEED); // Run in full speed

  digitalWrite(I3, HIGH);
  digitalWrite(I4, LOW);

  digitalWrite(I1, HIGH);
  digitalWrite(I2, LOW);
  delay(400);
}

/**
   Move right
*/
void rightMove() {

  Serial.println("rightMove()");

  analogWrite(E1, MAX_SPEED); // Run in half speed
  analogWrite(E2, MAX_SPEED); // Run in full speed

  digitalWrite(I3, LOW);
  digitalWrite(I4, HIGH);

  digitalWrite(I1, LOW);
  digitalWrite(I2, HIGH);
  delay(400);
}

/**
   Move back
*/
void back() {

  Serial.println("back()");

  analogWrite(E1, MAX_SPEED); // Run in half speed
  analogWrite(E2, MAX_SPEED); // Run in full speed

  digitalWrite(I3, HIGH);
  digitalWrite(I4, LOW);

  digitalWrite(I1, LOW);
  digitalWrite(I2, HIGH);
  delay(1500);
}



// Move the car in the direction where most space is.
void change_direction() {
  stoping();

  ///////////////////////////////////////////////////////////////////////////////////////////////////////
  Serial.println("Right");
  servo1.write(SERVO_RIGHT);

  right = avarage_distance();

  delay(SERVO_TURN_TIME);
  //////////////////////////////////////////////////////////////////////////////////////////////////////////
  Serial.println("Center");
  servo1.write(SERVO_CENTER);

  center = avarage_distance();

  delay(SERVO_TURN_TIME);
  //////////////////////////////////////////////////////////////////////////////////////////////////////////
  Serial.println("Left");
  servo1.write(SERVO_LEFT);

  left = avarage_distance();

  delay(SERVO_TURN_TIME);

  Serial.println("Print meeting:");
  Serial.print(left);
  Serial.println(" cm left");
  Serial.print(center);
  Serial.println(" cm center");

  Serial.print(right);
  Serial.println(" cm right");

  Serial.println("---");
  servo1.write(SERVO_CENTER);
  delay(3000);

  if ((left > center) && (left > right)) {
    Serial.println("Go LEFT");
    leftMove();
    cm = left;
  } else if ((right > center) && (right > left)) {
    Serial.println("Go RIGHT");

    rightMove();
    cm = right;
  } else {
    Serial.println("Go Forward");

    forward();
    cm = center;
  }
  delay(100);
}

// Measure the distance
int avarage_distance() {
  Serial.println("avarage_distance()");
  delay(100); // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
  int cm = sonar.ping_median(PING_MEDIAN) / US_ROUNDTRIP_CM;

  // Serial.print(cm); // Send 3 pins and return the median ping converted to centimeters.
  // Serial.println("cm");

  return cm;
}