![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
123
Dependencies: 3W_8Dir_p2pcontrol mbed
Fork of DXL_SDK_For_F446RE by
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
diff -r edccfcb47ab8 -r 1fe7b6875e86 DXL_SDK_For_F446RE.lib --- /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
diff -r edccfcb47ab8 -r 1fe7b6875e86 dxl_hal.cpp --- 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
diff -r edccfcb47ab8 -r 1fe7b6875e86 dxl_hal.h --- 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
diff -r edccfcb47ab8 -r 1fe7b6875e86 dynamixel.cpp --- 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(); - -}
diff -r edccfcb47ab8 -r 1fe7b6875e86 dynamixel.h --- 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
diff -r edccfcb47ab8 -r 1fe7b6875e86 main.cpp --- /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; + } +
diff -r edccfcb47ab8 -r 1fe7b6875e86 mbed.bld --- /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