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@6:e6e7a2ba9f65, 2017-04-08 (annotated)

Committer:
stanley1228
Date:
Sat Apr 08 21:29:21 2017 +0800
Revision:
6:e6e7a2ba9f65
Parent:
5:77e9dc268338
Child:
7:9127ccc07448
1.Add ReadMotorInfo_Initial()

2.Add ReadMotorInfo

3.Add Read motor pos with 2ms timer

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 6:e6e7a2ba9f65 106 Ticker tReadMotorInfo;
stanley1228 1:249f89a36223 107
stanley1228 1:249f89a36223 108 void fMobusPoll()
stanley1228 1:249f89a36223 109 {
stanley1228 1:249f89a36223 110 //(void)eMBPoll( ); origianl
stanley1228 1:249f89a36223 111 //
stanley1228 1:249f89a36223 112 eMBPoll();
stanley1228 1:249f89a36223 113
stanley1228 1:249f89a36223 114 /* Here we simply count the number of poll cycles. */
stanley1228 5:77e9dc268338 115 //usRegHoldingBuf[DEF_INX_TARGET_POSX]++;
stanley1228 1:249f89a36223 116
stanley1228 1:249f89a36223 117 if(usRegHoldingBuf[DEF_INX_TARGET_POSX]==200)//stanley
stanley1228 1:249f89a36223 118 {
stanley1228 1:249f89a36223 119 myled=!myled;
stanley1228 5:77e9dc268338 120 //usRegHoldingBuf[DEF_INX_TARGET_POSX]=0;
stanley1228 1:249f89a36223 121 }
stanley1228 1:249f89a36223 122 }
stanley1228 1:249f89a36223 123
stanley1228 6:e6e7a2ba9f65 124 unsigned int Modbus_Initial(void)
stanley1228 0:f147c77caac0 125 {
stanley1228 4:8fc956bd0e78 126 eMBErrorCode eStatus;
stanley1228 0:f147c77caac0 127
stanley1228 0:f147c77caac0 128 eStatus = eMBInit( MB_RTU, SLAVE_ID, 0, 115200, MB_PAR_NONE );
stanley1228 0:f147c77caac0 129
stanley1228 0:f147c77caac0 130 /* Enable the Modbus Protocol Stack. */
stanley1228 0:f147c77caac0 131 eMBEnable( );
stanley1228 0:f147c77caac0 132
stanley1228 4:8fc956bd0e78 133 //trial initial value
stanley1228 0:f147c77caac0 134 usRegHoldingBuf[DEF_INX_TARGET_POSX]=0;
stanley1228 0:f147c77caac0 135 usRegHoldingBuf[DEF_INX_TARGET_POSY]=1000;
stanley1228 0:f147c77caac0 136 usRegHoldingBuf[DEF_INX_TARGET_POSZ]=2000;
stanley1228 4:8fc956bd0e78 137 usRegHoldingBuf[DEF_INX_OPMODE]=IDLE;
stanley1228 0:f147c77caac0 138
stanley1228 0:f147c77caac0 139 float fSpeedRatio=0.345;
stanley1228 0:f147c77caac0 140 USHORT* usp=(USHORT*)&fSpeedRatio;
stanley1228 0:f147c77caac0 141 usRegHoldingBuf[DEF_INX_SPPED_RATIO_L]=*usp;
stanley1228 0:f147c77caac0 142 usRegHoldingBuf[DEF_INX_SPPED_RATIO_H]=*(usp+1);
stanley1228 0:f147c77caac0 143
stanley1228 0:f147c77caac0 144 for(int i=0;i<DEF_MAX_AXIS;i++)
stanley1228 0:f147c77caac0 145 {
stanley1228 0:f147c77caac0 146 usRegHoldingBuf[DEF_INX_TARPOS1+i]=500+i;
stanley1228 0:f147c77caac0 147 usRegHoldingBuf[DEF_INX_POSVAL1+i]=1000+i;
stanley1228 0:f147c77caac0 148 usRegHoldingBuf[DEF_INX_VELVAL1+i]=2000+i;
stanley1228 0:f147c77caac0 149 }
stanley1228 0:f147c77caac0 150
stanley1228 4:8fc956bd0e78 151
stanley1228 4:8fc956bd0e78 152 tMobusPoll.attach_us(&fMobusPoll,5000);
stanley1228 0:f147c77caac0 153
stanley1228 4:8fc956bd0e78 154 return 0;
stanley1228 4:8fc956bd0e78 155 }
stanley1228 4:8fc956bd0e78 156
stanley1228 4:8fc956bd0e78 157 unsigned int Jog_Motion(void)
stanley1228 4:8fc956bd0e78 158 {
stanley1228 4:8fc956bd0e78 159 unsigned int rt=0;
stanley1228 4:8fc956bd0e78 160
stanley1228 4:8fc956bd0e78 161 float Ang_rad[MAX_AXIS_NUM]={0};
stanley1228 4:8fc956bd0e78 162 unsigned short int velocity[MAX_AXIS_NUM]={10,10,10,10,10,10,10};
stanley1228 4:8fc956bd0e78 163
stanley1228 4:8fc956bd0e78 164 for(int i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 4:8fc956bd0e78 165 {
stanley1228 4:8fc956bd0e78 166 Ang_rad[i]=usRegHoldingBuf[DEF_INX_TARPOS1+i]*DEF_RATIO_PUS_TO_RAD;
stanley1228 4:8fc956bd0e78 167
stanley1228 4:8fc956bd0e78 168 DBGMSG(("HB[%d]=%d\n",i,usRegHoldingBuf[DEF_INX_TARPOS1+i]))
stanley1228 4:8fc956bd0e78 169 DBGMSG(("Ang_rad[%d]=%f\n",i,Ang_rad[i]))
stanley1228 4:8fc956bd0e78 170 }
stanley1228 4:8fc956bd0e78 171
stanley1228 4:8fc956bd0e78 172
stanley1228 4:8fc956bd0e78 173
stanley1228 4:8fc956bd0e78 174
stanley1228 4:8fc956bd0e78 175 //== Output_to_Dynamixel==//
stanley1228 4:8fc956bd0e78 176 rt=Output_to_Dynamixel(Ang_rad,velocity);
stanley1228 4:8fc956bd0e78 177
stanley1228 4:8fc956bd0e78 178 return rt;
stanley1228 4:8fc956bd0e78 179 }
stanley1228 4:8fc956bd0e78 180
stanley1228 4:8fc956bd0e78 181
stanley1228 4:8fc956bd0e78 182
stanley1228 4:8fc956bd0e78 183 unsigned int P2P_Motion(void)
stanley1228 4:8fc956bd0e78 184 {
stanley1228 4:8fc956bd0e78 185 //==================
stanley1228 4:8fc956bd0e78 186 //== Test IK it need 120ms to calculate
stanley1228 4:8fc956bd0e78 187 //===================
stanley1228 6:e6e7a2ba9f65 188 //unsigned short int Cur_Pus[MAX_AXIS_NUM]={0};
stanley1228 4:8fc956bd0e78 189 unsigned short int Tar_Pus[MAX_AXIS_NUM]={0};
stanley1228 4:8fc956bd0e78 190 //short int Ofset_Pus[MAX_AXIS_NUM]={0};
stanley1228 5:77e9dc268338 191 short int Tar_Point[3]={0};
stanley1228 5:77e9dc268338 192 Tar_Point[0]=(short int)usRegHoldingBuf[DEF_INX_TARGET_POSX];
stanley1228 5:77e9dc268338 193 Tar_Point[1]=(short int)usRegHoldingBuf[DEF_INX_TARGET_POSY];
stanley1228 5:77e9dc268338 194 Tar_Point[2]=(short int)usRegHoldingBuf[DEF_INX_TARGET_POSZ];
stanley1228 5:77e9dc268338 195
stanley1228 5:77e9dc268338 196 DBGMSG(("Tar_Point[%d]=%d\n",0,Tar_Point[0]))
stanley1228 5:77e9dc268338 197 DBGMSG(("Tar_Point[%d]=%d\n",1,Tar_Point[1]))
stanley1228 5:77e9dc268338 198 DBGMSG(("Tar_Point[%d]=%d\n",2,Tar_Point[2]))
stanley1228 5:77e9dc268338 199
stanley1228 4:8fc956bd0e78 200 //==Target position pulse==//
stanley1228 4:8fc956bd0e78 201 float theta[MAX_AXIS_NUM]={0.0};
stanley1228 4:8fc956bd0e78 202 unsigned int rt=0,i=0;
stanley1228 4:8fc956bd0e78 203 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 5:77e9dc268338 204 DBGMSG(("rt=%d\n",rt))
stanley1228 4:8fc956bd0e78 205 if(rt)
stanley1228 4:8fc956bd0e78 206 {
stanley1228 4:8fc956bd0e78 207 return rt;
stanley1228 4:8fc956bd0e78 208 }
stanley1228 4:8fc956bd0e78 209
stanley1228 4:8fc956bd0e78 210 for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 4:8fc956bd0e78 211 {
stanley1228 4:8fc956bd0e78 212 Tar_Pus[i]=theta[i]*DEF_RATIO_RAD_TO_PUS;
stanley1228 5:77e9dc268338 213 DBGMSG(("Tar_Pus[%d]=%d\n",i,Tar_Pus[i]))
stanley1228 4:8fc956bd0e78 214 }
stanley1228 4:8fc956bd0e78 215
stanley1228 4:8fc956bd0e78 216 //==Current position pulse==//
stanley1228 6:e6e7a2ba9f65 217 //for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 6:e6e7a2ba9f65 218 //{
stanley1228 6:e6e7a2ba9f65 219 // Cur_Pus[i]=dxl_read_word(getMapAxisNO(i),PRESENT_POS);
stanley1228 6:e6e7a2ba9f65 220 //}
stanley1228 4:8fc956bd0e78 221
stanley1228 4:8fc956bd0e78 222 //==offset postion pulse==// ©|¥¼¥[¤J³t«×³W¹º
stanley1228 4:8fc956bd0e78 223 //for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 4:8fc956bd0e78 224 //{
stanley1228 4:8fc956bd0e78 225 // Ofset_Pus[i]=Tar_Pus[i]-Cur_Pus[i];
stanley1228 4:8fc956bd0e78 226 //}
stanley1228 4:8fc956bd0e78 227
stanley1228 4:8fc956bd0e78 228 //== Output_to_Dynamixel==//
stanley1228 4:8fc956bd0e78 229 unsigned short int velocity[MAX_AXIS_NUM]={10,10,10,10,10,10,10};
stanley1228 4:8fc956bd0e78 230 rt=Output_to_Dynamixel_pulse(Tar_Pus,velocity);
stanley1228 4:8fc956bd0e78 231
stanley1228 4:8fc956bd0e78 232 return rt;
stanley1228 4:8fc956bd0e78 233 }
stanley1228 4:8fc956bd0e78 234
stanley1228 6:e6e7a2ba9f65 235 void ReadMotorInfo()
stanley1228 6:e6e7a2ba9f65 236 {
stanley1228 6:e6e7a2ba9f65 237 static float pos_pus[MAX_AXIS_NUM]={0};
stanley1228 6:e6e7a2ba9f65 238 unsigned int rt=0;
stanley1228 6:e6e7a2ba9f65 239
stanley1228 6:e6e7a2ba9f65 240 rt=Read_pos(pos_pus,DEF_UNIT_PUS);
stanley1228 6:e6e7a2ba9f65 241 //DBGMSG(("Read_pos rt==%d\n",rt));
stanley1228 6:e6e7a2ba9f65 242 if(rt==0)
stanley1228 6:e6e7a2ba9f65 243 {
stanley1228 6:e6e7a2ba9f65 244 for(int i=0; i<MAX_AXIS_NUM;i++)
stanley1228 6:e6e7a2ba9f65 245 usRegHoldingBuf[DEF_INX_TARPOS1+i]=(unsigned int)pos_pus[i];
stanley1228 6:e6e7a2ba9f65 246
stanley1228 6:e6e7a2ba9f65 247 }
stanley1228 6:e6e7a2ba9f65 248 else
stanley1228 6:e6e7a2ba9f65 249 {
stanley1228 6:e6e7a2ba9f65 250 usRegHoldingBuf[DEF_INX_ERR_STATUS]=rt;
stanley1228 6:e6e7a2ba9f65 251 }
stanley1228 6:e6e7a2ba9f65 252 //for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 6:e6e7a2ba9f65 253 //{
stanley1228 6:e6e7a2ba9f65 254 // DBGMSG(("X%d=%.2f,",getMapAxisNO(i),pos_deg[i]));
stanley1228 6:e6e7a2ba9f65 255 //}
stanley1228 6:e6e7a2ba9f65 256 //for(i=ID_AXIS1;i<=ID_AXIS7;i++)
stanley1228 6:e6e7a2ba9f65 257 //{
stanley1228 6:e6e7a2ba9f65 258 // pos = dxl_read_word(i, PRESENT_POS);
stanley1228 6:e6e7a2ba9f65 259 // if(dxl_get_result()!=COMM_RXSUCCESS)
stanley1228 6:e6e7a2ba9f65 260 // pos=-1;
stanley1228 6:e6e7a2ba9f65 261 //}
stanley1228 6:e6e7a2ba9f65 262 }
stanley1228 6:e6e7a2ba9f65 263
stanley1228 6:e6e7a2ba9f65 264 void ReadMotorInfo_Initial()
stanley1228 6:e6e7a2ba9f65 265 {
stanley1228 6:e6e7a2ba9f65 266 tReadMotorInfo.attach_us(&ReadMotorInfo,5000);
stanley1228 6:e6e7a2ba9f65 267 }
stanley1228 6:e6e7a2ba9f65 268
stanley1228 4:8fc956bd0e78 269
stanley1228 4:8fc956bd0e78 270 int main( void )
stanley1228 4:8fc956bd0e78 271 {
stanley1228 4:8fc956bd0e78 272 unsigned int rt=0;
stanley1228 6:e6e7a2ba9f65 273 Modbus_Initial();
stanley1228 4:8fc956bd0e78 274
stanley1228 0:f147c77caac0 275 myled=1;//stanley
stanley1228 0:f147c77caac0 276
stanley1228 4:8fc956bd0e78 277
stanley1228 4:8fc956bd0e78 278 rt=dxl_initialize( 1, 1);
stanley1228 6:e6e7a2ba9f65 279
stanley1228 6:e6e7a2ba9f65 280 ReadMotorInfo_Initial();
stanley1228 6:e6e7a2ba9f65 281
stanley1228 4:8fc956bd0e78 282 DBGMSG(("dxl_initialize rt=%d\n",rt))
stanley1228 4:8fc956bd0e78 283
stanley1228 4:8fc956bd0e78 284 while(1)
stanley1228 4:8fc956bd0e78 285 {
stanley1228 4:8fc956bd0e78 286
stanley1228 4:8fc956bd0e78 287 switch(usRegHoldingBuf[DEF_INX_OPMODE])
stanley1228 4:8fc956bd0e78 288 {
stanley1228 4:8fc956bd0e78 289
stanley1228 4:8fc956bd0e78 290 case JOG:
stanley1228 4:8fc956bd0e78 291 //usRegHoldingBuf[DEF_INX_STATE]=BUSY;
stanley1228 4:8fc956bd0e78 292 Jog_Motion();
stanley1228 4:8fc956bd0e78 293 //usRegHoldingBuf[DEF_INX_STATE]=DONE;
stanley1228 4:8fc956bd0e78 294 usRegHoldingBuf[DEF_INX_OPMODE]=IDLE;
stanley1228 4:8fc956bd0e78 295
stanley1228 4:8fc956bd0e78 296 break;
stanley1228 4:8fc956bd0e78 297 case IDLE:
stanley1228 5:77e9dc268338 298 break;
stanley1228 4:8fc956bd0e78 299 case P2P:
stanley1228 4:8fc956bd0e78 300 P2P_Motion();
stanley1228 4:8fc956bd0e78 301 usRegHoldingBuf[DEF_INX_OPMODE]=IDLE;
stanley1228 5:77e9dc268338 302 break;
stanley1228 4:8fc956bd0e78 303 case SEWING:
stanley1228 4:8fc956bd0e78 304 case LINE:
stanley1228 4:8fc956bd0e78 305
stanley1228 4:8fc956bd0e78 306
stanley1228 4:8fc956bd0e78 307
stanley1228 4:8fc956bd0e78 308 break;
stanley1228 4:8fc956bd0e78 309
stanley1228 4:8fc956bd0e78 310
stanley1228 4:8fc956bd0e78 311
stanley1228 4:8fc956bd0e78 312 }
stanley1228 4:8fc956bd0e78 313 wait_ms(100);
stanley1228 4:8fc956bd0e78 314
stanley1228 4:8fc956bd0e78 315 }
stanley1228 1:249f89a36223 316
stanley1228 0:f147c77caac0 317 }
stanley1228 0:f147c77caac0 318
stanley1228 0:f147c77caac0 319 eMBErrorCode
stanley1228 0:f147c77caac0 320 eMBRegInputCB( UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNRegs )
stanley1228 0:f147c77caac0 321 {
stanley1228 0:f147c77caac0 322 eMBErrorCode eStatus = MB_ENOERR;
stanley1228 0:f147c77caac0 323 int iRegIndex;
stanley1228 0:f147c77caac0 324
stanley1228 0:f147c77caac0 325 if( ( usAddress >= REG_INPUT_START )
stanley1228 0:f147c77caac0 326 && ( usAddress + usNRegs <= REG_INPUT_START + REG_INPUT_NREGS ) )
stanley1228 0:f147c77caac0 327 {
stanley1228 0:f147c77caac0 328 iRegIndex = ( int )( usAddress - usRegInputStart );
stanley1228 0:f147c77caac0 329 while( usNRegs > 0 )
stanley1228 0:f147c77caac0 330 {
stanley1228 0:f147c77caac0 331 *pucRegBuffer++ =
stanley1228 0:f147c77caac0 332 ( unsigned char )( usRegInputBuf[iRegIndex] >> 8 );
stanley1228 0:f147c77caac0 333 *pucRegBuffer++ =
stanley1228 0:f147c77caac0 334 ( unsigned char )( usRegInputBuf[iRegIndex] & 0xFF );
stanley1228 0:f147c77caac0 335 iRegIndex++;
stanley1228 0:f147c77caac0 336 usNRegs--;
stanley1228 0:f147c77caac0 337 }
stanley1228 0:f147c77caac0 338 }
stanley1228 0:f147c77caac0 339 else
stanley1228 0:f147c77caac0 340 {
stanley1228 0:f147c77caac0 341 eStatus = MB_ENOREG;
stanley1228 0:f147c77caac0 342 }
stanley1228 0:f147c77caac0 343
stanley1228 0:f147c77caac0 344 return eStatus;
stanley1228 0:f147c77caac0 345 }
stanley1228 0:f147c77caac0 346
stanley1228 0:f147c77caac0 347 eMBErrorCode
stanley1228 0:f147c77caac0 348 eMBRegHoldingCB( UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNRegs, eMBRegisterMode eMode ) //change variable to REG_HOLDING ,original is all input register
stanley1228 0:f147c77caac0 349 {
stanley1228 0:f147c77caac0 350 eMBErrorCode eStatus = MB_ENOERR;
stanley1228 0:f147c77caac0 351 int iRegIndex;
stanley1228 0:f147c77caac0 352
stanley1228 0:f147c77caac0 353 if (eMode == MB_REG_READ)
stanley1228 0:f147c77caac0 354 {
stanley1228 0:f147c77caac0 355 if( ( usAddress >= REG_HOLDING_START )
stanley1228 0:f147c77caac0 356 && ( usAddress + usNRegs <= REG_HOLDING_START + REG_HOLDING_NREGS ) )
stanley1228 0:f147c77caac0 357 {
stanley1228 0:f147c77caac0 358 iRegIndex = ( int )( usAddress - usRegHoldingStart );
stanley1228 0:f147c77caac0 359 while( usNRegs > 0 )
stanley1228 0:f147c77caac0 360 {
stanley1228 0:f147c77caac0 361 *pucRegBuffer++ =
stanley1228 0:f147c77caac0 362 ( unsigned char )( usRegHoldingBuf[iRegIndex] >> 8 );
stanley1228 0:f147c77caac0 363 *pucRegBuffer++ =
stanley1228 0:f147c77caac0 364 ( unsigned char )( usRegHoldingBuf[iRegIndex] & 0xFF );
stanley1228 0:f147c77caac0 365 iRegIndex++;
stanley1228 0:f147c77caac0 366 usNRegs--;
stanley1228 0:f147c77caac0 367 }
stanley1228 0:f147c77caac0 368 }
stanley1228 0:f147c77caac0 369 }
stanley1228 0:f147c77caac0 370
stanley1228 0:f147c77caac0 371 if (eMode == MB_REG_WRITE)
stanley1228 0:f147c77caac0 372 {
stanley1228 0:f147c77caac0 373 if( ( usAddress >= REG_HOLDING_START )
stanley1228 0:f147c77caac0 374 && ( usAddress + usNRegs <= REG_HOLDING_START + REG_HOLDING_NREGS ) )
stanley1228 0:f147c77caac0 375 {
stanley1228 0:f147c77caac0 376 iRegIndex = ( int )( usAddress - usRegHoldingStart );
stanley1228 0:f147c77caac0 377 while( usNRegs > 0 )
stanley1228 0:f147c77caac0 378 {
stanley1228 0:f147c77caac0 379 usRegHoldingBuf[iRegIndex] = ((unsigned int) *pucRegBuffer << 8) | ((unsigned int) *(pucRegBuffer+1));
stanley1228 0:f147c77caac0 380 pucRegBuffer+=2;
stanley1228 0:f147c77caac0 381 iRegIndex++;
stanley1228 0:f147c77caac0 382 usNRegs--;
stanley1228 0:f147c77caac0 383 }
stanley1228 0:f147c77caac0 384 }
stanley1228 0:f147c77caac0 385 }
stanley1228 0:f147c77caac0 386
stanley1228 0:f147c77caac0 387 return eStatus;
stanley1228 0:f147c77caac0 388 }
stanley1228 0:f147c77caac0 389
stanley1228 0:f147c77caac0 390
stanley1228 0:f147c77caac0 391 eMBErrorCode
stanley1228 0:f147c77caac0 392 eMBRegCoilsCB( UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNCoils,
stanley1228 0:f147c77caac0 393 eMBRegisterMode eMode )
stanley1228 0:f147c77caac0 394 {
stanley1228 0:f147c77caac0 395 return MB_ENOREG;
stanley1228 0:f147c77caac0 396 }
stanley1228 0:f147c77caac0 397
stanley1228 0:f147c77caac0 398 eMBErrorCode
stanley1228 0:f147c77caac0 399 eMBRegDiscreteCB( UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNDiscrete )
stanley1228 0:f147c77caac0 400 {
stanley1228 0:f147c77caac0 401 return MB_ENOREG;
stanley1228 0:f147c77caac0 402 }
stanley1228 0:f147c77caac0 403