DIY DIY مكنسة كهربائية روبوت على Arduino DIY

/*
برنامج للتحكم في الروبوت بمحركين.
يتحول الروبوت عندما تغير المحركات سرعتها واتجاهها.
مصدات أمامية على الجانبين الأيسر والأيمن تكتشف العقبات.
يمكن توصيل السونار بالموجات فوق الصوتية إلى المدخلات التناظرية (تم اختبارها على LV-MaxSonar-EZ1):
- وضع دبابيس في صفيف sonarPins بالترتيب التالي: اليسار ، اليمين ، الأمامي ، الآخرين ..
أمثلة:
1. السونار الأيمن والأيسر فقط المتصل بالدبابيس 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 سونارات متصلة بالدبابيس 1،2،3،4،5: sonarPins [] = {1،2،3،4،5}
يستخدم درع المحرك لتشغيل المحركات.
*/
const int Baud = 9600 ؛ // سرعة منفذ UART

// خصائص السونار
int sonarPins [] = {1، 2}؛ // Analog Pin Nums to sonar sensor pin AN
ثوابت طويلة MinLeftDistance = 20 ؛ // الحد الأدنى المسموح به للمسافة اليسرى
ثبات MinRightDistance = 20 ؛ // الحد الأدنى المسموح به للمسافة الصحيحة
ثابت طويل MinFrontDistance = 15 ؛ // الحد الأدنى المسموح به للمسافة الأمامية
const int SamplesAmount = 15 ؛ // المزيد من العينات - قياس أكثر سلاسة وتأخرًا أكبر
const int SonarDisplayFrequency = 10 ؛ // عرض سطر واحد فقط من هذه الأسطر - ليس كلها
int sonarDisplayFrequencyCount = 0 ؛
عامل ثابت طويل = 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 السطران التاليان إذا كان لدى Motor Shield فواصل
// const int pinRightMotorBreak = PUT_BREAK_PIN_HERE ؛ // يمكن وضع علامة على هذا على درع المحرك كـ "BREAKE A"
// const int pinLeftMotorBreak = PUT_BREAK_PIN_HERE ؛ // يمكن وضع علامة على هذا على درع المحرك كـ "BREAKE B"

// الحقول
ثابت int turnRightTimeout = 100 ؛
const int turnLeftTimeout = 150 ؛
// تعيين في عداد الوقت الذي يتم فيه تشغيل المحرك مرة أخرى: N / 10 (بالمللي ثانية)
عدد العد التنازلي أثناء تحريك الحق ؛
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 () ؛
}

// الحلقة الرئيسية
حلقة فارغة () {

checkAndSetRightSide () ؛
checkAndSetLeftSide () ؛

processRightSide () ؛
processLeftSide () ؛

تأخير (10) ؛ // كرر كل 10 مللي ثانية
}

//---------------------------------------------------
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) ؛
}

startMotors () {
setMotorSpeed ​​(pinRightMotorSpeed، 255) ؛
setMotorSpeed ​​(pinLeftMotorSpeed، 255) ؛
}

باطل waitWhileAnyBumperIsPressed () {
بينما (checkBumperIsNotPressed (pinRightBumper)
&& checkBumperIsNotPressed (pinLeftBumper)) {
تأخير (20) ؛ // تحقق كل 20 مللي ثانية
}
}

باطلة processRightSide () {
إذا تحقق (countDownWhileMovingToRight MinFrontDistance) // إذا لم يتم الوصول إلى الحد الأدنى المسموح به للمسافة الأمامية
العودة
if (checkCounterIsNotSet (countDownWhileMovingToLeft)) // إذا كان العداد لا يقوم بالعد التنازلي بعد
runLeftMotorBackward () ؛ // تشغيل المحرك الأيمن للخلف
countDownWhileMovingToLeft = turnLeftTimeout؛ // اضبط العداد على القيمة القصوى لبدء العد التنازلي
}

bool checkCounterIsNotSet (عداد دولي) {
عداد العودة = SamplesAmount)
sampleIndex [pinIndex] = 0 ؛
عينات [pinIndex] [sampleIndex [pinIndex]] = القيمة ؛
العودة صحيحة ؛
}

calculateAvarageDistance الطويل (int pinIndex) {
متوسط ​​طويل = 0 ؛
for (int i = 0؛ i متوسط ​​+ = عينات [pinIndex] [i] ؛
متوسط ​​العائد / عدد العينات ؛
}