Servo and Ultrasonic HC-SR04 Problem

Projects Programming
1

Hi everyone, I have just began to working Arduino. I want to built a robot that has servo with HC-SR04.(like a sonar) Road of robot was blocked. Servo turn then HC-SR04 measuring distance. Finally the robot goes to unblocked way. But servo turn 4 directions all time that is ultrasonic sensor not measuring distance. If i use ir sensor instead of HC-SR04 the robot working perfectly. Can't I use servo and HC-SR04 together.Thx.

Here is the code:

#include <Servo.h> 
int echoPin = 13; 
int trigPin = 12; 
int motor1Pin1 = 3;    
int motor1Pin2 = 4;    
int motor2Pin1 = 5;    
int motor2Pin2 = 6;    
int enablePin1 = 9;    
int enablePin2 = 10; 
int horn = 7;
int sw=8;
Servo myservo;
int pos = 0;


void setup()
{
  Serial.begin (9600); 
  pinMode(trigPin, OUTPUT); 
  pinMode(echoPin, INPUT); 
  pinMode(motor1Pin1, OUTPUT);
  pinMode(motor1Pin2, OUTPUT);
  pinMode(enablePin1, OUTPUT);
  pinMode(motor2Pin1, OUTPUT);
  pinMode(motor2Pin2, OUTPUT);
  pinMode(enablePin2, OUTPUT);
  pinMode(horn, OUTPUT);
  pinMode(sw, INPUT);
  myservo.attach(11);
}
void loop(){
start:  int duration, cm; 
  digitalWrite(trigPin, LOW); 
  delayMicroseconds(2); 
  digitalWrite(trigPin, HIGH); 
  delayMicroseconds(10); 
  digitalWrite(trigPin, LOW); 
  duration = pulseIn(echoPin, HIGH); 
  cm = duration / 29 / 2;


if (cm>=10) goto fw;

else goto search;

search:
     digitalWrite(enablePin1, LOW);
     digitalWrite(enablePin2, LOW);
     digitalWrite(horn,HIGH);
     goto turn45;
     return;
fw:
     digitalWrite(horn,LOW); 
     digitalWrite(enablePin1, HIGH);
     digitalWrite(enablePin2, HIGH);
     digitalWrite(motor1Pin1, HIGH);   
     digitalWrite(motor1Pin2, LOW);  
     digitalWrite(motor2Pin1, HIGH);   
     digitalWrite(motor2Pin2, LOW);   
     return; 
     
turn45:
      myservo.write(45);              
      delay(1000);
      if (cm>=10) goto rh45;
      else goto turn0;
      return;
turn0:
      myservo.write(0);              
      delay(1000);
      if (cm>=10) goto rh0;
      else goto turn135;
      return;
turn135:
      myservo.write(135);              
      delay(1000);
      if (cm>=10) goto lh135;
      else goto turn180;
      return;
turn180:
      myservo.write(180);              
      delay(1000);
      if (cm>=10) goto lh180;
      else goto search;
      return;

lh135:
     digitalWrite(enablePin1, HIGH);
     digitalWrite(enablePin2, HIGH);
     digitalWrite(motor1Pin1, HIGH);   
     digitalWrite(motor1Pin2, LOW);  
     digitalWrite(motor2Pin1, LOW);   
     digitalWrite(motor2Pin2, HIGH);
     delay(250);
     myservo.write(90);              
     delay(500);
     goto start;
     return; 
lh180:
     digitalWrite(enablePin1, HIGH);
     digitalWrite(enablePin2, HIGH);
     digitalWrite(motor1Pin1, HIGH);   
     digitalWrite(motor1Pin2, LOW);  
     digitalWrite(motor2Pin1, LOW);   
     digitalWrite(motor2Pin2, HIGH);
     delay(500);
     myservo.write(90);              
     delay(500);
     goto start;
      
rh45:
     digitalWrite(enablePin1, HIGH);
     digitalWrite(enablePin2, HIGH);
     digitalWrite(motor1Pin1, LOW);   
     digitalWrite(motor1Pin2, HIGH);  
     digitalWrite(motor2Pin1, HIGH);   
     digitalWrite(motor2Pin2, LOW);
     delay(250);
     myservo.write(90);              
     delay(500);
     goto start;
     return; 
rh0:
     digitalWrite(enablePin1, HIGH);
     digitalWrite(enablePin2, HIGH);
     digitalWrite(motor1Pin1, LOW);   
     digitalWrite(motor1Pin2, HIGH);  
     digitalWrite(motor2Pin1, HIGH);   
     digitalWrite(motor2Pin2, LOW);
     delay(500);
     myservo.write(90);              
     delay(500);
     goto start;
     return; 
}

Moderator edit: added code tags.

2

Get rid of those gotos and labels.

Code after an unconditional goto is not executed. Putting code there makes you look silly. Like all those return statements.

Use function calls, instead.

The logic of your program is hard to follow, jumping all over the place like that.