123

Files at this revision

API Documentation at this revision

Comitter:
peter16688
Date:
Tue Sep 12 13:22:08 2017 +0000
Commit message:
3WOK_8dir_1m_PD_p2pcontrol

Changed in this revision

dxl_hal.cpp Show annotated file Show diff for this revision Revisions of this file
dxl_hal.h Show annotated file Show diff for this revision Revisions of this file
dynamixel.cpp Show annotated file Show diff for this revision Revisions of this file
dynamixel.h Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r a49e4e666e2d dxl_hal.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/dxl_hal.cpp	Tue Sep 12 13:22:08 2017 +0000
@@ -0,0 +1,141 @@
+// Dynamixel SDK platform dependent source
+#include "dxl_hal.h"
+#include "mbed.h"
+#include "Serial.h"
+
+#define DEF_TX_EN 1
+#define DEF_RX_EN 0
+
+ 
+
+static DigitalOut gDir(D3);
+Serial dxl(PA_9, PA_10);//UART1 TX, RX F446RE
+extern Serial pc;
+
+extern DigitalOut myled;
+
+
+#define DEF_US_PER_BYTE_1MBPS (20) //roughly 20us/byte for1Mbps use   stanley 
+
+int dxl_hal_open( int devIndex, float baudrate )
+{
+    // Opening device
+    // devIndex: Device index
+    // baudrate: Real baudrate (ex> 115200, 57600, 38400...)
+    // Return: 0(Failed), 1(Succeed)
+   
+    gDir=DEF_RX_EN;
+    dxl.baud((int)baudrate);
+    
+    return 1;
+}
+ 
+void dxl_hal_close()
+{
+    // Closing device
+    
+    //is there a close method for Serial class?
+ 
+}
+ 
+void dxl_hal_clear(void)
+{
+    // Clear communication buffer
+    //char dumpster=0;
+    
+    gDir=DEF_RX_EN;
+    while (dxl.readable())
+    { 
+        dxl.getc(); 
+    } 
+    
+    //Wait_timer.reset(); // i think they expect a reset on a dxl_hal_clear()
+    //Wait_timer.stop();
+ 
+}
+ 
+int dxl_hal_tx( unsigned char *pPacket, int numPacket )
+{
+    // Transmiting date
+    // *pPacket: data array pointer
+    // numPacket: number of data array
+    // Return: number of data transmitted. -1 is error.
+    
+    int i=0;
+ 
+    gDir=DEF_TX_EN;
+    for (i=0; i<numPacket; i++) 
+    {
+       while ( !dxl.writeable() );  // wait until you can write, may want to add timeout here
+       dxl.putc(pPacket[i]);  //write
+    }   
+ 
+    wait_us(DEF_US_PER_BYTE_1MBPS*numPacket); 
+    gDir=DEF_RX_EN;
+    
+    
+    //pc.printf("dxl_hal_tx numPacket=%d\n",numPacket);//stanley
+ 
+    return numPacket;
+}
+ 
+int dxl_hal_rx( unsigned char *pPacket, int numPacket )
+{
+    // Recieving date
+    // *pPacket: data array pointer
+    // numPacket: number of data array
+    // Return: number of data recieved. -1 is error.
+    
+    int i=0;
+
+	while(dxl.readable())
+	{
+		pPacket[i] = dxl.getc();
+		i++;
+	}
+
+	return i;
+
+    //gDir=DEF_RX_EN;
+    //pc.printf("gDir=%d\n",gDir);
+    //for (i=0; i<numPacket; i++) 
+    //{
+    //    while ( !dxl.readable() );  // wait until you can read, may want to add timeout here
+    //    
+    //        //temp= dxl.readable();//stanley
+    //        //pc.printf("dxl.readable=%d\n",temp);
+    //     pPacket[i] = dxl.getc();  // read        
+    //    
+    //}
+    
+}
+
+
+  
+Timer Wait_timer;
+ 
+float   gfRcvWaitTime   = 0.0f;
+
+void dxl_hal_set_timeout( int NumRcvByte )
+{
+    // Start stop watch
+    // NumRcvByte: number of recieving data(to calculate maximum waiting time)
+    
+	Wait_timer.reset();
+	Wait_timer.start(); //start timer
+    gfRcvWaitTime = 900.0f+NumRcvByte*DEF_US_PER_BYTE_1MBPS; //900+NumRcvByte*20
+ 
+}
+ 
+int dxl_hal_timeout(void)
+{
+    // Check timeout
+    // Return: 0 is false (no timeout), 1 is true(timeout occurred)
+    
+    if(Wait_timer.read_us() > gfRcvWaitTime)
+	{ //I'm assuming wait gfRcvWaitTime and gfByteTransTime are in units of (us)
+        return 1; // timeout!
+    } 
+    else 
+		return 0;
+}
\ No newline at end of file
diff -r 000000000000 -r a49e4e666e2d dxl_hal.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/dxl_hal.h	Tue Sep 12 13:22:08 2017 +0000
@@ -0,0 +1,25 @@
+// Dynamixel SDK platform dependent header
+#ifndef _DYNAMIXEL_HAL_HEADER
+#define _DYNAMIXEL_HAL_HEADER
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+int dxl_hal_open( int devIndex, float baudrate );
+void dxl_hal_close(void);
+void dxl_hal_clear(void);
+int dxl_hal_tx( unsigned char *pPacket, int numPacket );
+int dxl_hal_rx( unsigned char *pPacket, int numPacket );
+void dxl_hal_set_timeout( int NumRcvByte );
+int dxl_hal_timeout(void);
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff -r 000000000000 -r a49e4e666e2d dynamixel.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/dynamixel.cpp	Tue Sep 12 13:22:08 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();
+
+}
diff -r 000000000000 -r a49e4e666e2d dynamixel.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/dynamixel.h	Tue Sep 12 13:22:08 2017 +0000
@@ -0,0 +1,107 @@
+// Dynamixel SDK platform independent header
+#ifndef _DYNAMIXEL_HEADER
+#define _DYNAMIXEL_HEADER
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+///////////// device control methods ////////////////////////
+int dxl_initialize( int devIndex, int baudnum );
+void dxl_terminate(void);
+
+
+///////////// set/get packet methods //////////////////////////
+void dxl_set_txpacket_id( int id );
+#define BROADCAST_ID        (254)
+
+void dxl_set_txpacket_instruction( int instruction );
+#define INST_PING           (1)
+#define INST_READ           (2)
+#define INST_WRITE          (3)
+#define INST_REG_WRITE      (4)
+#define INST_ACTION         (5)
+#define INST_RESET          (6)
+#define INST_SYNC_WRITE     (131)
+
+#define MAXNUM_TXPARAM      (150)
+void dxl_set_txpacket_parameter( int index, int value );
+void dxl_set_txpacket_length( int length );
+
+int dxl_get_rxpacket_error( int errbit );
+#define ERRBIT_VOLTAGE      (1)
+#define ERRBIT_ANGLE        (2)
+#define ERRBIT_OVERHEAT     (4)
+#define ERRBIT_RANGE        (8)
+#define ERRBIT_CHECKSUM     (16)
+#define ERRBIT_OVERLOAD     (32)
+#define ERRBIT_INSTRUCTION  (64)
+
+#define MAXNUM_RXPARAM      (60)
+int dxl_get_rxpacket_parameter( int index );
+int dxl_get_rxpacket_length(void);
+
+// utility for value
+int dxl_makeword( int lowbyte, int highbyte );
+int dxl_get_lowbyte( int word );
+int dxl_get_highbyte( int word );
+
+
+////////// packet communication methods ///////////////////////
+void dxl_tx_packet(void);
+void dxl_rx_packet(void);
+void dxl_txrx_packet(void);
+
+int dxl_get_result(void);
+#define COMM_TXSUCCESS      (0)
+#define COMM_RXSUCCESS      (1)
+#define COMM_TXFAIL         (2)
+#define COMM_RXFAIL         (3)
+#define COMM_TXERROR        (4)
+#define COMM_RXWAITING      (5)
+#define COMM_RXTIMEOUT      (6)
+#define COMM_RXCORRUPT      (7)
+
+
+//MX64 Register stanley///
+#define ID_ADDRESS        3
+#define GOAL_POSITION     30
+#define PRESENT_POS       36
+#define RETURN_DELAY_TIME	5
+#define CW_ANGLE_LIMIT_L  6
+#define CW_ANGLE_LIMIT_H  7
+#define CCW_ANGLE_LIMIT_L 8
+#define CCW_ANGLE_LIMIT_H 9
+#define MAX_TORQUE			14
+#define MULTITURN_OFFSET	20
+#define GOAL_POSITION	30
+#define GOAL_SPEED		32
+#define TORQUE_LIMIT	34
+#define PRESENT_POS		36
+
+#define STILL_MOVING	46
+
+
+#define ADDRESS_LED 25
+
+
+//////////// high communication methods ///////////////////////
+void dxl_ping( int id );
+int dxl_read_byte( int id, int address );
+void dxl_write_byte( int id, int address, int value );
+int dxl_read_word( short int id, short int address );
+void dxl_write_word( short int id, short int address, short int value );
+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
+void setPosition(int ServoID, int Position, int Speed);//stanley
+
+
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
\ No newline at end of file