BeagleBone Dynamixel AX12 Interface - Part 2

 

I have made some progress interfacing the Dynamixel AX-12 servo with the Beaglebone.

In this video I have breadboarded a sparkfun logic level converter to convert the Beaglebone's 3.3v UART logic to the Dynamixel's 5v logic and a Tri state bus buffer to convert the Beaglebone's UART signals to half duplex.

I have run into one snag however...
It seems that the Beaglebone's UART are only able to properly work up to 115.2k baudrate by default. If anyone knows how to adjust the maximum baudrate of the Beaglebone's UART to 1M or even 3M baud please comment!

MWScore New Features

Spending the last few days before RoboGames 2012 getting the Mech Warfare MWScore Scoring Server software into shape.

An exciting new feature I have added is the ability for match data to be broadcast to other computers on the same network as the scoring server PC.

 

Source for everything related to the MWScore system (PC server software, transponder firmware and transponder board designes) can be found on my MWScore project page.

BeagleBone Sparkfun 9DOF Razor IMU

BeagleBone Dynamixel AX12 Interface - Part 1

BeagleBone UART Serial Xbee Test

MWScore System Demo Part Two

MWScore System Demo

Mech Warfare MWScore Transponder Assembly

MWScore Transponder Bare Boards

30 MWScore transponder boards ready to be assembled....

Using stdio.h's "printf" Function on an Atmel AVR Microcontroller

#include <avr/io.h>
#include <stdio.h>

#define BAUDRATE 57600

static void put_char(uint8_t c, FILE* stream);
static FILE mystdout = FDEV_SETUP_STREAM(put_char, NULL, _FDEV_SETUP_WRITE);

int main(void)
{

    UBRR0H = ((F_CPU / 16 + BAUDRATE / 2) / BAUDRATE - 1) >> 8;
    UBRR0L = ((F_CPU / 16 + BAUDRATE / 2) / BAUDRATE - 1);
    UCSR0B |= (1 << TXEN0);

    stdout = &mystdout;

    for(int i = 0; i >= 10; i++)
        printf("testing %d\n\r", i);
}

static void put_char(uint8_t c, FILE* stream)
{
    if (c == '\n') 
     put_char('\r', stream);
  
   while(!(UCSR0A & (1 << UDRE0)));
    UDR1 = c;
}

AVR AX12 Library Improvements - Return Packet Handling

#ifndef _AX_H_
#define _AX_H_

#include <avr/io.h>

#define AX_BAUDRATE 1000000

#define AX_OK      1
#define AX_TIMEOUT 2

#define AX_ID           2
#define AX_LENGTH       3
#define AX_INSTRUCTION  4
#define AX_ERROR        4
#define AX_PARAMETER    5

#define AX_BROADCAST_ID  254
#define AX_PACKET_SIZE   128

#define AX_PING        1
#define AX_READ        2
#define AX_WRITE       3
#define AX_REG_WRITE   4
#define AX_ACTION      5
#define AX_RESET       6
#define AX_SYNC_WRITE  131

#define AX_SUCCESS     1
#define AX_RX_CORRUPT  2
#define AX_RX_TIMEOUT  3
#define AX_TX_FAIL     4
#define AX_TX_TIMEOUT  5

void ax_init(void);
void ax_settx(void);
void ax_setrx(void);
void ax_write(uint8_t c);

uint8_t ax_calculatechecksum(volatile uint8_t* packet);
uint8_t ax_writepacket(volatile uint8_t* packet, uint8_t legnth);
uint8_t ax_readpacket(volatile uint8_t* packet, uint8_t legnth);
uint8_t ax_txrx(volatile uint8_t* txpacket, volatile uint8_t* rxpacket);

