VIDEO
#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
Nice!
ReplyDeleteI'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
@Shannon Hex
DeleteThank 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.