Robot casero de limpieza con Bluetooth (4 / 5 paso)

Paso 4: El código

LAS BIBLIOTECAS:

El "SweeperBot" hace uso de alambre, I2Cdev, TimerOne (de interrupción), las bibliotecas Adafruit Sensor, HMC5883 y LCD. Puede descargar el archivo Zip, que contiene el código para el Arduino y todas las bibliotecas. También contiene el proyecto de Bluetooth para Windows Phone.

EL CÓDIGO DE TELÉFONO 8.1 DE WINDOWS:

El proyecto que controla el robot vía Bluetooth desde el Windows Phone fue adaptado de un ejemplo en internet. Fue alterado para tener cuatro botones, uno para cada dirección. Arriba, abajo, izquierda y derecha. Cada botón envía una carta al robot, respectivamente U, D, L y R. Cuando el robot recibe éstos, hace que uno de los siguientes:

• U - el robot establecerá la dirección a seguir hacia adelante;

• D - el robot girará a su parte trasera y siga recto;

• L - el robot establecerá la dirección a seguir exactamente 90 grados a la izquierda de donde está "mirando";

• R - el robot establecerá la dirección a seguir exactamente 90 grados desde donde actualmente es "mirada";

Lo que el robot encarga de estos comandos puede cambiarse con el código de Arduino. Es posible añadir más botones para activar el robot, o detener o incluso hacerlo retroceder. Este código es sólo un ejemplo. También es posible hacer programas similares a otros dispositivos, como androides un para IOS. Lo único que tienes que enviar el robot son estas letras - U, D, L o R.

 private async void RedButton_Click_1(object sender, RoutedEventArgs e) { string command = "U"; await connectionManager.SendCommand(command); } private async void GreenButton_Click_1(object sender, RoutedEventArgs e) { string command = "L"; await connectionManager.SendCommand(command); } private async void YellowButton_Click_1(object sender, RoutedEventArgs e) { string command = "R"; await connectionManager.SendCommand(command); } private async void RedButton_Copy_Click(object sender, RoutedEventArgs e) { string command = "D"; await connectionManager.SendCommand(command); } 

Para implementar este código en tu Windows Phone necesita Visual Studio y un teléfono desbloqueado de desarrollador. Esto puede hacerse siguiendo las instrucciones en este link.

EL CÓDIGO DE ARDUINO:

En Resumen, la lógica principal del robot es el siguiente:

1. Distancia más cercana del objeto encontrado en cada uno de los sensores de 6 posibles;
2. Comportamiento del robot proceso distancias y punto deseado dirección con un vector X, Y (sin embargo, si se recibe un mensaje desde el smartphone, paso 2 se sustituye por la dirección del dispositivo);
3. Ofrece brújula digital la dirección el robot va, para el control de las ruedas;
4. Información X', Y' de brújula se normaliza al espacio [-1,1];
5. Producto cruz entre vectores de dirección deseada y la real dirección encuentra error entre ellos;
6. Método PID calcula PWM (energía) para cada motor de la C.C. para las dos ruedas (2WD);
7. Activa la interrupción PWM;

Como el proyecto evolucionó, descubrí la librería estándar disponible en GitHub para los sensores ultrasónicos fueron contradictorios con la activación de PWM de los motores, porque utilizan el mismo temporizador. Como este proyecto utilice 5 pernos para configurar el PWM de los tres motores del DC (y esto sólo es posible porque el barrendero de alfombra se convierte sólo en una dirección) y 7 más para los sensores, de que el microcontrolador tiene que estar constantemente revisando si la lectura fue recibida, no fue posible utilizar el material disponible en internet.