uint8_t ax_ping(uint8_t id);
uint8_t ax_readbyte(uint8_t id, uint8_t address, uint8_t* value);
uint8_t ax_readword(uint8_t id, uint8_t address, uint16_t* value);
uint8_t ax_readtable(uint8_t id, uint8_t start_address, uint8_t end_address, uint8_t* table);
uint8_t ax_writebyte(uint8_t id, uint8_t address, uint8_t value);
uint8_t ax_writeword(uint8_t id, uint8_t address, uint16_t value);
uint8_t ax_syncwrite(uint8_t address, uint8_t legnth, uint8_t number, uint8_t* param);
uint8_t ax_reset(uint8_t id);

uint16_t ax_makeword(uint8_t lowbyte, uint8_t highbyte);
uint8_t ax_getlowbyte(uint16_t word);
uint8_t ax_gethighbyte(uint16_t word);

#endif

#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>

#include "ax.h"

#define AX_TIMEOUT 6000

volatile uint8_t ax_txpacket[AX_PACKET_SIZE];
volatile uint8_t ax_rxpacket[AX_PACKET_SIZE];
volatile uint8_t ax_rxindex = 0;

ISR(USART0_RX_vect)
{
 ax_rxpacket[ax_rxindex] = UDR0;
 ax_rxindex += 1;
}

void ax_init(void)
{
 UBRR0H = ((F_CPU / 16 + AX_BAUDRATE / 2) / AX_BAUDRATE - 1) >> 8;
 UBRR0L = ((F_CPU / 16 + AX_BAUDRATE / 2) / AX_BAUDRATE - 1);
 ax_rxindex = 0;
}

void ax_settx(void)
{
 UCSR0B &= ~(1 << RXEN0);
 UCSR0B &= ~(1 << RXCIE0);
 UCSR0B |=  (1 << TXEN0);
}

void ax_setrx(void)
{
 while(bit_is_clear(UCSR0A, TXC0));
 UCSR0B &= ~(1 << TXEN0);
 
 _delay_us(1); // need a small delay here for unkown reasons
 
 UCSR0B |=  (1 << RXEN0);
 UCSR0B |=  (1 << RXCIE0);
 ax_rxindex = 0;
}

void ax_write(uint8_t c)
{
 while(bit_is_clear(UCSR0A, UDRE0));
 UDR0 = c;
}

uint8_t ax_calculatechecksum(volatile uint8_t* packet)
{
 uint16_t checksum = 0;
 
 for(uint8_t i = AX_ID; i <= (packet[AX_LENGTH] + 2); i++)
  checksum += packet[i];
 
 return ~(checksum % 256);
}

uint8_t ax_writepacket(volatile uint8_t* txpacket, uint8_t packetlength)
{ 
 for(uint8_t i = 0; i < packetlength; i++)
  ax_write(txpacket[i]);
 
 return AX_SUCCESS;
}

uint8_t ax_readpacket(volatile uint8_t* rxpacket, uint8_t packetlength)
{
 uint32_t ulcounter = 0;

 while(ax_rxindex < packetlength)
 {
  if(ulcounter++ > AX_TIMEOUT)
   return AX_RX_TIMEOUT;
 }

 if((rxpacket[0] != 255) || (rxpacket[1] != 255))
  return AX_RX_CORRUPT;
  
 if(rxpacket[packetlength - 1] != ax_calculatechecksum(rxpacket))
  return AX_RX_CORRUPT;

 return AX_SUCCESS;
}

uint8_t ax_txrx(volatile uint8_t* txpacket, volatile uint8_t* rxpacket)
{
 uint8_t rxlength = 0;
 uint8_t txlength = ax_txpacket[AX_LENGTH] + 4;
 
 txpacket[0] = (uint8_t) 0xff;
 txpacket[1] = (uint8_t) 0xff;
 txpacket[txlength - 1] = (uint8_t) ax_calculatechecksum(txpacket);
 
 ax_settx();
 ax_writepacket(txpacket, txlength);
 ax_setrx();
 
 if(txpacket[AX_ID] != AX_BROADCAST_ID)
 { 
  if(txpacket[AX_INSTRUCTION] == AX_READ)
   rxlength = txpacket[AX_PARAMETER + 1] + 6;
  else
   rxlength = 6;
   
  return ax_readpacket(rxpacket, rxlength);   
 }
  
 return AX_SUCCESS;
}

