Help rover program?

Hi I’m having problems in my project.
I have built a rover with 2 servo motors, 1 HCSR proximity sensor.
It moves forward until the sensor sees an obstacle, then It turns left or right.

Now I have made changes:

  1. A Led wich turns on when The HCSR finds an obstacle.
  2. Another servo motor in wich I put the HCSR, so when It finds an obstable It can move right or left to see wich direction is free.

My program works fine on the rover without modifications. I can’t make the proper changes for the moving HCSR.

This is the first program:

#include <Servo.h>

Servo MotoreSinistro;

Servo MotoreDestro;

const int periodoLoop = 20; // time of 20ms = a frequance of 50Hz

unsigned long ritardoLoop = 0;

//trig and echo pin for ultrasound sensor

const int TrigPin = 8;

const int EchoPin = 9;

int distanza; // distance noticed by the sensor

int durata; // echo duration noticed by the ultrasound sensor

int minimaDistanza = 10; // distance in wich the robot has to change direction

unsigned long tempoRotazioneSx = 1100; // 1,1 seconds (circa) to do 90° rotation


// definizione degli stati

#define VAI_AVANTI 0

#define GIRA_SX 1

int stato = VAI_AVANTI; // 0 Default

void setup(){


pinMode(TrigPin, OUTPUT);

pinMode(EchoPin, INPUT);




void loop(){


if(millis() - ritardoLoop >= periodoLoop){

letturaSensoreUltrasuoni(); //reads and memorize the measured distance


ritardoLoop = millis();



void azioneRobot(){

if(stato == VAI_AVANTI){ // no obstacles are noticed

if(distanza > minimaDistanza || distanza < 0){ //the distance can have negative numbers if there’s no obstacle in front





else{ //

stato = GIRA_SX;



else if(stato == GIRA_SX){

unsigned long tempoRotazioneSx = 1100; //VERIFY ROTATION TIME

unsigned long tempoInizioRotazione = millis(); // memorize the moment when the rotation starts

while((millis()-tempoInizioRotazione) < tempoRotazioneSx){





stato = VAI_AVANTI;



void letturaSensoreUltrasuoni(){

digitalWrite(TrigPin, HIGH);

delayMicroseconds(10); // keeps high the trigger for at least 10us

digitalWrite(TrigPin, LOW);

durata = pulseIn(EchoPin, HIGH);

distanza = (durata/2)/29;


void printOutput(){

Serial.print("distanza: ");





Wich proper program changes do I have to do to make the third servo motor work with the HCSR?

Thank You.

void loop(){


Print the distance, then measure the distance. Why do you have the cart on the wrong end of the horse?


Why do you have your head stuck in the sand?

Where do you try to move the servo that is carrying the ping sensor?

Wich horse?


I want to move the servo carrying the sensor right and left of the rover,so that It can find a free path a follow that one.

Thank You.

Wich horse?

Have you ever seen a horse pulling a cart? What happens when you try to put the cart in front of the horse, have it try to push the part? Nothing good...

Get the distance FIRST and then print the distance.

I want to move the servo carrying the sensor

I do not know which instance of the Servo class that is.

right and left of the rover

So, why don't you?

so that It can find a free path a follow that one.

It will take more than waving the servo around while reading distances from unknown directions.

You need to create an array of directions and an array of distances. Then, you use a for loop to move the servo to the position in the nth position of the direction array and record the distance in the nth position of the distance array.

Then, you loop through the distance array, finding the largest distance and its position. You use the position of the largest distance to access the direction from the direction array, and go in that direction.