Ultrasonic Module HC-SR04 Distance Sensor For Arduino, I was able to use the smoothing example in the Arduino Interface software, and something that I am calling threading to get the motor to turn on variable speeds based on distance, including reverse, and the servo to turn right or left depending on the difference of the two sensors. The issue is that the sensors are still buggy. With the smoothing, I am now getting better numbers because the errors are averaged out. However, when an error happens on the sensor, it takes a second pause which given my written delay, it is taking as long as 4 seconds to get a measurement sometimes. This causes the entire thing to slow down and become not reliable for a moving car that is trying to make decisions about going right or left or speeding up or slowing down. I am hoping to have better luck with some IR Sensors. I am hoping that it will give me more predictable results along with a faster response time. The code for what I did is below. It does work as is. But is has issues.
--------------------------------------------
//Two Ping Sensors with one Motor and one Servo
//This code is for controling a single motor and a servo with a pair of ping sensors (4-pin, HC-SR04).
//Orginally used for a Lego Technics car that self drives around a room
//by Philip Leete
//Define information about ping sensors
int trigPin_R = 2, trigPin_L = 8;
int echoPin_R = 3, echoPin_L = 9;
int duration_R, duration_L;
int distance_R, distance_L;
int difference = 0; //uses avg. distances to calculate
//This sections is required for the smoothing
const int numReadings = 10;
int readings_R[numReadings];
int readings_L[numReadings]; // the readings from the analog input
int index_R = 0, index_L = 0; // the index of the current reading
int total_R = 0, total_L = 0; // the running total
int average_R = 0, average_L = 0; // the average of the light values
int counter = 0; // this counter will be used to start/stop sensors
//Define information about motors
#define fwd_motor 6 //fwd motor connected to digital pin 6
#define rev_motor 11 //rev motor connected to digital pin 11
int rev_motor_speed = 0; //a integer to be used to determine if the motor is on or off backwards
//Define information about steering servo
int center = 90; //the center position for driving straight with the servo
#include <Servo.h>
Servo stearing;
void setup() {
Serial.begin (9600); //Turn on Serial Monitor
//Define what will be inputs and what will be outputs
pinMode(trigPin_R, OUTPUT);
pinMode(echoPin_R, INPUT);
pinMode(trigPin_L, OUTPUT);
pinMode(echoPin_L, INPUT);
pinMode(fwd_motor, OUTPUT);
pinMode(rev_motor, OUTPUT);
stearing.attach(5); //Where the steering servo will be put
//These for statements are to initalize all of the smoothing settings to zero
for (int thisReading = 0; thisReading < numReadings; thisReading++)
readings_R[thisReading] = 0;
for (int thisReading = 0; thisReading < numReadings; thisReading++)
readings_L[thisReading] = 0;
}
//create a function called smooth_sensors so it can be called later in the void loop
void smooth_sensors(){
while (counter < numReadings){
counter = (counter + counter++);
//Activate the Left Ping Sensor
digitalWrite(trigPin_L, LOW);
delayMicroseconds(2);
digitalWrite(trigPin_L, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin_L, LOW);
duration_L = pulseIn(echoPin_L, HIGH);
distance_L = (duration_L)/140; //conversion to inches
//Activate the Right Ping Sensor
digitalWrite(trigPin_R, LOW);
delayMicroseconds(2);
digitalWrite(trigPin_R, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin_R, LOW);
duration_R = pulseIn(echoPin_R, HIGH);
distance_R = (duration_R)/140; //conversion to inches
//This section is for smoothing
total_R= total_R - readings_R[index_R]; // subtract the last reading:
total_L= total_L - readings_L[index_L];
readings_R[index_R] = distance_R; // read from the sensor:
readings_L[index_L] = distance_L;
total_R= total_R + readings_R[index_R]; // add the reading to the total:
total_L= total_L + readings_L[index_L];
index_R = index_R + 1, index_L = index_R + 1; // advance to the next position in the array:
if (index_R >= numReadings) // if we're at the end of the array...
index_R = 0; // ...wrap around to the beginning
if (index_L >= numReadings)
index_L = 0;
average_R = total_R / numReadings; // calculate the average rt. distance
average_L = total_L / numReadings; // calculate the average lft. distance
difference = average_R - average_L; // calculate the difference beween sensors
//this sections prints out info to the serial monitor
// Serial.print(average_L);
// Serial.println(" Left Sensor Avgerage");
// Serial.print(distance_L);
// Serial.println(" Left Sensor Data");
// Serial.print(average_R);
// Serial.println(" Right Sensor Avgerage");
// Serial.print(distance_R);
// Serial.println(" Right Sensor Data");
delay(2); // delay in between reads for stability
}
// Serial.println("finished new average");
counter = 0;
}
void loop() {
smooth_sensors();
//Case #1 - No Time to Turn so Back Up
if (average_R <= 5 && average_L <= 5){
analogWrite (fwd_motor, 0);
rev_motor_speed = 0;
stearing.write(center);
Serial.println("stop and Center stering c1");
delay (1000);
while(average_R <= 10 || average_L <= 10){
analogWrite (rev_motor, 50);
Serial.println("reverse slowly - Straight c1");
rev_motor_speed = 50;
smooth_sensors();
delay(1000);
}
}
//Case #2 - Need to Turn with in Turning Range and drive slow
if (average_R <= 10 || average_L <= 10){
if (rev_motor_speed > 0){
analogWrite(rev_motor, 0);
rev_motor_speed = 0;
Serial.println("Turn off rev. motor c2");
delay(1000); //if the Rev Motor is on, turn it OFF and wait 1 sec.
}
if (difference < 0){ //indicates right sensor is closer so turn left
stearing.write(center - 30); //turn left
analogWrite(fwd_motor, 50); //drive fwd slowly
Serial.println(" Turning Left and Drive Fwd Slow c2");
delay(1000);
}
if (difference > 0){
stearing.write(center + 30); //turn right
analogWrite(fwd_motor, 50); //drive fwd slowly
Serial.println(" Turn Right and driving fwd slowly c2");
delay(1000);
}
else {
stearing.write(center);
analogWrite(fwd_motor, 50); //drive fwd slowly
Serial.println(" Drive fwd slowly c2");
delay(1000);
}
}
//Case #3 - No obstacles so drive fwd at full speed.
if (average_R > 10 && average_L > 10){
if (rev_motor_speed > 0){
analogWrite(rev_motor, 0);
rev_motor_speed = 0;
stearing.write(center);
Serial.println("turn off Rev. Motor and center stearing c3");
delay(1000); //if the Rev Motor is on, turn it OFF and wait 1 sec.
}
while (average_R > 10 && average_L > 10){
analogWrite(fwd_motor, 255);
stearing.write(center);
smooth_sensors();
Serial.println("Drive Forward Full Speed c3");
delay(1000);
}
}
}
No comments:
Post a Comment