Направи си сам прахосмукачка с робот на 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}; // Аналогови пинови номера към сонарен датчик Pin AN
const long MinLeftDistance = 20; // Минимално разрешено ляво разстояние
const long MinRightDistance = 20; // Минимално разрешено дясно разстояние
const long 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; // където е свързана дясната броня

// коментирайте следващите 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 (Бод);
initPins ();

// коментирайте следващите 4 реда, ако Motor Shield има счупвания
// pinMode (pinLeftMotorBreak, OUTPUT);
// pinMode (pinRightMotorBreak, OUTPUT);
// digitalWrite (pinLeftMotorBreak, LOW); // изключвам почивките
// digitalWrite (pinRightMotorBreak, LOW); // изключвам почивките

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

// Основен цикъл
void loop () {

verifyAndSetRightSide ();
verifyAndSetLeftSide ();

processRightSide ();
processLeftSide ();

забавяне (10); // повтаря се на всеки 10 милисекунди
}

//---------------------------------------------------
void 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);
}

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

void waitWhileAnyBumperIsPress () {
докато (checkBumperIsNotPress (pinRightBumper)
&& checkBumperIsNotPress (pinLeftBumper)) {
забавяне (20); // проверявайте на всеки 20 милисекунди
}
}

void processRightSide () {
ако (countDownWhileMovingToRight MinFrontDistance) // проверява дали минималното разрешено предно разстояние не е достигнато
се върне;
if (checkCounterIsNotSet (countDownWhileMovingToLeft)) // ако броячът все още не се отброява
runLeftMotorBackward (); // пуснете десния мотор назад
countDownWhileMovingToLeft = turnLeftTimeout; // задайте брояча на максимална стойност, за да започне отброяването му
}

bool checkCounterIsNotSet (int counter) {
връщане брояч = SamplesAmount)
sampleIndex [pinIndex] = 0;
sample [pinIndex] [sampleIndex [pinIndex]] = стойност;
връщане вярно;
}

дълъг изчисляванеAvarageDistance (int pinIndex) {
дълга средна = 0;
за (int i = 0; i среден + = проби [pinIndex] [i];
възвратна средна стойност / SamplesAmount;
}