Project Details

//Rangefinder Arduino code

//Ultrasonic Rangefinder
#include <PWMServo.h>
#include <NewPing.h>

PWMServo servo1;

unsigned int duration;

long timer;

#define svomin 10
#define svomax 150
#define svodormant 80

#define lmotor 1
#define rmotor 2

#define mStop 0
#define mForward 1
#define mReverse 2
#define mLeft 3
#define mRight 4

#define lmotorf 3
#define lmotorr 2
#define rmotorf 5
#define rmotorr 4

#define trigPin 14
#define echoPin 15
#define button1 16
#define avoid 17

#define maxdistance 100

long runTime = 0;
int robotState = mStop;
int mbm = 52;
int svo = svomin;
int count = svomax - svomin;
float distance[180];
int buttonState;
int lastButtonState = LOW;
int scanState = false;
int avoidState;
int lastAvoidState = LOW;
long lastDebounceTime = 0;
long debounceDelay = 50;

NewPing sonar(trigPin, echoPin, maxdistance);

void setup() {
  servo1.attach(SERVO_PIN_A);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(button1, INPUT);
  pinMode(avoid, INPUT);
  pinMode(lmotorf, OUTPUT);
  pinMode(lmotorr, OUTPUT);
  pinMode(rmotorf, OUTPUT);
  pinMode(rmotorr, OUTPUT);
  Serial.begin(57600);
  //Serial.print("AT+BAUD7");
  svo = svodormant;
  servo1.write(svo);
  delay(1000);
  timer = millis() + mbm;
  scanState = false;
}

void loop() {
  
  int reading = digitalRead(button1);
  avoidState = digitalRead(avoid);
  
  if (scanState == true) {
    robotStop();
  }
  
  if (avoidState == LOW && scanState == false && robotState == mForward) {
    scanState = true;
    svo = svomin;
    servo1.write(svo);
  }
  
  if (reading != lastButtonState) {
    lastDebounceTime = millis();
  }
  
  if((millis() - lastDebounceTime) > debounceDelay) {
    if(reading != buttonState) {
      buttonState = reading;
      if(buttonState == HIGH) {
        scanState = true;
        svo = svomin;
        servo1.write(svo);
      }
    }
  }
  lastButtonState = reading;
  
  if(timer < millis() && scanState==true) {    
    getDist2();
    svo++;
    if(svo > svomax) { 
      svo=svodormant; 
      smooth();
      smooth();
      Serial.println("");
      Serial.println("BEGIN");
      for(int i=0;i<=count;i++) {
        Serial.print("S");
        Serial.println(i+10,DEC);
        Serial.print("V");
        Serial.println(distance[i], DEC); 
      }
      scanState = false;
      robotThink();
    }
    servo1.write(svo);
    if(svo==svomin) { delay(1000); }
    timer = millis() + mbm;
  }
  if (runTime < millis()) {
    motor(lmotor, mStop);
    motor(rmotor, mStop);
    robotThink();
  }
}

void getDist2() {
  duration = sonar.ping();
  distance[svo-svomin] = duration/US_ROUNDTRIP_CM;
  if(duration==0) { distance[svo-svomin] = maxdistance; }
}

void smooth() {
  
  int p;
  float temp;
  float mean;
  
  for(int i=0;i<=count;i++) {
    mean=0;
    for(int t=-4;t<=4;t++) {
      p=i+t;
      if(p>count) {
        p=p-(count+1);
      }
      if(p<0) {
        p=p+(count+1);
      }
      mean = mean + distance[p];  
    }
    mean = mean / 9;
    distance[i] = mean;
  }
}

void motor(int m, int d) {
  int mf;
  int mr;
  switch (m) {
    case lmotor:
      mf = lmotorf;
      mr = lmotorr;
      break;
    case rmotor:
      mf = rmotorf;
      mr = rmotorr;
      break;
    default:
      mf = 0;
      mr = 0;
  }
  switch (d) {
    case mStop:
      digitalWrite(mf, LOW);
      digitalWrite(mr, LOW);
      break;
    case mForward:
      digitalWrite(mf, HIGH);
      digitalWrite(mr, LOW);
      break;
    case mReverse:
      digitalWrite(mf, LOW);
      digitalWrite(mr, HIGH);
      break;
    default:
      digitalWrite(mf, LOW);
      digitalWrite(mr, LOW);
  }
}

void robotStop() {
  motor(lmotor, mStop);
  motor(rmotor, mStop);
  runTime = 1000;
  robotState = mStop;
}

void robotForward() {
  motor(lmotor, mForward);
  motor(rmotor, mForward);
  runTime = 1000;
  robotState = mForward;
}

void robotReverse() {
  motor(lmotor, mReverse);
  motor(rmotor, mReverse);
  runTime = 1000;
  robotState = mReverse;
}

void robotLeft() {
  motor(lmotor, mReverse);
  motor(rmotor, mForward);
  runTime = 1000;
  robotState = mLeft;
}

void robotRight() {
  motor(lmotor, mForward);
  motor(rmotor, mReverse);
  runTime = 1000;
  robotState = mRight;
}

void robotThink() {
  if (avoidState == HIGH && scanState == false) {
    robotForward();
  }
  if (avoidState == LOW && scanState == false) {
    robotRight();
  }
  if (avoidState == LOW && scanState == true) {
    robotStop();
  }
}