Robot Project, first steps with 180° servo and integrate Ultra Sonic sensor

Hello to everyone,

i use a arduino Mega 2560, Tower Pro MG995(180° servo) and a SDM-I0 UART Sonic Sensor(4pin)

My idea is to build a robot who measure permanently the distance with the Ultra Sonic sensor. If the robot drives to an object he should stop and lookaround with the servo motor. The sensor of the robot should look first left save the sensor data in int= leftdist and after that turn to the right side to safe the distance there.

After that all he should compare with the distances and decide where is more space to drive.

Everything is working when i test it alone, but when i combine the two functions, the serial monitor gives me only the first measurement of the distance in the front, nothing else…

I hope you may help me, i sit on this projects now for weeks and i have no idea whats wrong.
Thank you a lot :slight_smile:

Robot_Project.ino (4.42 KB)

  for(pos = 90; pos < 180; pos += 1)  // goes from 0 degrees to 180 degrees 
  {                                  // in steps of 1 degree 
    myservo.write(pos);              // tell servo to go to position in variable 'pos' 
    delay(40);                      // waits 15ms for the servo to reach the position   
  }

Why do you want the servo to move slowly to the new position? When you are walking along, scanning for obstacles, do you move your eyes slowly, while closed? Or do you move your eyes quickly?

//*****Ultrasonic Sensor****
 unsigned char pin = 0;
  unsigned int time_flag = 0;
  
  pinMode(TrigPin, OUTPUT); //pin is output
  pinMode(EchoPin, INPUT); // pin is now input
  digitalWrite(TrigPin, HIGH);
  delayMicroseconds(2);
  digitalWrite(TrigPin, LOW);
  delayMicroseconds(10);
  digitalWrite(TrigPin, HIGH);

  TCCR1A = 0x00;
  TCNT1 = 0x0000;
  TCCR1B = 0x01;
  pin = digitalRead(EchoPin);
  while(pin)
  {
    pin = digitalRead(EchoPin);
    time_flag++;
    if(time_flag > TIMEOUT_OVERFLOW)
      break;
  }
  TCCR1B = 0x00;

This block of code occurs three times in your sketch. Why? Create a function that measures a distance, and call that function instead of copying and pasting the code.

  delay(100);


  delay(3000);

Your look() function gets two values and takes nearly 7 seconds. Your robot is never going to win any speed contests. There is no excuse for the delay()s.

Everything is working when i test it alone, but when i combine the two functions

What two functions?

Thank you for you reply.

Why do you want the servo to move slowly to the new position? When you are walking along, scanning for obstacles, do you move your eyes slowly, while closed? Or do you move your eyes quickly?

Yes I know first i tried to write the position to the servo like 0, 180 or 90°. But i had a lot of problems with that to call the functions lookright() lookleft() in a row and the servo goes only to the left side, or when i call first the lookright() and then lookleft() function he looked only to the left. And the servo had big problems to find the positon and need a lot of time to calibrate. So I choose the easy way and it works. Time is not so important, important is that it works.

This block of code occurs three times in your sketch. Why? Create a function that measures a distance, and call that function instead of copying and pasting the code.

Yes i know i can make a function for that but first i want to make it work so i tried everything.

Your look() function gets two values and takes nearly 7 seconds. Your robot is never going to win any speed contests. There is no excuse for the delay()s.

This was only to check on which point the programm is. Sorry I am a beginner with that all and i sit now weeks on it and try to play with it, its not the final code :)

Everything is working when i test it alone, but when i combine the two functions. What two functions?

I mean when i test only to measure the distance with the sensor. Or i only let the servo motor turning around.

Thanks for help

I tried to create a function for saving the sensor measurements but i have no idea how i can save them in other variables like leftdistance or rightdistance. How can i manage the parameter when i call the function.

It would be nice if you can explain me how i can do this and show me how it looks like for my next functions with different parameters.

Thank you.

