Aspirator robot robot de bricolaj pe Arduino DIY

/*
Program pentru controlul unui robot cu două motoare.
Robotul se transformă atunci când motoarele își schimbă viteza și direcția.
Barele frontale din partea stângă și din dreapta detectează obstacole.
Sunetele cu ultrasunete pot fi conectate la intrări analogice (testate pe LV-MaxSonar-EZ1):
- puneți știfturile în rândul sonarPins în ordinea următoare: stânga, dreapta, față, altele ..
Exemple:
1. numai sunete stânga și dreapta conectate la pinii 2 și 3: sonarPins [] = {2,3}
2. Sonars stânga, dreapta și față conectate la pinii 2, 3 și 5: sonarPins [] = {2,3,5}
3. numai sonar frontal conectat la pinul 5: sonarPins [] = {-1, -1,5}
4. doar sonar stânga conectat la pinul 2: sonarPins [] = {2}
5. doar sonar drept conectat la pinii 3: sonarPins [] = {-1,3}
6,5 sunete conectate la pinii 1,2,3,4,5: sonarPins [] = {1,2,3,4,5}
Scutul motorului este utilizat pentru rularea motoarelor.
*/
const int Baud = 9600; // Viteza portului UART

// Proprietăți sonore
int sonarPins [] = {1, 2}; // Numerele PIN analogice pentru senzorul sonar Pin AN
const long MinLeftDistance = 20; // Distanța minimă permisă la stânga
const long MinRightDistance = 20; // Distanța minimă permisă
const long MinFrontDistance = 15; // Distanța minimă admisă față
const int SamplesAmount = 15; // mai multe probe - măsurare mai lină și lag mai mare
const int SonarDisplayFrequency = 10; // afișează doar una dintre aceste linii - nu toate
int sonarDisplayFrequencyCount = 0;
const lung Factor = 2,54 / 2;
mostre lungi [sizeof (sonarPins)] [SamplesAmount];
int sampleIndex [sizeof (sonarPins)];

// partea dreapta
const int pinRightMotorDirection = 4; // acest lucru poate fi marcat pe scutul motorului drept "DIR A"
const int pinRightMotorSpeed ​​= 3; // acest lucru poate fi marcat pe scutul motorului ca "PWM A"
const int pinRightBumper = 2; // unde este conectat bara de protecție dreapta

// partea stanga
const int pinLeftMotorDirection = 7; // acest lucru poate fi marcat pe scutul motorului drept "DIR B"
const int pinLeftMotorSpeed ​​= 6; // acest lucru poate fi marcat pe scutul motorului ca "PWM B"
const int pinLeftBumper = 8; // unde este conectat bara de protecție dreapta

// dezacord următoarele 2 linii dacă Motor Shield are pauze
// const int pinRightMotorBreak = PUT_BREAK_PIN_HERE; // acest lucru poate fi marcat pe scutul motorului ca "BREAKE A"
// const int pinLeftMotorBreak = PUT_BREAK_PIN_HERE; // acest lucru poate fi marcat pe scutul motorului ca "BREAKE B"

// câmpuri
const int turnRightTimeout = 100;
const int turnLeftTimeout = 150;
// setează în contorizarea timpului în care un motor funcționează înapoi: N / 10 (în milisecunde)
int countDownWhileMovingToRight;
int countDownWhileMovingToLeft;

// Inițializare
void setup () {
Serial.begin (Baud);
initPins ();

// dezacord următoarele 4 linii dacă Motor Shield are pauze
// pinMode (pinLeftMotorBreak, OUTPUT);
// pinMode (pinRightMotorBreak, OUTPUT);
// digitalWrite (pinLeftMotorBreak, LOW); // opri pauzele
// digitalWrite (pinRightMotorBreak, LOW); // opri pauzele

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

// Bucla principală
void loop () {

verifyAndSetRightSide ();
verifyAndSetLeftSide ();

processRightSide ();
processLeftSide ();

întârziere (10); // repetați la fiecare 10 milisecunde
}

//---------------------------------------------------
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 waitWhileAnyBumperIsPressed () {
while (checkBumperIsNotPressed (pinRightBumper)
&& checkBumperIsNotPressed (pinLeftBumper)) {
întârziere (20); // verificați la fiecare 20 de milisecunde
}
}

void processRightSide () {
if (countDownWhileMovingToRight MinFrontDistance) // verifică dacă distanța minimă admisă nu este atinsă
return;
if (checkCounterIsNotSet (countDownWhileMovingToLeft)) // dacă contorul nu contează încă
runLeftMotorBackward (); // rulați motorul drept înapoi
countDownWhileMovingToLeft = turnLeftTimeout; // setați contorul la valoarea maximă pentru a-l începe să numere în jos
}

bool checkCounterIsNotSet (int counter) {
contor retur = SamplesAmount)
sampleIndex [pinIndex] = 0;
mostre [pinIndex] [sampleIndex [pinIndex]] = valoare;
întoarce-te adevărat;
}

lung calculAvarageDistance (int pinIndex) {
medie lungă = 0;
for (int i = 0; i medie + = mostre [pinIndex] [i];
rentabilitate medie / SamplesAmount;
}