DIY DIY robot vysavač na Arduino DIY

/*
Program pro ovládání robota se dvěma motory.
Robot se otáčí, když motory mění svou rychlost a směr.
Přední nárazníky na levé a pravé straně detekují překážky.
Ultrazvukové sonary lze připojit k analogovým vstupům (testováno na LV-MaxSonar-EZ1):
- umístěte kolíky do pole sonarPins v následujícím pořadí: vlevo, vpravo, vpředu, ostatní ..
Příklady:
1. pouze levý a pravý sonar připojený k pinům 2 a 3: sonarPins [] = {2,3}
2. levé, pravé a přední sonary připojené k pinům 2, 3 a 5: sonarPins [] = {2,3,5}
3. pouze přední sonar připojený k pinu 5: sonarPins [] = {-1, -1,5}
4. SonarPins [] = {2} je připojen pouze na pin 2: sonarPins
5. pouze pravý sonar připojený k pinům 3: sonarPins [] = {-1,3}
6,5 sonarů spojených s kolíky 1,2,3,4,5: sonarPins [] = {1,2,3,4,5}
Štít motoru se používá k běhu motorů.
*/
const int Baud = 9600; // Rychlost portu UART

// Vlastnosti sonaru
int sonarPins [] = {1, 2}; // Analog Pin Nums to sonar Sensor Pin AN
const long MinLeftDistance = 20; // Minimální povolená vzdálenost vlevo
const long MinRightDistance = 20; // Minimální povolená správná vzdálenost
const long MinFrontDistance = 15; // Minimální povolená přední vzdálenost
const int SamplesAmount = 15; // více vzorků - plynulejší měření a větší zpoždění
const int SonarDisplayFrequency = 10; // zobrazí pouze jeden z těchto řádků - ne všechny
int sonarDisplayFrequencyCount = 0;
const long Factor = 2,54 / 2;
dlouhé vzorky [sizeof (sonarPins)] [SamplesAmount];
int sampleIndex [sizeof (sonarPins)];

// pravá strana
const int pinRightMotorDirection = 4; // to může být označeno na krytu motoru jako „DIR A“
const int pinRightMotorSpeed ​​= 3; // toto může být označeno na krytu motoru jako „PWM A“
const int pinRightBumper = 2; // kde je připojen pravý nárazník

// levá strana
const int pinLeftMotorDirection = 7; // toto může být označeno na krytu motoru jako „DIR B“
const int pinLeftMotorSpeed ​​= 6; // toto může být označeno na krytu motoru jako „PWM B“
const int pinLeftBumper = 8; // kde je připojen pravý nárazník

// zrušte další 2 řádky, pokud má motorový štít přestávky
// const int pinRightMotorBreak = PUT_BREAK_PIN_HERE; // to může být na štítu motoru označeno jako „BREAKE A“
// const int pinLeftMotorBreak = PUT_BREAK_PIN_HERE; // to může být označeno na krytu motoru jako „BREAKE B“

// pole
const int turnRightTimeout = 100;
const int turnLeftTimeout = 150;
// nastaví v čítači, jak dlouho motor běží zpět: N / 10 (v milisekundách)
int countDownWhileMovingToRight;
int countDownWhileMovingToLeft;

// Inicializace
neplatné nastavení () {
Serial.begin (Baud);
initPins ();

// odkomentujte další 4 řádky, pokud má motorový štít přestávky
// pinMode (pinLeftMotorBreak, OUTPUT);
// pinMode (pinRightMotorBreak, OUTPUT);
// digitalWrite (pinLeftMotorBreak, LOW); // vypněte přestávky
// digitalWrite (pinRightMotorBreak, LOW); // vypněte přestávky

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

// Hlavní smyčka
void loop () {

ověřitAndSetRightSide ();
ověřitAndSetLeftSide ();

processRightSide ();
processLeftSide ();

zpoždění (10); // opakovat každých 10 milisekund
}

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

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

void waitWhileAnyBumperIsPressed () {
while (checkBumperIsNotPressed (pinRightBumper)
&& checkBumperIsNotPressed (pinLeftBumper)) {
zpoždění (20); // kontrola každých 20 milisekund
}
}

void processRightSide () {
if (countDownWhileMovingToRight MinFrontDistance) // kontroluje, zda není dosažena minimální povolená přední vzdálenost
návrat
if (checkCounterIsNotSet (countDownWhileMovingToLeft)) // pokud čítač ještě neodpočítává
runLeftMotorBackward (); // běh pravého motoru dozadu
countDownWhileMovingToLeft = turnLeftTimeout; // nastaví počítadlo na maximální hodnotu a začne odpočítávat
}

bool checkCounterIsNotSet (int counter) {
čítač návratu = SamplesAmount)
sampleIndex [pinIndex] = 0;
samples [pinIndex] [sampleIndex [pinIndex]] = hodnota;
návrat true;
}

dlouhý výpočetAvarageDistance (int pinIndex) {
dlouhý průměr = 0;
pro (int i = 0; i průměr + = vzorky [pinIndex] [i];
průměr návratu / SamplesAmount;
}