The Sentry Bot Code

Today I started to write the code for my robot. First I did just a simple move forward --> avoid obstacle --> move forward again type of thing. Next thing is to find out if it works :) And if it works, it's easy to move on to next phases.

#include <Servo.h>

Servo us_servo;
Servo vas;
Servo oik;

int temp = 0;
// Pins for ultra URM37 V3.2
int ultraData = 11;
int ultraTrigger = 10;
int ultraEnable = 9;
int ultraPower = 12;

int readUltra(){
  int ultraValue = 0;
  int timecount = 0;
  int val = 0;

  // Wake up and get ready for sensing
   pinMode(ultraPower, OUTPUT);
  digitalWrite(ultraPower, HIGH);  // Set to HIGH to provide 5V power
  pinMode(ultraEnable, OUTPUT);
  digitalWrite(ultraEnable, HIGH); // Set Enable Pin to HIGH
  pinMode(ultraTrigger, OUTPUT);   // Switch signalpin to output
  pinMode(ultraData,INPUT);
  delay(200); //Give sensor some time to start up --Added By crystal  from Singapo, Thanks Crystal.


  /* Send high-low-high pulse to activate the trigger pulse of the sensor
  * -------------------------------------------------------------------
  */
  digitalWrite(ultraTrigger, HIGH); // Send High pulse
  delayMicroseconds(500);
  digitalWrite(ultraTrigger, LOW); // Send low pulse
  delayMicroseconds(200);
  digitalWrite(ultraTrigger, HIGH); // Send High pulse
  delayMicroseconds(200);


  /* Listening for echo pulse
  * -------------------------------------------------------------------
  */
  // Loop until pin reads a high value
  do{
       val = digitalRead(ultraData);
       //Serial.println("BBB");
     }while(val == HIGH);
  // Loop until pin reads a high value
  do{
     val = digitalRead(ultraData);
     timecount ++;            // Count echo pulse time
     delayMicroseconds(50);
     }while(val == LOW);
    ultraValue = timecount; // Append echo pulse time to ultraValue

// Turn the lights out when you leave...
  digitalWrite(ultraTrigger,LOW);
  digitalWrite(ultraEnable,LOW);
  digitalWrite(ultraPower,LOW);

  return ultraValue;
}

void setup(){

  Serial.begin(9600);
   pinMode(ledPin, OUTPUT);
   pinMode(ledPin2, OUTPUT);
   vas.attach(7); // left motor to pin 7
   oik.attach(8); // right motor to pin 8
   us_servo.attach(9) // Ultrasonic sweeper servo to pin 9
}
void loop(){
  int pos = 0;
 
  vas.write(110); // slowly forward
  oik.write(110);
 
   for(pos = 50; pos < 130; pos += 1){
     us_servo.write(pos);
     temp = readUltra();
     if (temp < 30)
       break;
     delay(10);                       // waits 10ms for the servo to reach the position
   }
   for(pos = 130; pos>=51; pos-=1){                               
      us_servo.write(pos);              // tell servo to go to position in variable 'pos'
      temp = readUltra();
      if (temp < 30)
        break;
      delay(10);                       // waits 10ms for the servo to reach the position
    }
  if (temp < 30){
    vas.write(90); // stop movement
    oik.write(90);
    for(int i=0;i<10;i++){ // wait 500ms and check if the obstacle is still there
      temp = readUltra();
      delay(50);
    }
  }
  do {
    temp = readUltra();
    vas.write(80); // turn left
    oik.write(100);
    delay(10);
  }
  while (temp < 50) // thereshold to move forward again
  Serial.println(temp); // for debugging
}