Robotas dulkių siurblys „pasidaryk pats“ pasidaryk pats „pasidaryk pats“

/*
Roboto su dviem varikliais valdymo programa.
Robotas sukasi, kai varikliai keičia savo greitį ir kryptį.
Priekiniai buferiai kairėje ir dešinėje pusėse nustato kliūtis.
Ultragarsinius sonarus galima prijungti prie analoginių įėjimų (išbandytų LV-MaxSonar-EZ1):
- įkiškite kaiščius į matricos sonarinius kaiščius tokia tvarka: kairėn, dešinėn, priekyje, kiti ..
Pavyzdžiai:
1. tik kairiieji ir dešinieji sonai, sujungti su 2 ir 3 kaiščiais: sonarPins [] = {2,3}
2. kairysis, dešinysis ir priekiniai sonai, sujungti su 2, 3 ir 5 kaiščiais: sonarPins [] = {2,3,5}
3. tik priekinis sonaras, prijungtas prie 5 kaiščio: sonarPins [] = {-1, -1.5}
4. tik 2 kaištis prijungtas prie kairiojo sonaro: sonarPins [] = {2}
5. tik dešinysis sonaras, prijungtas prie 3 kaiščių: sonarPins [] = {-1,3}
6.5 kaiščiai sujungti su kaiščiais 1,2,3,4,5: sonarPins [] = {1,2,3,4,5}
Variklio skydas naudojamas varikliams paleisti.
*/
const int Baud = 9600; // UART prievado greitis

// Sonaro savybės
int sonarPins [] = {1, 2}; // Analoginiai PIN kodai sonaro jutiklio kaiščiui AN
const long MinLeftDistance = 20; // Mažiausias leidžiamas kairysis atstumas
const long MinRightDistance = 20; // Mažiausias leistinas tinkamas atstumas
const long MinFrontDistance = 15; // Mažiausias leidžiamas priekinis atstumas
const int SamplesAmount = 15; // daugiau mėginių - sklandesnis matavimas ir didesnis atsilikimas
const int SonarDisplayFrequency = 10; // rodyti tik vieną iš šių eilučių - ne visas
int sonarDisplayFrequencyCount = 0;
ilgio koeficientas = 2,54 / 2;
ilgi mėginiai [dydis (sonarPins)] [SamplesAmount];
int sampleIndex [dydisof (sonarPins)];

// dešinė pusė
const int pinRightMotorDirection = 4; // ant variklio skydo tai galima pažymėti kaip "DIR A"
const int pinRightMotorSpeed ​​= 3; // ant variklio skydo tai galima pažymėti kaip "PWM A"
const int pinRightBumper = 2; // kur prijungtas dešinysis buferis

// kairioji pusė
const int pinLeftMotorDirection = 7; // ant variklio skydo tai galima pažymėti kaip "DIR B"
const int pinLeftMotorSpeed ​​= 6; // ant variklio skydo tai galima pažymėti kaip "PWM B"
const int pinLeftBumper = 8; // kur prijungtas dešinysis buferis

// nekomentuoti kitų 2 eilučių, jei „Motor Shield“ turi pertraukų
// const int pinRightMotorBreak = PUT_BREAK_PIN_HERE; // ant variklio skydelio tai galima pažymėti kaip "BREAKE A"
// const int pinLeftMotorBreak = PUT_BREAK_PIN_HERE; // tai ant variklio skydo galima pažymėti kaip "BREAKE B"

// laukai
const int turnRightTimeout = 100;
const int turnLeftTimeout = 150;
// nustatykite, kiek laiko variklis sukasi atgal: N / 10 (milisekundėmis)
int countDownWhileMovingToRight;
int countDownWhileMovingToLeft;

// Iniciacija
negaliojanti sąranka () {
Serialas.beginas (Baudas);
initPins ();

// nekomentuokite kitų 4 eilučių, jei „Motor Shield“ turi pertraukų
// pinMode (pinLeftMotorBreak, OUTPUT);
// pinMode (pinRightMotorBreak, OUTPUT);
// „digitalWrite“ („pinLeftMotorBreak“, LOW); // išjungti pertraukėles
// „digitalWrite“ („pinRightMotorBreak“, LOW); // išjungti pertraukėles

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

// Pagrindinė kilpa
tuščia kilpa () {

patikrintiAndSetRightSide ();
„testAndSetLeftSide“ ();

„processRightSide“ ();
processLeftSide ();

atidėjimas (10); // pakartokite kas 10 milisekundžių
}

//---------------------------------------------------
negaliojantys „initPins“ () {
pinMode (pinRightMotorDirection, OUTPUT);
pinMode (pinRightMotorSpeed, OUTPUT);
pinMode (pinRightBumper, INPUT);
pinMode (pinLeftMotorDirection, OUTPUT);
pinMode (pinLeftMotorSpeed, OUTPUT);
pinMode (pinLeftBumper, INPUT);
už (int i = 0; i pinMode (sonarPins [i], INPUT);
}

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

void waitWhileAnyBumperIsPressed () {
tuo tarpu (checkBumperIsNotPressed (pinRightBumper)
&& checkBumperIsNotPressed (pinLeftBumper)) {
atidėjimas (20); // patikrinkite kas 20 milisekundžių
}
}

negaliojantis procesasRightSide () {
if (countDownWhileMovingToRight MinFrontDistance) // patikrina, ar nepasiektas mažiausias leidžiamas priekinis atstumas
grįžti
if (checkCounterIsNotSet (countDownWhileMovingToLeft)) // jei skaitiklis dar neskaičiuojamas
runLeftMotorBackward (); // paleiskite dešinįjį variklį atgal
countDownWhileMovingToLeft = turnLeftTimeout; // nustatykite skaitiklį į didžiausią vertę, kad pradėtumėte jo skaičiavimą
}

bool checkCounterIsNotSet (vidinis skaitiklis) {
grąžinimo skaitiklis = SamplesAmount)
„sampleIndex“ [pinIndex] = 0;
pavyzdžiai [pinIndex] [sampleIndex [pinIndex]] = reikšmė;
grįžti tiesa;
}

ilgas apskaičiuotiAvarageDistance (int pinIndex) {
ilgas vidurkis = 0;
už (int i = 0; i vidutinis + = mėginių [pinIndex] [i];
grąžinimo vidurkis / SamplesAmount;
}