uint8_t ax_ping(uint8_t id)
{ 
 ax_txpacket[AX_ID]          = (uint8_t) id;
 ax_txpacket[AX_LENGTH]      = (uint8_t) 2;
 ax_txpacket[AX_INSTRUCTION] = (uint8_t) AX_PING;
 
 return ax_txrx(ax_txpacket, ax_rxpacket);
}

uint8_t ax_readbyte(uint8_t id, uint8_t address, uint8_t* value)
{
 uint8_t result;
 
 ax_txpacket[AX_ID]          = (uint8_t) id;
 ax_txpacket[AX_LENGTH]      = (uint8_t) 4;
 ax_txpacket[AX_INSTRUCTION] = (uint8_t) AX_READ;
 ax_txpacket[AX_PARAMETER]   = (uint8_t) address;
 ax_txpacket[AX_PARAMETER+1] = (uint8_t) 1;
 
 result = ax_txrx(ax_txpacket, ax_rxpacket);
 
 if(result == AX_SUCCESS)
  *value = ax_rxpacket[AX_PARAMETER];
 
 return result;
}

uint8_t ax_readword(uint8_t id, uint8_t address, uint16_t* value)
{
 uint8_t result;
 
 ax_txpacket[AX_ID]          = (uint8_t) id;
 ax_txpacket[AX_LENGTH]      = (uint8_t) 4;
 ax_txpacket[AX_INSTRUCTION] = (uint8_t) AX_READ;
 ax_txpacket[AX_PARAMETER]   = (uint8_t) address;
 ax_txpacket[AX_PARAMETER+1] = (uint8_t) 2;
 
 result = ax_txrx(ax_txpacket, ax_rxpacket);
 
 if(result == AX_SUCCESS)
  *value = (uint16_t) ax_makeword(ax_rxpacket[AX_PARAMETER], ax_rxpacket[AX_PARAMETER+1]);
 
 return result;
}

uint8_t ax_readtable(uint8_t id, uint8_t start_address, uint8_t end_address, uint8_t* table)
{
 uint8_t result;
 uint8_t length = end_address - start_address + 1;
 
 ax_txpacket[AX_ID]          = (uint8_t) id;
 ax_txpacket[AX_LENGTH]      = (uint8_t) 4;
 ax_txpacket[AX_INSTRUCTION] = (uint8_t) AX_READ;
 ax_txpacket[AX_PARAMETER]   = (uint8_t) start_address;
 ax_txpacket[AX_PARAMETER+1] = (uint8_t) length;
 
 result = ax_txrx(ax_txpacket, ax_rxpacket);
 
 if(result == AX_SUCCESS)
 {
  for(uint8_t i = 0; i < length; i++)
   table[start_address + i] = ax_rxpacket[AX_PARAMETER + i];
 }
 
 return result;
}

uint8_t ax_writebyte(uint8_t id, uint8_t address, uint8_t value)
{ 
 ax_txpacket[AX_ID]          = (uint8_t) id;
 ax_txpacket[AX_LENGTH]      = (uint8_t) 4;
 ax_txpacket[AX_INSTRUCTION] = (uint8_t) AX_WRITE;
 ax_txpacket[AX_PARAMETER]   = (uint8_t) address;
 ax_txpacket[AX_PARAMETER+1] = (uint8_t) value;
 
 return ax_txrx(ax_txpacket, ax_rxpacket);
}

uint8_t ax_writeword(uint8_t id, uint8_t address, uint16_t value)
{ 
 ax_txpacket[AX_ID]          = (uint8_t) id;
 ax_txpacket[AX_LENGTH]      = (uint8_t) 5;
 ax_txpacket[AX_INSTRUCTION] = (uint8_t) AX_WRITE;
 ax_txpacket[AX_PARAMETER]   = (uint8_t) address;
 ax_txpacket[AX_PARAMETER+1] = (uint8_t) ax_getlowbyte(value);
 ax_txpacket[AX_PARAMETER+2] = (uint8_t) ax_gethighbyte(value);
 
 return ax_txrx(ax_txpacket, ax_rxpacket);
}

