ARGH!! Okay, so yesterday, it made sense in my head. However, to talk about what you want to happen and what you can write are two different things. How frustrating!
First, to refresh on what I am trying to do. I want to build a car (out of legos first) that will avoid obstacles and navigate around a room without any input from a person. I am trying to do this by using two ultrasonic ping sensors as the eyes, a servo for steering, and a motor to control forward and backward speed.
I ended up with three ideas (or logic circles) in the code.
First Idea: No time to turn so stop moving and back up.
Second Idea: I am inside the turning range so turn. Just pick the correct direction and slow down.
Third Idea: The coast is clear so go forward as fast as you can
In the code below, I used global variables and writing functions that I can call later on in the script. The biggest issue is that everything depends on the accuracy of the two ping sensors. I am not having luck. THIS IS NOT THE CASE. I am getting some garbage data as things change and that will spin the code into one loop or another. Yet when the sensor is doing what it should, it is within 2 inches. Either way, the results are not nearly consistent enough.
So, I am going to explore some ideas for storing a group of sensor reading and taking averages of the sensor data. I think it's called "Smoothing." The other idea I need to try is something that I think might be called "Threading" but I am not sure. The concept is that you take the important piece, (The avg. sensor data) and call for it to refresh when ever the robot needs to make a decision.
The idea of these two thoughts is that Smoothing will help to eliminate data by letting the sensors run faster and gather more data. As for Threading, I think I can call for the Avg. Smooth Ping Data every time the code gets to a decision point. It should speed up the process and fix the reliability issues.
Regardless, below is the code that does work but is Very Buggy! It needs some serious help.
//-------------------------------------------------------
//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.
//Orginally used for a Lego Technics car that self drives around a room
//by Philip Leete
#define trigPinR 2
#define echoPinR 3
#define trigPinL 8
#define echoPinL 9
#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
int center = 90; //the center position for driving straight with the servo
int distanceR = 0; //distance of the right sensor, inches
int distanceL = 0; //distance of the left sensor, inches
int difference = 0; //the difference between the Right and Left Sensor
#include <Servo.h>
Servo stearing;
void setup() {
Serial.begin (9600);
pinMode(trigPinR, OUTPUT);
pinMode(echoPinR, INPUT);
pinMode(trigPinL, OUTPUT);
pinMode(echoPinL, INPUT);
pinMode(fwd_motor, OUTPUT);
pinMode(rev_motor, OUTPUT);
stearing.attach(5);
stearing.write(75);
analogWrite(fwd_motor, 100); //Test motor Fwd for 2 seconds
delay (2000);
analogWrite(fwd_motor, 0);
delay (1000);
analogWrite(rev_motor, 100); //Test motor Rev for 2 seconds
delay (2000);
analogWrite(rev_motor, 0);
delay (10000);
}
//this is the distance_sensors function that I will call later
void distance_sensors() {
digitalWrite(trigPinL, LOW);
delayMicroseconds(2);
digitalWrite(trigPinL, HIGH);
delayMicroseconds(10);
digitalWrite(trigPinL, LOW);
distanceL = pulseIn(echoPinL, HIGH)/140; // the divide by 140 is a conversion to inches
delayMicroseconds(150);
Serial.print(distanceL);
Serial.println(" in. left");
digitalWrite(trigPinR, LOW);
delayMicroseconds (2);
digitalWrite(trigPinR, HIGH);
delayMicroseconds(15);
digitalWrite(trigPinR, LOW);
distanceR = pulseIn(echoPinR, HIGH)/140;
delayMicroseconds(150);
Serial.print(distanceR);
Serial.println(" in. right");
difference = distanceR - distanceL;
}
void loop() {
distance_sensors();
//Case #1 - No Time to Turn so Back Up
if (distanceR <= 10 && distanceL <= 10){
analogWrite (fwd_motor, 0);
Serial.println("stop");
delay (2000);
stearing.write(center);
delay (500);
while(distanceR <= 20 || distanceL <= 20){
analogWrite (rev_motor, 50);
rev_motor_speed = 50;
distance_sensors();
Serial.println("reverse slowly - Straight");
delay(500);
}
}
//Case #2 - Need to Turn with in Turning Range and drive slow
if (distanceR <=20 || distanceL <= 20){
if (rev_motor_speed > 0){
analogWrite(rev_motor, 0);
rev_motor_speed = 0;
Serial.println("Turn off rev. motor");
delay(2000); //if the Rev Motor is on, turn it OFF and wait 2 sec.
}
analogWrite(fwd_motor, 50); //drive fwd slowly
Serial.println("driving fwd slowly");
if (difference < 0){ //indicates right sensor is closer so turn left
stearing.write(center + 10); //turn soft left
Serial.print(center + 10);
Serial.println(" Turning Soft Left");
delay(500);
distance_sensors();
}
else {
stearing.write(center - 10); //turn soft right
Serial.print(center - 10);
Serial.println(" Turning Soft Right");
delay(500);
distance_sensors();
}
}
//Case #3 - No obstacles so drive fwd at full speed.
if (distanceR > 20 && distanceL > 20){
if (rev_motor_speed > 0){
analogWrite(rev_motor, 0);
Serial.println("turn off Rev. Motor");
delay(2000); //if the Rev Motor is on, turn it OFF and wait 2 sec.
}
while (distanceR > 20 && distanceL > 20){
analogWrite(fwd_motor, 255);
distance_sensors();
Serial.println("Drive Forward Full Speed");
delay(250);
}
}
}
No comments:
Post a Comment