Por lo tanto, para evitar conflictos, se configuró sólo la interrupción del timer 1. A través de esta interrupción, el Arduino puede comprobar si hubo lecturas en cada sensor: Si no, aumenta una variable para cada uno de ellos, que se utilizará como parámetro para encontrar la distancia al objeto.

 void callback() { //callback is the interruption name noInterrupts(); //if no reading, increases variable sonarCount[n], where n is the number of sensor if ((sonar[0] == 1) && (digitalRead(ECHO))) sonarCount[0]++; //if there is readind on that sensor, sonar[n] = 2 accuses object found if ((sonar[0] == 1) && (!digitalRead(ECHO)) && (sonarCount[0] > 0)) sonar[0] = 2; 

También, dentro de esta interrupción, lanza toda su potencia al motor. Si ha habido un cierto número de interrupciones que supera el número de variables PWM_R o PWM_L, luego lanza cero potencia que da el motor.

 if (PWMCount == 0) { if (PWML) { //if pwm left is positive then turn to a certain way digitalWrite(MOTOR + 2, HIGH); digitalWrite(MOTOR + 3, LOW); } else { //else turn to the other side the left motor digitalWrite(MOTOR + 2, LOW); digitalWrite(MOTOR + 3, HIGH); } (...) if (PWMCount >= abs(PWM_L)) { //if the counter equals the power we want digitalWrite(MOTOR + 2, LOW); //shut down both pins of this motor digitalWrite(MOTOR + 3, LOW); } (...) PWMCount++; //increases counter if (PWMCount == 250) PWMCount = 0; //if counter equals maximum value we want Timer1.attachInterrupt(callback); //250 equals 100% power } 

Durante el bucle principal, el robot se queda buscando obstáculos y calcular rutas para evitarlos, o cambiar su trayectoria. Si el desarrollador desea configurar método de control de campos potenciales, o cambiar el comportamiento del robot, él sólo debe trabajar una mejor función de obst(). Las otras partes del programa son necesarias y no necesitan ser cambiado. Cuando el robot recibe un mensaje a través de Bluetooth, se detiene evitando los obstáculos y escuchar para entradas del usuario.

 void loop() //main loop { lcd.clear(); //clears previous messages on lcd delay(10); search(); //search obstacles and saves the distance value to each sensor readings(); //reads compass and calculates the error from its desired direction calcPWM(); //calculates the PWM for each motor based on PID if (!mensagem) { //message from Bluetooth? if not, continue doing this obst(); //here is where the robot defines its behavior } //and where it defines the direction it want to go (...) 

De todas las partes del código, todos ellos son bastante básicas, la solo función que es interesante tomar en cuenta aquí, es el readings(). En su interior, el robot obtiene el valor X, Y y Z de la posición actual en la brújula digital. ¿Por lo tanto, cómo hacer la lectura en algo que hace que el robot sabe a que dirección lo está buscando?

En primer lugar obtendrá los valores crudos:

 double rawX = event.magnetic.x; double rawY = event.magnetic.y; double rawZ = event.magnetic.z; 

Compruebe si el robot está en el plano de la derecha. Esto se puede ver si el Z (eje perpendicular) es con un valor entre una cierta gama pequeña

 if ((rawZ > Zprim + 10) || (rawZ < Zprim - 10)) return; //if not right, return 

Ahora es el momento para normalizar los valores X e Y a un espacio entre [-1,1], con el fin de calcular sin() y cos() de estos valores.

 //Ymin, Ymax, Xmin and Xmax are obtained when the calibration occurs vector[1] = ((rawY) - ((Ymax + Ymin) / 2)) / ((Ymax - Ymin) / 2); vector[0] = ((rawX) - ((Xmax + Xmin) / 2)) / ((Xmax - Xmin) / 2); 

Ahora hacemos vectorOBJ [n] vector bidireccional hacia donde, en el plan, queremos que el robot para ir. Así, para encontrar el error entre dos vectores que tenemos que hacer el producto cruz entre los dos. El valor resultante es otro vector, pero perpendicular a ambos, hacia arriba o abajo, dependiendo de su signo.

 error = (vectorOBJ[0] * vector[0] + vectorOBJ[1] * vector[1]) / (sqrt(vectorOBJ[0] * vectorOBJ[0] + vectorOBJ[1] * vectorOBJ[1]) * sqrt(vector[0] * vector[0] + vector[1] * vector[1])); error = (acos(error)) * 180 / PI; //from rad to degrees //find out the way the result vector is pointing sentido = vector[0] * vectorOBJ[1] - vector[1] * vectorOBJ[0]; if (sentido > 0) error = -error;