Hello,
I needed help from all of you out there. I have wired my ultrasonic obstacle avoidance smart car and I had trouble with programming it. This is the code we used.
#Include <Servo.h>
int pinLB = 6; // Define pin left after 6
int pinLF = 9; // Define the 9-pin front left
int pinRB = 10; //10 pin definitions right rear
int pinRF = 11; // Define the 11-pin front right
int inputPin = A0; // Define pin ultrasonic signal reception
int outputPin = A1; // Define pin ultrasonic signal transmitter
int Fspeedd = 0; //-Speed
int Rspeedd = 0; // Right speed
int Lspeedd = 0; // Left-speed
int directionn = 0; // Front Left = 8 after = 2 = 4 Right = 6
Servo myservo; // Set myservo
int delay_time = 250; // settling time after steering servo motors
int Fgo = 8; // Forward
int Rgo = 6; // Right
int Lgo = 4; // Left
int Bgo = 2; // Reverse
void setup ()
{
Serial.begin (9600); // Define motor output pin
pinMode (pinLB, OUTPUT); // pin 8 (PWM)
pinMode (pinLF, OUTPUT); // pin 9 (PWM)
pinMode (pinRB, OUTPUT); // pin 10 (PWM)
pinMode (pinRF, OUTPUT); // pin 11 (PWM)
pinMode (inputPin, INPUT); // Define ultrasound input pin
pinMode (outputPin, OUTPUT); // Define ultrasonic output pin
myservo.attach (5); // Define servo motor output section 5 pin (PWM)
}
void advance (int a) // Forward
{
digitalWrite (pinRB, LOW); // The motor (rear right) action
digitalWrite (pinRF, HIGH);
digitalWrite (pinLB, LOW); // The motor (left rear) action
digitalWrite (pinLF, HIGH);
delay (a * 100);
}
void right (int b) // Turn right (single wheel)
{
digitalWrite (pinRB, LOW); // The motor (rear right) action
digitalWrite (pinRF, HIGH);
digitalWrite (pinLB, HIGH);
digitalWrite (pinLF, HIGH);
delay (b * 100);
}
void left (int c) // Turn left (single wheel)
{
digitalWrite (pinRB, HIGH);
digitalWrite (pinRF, HIGH);
digitalWrite (pinLB, LOW); // The motor (left rear) action
digitalWrite (pinLF, HIGH);
delay (c * 100);
}
void turnR (int d) // Turn right (wheel)
{
digitalWrite (pinRB, LOW); // The motor (rear right) action
digitalWrite (pinRF, HIGH);
digitalWrite (pinLB, HIGH);
digitalWrite (pinLF, LOW); // The motor (front left) action
delay (d * 100);
}
void turnL (int e) // Turn left (wheel)
{
digitalWrite (pinRB, HIGH);
digitalWrite (pinRF, LOW); // The motor (front right) action
digitalWrite (pinLB, LOW); // The motor (left rear) action
digitalWrite (pinLF, HIGH);
delay (e * 100);
}
void stopp (int f) // Stop
{
digitalWrite (pinRB, HIGH);
digitalWrite (pinRF, HIGH);
digitalWrite (pinLB, HIGH);
digitalWrite (pinLF, HIGH);
delay (f * 100);
}
void back (int g) // Check out
{
digitalWrite (pinRB, HIGH); // The motor (rear right) action
digitalWrite (pinRF, LOW);
digitalWrite (pinLB, HIGH); // The motor (left rear) action
digitalWrite (pinLF, LOW);
delay (g * 100);
}
void detection () // Measure three angles (0.90.179)
{
int delay_time = 250; Settling time // servo motor after turning
ask_pin_F (); // Read from front
if (Fspeedd <10) // If the distance is less than 10 cm in front of
{
stopp (1); // Clear the output data
back (2); // Check out 0.2 seconds
}
if (Fspeedd <25) // If the distance is less than 25 cm in front of
{
stopp (1); // Clear the output data
ask_pin_L (); // Read from left
delay (delay_time); // Wait for a stable servo motor
ask_pin_R (); // Read from the right
delay (delay_time); // Wait for a stable servo motor
if (Lspeedd> Rspeedd) // If the distance is greater than the right from the left
{
directionn = Rgo; // Right away
}
if (Lspeedd <= Rspeedd) // If the left is less than or equal to the distance from the right
{
directionn = Lgo; // Turn Left
}
if (Lspeedd <10 && Rspeedd <10) // If the distance to the left and right are less than 10 cm distance
{
directionn = Bgo; // To go after
}
}
else // Add as front not less than (greater than) 25 cm
{
directionn = Fgo; // Move forward
}
}
void ask_pin_F () // Measure the distance from the front
{
myservo.write (90);
digitalWrite (outputPin, LOW); // Let ultrasonic transmitter low voltage 2 μ s
delayMicroseconds (2);
digitalWrite (outputPin, HIGH); // Let ultrasonic transmitter high voltage 10 μ s, where at least 10 μ s
delayMicroseconds (10);
digitalWrite (outputPin, LOW); // Maintain low voltage ultrasonic transmitter
float Fdistance = pulseIn (inputPin, HIGH); // Read worse time difference
Fdistance = Fdistance/5.8/10; // Time to turn to the distance (unit: cm)
Serial.print ("F distance:"); // Output distance (unit: cm)
Serial.println (Fdistance); // Display the distance
Fspeedd = Fdistance; // Read into the distance Fspeedd (former speed)
}
void ask_pin_L () // Measure the distance from the left
{
myservo.write (5);
delay (delay_time);
digitalWrite (outputPin, LOW); // Let ultrasonic transmitter low voltage 2 μ s
delayMicroseconds (2);
digitalWrite (outputPin, HIGH); // Let ultrasonic transmitter high voltage 10 μ s, where at least 10 μ s
delayMicroseconds (10);
digitalWrite (outputPin, LOW); // Maintain low voltage ultrasonic transmitter
float Ldistance = pulseIn (inputPin, HIGH); // Read worse time difference
Ldistance = Ldistance/5.8/10; // Time to turn to the distance (unit: cm)
Serial.print ("L distance:"); // Output distance (unit: cm)
Serial.println (Ldistance); // Display the distance
Lspeedd = Ldistance; // Read into the distance Lspeedd (left-speed)
}
void ask_pin_R () // Measure the distance from the right
{
myservo.write (177);
delay (delay_time);
digitalWrite (outputPin, LOW); // Let ultrasonic transmitter low voltage 2 μ s
delayMicroseconds (2);
digitalWrite (outputPin, HIGH); // Let ultrasonic transmitter high voltage 10 μ s, where at least 10 μ s
delayMicroseconds (10);
digitalWrite (outputPin, LOW); // Maintain low voltage ultrasonic transmitter
float Rdistance = pulseIn (inputPin, HIGH); // Read worse time difference
Rdistance = Rdistance/5.8/10; // Time to turn to the distance (unit: cm)
Serial.print ("R distance:"); // Output distance (unit: cm)
Serial.println (Rdistance); // Display the distance
Rspeedd = Rdistance; // Will read into the distance Rspeedd (Right-speed)
}
void loop ()
{
myservo.write (90); // Let servo motor position ready to return to the pre-prepared next time measurement
detection (); // Measure the angle and direction of judgment to where to move
if (directionn == 2) // If directionn (direction) = 2 (reverse)
{
back (8); // Retrogression (car)
turnL (2); // Move slightly to the left (to prevent stuck in dead alley)
Serial.print ("Reverse"); // Display direction (backwards)
}
if (directionn == 6) // If directionn (direction) = 6 (right turn)
{
back (1);
turnR (6); // Right
Serial.print ("Right"); // Display direction (turn left)
}
if (directionn == 4) // If directionn (direction) = 4 (turn left)
{
back (1);
turnL (6); // Left
Serial.print ("Left"); // Display direction (turn right)
}
if (directionn == 8) // If directionn (direction) = 8 (forward)
{
advance (1); // Normal Forward
Serial.print ("Advance"); // Display direction (forward)
Serial.print (" ");
}
}
