Diff: dynamixel.cpp

Revision:

6:9e064e338299

Parent:

5:edccfcb47ab8

--- a/dynamixel.cpp	Fri Feb 10 22:55:30 2017 +0800
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,426 +0,0 @@
-#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();
-
-}