uint8_t ax_syncwrite(uint8_t address, uint8_t length, uint8_t number, uint8_t* param)
{ 
 ax_txpacket[AX_ID]          = (uint8_t) AX_BROADCAST_ID;
 ax_txpacket[AX_INSTRUCTION] = (uint8_t) AX_SYNC_WRITE;
 ax_txpacket[AX_PARAMETER]   = (uint8_t) address;
 ax_txpacket[AX_PARAMETER+1] = (uint8_t) length;
 ax_txpacket[AX_LENGTH]      = (uint8_t) ((length + 1) * number + 4);
 
 for(uint8_t i = 0; i < ((length + 1) * number); i++)
  ax_txpacket[AX_PARAMETER + 2 + i] = (uint8_t) param[i];
 
 return ax_txrx(ax_txpacket, ax_rxpacket);
}

uint8_t ax_reset(uint8_t id)
{
 ax_txpacket[AX_ID]          = (uint8_t) id;
 ax_txpacket[AX_LENGTH]      = (uint8_t) 2;
 ax_txpacket[AX_INSTRUCTION] = (uint8_t) AX_RESET;
 
 return ax_txrx(ax_txpacket, ax_rxpacket);
}

uint16_t ax_makeword(uint8_t lowbyte, uint8_t highbyte)
{
 return ((highbyte << 8) + lowbyte);
}

uint8_t ax_getlowbyte(uint16_t word)
{
 return (word & 0xff);
}

uint8_t ax_gethighbyte(uint16_t word)
{
 return ((word & 0xff00) >> 8);
}

#ifndef _AX12_H_
#define _AX12_H_

#define AX12_MIN_VALUE     0
#define AX12_MAX_VALUE     1023
#define AX12_CENTER_VALUE  512
#define AX12_MIN_ANGLE    -150
#define AX12_MAX_ANGLE     150

#define AX12_MODEL_NUMBER_L          0
#define AX12_MODEL_NUMBER_H          1
#define AX12_VERSION                 2
#define AX12_SERVO_ID                3
#define AX12_BAUD_RATE               4
#define AX12_RETURN_DELAY_TIME       5
#define AX12_CW_ANGLE_LIMIT_L        6
#define AX12_CW_ANGLE_LIMIT_H        7
#define AX12_CCW_ANGLE_LIMIT_L       8
#define AX12_CCW_ANGLE_LIMIT_H       9
#define AX12_LIMIT_TEMPERATURE       11
#define AX12_LOW_LIMIT_VOLTAGE       12
#define AX12_HIGH_LIMIT_VOLTAGE      13
#define AX12_MAX_TORQUE_L            14
#define AX12_MAX_TORQUE_H            15
#define AX12_RETURN_LEVEL            16
#define AX12_ALARM_LED               17
#define AX12_ALARM_SHUTDOWN          18
#define AX12_DOWN_CALIBRATION_L      20
#define AX12_DOWN_CALIBRATION_H      21
#define AX12_UAX_CALIBRATION_L       22
#define AX12_UAX_CALIBRATION_H       23
#define AX12_TORQUE_ENABLE           24
#define AX12_LED                     25
#define AX12_CW_COMPLIANCE_MARGIN    26
#define AX12_CCW_COMPLIANCE_MARGIN   27
#define AX12_CW_COMPLIANCE_SLOPE     28
#define AX12_CCW_COMPLIANCE_SLOPE    29
#define AX12_GOAL_POSITION_L         30
#define AX12_GOAL_POSITION_H         31
#define AX12_GOAL_SPEED_L            32
#define AX12_GOAL_SPEED_H            33
#define AX12_TORQUE_LIMIT_L          34
#define AX12_TORQUE_LIMIT_H          35
#define AX12_PRESENT_POSITION_L      36
#define AX12_PRESENT_POSITION_H      37
#define AX12_PRESENT_SPEED_L         38
#define AX12_PRESENT_SPEED_H         39
#define AX12_PRESENT_LOAD_L          40
#define AX12_PRESENT_LOAD_H          41
#define AX12_PRESENT_VOLTAGE         42
#define AX12_PRESENT_TEMPERATURE     43
#define AX12_REGISTERED_INSTRUCTION  44
#define AX12_MOVING                  46
#define AX12_LOCK                    47
#define AX12_PUNCH_L                 48
#define AX12_PUNCH_H                 49

