Направите ДИИ роботски усисивач на Ардуино уради сам

/*
Програм за контролу робота са два мотора.
Робот се окреће када мотори мењају брзину и смер.
Предњи одбојници с лијеве и десне стране откривају препреке.
Ултразвучни сонари се могу повезати на аналогне улазе (тестирани на ЛВ-МакСонар-ЕЗ1):
- ставите игле у низу сонарПинс следећим редоследом: лево, десно, предње, итд.
Примери:
1. само леви и десни сонари повезани на игле 2 и 3: сонарПинс [] = {2,3}
2. леви, десни и предњи сонари повезани на игле 2, 3 и 5: сонарПинс [] = {2,3,5}
3. само предњи сонар спојен на пин 5: сонарПинс [] = {-1, -1.5}
4. само леви сонар спојен на пин 2: сонарПинс [] = {2}
5. једини десни сонар спојен на игле 3: сонарПинс [] = {-1,3}
6,5 сонара повезаних на игле 1,2,3,4,5: сонарПинс [] = {1,2,3,4,5}
Моторни штит користи се за покретање мотора.
*/
цонст инт Бауд = 9600; // УАРТ брзина порта

// Сонарна својства
инт сонарПинс [] = {1, 2}; // Аналогни пинови са сонарским сензором Пин АН
цонст лонг МинЛефтДистанце = 20; // Минимално дозвољено лево растојање
цонст лонг МинРигхтДистанце = 20; // Минимално дозвољено право растојање
Мин.ФронтДистанце = 15; // Минимално дозвољено предње растојање
цонст инт СамплесАмоунт = 15; // више узорака - глађе мерење и веће заостајање
цонст инт СонарДисплаиФрекуенци = 10; // приказује само један од ових редова - не сви
инт сонарДисплаиФрекуенциЦоунт = 0;
Фактор дужине = 2,54 / 2;
дуги узорци [сизеоф (сонарПинс)] [СамплесАмоунт];
инт самплеИндек [сизеоф (сонарПинс)];

// десна страна
цонст инт пинРигхтМоторДирецтион = 4; // ово се на оклопу мотора може означити као "ДИР А"
цонст инт пинРигхтМоторСпеед = 3; // ово се на оклопу мотора може означити као "ПВМ А"
цонст инт пинРигхтБумпер = 2; // где је спојен десни браник

// лева страна
цонст инт пинЛефтМоторДирецтион = 7; // ово се на оклопу мотора може означити као "ДИР Б"
цонст инт пинЛефтМоторСпеед = 6; // ово се на оклопу мотора може означити као "ПВМ Б"
цонст инт пинЛефтБумпер = 8; // где је спојен десни браник

// коментирајте сљедећа два реда ако Мотор Схиелд има пукнуће
// цонст инт пинРигхтМоторБреак = ПУТ_БРЕАК_ПИН_ХЕРЕ; // ово се на оклопу мотора може означити као "БРЕАКЕ А"
// цонст инт пинЛефтМоторБреак = ПУТ_БРЕАК_ПИН_ХЕРЕ; // ово се на оклопу мотора може означити као "БРЕАКЕ Б"

// поља
цонст инт турнРигхтТимеоут = 100;
цонст инт турнЛефтТимеоут = 150;
// подесите у бројачу колико дуго мотор ради: Н / 10 (у милисекундама)
инт цоунтДовнВхилеМовингТоРигхт;
инт цоунтДовнВхилеМовингТоЛефт;

// Иницијализација
воид сетуп () {
Сериал.бегин (Бауд);
инитПинс ();

// коментирајте наредна 4 реда ако моторни штит има провале
// пинМоде (пинЛефтМоторБреак, ОУТПУТ);
// пинМоде (пинРигхтМоторБреак, ОУТПУТ);
// дигиталВрите (пинЛефтМоторБреак, ЛОВ); // искључити прекиде
// дигиталВрите (пинРигхтМоторБреак, ЛОВ); // искључити прекиде

рунРигхтМоторФорвард ();
рунЛефтМоторФорвард ();
стартМоторс ();
}

// Главна петља
воид лооп () {

верифиАндСетРигхтСиде ();
верифиАндСетЛефтСиде ();

процессРигхтСиде ();
процессЛефтСиде ();

кашњење (10); // понављајте сваких 10 милисекунди
}

//---------------------------------------------------
воид инитПинс () {
пинМоде (пинРигхтМоторДирецтион, ОУТПУТ);
пинМоде (пинРигхтМоторСпеед, ОУТПУТ);
пинМоде (пинРигхтБумпер, ИНПУТ);
пинМоде (пинЛефтМоторДирецтион, ОУТПУТ);
пинМоде (пинЛефтМоторСпеед, ОУТПУТ);
пинМоде (пинЛефтБумпер, ИНПУТ);
фор (инт и = 0; и <сизеоф (сонарПинс); и ++)
пинМоде (сонарПинс [и], ИНПУТ);
}

воид стартМоторс () {
сетМоторСпеед (пинРигхтМоторСпеед, 255);
сетМоторСпеед (пинЛефтМоторСпеед, 255);
}

воид ваитВхилеАниБумперИсПресс () {
док (цхецкБумперИсНотПресс (пинРигхтБумпер)
&& цхецкБумперИсНотПресс (пинЛефтБумпер) {
кашњење (20); // проверавати сваких 20 милисекунди
}
}

воид процессРигхтСиде () {
иф (цоунтДовнВхилеМовингТоРигхт МинФронтДистанце) // проверава да ли није достигнута минимална дозвољена удаљеност
повратак
иф (цхецкЦоунтерИсНотСет (цоунтДовнВхилеМовингТоЛефт)) // ако бројач још не одбројава
рунЛефтМоторБацквард (); // покрените десни мотор уназад
цоунтДовнВхилеМовингТоЛефт = турнЛефтТимеоут; // постави бројач на максималну вредност да би се започело одбројавање
}

боол цхецкЦоунтерИсНотСет (инт бројач) {
повратни бројач = СамплесАмоунт)
самплеИндек [пинИндек] = 0;
сампле [пинИндек] [самплеИндек [пинИндек]] = вредност;
повратак истинит;
}

дуга калкулацијаАварагеДистанце (инт пинИндек) {
дуги просек = 0;
фор (инт и = 0; и <СамплесАмоунт; и ++)
просек + = узорци [пинИндек] [и];
повратни просек / СамплесАмоунт;
}