เครื่องดูดฝุ่นหุ่นยนต์ DIY DIY บน Arduino DIY

/*
โปรแกรมสำหรับควบคุมหุ่นยนต์ด้วยมอเตอร์สองตัว
หุ่นยนต์จะหมุนเมื่อมอเตอร์เปลี่ยนความเร็วและทิศทาง
กันชนหน้าทางซ้ายและขวาตรวจจับสิ่งกีดขวาง
อัลตราโซนิกโซนาร์สามารถเชื่อมต่อกับอินพุตอะนาล็อก (ทดสอบกับ LV-MaxSonar-EZ1):
- ใส่พินในอาร์เรย์ sonarPins ตามลำดับต่อไปนี้: ซ้าย, ขวา, ด้านหน้า, อื่น ๆ ..
ตัวอย่าง:
1. เฉพาะ sonars ซ้ายและขวาที่เชื่อมต่อกับหมุด 2 และ 3: sonarPins [] = {2,3}
2. โซนาร์ด้านซ้ายขวาและด้านหน้าเชื่อมต่อกับหมุด 2, 3 และ 5: sonarPins [] = {2,3,5}
3. เฉพาะโซนาร์ด้านหน้าที่เชื่อมต่อกับขา 5: sonarPins [] = {-1, -1.5}
4. เหลือโซนาร์เท่านั้นที่เชื่อมต่อกับพิน 2: sonarPins [] = {2}
5. โซนาร์ด้านขวาเท่านั้นที่เชื่อมต่อกับหมุด 3: sonarPins [] = {-1,3}
6.5 sonars เชื่อมต่อกับพิน 1,2,3,4,5: sonarPins [] = {1,2,3,4,5}
โล่มอเตอร์ใช้ในการเรียกใช้มอเตอร์
*/
const int Baud = 9600; // ความเร็วพอร์ต UART

// คุณสมบัติ Sonar
int sonarPins [] = {1, 2}; // สัญญาณอนาล็อกพินไปยังเซ็นเซอร์โซนาร์พิน AN
const ยาว MinLeftDistance = 20; // ระยะทางขั้นต่ำที่อนุญาต
const ยาว MinRightDistance = 20; // ระยะทางขั้นต่ำที่อนุญาต
const ยาว MinFrontDistance = 15; // ระยะทางด้านหน้าต่ำสุดที่อนุญาต
const int SamplesAmount = 15; // ตัวอย่างเพิ่มเติม - การวัดที่ราบรื่นยิ่งขึ้นและความล่าช้าที่ใหญ่กว่า
const int SonarDisplayFrequency = 10; // แสดงเพียงหนึ่งในบรรทัดเหล่านี้ - ไม่ใช่ทั้งหมด
int sonarDisplayFrequencyCount = 0;
const long factor = 2.54 / 2;
ตัวอย่างยาว [sizeof (sonarPins)] [SamplesAmount];
int sampleIndex [sizeof (sonarPins)];

// ด้านขวา
const int pinRightMotorDirection = 4; // สามารถทำเครื่องหมายบนตัวป้องกันมอเตอร์เป็น "DIR A"
const int pinRightMotorSpeed ​​= 3; // สามารถทำเครื่องหมายบนตัวป้องกันมอเตอร์เป็น "PWM A"
const int pinRightBumper = 2; // ตำแหน่งที่กันชนด้านขวาเชื่อมต่อ

// ด้านซ้าย
const int pinLeftMotorDirection = 7; // สามารถทำเครื่องหมายบนตัวป้องกันมอเตอร์เป็น "DIR B"
const int pinLeftMotorSpeed ​​= 6; // สามารถทำเครื่องหมายบนตัวป้องกันมอเตอร์เป็น "PWM B"
const int pinLeftBumper = 8; // ตำแหน่งที่กันชนด้านขวาเชื่อมต่อ

// uncomment 2 บรรทัดถัดไปถ้า Motor Shield มีตัวแบ่ง
// const int pinRightMotorBreak = PUT_BREAK_PIN_HERE; // สามารถทำเครื่องหมายบนตัวป้องกันมอเตอร์เป็น "BREAKE A"
// const int pinLeftMotorBreak = PUT_BREAK_PIN_HERE; // สามารถทำเครื่องหมายบนตัวป้องกันมอเตอร์เป็น "BREAKE B"

// สาขา
const int turnRightTimeout = 100;
const int turnLeftTimeout = 150;
// ตั้งตัวนับระยะเวลาที่มอเตอร์ทำงาน: N / 10 (เป็นมิลลิวินาที)
int countDownWhileMovingToRight;
int countDownWhileMovingToLeft;

// การเริ่มต้น
การตั้งค่าเป็นโมฆะ () {
Serial.begin (Baud);
initPins ();

// uncomment 4 บรรทัดถัดไปถ้า Motor Shield มีตัวแบ่ง
// pinMode (pinLeftMotorBreak, OUTPUT);
// pinMode (pinRightMotorBreak, OUTPUT);
// digitalWrite (pinLeftMotorBreak, LOW); // ปิดตัวแบ่ง
// digitalWrite (pinRightMotorBreak, LOW); // ปิดตัวแบ่ง

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

// Main loop
void loop () {

VerifyAndSetRightSide ();
VerifyAndSetLeftSide ();

processRightSide ();
processLeftSide ();

ดีเลย์ (10); // ทำซ้ำทุกๆ 10 มิลลิวินาที
}

//---------------------------------------------------
โมฆะ initPins () {
pinMode (pinRightMotorDirection, OUTPUT);
pinMode (pinRightMotorSpeed, OUTPUT);
pinMode (pinRightBumper, INPUT);
pinMode (pinLeftMotorDirection, OUTPUT);
pinMode (pinLeftMotorSpeed, OUTPUT);
pinMode (pinLeftBumper, INPUT);
สำหรับ (int i = 0; i pinMode (sonarPins [i], INPUT);
}

โมฆะ startMotors () {
setMotorSpeed ​​(pinRightMotorSpeed, 255);
setMotorSpeed ​​(pinLeftMotorSpeed, 255);
}

เป็นโมฆะ waitWhileAnyBumperIsPressed () {
ในขณะที่ (checkBumperIsNotPressed (pinRightBumper)
&& checkBumperIsNotPressed (pinLeftBumper)) {
หน่วงเวลา (20); // ตรวจสอบทุก ๆ 20 มิลลิวินาที
}
}

เป็นโมฆะ processRightSide () {
ถ้า (countDownWhileMovingToRight MinFrontDistance) // ตรวจสอบว่าระยะทางด้านหน้าขั้นต่ำที่อนุญาตนั้นไม่ถึง
กลับ;
if (checkCounterIsNotSet (countDownWhileMovingToLeft)) // ถ้าตัวนับยังไม่นับ
runLeftMotorBackward (); // ใช้มอเตอร์ทางขวาถอยหลัง
countDownWhileMovingToLeft = turnLeftTimeout; // ตั้งค่าตัวนับเป็นค่าสูงสุดเพื่อเริ่มนับถอยหลัง
}

บูล checkCounterIsNotSet (ตัวนับ int) {
return counter = SamplesAmount)
sampleIndex [pinIndex] = 0;
ตัวอย่าง [pinIndex] [sampleIndex [pinIndex]] = ค่า;
กลับจริง
}

การคำนวณระยะยาวระยะห่าง (int pinIndex) {
ค่าเฉลี่ยยาว = 0;
สำหรับ (int i = 0; i ค่าเฉลี่ย + = ตัวอย่าง [pinIndex] [i];
ผลตอบแทนเฉลี่ย / SamplesAmount;
}