ArduBot (7 / 8 paso)

Paso 7: programa de

Necesitamos definir algunas matrices para almacenar las coordenadas polares y cartesianas para representar la planta de cualquier habitación es el robot. También necesitaremos otra matriz para indicar el tiempo de retardo entre pings para calcular theta.

const int pingPin = 7;
const int DIRA = 13;   establece la dirección del motor de un
const int DIRB = 12;   Dirección de sistemas de motor b
const int PWMA = 11;   pulso de modulación de anchura de para el motor de un
const int PWMB = 3;    modulación de anchura de pulso para motor b
int n = 16;            número de puntos de datos registrados (scan); -1
int ping [16];          almacena los datos de PING para completar
int theta [16];         almacena los datos polares para n puntos de datos
int x [16];             tiendas cartesianos x valor de datos del punto n
int y [16];             almacena valor cartesiano y n puntos de datos
int t [16];             permite calcular theta; tiendas cambian en el tiempo de comienzo de la exploración para n puntos de datos
int spindelay = 3200;  tiempo en ms que tarda robot gire una vuelta completa
int delta = 3200/16;             Delta = spindelay/n; cantidad de tiempo para una rotación
duración larga sin signo, cm, pulgadas, dir;   duración mide el tiempo entre PINGs
minsafe largo = 15;                         mínima distancia de seguridad delante de robot
int cuenta;                                 utilizado para ayudar a identificar cuando el robot está en una esquina
omega int = 2*3.1416/3.2;             velocidad angular de giro robot = 2pi/spindelay

void setup() {}
pinMode (DIRA, salida); dirección de //Sets de motor A
pinMode (DIRB, salida); dirección de //Sets de motor B
pinMode (PWMA, salida); //Turns A encendido y apagado
pinMode (PWMB, salida); //Turns B y

/ * Si DIRA = baja, giros de Motor A FWD
Si DIRB = baja, giros de Motor B FWD
*/

inicializar la comunicación serial:
Serial.Begin(9600);
}

anular (FWD) {//sets motores hacia adelante
digitalWrite (DIRA, alto);
digitalWrite (PWMA, alto);
digitalWrite (DIRB, alto);
digitalWrite (PWMB, alto);
}

vacío (REV) {//sets motores para invertir
digitalWrite (DIRA, LOW);
digitalWrite (PWMA, alto);
digitalWrite (DIRB, LOW);
digitalWrite (PWMB, alto);
}

anular (derecha) {//turns en punto
digitalWrite (DIRA, alto);
digitalWrite (PWMA, alto);
digitalWrite (DIRB, LOW);
digitalWrite (PWMB, alto);
}

Anular izquierdo () {//turns en lugar
digitalWrite (DIRA, LOW);
digitalWrite (PWMA, alto);
digitalWrite (DIRB, alto);
digitalWrite (PWMB, alto);
}

void STOP () {}
digitalWrite (PWMA, LOW);
digitalWrite (PWMB, bajo);
}

{} de be larga sin signo
pinMode (pingPin, salida); Hacer el Pingpin salida
digitalWrite (pingPin, LOW); Enviar un pulso bajo
delayMicroseconds(2); Espere dos microsegundos
digitalWrite (pingPin, HIGH); Enviar un pulso de alta
delayMicroseconds(5); esperar 5 segundos de micro
digitalWrite (pingPin, LOW); Enviar un pulso bajo
pinMode(pingPin,INPUT); Pingpin a la entrada del interruptor
duración = pulseIn (pingPin, HIGH); Escuchar para eco

/ * Convertir segundos micro pulgadas
*/

pulgadas = microsecondsToInches(duration);
cm = microsecondsToCentimeters(duration);
Serial.Print (cm);
Serial.Print ("CM");
Serial.println ();

volver cm;

}

largo microsecondsToInches(long microseconds) {}
volver microsegundos / 74 / 2;
}
largo microsecondsToCentimeters(long microseconds) {}
volver microsegundos / 29 / 2;
}

sin firmar largo leftping () {//records distancia a la izquierda del robot y lo devuelve al centro
int izquierda;
(IZQUIERDA);
retardo (420);
STOP ();
Delay(10);

Serial.Print ("izquierda:");
= izquierda (PING);
retardo (420);

(DERECHA);
retardo (420);
STOP ();
vuelta izquierda;
}

sin firmar largo rightping () {//records distancia a la derecha del robot y lo devuelve al centro
int derecha;
(DERECHA);
retardo (420);
STOP ();
Delay (10);

Serial.Print ("derecho:");
= derecha (PING);
retardo (420);

(IZQUIERDA);
retardo (420);
STOP();
volver a derecho;
}

forwardping largo sin signo () {//essentiallhy PING
int medio;
STOP ();
Delay (10);
Serial.Print ("media:");
medio = (PING);
retorno medio;

}

sin firmar largo elegir () {}
leftping();
forwardping();
rightping();
if(leftping > rightping)
Si (leftping > forwardping)
(IZQUIERDA);
retardo (420);
STOP();
DIR = forwardping ();
Si (rightping > leftping)
Si (rightping > forwardping) {}
(DERECHA);
retardo (420);
STOP();
DIR = forwardping ();
} else
STOP ();
DIR = forwardping ();

}

exploración del vacío () {}
Delta = spindelay/n;
int c;
int i;
int t;

para (t = 1; t < 17; t ++) {}
Theta [t] = t/8 * 3.1416;
}


para (c = 0; c < 16; c ++) {}
(IZQUIERDA);
Delay(Delta);
STOP ();
Delay (200);

ping [c] = (PING);

x [c] = ping[c]*sin(theta[c]);
y [c] = ping[c]*cos(theta[c]);
}

para (i = 0; i < 16; i ++) {}
Serial.println(Theta[i]);
}
}

void loop () {}
Scan();
Delay(10000);
}