Manifiesto

Nosotros exploramos... y ustedes nos llaman criminales. Nosotros buscamos ampliar nuestro conocimiento... y ustedes nos llaman criminales. Nosotros existimos sin color de piel, sin nacionalidad, sin prejuicios religiosos... y ustedes nos llaman criminales. Ustedes construyen bombas atómicas, hacen la guerra, asesinan, engañan, y nos mienten y tratan de hacernos creer que es por nuestro bien, ahora nosotros somos los criminales.

Fragmento de "La Conciencia de un Hacker"

11 mar 2012

Control servo por USB Pinguino + PyGTK



He continuado haciendo pequeños ejemplos con pinguino (http://pinguino.cc/) y pyGTK.
En esta ocasión controlo la posición de un servo a través de un programita en python que se conecta por USB a pinguino.
Del lado de pinguino tenemos:


// File: ServoPC.pde
//
// February 2012
// aztk 
//
// Servo control from PC

void setup()
{
     servo.attach(0);
     servo.setMinimumPulse(0);
}
 
void loop()
{
 if (USB.available()) 
 {
  u8 value_s[1];
  value_s[0] = USB.read();
          servo.write(0, value_s[0]);
 }
}


Nota: la biblioteca servo de pinguino está diseñada para servos que tienen su espectro de 1000us a 2000us, si como yo usan otro tipo de servo tienen que modificarla, ésta se encuentra en pinguino_beta9-05_linux/tools/share/sdcc/include/pic16/servos.c
Por ejemplo, yo usé servos que van de 600us a 2400us (HS-322HD) por lo que en las partes que corresponde dejé los sig valores:
Referencias: rutina para controlar 18 servos (creador de la biblioteca)


static void ServosPulseDown()
{
 timingindex = 0;
 
 for(timedivision=0;timedivision < 251;timedivision++){
  if (timings[timevalue][timingindex] == timedivision){
   PORTB = PORTB ^ timings[MaskPort_B][timingindex];
   PORTC = PORTC ^ timings[MaskPort_C][timingindex];
   PORTA = PORTA ^ timings[MaskPort_A][timingindex];
   timingindex++;
  }
  // the following routine adds the requiered delay for every tick of timedivision, so every timedivision last 4 usec.
  // ******** CAMBIADO 6 -> 0x12 ********************** 0x0d
  __asm 
   movlw 0x0e
   movwf _loopvar
  bucle:
   NOP
   decfsz _loopvar,1
   goto bucle
  __endasm;
 }
}



//interrupt handler that handles servos
void servos_interrupt(void)
{
 if (PIR1bits.TMR1IF) {
  PIR1bits.TMR1IF=0;
  T1CON=0x00;
  if (phase) {
  //case before 1st ms:
   ServosPulseUp();
   // Load at TMR1 54159d(also 0xFFFF - 12000d (- 54usec for adjustments)).
   // ************* CAMBIADO *************
   TMR1H= 0xe6; //0xd3;
   TMR1L= 0x00; //0x8f;
   // timer 1 prescaler 1 source is internal oscillator Fosc/4 (CPU clock or Fosc=48Mhz).
   T1CON=1;
   phase = 0;
  } 
  else {
  //case before 2nd ms:
   //The following call takes 1 ms aprox.:
   ServosPulseDown();
   // Now it takes place a 18 ms delay, after that a interrupt will be generated.
   // Loading at TMR1 11535d (que es: 0xFFFF - (4,5 x 12000(duracion 1ms)) = 0x2D0F => a 4,5 ms) 
   // This 4,5 x 4 (with preescaler x 4) we get 18 ms delay.
   TMR1H= 0x2d;
   TMR1L= 0x0f;
   // timer 1 prescaler 1 source is internal oscillator Fosc/4 (recordemos que Fosc=48Mhz).
   if (needreordering) 
    SortServoTimings();  // This takes more than 1 ms, but it's call only if needed.
   T1CON= ( 1 | 2 << 4 ) ; // activate timer1 and prescaler = 1:4
   phase = 1;  //This indicates that after next interrupt it will start the servos cycle.
  } 
 }
 return;
}


Ese ajuste lo hice primeramente calculándolo, y retocándolo con pruebas en su funcionamiento.

Muy bien, ya que tenemos el firmware apropiado del lado de pinguino, del lado del PC va el siguiente programa python:


#!/usr/bin/env python
# -*- coding: utf-8 -*-
#
#       control_servo.py
#       
#       Copyleft 2012 aztk 
#
#       Control de un servo conectado a pinguino.
#       
#       This program is free software; you can redistribute it and/or modify
#       it under the terms of the GNU General Public License as published by
#       the Free Software Foundation; either version 2 of the License, or
#       (at your option) any later version.
#       
#       This program is distributed in the hope that it will be useful,
#       but WITHOUT ANY WARRANTY; without even the implied warranty of
#       MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#       GNU General Public License for more details.
#       
#       You should have received a copy of the GNU General Public License
#       along with this program; if not, write to the Free Software
#       Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
#       MA 02110-1301, USA.
 
import usb  # requires pyusb available at https://sourceforge.net/projects/pyusb/files/ 

import pygtk
pygtk.require('2.0')
import gtk
            
#-------------------------------------------------------------------------------
# Pinguino Class by Marin Purgar (marin.purgar@gmail.com)
#------------------------------------------------------------------------------- 
class Pinguino():
 
 VENDOR = 0x04D8
 PRODUCT = 0xFEAA
 CONFIGURATION = 3
 INTERFACE = 0
 ENDPOINT_IN = 0x82
 ENDPOINT_OUT = 0x01
 
 device = None
 handle = None
 
 def __init__(self,):
  for bus in usb.busses():
   for dev in bus.devices:
    if dev.idVendor == self.VENDOR and dev.idProduct == self.PRODUCT:
     self.device = dev
  return None
 
 def open(self):
  if not self.device:
   print "Unable to find device!"
   return None
  try:
   self.handle = self.device.open()
   self.handle.setConfiguration(self.CONFIGURATION)
   self.handle.claimInterface(self.INTERFACE)
  except usb.USBError, err:
   print err
   self.handle = None
  return self.handle
 
 def close(self):
  try:
   self.handle.releaseInterface()
  except Exception, err:
   print err
  self.handle, self.device = None, None
 
 def read(self, length, timeout = 0):
  return self.handle.bulkRead(self.ENDPOINT_IN, length, timeout)
 
 def write(self, buffer, timeout = 0):
  return self.handle.bulkWrite(self.ENDPOINT_OUT, buffer, timeout)

# Ventana principal
class Ventana:
    def __init__(self):
        # Create a new window
        self.window = gtk.Window(gtk.WINDOW_TOPLEVEL)
        self.window.set_title("PinguinoUSBServo")
        self.window.resize(300, 80)
        self.window.connect('destroy', self.destroy)

        self.vbox1 = gtk.VBox(False, 10)
        
        # Control scale
        self.adjustment = gtk.Adjustment(0, 0, 250, 1, 1, 0)
        self.scale1 = gtk.HScale(self.adjustment)
        #self.scale1.set_update_policy(gtk.UPDATE_DELAYED)
        self.scale1.set_digits(0)
        self.scale1.set_draw_value(False)
        self.vbox1.pack_start(self.scale1, False, False, 3)
        
        # Etiqueta
        self.labelv = gtk.Label("Welcome")
        self.vbox1.pack_start(self.labelv, False, False, 3)
        
        # Empaquetamos y mostramos
        self.window.add(self.vbox1)
        self.window.show_all()
        
        # Conectamos senales y eventos
        self.adjustment.connect('value_changed', self.enviarcmd, self.labelv)
       
        print "Wellcome"
        
    def enviarcmd(self, widget, label):
  # Enviamos el valor del scale al pinguino
  label.set_text("angle: " + str(round(widget.value) * 0.72) + "º")
  pinguino.write(chr(widget.value), 1)
  #print "value: " + str(widget.value)
   
    def destroy(self, widget, data = None):
  print "Goodbye"
  gtk.main_quit()
        
    def main(self):
  gtk.main()

if __name__ == '__main__':
 ventana = Ventana()
 pinguino = Pinguino()
 if pinguino.open() == None:
  print "Unable to open Pinguino device!"
  exit(1)
 pinguino.write(chr(0), 1)
 ventana.main()
 pinguino.write(chr(0), 1)
 pinguino.close()


Vean el vídeo mostrando el funcionamiento. Espero que la información presentada les sea de utilidad para sus proyectos.

Saludos!!!

2 comentarios:

  1. Hola, estoy comenzando con esto de pinguino, logre construir mi propia placa, pero no consigo como programar los servos. Use tu primer codigo y al compilar me lanza error.

    ResponderEliminar

Hey you!
Deja un comentario! va?!