Aspirapolvere robot fai-da-te fai-da-te su Arduino DIY

/*
Programma per il controllo di un robot con due motori.
Il robot gira quando i motori cambiano la loro velocità e direzione.
I paraurti anteriori sui lati sinistro e destro rilevano ostacoli.
I sonar a ultrasuoni possono essere collegati agli ingressi analogici (testati su LV-MaxSonar-EZ1):
- metti i pin in array sonarPin nel seguente ordine: sinistra, destra, fronte, altri ..
Esempi:
1. solo sonar sinistro e destro collegati ai pin 2 e 3: sonarPins [] = {2,3}
2. ecoscandagli sinistro, destro e anteriore collegati ai pin 2, 3 e 5: sonarPins [] = {2,3,5}
3. solo il sonar anteriore collegato al pin 5: sonarPins [] = {-1, -1.5}
4. ha lasciato solo il sonar collegato al pin 2: sonarPins [] = {2}
5. solo il sonar destro collegato ai pin 3: sonarPins [] = {-1,3}
6.5 sonar collegati ai pin 1,2,3,4,5: sonarPins [] = {1,2,3,4,5}
Lo schermo del motore viene utilizzato per far funzionare i motori.
*/
const int Baud = 9600; // Velocità della porta UART

// Proprietà del sonar
int sonarPins [] = {1, 2}; // Numeri pin analogici al pin AN del sensore sonar
const long MinLeftDistance = 20; // Distanza minima consentita
const long MinRightDistance = 20; // Distanza minima consentita
const long MinFrontDistance = 15; // Distanza frontale minima consentita
const int SamplesAmount = 15; // più campioni: misurazione più fluida e ritardo maggiore
const int SonarDisplayFrequency = 10; // visualizza solo una di queste righe, non tutte
int sonarDisplayFrequencyCount = 0;
fattore lungo const = 2,54 / 2;
campioni lunghi [sizeof (sonarPins)] [SamplesAmount];
int sampleIndex [sizeof (sonarPins)];

// lato destro
const int pinRightMotorDirection = 4; // questo può essere contrassegnato sulla protezione del motore come "DIR A"
const int pinRightMotorSpeed ​​= 3; // questo può essere contrassegnato sulla protezione del motore come "PWM A"
const int pinRightBumper = 2; // dove è collegato il paraurti destro

// lato sinistro
const int pinLeftMotorDirection = 7; // questo può essere contrassegnato sulla protezione del motore come "DIR B"
const int pinLeftMotorSpeed ​​= 6; // questo può essere contrassegnato sulla protezione del motore come "PWM B"
const int pinLeftBumper = 8; // dove è collegato il paraurti destro

// decommenta le 2 righe successive se Motor Shield presenta delle interruzioni
// const int pinRightMotorBreak = PUT_BREAK_PIN_HERE; // questo può essere contrassegnato sulla protezione del motore come "BREAKE A"
// const int pinLeftMotorBreak = PUT_BREAK_PIN_HERE; // questo può essere contrassegnato sulla protezione del motore come "BREAKE B"

// campi
const int turnRightTimeout = 100;
const int turnLeftTimeout = 150;
// imposta in contro quanto tempo un motore sta tornando indietro: N / 10 (in millisecondi)
int countDownWhileMovingToRight;
int countDownWhileMovingToLeft;

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

// decommenta le 4 righe successive se Motor Shield presenta delle interruzioni
// pinMode (pinLeftMotorBreak, OUTPUT);
// pinMode (pinRightMotorBreak, OUTPUT);
// digitalWrite (pinLeftMotorBreak, LOW); // disattiva le pause
// digitalWrite (pinRightMotorBreak, LOW); // disattiva le pause

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

// Ciclo principale
void loop () {

confirmAndSetRightSide ();
confirmAndSetLeftSide ();

processRightSide ();
processLeftSide ();

delay (10); // ripeti ogni 10 millisecondi
}

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

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

void waitWhileAnyBumperIsPressed () {
while (checkBumperIsNotPressed (pinRightBumper)
&& checkBumperIsNotPressed (pinLeftBumper)) {
ritardo (20); // verifica ogni 20 millisecondi
}
}

void processRightSide () {
if (countDownWhileMovingToRight MinFrontDistance) // controlla se la distanza frontale minima consentita non viene raggiunta
tornare;
if (checkCounterIsNotSet (countDownWhileMovingToLeft)) // se il contatore non sta ancora eseguendo il conto alla rovescia
runLeftMotorBackward (); // esegue il motore destro all'indietro
countDownWhileMovingToLeft = turnLeftTimeout; // imposta il contatore sul valore massimo per iniziare il conto alla rovescia
}

bool checkCounterIsNotSet (int counter) {
return counter = SamplesAmount)
sampleIndex [pinIndex] = 0;
samples [pinIndex] [sampleIndex [pinIndex]] = valore;
ritorno vero;
}

long compareAvarageDistance (int pinIndex) {
media lunga = 0;
per (int i = 0; i media + = campioni [pinIndex] [i];
return return / SamplesAmount;
}