#endif

Mech Warfare 2012 Scoring System Transponder Prototype

 

Using Python and wxPython to Display A Motion JPEG From the TRENDnet Wireless Internet Camera

import httplib
import base64
import StringIO
import threading
import time
import wx

class Trendnet():
  
  def __init__(self, ip='1.1.1.1', username='admin', password='admin'):
    self.IP = ip
    self.Username = username
    self.Password = password
    self.Connected = False

  def Connect(self):
    if self.Connected == False:
      try:
        print 'Atempting to connect to camera', self.IP, self.Username, self.Password
        h = httplib.HTTP(self.IP)
        h.putrequest('GET','/cgi/mjpg/mjpeg.cgi')
        h.putheader('Authorization', 'Basic %s' % base64.encodestring('%s:%s' % (self.Username, self.Password))[:-1])
        h.endheaders()
        errcode, errmsg, headers = h.getreply()
        self.File = h.getfile()
        print 'Connected!'
        self.Connected = True
      except:
        print 'Unable to connect!'
        self.Connected = False
      
  def Disconnect(self):
    self.Connected = False
    print 'Camera Disconnected!'

  def Update(self):
    if self.Connected: 
      s = self.File.readline()    # '--myboundry'
      s = self.File.readline()    # 'Content-Length: #####'
      framesize = int(s[16:])
      s = self.File.read(framesize)  # jpeg data
      while s[0] != chr(0xff):
        s = s[1:]
      return StringIO.StringIO(s)
      
class CameraPanel(wx.Panel):
  
  def __init__(self, parent, camera):
    wx.Panel.__init__(self, parent, id=wx.ID_ANY, style=wx.SIMPLE_BORDER)
    self.Camera = camera
    self.Bind(wx.EVT_PAINT, self.OnPaint)
    self.Bind(wx.EVT_ERASE_BACKGROUND, self.OnEraseBackground)
  
  def OnEraseBackground(self, event):
    pass
  
  def OnPaint(self, event):
    dc = wx.BufferedPaintDC(self)
    
    if self.Camera.Connected == True:
      try:
        stream = self.Camera.Update()
        if stream != None:
          img = wx.ImageFromStream(stream)
          bmp = wx.BitmapFromImage(img)
          dc.DrawBitmap(bmp, 0, 0, True)
      except:
        pass
    else:
      dc.SetBrush(wx.WHITE_BRUSH)
      dc.DrawRectangle(-1, -1, 620, 480)
      
    dc.DrawCircle(340, 270, 5)
    dc.DrawCircle(280, 270, 5)

if __name__ == '__main__':

  def CamThread():
    while True:
      campanel.Refresh()
      time.sleep(.01)
      
  app = wx.App(0)
  
  wx.Log_SetActiveTarget(wx.LogStderr())
  
  frame = wx.Frame(parent=None, id=wx.ID_ANY, title='UpCam.py', style=wx.DEFAULT_FRAME_STYLE & ~wx.RESIZE_BORDER)
  camera = Trendnet('192.168.1.102')
  camera.Connect()
  campanel = CameraPanel(frame, camera)
  campanel.SetSize((620,480))
  
  sizer = wx.BoxSizer(wx.VERTICAL)
  sizer.Add(campanel, 1, wx.EXPAND|wx.ALL, 5)
  frame.SetSizer(sizer)
  frame.Fit()
  frame.Show(True)
  
  thread = threading.Thread(target=CamThread)
  thread.start()
  
  app.MainLoop()