Robotový vysávač robotov na

/*
Program na ovládanie robota s dvoma motormi.
Robot sa otáča, keď motory menia svoju rýchlosť a smer.
Predné nárazníky na ľavej a pravej strane zisťujú prekážky.
Ultrazvukové sonary môžu byť pripojené k analógovým vstupom (testované na LV-MaxSonar-EZ1):
- umiestnite kolíky do poľa sonarPins v tomto poradí: vľavo, vpravo, vpredu, ďalšie ..
Príklady:
1. iba ľavý a pravý sonar pripojený k pinom 2 a 3: sonarPins [] = {2,3}
2. ľavý, pravý a predný sonar pripojený k pinom 2, 3 a 5: sonarPins [] = {2,3,5}
3. iba čelný sonar pripojený k pinu 5: sonarPins [] = {-1, -1,5}
4. sonar ponechaný iba na kolíku 2: sonarPins [] = {2}
5. iba pravý sonar pripojený k pinom 3: sonarPins [] = {-1,3}
6,5 sonarov spojených s kolíkmi 1,2,3,4,5: sonarPins [] = {1,2,3,4,5}
Štít motora sa používa na chod motorov.
*/
const int Baud = 9600; // Rýchlosť portu UART

// Sonarové vlastnosti
int sonarPins [] = {1, 2}; // Analógové pinové čísla na sonarový senzor Pin AN
konštanta dlhá minLeftDistance = 20; // Minimálna povolená vzdialenosť vľavo
konštanta dlhá MinRightDistance = 20; // Minimálna povolená správna vzdialenosť
konštanta dlhá MinFrontDistance = 15; // Minimálna povolená predná vzdialenosť
const int SamplesAmount = 15; // viac vzoriek - plynulejšie meranie a väčšie oneskorenie
const int SonarDisplayFrequency = 10; // zobrazí iba jeden z týchto riadkov - nie všetky
int sonarDisplayFrequencyCount = 0;
konštantný dlhý faktor = 2,54/2;
dlhé vzorky [sizeof (sonarPins)] [SamplesAmount];
int sampleIndex [sizeof (sonarPins)];

// pravá strana
const int pinRightMotorDirection = 4; // toto môže byť označené na štítku motora ako „DIR A“
const int pinRightMotorSpeed ​​= 3; // toto môže byť označené na štítku motora ako „PWM A“
const int pinRightBumper = 2; // ak je pripojený pravý nárazník

// ľavá strana
const int pinLeftMotorDirection = 7; // toto môže byť označené na štítku motora ako „DIR B“
const int pinLeftMotorSpeed ​​= 6; // toto môže byť označené na štítku motora ako „PWM B“
const int pinLeftBumper = 8; // ak je pripojený pravý nárazník

// odškrtnite ďalšie 2 riadky, ak má Motor Shield prestávky
// const int pinRightMotorBreak = PUT_BREAK_PIN_HERE; // toto môže byť označené na štítku motora ako „BREAKE A“
// const int pinLeftMotorBreak = PUT_BREAK_PIN_HERE; // toto môže byť označené na štítku motora ako „BREAKE B“

// polia
const int turnRightTimeout = 100;
const int turnLeftTimeout = 150;
// nastaví počítadlo času, počas ktorého beží motor späť: N / 10 (v milisekundách)
int countDownWhileMovingToRight;
int countDownWhileMovingToLeft;

// Inicializácia
neplatné nastavenie () {
Serial.begin (Baud);
initPins ();

// odškrtnite ďalšie 4 riadky, ak má Motor Shield prestávky
// pinMode (pinLeftMotorBreak, OUTPUT);
// pinMode (pinRightMotorBreak, OUTPUT);
// digitalWrite (pinLeftMotorBreak, LOW); // vypnite prestávky
// digitalWrite (pinRightMotorBreak, LOW); // vypnite prestávky

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

// Hlavná slučka
void loop () {

overiťAndSetRightSide ();
veriťAndSetLeftSide ();

processRightSide ();
processLeftSide ();

oneskorenie (10); // opakovanie každých 10 milisekúnd
}

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

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

void waitWhileAnyBumperIsPressed () {
while (checkBumperIsNotPressed (pinRightBumper)
&& checkBumperIsNotPressed (pinLeftBumper)) {
oneskorenie (20); // kontrola každých 20 milisekúnd
}
}

void processRightSide () {
if (countDownWhileMovingToRight MinFrontDistance) // skontroluje, či nie je dosiahnutá minimálna povolená predná vzdialenosť
return;
if (checkCounterIsNotSet (countDownWhileMovingToLeft)) // ak počítadlo ešte neodpočítava
runLeftMotorBackward (); // spustite pravý motor dozadu
countDownWhileMovingToLeft = turnLeftTimeout; // nastaví počítadlo na maximálnu hodnotu a začne odpočítavanie
}

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

dlhý výpočetAvarageDistance (int pinIndex) {
dlhý priemer = 0;
pre (int i = 0; i priemer + = vzorky [pinIndex] [i];
priemer návratnosti / množstvo vzoriek;
}