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:

Código:


// File: ServoPC.pde
//
// February 2012
// aztk <aztecaymaya@gmail.com>
//
// 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 (http://servocity.com/html/hs-322hd_standard_deluxe.html) por lo que en las partes que corresponde dejé los sig valores:
Referencias: http://linuxdroids.wordpress.com/2010/07/29/pinguino-rutina-para-controlar-18-servos/

Código:


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 -> 0x0e *********************
      __asm
         movlw 0x0e
         movwf _loopvar
      bucle:
         NOP
         decfsz _loopvar,1
         goto bucle
      __endasm;
   }
}


Código:


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:

Código:


#!/usr/bin/env python
# -*- coding: utf-8 -*-
#
#      control_servo.py
#     
#      Copyleft 2012 aztk <aztecaymaya@gmail.com>
#
#      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()


Y aquí un vídeo mostrando el funcionamiento. Espero les sea de utilidad para sus proyectos la información presentada. Saludos!!!