void measure_distance() {

unsigned char pin = 0; unsigned int time_flag = 0;

pinMode(TrigPin, OUTPUT); //pin is output pinMode(EchoPin, INPUT); // pin is now input digitalWrite(TrigPin, HIGH); delayMicroseconds(2); digitalWrite(TrigPin, LOW); delayMicroseconds(10); digitalWrite(TrigPin, HIGH);

// wait for a LOW pulse //ultrasoundDuration = pulseIn(EchoPin, HIGH);// will not work for HIGH pulse

TCCR1A = 0x00; TCNT1 = 0x0000; TCCR1B = 0x01; pin = digitalRead(EchoPin); while(pin) { pin = digitalRead(EchoPin); time_flag++; if(time_flag > TIMEOUT_OVERFLOW) break; } TCCR1B = 0x00; ultrasoundDuration = TCNT1; ultrasoundDuration = ultrasoundDuration / 16; // get results Serial.print(ultrasoundDuration); Serial.print(" us, "); //Serial.print(ultrasoundDuration/148, DEC); //divide with 148 and you get inches //Serial.print(" inches, "); Serial.print(ultrasoundDuration*0.017, DEC); //divide with 58 and you get cm Serial.print(" cm"); Serial.println(); delay(100); } }

How can i manage the parameter when i call the function.

Think of how functions like digitalRead manage to do this.

I hope now you all liked my code writing more :slight_smile:
I create the functions and it works.
Only the function look() when i try to return two values I am not sure if its right.
Leftdist and rightdist are not printed in the serial monitor. Can you please say me why and what I have to change?

After the look() function the serial monitor is not printing anymore. I have to restart it always. Why? And how i can fix that?

Is there a function to read some values from the serial monitor directly?

Thank you a lot.

Robot_Project.ino (2.78 KB)

#include <Servo.h>


int servopin = 51;
int sensorpin = 0;                 // analog pin used to connect the sharp sensor
int dist = 0;                 // variable to store the values from sensor(initially zero)

int leftdist;
int rightdist;


#define TIMEOUT_OVERFLOW 1000

int TrigPin = 2;
int EchoPin = 4;
unsigned long ultrasoundDuration;
unsigned long  distance_sensor;

Servo myservo;

int pos = 0;    // variable to store the servo position


void setup()
{

  myservo.attach(servopin);

  Serial.begin(9600);               // starts the serial monitor

  //myservo.attach(servopin,1400,2400 );

}


void loop()
{

  getDistance_sensor();
  dist=distance_sensor;

  Serial.print("Distance is = ");
  Serial.print( dist , DEC);
  Serial.print(" cm");
  Serial.println();
  delay(100);

  if(dist< 10) {

    look(rightdist, leftdist);


    Serial.print("left Distance is = ");
    Serial.print( leftdist , DEC);
    Serial.print(" cm");
    Serial.println();
    delay(1000);

    Serial.print("right Distance is = ");
    Serial.print( rightdist , DEC);
    Serial.print(" cm");
    Serial.println();
    delay(1000);


  } 

}




void look(int &rightlook, int &leftlook) {
  //look left
  for(pos = 90; pos < 180; pos += 1)  // goes from 0 degrees to 180 degrees 
  {                                  // in steps of 1 degree 
    myservo.write(pos);              // tell servo to go to position in variable 'pos' 
    delay(40);                      // waits 40ms for the servo to reach the position   
  }

  getDistance_sensor();
  leftlook=distance_sensor;

  delay(2000);

  //look riht
  for(pos=180; pos>=1; pos-=1)     // goes from 180 degrees to 0 degrees
  {
    myservo.write(pos);              // tell servo to go to position in variable 'pos' 
    delay(40);                     // waits 40ms for the servo to reach the position 
  }

  getDistance_sensor();
  rightlook=distance_sensor;


  delay(2000);

  //look forward
  for(pos = 1; pos < 90; pos +=1)
  {
    myservo.write(pos);
    delay(40);
  }


}




int getDistance_sensor(){

  unsigned char pin = 0;
  unsigned int time_flag = 0;

  pinMode(TrigPin, OUTPUT); //pin is output
  pinMode(EchoPin, INPUT); // pin is now input
  digitalWrite(TrigPin, HIGH);
  delayMicroseconds(2);
  digitalWrite(TrigPin, LOW);
  delayMicroseconds(10);
  digitalWrite(TrigPin, HIGH);

  // wait for a LOW pulse
  //ultrasoundDuration = pulseIn(EchoPin, HIGH);// will not work for HIGH pulse

  TCCR1A = 0x00;
  TCNT1 = 0x0000;
  TCCR1B = 0x01;
  pin = digitalRead(EchoPin);
  while(pin)
  {
    pin = digitalRead(EchoPin);
    time_flag++;
    if(time_flag > TIMEOUT_OVERFLOW)
      break;
  }
  TCCR1B = 0x00;
  ultrasoundDuration = TCNT1;
  ultrasoundDuration = ultrasoundDuration / 16;
  // get results

  distance_sensor= ultrasoundDuration*0.017;

  return distance_sensor;
}