Aspirador de pó robô DIY DIY no Arduino DIY

/*
Programa para controlar um robô com dois motores.
O robô gira quando os motores mudam de velocidade e direção.
Pára-choques dianteiros nos lados esquerdo e direito detectam obstáculos.
Os sonares ultrassônicos podem ser conectados a entradas analógicas (testadas no LV-MaxSonar-EZ1):
- colocar pinos no array sonarPins na seguinte ordem: esquerda, direita, frente, outras.
Exemplos:
1. apenas sonares esquerdo e direito conectados aos pinos 2 e 3: sonarPins [] = {2,3}
2. sonares esquerdo, direito e dianteiro conectados aos pinos 2, 3 e 5: sonarPins [] = {2,3,5}
3. somente sonar frontal conectado ao pino 5: sonarPins [] = {-1, -1,5}
4. apenas o sonar esquerdo conectado ao pino 2: sonarPins [] = {2}
5. único sonar direito conectado aos pinos 3: sonarPins [] = {-1,3}
6.5 sonares conectados aos pinos 1,2,3,4,5: sonarPins [] = {1,2,3,4,5}
A blindagem do motor é usada para acionar motores.
*/
const int Baud = 9600; // velocidade da porta UART

// Propriedades do sonar
int sonarPins [] = {1, 2}; // Números de pinos analógicos no sensor de sonar Pinos AN
const long MinLeftDistance = 20; // Distância esquerda mínima permitida
const long MinRightDistance = 20; // Distância certa mínima permitida
const long MinFrontDistance = 15; // Distância frontal mínima permitida
const int SamplesAmount = 15; // mais amostras - medição mais suave e maior atraso
const int SonarDisplayFrequency = 10; // exibe apenas uma dessas linhas - nem todas
int sonarDisplayFrequencyCount = 0;
Fator const long = 2,54 / 2;
amostras longas [sizeof (sonarPins)] [SamplesAmount];
int sampleIndex [sizeof (sonarPins)];

// lado direito
const int pinRightMotorDirection = 4; // isso pode ser marcado na blindagem do motor como "DIR A"
const int pinRightMotorSpeed ​​= 3; // isso pode ser marcado na blindagem do motor como "PWM A"
const int pinRightBumper = 2; // onde o pára-choque direito está conectado

// lado esquerdo
const int pinLeftMotorDirection = 7; // isso pode ser marcado na blindagem do motor como "DIR B"
const int pinLeftMotorSpeed ​​= 6; // isso pode ser marcado na blindagem do motor como "PWM B"
const int pinLeftBumper = 8; // onde o pára-choque direito está conectado

// descomente as próximas 2 linhas se o Motor Shield estiver com quebras
// const int pinRightMotorBreak = PUT_BREAK_PIN_HERE; // isso pode ser marcado na blindagem do motor como "BREAKE A"
// const int pinLeftMotorBreak = PUT_BREAK_PIN_HERE; // isso pode ser marcado na blindagem do motor como "BREAKE B"

// campos
const int turnRightTimeout = 100;
const int turnLeftTimeout = 150;
// define no contador quanto tempo um motor está funcionando de volta: N / 10 (em milissegundos)
int countDownWhileMovingToRight;
int countDownWhileMovingToLeft;

// Inicialização
configuração nula () {
Serial.begin (Baud);
initPins ();

// descomente as próximas 4 linhas se o Motor Shield estiver com quebras
// pinMode (pinLeftMotorBreak, OUTPUT);
// pinMode (pinRightMotorBreak, OUTPUT);
// digitalWrite (pinLeftMotorBreak, LOW); // desativar pausas
// digitalWrite (pinRightMotorBreak, LOW); // desativar pausas

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

// loop principal
loop vazio () {

verificaAndSetRightSide ();
verificaAndSetLeftSide ();

processRightSide ();
processLeftSide ();

delay (10); // repita a cada 10 milissegundos
}

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

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

void waitWhileAnyBumperIsPressed () {
while (checkBumperIsNotPressed (pinRightBumper)
&& checkBumperIsNotPressed (pinLeftBumper)) {
delay (20); // verifica a cada 20 milissegundos
}
}

void processRightSide () {
if (countDownWhileMovingToRight MinFrontDistance) // verifica se a distância frontal mínima permitida não é atingida
retornar
if (checkCounterIsNotSet (countDownWhileMovingToLeft)) // se o contador ainda não estiver em contagem regressiva
runLeftMotorBackward (); // executa o motor direito para trás
countDownWhileMovingToLeft = turnLeftTimeout; // define o contador para o valor máximo para iniciar a contagem regressiva
}

bool checkCounterIsNotSet (contador int) {
contador de retorno = SamplesAmount)
sampleIndex [pinIndex] = 0;
amostras [pinIndex] [sampleIndex [pinIndex]] = valor;
return true;
}

Long calculAvarageDistance (int pinIndex) {
média longa = 0;
for (int i = 0; i média + = amostras [pinIndex] [i];
média de retorno / SamplesAmount;
}