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:
- 5:77e9dc268338
- Parent:
- 4:8fc956bd0e78
- Child:
- 6:e6e7a2ba9f65
diff -r 8fc956bd0e78 -r 77e9dc268338 main.cpp --- a/main.cpp Fri Mar 31 16:51:55 2017 +0800 +++ b/main.cpp Fri Mar 31 17:51:51 2017 +0800 @@ -111,12 +111,12 @@ eMBPoll(); /* Here we simply count the number of poll cycles. */ - usRegHoldingBuf[DEF_INX_TARGET_POSX]++; + //usRegHoldingBuf[DEF_INX_TARGET_POSX]++; if(usRegHoldingBuf[DEF_INX_TARGET_POSX]==200)//stanley { myled=!myled; - usRegHoldingBuf[DEF_INX_TARGET_POSX]=0; + //usRegHoldingBuf[DEF_INX_TARGET_POSX]=0; } } @@ -187,11 +187,20 @@ 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; @@ -200,6 +209,7 @@ 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==// @@ -247,9 +257,11 @@ break; case IDLE: + break; case P2P: P2P_Motion(); usRegHoldingBuf[DEF_INX_OPMODE]=IDLE; + break; case SEWING: case LINE: