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@4:8fc956bd0e78, 2017-03-31 (annotated)

Committer:
stanley1228
Date:
Fri Mar 31 16:51:55 2017 +0800
Revision:
4:8fc956bd0e78
Parent:
1:249f89a36223
Child:
5:77e9dc268338
1.add new enum state but not use yet

2.Add Modbus_Initail() to contain relative things

3.Add Jog_Motion() and can debug with uart

4.Add IDLE to opmode

5.Add P2P_Motion() but the velocity planning not yet

Who changed what in which revision?

UserRevisionLine numberNew contents of line
stanley1228 4:8fc956bd0e78 1 //1.IK ¯ÊAlpha beta gamma
stanley1228 4:8fc956bd0e78 2
stanley1228 0:f147c77caac0 3 #include "mb.h"
stanley1228 0:f147c77caac0 4 #include "mbport.h"
stanley1228 0:f147c77caac0 5 #include "mbed.h" //stanley
stanley1228 0:f147c77caac0 6 #include "dynamixel.h"
stanley1228 4:8fc956bd0e78 7 #include "RobotControl_7Axis.h"
stanley1228 0:f147c77caac0 8
stanley1228 0:f147c77caac0 9 #define DEBUG 1
stanley1228 0:f147c77caac0 10
stanley1228 0:f147c77caac0 11 #if (DEBUG)
stanley1228 0:f147c77caac0 12 #define DBGMSG(x) pc.printf x;
stanley1228 0:f147c77caac0 13 #else
stanley1228 0:f147c77caac0 14 #define DBGMSG(x)
stanley1228 0:f147c77caac0 15 #endif
stanley1228 0:f147c77caac0 16
stanley1228 0:f147c77caac0 17
stanley1228 0:f147c77caac0 18 DigitalOut myled(LED1); //stanley
stanley1228 0:f147c77caac0 19
stanley1228 0:f147c77caac0 20
stanley1228 0:f147c77caac0 21 Serial pc(USBTX, USBRX); //stanley
stanley1228 0:f147c77caac0 22
stanley1228 0:f147c77caac0 23 /* ----------------------- Defines ------------------------------------------*/
stanley1228 0:f147c77caac0 24 #define REG_INPUT_START (3001) //not use now stanley
stanley1228 0:f147c77caac0 25 #define REG_INPUT_NREGS 15 //not use now stanley
stanley1228 0:f147c77caac0 26 #define REG_HOLDING_START (4001)
stanley1228 4:8fc956bd0e78 27 #define REG_HOLDING_NREGS 29
stanley1228 0:f147c77caac0 28
stanley1228 0:f147c77caac0 29 #define SLAVE_ID 0x0A
stanley1228 0:f147c77caac0 30 #define DEF_MAX_AXIS 7
stanley1228 0:f147c77caac0 31 //==
stanley1228 0:f147c77caac0 32 //Modbus struct
stanley1228 0:f147c77caac0 33 //==
stanley1228 0:f147c77caac0 34 //Input register 0~14
stanley1228 0:f147c77caac0 35 //public UInt16[] PosVal = new UInt16[DEF_MAX_AXIS]; 7
stanley1228 0:f147c77caac0 36 //public Int16[] VelValue = new Int16[DEF_MAX_AXIS]; 7
stanley1228 0:f147c77caac0 37 //public Int16 Err_State = 0; 1
stanley1228 0:f147c77caac0 38
stanley1228 0:f147c77caac0 39 ////Holding register 0~5
stanley1228 0:f147c77caac0 40 //public Int16 TargetPosX; 1
stanley1228 0:f147c77caac0 41 //public Int16 TargetPosY; 1
stanley1228 0:f147c77caac0 42 //public Int16 TargetPosZ; 1
stanley1228 0:f147c77caac0 43 //public Int16 OPMode; //P2P 1
stanley1228 0:f147c77caac0 44 //public Int16 SpeedRatio; //0~1 1
stanley1228 0:f147c77caac0 45
stanley1228 0:f147c77caac0 46 enum{
stanley1228 0:f147c77caac0 47 DEF_INX_TARGET_POSX=0,
stanley1228 0:f147c77caac0 48 DEF_INX_TARGET_POSY,
stanley1228 0:f147c77caac0 49 DEF_INX_TARGET_POSZ,
stanley1228 0:f147c77caac0 50 DEF_INX_OPMODE,
stanley1228 0:f147c77caac0 51 DEF_INX_SPPED_RATIO_L,
stanley1228 0:f147c77caac0 52 DEF_INX_SPPED_RATIO_H,
stanley1228 0:f147c77caac0 53
stanley1228 0:f147c77caac0 54 DEF_INX_TARPOS1=6,
stanley1228 0:f147c77caac0 55 DEF_INX_TARPOS2,
stanley1228 0:f147c77caac0 56 DEF_INX_TARPOS3,
stanley1228 0:f147c77caac0 57 DEF_INX_TARPOS4,
stanley1228 0:f147c77caac0 58 DEF_INX_TARPOS5,
stanley1228 0:f147c77caac0 59 DEF_INX_TARPOS6,
stanley1228 0:f147c77caac0 60 DEF_INX_TARPOS7,
stanley1228 0:f147c77caac0 61
stanley1228 0:f147c77caac0 62 DEF_INX_POSVAL1=13,
stanley1228 0:f147c77caac0 63 DEF_INX_POSVAL2,
stanley1228 0:f147c77caac0 64 DEF_INX_POSVAL3,
stanley1228 0:f147c77caac0 65 DEF_INX_POSVAL4,
stanley1228 0:f147c77caac0 66 DEF_INX_POSVAL5,
stanley1228 0:f147c77caac0 67 DEF_INX_POSVAL6,
stanley1228 0:f147c77caac0 68 DEF_INX_POSVAL7,
stanley1228 0:f147c77caac0 69
stanley1228 0:f147c77caac0 70 DEF_INX_VELVAL1=20,
stanley1228 0:f147c77caac0 71 DEF_INX_VELVAL2,
stanley1228 0:f147c77caac0 72 DEF_INX_VELVAL3,
stanley1228 0:f147c77caac0 73 DEF_INX_VELVAL4,
stanley1228 0:f147c77caac0 74 DEF_INX_VELVAL5,
stanley1228 0:f147c77caac0 75 DEF_INX_VELVAL6,
stanley1228 4:8fc956bd0e78 76 DEF_INX_VELVAL7,
stanley1228 4:8fc956bd0e78 77
stanley1228 4:8fc956bd0e78 78 DEF_INX_ERR_STATUS,
stanley1228 4:8fc956bd0e78 79 DEF_INX_STATE
stanley1228 4:8fc956bd0e78 80 };
stanley1228 4:8fc956bd0e78 81
stanley1228 4:8fc956bd0e78 82 enum eState
stanley1228 4:8fc956bd0e78 83 {
stanley1228 4:8fc956bd0e78 84 DONE=0,
stanley1228 4:8fc956bd0e78 85 BUSY
stanley1228 4:8fc956bd0e78 86
stanley1228 0:f147c77caac0 87 };
stanley1228 0:f147c77caac0 88
stanley1228 0:f147c77caac0 89 enum eOPMode
stanley1228 0:f147c77caac0 90 {
stanley1228 4:8fc956bd0e78 91 JOG=0,
stanley1228 0:f147c77caac0 92 P2P,
stanley1228 0:f147c77caac0 93 SEWING,
stanley1228 4:8fc956bd0e78 94 LINE,
stanley1228 4:8fc956bd0e78 95 IDLE
stanley1228 0:f147c77caac0 96 };
stanley1228 0:f147c77caac0 97 /* ----------------------- Static variables ---------------------------------*/
stanley1228 0:f147c77caac0 98 static USHORT usRegInputStart = REG_INPUT_START; //not use now stanley
stanley1228 0:f147c77caac0 99 static USHORT usRegInputBuf[REG_INPUT_NREGS]; //not use now stanley
stanley1228 0:f147c77caac0 100
stanley1228 0:f147c77caac0 101 static USHORT usRegHoldingStart = REG_HOLDING_START;
stanley1228 0:f147c77caac0 102 static USHORT usRegHoldingBuf[REG_HOLDING_NREGS];
stanley1228 0:f147c77caac0 103
stanley1228 0:f147c77caac0 104 /* ----------------------- Start implementation -----------------------------*/
stanley1228 1:249f89a36223 105 Ticker tMobusPoll;
stanley1228 1:249f89a36223 106
stanley1228 1:249f89a36223 107 void fMobusPoll()
stanley1228 1:249f89a36223 108 {
stanley1228 1:249f89a36223 109 //(void)eMBPoll( ); origianl
stanley1228 1:249f89a36223 110 //
stanley1228 1:249f89a36223 111 eMBPoll();
stanley1228 1:249f89a36223 112
stanley1228 1:249f89a36223 113 /* Here we simply count the number of poll cycles. */
stanley1228 1:249f89a36223 114 usRegHoldingBuf[DEF_INX_TARGET_POSX]++;
stanley1228 1:249f89a36223 115
stanley1228 1:249f89a36223 116 if(usRegHoldingBuf[DEF_INX_TARGET_POSX]==200)//stanley
stanley1228 1:249f89a36223 117 {
stanley1228 1:249f89a36223 118 myled=!myled;
stanley1228 1:249f89a36223 119 usRegHoldingBuf[DEF_INX_TARGET_POSX]=0;
stanley1228 1:249f89a36223 120 }
stanley1228 1:249f89a36223 121 }
stanley1228 1:249f89a36223 122
stanley1228 4:8fc956bd0e78 123 unsigned int Modbus_Initail(void)
stanley1228 0:f147c77caac0 124 {
stanley1228 4:8fc956bd0e78 125 eMBErrorCode eStatus;
stanley1228 0:f147c77caac0 126
stanley1228 0:f147c77caac0 127 eStatus = eMBInit( MB_RTU, SLAVE_ID, 0, 115200, MB_PAR_NONE );
stanley1228 0:f147c77caac0 128
stanley1228 0:f147c77caac0 129 /* Enable the Modbus Protocol Stack. */
stanley1228 0:f147c77caac0 130 eMBEnable( );
stanley1228 0:f147c77caac0 131
stanley1228 4:8fc956bd0e78 132 //trial initial value
stanley1228 0:f147c77caac0 133 usRegHoldingBuf[DEF_INX_TARGET_POSX]=0;
stanley1228 0:f147c77caac0 134 usRegHoldingBuf[DEF_INX_TARGET_POSY]=1000;
stanley1228 0:f147c77caac0 135 usRegHoldingBuf[DEF_INX_TARGET_POSZ]=2000;
stanley1228 4:8fc956bd0e78 136 usRegHoldingBuf[DEF_INX_OPMODE]=IDLE;
stanley1228 0:f147c77caac0 137
stanley1228 0:f147c77caac0 138 float fSpeedRatio=0.345;
stanley1228 0:f147c77caac0 139 USHORT* usp=(USHORT*)&fSpeedRatio;
stanley1228 0:f147c77caac0 140 usRegHoldingBuf[DEF_INX_SPPED_RATIO_L]=*usp;
stanley1228 0:f147c77caac0 141 usRegHoldingBuf[DEF_INX_SPPED_RATIO_H]=*(usp+1);
stanley1228 0:f147c77caac0 142
stanley1228 0:f147c77caac0 143 for(int i=0;i<DEF_MAX_AXIS;i++)
stanley1228 0:f147c77caac0 144 {
stanley1228 0:f147c77caac0 145 usRegHoldingBuf[DEF_INX_TARPOS1+i]=500+i;
stanley1228 0:f147c77caac0 146 usRegHoldingBuf[DEF_INX_POSVAL1+i]=1000+i;
stanley1228 0:f147c77caac0 147 usRegHoldingBuf[DEF_INX_VELVAL1+i]=2000+i;
stanley1228 0:f147c77caac0 148 }
stanley1228 0:f147c77caac0 149
stanley1228 4:8fc956bd0e78 150
stanley1228 4:8fc956bd0e78 151 tMobusPoll.attach_us(&fMobusPoll,5000);
stanley1228 0:f147c77caac0 152
stanley1228 4:8fc956bd0e78 153 return 0;
stanley1228 4:8fc956bd0e78 154 }
stanley1228 4:8fc956bd0e78 155
stanley1228 4:8fc956bd0e78 156 unsigned int Jog_Motion(void)
stanley1228 4:8fc956bd0e78 157 {
stanley1228 4:8fc956bd0e78 158 unsigned int rt=0;
stanley1228 4:8fc956bd0e78 159
stanley1228 4:8fc956bd0e78 160 float Ang_rad[MAX_AXIS_NUM]={0};
stanley1228 4:8fc956bd0e78 161 unsigned short int velocity[MAX_AXIS_NUM]={10,10,10,10,10,10,10};
stanley1228 4:8fc956bd0e78 162
stanley1228 4:8fc956bd0e78 163 for(int i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 4:8fc956bd0e78 164 {
stanley1228 4:8fc956bd0e78 165 Ang_rad[i]=usRegHoldingBuf[DEF_INX_TARPOS1+i]*DEF_RATIO_PUS_TO_RAD;
stanley1228 4:8fc956bd0e78 166
stanley1228 4:8fc956bd0e78 167 DBGMSG(("HB[%d]=%d\n",i,usRegHoldingBuf[DEF_INX_TARPOS1+i]))
stanley1228 4:8fc956bd0e78 168 DBGMSG(("Ang_rad[%d]=%f\n",i,Ang_rad[i]))
stanley1228 4:8fc956bd0e78 169 }
stanley1228 4:8fc956bd0e78 170
stanley1228 4:8fc956bd0e78 171
stanley1228 4:8fc956bd0e78 172
stanley1228 4:8fc956bd0e78 173
stanley1228 4:8fc956bd0e78 174 //== Output_to_Dynamixel==//
stanley1228 4:8fc956bd0e78 175 rt=Output_to_Dynamixel(Ang_rad,velocity);
stanley1228 4:8fc956bd0e78 176
stanley1228 4:8fc956bd0e78 177 return rt;
stanley1228 4:8fc956bd0e78 178 }
stanley1228 4:8fc956bd0e78 179
stanley1228 4:8fc956bd0e78 180
stanley1228 4:8fc956bd0e78 181
stanley1228 4:8fc956bd0e78 182 unsigned int P2P_Motion(void)
stanley1228 4:8fc956bd0e78 183 {
stanley1228 4:8fc956bd0e78 184 //==================
stanley1228 4:8fc956bd0e78 185 //== Test IK it need 120ms to calculate
stanley1228 4:8fc956bd0e78 186 //===================
stanley1228 4:8fc956bd0e78 187 unsigned short int Cur_Pus[MAX_AXIS_NUM]={0};
stanley1228 4:8fc956bd0e78 188 unsigned short int Tar_Pus[MAX_AXIS_NUM]={0};
stanley1228 4:8fc956bd0e78 189 //short int Ofset_Pus[MAX_AXIS_NUM]={0};
stanley1228 4:8fc956bd0e78 190
stanley1228 4:8fc956bd0e78 191 //==Target position pulse==//
stanley1228 4:8fc956bd0e78 192 float theta[MAX_AXIS_NUM]={0.0};
stanley1228 4:8fc956bd0e78 193 unsigned int rt=0,i=0;
stanley1228 4:8fc956bd0e78 194 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);
stanley1228 4:8fc956bd0e78 195 if(rt)
stanley1228 4:8fc956bd0e78 196 {
stanley1228 4:8fc956bd0e78 197 return rt;
stanley1228 4:8fc956bd0e78 198 }
stanley1228 4:8fc956bd0e78 199
stanley1228 4:8fc956bd0e78 200 for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 4:8fc956bd0e78 201 {
stanley1228 4:8fc956bd0e78 202 Tar_Pus[i]=theta[i]*DEF_RATIO_RAD_TO_PUS;
stanley1228 4:8fc956bd0e78 203 }
stanley1228 4:8fc956bd0e78 204
stanley1228 4:8fc956bd0e78 205 //==Current position pulse==//
stanley1228 4:8fc956bd0e78 206 for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 4:8fc956bd0e78 207 {
stanley1228 4:8fc956bd0e78 208 Cur_Pus[i]=dxl_read_word(getMapAxisNO(i),PRESENT_POS);
stanley1228 4:8fc956bd0e78 209 }
stanley1228 4:8fc956bd0e78 210
stanley1228 4:8fc956bd0e78 211 //==offset postion pulse==// ©|¥¼¥[¤J³t«×³W¹º
stanley1228 4:8fc956bd0e78 212 //for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 4:8fc956bd0e78 213 //{
stanley1228 4:8fc956bd0e78 214 // Ofset_Pus[i]=Tar_Pus[i]-Cur_Pus[i];
stanley1228 4:8fc956bd0e78 215 //}
stanley1228 4:8fc956bd0e78 216
stanley1228 4:8fc956bd0e78 217 //== Output_to_Dynamixel==//
stanley1228 4:8fc956bd0e78 218 unsigned short int velocity[MAX_AXIS_NUM]={10,10,10,10,10,10,10};
stanley1228 4:8fc956bd0e78 219 rt=Output_to_Dynamixel_pulse(Tar_Pus,velocity);
stanley1228 4:8fc956bd0e78 220
stanley1228 4:8fc956bd0e78 221 return rt;
stanley1228 4:8fc956bd0e78 222 }
stanley1228 4:8fc956bd0e78 223
stanley1228 4:8fc956bd0e78 224
stanley1228 4:8fc956bd0e78 225 int main( void )
stanley1228 4:8fc956bd0e78 226 {
stanley1228 4:8fc956bd0e78 227 unsigned int rt=0;
stanley1228 4:8fc956bd0e78 228 Modbus_Initail();
stanley1228 4:8fc956bd0e78 229
stanley1228 0:f147c77caac0 230 myled=1;//stanley
stanley1228 0:f147c77caac0 231
stanley1228 4:8fc956bd0e78 232
stanley1228 4:8fc956bd0e78 233 rt=dxl_initialize( 1, 1);
stanley1228 4:8fc956bd0e78 234 DBGMSG(("dxl_initialize rt=%d\n",rt))
stanley1228 4:8fc956bd0e78 235
stanley1228 4:8fc956bd0e78 236 while(1)
stanley1228 4:8fc956bd0e78 237 {
stanley1228 4:8fc956bd0e78 238
stanley1228 4:8fc956bd0e78 239 switch(usRegHoldingBuf[DEF_INX_OPMODE])
stanley1228 4:8fc956bd0e78 240 {
stanley1228 4:8fc956bd0e78 241
stanley1228 4:8fc956bd0e78 242 case JOG:
stanley1228 4:8fc956bd0e78 243 //usRegHoldingBuf[DEF_INX_STATE]=BUSY;
stanley1228 4:8fc956bd0e78 244 Jog_Motion();
stanley1228 4:8fc956bd0e78 245 //usRegHoldingBuf[DEF_INX_STATE]=DONE;
stanley1228 4:8fc956bd0e78 246 usRegHoldingBuf[DEF_INX_OPMODE]=IDLE;
stanley1228 4:8fc956bd0e78 247
stanley1228 4:8fc956bd0e78 248 break;
stanley1228 4:8fc956bd0e78 249 case IDLE:
stanley1228 4:8fc956bd0e78 250 case P2P:
stanley1228 4:8fc956bd0e78 251 P2P_Motion();
stanley1228 4:8fc956bd0e78 252 usRegHoldingBuf[DEF_INX_OPMODE]=IDLE;
stanley1228 4:8fc956bd0e78 253 case SEWING:
stanley1228 4:8fc956bd0e78 254 case LINE:
stanley1228 4:8fc956bd0e78 255
stanley1228 4:8fc956bd0e78 256
stanley1228 4:8fc956bd0e78 257
stanley1228 4:8fc956bd0e78 258 break;
stanley1228 4:8fc956bd0e78 259
stanley1228 4:8fc956bd0e78 260
stanley1228 4:8fc956bd0e78 261
stanley1228 4:8fc956bd0e78 262 }
stanley1228 4:8fc956bd0e78 263 wait_ms(100);
stanley1228 4:8fc956bd0e78 264
stanley1228 4:8fc956bd0e78 265 }
stanley1228 1:249f89a36223 266
stanley1228 0:f147c77caac0 267 }
stanley1228 0:f147c77caac0 268
stanley1228 0:f147c77caac0 269 eMBErrorCode
stanley1228 0:f147c77caac0 270 eMBRegInputCB( UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNRegs )
stanley1228 0:f147c77caac0 271 {
stanley1228 0:f147c77caac0 272 eMBErrorCode eStatus = MB_ENOERR;
stanley1228 0:f147c77caac0 273 int iRegIndex;
stanley1228 0:f147c77caac0 274
stanley1228 0:f147c77caac0 275 if( ( usAddress >= REG_INPUT_START )
stanley1228 0:f147c77caac0 276 && ( usAddress + usNRegs <= REG_INPUT_START + REG_INPUT_NREGS ) )
stanley1228 0:f147c77caac0 277 {
stanley1228 0:f147c77caac0 278 iRegIndex = ( int )( usAddress - usRegInputStart );
stanley1228 0:f147c77caac0 279 while( usNRegs > 0 )
stanley1228 0:f147c77caac0 280 {
stanley1228 0:f147c77caac0 281 *pucRegBuffer++ =
stanley1228 0:f147c77caac0 282 ( unsigned char )( usRegInputBuf[iRegIndex] >> 8 );
stanley1228 0:f147c77caac0 283 *pucRegBuffer++ =
stanley1228 0:f147c77caac0 284 ( unsigned char )( usRegInputBuf[iRegIndex] & 0xFF );
stanley1228 0:f147c77caac0 285 iRegIndex++;
stanley1228 0:f147c77caac0 286 usNRegs--;
stanley1228 0:f147c77caac0 287 }
stanley1228 0:f147c77caac0 288 }
stanley1228 0:f147c77caac0 289 else
stanley1228 0:f147c77caac0 290 {
stanley1228 0:f147c77caac0 291 eStatus = MB_ENOREG;
stanley1228 0:f147c77caac0 292 }
stanley1228 0:f147c77caac0 293
stanley1228 0:f147c77caac0 294 return eStatus;
stanley1228 0:f147c77caac0 295 }
stanley1228 0:f147c77caac0 296
stanley1228 0:f147c77caac0 297 eMBErrorCode
stanley1228 0:f147c77caac0 298 eMBRegHoldingCB( UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNRegs, eMBRegisterMode eMode ) //change variable to REG_HOLDING ,original is all input register
stanley1228 0:f147c77caac0 299 {
stanley1228 0:f147c77caac0 300 eMBErrorCode eStatus = MB_ENOERR;
stanley1228 0:f147c77caac0 301 int iRegIndex;
stanley1228 0:f147c77caac0 302
stanley1228 0:f147c77caac0 303 if (eMode == MB_REG_READ)
stanley1228 0:f147c77caac0 304 {
stanley1228 0:f147c77caac0 305 if( ( usAddress >= REG_HOLDING_START )
stanley1228 0:f147c77caac0 306 && ( usAddress + usNRegs <= REG_HOLDING_START + REG_HOLDING_NREGS ) )
stanley1228 0:f147c77caac0 307 {
stanley1228 0:f147c77caac0 308 iRegIndex = ( int )( usAddress - usRegHoldingStart );
stanley1228 0:f147c77caac0 309 while( usNRegs > 0 )
stanley1228 0:f147c77caac0 310 {
stanley1228 0:f147c77caac0 311 *pucRegBuffer++ =
stanley1228 0:f147c77caac0 312 ( unsigned char )( usRegHoldingBuf[iRegIndex] >> 8 );
stanley1228 0:f147c77caac0 313 *pucRegBuffer++ =
stanley1228 0:f147c77caac0 314 ( unsigned char )( usRegHoldingBuf[iRegIndex] & 0xFF );
stanley1228 0:f147c77caac0 315 iRegIndex++;
stanley1228 0:f147c77caac0 316 usNRegs--;
stanley1228 0:f147c77caac0 317 }
stanley1228 0:f147c77caac0 318 }
stanley1228 0:f147c77caac0 319 }
stanley1228 0:f147c77caac0 320
stanley1228 0:f147c77caac0 321 if (eMode == MB_REG_WRITE)
stanley1228 0:f147c77caac0 322 {
stanley1228 0:f147c77caac0 323 if( ( usAddress >= REG_HOLDING_START )
stanley1228 0:f147c77caac0 324 && ( usAddress + usNRegs <= REG_HOLDING_START + REG_HOLDING_NREGS ) )
stanley1228 0:f147c77caac0 325 {
stanley1228 0:f147c77caac0 326 iRegIndex = ( int )( usAddress - usRegHoldingStart );
stanley1228 0:f147c77caac0 327 while( usNRegs > 0 )
stanley1228 0:f147c77caac0 328 {
stanley1228 0:f147c77caac0 329 usRegHoldingBuf[iRegIndex] = ((unsigned int) *pucRegBuffer << 8) | ((unsigned int) *(pucRegBuffer+1));
stanley1228 0:f147c77caac0 330 pucRegBuffer+=2;
stanley1228 0:f147c77caac0 331 iRegIndex++;
stanley1228 0:f147c77caac0 332 usNRegs--;
stanley1228 0:f147c77caac0 333 }
stanley1228 0:f147c77caac0 334 }
stanley1228 0:f147c77caac0 335 }
stanley1228 0:f147c77caac0 336
stanley1228 0:f147c77caac0 337 return eStatus;
stanley1228 0:f147c77caac0 338 }
stanley1228 0:f147c77caac0 339
stanley1228 0:f147c77caac0 340
stanley1228 0:f147c77caac0 341 eMBErrorCode
stanley1228 0:f147c77caac0 342 eMBRegCoilsCB( UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNCoils,
stanley1228 0:f147c77caac0 343 eMBRegisterMode eMode )
stanley1228 0:f147c77caac0 344 {
stanley1228 0:f147c77caac0 345 return MB_ENOREG;
stanley1228 0:f147c77caac0 346 }
stanley1228 0:f147c77caac0 347
stanley1228 0:f147c77caac0 348 eMBErrorCode
stanley1228 0:f147c77caac0 349 eMBRegDiscreteCB( UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNDiscrete )
stanley1228 0:f147c77caac0 350 {
stanley1228 0:f147c77caac0 351 return MB_ENOREG;
stanley1228 0:f147c77caac0 352 }
stanley1228 0:f147c77caac0 353