Robotu putekļsūcējs DIY DIY

/*
Robota ar diviem motoriem vadības programma.
Robots pagriežas, kad motori maina ātrumu un virzienu.
Priekšējie buferi kreisajā un labajā pusē nosaka šķēršļus.
Ultraskaņas sonārus var savienot ar analogo ieeju (pārbaudīta uz LV-MaxSonar-EZ1):
- ielieciet tapas ar hidrolokatoru tabulā šādā secībā: pa kreisi, pa labi, priekšā, citi.
Piemēri:
1. tikai kreisās un labās puses hidrolokatori, kas savienoti ar 2. un 3. tapu: sonarPins [] = {2,3}
2. kreisās, labās un priekšējās daļiņas, kas savienotas ar 2., 3. un 5. tapu: sonarPins [] = {2,3,5}
3. tikai priekšējais sonārs, kas savienots ar 5. tapu: sonarPins [] = {-1, -1.5}
4. tikai kreisais sonārs, kas savienots ar 2. tapu: sonarPins [] = {2}
5. tikai labais hidrolokatoru savienots ar 3. tapām: sonarPins [] = {-1,3}
6,5 sonāri, kas savienoti ar tapām 1,2,3,4,5: sonarPins [] = {1,2,3,4,5}
Motoru vairogs tiek izmantots motoru vadīšanai.
*/
const int Baud = 9600; // UART porta ātrums

// Sonāra īpašības
int sonarPins [] = {1, 2}; // Analogu tapu skaitļi sonāra sensoram PIN AN
const long MinLeftDistance = 20; // Minimālais pieļaujamais kreisais attālums
const long MinRightDistance = 20; // Minimālais pieļaujamais pareizais attālums
const long MinFrontDistance = 15; // Minimālais pieļaujamais priekšējais attālums
const int SamplesAmount = 15; // vairāk paraugu - vienmērīgāks mērījums un lielāks nobīde
const int SonarDisplayFrequency = 10; // parādīt tikai vienu no šīm rindām - ne visas
int sonarDisplayFrequencyCount = 0;
const long Factor = 2,54 / 2;
garie paraugi [izmērs (sonarPins)] [SamplesAmount];
int sampleIndex [lielums (sonarPins)];

// labā puse
const int pinRightMotorDirection = 4; // to var marķēt uz motora vairoga kā "DIR A"
const int pinRightMotorSpeed ​​= 3; // to var norādīt uz motora vairoga kā "PWM A"
const int pinRightBumper = 2; // kur ir savienots labais buferis

// kreisā puse
const int pinLeftMotorDirection = 7; // to var marķēt uz motora vairoga kā "DIR B"
const int pinLeftMotorSpeed ​​= 6; // to var marķēt uz motora vairoga kā "PWM B"
const int pinLeftBumper = 8; // kur ir savienots labais buferis

// nekomentē nākamās 2 līnijas, ja Motor Shield ir pārtraukumi
// const int pinRightMotorBreak = PUT_BREAK_PIN_HERE; // to var marķēt uz motora vairoga kā "BREAKE A"
// const int pinLeftMotorBreak = PUT_BREAK_PIN_HERE; // to var marķēt uz motora vairoga kā "BREAKE B"

// lauki
const int turnRightTimeout = 100;
const int turnLeftTimeout = 150;
// iestatiet skaitītājā, cik ilgi motors darbojas atpakaļ: N / 10 (milisekundēs)
int countDownWhileMovingToRight;
int countDownWhileMovingToLeft;

// Inicializācija
Nederīga iestatīšana () {
Serial.begin (Baud);
initPins ();

// nekomentē nākamās 4 līnijas, ja Motor Shield ir pārtraukumi
// pinMode (pinLeftMotorBreak, OUTPUT);
// pinMode (pinRightMotorBreak, OUTPUT);
// digitalWrite (pinLeftMotorBreak, LOW); // izslēgt pārtraukumus
// digitalWrite (pinRightMotorBreak, LOW); // izslēgt pārtraukumus

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

// Galvenā cilpa
tukšs cilpa () {

pārbaudeAndSetRightSide ();
pārbaudeAndSetLeftSide ();

processRightSide ();
processLeftSide ();

kavēšanās (10); // atkārtojiet ik pēc 10 milisekundēm
}

//---------------------------------------------------
spēkā neesoši initPins () {
pinMode (pinRightMotorDirection, OUTPUT);
pinMode (pinRightMotorSpeed, OUTPUT);
pinMode (pinRightBumper, INPUT);
pinMode (pinLeftMotorDirection, OUTPUT);
pinMode (pinLeftMotorSpeed, OUTPUT);
pinMode (pinLeftBumper, INPUT);
priekš (int i = 0; i pinMode (sonarPins [i], INPUT);
}

spēkā neesošs startMotors () {
setMotorSpeed ​​(pinRightMotorSpeed, 255);
setMotorSpeed ​​(pinLeftMotorSpeed, 255);
}

void waitWhileAnyBumperIsPressed () {
kamēr (checkBumperIsNotPressed (pinRightBumper)
&& checkBumperIsNotPressed (pinLeftBumper)) {
kavēšanās (20); // jāpārbauda ik pēc 20 milisekundēm
}
}

spēkā neesošs processRightSide () {
if (countDownWhileMovingToRight MinFrontDistance) // pārbauda, ​​vai nav sasniegts minimālais pieļaujamais priekšējais attālums
atgriezties
if (checkCounterIsNotSet (countDownWhileMovingToLeft)) // ja skaitītājs vēl netiek skaitīts
runLeftMotorBackward (); // darbiniet labo motoru atpakaļ
countDownWhileMovingToLeft = turnLeftTimeout; // iestatiet skaitītāju uz maksimālo vērtību, lai sāktu tā skaitīšanu
}

bool checkCounterIsNotSet (iekšējais skaitītājs) {
atgriezes skaitītājs = SamplesAmount)
sampleIndex [pinIndex] = 0;
paraugi [pinIndex] [sampleIndex [pinIndex]] = vērtība;
atgriezties taisnība;
}

garš calcAvarageDistance (int pinIndex) {
garš vidējais = 0;
priekš (int i = 0; i vidējais + = paraugi [pinIndex] [i];
vidējais atgriešanās / paraugiApmērs;
}