Paso 4: codificación
La base de mi código vino otro instructivo:
Movimiento después de Robot
Sobre todo he ajustado para incluir otro servo y tener hacer diversos movimientos, como el otro servo realiza el movimiento de la cabeza. Quiero trabajar con él para trabajar con un pan y tilt servo pero que está más abajo de la línea.
CÓDIGO:
#include
Servo yservo; Servo xservo; //servos para x e y
const int Lin = 10, Rin = 12, Lout = 11, derrota = 13, yserv = 9, xserv = 8; ajuste sensor pernos y pasador de servo
establecer las variables de duración / / y la distancia resultado en pulgadas de largo, Rduration, Lduration, Rinches, Linches;
int ythreshold = 24; Umbral del detector en pulgadas
int xthreshold = 12; ángulo de int = 80; Ángulo inicial int ángulo = 90;
counter1 int = 0; counter2 int = 0; Boolean correcto = false;
int maxcounter1 = 45; int maxcounter2 = 3;
Boolean debug = false; Comunicación en serie para la depuración. Establecido en true para la comunicación serial.
void setup() {/ / inicializar la comunicación serial: Si (depuración) {Serial.begin(9600);} yservo.attach(9); //attach servo al pin 9 xservo.attach(8); / / acople servo al pin 8}
void loop() {//Most del código de sensor ha sido tomado desde el código de sensor PING de David Mellis //I modificó un sensor de 4 pines como oppsed al sensor 3 pin / / dar un breve impulso de baja previamente para asegurar un pulso limpio alto: pinMode (derrota, salida); digitalWrite (derrota, LOW); delayMicroseconds(2); digitalWrite (derrota, alto); delayMicroseconds(5); digitalWrite (derrota, baja);
Rduration = pulseIn (Rin, alto); pinMode (patán, salida); digitalWrite (patán, LOW); delayMicroseconds(2); digitalWrite (patán, alto); delayMicroseconds(5); digitalWrite (patán, LOW);
Lduration = pulseIn (Lin, alto);
convertir el tiempo en una distancia Rinches = microsecondsToInches(Rduration); Linches = microsecondsToInches(Lduration); Si (depurar) {Serial.print ("izquierda:"); Serial.Print(Linches); Serial.println ("en"); Serial.Print ("derecho:"); Serial.Print(Rinches); Serial.println ("en"); } follow(); }
largo microsecondsToInches (largo microsegundos) {/ / según ficha técnica de paralaje para el PING))), hay / / 73,746 microsegundos por pulgada (es decir sonido viajes en 1130 pies / segundo). Esto da la distancia recorrida por el ping, saliente / / y el retorno, por lo que dividimos por 2 para obtener la distancia del obstáculo. Ver: http://www.parallax.com/dl/docs/prod/acc/28015-PI... volver microsegundos / 74 / 2; }
void follow() {si (Linches < = ythreshold || Rinches < = ythreshold) {si (Linches + 2 < Rinches) {ángulo = ángulo - 2;} Si (Rinches + 2 < Linches) {ángulo ángulo = 2;}} Si (ángulo > 160) {ángulo = 160;} si (ángulo < 0) {ángulo = 0;} yservo.write(angle);
/ * Si (Linches < = xthreshold || Rinches < = xthreshold) {si (Linches + 2 < Rinches) {ángulo = ángulo - 2;} Si (Rinches + 2 < Linches) {ángulo ángulo = 2;} Si (ángulo >} else {ángulo = 90; * / si (Linches < = xthreshold || Rinches < = xthreshold) {si (counter1 == maxcounter1 || counter1 == 0) {counter2 ++; si (counter2 > = maxcounter2) {counter2 = 0; if(counter1 == maxcounter1) counter1--; else counter1 ++; derecha =! derecho;}} if(right) otra cosa {ángulo ángulo = 2; counter1--;} más {ángulo = ángulo 2; counter1 ++;}} otro ángulo = 90;
xservo.Write(xangle); }
