Código Raspberry

monkeyserna edited this page May 26, 2014 · 16 revisions

El servidor de comandos realiza una gran parte de las funcionalidades de la Raspberry Pi. Está escrito en lenguaje python, y sus tareas son:

  • Crear un servidor de WebSocket para recibir comandos y scripts desde Android.
  • Establecer una comunicación serie por USB con Arduino para enviar los distintos comandos disponibles.
  • Realizar lecturas del sonar.
  • Controlar el servo.
  • Indicar la ejecución de un script (proveniente del modo de programación) mediante un LED.

Como particularidad permite crear un servidor WebSocket cifrado con SSL, si se ejecuta con la opción --ssl --cert <certificado> al arrancar el servidor.

El fichero se encuentra en la carpeta raíz del servidor, por defecto en /home/pi/server/cmdserver.py.

A continuación se describe el código fuente de este archivo.

## CONSTANTES

Al inicio del ficero se definen una serie de valores. Concretamente:

 * `SERVER_IP = ''` IP del servidor. Por defecto localhost. 
 * `WS_PORT = 8000` Se indica el puerto 8000 para el WebSocket.
 * `LEDPIN = 4` Pin al que se conecta el circuito del led.
 * `SERVOPIN = 17` Pin de la Raspberry al que se conecta la toma de control del servomotor.
 * `ECHOPIN = 27` Pin al que se conecta la toma ECHO del sonar.
 * `TRIGPIN = 17` Pin al que se conecta la toma TRIG del sonar.
 * `SONARTIMEOUT = 500` Tiempo máximo de espera entre que se emite el ultrasonido (TRIG) y se recibe (ECHO).
 * `DUE_PORT =  '/dev/ttyACM0'` Punto de montaje de la Arduino Due.
 * `DUE_BAUDS = 9600` Velocidad de la transmisión USB con el Arduino (deben tener ambas la misma velocidad).

