#define TRIG_PIN A4 // Pin A4 on th

#define TRIG_PIN A4 // Pin A4 on the Motor Drive Shield soldered to the ultrasonic sensor
#define ECHO_PIN A5 // Pin A5 on the Motor Drive Shield soldered to the ultrasonic sensor
#define MAX_DISTANCE 200 // sets maximum useable sensor measuring distance to 200cm
#define MAX_SPEED 180 // sets speed of DC traction motors to 180/256 or about 70% of full speed - to get power drain down.
#define MAX_SPEED_OFFSET 10 // this sets offset to allow for differences between the two DC traction motors
#define COLL_DIST 10 // sets distance at which robot stops and reverses to 10cm
#define TURN_DIST COLL_DIST+10 // sets distance at which robot veers away from object (not reverse) to 20cm (10+10)
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE); // sets up sensor library to use the correct pins to measure distance.

AF_DCMotor motor1(1, MOTOR12_1KHZ); // create motor #1 using M1 output on Motor Drive Shield, set to 1kHz PWM frequency
AF_DCMotor motor2(4, MOTOR12_1KHZ); // create motor #2, using M2 output, set to 1kHz PWM frequency
Servo myservo; // create servo object to control a servo

int pos = 0; // this sets up variables for use in the sketch (code)
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;

//-------------------------------------------- SETUP LOOP ----------------------------------------------------------------------------
void setup() {
myservo.attach(9); // attaches the servo on pin 9 (SERVO_2 on the Motor Drive Shield to the servo object
myservo.write(90); // tells the servo to position at 90-degrees ie. facing forward.
delay(2000); // delay for two seconds
checkPath(); // run the CheckPath routine to find the best path to begin travel
motorSet = "FORWARD"; // set the director indicator variable to FORWARD
myservo.write(90); // make sure servo is still facing forward
moveForward(); // run function to make robot move forward
}
//------------------------------------------------------------------------------------------------------------------------------------

