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:
- 0:f147c77caac0
- Child:
- 1:249f89a36223
diff -r 000000000000 -r f147c77caac0 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Mar 31 02:08:44 2017 +0000 @@ -0,0 +1,233 @@ +#include "mb.h" +#include "mbport.h" +#include "mbed.h" //stanley +#include "dynamixel.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 28 + +#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 +}; + +enum eOPMode +{ + JOG, + P2P, + SEWING, + LINE +}; +/* ----------------------- 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 -----------------------------*/ + +int main( void ) +{ + eMBErrorCode eStatus; + + eStatus = eMBInit( MB_RTU, SLAVE_ID, 0, 115200, MB_PAR_NONE ); + + /* Enable the Modbus Protocol Stack. */ + eMBEnable( ); + //eStatus = eMBEnable( ); + + // Initialise some registers + // usRegInputBuf[1] = 0x1234; + // usRegInputBuf[2] = 0x5678; + // usRegInputBuf[3] = 0x9abc; + //usRegInputBuf[4] = 0x1000; + // usRegInputBuf[5] = 0x1001; + // usRegInputBuf[6] = 0x1002; + + usRegHoldingBuf[DEF_INX_TARGET_POSX]=0; + usRegHoldingBuf[DEF_INX_TARGET_POSY]=1000; + usRegHoldingBuf[DEF_INX_TARGET_POSZ]=2000; + usRegHoldingBuf[DEF_INX_OPMODE]=JOG; + + 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; + } + + + + myled=1;//stanley + + for( ;; ) + { + //(void)eMBPoll( ); origianl + // + eStatus=eMBPoll(); + + /* Here we simply count the number of poll cycles. */ + usRegHoldingBuf[DEF_INX_TARGET_POSX]++; + + wait_ms(5); //stanley + if(usRegHoldingBuf[DEF_INX_TARGET_POSX]==200)//stanley + { + myled=!myled; + usRegHoldingBuf[DEF_INX_TARGET_POSX]=0; + } + } +} + +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; +} +