File content as of revision 0:a49e4e666e2d:

#include "dxl_hal.h"
#include "dynamixel.h"

#define ID                  (2)
#define LENGTH              (3)
#define INSTRUCTION         (4)
#define ERRBIT              (4)
#define PARAMETER           (5)
#define DEFAULT_BAUDNUMBER  (1)

unsigned char gbInstructionPacket[MAXNUM_TXPARAM+10] = {0};
unsigned char gbStatusPacket[MAXNUM_RXPARAM+10] = {0};
unsigned char gbRxPacketLength = 0;
unsigned char gbRxGetLength = 0;
int gbCommStatus = COMM_RXSUCCESS;
int giBusUsing = 0;

#include "mbed.h"
#include "Serial.h"
extern Serial pc;//stanley


//DigitalOut gTest(D4);

int dxl_initialize( int devIndex, int baudnum )
{
    float baudrate; 
    baudrate = 2000000.0f / (float)(baudnum + 1);
 
    //gTest=0;//stanley
    
    if( dxl_hal_open(devIndex, baudrate) == 0 )
        return 0;

    gbCommStatus = COMM_RXSUCCESS;
    giBusUsing = 0;
    return 1;
}

void dxl_terminate()
{
    dxl_hal_close();
}

void dxl_tx_packet()
{
    unsigned char i;
    unsigned char TxNumByte, RealTxNumByte;
    unsigned char checksum = 0;

    if( giBusUsing == 1 )
        return;
    
    giBusUsing = 1;

    if( gbInstructionPacket[LENGTH] > (MAXNUM_TXPARAM+2) )
    {
        gbCommStatus = COMM_TXERROR;
        giBusUsing = 0;
        return;
    }
    
    if( gbInstructionPacket[INSTRUCTION] != INST_PING
        && gbInstructionPacket[INSTRUCTION] != INST_READ
        && gbInstructionPacket[INSTRUCTION] != INST_WRITE
        && gbInstructionPacket[INSTRUCTION] != INST_REG_WRITE
        && gbInstructionPacket[INSTRUCTION] != INST_ACTION
        && gbInstructionPacket[INSTRUCTION] != INST_RESET
        && gbInstructionPacket[INSTRUCTION] != INST_SYNC_WRITE )
    {
        gbCommStatus = COMM_TXERROR;
        giBusUsing = 0;
        return;
    }
    
    gbInstructionPacket[0] = 0xff;
    gbInstructionPacket[1] = 0xff;
    for( i=0; i<(gbInstructionPacket[LENGTH]+1); i++ )
        checksum += gbInstructionPacket[i+2];
    gbInstructionPacket[gbInstructionPacket[LENGTH]+3] = ~checksum;
    
    if( gbCommStatus == COMM_RXTIMEOUT || gbCommStatus == COMM_RXCORRUPT )
        dxl_hal_clear();

    TxNumByte = gbInstructionPacket[LENGTH] + 4;
    RealTxNumByte = dxl_hal_tx( (unsigned char*)gbInstructionPacket, TxNumByte );

    if( TxNumByte != RealTxNumByte )
    {
        gbCommStatus = COMM_TXFAIL;
        giBusUsing = 0;
        return;
    }

    if( gbInstructionPacket[INSTRUCTION] == INST_READ )
        dxl_hal_set_timeout( gbInstructionPacket[PARAMETER+1] + 6 );
    else
        dxl_hal_set_timeout( 6 );

    gbCommStatus = COMM_TXSUCCESS;
}

void dxl_rx_packet()
{
    unsigned char i, j, nRead;
    unsigned char checksum = 0;

    if( giBusUsing == 0 )
        return;

    if( gbInstructionPacket[ID] == BROADCAST_ID )
    {
        gbCommStatus = COMM_RXSUCCESS;
        giBusUsing = 0;
        return;
    }
    
    if( gbCommStatus == COMM_TXSUCCESS )
    {
        gbRxGetLength = 0;
        gbRxPacketLength = 6;
    }
   
    nRead = dxl_hal_rx( (unsigned char*)&gbStatusPacket[gbRxGetLength], gbRxPacketLength - gbRxGetLength );
    
    gbRxGetLength += nRead;
    if( gbRxGetLength < gbRxPacketLength )
    {
        if( dxl_hal_timeout() == 1 )
        {
            if(gbRxGetLength == 0)
                gbCommStatus = COMM_RXTIMEOUT;
            else
                gbCommStatus = COMM_RXCORRUPT;
            giBusUsing = 0;
            return;
        }
    }
    //pc.printf("133\n");//stanley
    // Find packet header
    for( i=0; i<(gbRxGetLength-1); i++ )
    {
        if( gbStatusPacket[i] == 0xff && gbStatusPacket[i+1] == 0xff )
        {
            break;
        }
        else if( i == gbRxGetLength-2 && gbStatusPacket[gbRxGetLength-1] == 0xff )
        {
            break;
        }
    }   
    if( i > 0 )
    {
        for( j=0; j<(gbRxGetLength-i); j++ )
            gbStatusPacket[j] = gbStatusPacket[j + i];
            
        gbRxGetLength -= i;     
    }

    if( gbRxGetLength < gbRxPacketLength )
    {
        gbCommStatus = COMM_RXWAITING;
        return;
    }

    // Check id pairing
    if( gbInstructionPacket[ID] != gbStatusPacket[ID])
    {
        gbCommStatus = COMM_RXCORRUPT;
        giBusUsing = 0;
        return;
    }
    
    gbRxPacketLength = gbStatusPacket[LENGTH] + 4;
    if( gbRxGetLength < gbRxPacketLength )
    {
        nRead = dxl_hal_rx( (unsigned char*)&gbStatusPacket[gbRxGetLength], gbRxPacketLength - gbRxGetLength );
        gbRxGetLength += nRead;
        if( gbRxGetLength < gbRxPacketLength )
        {
            gbCommStatus = COMM_RXWAITING;
            return;
        }
    }

    // Check checksum
    for( i=0; i<(gbStatusPacket[LENGTH]+1); i++ )
        checksum += gbStatusPacket[i+2];
    checksum = ~checksum;

    if( gbStatusPacket[gbStatusPacket[LENGTH]+3] != checksum )
    {
        gbCommStatus = COMM_RXCORRUPT;
        giBusUsing = 0;
        return;
    }
    
    gbCommStatus = COMM_RXSUCCESS;
    giBusUsing = 0;
}

void dxl_txrx_packet()
{
    
    //pc.printf("before dxl_tx_packet\n");
    dxl_tx_packet();

     //pc.printf("after dxl_tx_packet\n");
    if( gbCommStatus != COMM_TXSUCCESS )
        return; 
    
    
    
    do{
        //pc.printf("before dxl_rx_packet\n");
        dxl_rx_packet();    
        //pc.printf("after dxl_rx_packet\n");    
    }while( gbCommStatus == COMM_RXWAITING );   
}

int dxl_get_result()
{
    return gbCommStatus;
}

void dxl_set_txpacket_id( int id )
{
    gbInstructionPacket[ID] = (unsigned char)id;
}

void dxl_set_txpacket_instruction( int instruction )
{
    gbInstructionPacket[INSTRUCTION] = (unsigned char)instruction;
}

void dxl_set_txpacket_parameter( int index, int value )
{
    gbInstructionPacket[PARAMETER+index] = (unsigned char)value;
}

void dxl_set_txpacket_length( int length )
{
    gbInstructionPacket[LENGTH] = (unsigned char)length;
}

int dxl_get_rxpacket_error( int errbit )
{
    if( gbStatusPacket[ERRBIT] & (unsigned char)errbit )
        return 1;

    return 0;
}

int dxl_get_rxpacket_length()
{
    return (int)gbStatusPacket[LENGTH];
}

int dxl_get_rxpacket_parameter( int index )
{
    return (int)gbStatusPacket[PARAMETER+index];
}

int dxl_makeword( int lowbyte, int highbyte )
{
    unsigned short word;

    word = highbyte;
    word = word << 8;
    word = word + lowbyte;
    return (int)word;
}

int dxl_get_lowbyte( int word )
{
    unsigned short temp;

    temp = word & 0xff;
    return (int)temp;
}

int dxl_get_highbyte( int word )
{
    unsigned short temp;

    temp = word & 0xff00;
    temp = temp >> 8;
    return (int)temp;
}

void dxl_ping( int id )
{
    while(giBusUsing);

    gbInstructionPacket[ID] = (unsigned char)id;
    gbInstructionPacket[INSTRUCTION] = INST_PING;
    gbInstructionPacket[LENGTH] = 2;
    
    dxl_txrx_packet();
}

int dxl_read_byte( int id, int address )
{
    while(giBusUsing);

    gbInstructionPacket[ID] = (unsigned char)id;
    gbInstructionPacket[INSTRUCTION] = INST_READ;
    gbInstructionPacket[PARAMETER] = (unsigned char)address;
    gbInstructionPacket[PARAMETER+1] = 1;
    gbInstructionPacket[LENGTH] = 4;
    
    dxl_txrx_packet();

    return (int)gbStatusPacket[PARAMETER];
}