##### Código fuente
```python
SERVER_IP = ''
WS_PORT = 8000

LEDPIN = 4
SERVOPIN = 17
ECHOPIN = 27
TRIGPIN = 22
SONARTIMEOUT = 500

DUE_PORT =  '/dev/ttyACM0' # 'COM7' #
DUE_BAUDS = 9600

main

Este bloque de código se ejecuta al comenzar el servidor.

En primer lugar, hace un análisis sintáctico de los parámetros recibidos. Se pueden especificar los siguientes parámetros:

  • --host Especifica la IP. Por defecto '', equivalente a localhost.
  • --port Especifica el puerto del servidor. Por defecto 8000.
  • --ssl Si se especifica esta opción se crea un websocket cifrado
  • --cert Certificado de seguridad
  • --ver Versión de SSL

Una vez obtenidos los parámetros, se define un destructor del servidor para cerrar el servidor en caso de recibir una interrupción de sistema (Como por ejemplo la interrupción resultante de pulsar Ctrl+C en el terminal).

A continuación se inicia el servo, haciendo uso de la clase PWM disponible en la librería RPIO.

Una vez inicializado el servo, se inicia la comunicación por USB con arduino.

Finalmente se ejecuta el servidor de WebSocket.

if __name__ == "__main__":
	parser = OptionParser(usage="usage: %prog [options]", version="%prog 1.0")
	parser.add_option("--host", default=SERVER_IP, type='string', action="store", dest="host", help="hostname (localhost)")
	parser.add_option("--port", default=WS_PORT, type='int', action="store", dest="port", help="port (8000)")
	parser.add_option("--ssl", default=0, type='int', action="store", dest="ssl", help="ssl (1: on, 0: off (default))")
	parser.add_option("--cert", default='./cert.pem', type='string', action="store", dest="cert", help="cert (./cert.pem)")
	parser.add_option("--ver", default=ssl.PROTOCOL_TLSv1, type=int, action="store", dest="ver", help="ssl version")
	(options, args) = parser.parse_args()

	if options.ssl == 1:
		server = SimpleSSLWebSocketServer(options.host, options.port, WebServer, options.cert, options.cert, version=options.ver)
	else:	
		server = SimpleWebSocketServer(options.host, options.port, WebServer)
	
	def close_sig_handler(signal, frame):
		server.close()
		sys.exit()	
	signal.signal(signal.SIGINT, close_sig_handler)

	servo = PWM.Servo()
	serialArduino = serial.Serial(DUE_PORT,DUE_BAUDS) 
	server.serveforever()

WebServer

Esta clase hereda de WebSocket, una clase definida en la librería SimpleWebSocketServer, importada previamente. En ella, se define el comportamiento cuando ocurren 3 eventos en la comunicación: Conexión, Recepción y Desconexión.

Al conectarse un cliente (generalmente el dispositivo Android), únicamente se muestra un mensaje por terminal.

Cuando el cliente se desconecta, ademas del mensaje informativo escrito en la terminal, se apaga el servomotor para disminuir el consumo eléctrico. Este servo volverá a encenderse cuando reciba una señal de posición (no es necesario volver a inicializarlo). Para garantizar que el servo ha terminado de realizar su movimiento, esta función espera 2 segundos antes de enviar la señal de apagado al servo.

Cuando se recibe un mensaje, pueden ocurrir 3 casos:

  • Si mensaje contiene 2 bytes, únicamente puede ser un comando de movimiento del servo. Por lo que se envía su valor a la función moveServo()
  • Si el mensaje contiene 3 bytes, es un comando arduino. En este caso se reenvía el mensaje de 3 bytes a Arduino mediante comunicación USB.
  • Si el mensaje contiene más de 3 bytes, es un script. En este caso se reenvía el mensaje a la función executeScript(msg). Una vez ejecutado el script, se cierra la comunicación.
class WebServer(WebSocket):
	def handleMessage(self):
		if self.data is None:
			self.data = ''                            
		try:
			msg = str(self.data)
			
			if len(msg) == 3:
				cmd = struct.unpack('B', msg[0])[0]
				p1 = struct.unpack('B', msg[1])[0]
				p2 = struct.unpack('B', msg[2])[0]
				print cmd, p1,p2	

				serialArduino.write(msg)
			elif len(msg) == 2:
				p1 = struct.unpack('B', msg[1])[0] * 10
				moveServo(p1)
			else:
				executeScript(msg)
				self.sendClose()
		except Exception as n:
			print "  ERROR:", n
		
	def handleConnected(self):
		print self.address, 'connected'

	def handleClose(self):
		time.sleep(2)
		servo.stop_servo(SERVOPIN)
		print self.address, 'closed'

Funciones del modo Code

El modo de programación gráfica envía mediante WebSockets scripts en Python que son ejecutados en el mismo servidor, haciendo uso de la función exec y otras funciones auxiliares que definen comportamientos propios. Concretamente las funciones auxiliares que son ejecutadas desde scripts provenientes del modo de programación son las siguientes:

  • readSonar() Devuelve el valor leído por el sonar en centímetros.
  • moveServo(pos) Establece la posición del servo.
  • sendToArduino(cmd,p1,p2) Envía un comando a Arduino (mover motor, encender led,...)
  • receiveFromArduino(cmd,p1,p2) Envía un comando a Arduino y recibe un mensaje de respuesta (lecutra de sensores)
  • map(x, in_min, in_max, out_min, out_max) Realiza un escalado de x desde [in_min,in_max] hasta [out_min,out,max]

Cuando el cliente Android (o web) envía un script websocket, el bucle principal realiza lo siguiente:

executeScript(msg)
self.sendClose()

Es decir, llama a la función executeScript(msg) con el bloque de código a ejecutar y una vez finalizada la ejecución finaliza la comunicación.

executeScript(msg)

Esta función es invocada al recibir un script por WebSocket.

Parámetros

Recibe el parámetro:

  1. msg: Script realizado en Blockly (y traducido a Python) a ejecutar.
Descripción de la función

Esta función ejecuta un script escrito en Blockly ya traducido a Python. Antes de realizar la ejecución con exec, invoca a la función de inicialiación. Una vez ejecutado el script, llama al destructor.

Código fuente
def executeScript(receivedCommand):
	print "SCRIPT RECEIVED:\n",receivedCommand
	initScript()
	print "STARTING BLOCKLY SCRIPT..."
	exec(receivedCommand)
	print "...BLOCKLY SCRIPT DONE"
	destructScript()

initScript()

Esta función se invoca antes de comenzar la ejecución de un script realizado en el modo de programación.

Parámetros

No recibe parámetros.

Descripción de la función

Esta función realiza las acciones necesarias, previas a la ejecución de un bloque escrito mediante Blockly.

Realiza 2 acciones:

  1. Inicializa el sonar.
  2. Enciende el led que indica que un script está siendo ejecutado.
Código fuente
def initScript():
	print "INIT SONAR..."
	GPIO.setwarnings(False)
	GPIO.setmode(GPIO.BCM)	
	GPIO.setup(LEDPIN,GPIO.OUT)	
	GPIO.setup(TRIGPIN,GPIO.OUT)
	GPIO.setup(ECHOPIN,GPIO.IN)
	GPIO.output(LEDPIN, GPIO.HIGH)
	GPIO.output(TRIGPIN, GPIO.LOW)
	readSonar() #FIRST READ IS UNSTABLE
	time.sleep(0.1)
	print "...SONAR DONE"

destructScript()

Esta función se invoca una vez finalizada la ejecución del script realizado en el modo de programación.

Parámetros

No recibe parámetros.

Descripción de la función

Realiza las acciones necesarias una vez finalizada la ejecución de un bloque escrito en el modo de programación gráfica. Apaga el led y desactiva el uso de los pines en la Raspberry Pi.

Código fuente
def destructScript():	
	GPIO.output(LEDPIN, GPIO.LOW)
	GPIO.cleanup()	

readSonar()

Esta función es invocada mediante Blockly con el bloque read distance (cm).`.

