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
Diff: main.cpp
- Revision:
- 6:e6e7a2ba9f65
- Parent:
- 5:77e9dc268338
- Child:
- 7:9127ccc07448
diff -r 77e9dc268338 -r e6e7a2ba9f65 main.cpp --- a/main.cpp Fri Mar 31 17:51:51 2017 +0800 +++ b/main.cpp Sat Apr 08 21:29:21 2017 +0800 @@ -103,6 +103,7 @@ /* ----------------------- Start implementation -----------------------------*/ Ticker tMobusPoll; +Ticker tReadMotorInfo; void fMobusPoll() { @@ -120,7 +121,7 @@ } } -unsigned int Modbus_Initail(void) +unsigned int Modbus_Initial(void) { eMBErrorCode eStatus; @@ -184,7 +185,7 @@ //================== //== Test IK it need 120ms to calculate //=================== - unsigned short int Cur_Pus[MAX_AXIS_NUM]={0}; + //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}; @@ -213,10 +214,10 @@ } //==Current position pulse==// - for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++) - { - Cur_Pus[i]=dxl_read_word(getMapAxisNO(i),PRESENT_POS); - } + //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++) @@ -231,16 +232,53 @@ return rt; } +void ReadMotorInfo() +{ + static float pos_pus[MAX_AXIS_NUM]={0}; + unsigned int rt=0; + + rt=Read_pos(pos_pus,DEF_UNIT_PUS); + //DBGMSG(("Read_pos rt==%d\n",rt)); + if(rt==0) + { + for(int i=0; i<MAX_AXIS_NUM;i++) + usRegHoldingBuf[DEF_INX_TARPOS1+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,5000); +} + int main( void ) { unsigned int rt=0; - Modbus_Initail(); + Modbus_Initial(); myled=1;//stanley rt=dxl_initialize( 1, 1); + + ReadMotorInfo_Initial(); + DBGMSG(("dxl_initialize rt=%d\n",rt)) while(1)