DIY DIY Robotstofzuiger op Arduino DIY

/*
Programma voor het besturen van een robot met twee motoren.
De robot draait wanneer motoren hun snelheid en richting veranderen.
Voorbumpers aan de linker- en rechterkant detecteren obstakels.
Ultrasone sonars kunnen worden aangesloten op analoge ingangen (getest op LV-MaxSonar-EZ1):
- plaats pinnen in array sonarPins in de volgende volgorde: links, rechts, voorkant, anderen
Voorbeelden:
1. alleen linker en rechter sonars verbonden met pinnen 2 en 3: sonarPins [] = {2,3}
2. linkse, rechtse en voorste sonars aangesloten op pinnen 2, 3 en 5: sonarPins [] = {2,3,5}
3. alleen voorste sonar aangesloten op pin 5: sonarPins [] = {-1, -1.5}
4. alleen linker sonar aangesloten op pin 2: sonarPins [] = {2}
5. alleen rechter sonar aangesloten op pinnen 3: sonarPins [] = {-1,3}
6.5 sonars verbonden met pinnen 1,2,3,4,5: sonarPins [] = {1,2,3,4,5}
Motorafscherming wordt gebruikt om motoren te laten draaien.
*/
const int Baud = 9600; // UART-poortsnelheid

// Sonar-eigenschappen
int sonarPins [] = {1, 2}; // Analoge pin-nummers naar sonarsensor Pin AN
const lang MinLeftDistance = 20; // Minimaal toegestane linker afstand
const lang MinRightDistance = 20; // Minimum toegestane juiste afstand
const long MinFrontDistance = 15; // Minimaal toegestane afstand vooraan
const int SamplesAmount = 15; // meer samples - soepeler meten en grotere vertraging
const int SonarDisplayFrequency = 10; // toon slechts een van deze regels - niet allemaal
int sonarDisplayFrequencyCount = 0;
const lange factor = 2,54 / 2;
lange monsters [sizeof (sonarPins)] [SamplesAmount];
int sampleIndex [sizeof (sonarPins)];

// rechterkant
const int pinRightMotorDirection = 4; // dit kan op het motorscherm worden gemarkeerd als "DIR A"
const int pinRightMotorSpeed ​​= 3; // dit kan op het motorschild worden gemarkeerd als "PWM A"
const int pinRightBumper = 2; // waar de rechterbumper is aangesloten

// linkerkant
const int pinLeftMotorDirection = 7; // dit kan op het motorscherm worden gemarkeerd als "DIR B"
const int pinLeftMotorSpeed ​​= 6; // dit kan op het motorscherm worden gemarkeerd als "PWM B"
const int pinLeftBumper = 8; // waar de rechterbumper is aangesloten

// commentaar volgend 2 regels als Motor Shield breekt
// const int pinRightMotorBreak = PUT_BREAK_PIN_HERE; // dit kan op het motorschild worden gemarkeerd als "BREAKE A"
// const int pinLeftMotorBreak = PUT_BREAK_PIN_HERE; // dit kan op het motorscherm worden gemarkeerd als "BREAK B"

// velden
const int turnRightTimeout = 100;
const int turnLeftTimeout = 150;
// stel in teller hoe lang een motor terugloopt: N / 10 (in milliseconden)
int countDownWhileMovingToRight;
int countDownWhileMovingToLeft;

// Initialisatie
ongeldige setup () {
Serial.begin (Baud);
initPins ();

// uncomment volgende 4 regels als Motor Shield breekt
// pinMode (pinLeftMotorBreak, OUTPUT);
// pinMode (pinRightMotorBreak, OUTPUT);
// digitalWrite (pinLeftMotorBreak, LOW); // pauzes uitschakelen
// digitalWrite (pinRightMotorBreak, LOW); // pauzes uitschakelen

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

// Hoofdlus
leegte lus () {

verifAndSetRightSide ();
verifAndSetLeftSide ();

processRightSide ();
processLeftSide ();

delay (10); // Herhaal elke 10 milliseconden
}

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

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

ongeldig waitWhileAnyBumperIsPressed () {
while (checkBumperIsNotPressed (pinRightBumper)
&& checkBumperIsNotPressed (pinLeftBumper)) {
delay (20); // Controleer elke 20 milliseconden
}
}

nietig processRightSide () {
if (countDownWhileMovingToRight MinFrontDistance) // controleert of de minimaal toegestane frontafstand niet wordt bereikt
terugkeer
if (checkCounterIsNotSet (countDownWhileMovingToLeft)) // als de teller nog niet aftelt
runLeftMotorBackward (); // Draai de rechtermotor achteruit
countDownWhileMovingToLeft = turnLeftTimeout; // Zet de teller op maximale waarde om het aftellen te starten
}

bool checkCounterIsNotSet (int counter) {
return teller = SamplesAmount)
sampleIndex [pinIndex] = 0;
samples [pinIndex] [sampleIndex [pinIndex]] = waarde;
waar terugkeren;
}

long calculAvarageDistance (int pinIndex) {
lang gemiddelde = 0;
voor (int i = 0; i gemiddeld + = monsters [pinIndex] [i];
retourgemiddelde / SamplesAmount;
}