Hướng dẫn làm robot tự tránh vật cản sử dụng cảm biến siêu âm

 

 

 

 

 

 

 

 

 

 

 

 

 

Sau đó đặt vào khe chờ sẵn trên khung.

 

 

Bước 7

 

 

 

Dán pin lên khung robot và tiến hành nối dây. Các bạn có thể dùng sẵn bộ khung 4 pin tiểu 1.5V được tặng kèm khi mua bộ khung hoặc mua 2 pin cell 18650 như thế này để có thể sạc lại pin sau khi pin hết.

 

 

 

 

 

 

 

 

Giờ các bạn nối đầu dây âm của pin ( dây màu đen) vào chân GND trên module L293. Đầu dây dương còn lại của pin các bạn nối vào một đầu dây của công tắc. Đầu dây còn lại của công tắc các bạn nối với chân M+ trên Module L293

Bước 8

Do Arduino không có sẵn thư viện NewPing và thư viện AFMotor vậy nên chúng ta cần phải thêm vào Arduino. 

Các bạn tải thư viện NewPing về tại đây và thư viện AFMotor tại đây

Sau khi thêm thư viện xong các bạn copy đoạn code này vào Arduino trên máy tính

 

 

 

#include < AFMortor.h>

#include < Servo.h>

#include < NewPing.h>

#define TRIG_PIN A0

#define ECHO_PIN A1

#define MAX_DISTANCE_POSSIBLE 1000

#define MAX_SPEED 150

#define MOTORS_CALIBRATION_OFFSET 3

#define COLL_DIST 20

#define TURN_DIST COLL_DIST 10

NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE_POSSIBLE);

AF_DCMotor leftMotor(4, MOTOR12_8KHZ);

AF_DCMotor rightMotor(2, MOTOR12_8KHZ);

Servo neckControllerServoMotor;

int pos = 0;

int maxDist = 0;

int maxAngle = 0;

int maxRight = 0;

int maxLeft = 0;

int maxFront = 0;

int course = 0;

int curDist = 0;

String motorSet = "";

int speedSet = 0;

void setup() {

neckControllerServoMotor.attach(10);

neckControllerServoMotor.write(90);

delay(2000);

checkPath();

motorSet = "FORWARD";

neckControllerServoMotor.write(90);

moveForward();

}

void loop() {

checkForward();

checkPath();

}

void checkPath() {

int curLeft = 0;

int curFront = 0;

int curRight = 0;

int curDist = 0;

neckControllerServoMotor.write(144);

delay(120);

for(pos = 144; pos >= 36; pos-=18)

{

neckControllerServoMotor.write(pos);

delay(90);

checkForward();

curDist = readPing();

if (curDist < COLL_DIST) {

checkCourse();

break;

}

if (curDist < TURN_DIST) {

changePath();

}

if (curDist > curDist) {maxAngle = pos;}

if (pos > 90 && curDist > curLeft) { curLeft = curDist;}

if (pos == 90 && curDist > curFront) {curFront = curDist;}

if (pos < 90 && curDist > curRight) {curRight = curDist;}

}

maxLeft = curLeft;

maxRight = curRight;

maxFront = curFront;

}

void setCourse() {

if (maxAngle < 90) {turnRight();}

if (maxAngle > 90) {turnLeft();}

maxLeft = 0;

maxRight = 0;

maxFront = 0;

}

void checkCourse() {

moveBackward();

delay(500);

moveStop();

setCourse();

}

void changePath() {

if (pos < 90) {lookLeft();}

if (pos > 90) {lookRight();}

}

int readPing() {

delay(70);

unsigned int uS = sonar ping();

int cm = uS/US_ROUNDTRIP_CM;

return cm;

}

void checkForward() { if (motorSet=="FORWARD") {leftMotor.run(FORWARD); rightMotor.run(FORWARD); } }

void checkBackward() { if (motorSet=="BACKWARD") {leftMotor.run(BACKWARD); rightMotor.run(BACKWARD); } }

void moveStop() {leftMotor.run(RELEASE); rightMotor.run(RELEASE);}

void moveForward() {

motorSet = "FORWARD";

leftMotor.run(FORWARD);

rightMotor.run(FORWARD);

for (speedSet = 0; speedSet < MAX_SPEED; speedSet =2)

{

leftMotor.setSpeed(speedSet MOTORS_CALIBRATION_OFFSET);

rightMotor.setSpeed(speedSet);

delay(5);

}

}

void moveBackward() {

motorSet = "BACKWARD";

leftMotor.run(BACKWARD);

rightMotor.run(BACKWARD);

for (speedSet = 0; speedSet < MAX_SPEED; speedSet =2)

{

leftMotor.setSpeed(speedSet MOTORS_CALIBRATION_OFFSET);

rightMotor.setSpeed(speedSet);

delay(5);

}

}

void turnRight() {

motorSet = "RIGHT";

leftMotor.run(FORWARD);

rightMotor.run(BACKWARD);

delay(400);

motorSet = "FORWARD";

leftMotor.run(FORWARD);

rightMotor.run(FORWARD);

}

void turnLeft() {

motorSet = "LEFT";

leftMotor.run(BACKWARD);

rightMotor.run(FORWARD);

delay(400);

motorSet = "FORWARD";

leftMotor.run(FORWARD);

rightMotor.run(FORWARD);

}

void lookRight() {rightMotor.run(BACKWARD); delay(400); rightMotor.run(FORWARD);}

void lookLeft() {leftMotor.run(BACKWARD); delay(400); leftMotor.run(FORWARD);}

 

 

 

 

 

Ấn vào biểu tượng V để dịch code

 

 

 

 

Ấn vào biểu tượng mũi tên để nạp code

Vậy là chúng ta đã hoàn thành rồi. Đây là thành quả của chúng ta.