Revision 0:a49e4e666e2d, committed 2017-09-12

Comitter:

peter16688

Date:

Tue Sep 12 13:22:08 2017 +0000

Commit message:

3WOK_8dir_1m_PD_p2pcontrol

Changed in this revision

--- /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

--- /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

--- /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();
+
+}

--- /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