sensor pause

system · February 8, 2012, 7:56pm

Hi there everybody

i am trying to make a wall avoiding robot using a HC-SR04 ultrasonic sensor, a dagu channel4, a dagu rover 5 and a servo.
but is isn't going that well.

i want to let the ultrasonic sensor to continuously (or take very fast samples)meassure the distance in front of it while it is driving forward.
and when it get's below a set distance to stop the sensor reading and move the servo to the right and meassure the distance, then move to the left and meassure the distance.
and then decide which way to go.

so far i did manage to make it stop when there is a wall in front of it.
but when i try to move the servo, the servo goes all crazy, because it keeps meassuring and then it thinks the distance is more then the set distance and so moves the servo back.

this is my code so far

#include "Ultrasonic.h"
#define  TRIG_PIN  6
#define  ECHO_PIN  7
Ultrasonic OurModule(TRIG_PIN, ECHO_PIN);
#define PwmPinMotorA 53
#define PwmPinMotorB 51
#define DirectionPinMotorA 52
#define DirectionPinMotorB 50
int pos = 0;
#include <Servo.h> 
Servo myservo;  

void setup() {
Serial.begin(9600);
pinMode(PwmPinMotorA, OUTPUT);
pinMode(PwmPinMotorB, OUTPUT);
pinMode(DirectionPinMotorA, OUTPUT);
pinMode(DirectionPinMotorB, OUTPUT);
myservo.attach(9); 
}

void forward(){
digitalWrite(DirectionPinMotorA, HIGH);
analogWrite(PwmPinMotorA, 150);
digitalWrite(DirectionPinMotorB, LOW);
analogWrite(PwmPinMotorB, 150);
}

void backward(){
digitalWrite(DirectionPinMotorA, LOW);
analogWrite(PwmPinMotorA, 150);
digitalWrite(DirectionPinMotorB, HIGH);
analogWrite(PwmPinMotorB, 150);
}

void left(){
digitalWrite(DirectionPinMotorA, HIGH);
analogWrite(PwmPinMotorA, 150);
digitalWrite(DirectionPinMotorB, HIGH);
analogWrite(PwmPinMotorB, 150);
}

void right(){
digitalWrite(DirectionPinMotorA, LOW);
analogWrite(PwmPinMotorA, 255);
digitalWrite(DirectionPinMotorB, LOW);
analogWrite(PwmPinMotorB, 255);
}

void pause(){
digitalWrite(DirectionPinMotorA, LOW);
analogWrite(PwmPinMotorA, 0);
digitalWrite(DirectionPinMotorB, LOW);
analogWrite(PwmPinMotorB, 0);
}

void pos1() {
OurModule.Ranging(CM);
}
void pos2() {
OurModule.Ranging(CM);
}

void loop() {
     pos = OurModule.Ranging(CM); 
     myservo.write(90);
     if (pos <30) {
     forward();
     } 
     
     else{
     pause();
     myservo.write(10);
      }
     }

robtillaart · February 8, 2012, 9:00pm

you must make a statemachine, here the concept, not complete but you should get the idea

int state = MOVING;

void loop()
{
  switch (state)
  {
    case MOVING: 
      doMove();
      break;

    case DETERMINE: 
      doDetermine();
      break;

   default: 
      stop();  // for safety
      break;
   }
}

doMove()
{
  distance = Measure();
  if (distance < 30)
  {
    state = DETERMINE;
    stop();
  }
  else
  {
    forward();
  }
}

doDetermine()
{
  // rotate servo left
  distanceLeft = Measure();
  // rotate servo right
  distanceRight = Measure();

  if (distanceLeft <= distanceRight)
  {
    right();
    // delay(100); ???
  }
  else
  {
    left();
    // delay(100); ???
  }
  state = MOVING;
}

YOu can make the doDetermine() more intelligent, not just looking left and right but at every angle and choose the one with the most space. And of course it should move backwards if there is no left or right space.... (homework)

system · February 8, 2012, 9:27pm

thank you very much
i will go work on that, and probably upload it by suaterday

system · February 9, 2012, 5:56pm

I have tried to fit a switch case machine in my code and i came up with this so far.

but it doesn't work yet

