Tuesday, January 31, 2012

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

2 comments:

  1. Nice!

    I'm using this code to control a hexapod robot I am making with 5 servos each leg, so happy you have completed the packet return code :). What is the 3rd file for?

    Thanks a bunch

    ReplyDelete
    Replies
    1. @Shannon Hex
      Thank you. I am happy to hear back from someone that found it usefull!

      The third file (ax12.h) is just a bunch of AX-12 specific defines that I have found useful in my projects.

      Delete