Revisions to How do I use two ultrasonic sensors to control a 5 V motor?

Improved Code

Source Link

edited Apr 19, 2014 at 15:03

6.3k
10
32
62

int motorPin = 9;
int trigPin = 13;
int echoPin = 12;
int trigPin2 = 5;
int echoPin2 = 4;
int postdist = 112;
int postdist2 = 112;

void setup() {
  Serial.begin (9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(motorPin, OUTPUT);
}

void loop() {
  //First Read
  int duration, distance;
  digitalWrite (trigPin, HIGH);
  delayMicroseconds (1000);
  digitalWrite (trigPin, LOW);
  duration = pulseIn (echoPin, HIGH);
  distance = (duration/2) / 29.1;
  int ltol = postdist - 5;
  int htol = postdist + 5;
  if (distance > 20 || !(distance > ltol && distance < htol)) {  // Distance from sensor
    Serial.println("Out of range");
    distance = postdist;
  }
  //Second Read
  int duration2, distance2;
  digitalWrite (trigPin2, HIGH);
  delayMicroseconds (1000);
  digitalWrite (trigPin2, LOW);
  duration2 = pulseIn (echoPin2, HIGH);
  distance2 = (duration2/2) / 29.1;
  int ltol2 = postdist2 - 5;
  int htol2 = postdist2 + 5;
  if (distance2 > 20 || !(distance2 > ltol2 && distance2 < htol2)) {  // Distance from sensor
    Serial.println("Out of range");
    distance2 = postdist2;
  }
  //Math:
  Serial.print("Distances-1: ");
  Serial.print(distance);
  Serial.print(" cm 2: ");
  Serial.print("Distances-1: ");
  Serial.print(distance2);
  Serial.println(" cm");
  postdist = distance;
  postdist2 = distance2;
  int speed = 112 + map(distance, 0, 20, 0, 112) - map(distance2, 0, 20, 0, 112); 
  if(speed < 60) {speed = 60;} //Modify 60 to meet your needs for minimum threshold
  analogWrite (motorPin, speed);
  Serial.print("Motor Speed: ");
  Serial.println(speed);
  delay(500);
}

Changelog: forgot to edit postdist/postdist2 at end of cycle, added minimum threshold, prevent very small changes in voltage to keep voltage stable and prevent noise.

int motorPin = 9;
int trigPin = 13;
int echoPin = 12;
int trigPin2 = 5;
int echoPin2 = 4;
int postdist = 112;
int postdist2 = 112;

void setup() {
  Serial.begin (9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(motorPin, OUTPUT);
}

void loop() {
  //First Read
  int duration, distance;
  digitalWrite (trigPin, HIGH);
  delayMicroseconds (1000);
  digitalWrite (trigPin, LOW);
  duration = pulseIn (echoPin, HIGH);
  distance = (duration/2) / 29.1;
  if (distance > 20) {  // Distance from sensor
    Serial.println("Out of range");
    distance = postdist;
  }
  //Second Read
  int duration2, distance2;
  digitalWrite (trigPin2, HIGH);
  delayMicroseconds (1000);
  digitalWrite (trigPin2, LOW);
  duration2 = pulseIn (echoPin2, HIGH);
  distance2 = (duration2/2) / 29.1;
  if (distance2 > 20) {  // Distance from sensor
    Serial.println("Out of range");
    distance2 = postdist2;
  }
  //Math:
  Serial.print("Distances-1: ");
  Serial.print(distance);
  Serial.print(" cm 2: ");
  Serial.print("Distances-1: ");
  Serial.print(distance2);
  Serial.println(" cm");
  int speed = 112 + map(distance, 0, 20, 0, 112) - map(distance2, 0, 20, 0, 112); 
  analogWrite (motorPin, speed);
  Serial.print("Motor Speed: ");
  Serial.println(speed);
  delay(500);
}

int motorPin = 9;
int trigPin = 13;
int echoPin = 12;
int trigPin2 = 5;
int echoPin2 = 4;
int postdist = 112;
int postdist2 = 112;

void setup() {
  Serial.begin (9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(motorPin, OUTPUT);
}

void loop() {
  //First Read
  int duration, distance;
  digitalWrite (trigPin, HIGH);
  delayMicroseconds (1000);
  digitalWrite (trigPin, LOW);
  duration = pulseIn (echoPin, HIGH);
  distance = (duration/2) / 29.1;
  int ltol = postdist - 5;
  int htol = postdist + 5;
  if (distance > 20 || !(distance > ltol && distance < htol)) {  // Distance from sensor
    Serial.println("Out of range");
    distance = postdist;
  }
  //Second Read
  int duration2, distance2;
  digitalWrite (trigPin2, HIGH);
  delayMicroseconds (1000);
  digitalWrite (trigPin2, LOW);
  duration2 = pulseIn (echoPin2, HIGH);
  distance2 = (duration2/2) / 29.1;
  int ltol2 = postdist2 - 5;
  int htol2 = postdist2 + 5;
  if (distance2 > 20 || !(distance2 > ltol2 && distance2 < htol2)) {  // Distance from sensor
    Serial.println("Out of range");
    distance2 = postdist2;
  }
  //Math:
  Serial.print("Distances-1: ");
  Serial.print(distance);
  Serial.print(" cm 2: ");
  Serial.print("Distances-1: ");
  Serial.print(distance2);
  Serial.println(" cm");
  postdist = distance;
  postdist2 = distance2;
  int speed = 112 + map(distance, 0, 20, 0, 112) - map(distance2, 0, 20, 0, 112); 
  if(speed < 60) {speed = 60;} //Modify 60 to meet your needs for minimum threshold
  analogWrite (motorPin, speed);
  Serial.print("Motor Speed: ");
  Serial.println(speed);
  delay(500);
}

Changelog: forgot to edit postdist/postdist2 at end of cycle, added minimum threshold, prevent very small changes in voltage to keep voltage stable and prevent noise.

Fixed Minor issues/added code

Source Link

edited Apr 13, 2014 at 22:57

Anonymous Penguin

6.3k
10
32
62

int speed = 112 + map(distance_1distance, 0, 20, 0, 112) - map(distance_1distance2, 0, 20, 0, 112);

So if both sensors gave equal values, they would offset each other. To make it still work if an object is removed, you could just use the last distance_1distance/distance_2 value and update the other.

[Will add code in a bit]

****As I understand, this is what you want:

int motorPin = 9;
int trigPin = 13;
int echoPin = 12;
int trigPin2 = 5;
int echoPin2 = 4;
int postdist = 112;
int postdist2 = 112;

void setup() {
  Serial.begin (9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(motorPin, OUTPUT);
}

void loop() {
  //First Read
  int duration, distance;
  digitalWrite (trigPin, HIGH);
  delayMicroseconds (1000);
  digitalWrite (trigPin, LOW);
  duration = pulseIn (echoPin, HIGH);
  distance = (duration/2) / 29.1;
  if (distance > 20) {  // Distance from sensor
    Serial.println("Out of range");
    distance = postdist;
  }
  //Second Read
  int duration2, distance2;
  digitalWrite (trigPin2, HIGH);
  delayMicroseconds (1000);
  digitalWrite (trigPin2, LOW);
  duration2 = pulseIn (echoPin2, HIGH);
  distance2 = (duration2/2) / 29.1;
  if (distance2 > 20) {  // Distance from sensor
    Serial.println("Out of range");
    distance2 = postdist2;
  }
  //Math:
  Serial.print("Distances-1: ");
  Serial.print(distance);
  Serial.print(" cm 2: ");
  Serial.print("Distances-1: ");
  Serial.print(distance2);
  Serial.println(" cm");
  int speed = 112 + map(distance, 0, 20, 0, 112) - map(distance2, 0, 20, 0, 112); 
  analogWrite (motorPin, speed);
  Serial.print("Motor Speed: ");
  Serial.println(speed);
  delay(500);
}

int speed = 112 + map(distance_1, 0, 20, 0, 112) - map(distance_1, 0, 20, 0, 112);

So if both sensors gave equal values, they would offset each other. To make it still work if an object is removed, you could just use the last distance_1/distance_2 value and update the other.

[Will add code in a bit]

int speed = 112 + map(distance, 0, 20, 0, 112) - map(distance2, 0, 20, 0, 112);

So if both sensors gave equal values, they would offset each other. To make it still work if an object is removed, you could just use the last distance/distance_2 value and update the other.

****As I understand, this is what you want:

int motorPin = 9;
int trigPin = 13;
int echoPin = 12;
int trigPin2 = 5;
int echoPin2 = 4;
int postdist = 112;
int postdist2 = 112;

void setup() {
  Serial.begin (9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(motorPin, OUTPUT);
}

void loop() {
  //First Read
  int duration, distance;
  digitalWrite (trigPin, HIGH);
  delayMicroseconds (1000);
  digitalWrite (trigPin, LOW);
  duration = pulseIn (echoPin, HIGH);
  distance = (duration/2) / 29.1;
  if (distance > 20) {  // Distance from sensor
    Serial.println("Out of range");
    distance = postdist;
  }
  //Second Read
  int duration2, distance2;
  digitalWrite (trigPin2, HIGH);
  delayMicroseconds (1000);
  digitalWrite (trigPin2, LOW);
  duration2 = pulseIn (echoPin2, HIGH);
  distance2 = (duration2/2) / 29.1;
  if (distance2 > 20) {  // Distance from sensor
    Serial.println("Out of range");
    distance2 = postdist2;
  }
  //Math:
  Serial.print("Distances-1: ");
  Serial.print(distance);
  Serial.print(" cm 2: ");
  Serial.print("Distances-1: ");
  Serial.print(distance2);
  Serial.println(" cm");
  int speed = 112 + map(distance, 0, 20, 0, 112) - map(distance2, 0, 20, 0, 112); 
  analogWrite (motorPin, speed);
  Serial.print("Motor Speed: ");
  Serial.println(speed);
  delay(500);
}

Source Link

created Apr 13, 2014 at 22:41

Anonymous Penguin

6.3k
10
32
62

This is very simple using the map() instruction. Here's the code you would use (feel free to modify values:

int speed = 112 + map(distance_1, 0, 20, 0, 112) - map(distance_1, 0, 20, 0, 112);

So if both sensors gave equal values, they would offset each other. To make it still work if an object is removed, you could just use the last distance_1/distance_2 value and update the other.

One issue with this approach is, if the line of transformation of the object is not perpendicular to the line of the pings, the value will go:

10 → 11 → 12 → 13 → 14 → 15 → 16 → 17 → 18 → 19 → 20 → [Out of range]

I've added some pretty animations to show you what I mean:

Perpendicular:

Collinear:

[Will add code in a bit]

Stack Exchange Network

Return to Answer