SONAR Based Obstacle Avoidance Robot

#include <Servo.h>


// ULTRASONIC SENSOR
const int trigPin = 7; //Define Trig pin
const int echoPin = 6; //Define Echo pin

//ARRAYS
static float dist_table[82];
static float pos_table[82];
int face_here = 0;

//SERVO
Servo servo;

//74HC595 PINS
int outputEnablePin = 12;//Pin connected to Output enable of 74HC595
int dataPin = 11; //Pin connected to DS of 74HC595
int latchPin = 10;//Pin connected to ST_CP of 74HC595
int clockPin = 9;//Pin connected to SH_CP of 74HC595

//REGISTER ADDRESSES and CONTROL
byte FWD = 0x46; //Foward
byte BCK = 0x8A; //Backwards
byte L = 0x4A; //Left
byte R = 0x86; //Right
byte CLEAR = 0x3; //Clear Register

byte data;
int delayTime;

int PWM=250;


void setup() {
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  pinMode(dataPin, OUTPUT);
  pinMode(clockPin, OUTPUT);
  pinMode(latchPin, OUTPUT);
   pinMode(outputEnablePin, OUTPUT); 

  servo.attach(8);
  Serial.begin(9600);
  servo.write(87);

  Clear_Reg();

  //START SEQUENCE
  delay(10000);
  SonarscanA();
  delay(50);
  servo.write(87);
  delay(50);
  Positioning();
  delay(50);
  Short_Exploration();

}


void loop() {}

//Distance Measurments
float UltrasonicMeasuring () {
  float duration, distance;
  digitalWrite(trigPin, LOW); //Start low for 2 milliseconds
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH); //Sonic burst of 8 cycles for 10 milliseconds
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = (duration * .0343) / 2;
  return distance;
}

//SCAN Process
float*SonarscanA() {
  float distance;
  float distA;
  float distB;
  int pos = 10;
  int p = 20;
  int i = 0;
  int j = 0;
  int prev = 0;
  int check;
  int RD;
  servo.write(10);
  delay(50);

  for (pos = 10; pos <= 170; pos += 2) {
    servo.write(pos);
    Serial.print(pos);
    Serial.print(",");
    delayMicroseconds(50);
    distA = UltrasonicMeasuring ();
    delay(p);
    distB = UltrasonicMeasuring ();
    delay(p);
    distance = (distA + distB) / 2;

    RD = round(distance);
    Serial.print(RD);
    Serial.print(".");

    dist_table[i] = distance;
    pos_table[i] = pos;
    i = i + 1;
  }
  for (pos = 170; pos >= 10; pos -= 2) {
    servo.write(pos);
    Serial.print(pos);
    Serial.print(",");
    delayMicroseconds(50);
    distA = UltrasonicMeasuring ();
    delay(p);
    distB = UltrasonicMeasuring ();
    delay(p);
    distance = (distA + distB) / 2;

    RD = round(distance);
    Serial.print(RD);
    Serial.print(".");

    dist_table[81 - j] = (dist_table[81 - j] + distance) / 2;
    j = j + 1;
  }

  for (int c = 0; c < 81; c++) {
    check = dist_table[c];
    if (check >= prev) {
      face_here = pos_table[c];
      prev = check;
    }
  }
  return face_here;
}

//Determin direction to face after scan
void Positioning() {
  if (face_here >= 0 && face_here <= 19) {
    data = R;
    delayTime = 510;
    Register();
  }
  if (face_here >= 20 && face_here <= 39) {
    data = R;
    delayTime = 410;
    Register();
  }
  if (face_here >= 40 && face_here <= 59) {
    data = R;
    delayTime = 310;
    Register();
  }
  if (face_here >= 60 && face_here <= 79) {
    data = R;
    delayTime = 160;
    Register();
  }
  if (face_here >= 80 && face_here <= 99) {
    data = FWD;
    delayTime = 100;
    Register();
  }
  if (face_here >= 100 && face_here <= 119) {
    data = L;
    delayTime = 160;
    Register();
  }
  if (face_here >= 120 && face_here <= 139) {
    data = L;
    delayTime = 310;
    Register();
  }
  if (face_here >= 140 && face_here <= 159) {
    data = L;
    delayTime = 410;
    Register();
  }
  if (face_here >= 160 && face_here <= 179) {
    data = L;
    delayTime = 510;
    Register();
  }
}


//Register
void shiftWrite(int desiredPin, boolean desiredState) {
  bitWrite(data, desiredPin, desiredState);
  shiftOut(dataPin, clockPin, MSBFIRST, data);
  digitalWrite(latchPin, HIGH);
  digitalWrite(latchPin, LOW);
}

void Register()
{
  int index;
  setpwm(PWM);
  for (index = 0; index <= 7; index++) {
    shiftWrite(index, HIGH);
    delay(delayTime);
    shiftWrite(index, LOW);
  }
}

//TEST EXPLORATION 
void Short_Exploration() {
  int thresh = 15; //Sensor distance threshold
  float distance;
  int finish = 0;
  int RD;
  int pos = 90;
  while (finish < 10) {  //As a test the robot completes 10 turns before stopping
    Serial.print(pos);
    Serial.print(",");
    distance = UltrasonicMeasuring ();
    RD = round(distance);
    Serial.print(RD);
    Serial.print(".");
    if (distance < thresh) {
      Clear_Reg();
      Decide_Direction();
      finish = finish + 1;
    }
    else {
      Move_FWD();
    }
  }
}

//MOVE FOWARD Function
void Move_FWD() {
  data = FWD;
  digitalWrite(latchPin, 0);
  shiftOut(dataPin, clockPin, data);
  digitalWrite(latchPin, 1);
}

//Shifting bits into the register
void shiftOut(int myDataPin, int myClockPin, byte myDataOut) {
  int i = 0;
  int pinState;
  pinMode(myClockPin, OUTPUT);
  pinMode(myDataPin, OUTPUT);
  digitalWrite(myDataPin, 0);
  digitalWrite(myClockPin, 0);
  for (i = 7; i >= 0; i--)  {
    digitalWrite(myClockPin, 0);
    if ( myDataOut & (1 << i) ) {
      pinState = 1;
    }
    else {
      pinState = 0;
    }
    digitalWrite(myDataPin, pinState);
    digitalWrite(myClockPin, 1);
    digitalWrite(myDataPin, 0);
  }
  digitalWrite(myClockPin, 0);
}

// Direction Decision
float Decide_Direction() {
  float distA, distB;
  float Left, Right;
  float Turn;
  int RD;
  int l = 130;
  int r = 50;
  servo.write(130);
  Serial.print(l);
  Serial.print(",");
  delay(350);
  distA = UltrasonicMeasuring ();
  delay(20);
  distB = UltrasonicMeasuring ();
  Left = (distA + distB) / 2;

  RD = round(Left);
  Serial.print(RD);
  Serial.print(".");

  servo.write(50);
  Serial.print(r);
  Serial.print(",");
  delay(350);
  distA = UltrasonicMeasuring ();
  delay(20);
  distB = UltrasonicMeasuring ();
  Right = (distA + distB) / 2;

  RD = round(Right);
  Serial.print(RD);
  Serial.print(".");

  servo.write(87);

  if (Left > Right) {
    data = L;
    delayTime = 350;
    Register();
  }
  if (Right > Left) {
    data = R;
    delayTime = 350;
    Register();
  }
}

//CLAR REGISTER
void Clear_Reg() {
  data = CLEAR;
  delayTime = 150;
  Register();
}


//PWM setup
void setpwm(byte pwm) // 0 to 255
{
  analogWrite(outputEnablePin, 255-pwm);
}