DIY DIY Robot Máy hút bụi trên Arduino DIY

/*
Chương trình điều khiển robot với hai động cơ.
Robot quay khi động cơ thay đổi tốc độ và hướng của chúng.
Cản trước ở bên trái và bên phải phát hiện chướng ngại vật.
Sonar siêu âm có thể được kết nối với đầu vào tương tự (được thử nghiệm trên LV-MaxSonar-EZ1):
- đặt các chân trong mảng sonarPins theo thứ tự sau: trái, phải, trước, những thứ khác ..
Ví dụ:
1. chỉ sonar trái và phải được kết nối với chân 2 và 3: sonarPins [] = {2,3}
2. sonar trái, phải và trước được kết nối với chân 2, 3 và 5: sonarPins [] = {2,3,5}
3. chỉ sonar phía trước được kết nối với pin 5: sonarPins [] = {-1, -1.5}
4. chỉ còn lại sonar được kết nối với pin 2: sonarPins [] = {2}
5. sonar duy nhất được kết nối với chân 3: sonarPins [] = {-1,3}
6,5 sonar được kết nối với các chân 1,2,3,4,5: sonarPins [] = {1,2,3,4,5}
Khiên động cơ được sử dụng để chạy động cơ.
*/
const int Baud = 9600; // Tốc độ cổng UART

// Thuộc tính Sonar
int sonarPins [] = {1, 2}; // Số pin tương tự Số cho cảm biến sonar Pin AN
const dài MinLeftDistance = 20; // Khoảng cách bên trái tối thiểu được phép
const dài MinRightDistance = 20; // Khoảng cách tối thiểu được phép
const dài MinFrontDistance = 15; // Khoảng cách phía trước tối thiểu được phép
const int SamplesAmount = 15; // nhiều mẫu hơn - phép đo mượt mà hơn và độ trễ lớn hơn
const int SonarDisplayFrequency = 10; // chỉ hiển thị một trong những dòng này - không phải tất cả
int sonarDisplayFrequencyCount = 0;
const dài Hệ số = 2,54 / 2;
mẫu dài [sizeof (sonarPins)] [SamplesAmount];
int sample Index [sizeof (sonarPins)];

// bên phải
const int pinRightMotorDirection = 4; // cái này có thể được đánh dấu trên tấm chắn động cơ là "TRỰC TIẾP A"
const int pinRightMotorSpeed ​​= 3; // cái này có thể được đánh dấu trên tấm chắn động cơ là "PWM A"
const int pinRightBumper = 2; // nơi kết nối đúng

// bên trái
const int pinLeftMotorDirection = 7; // cái này có thể được đánh dấu trên tấm chắn động cơ là "DIR B"
const int pinLeftMotorSpeed ​​= 6; // cái này có thể được đánh dấu trên tấm chắn động cơ là "PWM B"
const int pinLeftBumper = 8; // nơi kết nối đúng

// bỏ ghi chú 2 dòng tiếp theo nếu Motor Shield bị hỏng
// const int pinRightMotorBreak = PUT_BREAK_PIN_HERE; // cái này có thể được đánh dấu trên tấm chắn động cơ là "BREAK A"
// const int pinLeftMotorBreak = PUT_BREAK_PIN_HERE; // cái này có thể được đánh dấu trên tấm chắn động cơ là "BREAK B"

// các trường
const int TurnRightTimeout = 100;
const int TurnLeftTimeout = 150;
// đặt trong bộ đếm thời gian một động cơ chạy lại: N / 10 (tính bằng mili giây)
int đếmDownWhileMovingToRight;
int đếmDownWhileMovingToLeft;

// Khởi tạo
void setup () {
Nối tiếp.begin (Baud);
initPins ();

// bỏ ghi 4 dòng tiếp theo nếu Motor Shield bị hỏng
// pinMode (pinLeftMotorBreak, OUTPUT);
// pinMode (pinRightMotorBreak, OUTPUT);
// kỹ thuật sốWrite (pinLeftMotorBreak, THẤP); // tắt ngắt
// kỹ thuật sốWrite (pinRightMotorBreak, THẤP); // tắt ngắt

runRightMotorForward ();
runLeftMotorForward ();
startMotors ();
}

// Vòng lặp chính
void loop () {

xác minhAndSetRightSide ();
xác minhAndSetLeftSide ();

processRightSide ();
processLeftSide ();

độ trễ (10); // lặp lại cứ sau 10 mili giây
}

//---------------------------------------------------
void initPins () {
pinMode (pinRightMotorDirection, OUTPUT);
pinMode (pinRightMotorSpeed, OUTPUT);
pinMode (pinRightBumper, INPUT);
pinMode (pinLeftMotorDirection, OUTPUT);
pinMode (pinLeftMotorSpeed, OUTPUT);
pinMode (pinLeftBumper, INPUT);
for (int i = 0; i pinMode (sonarPins [i], INPUT);
}

void startMotors () {
setMotorSpeed ​​(pinRightMotorSpeed, 255);
setMotorSpeed ​​(pinLeftMotorSpeed, 255);
}

void WaitWhileAnyBumperIsPression () {
while (checkBumperIsNotPression (pinRightBumper)
&& checkBumperIsNotPression (pinLeftBumper)) {
độ trễ (20); // kiểm tra cứ sau 20 mili giây
}
}

void processRightSide () {
if (CountDownWhileMovingToRight MinFrontDistance) // kiểm tra nếu không đạt được khoảng cách phía trước tối thiểu cho phép
trở về
if (checkCorerIsNotset (CountDownWhileMovingToLeft)) // nếu bộ đếm chưa đếm ngược
runLeftMotorBackward (); // chạy đúng động cơ lùi
CountDownWhileMovingToLeft = TurnLeftTimeout; // đặt bộ đếm thành giá trị tối đa để bắt đầu đếm ngược
}

bool checkCountIsNotset (int counter) {
lượt truy cập = SamplesAmount)
sample Index [pin Index] = 0;
samples [pin Index] [sample Index [pin Index]] = value;
trả lại đúng sự thật;
}

tính toán dàiAvarageDistance (int pin Index) {
trung bình dài = 0;
for (int i = 0; i trung bình + = mẫu [pin Index] [i];
lợi nhuận trung bình / SamplesAmount;
}