Human Detection Robotics System Using Arduino Uno © CC BY-NC

#include <NewPing.h> // 
#include <AFMotor.h> // You can download the code library below
#include <Servo.h> //
 
// Ultranic Pin Configuration
 
#define TRIG_PIN A0
#define ECHO_PIN A1
 
#define MAX_DISTANCE 400
#define MAX_SPEED 255
#define MAX_SPEED_OFFSET -8
 
#define COLL_DIST 20
#define TURN_DIST COLL_DIST+10
#define ACT_TIME 250
 int calibrationTime = 30;        

//the time when the sensor outputs a low impulse
long unsigned int lowIn;         

//the amount of milliseconds the sensor has to be low 
//before we assume all motion has stopped
long unsigned int pause = 5000;  

boolean lockLow = true;
boolean takeLowTime;  

int pirPin = A3;    //the digital pin connected to the PIR sensor's output
int ledPin = A2;
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE); 
 
AF_DCMotor motorR(1, MOTOR12_1KHZ); // Set motor #1, 1kHz PWM
AF_DCMotor motorL(4, MOTOR12_1KHZ); // Set motor #2, 1kHz PWM
 
Servo myservo;  // Set servo object to control a servo 
String motorSet = "";
 
int curDist = 0, pos, speedSet = 0;
//int pos;
//int speedSet = 0;
 
void setup() {
  Serial.begin(9600);
  pinMode(pirPin, INPUT);
  pinMode(ledPin, OUTPUT);
  digitalWrite(pirPin, LOW);

  //give the sensor some time to calibrate
  Serial.print("calibrating sensor ");
    for(int i = 0; i < calibrationTime; i++){
      Serial.print(".");
      delay(1000);
      }
    Serial.println(" done");
    Serial.println("SENSOR ACTIVE");
    delay(50);
  myservo.attach(9);  // Set to attach the servo on pin 9 
  myservo.write(90);  // Write 90 to face servo forward
  delay(2000);
 
  motorSet = "FORWARD";
  moveForward();
  
}
 
void loop() {
  
  checkPath();
  if(digitalRead(pirPin) == HIGH){
       digitalWrite(ledPin, HIGH);   //the led visualizes the sensors output pin state
       if(lockLow){  
         //makes sure we wait for a transition to LOW before any further output is made:
         lockLow = false;            
         Serial.println("---");
         Serial.print("motion detected at ");
         Serial.print(millis()/1000);
         Serial.println(" sec"); 
         delay(50);
         }         
         takeLowTime = true;
       }

     if(digitalRead(pirPin) == LOW){       
       digitalWrite(ledPin, LOW);  //the led visualizes the sensors output pin state

       if(takeLowTime){
        lowIn = millis();          //save the time of the transition from high to LOW
        takeLowTime = false;       //make sure this is only done at the start of a LOW phase
        }
       //if the sensor is low for more than the given pause, 
       //we assume that no more motion is going to happen
       if(!lockLow && millis() - lowIn > pause){  
           //makes sure this block of code is only executed again after 
           //a new motion sequence has been detected
           lockLow = true;                        
           Serial.print("motion ended at ");      //output
           Serial.print((millis() - pause)/1000);
           Serial.println(" sec");
           delay(50);
           }
       }
  
}
 
void checkPath() {
  
  int curLeft = 0; int curRight = 0; int curFront = 0;
  curDist = 0;
  
  checkForward();
  myservo.write(135);
  delay(100);
  for (pos = 135; pos >= 45; pos -= 45) {
    myservo.write(pos);
    delay(170);
    curDist = readPing();
    
    if (curDist < COLL_DIST) { checkCourse(); break; }
    if (curDist < TURN_DIST) { changePath(); } 
  
  }    
}  
 
int readPing() {
  int cm = 0;
  while (cm < 2) {int uS = sonar.ping(); cm = uS/US_ROUNDTRIP_CM;}
  return cm;
}
 
void checkForward() { 
  if (motorSet=="FORWARD") { motorR.run(FORWARD); motorL.run(FORWARD); } 
}    
void changePath() {
 
  if (pos < 90) { veerLeft(); } 
  if (pos > 90) { veerRight(); }
  
}
 
void veerRight() {
  motorR.run(BACKWARD);  motorL.run(FORWARD); 
  delay(ACT_TIME); 
  motorR.run(FORWARD);   motorL.run(FORWARD);
  motorSet = "FORWARD";
}
 
void veerLeft() {
  motorL.run(BACKWARD);  motorR.run(FORWARD); 
  delay(ACT_TIME); 
  motorL.run(FORWARD);   motorR.run(FORWARD);
  motorSet = "FORWARD";
}
 
void checkCourse() {
  moveBackward();
  delay(ACT_TIME);
  moveStop();
  setCourse();
}
 
void setCourse() {
  if (pos < 90) { turnRight(); } 
  if (pos > 90) { turnLeft(); }
}
 
void moveBackward() {
  motorSet = "BACKWARD";
  
  motorR.run(BACKWARD); // Turn right motor backward    
  motorL.run(BACKWARD);  // Turn left motor backward
  
  for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2)
  {
    motorL.setSpeed(speedSet);
    motorR.setSpeed(speedSet+MAX_SPEED_OFFSET);
    delay(5);
  }
}  
 
void moveForward() {
  motorSet = "FORWARD";
  checkForward();
  for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2)  {
    motorL.setSpeed(speedSet);
    motorR.setSpeed(speedSet+MAX_SPEED_OFFSET);
    delay(4);
  }
}
 
void moveStop() { motorR.run(RELEASE); motorL.run(RELEASE); }
 
void turnRight() {
  motorSet = "RIGHT";
  motorR.run(FORWARD);      // Turn right motor forward
  motorL.run(BACKWARD);     // Turn left motor backward
  delay(ACT_TIME);
  motorSet = "FORWARD";
  checkForward();
}  
 
void turnLeft() {
  motorSet = "LEFT";
  motorR.run(BACKWARD);     // Turn right motor backward
  motorL.run(FORWARD);      // Turn left motor forward
  delay(ACT_TIME);
  motorSet = "FORWARD";
  checkForward();
}