next topic »

[on:]

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

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;  

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);
      }
     }

[Reply #1 on:]

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

Code:

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)

[Reply #2 on:]

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

[Reply #3 on:]

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 ;

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;



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;
   }
   
}

[Reply #4 on:]

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

Code:

#define MOVING  1
#define DETERMINE  2

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

[Reply #5 on:]

Code:

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

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

[Reply #6 on:]

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() {

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;
#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;
  }


}

[Reply #7 on:]

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

[Reply #8 on:]

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

[Reply #9 on:]

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

[Reply #10 on:]

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

[Reply #11 on:]

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

[Reply #12 on:]

Code:

#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?

[Reply #13 on:]

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;
#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;
  }


}

[Reply #14 on:]

Code:

    state = Determine;

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

Make state a variable:

Code:

int state = 0;

Code:

  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?

Code:

#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.