![](/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:
- 1:249f89a36223
- Parent:
- 0:f147c77caac0
- Child:
- 4:8fc956bd0e78
File content as of revision 1:249f89a36223:
#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 -----------------------------*/ 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; } } 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; } tMobusPoll.attach_us(&fMobusPoll,10000); myled=1;//stanley while(1); } 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; }