the error i am getting is this one:
In function 'void doMove()':
error: void value not ignored as it ought to be In function 'void doDetermine()':
At global scope:
In function 'void loop()':

and an other error i am getting is that it can't find MOVING in: int state = MOVING ;

#include "Ultrasonic.h"
#define  TRIG_PIN  6
#define  ECHO_PIN  7
Ultrasonic OurModule(TRIG_PIN, ECHO_PIN);
#define PwmPinMotorA 53
#define PwmPinMotorB 51
#define DirectionPinMotorA 52
#define DirectionPinMotorB 50
int pos = 0;
#include <Servo.h> 
Servo myservo;  
int distance = 0;
int distanceLeft = 0;
int distanceRight = 0;



void setup() {
Serial.begin(9600);
pinMode(PwmPinMotorA, OUTPUT);
pinMode(PwmPinMotorB, OUTPUT);
pinMode(DirectionPinMotorA, OUTPUT);
pinMode(DirectionPinMotorB, OUTPUT);
myservo.attach(9); 
}

void forward(){
digitalWrite(DirectionPinMotorA, HIGH);
analogWrite(PwmPinMotorA, 150);
digitalWrite(DirectionPinMotorB, LOW);
analogWrite(PwmPinMotorB, 150);
}

void backward(){
digitalWrite(DirectionPinMotorA, LOW);
analogWrite(PwmPinMotorA, 150);
digitalWrite(DirectionPinMotorB, HIGH);
analogWrite(PwmPinMotorB, 150);
}

void left(){
digitalWrite(DirectionPinMotorA, HIGH);
analogWrite(PwmPinMotorA, 150);
digitalWrite(DirectionPinMotorB, HIGH);
analogWrite(PwmPinMotorB, 150);
delay(300);
digitalWrite(DirectionPinMotorA, LOW);
analogWrite(PwmPinMotorA, 0);
digitalWrite(DirectionPinMotorB, LOW);
analogWrite(PwmPinMotorB, 0);
}

void right(){
digitalWrite(DirectionPinMotorA, LOW);
analogWrite(PwmPinMotorA, 255);
digitalWrite(DirectionPinMotorB, LOW);
analogWrite(PwmPinMotorB, 255);
delay(300);
digitalWrite(DirectionPinMotorA, LOW);
analogWrite(PwmPinMotorA, 0);
digitalWrite(DirectionPinMotorB, LOW);
analogWrite(PwmPinMotorB, 0);
}

void pause(){
digitalWrite(DirectionPinMotorA, LOW);
analogWrite(PwmPinMotorA, 0);
digitalWrite(DirectionPinMotorB, LOW);
analogWrite(PwmPinMotorB, 0);
}

void Measure() {
OurModule.Ranging(CM);
}

void doMove()
{
  distance = Measure();
  if (distance < 30)
  {
    state = doDetermine;
    stop();
  }
  else
  {
    forward();
  }
}


void doDetermine()  // optie 2 het bepalen van de afstand links en rechts.
{
  myservo.write(10);
  delay(400);
  distanceLeft = Measure();
  delay(20);
  myservo.write(170);
  delay(400);
  distanceRight = Measure();
  
  if (distanceLeft <= distanceRight)
  {
    right();
  }
  else
  {
    left();
  }
  state = MOVING;
}



int state = MOVING ;

void loop() {
  
   switch (state)
  {
    case MOVING: // optie 1 rijden  
      doMove();
      break;

    case doDetermine: // optie 2 afstanden bepalen
      doDetermine();
      break;

   default:   // standaard: sta stil
      stop();  // for safety
      break;
   }
   
}

wildbill · February 9, 2012, 10:24pm

You need to define MOVING and DETERMINE (and use it) e.g.

#define MOVING  1
#define DETERMINE  2

You have tried to use doDetermine instead of DETERMINE as Robtillart had it.

system · February 10, 2012, 3:27pm

void Measure() {
OurModule.Ranging(CM);
}

Measure the distance, but don't return the distance. Now, there's a useful function!

system · February 10, 2012, 5:18pm

i noticed too that i forgot the void's before Determine etc

i changed the code again and made this of it
but now getting this error:
error: declaration does not declare anything In function 'void Move()':
At global scope:

