![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
1.Combine library into this project 2.Use this to do the complete fuction
Dependencies: DXL_SDK_For_F446RE Matrix Modbus_For_F446RE RobotControl_7Axis mbed
main.cpp
- Committer:
- stanley1228
- Date:
- 2017-03-31
- Revision:
- 4:8fc956bd0e78
- Parent:
- 1:249f89a36223
- Child:
- 5:77e9dc268338
File content as of revision 4:8fc956bd0e78:
//1.IK ¯ÊAlpha beta gamma #include "mb.h" #include "mbport.h" #include "mbed.h" //stanley #include "dynamixel.h" #include "RobotControl_7Axis.h" #define DEBUG 1 #if (DEBUG) #define DBGMSG(x) pc.printf x; #else #define DBGMSG(x) #endif DigitalOut myled(LED1); //stanley Serial pc(USBTX, USBRX); //stanley /* ----------------------- Defines ------------------------------------------*/ #define REG_INPUT_START (3001) //not use now stanley #define REG_INPUT_NREGS 15 //not use now stanley #define REG_HOLDING_START (4001) #define REG_HOLDING_NREGS 29 #define SLAVE_ID 0x0A #define DEF_MAX_AXIS 7 //== //Modbus struct //== //Input register 0~14 //public UInt16[] PosVal = new UInt16[DEF_MAX_AXIS]; 7 //public Int16[] VelValue = new Int16[DEF_MAX_AXIS]; 7 //public Int16 Err_State = 0; 1 ////Holding register 0~5 //public Int16 TargetPosX; 1 //public Int16 TargetPosY; 1 //public Int16 TargetPosZ; 1 //public Int16 OPMode; //P2P 1 //public Int16 SpeedRatio; //0~1 1 enum{ DEF_INX_TARGET_POSX=0, DEF_INX_TARGET_POSY, DEF_INX_TARGET_POSZ, DEF_INX_OPMODE, DEF_INX_SPPED_RATIO_L, DEF_INX_SPPED_RATIO_H, DEF_INX_TARPOS1=6, DEF_INX_TARPOS2, DEF_INX_TARPOS3, DEF_INX_TARPOS4, DEF_INX_TARPOS5, DEF_INX_TARPOS6, DEF_INX_TARPOS7, DEF_INX_POSVAL1=13, DEF_INX_POSVAL2, DEF_INX_POSVAL3, DEF_INX_POSVAL4, DEF_INX_POSVAL5, DEF_INX_POSVAL6, DEF_INX_POSVAL7, DEF_INX_VELVAL1=20, DEF_INX_VELVAL2, DEF_INX_VELVAL3, DEF_INX_VELVAL4, DEF_INX_VELVAL5, DEF_INX_VELVAL6, DEF_INX_VELVAL7, DEF_INX_ERR_STATUS, DEF_INX_STATE }; enum eState { DONE=0, BUSY }; enum eOPMode { JOG=0, P2P, SEWING, LINE, IDLE }; /* ----------------------- Static variables ---------------------------------*/ static USHORT usRegInputStart = REG_INPUT_START; //not use now stanley static USHORT usRegInputBuf[REG_INPUT_NREGS]; //not use now stanley static USHORT usRegHoldingStart = REG_HOLDING_START; static USHORT usRegHoldingBuf[REG_HOLDING_NREGS]; /* ----------------------- Start implementation -----------------------------*/ Ticker tMobusPoll; void fMobusPoll() { //(void)eMBPoll( ); origianl // eMBPoll(); /* Here we simply count the number of poll cycles. */ usRegHoldingBuf[DEF_INX_TARGET_POSX]++; if(usRegHoldingBuf[DEF_INX_TARGET_POSX]==200)//stanley { myled=!myled; usRegHoldingBuf[DEF_INX_TARGET_POSX]=0; } } unsigned int Modbus_Initail(void) { eMBErrorCode eStatus; eStatus = eMBInit( MB_RTU, SLAVE_ID, 0, 115200, MB_PAR_NONE ); /* Enable the Modbus Protocol Stack. */ eMBEnable( ); //trial initial value usRegHoldingBuf[DEF_INX_TARGET_POSX]=0; usRegHoldingBuf[DEF_INX_TARGET_POSY]=1000; usRegHoldingBuf[DEF_INX_TARGET_POSZ]=2000; usRegHoldingBuf[DEF_INX_OPMODE]=IDLE; float fSpeedRatio=0.345; USHORT* usp=(USHORT*)&fSpeedRatio; usRegHoldingBuf[DEF_INX_SPPED_RATIO_L]=*usp; usRegHoldingBuf[DEF_INX_SPPED_RATIO_H]=*(usp+1); for(int i=0;i<DEF_MAX_AXIS;i++) { usRegHoldingBuf[DEF_INX_TARPOS1+i]=500+i; usRegHoldingBuf[DEF_INX_POSVAL1+i]=1000+i; usRegHoldingBuf[DEF_INX_VELVAL1+i]=2000+i; } tMobusPoll.attach_us(&fMobusPoll,5000); return 0; } unsigned int Jog_Motion(void) { unsigned int rt=0; float Ang_rad[MAX_AXIS_NUM]={0}; unsigned short int velocity[MAX_AXIS_NUM]={10,10,10,10,10,10,10}; for(int i=Index_AXIS1;i<MAX_AXIS_NUM;i++) { Ang_rad[i]=usRegHoldingBuf[DEF_INX_TARPOS1+i]*DEF_RATIO_PUS_TO_RAD; DBGMSG(("HB[%d]=%d\n",i,usRegHoldingBuf[DEF_INX_TARPOS1+i])) DBGMSG(("Ang_rad[%d]=%f\n",i,Ang_rad[i])) } //== Output_to_Dynamixel==// rt=Output_to_Dynamixel(Ang_rad,velocity); return rt; } unsigned int P2P_Motion(void) { //================== //== Test IK it need 120ms to calculate //=================== unsigned short int Cur_Pus[MAX_AXIS_NUM]={0}; unsigned short int Tar_Pus[MAX_AXIS_NUM]={0}; //short int Ofset_Pus[MAX_AXIS_NUM]={0}; //==Target position pulse==// float theta[MAX_AXIS_NUM]={0.0}; unsigned int rt=0,i=0; rt = IK_7DOF(L1,L2,L3,0,0,0,usRegHoldingBuf[DEF_INX_TARGET_POSX]*DEF_RATIO_PUS_TO_RAD,usRegHoldingBuf[DEF_INX_TARGET_POSY]*DEF_RATIO_PUS_TO_RAD,usRegHoldingBuf[DEF_INX_TARGET_POSZ]*DEF_RATIO_PUS_TO_RAD,0,0,0,(float)-DEF_PI*0.5F,theta); if(rt) { return rt; } for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) { Tar_Pus[i]=theta[i]*DEF_RATIO_RAD_TO_PUS; } //==Current position pulse==// for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) { Cur_Pus[i]=dxl_read_word(getMapAxisNO(i),PRESENT_POS); } //==offset postion pulse==// ©|¥¼¥[¤J³t«×³W¹º //for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) //{ // Ofset_Pus[i]=Tar_Pus[i]-Cur_Pus[i]; //} //== Output_to_Dynamixel==// unsigned short int velocity[MAX_AXIS_NUM]={10,10,10,10,10,10,10}; rt=Output_to_Dynamixel_pulse(Tar_Pus,velocity); return rt; } int main( void ) { unsigned int rt=0; Modbus_Initail(); myled=1;//stanley rt=dxl_initialize( 1, 1); DBGMSG(("dxl_initialize rt=%d\n",rt)) while(1) { switch(usRegHoldingBuf[DEF_INX_OPMODE]) { case JOG: //usRegHoldingBuf[DEF_INX_STATE]=BUSY; Jog_Motion(); //usRegHoldingBuf[DEF_INX_STATE]=DONE; usRegHoldingBuf[DEF_INX_OPMODE]=IDLE; break; case IDLE: case P2P: P2P_Motion(); usRegHoldingBuf[DEF_INX_OPMODE]=IDLE; case SEWING: case LINE: break; } wait_ms(100); } } eMBErrorCode eMBRegInputCB( UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNRegs ) { eMBErrorCode eStatus = MB_ENOERR; int iRegIndex; if( ( usAddress >= REG_INPUT_START ) && ( usAddress + usNRegs <= REG_INPUT_START + REG_INPUT_NREGS ) ) { iRegIndex = ( int )( usAddress - usRegInputStart ); while( usNRegs > 0 ) { *pucRegBuffer++ = ( unsigned char )( usRegInputBuf[iRegIndex] >> 8 ); *pucRegBuffer++ = ( unsigned char )( usRegInputBuf[iRegIndex] & 0xFF ); iRegIndex++; usNRegs--; } } else { eStatus = MB_ENOREG; } return eStatus; } eMBErrorCode eMBRegHoldingCB( UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNRegs, eMBRegisterMode eMode ) //change variable to REG_HOLDING ,original is all input register { eMBErrorCode eStatus = MB_ENOERR; int iRegIndex; if (eMode == MB_REG_READ) { if( ( usAddress >= REG_HOLDING_START ) && ( usAddress + usNRegs <= REG_HOLDING_START + REG_HOLDING_NREGS ) ) { iRegIndex = ( int )( usAddress - usRegHoldingStart ); while( usNRegs > 0 ) { *pucRegBuffer++ = ( unsigned char )( usRegHoldingBuf[iRegIndex] >> 8 ); *pucRegBuffer++ = ( unsigned char )( usRegHoldingBuf[iRegIndex] & 0xFF ); iRegIndex++; usNRegs--; } } } if (eMode == MB_REG_WRITE) { if( ( usAddress >= REG_HOLDING_START ) && ( usAddress + usNRegs <= REG_HOLDING_START + REG_HOLDING_NREGS ) ) { iRegIndex = ( int )( usAddress - usRegHoldingStart ); while( usNRegs > 0 ) { usRegHoldingBuf[iRegIndex] = ((unsigned int) *pucRegBuffer << 8) | ((unsigned int) *(pucRegBuffer+1)); pucRegBuffer+=2; iRegIndex++; usNRegs--; } } } return eStatus; } eMBErrorCode eMBRegCoilsCB( UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNCoils, eMBRegisterMode eMode ) { return MB_ENOREG; } eMBErrorCode eMBRegDiscreteCB( UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNDiscrete ) { return MB_ENOREG; }