void dxl_write_byte( int id, int address, int value )
{
    //pc.printf("id=%d,address=%x,value=%d\n",id,address,value);//stanley
    while(giBusUsing);

    //pc.printf("after giBusUsing\n",id,address,value);//stanley

    gbInstructionPacket[ID] = (unsigned char)id;
    gbInstructionPacket[INSTRUCTION] = INST_WRITE;
    gbInstructionPacket[PARAMETER] = (unsigned char)address;
    gbInstructionPacket[PARAMETER+1] = (unsigned char)value;
    gbInstructionPacket[LENGTH] = 4;
    
    //pc.printf("before dxl_txrx_packet\n",id,address,value);//stanley
    
    dxl_txrx_packet();
    
    //pc.printf("after dxl_txrx_packet\n",id,address,value);//stanley
}

int dxl_read_word( short int id, short int address )
{
    while(giBusUsing);

    gbInstructionPacket[ID] = (unsigned char)id;
    gbInstructionPacket[INSTRUCTION] = INST_READ;
    gbInstructionPacket[PARAMETER] = (unsigned char)address;
    gbInstructionPacket[PARAMETER+1] = 2;
    gbInstructionPacket[LENGTH] = 4;
    
    dxl_txrx_packet();

    return dxl_makeword((int)gbStatusPacket[PARAMETER], (int)gbStatusPacket[PARAMETER+1]);
}

void dxl_write_word( short int id, short int address, short int value )
{
    while(giBusUsing);

    gbInstructionPacket[ID] = (unsigned char)id;
    gbInstructionPacket[INSTRUCTION] = INST_WRITE;
    gbInstructionPacket[PARAMETER] = (unsigned char)address;
    gbInstructionPacket[PARAMETER+1] = (unsigned char)dxl_get_lowbyte(value);
    gbInstructionPacket[PARAMETER+2] = (unsigned char)dxl_get_highbyte(value);
    gbInstructionPacket[LENGTH] = 5;
    
    dxl_txrx_packet();
}

int syncWrite_u16base(unsigned short int start_addr, unsigned short int data_length, unsigned short int *param, unsigned short int param_length) // WORD(16bit) syncwrite() for DXL  stanley
{
    while(giBusUsing);
    
    gbInstructionPacket[ID] = (unsigned char)BROADCAST_ID;
    gbInstructionPacket[INSTRUCTION] = INST_SYNC_WRITE; 
    gbInstructionPacket[PARAMETER] = (unsigned char)start_addr; 
    gbInstructionPacket[PARAMETER+1] = (unsigned char)data_length*2; 
    

    int slaveNum=param_length/(data_length+1);
    int OneRawByte=(1+data_length*2);//ID(1byte) + worddata*len(2byte*len)

    
    int i=0; //offset of slave number(number of row)
    int j=0; //offset of data in raw
    int k=1;//offset of int *param 
    int index=0;

    for( i=0; i<slaveNum; i++ )
    { 
        index=PARAMETER+OneRawByte*i+2;
        gbInstructionPacket[index] = (unsigned char)param[i*(data_length+1)];//ID
        k=1;

        for(j=1;j<OneRawByte;j+=2)
        {
            gbInstructionPacket[index+j]= (unsigned char)(param[i*(data_length+1)+k]&0xff); //DATA L    
            gbInstructionPacket[index+j+1]= (unsigned char)(param[i*(data_length+1)+k]>>8); //DATA H
            k++;
        }
    } 
	
    gbInstructionPacket[LENGTH] = OneRawByte*slaveNum+4;

	//for(int i=0;i<50;i++)
	//	pc.printf("gbInstructionPacket[%d]=%x\n",i,gbInstructionPacket[i]);//stanley test
		


    dxl_txrx_packet();
    return 0;
}

void setPosition(int ServoID, int Position, int Speed)//stanley
{
    while(giBusUsing);

    gbInstructionPacket[ID] = (unsigned char)ServoID;
    gbInstructionPacket[INSTRUCTION] = INST_WRITE;
    gbInstructionPacket[PARAMETER] = (unsigned char)30;
    gbInstructionPacket[PARAMETER+1] = (unsigned char)dxl_get_lowbyte(Position);
    gbInstructionPacket[PARAMETER+2] = (unsigned char)dxl_get_highbyte(Position);
    gbInstructionPacket[PARAMETER+3] = (unsigned char)dxl_get_lowbyte(Speed);
    gbInstructionPacket[PARAMETER+4] = (unsigned char)dxl_get_highbyte(Speed);
    
    gbInstructionPacket[LENGTH] = 7;
    
    dxl_txrx_packet();

}