//---------------------------------------------MAIN LOOP ------------------------------------------------------------------------------
void loop() {
checkForward(); // check that if the robot is supposed to be moving forward, that the drive motors are set to move forward - this is needed to overcome some issues with only using 4 AA NiMH batteries
checkPath(); // set ultrasonic sensor to scan for any possible obstacles
}
//-------------------------------------------------------------------------------------------------------------------------------------
void checkPath() {
int curLeft = 0;
int curFront = 0;
int curRight = 0;
int curDist = 0;
myservo.write(144); // set servo to face left 54-degrees from forward
delay(120); // wait 120milliseconds for servo to reach position
for(pos = 144; pos >= 36; pos-=18) // loop to sweep the servo (& sensor) from 144-degrees left to 36-degrees right at 18-degree intervals.
{
myservo.write(pos); // tell servo to go to position in variable 'pos'
delay(90); // wait 90ms for servo to get to position
checkForward(); // check the robot is still moving forward
curDist = readPing(); // get the current distance to any object in front of sensor
if (curDist < COLL_DIST) { // if the current distance to object is less than the collision distance
checkCourse(); // run the checkCourse function
break; // jump out of this loop
}
if (curDist < TURN_DIST) { // if current distance is less than the turn distance
changePath(); // run the changePath function
}
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() { // set direction for travel based on a very basic distance map, simply which direction has the greatest distance to and object - turning right or left?
if (maxAngle < 90) {turnRight();}
if (maxAngle > 90) {turnLeft();}
maxLeft = 0;
maxRight = 0;
maxFront = 0;
}
//-------------------------------------------------------------------------------------------------------------------------------------
void checkCourse() { // we're about to hit something so move backwards, stop, find where the empty path is.
moveBackward();
delay(500);
moveStop();
setCourse();
}
//-------------------------------------------------------------------------------------------------------------------------------------
void changePath() {
if (pos < 90) {veerLeft();} // if current pos of sensor is less than 90-degrees, it means the object is on the right hand side so veer left
if (pos > 90) {veerRight();} // if current pos of sensor is greater than 90-degrees, it means the object is on the left hand side so veer right
}
//-------------------------------------------------------------------------------------------------------------------------------------

int readPing() { // read the ultrasonic sensor distance
delay(70);
unsigned int uS = sonar.ping();
int cm = uS/US_ROUNDTRIP_CM;
return cm;
}
//-------------------------------------------------------------------------------------------------------------------------------------
void checkForward() { if (motorSet=="FORWARD") {motor1.run(FORWARD); motor2.run(FORWARD); } } // make sure motors are going forward
//-------------------------------------------------------------------------------------------------------------------------------------
void checkBackward() { if (motorSet=="BACKWARD") {motor1.run(BACKWARD); motor2.run(BACKWARD); } } // make sure motors are going backward
//-------------------------------------------------------------------------------------------------------------------------------------

// In some cases, the Motor Drive Shield may just stop if the supply voltage is too low (due to using only four NiMH AA cells).
// The above functions simply remind the Shield that if it's supposed to go forward, then make sure it is going forward and vice versa.

//-------------------------------------------------------------------------------------------------------------------------------------
void moveStop() {motor1.run(RELEASE); motor2.run(RELEASE);} // stop the motors.
//-------------------------------------------------------------------------------------------------------------------------------------
void moveForward() {
motorSet = "FORWARD";
motor1.run(FORWARD); // turn it on going forward
motor2.run(FORWARD); // turn it on going forward
for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up to avoid loading down the batteries too quickly
{
motor1.setSpeed(speedSet+MAX_SPEED_OFFSET);
motor2.setSpeed(speedSet);
delay(5);
}
}
//-------------------------------------------------------------------------------------------------------------------------------------
void moveBackward() {
motorSet = "BACKWARD";
motor1.run(BACKWARD); // turn it on going forward
motor2.run(BACKWARD); // turn it on going forward
for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up to avoid loading down the batteries too quickly
{
motor1.setSpeed(speedSet+MAX_SPEED_OFFSET);
motor2.setSpeed(speedSet);
delay(5);
}
}
//-------------------------------------------------------------------------------------------------------------------------------------
void turnRight() {
motorSet = "RIGHT";
motor1.run(FORWARD); // turn motor 1 forward
motor2.run(BACKWARD); // turn motor 2 backward
delay(400); // run motors this way for 400ms
motorSet = "FORWARD";
motor1.run(FORWARD); // set both motors back to forward
motor2.run(FORWARD);
}
//-------------------------------------------------------------------------------------------------------------------------------------
void turnLeft() {
motorSet = "LEFT";
motor1.run(BACKWARD); // turn motor 1 backward
motor2.run(FORWARD); // turn motor 2 forward
delay(400); // run motors this way for 400ms
motorSet = "FORWARD";
motor1.run(FORWARD); // turn it on going forward
motor2.run(FORWARD); // turn it on going forward
}
//-------------------------------------------------------------------------------------------------------------------------------------
void veerRight() {motor2.run(BACKWARD); delay(400); motor2.run(FORWARD);} // veering right? set right motor backwards for 400ms
//-------------------------------------------------------------------------------------------------------------------------------------
void veerLeft() {motor1.run(BACKWARD); delay(400); motor1.run(FORWARD);} // veering left? set left motor backwards for 400ms

Kết quả (Việt) 2:

#define TRIG_PIN A4 // Pin A4 trên Motor Drive khiên hàn với cảm biến siêu âm
#define ECHO_PIN A5 // Pin A5 trên Motor Drive khiên hàn với cảm biến siêu âm
#define MAX_DISTANCE 200 // thiết lập khoảng cách đo cảm biến sử dụng được tối đa đến 200cm
#define MAX_SPEED 180 // bộ tốc độ của động cơ DC kéo đến 180/256 hoặc khoảng 70% tốc độ đầy đủ - để có được thoát điện năng xuống.
# define MAX_SPEED_OFFSET 10 // Điều này đặt ra để cho phép bù đắp cho sự khác biệt giữa hai động cơ kéo DC
# xác định COLL_DIST 10 // bộ khoảng cách mà con robot dừng lại và đảo ngược đến 10cm
#define TURN_DIST COLL_DIST + 10 // bộ khoảng cách mà con robot veers đi từ đối tượng (không đảo ngược) đến 20cm (10 + 10)
NewPing sonar (TRIG_PIN, ECHO_PIN, MAX_DISTANCE); // Thiết lập thư viện cảm biến để sử dụng các chân đúng để đo khoảng cách. AF_DCMotor motor1 (1, MOTOR12_1KHZ); // Tạo ra động cơ # 1 sử dụng M1 đầu ra trên Motor Drive Shield, thiết lập để 1kHz PWM tần số AF_DCMotor motor2 (4, MOTOR12_1KHZ); // Tạo ra động cơ # 2, sử dụng M2 đầu ra, thiết lập để 1kHz PWM tần Servo myservo; // Tạo đối tượng servo để điều khiển servo int pos = 0; // Điều này đặt ra các biến để sử dụng trong các ký họa (code) int maxDist = 0; int maxAngle = 0; int maxRight = 0; int maxLeft = 0; int maxFront = 0; int nhiên = 0; int curDist = 0; Chuỗi motorSet = ""; int speedSet = 0; // -------------------------------------- ------ SETUP LOOP ------------------------------------------ ---------------------------------- trống setup () { myservo.attach (9); // Gắn servo trên pin 9 (SERVO_2 trên Motor Drive Shield tới những đối tượng servo myservo.write (90);.. // cho các servo để vị trí ở 90 độ tức là hướng về phía trước chậm (năm 2000); // chậm trễ cho hai giây checkPath (); // chạy thường xuyên CheckPath để tìm ra con đường tốt nhất để bắt đầu chuyến du lịch motorSet = "Forward"; // gán biến chỉ đạo để chuyển tiếp myservo.write (90); // hãy chắc chắn vẫn là servo hướng về phía trước moveForward (); // chạy chức năng để làm Robot di chuyển LOOP ------------------------------------------------- ----------------------------- trống loop () { checkForward (); // kiểm tra xem nếu robot là vụ phải được di chuyển về phía trước, mà các động cơ ổ đĩa được thiết lập để di chuyển về phía trước - điều này là cần thiết để khắc phục một số vấn đề với chỉ sử dụng 4 pin AA NiMH checkPath (); // thiết lập cảm biến siêu âm để quét cho bất kỳ thể checkPath () { int curLeft = 0; int curFront = 0; int curRight = 0; int curDist = 0; myservo.write (144); // đặt servo để đối mặt với trái 54 độ từ phía trước chậm (120); // chờ 120milliseconds cho servo để đạt được vị trí cho (pos = 144; pos> = 36; pos- = 18) // vòng lặp để quét các servo (& cảm biến) từ 144-độ trái để 36 độ bên phải ở khoảng 18 độ. { myservo.write (pos); // cho servo để đi đến vị trí trong biến 'pos' chậm trễ (90); // chờ 90ms cho servo để có được vị trí checkForward (); // kiểm tra các robot vẫn còn di chuyển về phía trước curDist = readPing (); // lấy khoảng cách hiện tại với bất kỳ đối tượng ở phía trước của bộ cảm biến if (curDist <COLL_DIST) {// nếu khoảng cách hiện tại để phản đối là ít hơn khoảng cách va chạm checkCourse (); // chạy chức năng checkCourse nghỉ ; // nhảy ra khỏi vòng lặp này } if (curDist <TURN_DIST) {// nếu khoảng cách hiện tại là ít hơn khoảng cách lần lượt changePath (); // chạy chức năng 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 = setCourse () {// bộ hướng cho du lịch dựa trên một bản đồ khoảng cách rất cơ bản, đơn giản đó chỉ đạo có khoảng cách lớn nhất tới và đối tượng - rẽ phải hoặc trái? if (maxAngle <90) {turnRight ();} if (maxAngle> 90) {turnLeft ();} maxLeft = 0; maxRight = 0; maxFront = checkCourse () {// chúng tôi về để đạt một cái gì đó để di chuyển ngược trở lại, dừng lại, tìm nơi con đường trống changePath () { if (pos <90) {veerLeft ();} // nếu pos hiện của cảm biến là dưới 90 độ, nó có nghĩa là các đối tượng nằm ở phía bên tay phải để xoay chiều trái if (pos> 90) { veerRight ();} // nếu pos hiện của cảm biến lớn hơn 90 độ, nó có nghĩa là các đối tượng là ở phía bên trái để xoay chiều readPing () {// đọc khoảng cách cảm biến siêu âm chậm trễ (70); unsigned int uS = sonar.ping (); int cm = uS / US_ROUNDTRIP_CM; trở lại checkForward () {if (motorSet == "Forward") {motor1.run (FORWARD); motor2.run (FORWARD);}} // ​​đảm bảo động cơ được đi checkBackward () {if (motorSet == "lạc hậu") {motor1.run (lạc hậu); motor2.run (lạc hậu);}} // ​​đảm bảo động cơ được đi Trong một số trường hợp, Motor Drive Shield có thể chỉ dừng lại nếu điện áp cung cấp là quá thấp (do sử dụng chỉ có bốn tế bào AA NiMH). // Các hàm trên chỉ đơn giản nhắc nhở Shield rằng nếu đó là nghĩa vụ để đi về phía trước, thì chắc chắn rằng nó đang đi về phía trước và ngược moveStop () {motor1.run (CHÍ); motor2.run (CHÍ);} // dừng moveForward () { motorSet = "Forward"; motor1.run (FORWARD); // bật đi về phía trước motor2.run (FORWARD); // bật đi về phía trước cho (speedSet = 0; speedSet <MAX_SPEED; speedSet + = 2) // từ từ đưa tốc độ lên đến tránh tải xuống pin quá moveBackward () { motorSet = "lạc hậu"; motor1.run (lạc hậu); // bật đi về phía trước motor2.run (lạc hậu); // bật đi về phía trước cho (speedSet = 0; speedSet <MAX_SPEED; speedSet + = 2) // từ từ đưa tốc độ lên đến tránh tải xuống pin quá turnRight () { motorSet = "RIGHT"; motor1.run (FORWARD); // biến động cơ 1 phía trước motor2.run (lạc hậu); // biến động cơ 2 lạc hậu chậm trễ (400); // động cơ chạy theo cách này cho 400ms motorSet = "Forward"; motor1.run (FORWARD); // thiết lập cả hai động cơ trở lại turnLeft () { motorSet = "LEFT"; motor1.run (lạc hậu); // biến động cơ 1 lạc hậu motor2.run (FORWARD); // biến động cơ 2 về phía trước chậm (400); // động cơ chạy theo cách này cho 400ms motorSet = "Forward"; motor1.run (FORWARD); // bật đi về phía trước motor2.run (FORWARD); // bật đi veerRight () {motor2.run (lạc hậu); delay (400); motor2.run (FORWARD);} // lại chuyển hướng ngay lập động cơ bên phải ngược cho? veerLeft () {motor1.run (lạc hậu); delay (400); motor1.run (FORWARD);} // lại chuyển hướng trái đặt động cơ trái ngược cho 400ms?