Parámetros

No recibe parámetros.

Descripción de la función

Devuelve la lectura del sonar medida en centímetros.

Envía un ultrasonido activando el pin TRIG durante 0.01 milisegundos y espera a que la señal ECHO emita un pulso alto, que indica la recepción del sonido. Con el tiempo transcurrido desde el envío del ultrasonido hasta su recepción se calcula la distancia, medida en centímetros.

Código fuente
def readSonar():
	GPIO.output(TRIGPIN, True)
	time.sleep(0.00001)
	GPIO.output(TRIGPIN, False)

	maxTime = SONARTIMEOUT
	while GPIO.input(ECHOPIN) == 0 and maxTime > 0:
		maxTime -= 1	
	signaloff = time.time()
	if maxTime == 0:
		return -1

	maxTime = SONARTIMEOUT
	while GPIO.input(ECHOPIN) == 1 and maxTime > 0:
		maxTime -= 1	
	signalon = time.time()	
	if maxTime == 0:
		return -2	

	distance = (signalon - signaloff) * 17953.27521508037
	
	print "  SONAR:",distance		
	return distance

moveServo(pos)

Esta función se invoca mediante el bloque set servo position [-35,150] desde el modo de programación gráfica, y desde el modo de conducción libre cuando se desplaza la barra vertical.

Parámetros
  1. pos Posición a la que se establece el servo. El rango de actuación es [500,2500].
Descripción de la función

Establece la posición del servo, dentro de su rango de actuación [500,2500], donde:

  • 500 forma un ángulo de 180º sobre la horziontal (apunta hacia atrás).
  • 2500 forma un ángulo de -35º sobre la horziontal (apunta hacia abajo).

Si el valor recibido no está en contemplado en el rango de actuación del servo, se detiene en el límite de su rango de actuación. Es decir, si recibe como valor 3500, la posición a la que se orienta el servo es 2500. Si recibe 100, el movimiento que hará será el equivalente a recibir 500.

Código fuente
def moveServo(pos):
	servo.set_servo(SERVOPIN,pos)	
	print "  SERVO:", pos

sendToArduino(cmd,p1,p2)

Esta función es invocada por todos los bloques de movimiento de motores, el control del zumbador, y de los leds. Algunos ejemplos:

  • Move straight fast se traduce como sendToArduino(4,255,0).
  • Set led 2 ON se traduce como sendToArduino(12,2,1).
