Napravite DIY robotski usisavač na Arduino DIY

/*
Program za upravljanje robotom s dva motora.
Robot se okreće kada motori mijenjaju brzinu i smjer.
Prednji odbojnici s lijeve i desne strane otkrivaju prepreke.
Ultrazvučni sonari mogu se spojiti na analogne ulaze (testirano na LV-MaxSonar-EZ1):
- stavite igle u nizu sonarPins sljedećim redoslijedom: lijevo, desno, prednje, itd.
Primjeri:
1. samo lijevi i desni sonari spojeni na igle 2 i 3: sonarPins [] = {2,3}
2. lijevi, desni i prednji sonari spojeni na igle 2, 3 i 5: sonarPins [] = {2,3,5}
3. samo prednji sonar spojen na pin 5: sonarPins [] = {-1, -1.5}
4. samo je lijevi sonar spojen na pin 2: sonarPins [] = {2}
5. jedini desni sonar spojen na igle 3: sonarPins [] = {-1,3}
6,5 sonara spojenih na pinove 1,2,3,4,5: sonarPins [] = {1,2,3,4,5}
Motorni štit koristi se za pokretanje motora.
*/
const int Baud = 9600; // UART brzina priključka

// Sonarna svojstva
int sonarPins [] = {1, 2}; // Analogni brojevi pin na sonarnom senzoru Pin AN
minimalna minimalna udaljenost = 20; // Minimalna dopuštena lijeva udaljenost
const long MinRightDistance = 20; // Minimalna dopuštena desna udaljenost
Min.FrontDistance = 15; // Minimalno dopušteno prednje udaljenost
const int SamplesAmount = 15; // više uzoraka - glađe mjerenje i veće zaostajanje
const int SonarDisplayFrequency = 10; // prikazati samo jedan od tih redaka - ne sav
int sonarDisplayFrequencyCount = 0;
Faktor dugovanja = 2,54 / 2;
dugi uzorci [sizeof (sonarPins)] [SamplesAmount];
int sampleIndex [sizeof (sonarPins)];

// desna strana
const int pinRightMotorDirection = 4; // to se na oklopu motora može označiti kao "DIR A"
const int pinRightMotorSpeed ​​= 3; // to se na štitu motora može označiti kao "PWM A"
const int pinRightBumper = 2; // gdje je spojen desni branik

// lijeva strana
const int pinLeftMotorDirection = 7; // to se na oklopu motora može označiti kao "DIR B"
const int pinLeftMotorSpeed ​​= 6; // to se na štitu motora može označiti kao "PWM B"
const int pinLeftBumper = 8; // gdje je spojen desni branik

// komentirajte sljedeća dva retka ako je motorni štit pokvario
// const int pinRightMotorBreak = PUT_BREAK_PIN_HERE; // to se na oklopu motora može označiti kao "BREAKE A"
// const int pinLeftMotorBreak = PUT_BREAK_PIN_HERE; // to se na oklopu motora može označiti kao "BREAKE B"

// polja
const int turnRightTimeout = 100;
const int turnLeftTimeout = 150;
// postavite u brojaču koliko dugo motor radi: N / 10 (u milisekundama)
int countDownWhileMovingToRight;
int countDownWhileMovingToLeft;

// Inicijalizacija
poništavanje postave () {
Serijski.begin (Baud);
initPins ();

// komentirajte sljedeća 4 retka ako se motorni štit prijelomi
// pinMode (pinLeftMotorBreak, OUTPUT);
// pinMode (pinRightMotorBreak, OUTPUT);
// digitalWrite (pinLeftMotorBreak, LOW); // isključiti prekide
// digitalWrite (pinRightMotorBreak, LOW); // isključiti prekide

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

// Glavna petlja
void petlja () {

verifyAndSetRightSide ();
verifyAndSetLeftSide ();

processRightSide ();
processLeftSide ();

kašnjenje (10); // ponavljati svakih 10 milisekundi
}

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

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

void waitWhileAnyBumperIsPress () {
dok (checkBumperIsNotPress (pinRightBumper)
&& checkBumperIsNotPress (pinLeftBumper)) {
kašnjenje (20); // provjeravajte svakih 20 milisekundi
}
}

void processRightSide () {
ako (countDownWhileMovingToRight MinFrontDistance) // provjerava nije li dosegnuta minimalna dopuštena prednja udaljenost
povratak;
if (checkCounterIsNotSet (countDownWhileMovingToLeft)) // ako brojač još ne odbrojava
runLeftMotorBackward (); // pokrenuti desni motor unatrag
countDownWhileMovingToLeft = turnLeftTimeout; // postaviti brojač na maksimalnu vrijednost da bi se započelo odbrojavanje
}

bool checkCounterIsNotSet (int brojač) {
povratni brojač = SamplesAmount)
sampleIndex [pinIndex] = 0;
sample [pinIndex] [sampleIndex [pinIndex]] = vrijednost;
povratak istinit;
}

dugo izračunavanjeAvarageDistance (int pinIndex) {
dugi prosjek = 0;
za (int i = 0; i prosjek + = uzorci [pinIndex] [i];
povratni prosjek / SamplesAmount;
}