DIY DIY Robot Odkurzacz na Arduino DIY

/*
Program do sterowania robotem z dwoma silnikami.
Robot obraca się, gdy silniki zmieniają prędkość i kierunek.
Przednie zderzaki po lewej i prawej stronie wykrywają przeszkody.
Sonary ultradźwiękowe można podłączyć do wejść analogowych (testowane na LV-MaxSonar-EZ1):
- umieść szpilki w szyku sonarowym Szpilki w następującej kolejności: lewy, prawy, przód, inne ..
Przykłady:
1. tylko lewy i prawy sonar podłączony do pinów 2 i 3: sonarPins [] = {2,3}
2. sonary lewy, prawy i przedni podłączone do pinów 2, 3 i 5: sonarPins [] = {2,3,5}
3. tylko przedni sonar podłączony do pinu 5: sonarPins [] = {-1, -1.5}
4. pozostawiono tylko sonar podłączony do pinu 2: sonarPins [] = {2}
5. tylko prawy sonar podłączony do pinów 3: sonarPins [] = {-1,3}
6,5 sonary podłączone do pinów 1,2,3,4,5: sonarPins [] = {1,2,3,4,5}
Osłona silnika służy do uruchamiania silników.
*/
const int Baud = 9600; // Szybkość portu UART

// Właściwości sonaru
int sonarPins [] = {1, 2}; // Analog Pin Nums do sonaru sondy Pin AN
const long MinLeftDistance = 20; // Minimalna dozwolona lewa odległość
const long MinRightDistance = 20; // Minimalna dozwolona właściwa odległość
const long MinFrontDistance = 15; // Minimalna dozwolona odległość od przodu
const int SamplesAmount = 15; // więcej próbek - płynniejszy pomiar i większe opóźnienie
const int SonarDisplayFrequency = 10; // wyświetla tylko jedną z tych linii - nie wszystkie
int sonarDisplayFrequencyCount = 0;
const long Factor = 2,54 / 2;
długie próbki [sizeof (sonarPins)] [SamplesAmount];
int sampleIndex [sizeof (sonarPins)];

// prawa strona
const int pinRightMotorDirection = 4; // można to oznaczyć na ekranie silnika jako „DIR A”
const int pinRightMotorSpeed ​​= 3; // można to oznaczyć na ekranie silnika jako „PWM A”
const int pinRightBumper = 2; // gdzie podłączony jest prawy zderzak

// lewa strona
const int pinLeftMotorDirection = 7; // można to oznaczyć na ekranie silnika jako „DIR B”
const int pinLeftMotorSpeed ​​= 6; // można to oznaczyć na ekranie silnika jako „PWM B”
const int pinLeftBumper = 8; // gdzie podłączony jest prawy zderzak

// odkomentuj następne 2 linie, jeśli osłona silnika ma pęknięcia
// const int pinRightMotorBreak = PUT_BREAK_PIN_HERE; // można to oznaczyć na tarczy silnika jako „BREAKE A”
// const int pinLeftMotorBreak = PUT_BREAK_PIN_HERE; // można to oznaczyć na tarczy silnika jako „BREAKE B”

// pola
const int turnRightTimeout = 100;
const int turnLeftTimeout = 150;
// ustaw licznik, jak długo silnik się cofa: N / 10 (w milisekundach)
int countDownWhileMovingToRight;
int countDownWhileMovingToLeft;

// Inicjalizacja
void setup () {
Serial.begin (Baud);
initPins ();

// odkomentuj następne 4 linie, jeśli osłona silnika ma pęknięcia
// pinMode (pinLeftMotorBreak, OUTPUT);
// pinMode (pinRightMotorBreak, OUTPUT);
// digitalWrite (pinLeftMotorBreak, LOW); // wyłącz przerwy
// digitalWrite (pinRightMotorBreak, LOW); // wyłącz przerwy

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

// Pętla główna
void loop () {

replaceAndSetRightSide ();
replaceAndSetLeftSide ();

processRightSide ();
processLeftSide ();

opóźnienie (10); // powtarzanie co 10 milisekund
}

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

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

void waitWhileAnyBumperIsPressed () {
while (checkBumperIsNotPressed (pinRightBumper)
&& checkBumperIsNotPressed (pinLeftBumper)) {
opóźnienie (20); // sprawdzanie co 20 milisekund
}
}

void processRightSide () {
if (countDownWhileMovingToRight MinFrontDistance) // sprawdza, czy nie została osiągnięta minimalna dozwolona odległość od przodu
powrót
if (checkCounterIsNotSet (countDownWhileMovingToLeft)) // jeśli licznik jeszcze nie odlicza
runLeftMotorBackward (); // uruchom prawy silnik do tyłu
countDownWhileMovingToLeft = turnLeftTimeout; // ustaw licznik na maksymalną wartość, aby rozpocząć odliczanie
}

bool checkCounterIsNotSet (licznik int) {
return counter = SamplesAmount)
sampleIndex [pinIndex] = 0;
sample [pinIndex] [sampleIndex [pinIndex]] = wartość;
zwróć prawdę;
}

długo obliczyćAvarageDistance (int pinIndex) {
długa średnia = 0;
for (int i = 0; i średnia + = próbki [pinIndex] [i];
zwracana średnia / SamplesAmount;
}