SQL Hacking

SQL Hacking

Obstacle avoiding Robot Platform

This is a code for Obstacle Avoiding Robot Platform 

#include <AFMotor.h>
#define trigPin 3
#define echoPin 2
AF_DCMotor motor1(1, MOTOR12_64KHZ); 
AF_DCMotor motor2(4, MOTOR12_64KHZ); 

void setup() {
  Serial.begin (9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  motor1.setSpeed(120); 
  motor2.setSpeed(130); 
}

int CheckDistance()
{
  long duration, distance;
  digitalWrite(trigPin, LOW);  
  delayMicroseconds(2); 

  digitalWrite(trigPin, HIGH);
  delayMicroseconds(2); 
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = (duration/2) / 29.1;
  
  return distance;
}

void MotorForward(int delaytime)
{
  motor1.run(FORWARD);
  motor2.run(FORWARD);
  delay(delaytime);
}
void MotorBackward(int delaytime)
{
  motor1.run(BACKWARD);
  motor2.run(BACKWARD);
  delay(delaytime);
}
void MotorRelease()
{
  motor1.run(RELEASE);
  motor2.run(RELEASE);
  delay(100);
}

void MotorLeft()
{
  motor1.run(FORWARD);
  motor2.run(BACKWARD);
  delay(400);
}

void MotorRight()
{
  motor1.run(BACKWARD);
  motor2.run(FORWARD);
  delay(400);
}

void loop() 
{
  int testDistance = CheckDistance();
  
   Serial.print(testDistance);
   Serial.println(" test");
   
  if (testDistance >= 30 || testDistance <= 0){
    Serial.println("Out of range");
    
    MotorForward(300);
    MotorRelease();
  }
  else 
  {
    Serial.print(testDistance);
    Serial.println(" cm");
    
    MotorBackward(100);
    MotorRelease();
    MotorRight();
    MotorRelease();
    
   
  }
   
  
}