Parámetros

Recibe los parámetros:

  1. cmd Comando a enviar a Arduino
  2. p1 Parámetro 1 a enviar a Arduino
  3. p2 Parámetro 2 a enviar a Arduino
Descripción de la función

Esta función envía un comando de 3 bytes a Arduino mediante comunicación serie por puerto USB.

Código fuente
def sendToArduino(cmd,p1,p2):
	msg = bytearray(3)
	msg[0] = struct.pack('B', int(cmd))
	msg[1] = struct.pack('B', int(p1))
	msg[2] = struct.pack('B', int(p2))
	serialArduino.write(msg)
	print "   SEND:", msg[0], msg[1], msg[2]

receiveFromArduino(cmd,p1,p2)

Esta función es invocada por los bloques de lectura de sensores (todos los situados en el directorio Ground sensors).

Parámetros

Recibe los parámetros:

  1. cmd Comando a enviar a Arduino
  2. p1 Parámetro 1 a enviar a Arduino
  3. p2 Parámetro 2 a enviar a Arduino
Descripción de la función

Esta función envía un comando de 3 bytes a Arduino mediante comunicación serie por puerto USB y espera a recibir un mensaje de respuesta. La función devuelve el valor recibido desde Arduino.

Código fuente
def receiveFromArduino(cmd,p1,p2):
	sendToArduino(cmd,p1,p2)
	recMsg = serialArduino.readline().strip()
	print "RECEIVE:",recMsg		
	return recMsg

map(x, in_min, in_max, out_min, out_max)

Esta función es invocada mediante el bloque map.

Parámetros

Recibe los parámetros:

  1. x Valor a escalar
  2. in_min Cota inferior del valor a escalar.
  3. in_max Cota superior del valor a escalar.
  4. out_min Cota inferior del valor resultante.
  5. out_max Cota superior del valor resultante.
Descripción de la función

Escala el valor x desde el rango [in_min, in_max] hasta [out_min, out_max].

Por ejemplo, map(13,2,4,5,6) devuelve 10.5.

Código fuente
def map(x, in_min, in_max, out_min, out_max):
	try:
		res = (float(x) - float(in_min)) * (float(out_max) - float(out_min)) / (float(in_max) - float(in_min)) + float(out_min)		
		return res
	except Exception as n:
			print "  ERROR:", n
			return 0

Código fuente completo

#!/usr/bin/python
import  time,sys, struct, serial, signal, ssl, logging
import RPi.GPIO as GPIO
from math import sqrt
from SimpleWebSocketServer import WebSocket, SimpleWebSocketServer, SimpleSSLWebSocketServer
from optparse import OptionParser
from RPIO import PWM

logging.basicConfig(format='%(asctime)s %(message)s', level=logging.DEBUG)

SERVER_IP = ''
WS_PORT = 8000

LEDPIN = 4
SERVOPIN = 17
ECHOPIN = 27
TRIGPIN = 22
SONARTIMEOUT = 500

DUE_PORT =  '/dev/ttyACM0' # 'COM7' #
DUE_BAUDS = 9600
	
###########SONAR FUNCTIONS ###########
		
def initScript():
	print "INIT SONAR..."
	GPIO.setwarnings(False)
	GPIO.setmode(GPIO.BCM)	
	GPIO.setup(LEDPIN,GPIO.OUT)	
	GPIO.setup(TRIGPIN,GPIO.OUT)
	GPIO.setup(ECHOPIN,GPIO.IN)
	GPIO.output(LEDPIN, GPIO.HIGH)
	GPIO.output(TRIGPIN, GPIO.LOW)
	readSonar() #FIRST READ IS UNSTABLE
	time.sleep(0.1)
	print "...SONAR DONE"
	