and the this line is then highlighted: void Measure() {

#include "Ultrasonic.h"
#define  TRIG_PIN  6
#define  ECHO_PIN  7
Ultrasonic OurModule(TRIG_PIN, ECHO_PIN);
#define PwmPinMotorA 53
#define PwmPinMotorB 51
#define DirectionPinMotorA 52
#define DirectionPinMotorB 50
int pos = 0;
#include <Servo.h> 
Servo myservo;  
int distance = 0;
int distanceLeft = 0;
int distanceRight = 0;
#define MOVING;
#define Measure;
#define Determine;
#define state;

void setup() {
  Serial.begin(9600);
  pinMode(PwmPinMotorA, OUTPUT);
  pinMode(PwmPinMotorB, OUTPUT);
  pinMode(DirectionPinMotorA, OUTPUT);
  pinMode(DirectionPinMotorB, OUTPUT);
  myservo.attach(9); 
}

void forward(){
  digitalWrite(DirectionPinMotorA, HIGH);
  analogWrite(PwmPinMotorA, 150);
  digitalWrite(DirectionPinMotorB, LOW);
  analogWrite(PwmPinMotorB, 150);
}

void backward(){
  digitalWrite(DirectionPinMotorA, LOW);
  analogWrite(PwmPinMotorA, 150);
  digitalWrite(DirectionPinMotorB, HIGH);
  analogWrite(PwmPinMotorB, 150);
}

void left(){
  digitalWrite(DirectionPinMotorA, HIGH);
  analogWrite(PwmPinMotorA, 150);
  digitalWrite(DirectionPinMotorB, HIGH);
  analogWrite(PwmPinMotorB, 150);
  delay(300);
  digitalWrite(DirectionPinMotorA, LOW);
  analogWrite(PwmPinMotorA, 0);
  digitalWrite(DirectionPinMotorB, LOW);
  analogWrite(PwmPinMotorB, 0);
}

void right(){
  digitalWrite(DirectionPinMotorA, LOW);
  analogWrite(PwmPinMotorA, 255);
  digitalWrite(DirectionPinMotorB, LOW);
  analogWrite(PwmPinMotorB, 255);
  delay(300);
  digitalWrite(DirectionPinMotorA, LOW);
  analogWrite(PwmPinMotorA, 0);
  digitalWrite(DirectionPinMotorB, LOW);
  analogWrite(PwmPinMotorB, 0);
}

void pause(){
  digitalWrite(DirectionPinMotorA, LOW);
  analogWrite(PwmPinMotorA, 0);
  digitalWrite(DirectionPinMotorB, LOW);
  analogWrite(PwmPinMotorB, 0);
}



void Measure() {
  OurModule.Ranging(CM);
}


void Move()
{
  distance = Measure();
  if (distance < 30)
  {
    state = Determine;
    stop();
  }
  else
  {
    forward();
  }
}


void Determine() 
{
  myservo.write(10);
  delay(400);
  distanceLeft = Measure();
  delay(20);
  myservo.write(170);
  delay(400);
  distanceRight = Measure();

  if (distanceLeft <= distanceRight)
  {
    right();
  }
  else
  {
    left();
  }
  state = MOVING;
}




void loop() {
  int state = MOVING;


  switch (state)
  {
  case MOVING:  
    Move();
    break;

  case Determine: 
    Determine();
    break;

  default:   
    stop();  
    break;
  }


}

system · February 10, 2012, 5:34pm

Unless you really know what you're doing, don't put semicolons on #defines

system · February 10, 2012, 5:42pm

deleted the semicolens. but still getting the same error.
could my changing of the cases be wrong ?

system · February 10, 2012, 5:44pm

What is "Measure"?
A function, or an empty macro?

system · February 10, 2012, 5:50pm

with measure, it should meassure the distance with an ultrasonic sensor.
when i replace all the Measure(); for just OurModule.Ranging(CM); (i think this might just be easier)
when i compile i get:
In function 'void Move()':
error: expected primary-expression before '=' token At global scope:

with state = Determine; highlighted. removing the semicolon doesn't do anything

system · February 10, 2012, 5:54pm

state is a macro, you can't assign a value to it.
Post your code.

system · February 10, 2012, 5:55pm

#define state;

even with the semicolon defines a name, not a variable where you can store a value.

when i compile i get:
In function 'void Move()':
error: expected primary-expression before '=' token At global scope:

with state = Determine; highlighted. removing the semicolon doesn't do anything

Well, of course you do. Removing the ; did something (useful), just not what you wanted.

Why are you using #define at all?

system · February 10, 2012, 5:56pm

#include "Ultrasonic.h"
#define  TRIG_PIN  6
#define  ECHO_PIN  7
Ultrasonic OurModule(TRIG_PIN, ECHO_PIN);
#define PwmPinMotorA 53
#define PwmPinMotorB 51
#define DirectionPinMotorA 52
#define DirectionPinMotorB 50
int pos = 0;
#include <Servo.h> 
Servo myservo;  
int distance = 0;
int distanceLeft = 0;
int distanceRight = 0;
#define MOVING
#define Determine
#define state

void setup() {
  Serial.begin(9600);
  pinMode(PwmPinMotorA, OUTPUT);
  pinMode(PwmPinMotorB, OUTPUT);
  pinMode(DirectionPinMotorA, OUTPUT);
  pinMode(DirectionPinMotorB, OUTPUT);
  myservo.attach(9); 
}

void forward(){
  digitalWrite(DirectionPinMotorA, HIGH);
  analogWrite(PwmPinMotorA, 150);
  digitalWrite(DirectionPinMotorB, LOW);
  analogWrite(PwmPinMotorB, 150);
}

void backward(){
  digitalWrite(DirectionPinMotorA, LOW);
  analogWrite(PwmPinMotorA, 150);
  digitalWrite(DirectionPinMotorB, HIGH);
  analogWrite(PwmPinMotorB, 150);
}

void left(){
  digitalWrite(DirectionPinMotorA, HIGH);
  analogWrite(PwmPinMotorA, 150);
  digitalWrite(DirectionPinMotorB, HIGH);
  analogWrite(PwmPinMotorB, 150);
  delay(300);
  digitalWrite(DirectionPinMotorA, LOW);
  analogWrite(PwmPinMotorA, 0);
  digitalWrite(DirectionPinMotorB, LOW);
  analogWrite(PwmPinMotorB, 0);
}

void right(){
  digitalWrite(DirectionPinMotorA, LOW);
  analogWrite(PwmPinMotorA, 255);
  digitalWrite(DirectionPinMotorB, LOW);
  analogWrite(PwmPinMotorB, 255);
  delay(300);
  digitalWrite(DirectionPinMotorA, LOW);
  analogWrite(PwmPinMotorA, 0);
  digitalWrite(DirectionPinMotorB, LOW);
  analogWrite(PwmPinMotorB, 0);
}

void pause(){
  digitalWrite(DirectionPinMotorA, LOW);
  analogWrite(PwmPinMotorA, 0);
  digitalWrite(DirectionPinMotorB, LOW);
  analogWrite(PwmPinMotorB, 0);
}



void Move()
{
  distance =  OurModule.Ranging(CM);
  if (distance < 30)
  {
    state = Determine;
    stop();
  }
  else
  {
    forward();
  }
}


void Determine() 
{
  myservo.write(10);
  delay(400);
  distanceLeft = OurModule.Ranging(CM);
  delay(20);
  myservo.write(170);
  delay(400);
  distanceRight = OurModule.Ranging(CM);

  if (distanceLeft <= distanceRight)
  {
    right();
  }
  else
  {
    left();
  }
  state = MOVING;
}




void loop() {
  int state = MOVING;


  switch (state)
  {
  case MOVING:  
    Move();
    break;

  case Determine: 
    Determine();
    break;

  default:   
    stop();  
    break;
  }


}

system · February 10, 2012, 6:04pm

    state = Determine;

You can't assign a value to a name!!!

Make state a variable:

int state = 0;

  myservo.write(10);
  delay(400);
  distanceLeft = OurModule.Ranging(CM);
  delay(20);
  myservo.write(170);
  delay(400);
  distanceRight = OurModule.Ranging(CM);

Have you timed how long it actually takes the servo to move?

#define Determine
void Determine()

Do you want Determine to be a name or a function? Don't try to use the same name for two different things.

system · February 10, 2012, 6:43pm

thank you very much PaulS and for the very clear explaination

this is my code now

although the Determine part isn't working yet, i think i will be able to fix it.
If you see the problem quick could you please tell me

#include "Ultrasonic.h"
#define  TRIG_PIN  6
#define  ECHO_PIN  7
Ultrasonic OurModule(TRIG_PIN, ECHO_PIN);
#define PwmPinMotorA 53
#define PwmPinMotorB 51
#define DirectionPinMotorA 52
#define DirectionPinMotorB 50
int pos = 0;
#include <Servo.h> 
Servo myservo;  
int distance = 0;
int distanceLeft = 0;
int distanceRight = 0;
int state =0;

void setup() {
  Serial.begin(9600);
  pinMode(PwmPinMotorA, OUTPUT);
  pinMode(PwmPinMotorB, OUTPUT);
  pinMode(DirectionPinMotorA, OUTPUT);
  pinMode(DirectionPinMotorB, OUTPUT);
  myservo.attach(9); 
}

void forward(){
  digitalWrite(DirectionPinMotorA, HIGH);
  analogWrite(PwmPinMotorA, 150);
  digitalWrite(DirectionPinMotorB, LOW);
  analogWrite(PwmPinMotorB, 150);
}

void backward(){
  digitalWrite(DirectionPinMotorA, LOW);
  analogWrite(PwmPinMotorA, 150);
  digitalWrite(DirectionPinMotorB, HIGH);
  analogWrite(PwmPinMotorB, 150);
}

void left(){
  digitalWrite(DirectionPinMotorA, HIGH);
  analogWrite(PwmPinMotorA, 150);
  digitalWrite(DirectionPinMotorB, HIGH);
  analogWrite(PwmPinMotorB, 150);
  delay(300);
  digitalWrite(DirectionPinMotorA, LOW);
  analogWrite(PwmPinMotorA, 0);
  digitalWrite(DirectionPinMotorB, LOW);
  analogWrite(PwmPinMotorB, 0);
}

void right(){
  digitalWrite(DirectionPinMotorA, LOW);
  analogWrite(PwmPinMotorA, 255);
  digitalWrite(DirectionPinMotorB, LOW);
  analogWrite(PwmPinMotorB, 255);
  delay(300);
  digitalWrite(DirectionPinMotorA, LOW);
  analogWrite(PwmPinMotorA, 0);
  digitalWrite(DirectionPinMotorB, LOW);
  analogWrite(PwmPinMotorB, 0);
}

void pause(){
  digitalWrite(DirectionPinMotorA, LOW);
  analogWrite(PwmPinMotorA, 0);
  digitalWrite(DirectionPinMotorB, LOW);
  analogWrite(PwmPinMotorB, 0);
}



void Move()
{
  distance =  OurModule.Ranging(CM);
  if (distance < 30)
  {
    state = 2;
    pause();
  }
  else
  {
    forward();
  }
}


void Determine() 
{ 
  
  myservo.write(10);
  delay(400);
  distanceLeft = OurModule.Ranging(CM);
  delay(20);
  myservo.write(170);
  delay(400);
  distanceRight = OurModule.Ranging(CM);

  if (distanceLeft <= distanceRight)
  {
    right();
  }
  else
  {
    left();
  }
  state = 1;
}




void loop() {
  int state = 1;


  switch (state)
  {
  case 1: // 1 is moving 
    Move();
    break;

  case 2: // 2 is Determine
    Determine();
    break;

  default:   
  pause();
    break;
  }


}

system · February 10, 2012, 6:54pm

I don't think you ever run Determine(), unless I missed something:

  int state = 1;


  switch (state)
  {
  case 1: // 1 is moving 
    Move();
    break;

  case 2: // 2 is Determine
    Determine();
    break;

  default:   
  pause();
    break;
  }

Set state to 1, then switch on state, so it's never going to be 2, then repeat.

system · February 10, 2012, 7:04pm

i see what you mean.
when i make it int state = 2;
it will only turn right or left.

what would be a good way to solve this ?

system · February 10, 2012, 7:11pm

You have two versions of state. One is global and gets updated in Move() and Determine(), the other is local to loop(), and only gets updated there. See what happens when you get rid of the 'int state = 1;' from the start of loop(), and initialise the global state to 1.

system · February 11, 2012, 12:58pm

thank you very much
it finally works
thanks every body for helping me out
this is my final code:

#include "Ultrasonic.h" 
#define  TRIG_PIN  6    
#define  ECHO_PIN  7   
Ultrasonic OurModule(TRIG_PIN, ECHO_PIN); 
#define PwmPinMotorA 53
#define PwmPinMotorB 51
#define DirectionPinMotorA 52
#define DirectionPinMotorB 50
int pos = 0;
#include <Servo.h> 
Servo myservo;  
int distance = 0;
int distanceLeft = 0;
int distanceRight = 0;
int state = 1;

void setup() {
  Serial.begin(9600);
  pinMode(PwmPinMotorA, OUTPUT);
  pinMode(PwmPinMotorB, OUTPUT);
  pinMode(DirectionPinMotorA, OUTPUT);
  pinMode(DirectionPinMotorB, OUTPUT);
  myservo.attach(9); 
}

void backword(){
  digitalWrite(DirectionPinMotorA, HIGH);
  analogWrite(PwmPinMotorA, 150);
  digitalWrite(DirectionPinMotorB, LOW);
  analogWrite(PwmPinMotorB, 150);
}

void forward(){
  digitalWrite(DirectionPinMotorA, LOW);
  analogWrite(PwmPinMotorA, 150);
  digitalWrite(DirectionPinMotorB, HIGH);
  analogWrite(PwmPinMotorB, 150);
}

void left(){
  digitalWrite(DirectionPinMotorA, HIGH);
  analogWrite(PwmPinMotorA, 150);
  digitalWrite(DirectionPinMotorB, HIGH);
  analogWrite(PwmPinMotorB, 150);
  delay(300);
  digitalWrite(DirectionPinMotorA, LOW);
  analogWrite(PwmPinMotorA, 0);
  digitalWrite(DirectionPinMotorB, LOW);
  analogWrite(PwmPinMotorB, 0);
}

void right(){
  digitalWrite(DirectionPinMotorA, LOW);
  analogWrite(PwmPinMotorA, 255);
  digitalWrite(DirectionPinMotorB, LOW);
  analogWrite(PwmPinMotorB, 255);
  delay(300);
  digitalWrite(DirectionPinMotorA, LOW);
  analogWrite(PwmPinMotorA, 0);
  digitalWrite(DirectionPinMotorB, LOW);
  analogWrite(PwmPinMotorB, 0);
}

void pause(){
  digitalWrite(DirectionPinMotorA, LOW);
  analogWrite(PwmPinMotorA, 0);
  digitalWrite(DirectionPinMotorB, LOW);
  analogWrite(PwmPinMotorB, 0);
}



void Move()
{
  distance =  OurModule.Ranging(CM);
  if (distance < 30)
  {
    state = 2;
    pause();

  }
  else
  {
    forward();
  }
}


void Determine() 
{ 
  myservo.write(10);
  delay(400);
  distanceLeft = OurModule.Ranging(CM);
  delay(20);
  myservo.write(170);
  delay(400);
  distanceRight = OurModule.Ranging(CM);
  delay(20);
  myservo.write(90);

  if (distanceLeft <= distanceRight)
  {
    right();
  }
  else
  {
    left();
  }
  state = 1;
}




void loop() {


  switch (state)
  {
  case 1: // 1 is moving 
    Move();
    break;

  case 2: // 2 is Determine
    
    Determine();
    break;

  default:   
  pause();
    break;
  }


}

Topic		Replies	Views
Servo Driven Vehicle with Ultrasonic Sensor Programming	9	1209	May 5, 2021
Ultrasonic sensor mounted on a servo Programming	3	3199	May 5, 2021
Ultrasonic sensor and Servo Programming	3	977	May 5, 2021
Stopping my servo sweep when ultrasonic sensor distance is met Programming	3	1054	May 5, 2021
Car chasis with HC-SR04 sensor Programming	25	6079	May 5, 2021

sensor pause

Related topics