POSICIONAMIENTO Y ACELERACIÓN
Nuestro objetivo en este año 2018/2019 ha sido introducir y programar sensores de posicionamiento y aceleración mediante una Raspberry Pi para saber como se encuentra el casco en un incendio, por si este sufre alguna caída y por ello se tenga que llamar alguna unidad móvil y que se pueda rescatar en este caso en el bombero que este malherido. Para realizar esta función hemos utilizado los siguientes materiales:
1. MATERIALES NECESARIOS:
Sensor MPU 60-50
RASPBERRY PI
NODE-RED (IOT)
2. ENSAMBLAJE DE LOS COMPONENTES
Una vez que hemos adquirido todos los componentes, vamos a disponernos a instalar el sensor de gas MPU-6050 en la raspberry pi, así que el orden de conexión es el siguiente:
-Colocar el sensor en la salida VCC
-Poner dos pins GND para alimentarlo
-Poner dos pins analógicos, uno que será el pin analógico 4 y otro en el 5.
3. INSTALACIÓN EN PYTHON DE SMBUS Y PYGAME
Para que podamos ver la tetera y la posición en la que se encuentra el sensor de posicionamiento y aceleración, necesitaremos instalar las siguientes librerías para Python:
-Instalación de librería SMBUS: sudo apt-get install python-smbus
-Instalación de librería Pygame: sudo apt-get install python-pygame
Una vez que hemos instalado estas librerías, lo que debemos de hacer es crear un fichero que será el server.py y contendrá el siguiente código:
#!/usr/bin/python
import web
import smbus
import math
import time
urls = (
'/', 'index'
)
# Power management registers
power_mgmt_1 = 0x6b
power_mgmt_2 = 0x6c
gyro_scale = 131.0
accel_scale = 16384.0
address = 0x68 # This is the address value read via the i2cdetect command
def read_all():
raw_gyro_data = bus.read_i2c_block_data(address, 0x43, 6)
raw_accel_data = bus.read_i2c_block_data(address, 0x3b, 6)
gyro_scaled_x = twos_compliment((raw_gyro_data[0] << 8) + raw_gyro_data[1]) / gyro_scale
gyro_scaled_y = twos_compliment((raw_gyro_data[2] << 8) + raw_gyro_data[3]) / gyro_scale
gyro_scaled_z = twos_compliment((raw_gyro_data[4] << 8) + raw_gyro_data[5]) / gyro_scale
accel_scaled_x = twos_compliment((raw_accel_data[0] << 8) + raw_accel_data[1]) / accel_scale
accel_scaled_y = twos_compliment((raw_accel_data[2] << 8) + raw_accel_data[3]) / accel_scale
accel_scaled_z = twos_compliment((raw_accel_data[4] << 8) + raw_accel_data[5]) / accel_scale
return (gyro_scaled_x, gyro_scaled_y, gyro_scaled_z, accel_scaled_x, accel_scaled_y, accel_scaled_z)
def twos_compliment(val):
if (val >= 0x8000):
return -((65535 - val) + 1)
else:
return val
def dist(a, b):
return math.sqrt((a * a) + (b * b))
def get_y_rotation(x,y,z):
radians = math.atan2(x, dist(y,z))
return -math.degrees(radians)
def get_x_rotation(x,y,z):
radians = math.atan2(y, dist(x,z))
return math.degrees(radians)
def get_z_rotation(x,y,z):
radians = math.atan2(z, dist(x,y))
return math.degrees(radians)
bus = smbus.SMBus(1) # or bus = smbus.SMBus(1) for Revision 2 boards
class index:
def GET(self):
now = time.time()
K = 0.98
K1 = 1 - K
time_diff = 0.01
(gyro_scaled_x, gyro_scaled_y, gyro_scaled_z, accel_scaled_x, accel_scaled_y, accel_scaled_z) = read_all()
last_x = get_x_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)
last_y = get_y_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)
last_z = get_z_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)
gyro_offset_x = gyro_scaled_x
gyro_offset_y = gyro_scaled_y
gyro_offset_z = gyro_scaled_z
gyro_total_x = (last_x) - gyro_offset_x
gyro_total_y = (last_y) - gyro_offset_y
gyro_total_z = (last_z) - gyro_offset_z
for i in range(0, int(0.5 / time_diff)):
time.sleep(time_diff - 0.005)
(gyro_scaled_x, gyro_scaled_y, gyro_scaled_z, accel_scaled_x, accel_scaled_y, accel_scaled_z) = read_all()
gyro_scaled_x -= gyro_offset_x
gyro_scaled_y -= gyro_offset_y
gyro_scaled_z -= gyro_offset_z
gyro_x_delta = (gyro_scaled_x * time_diff)
gyro_y_delta = (gyro_scaled_y * time_diff)
gyro_z_delta = (gyro_scaled_z * time_diff)
gyro_total_x += gyro_x_delta
gyro_total_y += gyro_y_delta
gyro_total_z += gyro_z_delta
rotation_x = get_x_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)
rotation_y = get_y_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)
rotation_z = get_z_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)
last_x = K * (last_x + gyro_x_delta) + (K1 * rotation_x)
last_y = K * (last_y + gyro_y_delta) + (K1 * rotation_y)
last_z = K * (last_z + gyro_z_delta) + (K1 * rotation_z)
rotation = str(rotation_x)+" "+str(rotation_y)+" "+str(rotation_z)
return rotation
if __name__ == "__main__":
# Now wake the 6050 up as it starts in sleep mode
bus.write_byte_data(address, power_mgmt_1, 0)
app = web.application(urls, globals())
app.run()
Puede aparecernos un error en el fichero del server.py deberemos se arreglarlo llegando a la línea que contiene lo siguiente: bus = smbus.SMBus(1) # or bus = smbus.SMBus(1) for Revision 2 boards.
Esta línea nos dice que hay dos versiones de Smbus y si vemos que falla este, tendremos que poner smbus(1), es decir poner lo siguiente: bus = smbus.SMBus(1) cambiar a bus = smbus.SMBus(0)
Una vez que hemos realizado este código python en el servidor, nos dispondremos a crear el fichero client.py y que contendrá el siguiete código:
#!/usr/bin/python
import pygame
import urllib
from OpenGL.GL import *
from OpenGL.GLU import *
from OpenGL.GLUT import *
from math import radians
from pygame.locals import *
import sys
SCREEN_SIZE = (800, 600)
SCALAR = .5
SCALAR2 = 0.2
def resize(width, height):
glViewport(0, 0, width, height)
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
gluPerspective(45.0, float(width) / height, 0.001, 10.0)
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
gluLookAt(0.0, 1.0, -5.0,
0.0, 0.0, 0.0,
0.0, 1.0, 0.0)
def init():
glEnable(GL_DEPTH_TEST)
glClearColor(0.0, 0.0, 0.0, 0.0)
glShadeModel(GL_SMOOTH)
glEnable(GL_BLEND)
glEnable(GL_POLYGON_SMOOTH)
glHint(GL_POLYGON_SMOOTH_HINT, GL_NICEST)
glEnable(GL_COLOR_MATERIAL)
glEnable(GL_LIGHTING)
glEnable(GL_LIGHT0)
glLightfv(GL_LIGHT0, GL_AMBIENT, (0.3, 0.3, 0.3, 1.0));
def read_values():
link = "http://127.0.0.1:8080" # Change this address to your settings
f = urllib.urlopen(link)
myfile = f.read()
return myfile.split(" ")
def run():
pygame.init()
screen = pygame.display.set_mode(SCREEN_SIZE, HWSURFACE | OPENGL | DOUBLEBUF)
resize(*SCREEN_SIZE)
init()
clock = pygame.time.Clock()
angle = 0
glutInit(sys.argv)
while True:
then = pygame.time.get_ticks()
for event in pygame.event.get():
if event.type == QUIT:
return
if event.type == KEYUP and event.key == K_ESCAPE:
return
values = read_values()
x_angle = values[0]
y_angle = values[1]
z_angle = values[2]
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
glColor((1.,1.,1.))
glLineWidth(1)
glBegin(GL_LINES)
for x in range(-20, 25, 5):
glVertex3f(x/10.,-1,-1)
glVertex3f(x/10.,-1,1)
for x in range(-20, 25, 5):
glVertex3f(x/10.,-1, 1)
glVertex3f(x/10., 1, 1)
for z in range(-10, 15, 5):
glVertex3f(-2, -1, z/10.)
glVertex3f( 2, -1, z/10.)
for z in range(-10, 15, 5):
glVertex3f(-2, -1, z/10.)
glVertex3f(-2, 1, z/10.)
for z in range(-10, 15, 5):
glVertex3f( 2, -1, z/10.)
glVertex3f( 2, 1, z/10.)
for y in range(-10, 15, 5):
glVertex3f(-2, y/10., 1)
glVertex3f( 2, y/10., 1)
for y in range(-10, 15, 5):
glVertex3f(-2, y/10., 1)
glVertex3f(-2, y/10., -1)
for y in range(-10, 15, 5):
glVertex3f(2, y/10., 1)
glVertex3f(2, y/10., -1)
glEnd()
glPushMatrix()
glRotate(float(x_angle), 1, 0, 0)
glRotate(-float(y_angle), 0, 0, 1)
glRotate(-float(z_angle), 0, 1, 0)
glutWireTeapot(1)
#glutSolidTeapot(1)
glPopMatrix()
pygame.display.flip()
if __name__ == "__main__":
run()
Una vez que hayáis arreglado si nos ha dado error tendremos que ejecutar los dos ficheros con los siguientes comandos:
-python server.py y python client.py y ya nos aparecerá la tetera y podremos mover el sensor y la tetera se moverá al mismo tiempo, lo que veréis será lo sigueinte:
ECon esto ya tendremos nuestro sensor de posicionamiento y aceleración configurado en el que podremos ver como va girando al tacita a la vez que se gira el sensor de posicionamiento, y si os surge alguna duda podréis dejarlos siempre en el apartado ¿Tienes dudas? espero que os haya gustado este POST y que veáis todo lo que ha evolucionado el proyecto Heimdall desde que comenzó y que sobre todo os funcione este sensor. Por parte de Juan José Molina Garrido, Ángel Femenia Rocabado y Francisco José Ruiz Quesada Un gran saludo.
ENLACE DE INTERÉS:
Aquí os dejo el enlace del que sacamos este código de Python para que pudieramos ver la tetera y saber en que posición se encuentra el sensor en ese momento: