vojta@viky
166 lines
4.1 KiB
Arduino
166 lines
4.1 KiB
Arduino
|
||
#include <Servo.h>
|
||
#include <Servo.h>
|
||
Servo myservo;
|
||
int Echo_Pin=A0; // ultrasonic module ECHO to A0
|
||
int Trig_Pin=A1; // ultrasonic module TRIG to A1
|
||
#define Lpwm_pin 5 //pin of controlling speed---- ENA of motor driver board
|
||
#define Rpwm_pin 6 //pin of controlling speed---- ENB of motor driver board
|
||
int pinLB=2; //pin of controlling turning---- IN1 of motor driver board
|
||
int pinLF=4; //pin of controlling turning---- IN2 of motor driver board
|
||
int pinRB=7; //pin of controlling turning---- IN3 of motor driver board
|
||
int pinRF=8; //pin of controlling turning---- IN4 of motor driver board
|
||
volatile int D_mix;
|
||
volatile int D_mid;
|
||
volatile int D_max;
|
||
volatile int Front_Distance;
|
||
volatile int Left_Distance;
|
||
volatile int Right_Distance;
|
||
volatile int Right_IR_Value;
|
||
volatile int Left_IR_Value;
|
||
|
||
float checkdistance() {
|
||
digitalWrite(Trig_Pin, LOW);
|
||
delayMicroseconds(2);
|
||
digitalWrite(Trig_Pin, HIGH);
|
||
delayMicroseconds(10);
|
||
digitalWrite(Trig_Pin, LOW);
|
||
float distance = pulseIn(Echo_Pin, HIGH) / 58.00;
|
||
delay(10);
|
||
return distance;
|
||
}
|
||
|
||
void Detect_Left_and_Right__distance() {
|
||
myservo.write(180);
|
||
delay(400);
|
||
Left_Distance = checkdistance();
|
||
delay(600);
|
||
Serial.print("Left_Distance:");
|
||
Serial.println(Left_Distance);
|
||
myservo.write(0);
|
||
delay(400);
|
||
Right_Distance = checkdistance();
|
||
delay(600);
|
||
Serial.print("Right_Distance:");
|
||
Serial.println(Right_Distance);
|
||
myservo.write(90);
|
||
}
|
||
|
||
void Ultrasonic_obstacle_avoidance()
|
||
{
|
||
Front_Distance=checkdistance(); //obtain the value detected by ultrasonic sensor
|
||
if((Front_Distance < 20)&&(Front_Distance > 0)) {
|
||
stopp();//stop
|
||
delay(100);
|
||
myservo.write(180);
|
||
delay(500);
|
||
Left_Distance=checkdistance();//measure the distance
|
||
delay(100);
|
||
myservo.write(0);
|
||
delay(500);
|
||
Right_Distance=checkdistance();//measure the distance
|
||
delay(100);
|
||
if(Left_Distance > Right_Distance) {
|
||
rotate_left(150);//turn left
|
||
myservo.write(90);
|
||
delay(300);
|
||
}
|
||
else {
|
||
rotate_right(150);// turn right
|
||
myservo.write(90);
|
||
delay(300);
|
||
}
|
||
}
|
||
else {
|
||
go_forward(100);//go forward
|
||
}
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
void Obstacle_Avoidance_Main()
|
||
{
|
||
Ultrasonic_obstacle_avoidance();
|
||
}
|
||
|
||
|
||
|
||
void setup(){
|
||
myservo.attach(A2);
|
||
Serial.begin(9600);
|
||
D_mix = 10;
|
||
D_mid = 20;
|
||
D_max = 100;
|
||
Front_Distance = 0;
|
||
Left_Distance = 0;
|
||
Right_Distance = 0;
|
||
myservo.write(90);
|
||
pinMode(Echo_Pin, INPUT);
|
||
pinMode(Trig_Pin, OUTPUT);
|
||
pinMode(pinLB,OUTPUT); // /pin 2
|
||
pinMode(pinLF,OUTPUT); // pin 4
|
||
pinMode(pinRB,OUTPUT); // pin 7
|
||
pinMode(pinRF,OUTPUT); // pin 8
|
||
pinMode(Lpwm_pin,OUTPUT); // pin 5 (PWM)
|
||
pinMode(Rpwm_pin,OUTPUT); // pin 6(PWM)
|
||
}
|
||
|
||
void loop(){
|
||
Obstacle_Avoidance_Main();
|
||
|
||
}
|
||
|
||
|
||
|
||
void go_forward(unsigned char speed_val) // speed_val：0~255
|
||
{digitalWrite(pinRB,HIGH);
|
||
digitalWrite(pinRF,LOW);
|
||
digitalWrite(pinLB,HIGH);
|
||
digitalWrite(pinLF,LOW);
|
||
analogWrite(Lpwm_pin,speed_val+6);
|
||
analogWrite(Rpwm_pin,speed_val+5);
|
||
|
||
|
||
}
|
||
|
||
void go_backward(unsigned char speed_val) // speed_val：0~255
|
||
{
|
||
digitalWrite(pinRB,LOW);
|
||
digitalWrite(pinRF,HIGH);
|
||
digitalWrite(pinLB,LOW);
|
||
digitalWrite(pinLF,HIGH);
|
||
analogWrite(Lpwm_pin,speed_val);
|
||
analogWrite(Rpwm_pin,speed_val);
|
||
}
|
||
|
||
void rotate_left(unsigned char speed_val) // speed_val：0~255
|
||
{digitalWrite(pinRB,HIGH);
|
||
digitalWrite(pinRF,LOW );
|
||
digitalWrite(pinLB,LOW);
|
||
digitalWrite(pinLF,HIGH);
|
||
analogWrite(Lpwm_pin,speed_val);
|
||
analogWrite(Rpwm_pin,speed_val);
|
||
|
||
|
||
}
|
||
void rotate_right(unsigned char speed_val) // speed_val：0~255
|
||
{
|
||
digitalWrite(pinRB,LOW);
|
||
digitalWrite(pinRF,HIGH);
|
||
digitalWrite(pinLB,HIGH);
|
||
digitalWrite(pinLF,LOW);
|
||
analogWrite(Lpwm_pin,speed_val);
|
||
analogWrite(Rpwm_pin,speed_val);
|
||
|
||
}
|
||
void stopp() //stop
|
||
{
|
||
digitalWrite(pinRB,HIGH);
|
||
digitalWrite(pinRF,HIGH);
|
||
digitalWrite(pinLB,HIGH);
|
||
digitalWrite(pinLF,HIGH);
|
||
}
|