Sunday, December 30, 2012

Arduino Obstacle avoidance with analog motors

I seem to have this working for the most part. I need to do some fine tuning, but should have it running pretty effectively shortly.





Da Code

const int pingPin = 7; // pin for ping input

const int dangerThresh = 10; //threshold for obstacles (in cm)
int leftDistance, rightDistance; //distances on either side
long duration; //time it takes to recieve PING))) signal


void setup() {
   // initialize serial communication:
  Serial.begin(9600);
 
   //Setup Channel A
  pinMode(12, OUTPUT); //Initiates Motor Channel A pin
  pinMode(9, OUTPUT); //Initiates Brake Channel A pin

  //Setup Channel B
  pinMode(13, OUTPUT); //Initiates Motor Channel B pin
  pinMode(8, OUTPUT);  //Initiates Brake Channel B pin
}

void loop()
{
  int distanceFwd = ping(); //set distance ahead to ping distance
  if (distanceFwd > dangerThresh) //if path is clear
  {
   //MOVE FORWARD
   //Motor A forward
  digitalWrite(12, HIGH); //Establishes forward direction of Channel A
  digitalWrite(9, LOW);   //Disengage the Brake for Channel A
  analogWrite(3, 255);   //Spins the motor on Channel A
 
    //Motor B Forward
  digitalWrite(13, HIGH);  //Establishes backward direction of Channel B
  digitalWrite(8, LOW);   //Disengage the Brake for Channel B
  analogWrite(11, 255);    //Spins the motor on Channel B
  }
  else //if path is blocked
  {
   digitalWrite(9, HIGH);  //Engage the Brake for Channel A
   digitalWrite(8, HIGH);  //Engage the Brake for Channel B
   delay(1000);
   //MOVE BACK TURNING FIRST WAY
   //Motor A Backwards
   digitalWrite(12, LOW); //Establishes backward direction of Channel A
   digitalWrite(9, LOW);   //Disengage the Brake for Channel A
   analogWrite(3, 175);   //Spins the motor on Channel A
   delay(1000);
   rightDistance = ping(); //scan to the right
   delay(1000);
   digitalWrite(9, HIGH);  //Engage the Brake for Channel A
   digitalWrite(8, HIGH);  //Engage the Brake for Channel B
   delay(1000);
   //return to center
   //Motor B backwards
   digitalWrite(13, LOW);  //Establishes backward direction of Channel B
   digitalWrite(8, LOW);   //Disengage the Brake for Channel B
   analogWrite(11, 175);    //Spins the motor on Channel B
   delay(1000);
 
   digitalWrite(9, HIGH);  //Engage the Brake for Channel A
   digitalWrite(8, HIGH);  //Engage the Brake for Channel B
   delay(1000);
 
   //MOVE BACK TURNING SECOND WAY
   //Motor B Backwards
   digitalWrite(13, LOW);  //Establishes backward direction of Channel B
   digitalWrite(8, LOW);   //Disengage the Brake for Channel B
   analogWrite(11, 175);    //Spins the motor on Channel B
   delay(1000);
 
   digitalWrite(9, HIGH);  //Engage the Brake for Channel A
   digitalWrite(8, HIGH);  //Engage the Brake for Channel B
   delay(1000);
 
   leftDistance = ping(); //scan to the left
   delay(1000);
   //RETURN TO CENTER
   //Motor A forward
   digitalWrite(12, LOW); //Establishes backward direction of Channel A
   digitalWrite(9, LOW);   //Disengage the Brake for Channel A
   analogWrite(3, 175);   //Spins the motor on Channel A
   delay(1000);
   compareDistance();
  }
}

  void compareDistance()
 {
   if (leftDistance>rightDistance) //if left is less obstructed
  {
   digitalWrite(9, HIGH);  //Engage the Brake for Channel A
   digitalWrite(8, HIGH);  //Engage the Brake for Channel B
   delay(1000);
  //Motor A backwards
    digitalWrite(12, LOW); //Establishes backward direction of Channel A
    digitalWrite(9, LOW);   //Disengage the Brake for Channel A
    analogWrite(3, 100);   //Spins the motor on Channel A
    delay(2000);
  }
   else if (rightDistance>leftDistance) //if right is less obstructed
  {
   digitalWrite(9, HIGH);  //Engage the Brake for Channel A
   digitalWrite(8, HIGH);  //Engage the Brake for Channel B
   delay(1000);
    //Motor B backwards
    digitalWrite(13, LOW);  //Establishes backward direction of Channel B
    digitalWrite(8, LOW);   //Disengage the Brake for Channel B
    analogWrite(11, 100);    //Spins the motor on Channel B
    delay(2000);
  }
   else //if they are equally obstructed
  {
   digitalWrite(9, HIGH);  //Engage the Brake for Channel A
   digitalWrite(8, HIGH);  //Engage the Brake for Channel B
   delay(1000);
   //Motor A forward
   digitalWrite(12, LOW); //Establishes backward direction of Channel A
   digitalWrite(9, LOW);   //Disengage the Brake for Channel A
   analogWrite(3, 100);   //Spins the motor on Channel A
   //Motor B Forward
   digitalWrite(13, HIGH);  //Establishes backward direction of Channel B
   digitalWrite(8, LOW);   //Disengage the Brake for Channel B
   analogWrite(11, 100);    //Spins the motor on Channel B
   delay(2000);
  }
 }
 // establish variables for duration of the ping,
 // and the distance result in inches and centimeters:
long ping()
{
  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);

  // The same pin is used to read the signal from the PING))): a HIGH
  // pulse whose duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);

  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return duration / 29 / 2;


No comments:

Post a Comment