Aspirador de bricolatge Robot aspirador a Arduino DIY

/*
Programa de control d’un robot amb dos motors.
El robot gira quan els motors canvien la seva velocitat i direcció.
Els para-xocs davanters als costats esquerre i dret detecten els obstacles.
Els sonars d'ultrasons es poden connectar a entrades analògiques (provades amb LV-MaxSonar-EZ1):
- poseu els pins a la matriu de sonarPins en l'ordre següent: esquerre, dret, frontal, altres.
Exemples:
1. Només sonars esquerre i dret connectats als pins 2 i 3: sonarPins [] = {2,3}
2. Sonars esquerre, dret i frontal connectats als pins 2, 3 i 5: sonarPins [] = {2,3,5}
3. Només un sonar frontal connectat amb el pin 5: sonarPins [] = {-1, -1,5}
4. Només es queda el sonar connectat al pin 2: sonarPins [] = {2}
5. Només sonar dret connectat als pins 3: sonarPins [] = {-1,3}
6,5 sonars connectats als pins 1,2,3,4,5: sonarPins [] = {1,2,3,4,5}
El blindatge del motor s'utilitza per fer funcionar motors.
*/
const int Baud = 9600; // Velocitat del port UART

// Propietats del sonar
int sonarPins [] = {1, 2}; // Num. Pin Analògics al sensor sonar Pin AN
const long MinLeftDistance = 20; // Distància mínima permesa a l'esquerra
const long MinRightDistance = 20; // Distància mínima permesa
const long MinFrontDistance = 15; // Distància mínima permesa davant
const int SamplesAmount = 15; // més mostres: mesurament més suau i retard més gran
const int SonarDisplayFrequency = 10; // mostrar només una d’aquestes línies: no totes
int sonarDisplayFrequencyCount = 0;
const factor llarg = 2,54 / 2;
mostres llargues [sizeof (sonarPins)] [SamplesAmount];
int sampleIndex [sizeof (sonarPins)];

// costat dret
const int pinRightMotorDirection = 4; // es pot marcar a l'escut del motor com a "DIR A"
const int pinRightMotorSpeed ​​= 3; // es pot marcar a l'escut del motor com a "PWM A"
const int pinRightBumper = 2; // on està connectat el para-xocs dret

// costat esquerre
const int pinLeftMotorDirection = 7; // es pot marcar a l'escut del motor com a "DIR B"
const int pinLeftMotorSpeed ​​= 6; // es pot marcar a l'escut del motor com a "PWM B"
const int pinLeftBumper = 8; // on està connectat el para-xocs dret

// descompte les properes dues línies si el motor Shield té pauses
// const int pinRightMotorBreak = PUT_BREAK_PIN_HERE; // es pot marcar a l'escut del motor com a "BREAKE A"
// const int pinLeftMotorBreak = PUT_BREAK_PIN_HERE; // es pot marcar a l'escut del motor com a "BREAKE B"

// camps
const int turnRightTimeout = 100;
const int turnLeftTimeout = 150;
// posa en marxa el temps en què un motor retrocedeix: N / 10 (en mil·lisegons)
int countDownWhileMovingToRight;
int countDownWhileMovingToLeft;

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

// descompte les properes quatre línies si el motor Shield té pauses
// pinMode (pinLeftMotorBreak, OUTPUT);
// pinMode (pinRightMotorBreak, OUTPUT);
// digitalWrite (pinLeftMotorBreak, BAIX); // desactiva els descansos
// digitalWrite (pinRightMotorBreak, BAIX); // desactiva els descansos

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

// Llaç principal
void loop () {

verificarAndSetRightSide ();
verificarAndSetLeftSide ();

processRightSide ();
processLeftSide ();

retard (10); // repeteix cada 10 mil·lisegons
}

//---------------------------------------------------
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)) {
retard (20); // revisar cada 20 milisegons
}
}

void processRightSide () {
if (countDownWhileMovingToRight MinFrontDistance) // comprova si no s'arriba a la distància frontal mínima permesa
tornar
if (checkCounterIsNotSet (countDownWhileMovingToLeft)) // si el comptador encara no compta
runLeftMotorBackward (); // executar el motor dret enrere
countDownWhileMovingToLeft = turnLeftTimeout; // configureu el comptador al valor màxim per iniciar-lo al compte enrere
}

bool checkCounterIsNotSet (comptador d’int) {
comptador de devolució = Import de Samples)
sampleIndex [pinIndex] = 0;
sample [pinIndex] [sampleIndex [pinIndex]] = valor;
tornar veritat;
}

llarg calculaAvarageDistance (int pinIndex) {
mitjana llarga = 0;
de (int i = 0; i mitjana + = mostres [pinIndex] [i];
mitjana de retorn / SamplesAmount;
}