def readSonar():
	GPIO.output(TRIGPIN, True)
	time.sleep(0.00001)
	GPIO.output(TRIGPIN, False)

	maxTime = SONARTIMEOUT
	while GPIO.input(ECHOPIN) == 0 and maxTime > 0:
		maxTime -= 1	
	signaloff = time.time()
	if maxTime == 0:
		return -1

	maxTime = SONARTIMEOUT
	while GPIO.input(ECHOPIN) == 1 and maxTime > 0:
		maxTime -= 1	
	signalon = time.time()	
	if maxTime == 0:
		return -2	

	distance = (signalon - signaloff) * 17953.27521508037
	
	print "  SONAR:",distance		
	return distance
		
def destructScript():	
	GPIO.output(LEDPIN, GPIO.LOW)
	GPIO.cleanup()			

def moveServo(pos):
	servo.set_servo(SERVOPIN,pos)	
	print "  SERVO:", pos
		
def sendToArduino(cmd,p1,p2):
	msg = bytearray(3)
	msg[0] = struct.pack('B', int(cmd))
	msg[1] = struct.pack('B', int(p1))
	msg[2] = struct.pack('B', int(p2))
	serialArduino.write(msg)
	print "   SEND:", msg[0], msg[1], msg[2]

def receiveFromArduino(cmd,p1,p2):
	sendToArduino(cmd,p1,p2)
	recMsg = serialArduino.readline().strip()
	print "RECEIVE:",recMsg		
	return recMsg

	
def map(x, in_min, in_max, out_min, out_max):
	try:
		res = (float(x) - float(in_min)) * (float(out_max) - float(out_min)) / (float(in_max) - float(in_min)) + float(out_min)		
		return res
	except Exception as n:
			print "  ERROR:", n
			return 0
	
def executeScript(receivedCommand):
	print "SCRIPT RECEIVED:\n",receivedCommand
	initScript()
	print "STARTING BLOCKLY SCRIPT..."
	exec(receivedCommand)
	print "...BLOCKLY SCRIPT DONE"
	destructScript()
	
class WebServer(WebSocket):
	def handleMessage(self):
		if self.data is None:
			self.data = ''                            
		try:
			msg = str(self.data)
			
			if len(msg) == 3:
				cmd = struct.unpack('B', msg[0])[0]
				p1 = struct.unpack('B', msg[1])[0]
				p2 = struct.unpack('B', msg[2])[0]
				print cmd, p1,p2	

				serialArduino.write(msg)
			elif len(msg) == 2:
				p1 = struct.unpack('B', msg[1])[0] * 10
				moveServo(p1)
			else:
				executeScript(msg)
				self.sendClose()
		except Exception as n:
			print "  ERROR:", n
		
	def handleConnected(self):
		print self.address, 'connected'

	def handleClose(self):
		time.sleep(2)
		servo.stop_servo(SERVOPIN)
		print self.address, 'closed'

		
if __name__ == "__main__":
	parser = OptionParser(usage="usage: %prog [options]", version="%prog 1.0")
	parser.add_option("--host", default=SERVER_IP, type='string', action="store", dest="host", help="hostname (localhost)")
	parser.add_option("--port", default=WS_PORT, type='int', action="store", dest="port", help="port (8000)")
	parser.add_option("--ssl", default=0, type='int', action="store", dest="ssl", help="ssl (1: on, 0: off (default))")
	parser.add_option("--cert", default='./cert.pem', type='string', action="store", dest="cert", help="cert (./cert.pem)")
	parser.add_option("--ver", default=ssl.PROTOCOL_TLSv1, type=int, action="store", dest="ver", help="ssl version")
	(options, args) = parser.parse_args()

	if options.ssl == 1:
		server = SimpleSSLWebSocketServer(options.host, options.port, WebServer, options.cert, options.cert, version=options.ver)
	else:	
		server = SimpleWebSocketServer(options.host, options.port, WebServer)
	
	def close_sig_handler(signal, frame):
		server.close()
		sys.exit()
	
	servo = PWM.Servo()
	serialArduino = serial.Serial(DUE_PORT,DUE_BAUDS) 
	signal.signal(signal.SIGINT, close_sig_handler)
	server.serveforever()
You can’t perform that action at this time.
You signed in with another tab or window. Reload to refresh your session. You signed out in another tab or window. Reload to refresh your session.
Press h to open a hovercard with more details.