Diff: dynamixel.cpp

Revision:

0:e3a3ccc3b0f8

diff -r 000000000000 -r e3a3ccc3b0f8 dynamixel.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/dynamixel.cpp	Tue Sep 12 13:25:29 2017 +0000
@@ -0,0 +1,426 @@
+#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();
+
+}