Software
En esta sección se explica los procesos de recepción y envío de datos en el microprocesador, el protocolo de comunicación utilizado, el desarrollo de la interfaz gráfica del proyecto y su implementación. Cabe tener en cuenta que el proyecto se realizó utilizando el programa CodeWarrior V10.6 junto a la herramienta Processor Expert, sin la necesidad de interfaces extra o interconexiones con software alterno y bajo el Lenguaje de programación C para el DemoQE y Python para la programación de la interfaz gráfica en el Ordenador.
Para realizar esta labor, se implementó un maquina de Estados formada por 3 funciones principales, Esperar, Medir y Enviar, el valor de bandera Estado que se utiliza para transitar de un estado a otro, se altera mediante una interrupción por tiempo.
-
Esperar: Es un estado intermedio en el cual se está apenas iniciado el programa o luego de haber pasado por el estado Medir y Enviar previamente. Una vez ocurrido el llamado de interrupción por tiempo, El sistema pasa a la rutina de manejo de este donde cambia el valor de bandera Estado de Esperar a Medir. Mientras que no ocurra esta interrupción el sistema se mantiene en el estado Esperar.
-
Medir: Su función es adquirir los datos presentes en los puertos de entrada y guardarlo en las variables detallados en la Tabla 1 y posterior a esto se cambia el valor de bandera Estado a Enviar.
| Puerto de entrada del DemoQe | Componente del DemoQE asociado | Variable asociada en el código | Sensor |
|---|---|---|---|
| PTA0 | Canal 0 ADC | iADC1 | Sharp |
| PTA1 | Rx puerto Serial | iADC2 | Giroscopio |
| PTA6 | Bit5:IO | D1 | Knock |
| PTA7 | Bit4:I0 | D2 | Efecto Hall |
Tabla 1: Puertos de entrada y variables correspondientes en código.
- Enviar: En este estado se inserta la data de los canales analógicos y digitales dentro del protocolo de comunicación y la nueva trama se envía mediante comunicación serial al ordenador para su posterior uso.
` for(;;){ switch(estado){
case ESPERAR:
break;
case MEDIR:
CodError = AD1_MeasureChan(TRUE, 0); //Medida del canal 0 Sharp
CodError = AD1_GetChanValue16(0, &iADC1.u16);
iADC2.u8[1]=Data_Arduino[0]; //Medida del Giroscopio
iADC2.u8[0]= 0x00;
D1 = Bit5_GetVal(); //Medida sensor Digital knoc
D2 = Bit4_GetVal(); //Medida sensor Digital Efecto Hall
estado = ENVIAR;
break;
case ENVIAR:
//Protocolo de Comunicación, En la posición 0 se guarda el encabezado de la trama
//Sensores Analógicos
dTrama[2] = ((iADC1.u16 >> 4) &(0x7F)); //Byte 2 y 3 (D1, D2 Y A1) Sharp
dTrama[1] = ((iADC1.u16 >> 11)&(0x1F));
dTrama[4] = ((iADC2.u16)&(0x7F)); //Byte 3 y 4 (D3, D4 y A2) Giroscopo
dTrama[3] = ((iADC2.u16 >> 7)&(0x1F));
//Sensores Digitales
if(!D1){
dTrama[1] = dTrama[1]|(0x40);
}
if(!D2){
dTrama[1] = dTrama[1]|(0x20);
}
CodError = AS1_SendBlock(dTrama, 5, &Enviados2);
estado = ESPERAR;
break;
default:
break;
}
} `
El sensor Mpu6050 trabaja bajo el protocolo de comunicación I2C, para obtener sus medidas se utilizó un Arduino One debido a la amplia documentación para el manejo de este protocolo bajo su plataforma. Una vez conseguido los valores de interés se procede a establecer comunicación con el microprocesador DemoQE mediante serial.
`
uint8_t data;
//const size_t dataLength = sizeof(data);//sizeof(data) / sizeof(data[0]);
#include <Wire.h>
//Direccion I2C de la IMU
#define MPU 0x68
//Ratios de conversion
#define A_R 16384.0
#define G_R 131.0
//Conversion de radianes a grados 180/PI
#define RAD_A_DEG = 57.295779
//MPU-6050 da los valores en enteros de 16 bits
//Valores sin refinar
int16_t AcX, AcY, AcZ, GyX, GyY, GyZ;
//Angulos
float Acc[2];
float Gy[2];
float Angle[2];
void setup() {
// put your setup code here, to run once:
Wire.begin();
Wire.beginTransmission(MPU);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
Serial.begin(115200);
}
void loop() {
// put your main code here, to run repeatedly:
//Leer los valores del Acelerometro de la IMU
Wire.beginTransmission(MPU);
Wire.write(0x3B); //Pedir el registro 0x3B - corresponde al AcX
Wire.endTransmission(false);
Wire.requestFrom(MPU,6,true); //A partir del 0x3B, se piden 6 registros
AcX=Wire.read()<<8|Wire.read(); //Cada valor ocupa 2 registros
AcY=Wire.read()<<8|Wire.read();
AcZ=Wire.read()<<8|Wire.read();
//A partir de los valores del acelerometro, se calculan los angulos Y, X
//respectivamente, con la formula de la tangente.
Acc[1] = atan(-1*(AcX/A_R)/sqrt(pow((AcY/A_R),2) + pow((AcZ/A_R),2)))*RAD_TO_DEG;
Acc[0] = atan((AcY/A_R)/sqrt(pow((AcX/A_R),2) + pow((AcZ/A_R),2)))*RAD_TO_DEG;
//Leer los valores del Giroscopio
Wire.beginTransmission(MPU);
Wire.write(0x43);
Wire.endTransmission(false);
Wire.requestFrom(MPU,4,true); //A diferencia del Acelerometro, solo se piden 4 registros
GyX=Wire.read()<<8|Wire.read();
GyY=Wire.read()<<8|Wire.read();
GyZ=Wire.read()<<8|Wire.read();
//Calculo del angulo del Giroscopio
Gy[0] = GyX/G_R;
Gy[1] = GyY/G_R;
Gy[2] = GyZ/G_R;
//Aplicar el Filtro Complementario
Angle[0] = 0.98 *(Angle[0]+Gy[0]*0.010) + 0.02*Acc[0];
Angle[1] = 0.98 *(Angle[1]+Gy[1]*0.010) + 0.02*Acc[1];
Angle[2] = 0.98 *(Angle[2]+Gy[2]*0.010) + 0.02*Acc[2];
//Medida a enviar
data=Acc[1];
//Mostrar los valores por consola
Serial.print("AcX X: "); Serial.print(data); Serial.print("\n---------\n"); //funciono
Serial.write(data);
delay(256); //Nuestra dt sera, pues, 0.010, que es el intervalo de tiempo en cada medida
}`
Se efectuó mediante comunicación serial. Se estableció el siguiente protocolo de comunicación para las tramas enviadas a través de este método:
Donde:
- N: Número de canales analógicos a utilizar.
- Ax: Información del sensor Analógico, se utilizaron 2 (Sharp y el giroscopio).
- Dx: Información del sensor Digital, se utilizaron 2. (Knock y Efecto Hall).
`
//Protocolo de Comunicación, En la posición 0 se guarda el encabezado de la trama
//Sensores Analógicos
dTrama[2] = ((iADC1.u16 >> 4) &(0x7F)); //Byte 2 y 3 (D1, D2 Y A1) Sharp
dTrama[1] = ((iADC1.u16 >> 11)&(0x1F));
dTrama[4] = ((iADC2.u16)&(0x7F)); //Byte 3 y 4 (D3, D4 y A2) Giroscopo
dTrama[3] = ((iADC2.u16 >> 7)&(0x1F));
//Sensores Digitales
if(!D1){
dTrama[1] = dTrama[1]|(0x40);
}
if(!D2){
dTrama[1] = dTrama[1]|(0x20);
}
`
Primero se realiza la recepción de datos provenientes del DemoQE, para ello se lee el puerto de entrada de la comunicación serial y se procede a decodificar el protocolo de comunicación de los bloques de bytes recibidos:
`"""Inicio de la recepción de datos y la interfaz gráfica""" while True: #Mientras el puerto reciba información
ch = port.read(5) # Lee n número de bytes. 5 para nuestro caso
""" Extracción de datos del protocolo de comunicación """
p1 = format((ch[1] & 31), '05b') # Hacemos and con 31 para extraer los 5 bits que nos interesan
p2 = format((ch[2] & 127), '07b') # Hacemos and con 127 para extraer los 7 bits que nos interesan
p3 = format((ch[3] & 31), '05b') # Hacemos and con 31 para extraer los 5 bits que nos interesan
p4 = format((ch[4] & 127), '07b') # Hacemos and con 31 para extraer los 5 bits que nos interesan
D1 = format((ch[1] & 64), '07b') # Hacemos and con para extraer el bit del canal digital que nos interesan
D2 = format((ch[1] & 32), '07b') # Hacemos and con para extraer el bit del canal digital que nos interesan
mensaje_1 = p1 + p2 # Concatenamos los bits correspondientes al canal analógico 1
mensaje_2 = p3 + p4 # Concatenamos los bits correspondientes al canal analógico 2
mensaje_1int = int(mensaje_1, 2) * ( 3 / 4095) # llevamos el valor medido al valor de voltaje equivalente Sharp
mensaje_2int = int(mensaje_2, 2); # Llevamos el valor medido al valor enviado por el Giroscopio
mensaje_3int = int(D1, 2) * (3 / 64)
mensaje_4int = int(D2, 2) * (3 / 32)`
Luego estos datos son utilizados para el desarrollo de la interfaz gráfica con el fin generar movimiento en las articulaciones, esta interfaz gráfica fue desarrollada bajo el lenguaje de programación Python con el uso de la librería Pygame .
La interfaz gráfica funciona a partir de 3 imágenes vectorizadas correspondiente a la mano, brazo y antebrazo dispuestas de manera tal que se tengan 3 articulaciones (muñeca, codo y hombro) aunque para nuestro caso solo trabajaremos con las 2 últimas. La ubicación de cada una de ellas se realiza a través de correcciones espaciales en la pantalla de salida de Pygame .El movimiento de la interfaz se genera a partir de una posición inicial (brazo del usuario completamente extendido hacia abajo) se determina la variación de la medida con respecto a la anterior, y luego se compara con un rango de valores determinado empíricamente que excluyen las señales de ruido presente en los sensores. Seguidamente se realizan una serie de ajustes de rotación del brazo y la ubicación espacial de cada imagen vectorizada.
` display.fill(white)
"""Movimiento del brazo por señales recibidas por comunicación serial""" medida1 = mensaje_1int - medida_anterior1 medida2 = mensaje_2int - medida_anterior2
#prueba = prueba+1
n=0
m=0
if abs(medida1) > 0.03 and abs(medida1)<.11:
m=2*medida1
if abs(medida2) > 3 and abs(medida2)<40:# and abs(medida2) < 10:
n=medida2/40
if inicio == True:
n=-medida2/5
inicio = False
if mensaje_3int == 3.0:
msg="Se ha detectado un golpe en el dispositivo"
popupmsg(msg)
if mensaje_4int == 3.0 and mensaje_anterior4 == 0.0:
if Sensor_d2==False:
msg = "Se ha inhabilitado el movimiento del brazo"
popupmsg(msg)
if Sensor_d2 == True:
msg = "Se ha habilitado el movimiento del brazo"
popupmsg(msg)
Sensor_d2 = not Sensor_d2
if Sensor_d2 == True:
n=0; m=0
ua_image, ua_rect = upperarm.rotate(n)
ua_rect.center += np.asarray(origin)
ua_rect.center -= np.array([-np.cos(upperarm.rotation) * upperarm.offset,
np.sin(upperarm.rotation) * upperarm.offset])
fa_image, fa_rect = forearm.rotate((n+m))
h_image, h_rect = hand.rotate((n+m))
# generate (x,y) positions of each of the joints
joints_x = np.cumsum([0,
upperarm.scale * np.cos(upperarm.rotation),
forearm.scale * np.cos(forearm.rotation),
hand.length * np.cos(hand.rotation)]) + origin[0]
joints_y = np.cumsum([0,
upperarm.scale * np.sin(upperarm.rotation),
forearm.scale * np.sin(forearm.rotation),
hand.length * np.sin(hand.rotation)]) * -1 + origin[1]
joints = [(int(x), int(y)) for x,y in zip(joints_x, joints_y)]
def transform(rect, base, arm_part):
rect.center += np.asarray(base)
rect.center += np.array([np.cos(arm_part.rotation) * arm_part.offset,
-np.sin(arm_part.rotation) * arm_part.offset])
#transform(ua_rect, joints[0], upperarm)
transform(fa_rect, joints[1], forearm)
transform(h_rect, joints[2], hand)
# transform the hand a bit more because it's weird
h_rect.center += np.array([np.cos(hand.rotation),
-np.sin(hand.rotation)]) * -10
display.blit(ua_image, ua_rect)
display.blit(fa_image, fa_rect)
display.blit(h_image, h_rect)
medida_anterior1 = mensaje_1int
medida_anterior2 = mensaje_2int
mensaje_anterior4 = mensaje_4int
# check for quit
for event in pygame.event.get():
if event.type == pygame.locals.QUIT:
pygame.quit()
sys.exit()
pygame.display.update()
fpsClock.tick(30)`
Arm3.py armpart.py popupmsg.py hand.png forearm.png upperarm.png
