Arduino intelligent tracking and obstacle avoidance car, I have achieved obstacle avoidance and tracking, how to make the car on the track to avoid obstacles and then back to the track, how to achieve with ultrasound, how to adjust the ultrasound, and the angle of the servo?
Code:
int Echo = A1; // Echo echo pin (P2.0)
int Trig = A0; // Trig trigger foot (P2.1)
int Front_Distance = 0; //
int Left_Distance = 0;
int Right_Distance = 0;
int Left_motor_go=8; // Left motor forward (IN1)
int Left_motor_back=9; // Left_motor_back(IN2)
int Right_motor_go=10; // Right motor forward (IN3)
int Right_motor_back=11; // Right_motor_back(IN4)
int key=A2;// define the key A2 interface
int beep=A3;// Define buzzer A3 interface
const int SensorRight = 3; // right tracking infrared sensor (P3.2 OUT1)
const int SensorLeft = 4; //left tracking infrared sensor (P3.3 OUT2)
int SL; //left tracking infrared sensor status
int SR; //right tracking IR sensor status
int servopin = 2;//set the servo drive pin to digital port 2
int myangle;//Define the angle variable
int pulsewidth;//Define pulse width variable
int val;
void setup()
{
Serial.begin(9600); // Initialize the serial port
// initialize motor driver IO to output mode
pinMode(Left_motor_go,OUTPUT); // PIN 8 (PWM)
pinMode(Left_motor_back,OUTPUT); // PIN 9 (PWM)
pinMode(Right_motor_go,OUTPUT); // PIN 10 (PWM)
pinMode(Right_motor_back,OUTPUT);// PIN 11 (PWM)
pinMode(key,INPUT);// define key interface as input interface
pinMode(beep,OUTPUT);
pinMode(SensorRight, INPUT); //define right tracking infrared sensor as input
pinMode(SensorLeft, INPUT); //define the left tracing infrared sensor as input
//initialize the ultrasonic pins
pinMode(Echo, INPUT); // Define ultrasonic input pin
pinMode(Trig, OUTPUT); // Define ultrasonic output pin
pinMode(servopin, OUTPUT); // set the servo interface as output interface
}
//======================= the basic action of intelligent car =========================
//void run(int time) // forward
void run() // forward
{
digitalWrite(Right_motor_go,HIGH); // right_motor_forward
digitalWrite(Right_motor_back,LOW);
analogWrite(Right_motor_go,165); // PWM ratio 0~255 speed regulation, left and right wheel difference slightly increase or decrease
analogWrite(Right_motor_back,0);
digitalWrite(Left_motor_go,LOW); // Left motor forward
digitalWrite(Left_motor_back,HIGH);
analogWrite(Left_motor_go,0);// PWM ratio 0~255 speed regulation, left and right wheel difference slightly increase or decrease
analogWrite(Left_motor_back,160);
//delay(time * 100); //execution time, can be adjusted
}
void brake()
{
digitalWrite(Right_motor_go,LOW);
digitalWrite(Right_motor_back,LOW);
digitalWrite(Left_motor_go,LOW);
digitalWrite(Left_motor_back,LOW);
//delay(time * 100);//execution time, can be adjusted
}
void brake(int time) //brake, stop
{
digitalWrite(Right_motor_go,LOW);
digitalWrite(Right_motor_back,LOW);
digitalWrite(Left_motor_go,LOW);
digitalWrite(Left_motor_back,LOW). digitalWrite(Left_motor_back,LOW). digitalWrite(Left_motor_back,LOW);
delay(time * 100);//execution time, can be adjusted
}
void left(int time) //left turn (left wheel does not move, right wheel advances)
//void left() //left turn (left wheel does not move, right wheel advances)
{
digitalWrite(Right_motor_go,HIGH); // Right motor forward
digitalWrite(Right_motor_back,LOW);
analogWrite(Right_motor_go,180);
analogWrite(Right_motor_back,0);// PWM ratio 0~255 speed control
digitalWrite(Left_motor_go,LOW); //left_motor_back
digitalWrite(Left_motor_back,LOW);
analogWrite(Left_motor_go,0);
analogWrite(Left_motor_back,0);//PWM ratio 0~255 speed control
delay(time * 100); //execution time, can be adjusted
}
void spin_left(int time) //left turn (left wheel back, right wheel forward)
{
digitalWrite(Right_motor_go,HIGH); // right_motor_forward
digitalWrite(Right_motor_back,LOW);
analogWrite(Right_motor_go,200);
analogWrite(Right_motor_back,0);// PWM ratio 0~255 speed control
digitalWrite(Left_motor_go,HIGH); //left_motor_back
digitalWrite(Left_motor_back,LOW);
analogWrite(Left_motor_go,200);
analogWrite(Left_motor_back,0);//PWM ratio 0~255 speed control
delay(time * 100); //execution time, can be adjusted
}
void right(int time)
//void right() //right turn (right wheel not moving, left wheel forward)
{
digitalWrite(Right_motor_go,LOW); //right_motor_back
digitalWrite(Right_motor_back,LOW);
analogWrite(Right_motor_go,0);
analogWrite(Right_motor_back,0);//PWM ratio 0~255 speed control
digitalWrite(Left_motor_go,LOW);//left_motor_forward
digitalWrite(Left_motor_back,HIGH);
analogWrite(Left_motor_go,0);
analogWrite(Left_motor_back,200);//PWM ratio 0~255 speed control
delay(time * 100); //execution time, can be adjusted
}
void spin_right(int time) //right turn (right wheel back, left wheel forward)
{
digitalWrite(Right_motor_go,LOW); //right_motor_back
digitalWrite(Right_motor_back,HIGH);
analogWrite(Right_motor_go,0);
analogWrite(Right_motor_back,150);//PWM ratio 0~255 speed control
digitalWrite(Left_motor_go,LOW);//left_motor_forward
digitalWrite(Left_motor_back,HIGH);
analogWrite(Left_motor_go,0);
analogWrite(Left_motor_back,150);//PWM ratio 0~255 speed control
delay(time * 100); //execution time, can be adjusted
}
void back(int time) //back
{
digitalWrite(Right_motor_go,LOW); //Right wheel back
digitalWrite(Right_motor_back,HIGH);
analogWrite(Right_motor_go,0);
analogWrite(Right_motor_back,150);//PWM ratio 0~255 speed control
digitalWrite(Left_motor_go,HIGH); //left_motor_back
digitalWrite(Left_motor_back,LOW);
analogWrite(Left_motor_go,150);
analogWrite(Left_motor_back,0);//PWM ratio 0~255 speed control
delay(time * 100); //execution time, can be adjusted
}
//==========================================================
void keysacn()//key scan
{
int val;
val=digitalRead(key);//read the digital 7 port level value assigned to val
while(!digitalRead(key))//when the key is not pressed, keep looping
{
val=digitalRead(key);//This sentence can be omitted to let the loop run empty
}
while(digitalRead(key))//when the key is pressed
{
delay(10); //delay 10ms
val=digitalRead(key);//read the digital 7 port level value and assign it to val
if(val==HIGH) //second time to determine if the key is pressed
digitalWrite(beep,beep)
digitalWrite(beep,HIGH); //buzzer sounds
while(!digitalRead(key)) //judge whether the key is released or not
digitalWrite(beep,LOW); //buzzer stops
}
else
digitalWrite(beep,LOW); //buzzer stop
}
}
float Distance_test() // Measure the distance ahead
{
digitalWrite(Trig, LOW); // give the trigger pin a low level for 2μs
delayMicroseconds(2);
digitalWrite(Trig, HIGH); // give the trigger pin a high level of 10μs, at least 10μs here
delayMicroseconds(10);
digitalWrite(Trig, LOW); // keep the trigger pin low
float Fdistance = pulseIn(Echo, HIGH); // read high time (in microseconds)
Fdistance = Fdistance/58; // why divide by 58 equals centimeters, Y meters = (X seconds * 344) / 2
return Fdistance;
}
void servopulse(int servopin,int myangle)/* Define a pulse function to simulate the way to generate PWM values servo range is 0.5MS to 2.5MS 1.5MS Duty cycle is centered period is 20MS */
{
pulsewidth=(myangle*11)+500;//the angle is converted to 500-2480 pulse width value here myangle is 0-180 degrees so 180*11+50=2480 11 is to be converted to 90 degrees when the basic is 1.5MS
digitalWrite(servopin,HIGH);// set the servo interface level to high 90*11+50=1490uS is 1.5ms
delayMicroseconds(pulsewidth);//delay the pulse width value of microseconds Here is a call to the microsecond delay function
digitalWrite(servopin,LOW);// set the servo interface level to low
// delay(20-pulsewidth/1000);// the remaining time in the delay cycle here is called ms delay function
delay(20-(pulsewidth*0.001));// the remaining time in the delay cycle Here is the call of ms delay function
}
void front_detection()
{
// here the number of cycles is reduced, in order to increase the speed of response of the car to encounter obstacles
for(int i=0;i<=5;i++) //generate the number of PWM, equivalent delay to ensure that the response angle can be turned
{
servopulse(servopin,90);//simulate the generation of PWM
}
Front_Distance = Distance_test();
void left_detection()
{
for(int i=0;i<=15;i++) //generate the number of PWMs, equivalent delay to ensure that the response angle can be turned
{
servopulse(servopin,175);//simulate PWM generation
}
Left_Distance = Distance_test();
}
void right_detection()
{
for(int i=0;i<=15;i++) //generate the number of PWMs, equivalent delay to ensure that the response angle can be turned
{
servopulse(servopin,5);//simulate PWM generation
}
Right_Distance = Distance_test();
}
//===========================================================
void loop()
{
keysacn(); //call the key scan function
while(1)
{
SR = digitalRead(SensorRight);//signal indicates that in the white area, L1 on the car's floor is on; no signal indicates that pressed on the black line, L1 on the car's floor is off
SL = digitalRead(SensorLeft);// there is a signal indicates that in the white area, the car chassis L2 light; no signal indicates that the pressure on the black line, the car chassis L2 out
if (SL == LOW&&SR==LOW)
run(); //Call forward function
else if (SL == HIGH & SR == LOW)// left tracking infrared sensor, detect the signal, the car deviates from the track to the right, turn left
spin_left(1);
else if (SR == HIGH & SL == LOW) // right tracking infrared sensor, detect the signal, the car deviates from the track to the left, turn to the right
spin_right(1);
else // both are black, stop
brake();
front_detection();// Measure the distance ahead
if(Front_Distance < 30) // when encounter obstacle
{
brake(2);//brake first
back(2);//backward deceleration
brake(2);//Stop for distance measurement
left_detection();//measure the left distance from the obstacle
right_detection();//measure the right distance from the obstacle
if((Left_Distance < 30 ) &&( Right_Distance < 30 ))//when there are obstacles on the left and right sides are close to
spin_left(0.7);//rotate to turn around
else if(Left_Distance > Right_Distance)//left is more open than right
{
left(3);//turn left
brake(1);//brake, stabilize direction
}
else//the right side is emptier than the left side
{
right(3);//turn right
brake(1);//brake, stabilize direction
}
}
else
{
run(); //no obstacle, go straight
}
}
}