https://www.youtube.com/watch?v=-jQ1rAnhA88
Arduino https://shope.ee/flzWiiRYw
PJ#5 หุ่นยนต์หลบสิ่งกีดขวาง 2ล้อ 2WD Smart Car Robot DIY Arduino Obstacle Avoiding Car At
Home
9/6/2565 SONGCHAI PRAPATRUNGSEE
//ขา9ในProteus8 ทำPWMไม่ได้
#define maxSpeed 100 // sets speed of DC motors
int speedSet = 0;//PWM speed of DC motors
int moveForwardL=0;
int moveBackwardL=0;
int turnRightL=0;
int turnLeftL=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;
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
//กำหนดการทำงานของขา Ultrasonic
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
}
void loop (){
int distanceR = 0;
int distanceL = 0;
delay(40);// ถ้าไม่delay40 จะไม่สามารถทำงานได้ในProteus8
distance = readUltra();
if(distance<=15){
moveStop();
delay(100);
moveBackward();
delay(700);
moveStop();
delay(100);
distanceR = lookRight();
distanceL = lookLeft();
if(distanceR>=distanceL){
turnRight();
delay(700);
moveStop();
}else{
turnLeft();
delay(700);
moveStop();
}
}else{
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);
delay(500);
int distance = readUltra();
myservo.write(100);
delay(500);
return distance;
}
int lookLeft(){
myservo.write(160);
delay(500);
int distance = readUltra();
myservo.write(100);
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;
turnRightL=0;
turnLeftL=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;
turnRightL=0;
turnLeftL=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 turnRight() {
moveForwardL=0;
moveBackwardL=0;
turnLeftL=0;
if(turnRightL==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);
}
turnRightL=1;
}
}
void turnLeft() {
moveForwardL=0;
moveBackwardL=0;
turnRightL=0;
if(turnLeftL==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);
}
turnLeftL=1;
}
}
ไม่มีความคิดเห็น:
แสดงความคิดเห็น