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@7:9127ccc07448, 2017-04-09 (annotated)

Committer:
stanley1228
Date:
Sun Apr 09 21:50:03 2017 +0800
Revision:
7:9127ccc07448
Parent:
6:e6e7a2ba9f65
Child:
8:0adb0b96d630
1.change baudrate to 460800

2.comment readmotorinfo() now because when add this the system become more unstable

3. change velocity of axis7 to 500 but doesn't work

4. fMobusPoll ticker change to 5ms don't know how is the good

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