The code is below. I still need to tweak a bit and am thinking that I may want to use three sensors. There is still the issue that the ping sensor can't detect very well at angles greater then 45 degrees.
I will also try the Infrared Distance Sensors as a remedy to this problem.
Code
---------------------------------
//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
#include <NewPing.h>
#define TRIGGER_PIN_R 2 // Arduino pin tied to trigger pin on ping sensor.
#define ECHO_PIN_R 3 // Arduino pin tied to echo pin on ping sensor.
#define TRIGGER_PIN_L 8 // Arduino pin tied to trigger pin on ping sensor.
#define ECHO_PIN_L 9 // Arduino pin tied to echo pin on ping sensor.
#define MAX_DISTANCE 300 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
int distance_R = 0;
int distance_L = 0;
NewPing sonar_R(TRIGGER_PIN_R, ECHO_PIN_R, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
NewPing sonar_L(TRIGGER_PIN_L, ECHO_PIN_L, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
unsigned int pingSpeed_R = 50; // How frequently are we going to send out a ping (in milliseconds). 50ms would be 20 times a second.
unsigned long pingTimer_R = 75; // Holds the next ping time, start at 75ms to give time for the Arduino pins to stabilize.
unsigned int pingSpeed_L = 50; // How frequently are we going to send out a ping (in milliseconds). 50ms would be 20 times a second.
unsigned long pingTimer_L = 75; // Holds the next ping time, start at 75ms to give time for the Arduino pins to stabilize.
//This sections is required for the smoothing
const int numReadings = 2;
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 difference = 0;
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(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++);
// Notice how there's no delays in this sketch to allow you to do other processing in-line while doing distance pings.
if (millis() >= pingTimer_R) { // pingSpeed milliseconds since last ping, do another ping.
pingTimer_R += pingSpeed_R; // Set the next ping time.
distance_R = sonar_R.ping_cm(); // Send out the ping, get the results in centimeters.
// Notice how there's no delays in this sketch to allow you to do other processing in-line while doing distance pings.
}
if (millis() >= pingTimer_L) { // pingSpeed milliseconds since last ping, do another ping.
pingTimer_L += pingSpeed_L; // Set the next ping time.
distance_L = sonar_L.ping_cm(); // Send out the ping, get the results in centimeters.
}
//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(distance_L); // Print the result (0 = outside the set distance range, no ping echo)
Serial.println("cm Left");
Serial.print(distance_R); // Print the result (0 = outside the set distance range, no ping echo)
Serial.println("cm Right");
}
Serial.print(average_L);
Serial.println(" Left Sensor Avgerage");
Serial.print(average_R);
Serial.println(" Right Sensor Avgerage");
delay (5000);
counter = 0;
}
void loop() {
smooth_sensors();
//Case #1 - No Time to Turn so Back Up
if (average_R <= 3 && average_L <= 3){
analogWrite (fwd_motor, 0);
rev_motor_speed = 0;
stearing.write(center);
Serial.println("STOP and Center stering c1");
delay (3000);
while(average_R <= 8 || average_L <= 8){
analogWrite (rev_motor, 50);
Serial.println("reverse slowly - Straight c1");
rev_motor_speed = 50;
smooth_sensors();
delay(500);
}
}
//Case #2 - Need to Turn with in Turning Range and drive slow
if (average_R <= 8 || average_L <= 8){
if (rev_motor_speed > 0){
analogWrite(rev_motor, 0);
rev_motor_speed = 0;
Serial.println("STOP, Turn off rev. motor c2");
delay(2000); //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(500);
}
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(500);
}
else {
stearing.write(center);
analogWrite(fwd_motor, 50); //drive fwd slowly
Serial.println(" Drive fwd slowly c2");
delay(500);
}
}
//Case #3 - No obstacles so drive fwd at full speed.
if (average_R > 8 && average_L > 8){
if (rev_motor_speed > 0){
analogWrite(rev_motor, 0);
rev_motor_speed = 0;
stearing.write(center);
Serial.println("STOP, turn off Rev. Motor and center stearing c3");
delay(2000); //if the Rev Motor is on, turn it OFF and wait 1 sec.
}
while (average_R > 8 && average_L > 8){
analogWrite(fwd_motor, 255);
stearing.write(center);
smooth_sensors();
Serial.println("Drive Forward Full Speed c3");
delay(500);
}
}
}
No comments:
Post a Comment