![](/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-04-10
- Revision:
- 8:0adb0b96d630
- Parent:
- 7:9127ccc07448
File content as of revision 8:0adb0b96d630:
//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 DigitalOut led_D7(D7,PullUp); //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_TARVEL1=13, DEF_INX_TARVEL2, DEF_INX_TARVEL3, DEF_INX_TARVEL4, DEF_INX_TARVEL5, DEF_INX_TARVEL6, DEF_INX_TARVEL7, DEF_INX_POSVAL1 = 20, DEF_INX_POSVAL2, DEF_INX_POSVAL3, DEF_INX_POSVAL4, DEF_INX_POSVAL5, DEF_INX_POSVAL6, DEF_INX_POSVAL7, DEF_INX_ERR_STATE, 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; Ticker tReadMotorInfo; void fMobusPoll() { //(void)eMBPoll( ); origianl // //led_D7=1; eMBPoll(); //led_D7=0; /* 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_Initial(void) { eMBErrorCode eStatus; eStatus = eMBInit( MB_RTU, SLAVE_ID, 0, 460800, 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_TARVEL1+i]=20; } 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; velocity[i]=usRegHoldingBuf[DEF_INX_TARVEL1+i]; DBGMSG(("p[%d]=%d\n",i,usRegHoldingBuf[DEF_INX_TARPOS1+i])) DBGMSG(("v[%d]=%d\n",i,usRegHoldingBuf[DEF_INX_TARVEL1+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}; short int Tar_Point[3]={0}; Tar_Point[0]=(short int)usRegHoldingBuf[DEF_INX_TARGET_POSX]; Tar_Point[1]=(short int)usRegHoldingBuf[DEF_INX_TARGET_POSY]; Tar_Point[2]=(short int)usRegHoldingBuf[DEF_INX_TARGET_POSZ]; DBGMSG(("Tar_Point[%d]=%d\n",0,Tar_Point[0])) DBGMSG(("Tar_Point[%d]=%d\n",1,Tar_Point[1])) DBGMSG(("Tar_Point[%d]=%d\n",2,Tar_Point[2])) //==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); DBGMSG(("rt=%d\n",rt)) if(rt) { return rt; } for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) { Tar_Pus[i]=theta[i]*DEF_RATIO_RAD_TO_PUS; DBGMSG(("Tar_Pus[%d]=%d\n",i,Tar_Pus[i])) } //==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,500}; rt=Output_to_Dynamixel_pulse(Tar_Pus,velocity); return rt; } void ReadMotorInfo() { //static float pos_pus[MAX_AXIS_NUM]={0}; //unsigned int rt=0; led_D7=1; //rt=Read_pos(pos_pus,DEF_UNIT_PUS); //DBGMSG(("Read_pos rt==%d\n",rt)); static USHORT pulse=0; for(int i=Index_AXIS1;i<MAX_AXIS_NUM-1;i++) { DBGMSG(("gMapAxisID[%d]=%d\n",i,gMapAxisID[i])) pulse = dxl_read_word(gMapAxisID[i], PRESENT_POS); if(dxl_get_result()==COMM_RXSUCCESS) { pulse-=gr2m_offset_pulse[i]; usRegHoldingBuf[DEF_INX_POSVAL1+i]=pulse; DBGMSG(("pulse%d=%d\n",i,pulse)) } } led_D7=0; //if(rt==0) //{ // for(int i=0; i<MAX_AXIS_NUM;i++) // usRegHoldingBuf[DEF_INX_POSVAL1+i]=(unsigned int)pos_pus[i]; // //} //else //{ // usRegHoldingBuf[DEF_INX_ERR_STATUS]=rt; //} //for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) //{ // DBGMSG(("X%d=%.2f,",getMapAxisNO(i),pos_deg[i])); //} //for(i=ID_AXIS1;i<=ID_AXIS7;i++) //{ // pos = dxl_read_word(i, PRESENT_POS); // if(dxl_get_result()!=COMM_RXSUCCESS) // pos=-1; //} } void ReadMotorInfo_Initial() { tReadMotorInfo.attach_us(&ReadMotorInfo,40000); //Read_pos may cost 10ms } int main( void ) { unsigned int rt=0; Modbus_Initial(); myled=1;//stanley rt=dxl_initialize( 1, 1); //ReadMotorInfo_Initial(); 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: break; case P2P: P2P_Motion(); usRegHoldingBuf[DEF_INX_OPMODE]=IDLE; break; case SEWING: case LINE: break; } ReadMotorInfo(); wait_ms(20); } } 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; }