1 #! /usr/bin/python
   2 #coding=utf-8
   3 """Programita para probar el carro
   4 """
   5 
   6 from serial import Serial
   7 import struct
   8 import trama
   9 import sys
  10 
  11 
  12 class AGV(Serial):
  13 
  14   modos = dict(
  15   AVANZAR = "+"*4,
  16   RETROCEDER = "-"*4,
  17   FRENAR = "F"*4,
  18   POSICION = "P"*4,
  19   )
  20 
  21   def __init__(self, *args, **kwargs):
  22     #Asegura que el timeout tenga un valor rasonable
  23     timeout = kwargs.get('timeout',0.1)
  24     if timeout < 0.01: timeout = 0.1
  25     kwargs['timeout'] = timeout
  26     Serial.__init__(self, *args, **kwargs)
  27     self.buf = ''
  28 
  29   def readline(self, timeout=1):
  30     """timeout en segundos es el máximo tiempo que debe esperar para una trama completa"""
  31     tries = 0
  32     while 1:
  33       self.buf += self.read(512)
  34       pos = self.buf.find('*')
  35       if pos > 0:
  36         line, self.buf = self.buf[:pos+1], self.buf[pos+1:]
  37         return line
  38       tries += 1
  39       if tries * self.timeout > timeout:
  40         break
  41     line, self.buf = self.buf, ''
  42     return line
  43 
  44   def readlines(self, sizehint=None, timeout=1):
  45     """Lee todas las tramas que hay disponibles. Aborta después del timeout
  46     o cuando no hay mas datos."""
  47     lines = []
  48     while 1:
  49       line = self.readline(timeout=timeout)
  50       if line:
  51         lines.append(line)
  52       if not line or line[-1:] != '*':
  53         break
  54     return lines
  55 
  56   def trama2int(self, lista):
  57     """Devuelve el valor en entero obtenido del método leer registro"""
  58     cadena = "".join(lista)
  59     if len(cadena) != 6:
  60       return
  61     valor = ""
  62     #Sacamos el @ y el registro que pedimos, por eso empezamos del 2
  63     for i in xrange(2, len(cadena)):
  64       valor += cadena[i]
  65     return struct.unpack("i", valor)
  66 
  67   def imprimir_registros(self, *muchos_registros):
  68     """Pide con leer registro el valor y luego lo
  69     imprime en pantalla"""
  70     salida_pantalla = ""
  71     for registro in muchos_registros:
  72       self.write(trama.leer_registro(registro))
  73       salida_pantalla += "%s %s    " % (registro, self.trama2int(self.readlines(timeout=0.01)))
  74     print salida_pantalla
  75 
  76   def set_registro(self, registro, dato):
  77     """Le envía el valor del registro correspondiente"""
  78     if type(dato) == str:
  79       dato_int = int(dato, 16)
  80     else:
  81       dato_int = dato
  82     dato_string = struct.pack("i", dato_int)
  83     self.write(trama.dato(registro, dato_string))
  84 
  85   def reset(self):
  86     """Envía reset al Carro"""
  87     self.write(trama.leer_registro("RESET"))
  88 
  89   def prender(self):
  90     """Envía power al Carro"""
  91     self.write(trama.dato("PRENDER", "0000"))
  92 
  93   def apagar(self):
  94     """Envía apagar al Carro"""
  95     self.write(trama.dato("APAGAR", "0000"))
  96 
  97   def set_modo(self, modo):
  98     """Setea el modo de funcionamiento"""
  99     self.write(trama.dato("MODO", self.modos[modo]))
 100 
 101 if __name__=='__main__':
 102   PORT = "/dev/ttyUSB0"
 103   BAUDRATE = 9600
 104   try:
 105     modulo_config = sys.argv[1]
 106     if modulo_config.endswith(".py"): modulo_config = modulo_config[:len(modulo_config)-3:]
 107   except:
 108     print "hay que pasar un archivo de configuración"
 109   config = __import__(modulo_config)
 110   s = AGV(PORT, BAUDRATE)
 111   s.prender()
 112   s.reset()
 113   s.set_modo("POSICION")
 114   s.set_registro("Ki", config.Ki)
 115   s.set_registro("Kd", config.Kd)
 116   s.set_registro("Kp", config.Kp)
 117   s.set_registro("ACELERACION", config.ACELERACION)
 118   s.set_registro("FRENADO", config.FRENADO)
 119   s.set_registro("VELOCIDAD_FINAL", config.VELOCIDAD_FINAL)
 120   s.set_registro("POSICION_FINAL", 600)
 121   s.imprimir_registros("Ki", "Kd", "Kp", "ACELERACION", "POSICION_FINAL")
 122   for j in range(50):
 123     s.imprimir_registros("CUENTA", "VELOC_ACTUAL")
 124   s.set_registro("POSICION_FINAL", 0)
 125   for j in range(50):
 126     s.imprimir_registros("CUENTA", "VELOC_ACTUAL")
 127   s.reset()
 128   s.apagar()