https://www.youtube.com/watch?v=6U1z-7IhzV0
Arduino https://shope.ee/flzWiiRYw
PJ#7 หุ่นยนต์ไม่ตกโตํะ+หลบสิ่งกีดขวาง 2ล้อ 2WD Smart Car Robot DIY Arduino Obstacle Avoiding Car At
Home
15/6/2565 SONGCHAI PRAPATRUNGSEE
//ขา9ในProteus8 ทำPWMไม่ได้
#define maxSpeed 100 // sets speed of DC motors
int speedSet = 90;//PWM speed of DC motors
int moveForwardL=0;
int moveBackwardL=0;
int moveRightL=0;
int moveLeftL=0;
// Motor A
int motor1A = 5;
int motor1B = 4;
int speedMotor1 = 11; // ขา11ใช้ได้ ขา9ใช้ไม่ได้ในProteus8, PWM ควบคุมความเร็วมอเตอร์
// Motor B
int Motor2A = 3;
int Motor2B = 2;
int speedMotor2 = 6; // ขา6 PWM ควบคุมความเร็วมอเตอร์
Servo myservo; //ประกาศตัวแปรแทน Servo
//กำหนดขาและตัวแปร Ultrasonic
const int trigPin = 10;
const int echoPin = 8;
long duration;
int distance;
int irLeft=12;
int irRight=9;
int irL,irR;
void setup(){
Serial.begin(9600);
//กำหนดขาควบคุมการทำงานของ Motor ผ่านทาง L298N
pinMode(motor1A,OUTPUT);
pinMode(motor1B,OUTPUT);
pinMode(speedMotor1,OUTPUT);
pinMode(Motor2A,OUTPUT);
pinMode(Motor2B,OUTPUT);
pinMode(speedMotor2,OUTPUT);
myservo.attach(7); // กำหนดขา7 ควบคุมServo
myservo.write(40);
//กำหนดการทำงานของขา Ultrasonic
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
//กำหนดการทำงานของขา IR Infrared
pinMode(irLeft, INPUT_PULLUP);
pinMode(irRight, INPUT_PULLUP);
}
void loop (){
int distanceR = 0;
int distanceL = 0;
delay(40);// ถ้าไม่delay40 จะไม่สามารถทำงานได้ในProteus8
irL=digitalRead(irLeft);
irR= digitalRead(irRight);
if(irL==LOW&&irR==LOW){
distance = readUltra();
if(distance<=15){
moveStop();
delay(100);
moveBackward();
delay(900);
moveStop();
delay(100);
distanceR = lookRight();
distanceL = lookLeft();
if(distanceR>=distanceL){
moveRight();
delay(900);
moveStop();
}else{
moveLeft();
delay(900);
moveStop();
}
}else{
moveForward();}
}
if(irL==HIGH&&irR==HIGH){
moveBackward();
delay(900);
moveRight();
delay(900);
moveForward();
//delay(70000);
}
if(irL==HIGH&&irR==LOW){
moveBackward();
delay(900);
moveLeft();
delay(900);
moveForward();
}
if(irL==LOW&&irR==HIGH){
moveBackward();
delay(900);
moveRight();
delay(900);
moveForward();
}
}
//////////// Functinon //////////////////////////////////////////////////////////////////////////////////////////////////////////////
int readUltra(){
digitalWrite(trigPin, LOW);
delay(10);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = duration * 0.0346 / 2;
return distance;
Serial.print("Distance: ");
Serial.println(distance);
delay(100);
}
int lookRight(){
//myservo.write(40);
myservo.write(10);
delay(500);
int distance = readUltra();
//myservo.write(100);
myservo.write(40);
delay(500);
return distance;
}
int lookLeft(){
//myservo.write(160);
myservo.write(70);
delay(500);
int distance = readUltra();
//myservo.write(100);
myservo.write(40);
delay(500);
return distance;
}
void moveStop() {
// Motor 1
analogWrite(speedMotor1, 0); //ตั้งค่าความเร็ว PWM ผ่านตัวแปร ค่าต่ำลง มอเตอร์จะหมุนช้าลง
digitalWrite(motor1A, LOW);
digitalWrite(motor1B, LOW);
// Motor 2
analogWrite(speedMotor2, 0); //ตั้งค่าความเร็ว PWM ผ่านตัวแปร ค่าต่ำลง มอเตอร์จะหมุนช้าลง
digitalWrite(Motor2A, LOW);
digitalWrite(Motor2B, LOW);
}
void moveForward() {
// moveBackwardL=0;
// moveRightL=0;
// moveLeftL=0;
// if(moveForwardL==0){
// for (speedSet = 0; speedSet < maxSpeed; speedSet +=2) // slowly bring the speed up to avoid loading down the batteries too quickly
// {
// Motor 1
analogWrite(speedMotor1, speedSet); //ตั้งค่าความเร็ว PWM ผ่านตัวแปร ค่าต่ำลง มอเตอร์จะหมุนช้าลง
digitalWrite(motor1A, HIGH);
digitalWrite(motor1B, LOW);
// Motor 2
analogWrite(speedMotor2, speedSet); //ตั้งค่าความเร็ว PWM ผ่านตัวแปร ค่าต่ำลง มอเตอร์จะหมุนช้าลง
digitalWrite(Motor2A, HIGH);
digitalWrite(Motor2B, LOW);
// }
// moveForwardL=1;
//}
}
void moveBackward() {
// moveForwardL=0;
// moveRightL=0;
// moveLeftL=0;
// if(moveBackwardL==0){
// for (speedSet = 0; speedSet < maxSpeed; speedSet +=2) // slowly bring the speed up to avoid loading down the batteries too quickly
// {
// Motor 1
analogWrite(speedMotor1, speedSet); //ตั้งค่าความเร็ว PWM ผ่านตัวแปร ค่าต่ำลง มอเตอร์จะหมุนช้าลง
digitalWrite(motor1A, LOW);
digitalWrite(motor1B, HIGH);
// Motor 2
analogWrite(speedMotor2, speedSet); //ตั้งค่าความเร็ว PWM ผ่านตัวแปร ค่าต่ำลง มอเตอร์จะหมุนช้าลง
digitalWrite(Motor2A, LOW);
digitalWrite(Motor2B, HIGH);
//delay(5);
// }
// moveBackwardL=1;
//}
}
void moveRight() {
// moveForwardL=0;
// moveBackwardL=0;
// moveLeftL=0;
// if(moveRightL==0){
// for (speedSet = 0; speedSet < maxSpeed; speedSet +=2) // slowly bring the speed up to avoid loading down the batteries too quickly
// {
// Motor 1
analogWrite(speedMotor1, speedSet); //ตั้งค่าความเร็ว PWM ผ่านตัวแปร ค่าต่ำลง มอเตอร์จะหมุนช้าลง
digitalWrite(motor1A, HIGH);
digitalWrite(motor1B, LOW);
// Motor 2
analogWrite(speedMotor2, speedSet); //ตั้งค่าความเร็ว PWM ผ่านตัวแปร ค่าต่ำลง มอเตอร์จะหมุนช้าลง
digitalWrite(Motor2A, LOW);
digitalWrite(Motor2B, HIGH);
// }
// moveRightL=1;
//}
}
void moveLeft() {
// moveForwardL=0;
// moveBackwardL=0;
// moveRightL=0;
// if(moveLeftL==0){
// for (speedSet = 0; speedSet < maxSpeed; speedSet +=2) // slowly bring the speed up to avoid loading down the batteries too quickly
// {
// Motor 1
analogWrite(speedMotor1, speedSet); //ตั้งค่าความเร็ว PWM ผ่านตัวแปร ค่าต่ำลง มอเตอร์จะหมุนช้าลง
digitalWrite(motor1A, LOW);
digitalWrite(motor1B, HIGH);
// Motor 2
analogWrite(speedMotor2, speedSet); //ตั้งค่าความเร็ว PWM ผ่านตัวแปร ค่าต่ำลง มอเตอร์จะหมุนช้าลง
digitalWrite(Motor2A, HIGH);
digitalWrite(Motor2B, LOW);
// }
// moveLeftL=1;
//}
}
ไม่มีความคิดเห็น:
แสดงความคิดเห็น