Pages: [1]   Go Down
Author Topic: 2 distance sensors  (Read 643 times)
0 Members and 1 Guest are viewing this topic.
Netherlands
Offline Offline
Jr. Member
**
Karma: 0
Posts: 55
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Hi everybody.

I am working on my robot, and i want to add a ultrasonic distance sensor( a HC-SR04 ) i already have one installed to avoid objects.
Now i want to add a HC-SR04  to measure the distance between the robot and the floor to prevent it falling from the stairs or my desk.
but it isn't working out that well so if somebody could help me out i would be very happy.



Code:
#include "Ultrasonic.h"
#define  TRIG_PIN  45 // object avoiding HC-SR04
#define  ECHO_PIN  44 // object avoiding HC-SR04

#define  TRIG_PIN2  43 // preventing from falling HC-SR04
#define  ECHO_PIN2  42 // preventing from falling HC-SR04

Ultrasonic OurModule2(TRIG_PIN2, ECHO_PIN2); // preventing from falling module
Ultrasonic OurModule(TRIG_PIN, ECHO_PIN); // object sensing module
#define PwmPinMotorA 49          // this is for the motor controller from dagu
#define PwmPinMotorB 50
#define DirectionPinMotorA 48
#define DirectionPinMotorB 51


int pos = 0;             // servo's position
#include <Servo.h>     
Servo myservo;          // servo 1
Servo myservo2;         // servo 2
int distance = 0;           
int distance2 = 0;
int distanceLeft = 0;
int distanceRight = 0;
int state = 1;        // starting state for the switchcase

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

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

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

void left(){    // turning 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(){   // turning 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(){   // standing still
  digitalWrite(DirectionPinMotorA, LOW);
  analogWrite(PwmPinMotorA, 0);
  digitalWrite(DirectionPinMotorB, LOW);
  analogWrite(PwmPinMotorB, 0);
}



void Move()   // moving system for the switchcase
{
  distance =  OurModule.Ranging(CM);
  distance2 =  OurModule2.Ranging(CM);
  if (distance < 35)
  {
    state = 2;
    pause();

  }
      else
  {
    forward();
  }
 
  if (distance2 <= 6 ){
  state = 3;
  pause();
  }
    else
  {
    forward();
  }


}


void Determine()    // determining witch way it has to turn system
{
  myservo.write(30);
  delay(400);
  distanceLeft = OurModule.Ranging(CM);
  delay(20);
  myservo.write(130);
  delay(400);
  distanceRight = OurModule.Ranging(CM);
  delay(20);
  myservo.write(80);

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

void Turn180() {
     left();
     delay(3000);
     pause();
     state = 1;
    pause();
}


void loop() {


  myservo2.write(110);   // this is only for the second servo, don't mind it isn't importend
 
  switch (state)
  {
  case 1: // 1 is moving
    Move();
    break;

  case 2: // 2 is Determine
   
    Determine();
    break;
 
  case 3:
 
     Turn180();
     break;
  default:   
  pause();
    break;
  }

 
 
}


Logged

Global Moderator
UK
Offline Offline
Brattain Member
*****
Karma: 309
Posts: 26538
I don't think you connected the grounds, Dave.
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Quote
but it isn't working out that well
Your explanation isn't working out at all well.

Maybe all the delays are reducing responsiveness.
Logged

"Pete, it's a fool looks for logic in the chambers of the human heart." Ulysses Everett McGill.
Do not send technical questions via personal messaging - they will be ignored.

Global Moderator
UK
Offline Offline
Brattain Member
*****
Karma: 309
Posts: 26538
I don't think you connected the grounds, Dave.
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Maybe, but it is difficult to tell what you expect your system to do, and what it actually does do.
You need to report what you observe, and how that is different to what you expect.
Logged

"Pete, it's a fool looks for logic in the chambers of the human heart." Ulysses Everett McGill.
Do not send technical questions via personal messaging - they will be ignored.

Offline Offline
Full Member
***
Karma: 0
Posts: 162
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

I'll offer 2 points.
1 - The spec sheet says 2cm is the minimum range.  That may be approximate.
How close is your sensor from the floor?  Try raising it if it's low.

2.  Are you enabling both ultrasound units at the same time?
There could be some crosstalk.  The unit listens for the telltale 8-pulse echo and gets confused by the echo from the other unit.
Logged

Global Moderator
Melbourne, Australia
Offline Offline
Brattain Member
*****
Karma: 506
Posts: 19141
Lua rocks!
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

But i think you would need them because other wise the servo isn't in the right position yet before it starts to measure ?
or am i being completely wrong now ?

Rework without the delays. You can control the time things happen without only doing one thing at once. Like when you cook bacon, and eggs, at once:

http://www.gammon.com.au/blink
Logged

http://www.gammon.com.au/electronics

Please post technical questions on the forum - not to me by personal message. Thanks a lot.

Pages: [1]   Go Up
Jump to: