hsu han-lin / Mbed 2 deprecated RoboticArm

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@0:f147c77caac0, 2017-03-31 (annotated)

Committer:
stanley1228
Date:
Fri Mar 31 02:08:44 2017 +0000
Revision:
0:f147c77caac0
Child:
1:249f89a36223
1. Just combine library into one program

Who changed what in which revision?

UserRevisionLine numberNew contents of line
stanley1228 0:f147c77caac0 1 #include "mb.h"
stanley1228 0:f147c77caac0 2 #include "mbport.h"
stanley1228 0:f147c77caac0 3 #include "mbed.h" //stanley
stanley1228 0:f147c77caac0 4 #include "dynamixel.h"
stanley1228 0:f147c77caac0 5
stanley1228 0:f147c77caac0 6 #define DEBUG 1
stanley1228 0:f147c77caac0 7
stanley1228 0:f147c77caac0 8 #if (DEBUG)
stanley1228 0:f147c77caac0 9 #define DBGMSG(x) pc.printf x;
stanley1228 0:f147c77caac0 10 #else
stanley1228 0:f147c77caac0 11 #define DBGMSG(x)
stanley1228 0:f147c77caac0 12 #endif
stanley1228 0:f147c77caac0 13
stanley1228 0:f147c77caac0 14
stanley1228 0:f147c77caac0 15 DigitalOut myled(LED1); //stanley
stanley1228 0:f147c77caac0 16
stanley1228 0:f147c77caac0 17
stanley1228 0:f147c77caac0 18 Serial pc(USBTX, USBRX); //stanley
stanley1228 0:f147c77caac0 19
stanley1228 0:f147c77caac0 20 /* ----------------------- Defines ------------------------------------------*/
stanley1228 0:f147c77caac0 21 #define REG_INPUT_START (3001) //not use now stanley
stanley1228 0:f147c77caac0 22 #define REG_INPUT_NREGS 15 //not use now stanley
stanley1228 0:f147c77caac0 23 #define REG_HOLDING_START (4001)
stanley1228 0:f147c77caac0 24 #define REG_HOLDING_NREGS 28
stanley1228 0:f147c77caac0 25
stanley1228 0:f147c77caac0 26 #define SLAVE_ID 0x0A
stanley1228 0:f147c77caac0 27 #define DEF_MAX_AXIS 7
stanley1228 0:f147c77caac0 28 //==
stanley1228 0:f147c77caac0 29 //Modbus struct
stanley1228 0:f147c77caac0 30 //==
stanley1228 0:f147c77caac0 31 //Input register 0~14
stanley1228 0:f147c77caac0 32 //public UInt16[] PosVal = new UInt16[DEF_MAX_AXIS]; 7
stanley1228 0:f147c77caac0 33 //public Int16[] VelValue = new Int16[DEF_MAX_AXIS]; 7
stanley1228 0:f147c77caac0 34 //public Int16 Err_State = 0; 1
stanley1228 0:f147c77caac0 35
stanley1228 0:f147c77caac0 36 ////Holding register 0~5
stanley1228 0:f147c77caac0 37 //public Int16 TargetPosX; 1
stanley1228 0:f147c77caac0 38 //public Int16 TargetPosY; 1
stanley1228 0:f147c77caac0 39 //public Int16 TargetPosZ; 1
stanley1228 0:f147c77caac0 40 //public Int16 OPMode; //P2P 1
stanley1228 0:f147c77caac0 41 //public Int16 SpeedRatio; //0~1 1
stanley1228 0:f147c77caac0 42
stanley1228 0:f147c77caac0 43 enum{
stanley1228 0:f147c77caac0 44 DEF_INX_TARGET_POSX=0,
stanley1228 0:f147c77caac0 45 DEF_INX_TARGET_POSY,
stanley1228 0:f147c77caac0 46 DEF_INX_TARGET_POSZ,
stanley1228 0:f147c77caac0 47 DEF_INX_OPMODE,
stanley1228 0:f147c77caac0 48 DEF_INX_SPPED_RATIO_L,
stanley1228 0:f147c77caac0 49 DEF_INX_SPPED_RATIO_H,
stanley1228 0:f147c77caac0 50
stanley1228 0:f147c77caac0 51 DEF_INX_TARPOS1=6,
stanley1228 0:f147c77caac0 52 DEF_INX_TARPOS2,
stanley1228 0:f147c77caac0 53 DEF_INX_TARPOS3,
stanley1228 0:f147c77caac0 54 DEF_INX_TARPOS4,
stanley1228 0:f147c77caac0 55 DEF_INX_TARPOS5,
stanley1228 0:f147c77caac0 56 DEF_INX_TARPOS6,
stanley1228 0:f147c77caac0 57 DEF_INX_TARPOS7,
stanley1228 0:f147c77caac0 58
stanley1228 0:f147c77caac0 59 DEF_INX_POSVAL1=13,
stanley1228 0:f147c77caac0 60 DEF_INX_POSVAL2,
stanley1228 0:f147c77caac0 61 DEF_INX_POSVAL3,
stanley1228 0:f147c77caac0 62 DEF_INX_POSVAL4,
stanley1228 0:f147c77caac0 63 DEF_INX_POSVAL5,
stanley1228 0:f147c77caac0 64 DEF_INX_POSVAL6,
stanley1228 0:f147c77caac0 65 DEF_INX_POSVAL7,
stanley1228 0:f147c77caac0 66
stanley1228 0:f147c77caac0 67 DEF_INX_VELVAL1=20,
stanley1228 0:f147c77caac0 68 DEF_INX_VELVAL2,
stanley1228 0:f147c77caac0 69 DEF_INX_VELVAL3,
stanley1228 0:f147c77caac0 70 DEF_INX_VELVAL4,
stanley1228 0:f147c77caac0 71 DEF_INX_VELVAL5,
stanley1228 0:f147c77caac0 72 DEF_INX_VELVAL6,
stanley1228 0:f147c77caac0 73 DEF_INX_VELVAL7
stanley1228 0:f147c77caac0 74 };
stanley1228 0:f147c77caac0 75
stanley1228 0:f147c77caac0 76 enum eOPMode
stanley1228 0:f147c77caac0 77 {
stanley1228 0:f147c77caac0 78 JOG,
stanley1228 0:f147c77caac0 79 P2P,
stanley1228 0:f147c77caac0 80 SEWING,
stanley1228 0:f147c77caac0 81 LINE
stanley1228 0:f147c77caac0 82 };
stanley1228 0:f147c77caac0 83 /* ----------------------- Static variables ---------------------------------*/
stanley1228 0:f147c77caac0 84 static USHORT usRegInputStart = REG_INPUT_START; //not use now stanley
stanley1228 0:f147c77caac0 85 static USHORT usRegInputBuf[REG_INPUT_NREGS]; //not use now stanley
stanley1228 0:f147c77caac0 86
stanley1228 0:f147c77caac0 87 static USHORT usRegHoldingStart = REG_HOLDING_START;
stanley1228 0:f147c77caac0 88 static USHORT usRegHoldingBuf[REG_HOLDING_NREGS];
stanley1228 0:f147c77caac0 89
stanley1228 0:f147c77caac0 90 /* ----------------------- Start implementation -----------------------------*/
stanley1228 0:f147c77caac0 91
stanley1228 0:f147c77caac0 92 int main( void )
stanley1228 0:f147c77caac0 93 {
stanley1228 0:f147c77caac0 94 eMBErrorCode eStatus;
stanley1228 0:f147c77caac0 95
stanley1228 0:f147c77caac0 96 eStatus = eMBInit( MB_RTU, SLAVE_ID, 0, 115200, MB_PAR_NONE );
stanley1228 0:f147c77caac0 97
stanley1228 0:f147c77caac0 98 /* Enable the Modbus Protocol Stack. */
stanley1228 0:f147c77caac0 99 eMBEnable( );
stanley1228 0:f147c77caac0 100 //eStatus = eMBEnable( );
stanley1228 0:f147c77caac0 101
stanley1228 0:f147c77caac0 102 // Initialise some registers
stanley1228 0:f147c77caac0 103 // usRegInputBuf[1] = 0x1234;
stanley1228 0:f147c77caac0 104 // usRegInputBuf[2] = 0x5678;
stanley1228 0:f147c77caac0 105 // usRegInputBuf[3] = 0x9abc;
stanley1228 0:f147c77caac0 106 //usRegInputBuf[4] = 0x1000;
stanley1228 0:f147c77caac0 107 // usRegInputBuf[5] = 0x1001;
stanley1228 0:f147c77caac0 108 // usRegInputBuf[6] = 0x1002;
stanley1228 0:f147c77caac0 109
stanley1228 0:f147c77caac0 110 usRegHoldingBuf[DEF_INX_TARGET_POSX]=0;
stanley1228 0:f147c77caac0 111 usRegHoldingBuf[DEF_INX_TARGET_POSY]=1000;
stanley1228 0:f147c77caac0 112 usRegHoldingBuf[DEF_INX_TARGET_POSZ]=2000;
stanley1228 0:f147c77caac0 113 usRegHoldingBuf[DEF_INX_OPMODE]=JOG;
stanley1228 0:f147c77caac0 114
stanley1228 0:f147c77caac0 115 float fSpeedRatio=0.345;
stanley1228 0:f147c77caac0 116 USHORT* usp=(USHORT*)&fSpeedRatio;
stanley1228 0:f147c77caac0 117 usRegHoldingBuf[DEF_INX_SPPED_RATIO_L]=*usp;
stanley1228 0:f147c77caac0 118 usRegHoldingBuf[DEF_INX_SPPED_RATIO_H]=*(usp+1);
stanley1228 0:f147c77caac0 119
stanley1228 0:f147c77caac0 120 for(int i=0;i<DEF_MAX_AXIS;i++)
stanley1228 0:f147c77caac0 121 {
stanley1228 0:f147c77caac0 122 usRegHoldingBuf[DEF_INX_TARPOS1+i]=500+i;
stanley1228 0:f147c77caac0 123 usRegHoldingBuf[DEF_INX_POSVAL1+i]=1000+i;
stanley1228 0:f147c77caac0 124 usRegHoldingBuf[DEF_INX_VELVAL1+i]=2000+i;
stanley1228 0:f147c77caac0 125 }
stanley1228 0:f147c77caac0 126
stanley1228 0:f147c77caac0 127
stanley1228 0:f147c77caac0 128
stanley1228 0:f147c77caac0 129 myled=1;//stanley
stanley1228 0:f147c77caac0 130
stanley1228 0:f147c77caac0 131 for( ;; )
stanley1228 0:f147c77caac0 132 {
stanley1228 0:f147c77caac0 133 //(void)eMBPoll( ); origianl
stanley1228 0:f147c77caac0 134 //
stanley1228 0:f147c77caac0 135 eStatus=eMBPoll();
stanley1228 0:f147c77caac0 136
stanley1228 0:f147c77caac0 137 /* Here we simply count the number of poll cycles. */
stanley1228 0:f147c77caac0 138 usRegHoldingBuf[DEF_INX_TARGET_POSX]++;
stanley1228 0:f147c77caac0 139
stanley1228 0:f147c77caac0 140 wait_ms(5); //stanley
stanley1228 0:f147c77caac0 141 if(usRegHoldingBuf[DEF_INX_TARGET_POSX]==200)//stanley
stanley1228 0:f147c77caac0 142 {
stanley1228 0:f147c77caac0 143 myled=!myled;
stanley1228 0:f147c77caac0 144 usRegHoldingBuf[DEF_INX_TARGET_POSX]=0;
stanley1228 0:f147c77caac0 145 }
stanley1228 0:f147c77caac0 146 }
stanley1228 0:f147c77caac0 147 }
stanley1228 0:f147c77caac0 148
stanley1228 0:f147c77caac0 149 eMBErrorCode
stanley1228 0:f147c77caac0 150 eMBRegInputCB( UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNRegs )
stanley1228 0:f147c77caac0 151 {
stanley1228 0:f147c77caac0 152 eMBErrorCode eStatus = MB_ENOERR;
stanley1228 0:f147c77caac0 153 int iRegIndex;
stanley1228 0:f147c77caac0 154
stanley1228 0:f147c77caac0 155 if( ( usAddress >= REG_INPUT_START )
stanley1228 0:f147c77caac0 156 && ( usAddress + usNRegs <= REG_INPUT_START + REG_INPUT_NREGS ) )
stanley1228 0:f147c77caac0 157 {
stanley1228 0:f147c77caac0 158 iRegIndex = ( int )( usAddress - usRegInputStart );
stanley1228 0:f147c77caac0 159 while( usNRegs > 0 )
stanley1228 0:f147c77caac0 160 {
stanley1228 0:f147c77caac0 161 *pucRegBuffer++ =
stanley1228 0:f147c77caac0 162 ( unsigned char )( usRegInputBuf[iRegIndex] >> 8 );
stanley1228 0:f147c77caac0 163 *pucRegBuffer++ =
stanley1228 0:f147c77caac0 164 ( unsigned char )( usRegInputBuf[iRegIndex] & 0xFF );
stanley1228 0:f147c77caac0 165 iRegIndex++;
stanley1228 0:f147c77caac0 166 usNRegs--;
stanley1228 0:f147c77caac0 167 }
stanley1228 0:f147c77caac0 168 }
stanley1228 0:f147c77caac0 169 else
stanley1228 0:f147c77caac0 170 {
stanley1228 0:f147c77caac0 171 eStatus = MB_ENOREG;
stanley1228 0:f147c77caac0 172 }
stanley1228 0:f147c77caac0 173
stanley1228 0:f147c77caac0 174 return eStatus;
stanley1228 0:f147c77caac0 175 }
stanley1228 0:f147c77caac0 176
stanley1228 0:f147c77caac0 177 eMBErrorCode
stanley1228 0:f147c77caac0 178 eMBRegHoldingCB( UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNRegs, eMBRegisterMode eMode ) //change variable to REG_HOLDING ,original is all input register
stanley1228 0:f147c77caac0 179 {
stanley1228 0:f147c77caac0 180 eMBErrorCode eStatus = MB_ENOERR;
stanley1228 0:f147c77caac0 181 int iRegIndex;
stanley1228 0:f147c77caac0 182
stanley1228 0:f147c77caac0 183 if (eMode == MB_REG_READ)
stanley1228 0:f147c77caac0 184 {
stanley1228 0:f147c77caac0 185 if( ( usAddress >= REG_HOLDING_START )
stanley1228 0:f147c77caac0 186 && ( usAddress + usNRegs <= REG_HOLDING_START + REG_HOLDING_NREGS ) )
stanley1228 0:f147c77caac0 187 {
stanley1228 0:f147c77caac0 188 iRegIndex = ( int )( usAddress - usRegHoldingStart );
stanley1228 0:f147c77caac0 189 while( usNRegs > 0 )
stanley1228 0:f147c77caac0 190 {
stanley1228 0:f147c77caac0 191 *pucRegBuffer++ =
stanley1228 0:f147c77caac0 192 ( unsigned char )( usRegHoldingBuf[iRegIndex] >> 8 );
stanley1228 0:f147c77caac0 193 *pucRegBuffer++ =
stanley1228 0:f147c77caac0 194 ( unsigned char )( usRegHoldingBuf[iRegIndex] & 0xFF );
stanley1228 0:f147c77caac0 195 iRegIndex++;
stanley1228 0:f147c77caac0 196 usNRegs--;
stanley1228 0:f147c77caac0 197 }
stanley1228 0:f147c77caac0 198 }
stanley1228 0:f147c77caac0 199 }
stanley1228 0:f147c77caac0 200
stanley1228 0:f147c77caac0 201 if (eMode == MB_REG_WRITE)
stanley1228 0:f147c77caac0 202 {
stanley1228 0:f147c77caac0 203 if( ( usAddress >= REG_HOLDING_START )
stanley1228 0:f147c77caac0 204 && ( usAddress + usNRegs <= REG_HOLDING_START + REG_HOLDING_NREGS ) )
stanley1228 0:f147c77caac0 205 {
stanley1228 0:f147c77caac0 206 iRegIndex = ( int )( usAddress - usRegHoldingStart );
stanley1228 0:f147c77caac0 207 while( usNRegs > 0 )
stanley1228 0:f147c77caac0 208 {
stanley1228 0:f147c77caac0 209 usRegHoldingBuf[iRegIndex] = ((unsigned int) *pucRegBuffer << 8) | ((unsigned int) *(pucRegBuffer+1));
stanley1228 0:f147c77caac0 210 pucRegBuffer+=2;
stanley1228 0:f147c77caac0 211 iRegIndex++;
stanley1228 0:f147c77caac0 212 usNRegs--;
stanley1228 0:f147c77caac0 213 }
stanley1228 0:f147c77caac0 214 }
stanley1228 0:f147c77caac0 215 }
stanley1228 0:f147c77caac0 216
stanley1228 0:f147c77caac0 217 return eStatus;
stanley1228 0:f147c77caac0 218 }
stanley1228 0:f147c77caac0 219
stanley1228 0:f147c77caac0 220
stanley1228 0:f147c77caac0 221 eMBErrorCode
stanley1228 0:f147c77caac0 222 eMBRegCoilsCB( UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNCoils,
stanley1228 0:f147c77caac0 223 eMBRegisterMode eMode )
stanley1228 0:f147c77caac0 224 {
stanley1228 0:f147c77caac0 225 return MB_ENOREG;
stanley1228 0:f147c77caac0 226 }
stanley1228 0:f147c77caac0 227
stanley1228 0:f147c77caac0 228 eMBErrorCode
stanley1228 0:f147c77caac0 229 eMBRegDiscreteCB( UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNDiscrete )
stanley1228 0:f147c77caac0 230 {
stanley1228 0:f147c77caac0 231 return MB_ENOREG;
stanley1228 0:f147c77caac0 232 }
stanley1228 0:f147c77caac0 233