Aspirador DIY DIY Robot en Arduino DIY

/*
Programa para controlar un robot con dos motores.
El robot gira cuando los motores cambian su velocidad y dirección.
Los parachoques delanteros en los lados izquierdo y derecho detectan obstáculos.
Las sondas ultrasónicas se pueden conectar a entradas analógicas (probadas en LV-MaxSonar-EZ1):
- coloca los pines en la matriz sonarPins en el siguiente orden: izquierdo, derecho, frontal, otros ...
Ejemplos:
1. solo sonares izquierdo y derecho conectados a los pines 2 y 3: sonarPins [] = {2,3}
2. sonares izquierdo, derecho y frontal conectados a los pines 2, 3 y 5: sonarPins [] = {2,3,5}
3. solo sonda frontal conectada al pin 5: sonarPins [] = {-1, -1.5}
4. solo dejó la sonda conectada al pin 2: sonarPins [] = {2}
5. solo sonda derecha conectada a los pines 3: sonarPins [] = {-1,3}
6.5 sonares conectados a los pines 1,2,3,4,5: sonarPins [] = {1,2,3,4,5}
El escudo del motor se utiliza para hacer funcionar motores.
*/
const int Baud = 9600; // velocidad del puerto UART

// Propiedades de sonda
int sonarPins [] = {1, 2}; // Números de pin analógico al sensor de sonda Pin AN
const long MinLeftDistance = 20; // Distancia izquierda mínima permitida
const long MinRightDistance = 20; // Distancia mínima permitida
const long MinFrontDistance = 15; // Distancia frontal mínima permitida
const int SamplesAmount = 15; // más muestras - medición más suave y mayor retraso
const int SonarDisplayFrequency = 10; // muestra solo una de estas líneas, no todas
int sonarDisplayFrequencyCount = 0;
Factor largo constante = 2.54 / 2;
muestras largas [sizeof (sonarPins)] [SamplesAmount];
int sampleIndex [sizeof (sonarPins)];

// lado derecho
const int pinRightMotorDirection = 4; // esto se puede marcar en el escudo del motor como "DIR A"
const int pinRightMotorSpeed ​​= 3; // esto se puede marcar en el escudo del motor como "PWM A"
const int pinRightBumper = 2; // donde está conectado el parachoques derecho

// lado izquierdo
const int pinLeftMotorDirection = 7; // esto se puede marcar en el escudo del motor como "DIR B"
const int pinLeftMotorSpeed ​​= 6; // esto se puede marcar en el escudo del motor como "PWM B"
const int pinLeftBumper = 8; // donde está conectado el parachoques derecho

// descomenta las siguientes 2 líneas si Motor Shield tiene roturas
// const int pinRightMotorBreak = PUT_BREAK_PIN_HERE; // esto se puede marcar en el escudo del motor como "BREAKE A"
// const int pinLeftMotorBreak = PUT_BREAK_PIN_HERE; // esto se puede marcar en el escudo del motor como "BREAKE B"

// campos
const int turnRightTimeout = 100;
const int turnLeftTimeout = 150;
// establece en el contador cuánto tiempo está funcionando un motor: N / 10 (en milisegundos)
int countDownWhileMovingToRight;
int countDownWhileMovingToLeft;

// Inicialización
configuración nula () {
Serial.begin (Baudios);
initPins ();

// descomenta las siguientes 4 líneas si Motor Shield tiene roturas
// pinMode (pinLeftMotorBreak, OUTPUT);
// pinMode (pinRightMotorBreak, OUTPUT);
// digitalWrite (pinLeftMotorBreak, LOW); // apaga los descansos
// digitalWrite (pinRightMotorBreak, LOW); // apaga los descansos

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

// lazo principal
bucle vacío () {

verificarAndSetRightSide ();
verificarAndSetLeftSide ();

processRightSide ();
processLeftSide ();

delay (10); // repetir cada 10 milisegundos
}

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

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

nulo waitWhileAnyBumperIsPressed () {
while (checkBumperIsNotPressed (pinRightBumper)
&& checkBumperIsNotPressed (pinLeftBumper)) {
delay (20); // verifica cada 20 milisegundos
}
}

proceso nuloRightSide () {
if (countDownWhileMovingToRight MinFrontDistance) // comprueba si no se alcanza la distancia frontal mínima permitida
volver
if (checkCounterIsNotSet (countDownWhileMovingToLeft)) // si el contador aún no cuenta atrás
runLeftMotorBackward (); // ejecuta el motor derecho hacia atrás
countDownWhileMovingToLeft = turnLeftTimeout; // establece el contador en el valor máximo para comenzar la cuenta regresiva
}

bool checkCounterIsNotSet (int counter) {
return counter = SamplesAmount)
sampleIndex [pinIndex] = 0;
samples [pinIndex] [sampleIndex [pinIndex]] = valor;
volver verdadero;
}

largo CalculateAvarageDistance (int pinIndex) {
promedio largo = 0;
para (int i = 0; i promedio + = muestras [pinIndex] [i];
promedio de retorno / SamplesAmount;
}