Revision 6:1fe7b6875e86, committed 2017-09-12

Comitter:

peter16688

Date:

Tue Sep 12 13:34:00 2017 +0000

Parent:

5:edccfcb47ab8

Commit message:

123

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DXL_SDK_For_F446RE.lib	Tue Sep 12 13:34:00 2017 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/teams/AECL-601/code/3W_8Dir_p2pcontrol/#a49e4e666e2d

--- a/dxl_hal.cpp	Fri Feb 10 22:55:30 2017 +0800
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,141 +0,0 @@
-// 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

--- a/dxl_hal.h	Fri Feb 10 22:55:30 2017 +0800
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,25 +0,0 @@
-// 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

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

--- a/dynamixel.h	Fri Feb 10 22:55:30 2017 +0800
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,107 +0,0 @@
-// 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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Sep 12 13:34:00 2017 +0000
@@ -0,0 +1,359 @@
+#include "mbed.h"
+#include "dynamixel.h"
+#include "math.h"
+/*實驗說明:
+    ==利用運動學模型並以靜止的virtual leader作為輸入命令，以PI控制達成八個方向控制==
+    
+    測試紀錄 2017/6/16:
+*/
+//parameter
+#define pi 3.1416  
+#define r 0.05 // wheel radius [m]
+#define L 0.125 // distance between wheel center & geometric center [m]
+#define kp 0.6  //0.3
+#define kd 0.02 //0.02
+//define ID
+#define ID_LeftFront_Wheel 1
+#define ID_RightFront_Wheel 2
+#define ID_RightRear_Wheel 3
+#define ID_LeftRear_Wheel 4
+//control table
+#define PRESENT_POSITION 36
+#define MOVING_SPEED 32
+#define CW_MAX 1023
+#define CW_MIN 0
+#define CCW_MAX 2047
+#define CCW_MIN 1024
+//Encoder define
+#define OFFSET 1024
+#define MAX 4096
+#define MIN 0
+#define HIGH_POINT (MAX-OFFSET)//HIGH~4096
+#define  LOW_POINT (MIN+OFFSET)//0~LOW
+// initial pose
+#define x0 0   //[cm]
+#define y0 0   //[cm]
+#define theta0 0//[deg.]
+// error threshold
+#define ex 0.002   //[m]
+#define ey 0.002   //[m]
+#define etheta 0.003//[rad.]
+
+//=======odometry======= 
+int CW = 1024;
+int CCW = 0;
+float c1 = r/(3*L), c2 = 2*r/3;
+float f1[3] = {0,0,0}; // formation vector
+float Current_X[3] = {x0+f1[0], y0+f1[1], (theta0+f1[2])*pi/180+pi/3}; // X[0] = x; X[1] = y; X[2] = theta; shift 0 : origin to new
+float Next_X[3] = {0,0,0};
+double d_theta1,d_theta2,d_theta3,d_theta;
+int pos1_new,pos2_new,pos3_new,pos1_old,pos2_old,pos3_old;
+//========control law=======
+float X1[3],V1[3],Vc[3],X1_b[3];// X1_b(X_BAR) is defined as X1-f1
+float XL[3] = {0,0,0};// pose of virtual leader [m m rad.]
+float u[3] = {0};// control law
+float omg[3]={0,0,0};// avelocity of wheels
+float omg1,omg2,omg3;
+//========共用========
+int xdir,ydir;
+float err[3]= {0};
+bool s0 = true;
+int sw=13;
+float xL0,yL0;
+float dx ,dy ,dtheta; //robot
+float dt = 0.05; //(s)
+float VL[3]={0,0,0};
+int c = 4;
+//========Line_setting======== 
+float Line_Vmax = 0.3; // [m/s]
+float Line_Xaim = 0.4; //[m]
+float Line_Yaim = 0.4; //[m]
+int Line_Xdir[16] = { 1,-1, 1,-1, 0, 0,-1, 1,-1, 1,-1, 1, 0, 0, 1,-1}; //[direction]
+int Line_Ydir[16] = { 0, 0, 1,-1, 1,-1, 1,-1, 0, 0,-1, 1,-1, 1,-1, 1}; //[direction]
+int Err_Xdir[16] = { 1, 0, 1, 0, 0, 0,-1, 0,-1, 0,-1, 0, 0, 0, 1, 0};
+int Err_Ydir[16] = { 0, 0, 1, 0, 1, 0, 1, 0, 0, 0,-1, 0,-1, 0,-1, 0};
+int index = 0;
+float T = 2.67;
+float t1 = 1.34;
+char kb='p';
+
+DigitalOut myled1(LED1);
+DigitalIn mybutton(USER_BUTTON);
+
+//function initial
+int getDeltaTheta(int wheel_num,int pos_old,int pos_new);
+
+Timer t,clk; //timer
+
+int main()
+{
+    int rt=0;
+    rt=dxl_initialize( 1, 1);
+    printf("dxl_initialize rt=%d\n",rt);
+
+    printf("4W_8_points_tracking_0710\n");
+    printf("press USER_BUTTON to start: \n");
+    
+    //while(mybutton == 1){};  //藍色按鈕
+    while (kb=='p') {
+        scanf("%c",&kb);   //鍵盤按鈕
+    }
+    
+    //=======讀取第一筆資料為NEW初始值=======   
+    unsigned short temp=0;
+    temp = dxl_read_word(1, PRESENT_POSITION);
+    if(dxl_get_result()==COMM_RXSUCCESS)
+        pos1_old=temp;
+    
+    temp = dxl_read_word(2, PRESENT_POSITION);
+    if(dxl_get_result()==COMM_RXSUCCESS)
+        pos2_old=temp;
+    
+    temp = dxl_read_word(3, PRESENT_POSITION);
+    if(dxl_get_result()==COMM_RXSUCCESS)
+        pos3_old=temp;
+    
+
+    pos1_new=pos1_old;
+    pos2_new=pos2_old;
+    pos3_new=pos3_old;
+    
+
+//    xL0=0;
+//    yL0=0;
+    
+    
+    while(1) {
+
+        myled1 = 0;
+        X1[0] = Current_X[0];
+        X1[1] = Current_X[1];
+        X1[2] = Current_X[2];
+        
+        
+        if(s0==true) {
+            clk.start();
+            if(clk.read()<t1) {//1
+                VL[0]= (clk.read()*Line_Vmax/t1)*Line_Xdir[index];
+                VL[1]= (clk.read()*Line_Vmax/t1)*Line_Ydir[index];
+                VL[2]=0;
+
+                XL[0]= xL0 + (clk.read()*clk.read()*Line_Vmax/(2*t1))*Line_Xdir[index];// go to target
+                XL[1]= yL0 + (clk.read()*clk.read()*Line_Vmax/(2*t1))*Line_Ydir[index];
+                XL[2]=0+pi/3;
+            } else if(clk.read()>t1 && clk.read()<(T-t1) ) {//2
+                VL[0]= Line_Vmax*Line_Xdir[index];
+                VL[1]= Line_Vmax*Line_Ydir[index];
+                VL[2]=0;
+
+                XL[0]= xL0 + (clk.read()*Line_Vmax - Line_Vmax*t1/2)*Line_Xdir[index];
+                XL[1]= yL0 + (clk.read()*Line_Vmax - Line_Vmax*t1/2)*Line_Ydir[index];
+                XL[2]=0+pi/3;
+            } else if (clk.read()>T) {//4
+                VL[0]=0;
+                VL[1]=0;
+                VL[2]=0;
+
+                XL[0]=xL0 + Line_Xaim  * Line_Xdir[index];
+                XL[1]=yL0 + Line_Yaim  * Line_Ydir[index];
+                XL[2]=0+pi/3;
+                xL0=XL[0];
+                yL0=XL[1];
+                s0 = false;
+                clk.reset();
+                clk.stop();
+            } else {//3
+                VL[0]= (Line_Vmax + (clk.read()-T+t1)*(-1)*Line_Vmax/t1)*Line_Xdir[index];
+                VL[1]= (Line_Vmax + (clk.read()-T+t1)*(-1)*Line_Vmax/t1)*Line_Ydir[index];
+                VL[2]=0;
+
+                XL[0]=xL0 + (abs(Line_Xaim)  - ((T-clk.read())*abs(VL[0])/2))*Line_Xdir[index];
+                XL[1]=yL0 + (abs(Line_Yaim)  - ((T-clk.read())*abs(VL[1])/2))*Line_Ydir[index];
+                XL[2]=0+pi/3;
+            }
+        }
+
+
+//==odometry begining==// 
+// packet_tx_rx transfer, 1 cycle = 2 ms
+
+    int qei1 = 0;
+    int qei2 = 0;
+    int qei3 = 0;
+           
+
+    temp = dxl_read_word(1, PRESENT_POSITION);
+    if(dxl_get_result()==COMM_RXSUCCESS)
+        pos1_new=temp;
+    
+    temp = dxl_read_word(2, PRESENT_POSITION);
+    if(dxl_get_result()==COMM_RXSUCCESS)
+        pos2_new=temp;
+    
+    temp = dxl_read_word(3, PRESENT_POSITION);
+    if(dxl_get_result()==COMM_RXSUCCESS)
+        pos3_new=temp;
+    
+    
+    qei1=getDeltaTheta(1,pos1_old,pos1_new);
+    qei2=getDeltaTheta(2,pos2_old,pos2_new);
+    qei3=getDeltaTheta(3,pos3_old,pos3_new);
+    
+    d_theta1 = (qei1*360*pi)/(4096*180);       //degree to rad 
+    d_theta2 = (qei2*360*pi)/(4096*180);
+    d_theta3 = (qei3*360*pi)/(4096*180);
+    d_theta  = c1*(d_theta1  + d_theta2 + d_theta3) ;
+    
+    
+    //printf("pos1: %d || pos2: %d ||pos3: %d \n", pos1_new, pos2_new, pos3_new);
+    //printf("qei1: %d || qei2: %d || qei3: %d \n", qei1, qei2, qei3);
+    //printf("d_th1: %.1f || d_th2: %.1f || d_th3: %.1f || d_th4: %.1f || d_th: %.1f \n", d_theta1, d_theta2, d_theta3, d_theta4, d_theta);
+    
+//==compute theta==//
+    Next_X[2] = Current_X[2] + d_theta;
+    float theta = Current_X[2];
+    float Theta = Current_X[2] + d_theta/2;
+//==compute y==//
+    Next_X[1] = Current_X[1] + c2*(+d_theta1*cos(Theta)-d_theta2*cos(pi/3-Theta)-d_theta3*cos(pi/3+Theta));
+//==compute x==//
+    Next_X[0] = Current_X[0] + c2*(-d_theta1*sin(Theta)-d_theta2*sin(pi/3-Theta)+d_theta3*sin(pi/3+Theta));
+
+// compute velocity
+    dx =Next_X[0]-Current_X[0];
+    dy =Next_X[1]-Current_X[1];
+    dtheta =Next_X[2]-Current_X[2];
+    V1[0]=dx/dt;
+    V1[1]=dy/dt;
+    V1[2]=dtheta/dt;
+//==transition==//
+    Current_X[2] = Next_X[2];
+    Current_X[1] = Next_X[1];
+    Current_X[0] = Next_X[0];
+       
+    pos1_old = pos1_new;
+    pos2_old = pos2_new;
+    pos3_old = pos3_new;
+    
+    //printf("X: %.1f || Y: %.1f || Theta: %.1f\n", Next_X[0], Next_X[1], Next_X[2]);
+    printf("% .2f,% .2f,% .2f   ", XL[0],XL[1],XL[2]);
+    printf("% .2f,% .2f,% .2f \n", Current_X[0], Current_X[1], Current_X[2]);
+//==odometry end==//             
+//==control law beginning==//
+        X1_b[0] = X1[0]-f1[0];
+        X1_b[1] = X1[1]-f1[1];
+        X1_b[2] = X1[2]-f1[2];
+
+        u[0] = -kp*(X1_b[0]-XL[0])-kd*(V1[0]-VL[0]); 
+        u[1] = -kp*(X1_b[1]-XL[1])-kd*(V1[1]-VL[1]);
+        u[2] = -kp*(X1_b[2]-XL[2])-kd*(V1[2]-VL[2]);
+        
+
+        omg1 = (5*u[2])/2 + 20*u[1]*cos(theta) - 20*u[0]*sin(theta);
+        omg2 = (5*u[2])/2 - 20*u[1]*cos(pi/3 - theta) - 20*u[0]*sin(pi/3 - theta);
+        omg3 = (5*u[2])/2 - 20*u[1]*cos(pi/3 + theta) + 20*u[0]*sin(pi/3 + theta);
+        
+        
+        omg[0] = omg1*83.537;
+        omg[1] = omg2*83.537;
+        omg[2] = omg3*83.537;
+    
+        //printf("%.2f, %.2f, %.2f \n", omg1, omg2, omg3);
+        
+        //馬達正轉+反轉 (向前+向後)
+        int i = 0;
+        for (i=0; i<3; i++) {
+            if (omg[i]>0) //向前 -> 1,2,3輪正轉
+            {
+                   if (omg[i]>1023){omg[i] = 1023;}
+                   omg[i] = CW + omg[i];
+            }
+            else if (omg[i]<0) //向後 ->  1,2,3輪反轉
+            {
+                   if (omg[i]<-1023){omg[i] = -1023;}
+                   omg[i] = CCW - omg[i];   
+            }
+        }
+        
+        //printf("%.2f, %.2f, %.2f \n", X1_b[0], X1_b[1], X1_b[2]);
+        //printf("%.2f, %.2f, %.2f \n", u[0], u[1], u[2]);
+        //printf("%.2f, %.2f, %.2f \n", omg[0], omg[1], omg[2]);
+        
+        dxl_write_word(1,MOVING_SPEED,omg[0]);
+        dxl_write_word(2,MOVING_SPEED,omg[1]);
+        dxl_write_word(3,MOVING_SPEED,omg[2]);
+        
+//        dxl_write_word(1,MOVING_SPEED,CCW+100); //馬達測試
+//        dxl_write_word(2,MOVING_SPEED,CW+100);
+//        dxl_write_word(3,MOVING_SPEED,CCW+100);
+//        dxl_write_word(4,MOVING_SPEED,CW+100);
+       
+        
+        // define error // not abs() yet
+        err[0] = Current_X[0]-(Line_Xaim  * Err_Xdir[index]);
+        err[1] = Current_X[1]-(Line_Yaim  * Err_Ydir[index]);
+        err[2] = Current_X[2]-XL[2];
+        
+        
+    //printf("%.2f, %.2f, %.2f, X1_b X2_b X3_b \n", X1_b[0], X1_b[1], X1_b[2]);
+    //printf("%.2f, %.2f, %.2f, %.2f \n", v1, v2, v3, v4);
+    //printf("%.2f, %.2f, %.2f, %.2f, omg1 omg2 omg3 omg4\n", omg1, omg2, omg3, omg4);
+    //printf("%.2f, %.2f, %.2f, dx/dt dy/dt dth/dt \n", u[0], u[1], u[2]);
+    //printf("%.2f, %.2f, %.2f \n", err[0], err[1], err[2]);
+//==control law end==//
+
+        if ( abs(err[0])<ex && abs(err[1])<ey && abs(err[2])<etheta) //誤差判斷指令
+        {
+            printf("Arrived : %.2f, %.2f\n", XL[0], XL[1]);            
+            dxl_write_word(1,MOVING_SPEED,0); //Stop
+            dxl_write_word(2,MOVING_SPEED,0);
+            dxl_write_word(3,MOVING_SPEED,0);
+            dxl_write_word(4,MOVING_SPEED,0);
+            
+//            while(c>0) {
+//                wait(1);
+//                //printf("%d\n",c--);
+//                c--;
+//                myled1 = !myled1;
+//            }
+            if(s0==false && index < 16)
+            {
+            index += 1;
+            s0 = true;
+            printf("index = %d \n",index);
+            }
+            
+            if(index == 16)
+            {
+            printf("Finish the 8-points tracking");
+            return 0;   
+            }
+            
+        }
+        
+        // wait for err
+        wait_ms(50);
+        
+}
+
+   
+}
+
+
+int getDeltaTheta(int wheel_num,int pos_old,int pos_new){
+    int qei=0;
+    //遞增(穿越0點)
+    if(HIGH_POINT < pos_old && MAX >=pos_old && pos_new >=MIN && pos_new < LOW_POINT){
+        qei= (MAX - pos_old)+(pos_new);        
+    }//遞減
+    else if(LOW_POINT > pos_old && pos_old >=MIN && pos_new > HIGH_POINT && pos_new <= MAX){
+        qei = (pos_new - MAX - pos_old);
+    }else{
+        qei= pos_new - pos_old;
+    }
+    
+    if(wheel_num==1 || wheel_num==2 || wheel_num==3)//1,2,3輪遞增方向相反
+        qei=-qei;
+        
+    return qei;
+    }
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Sep 12 13:34:00 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/aae6fcc7d9bb
\ No newline at end of file