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@8:0adb0b96d630, 2017-04-10 (annotated)

Committer:
stanley1228
Date:
Mon Apr 10 23:08:29 2017 +0800
Revision:
8:0adb0b96d630
Parent:
7:9127ccc07448 
1.??Modbus ??????

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 8:0adb0b96d630 62 DEF_INX_TARVEL1=13,
stanley1228 8:0adb0b96d630 63 DEF_INX_TARVEL2,
stanley1228 8:0adb0b96d630 64 DEF_INX_TARVEL3,
stanley1228 8:0adb0b96d630 65 DEF_INX_TARVEL4,
stanley1228 8:0adb0b96d630 66 DEF_INX_TARVEL5,
stanley1228 8:0adb0b96d630 67 DEF_INX_TARVEL6,
stanley1228 8:0adb0b96d630 68 DEF_INX_TARVEL7,
stanley1228 0:f147c77caac0 69
stanley1228 8:0adb0b96d630 70 DEF_INX_POSVAL1 = 20,
stanley1228 8:0adb0b96d630 71 DEF_INX_POSVAL2,
stanley1228 8:0adb0b96d630 72 DEF_INX_POSVAL3,
stanley1228 8:0adb0b96d630 73 DEF_INX_POSVAL4,
stanley1228 8:0adb0b96d630 74 DEF_INX_POSVAL5,
stanley1228 8:0adb0b96d630 75 DEF_INX_POSVAL6,
stanley1228 8:0adb0b96d630 76 DEF_INX_POSVAL7,
stanley1228 4:8fc956bd0e78 77
stanley1228 8:0adb0b96d630 78 DEF_INX_ERR_STATE,
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 8:0adb0b96d630 150 usRegHoldingBuf[DEF_INX_TARVEL1+i]=20;
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 8:0adb0b96d630 169 velocity[i]=usRegHoldingBuf[DEF_INX_TARVEL1+i];
stanley1228 8:0adb0b96d630 170 DBGMSG(("p[%d]=%d\n",i,usRegHoldingBuf[DEF_INX_TARPOS1+i]))
stanley1228 8:0adb0b96d630 171 DBGMSG(("v[%d]=%d\n",i,usRegHoldingBuf[DEF_INX_TARVEL1+i]))
stanley1228 7:9127ccc07448 172 //DBGMSG(("Ang_rad[%d]=%f\n",i,Ang_rad[i]))
stanley1228 4:8fc956bd0e78 173 }
stanley1228 4:8fc956bd0e78 174
stanley1228 4:8fc956bd0e78 175
stanley1228 4:8fc956bd0e78 176
stanley1228 4:8fc956bd0e78 177
stanley1228 4:8fc956bd0e78 178 //== Output_to_Dynamixel==//
stanley1228 4:8fc956bd0e78 179 rt=Output_to_Dynamixel(Ang_rad,velocity);
stanley1228 4:8fc956bd0e78 180
stanley1228 4:8fc956bd0e78 181 return rt;
stanley1228 4:8fc956bd0e78 182 }
stanley1228 4:8fc956bd0e78 183
stanley1228 4:8fc956bd0e78 184
stanley1228 4:8fc956bd0e78 185
stanley1228 4:8fc956bd0e78 186 unsigned int P2P_Motion(void)
stanley1228 4:8fc956bd0e78 187 {
stanley1228 4:8fc956bd0e78 188 //==================
stanley1228 4:8fc956bd0e78 189 //== Test IK it need 120ms to calculate
stanley1228 4:8fc956bd0e78 190 //===================
stanley1228 6:e6e7a2ba9f65 191 //unsigned short int Cur_Pus[MAX_AXIS_NUM]={0};
stanley1228 4:8fc956bd0e78 192 unsigned short int Tar_Pus[MAX_AXIS_NUM]={0};
stanley1228 4:8fc956bd0e78 193 //short int Ofset_Pus[MAX_AXIS_NUM]={0};
stanley1228 5:77e9dc268338 194 short int Tar_Point[3]={0};
stanley1228 5:77e9dc268338 195 Tar_Point[0]=(short int)usRegHoldingBuf[DEF_INX_TARGET_POSX];
stanley1228 5:77e9dc268338 196 Tar_Point[1]=(short int)usRegHoldingBuf[DEF_INX_TARGET_POSY];
stanley1228 5:77e9dc268338 197 Tar_Point[2]=(short int)usRegHoldingBuf[DEF_INX_TARGET_POSZ];
stanley1228 5:77e9dc268338 198
stanley1228 5:77e9dc268338 199 DBGMSG(("Tar_Point[%d]=%d\n",0,Tar_Point[0]))
stanley1228 5:77e9dc268338 200 DBGMSG(("Tar_Point[%d]=%d\n",1,Tar_Point[1]))
stanley1228 5:77e9dc268338 201 DBGMSG(("Tar_Point[%d]=%d\n",2,Tar_Point[2]))
stanley1228 5:77e9dc268338 202
stanley1228 4:8fc956bd0e78 203 //==Target position pulse==//
stanley1228 4:8fc956bd0e78 204 float theta[MAX_AXIS_NUM]={0.0};
stanley1228 4:8fc956bd0e78 205 unsigned int rt=0,i=0;
stanley1228 4:8fc956bd0e78 206 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 207 DBGMSG(("rt=%d\n",rt))
stanley1228 4:8fc956bd0e78 208 if(rt)
stanley1228 4:8fc956bd0e78 209 {
stanley1228 4:8fc956bd0e78 210 return rt;
stanley1228 4:8fc956bd0e78 211 }
stanley1228 4:8fc956bd0e78 212
stanley1228 4:8fc956bd0e78 213 for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 4:8fc956bd0e78 214 {
stanley1228 4:8fc956bd0e78 215 Tar_Pus[i]=theta[i]*DEF_RATIO_RAD_TO_PUS;
stanley1228 5:77e9dc268338 216 DBGMSG(("Tar_Pus[%d]=%d\n",i,Tar_Pus[i]))
stanley1228 4:8fc956bd0e78 217 }
stanley1228 4:8fc956bd0e78 218
stanley1228 4:8fc956bd0e78 219 //==Current position pulse==//
stanley1228 6:e6e7a2ba9f65 220 //for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 6:e6e7a2ba9f65 221 //{
stanley1228 6:e6e7a2ba9f65 222 // Cur_Pus[i]=dxl_read_word(getMapAxisNO(i),PRESENT_POS);
stanley1228 6:e6e7a2ba9f65 223 //}
stanley1228 4:8fc956bd0e78 224
stanley1228 4:8fc956bd0e78 225 //==offset postion pulse==// ©|¥¼¥[¤J³t«×³W¹º
stanley1228 4:8fc956bd0e78 226 //for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 4:8fc956bd0e78 227 //{
stanley1228 4:8fc956bd0e78 228 // Ofset_Pus[i]=Tar_Pus[i]-Cur_Pus[i];
stanley1228 4:8fc956bd0e78 229 //}
stanley1228 4:8fc956bd0e78 230
stanley1228 4:8fc956bd0e78 231 //== Output_to_Dynamixel==//
stanley1228 7:9127ccc07448 232 unsigned short int velocity[MAX_AXIS_NUM]={10,10,10,10,10,10,500};
stanley1228 4:8fc956bd0e78 233 rt=Output_to_Dynamixel_pulse(Tar_Pus,velocity);
stanley1228 4:8fc956bd0e78 234
stanley1228 4:8fc956bd0e78 235 return rt;
stanley1228 4:8fc956bd0e78 236 }
stanley1228 4:8fc956bd0e78 237
stanley1228 6:e6e7a2ba9f65 238 void ReadMotorInfo()
stanley1228 6:e6e7a2ba9f65 239 {
stanley1228 8:0adb0b96d630 240 //static float pos_pus[MAX_AXIS_NUM]={0};
stanley1228 8:0adb0b96d630 241 //unsigned int rt=0;
stanley1228 6:e6e7a2ba9f65 242
stanley1228 7:9127ccc07448 243
stanley1228 7:9127ccc07448 244 led_D7=1;
stanley1228 8:0adb0b96d630 245 //rt=Read_pos(pos_pus,DEF_UNIT_PUS);
stanley1228 8:0adb0b96d630 246
stanley1228 6:e6e7a2ba9f65 247 //DBGMSG(("Read_pos rt==%d\n",rt));
stanley1228 8:0adb0b96d630 248 static USHORT pulse=0;
stanley1228 8:0adb0b96d630 249
stanley1228 8:0adb0b96d630 250 for(int i=Index_AXIS1;i<MAX_AXIS_NUM-1;i++)
stanley1228 6:e6e7a2ba9f65 251 {
stanley1228 8:0adb0b96d630 252
stanley1228 8:0adb0b96d630 253 DBGMSG(("gMapAxisID[%d]=%d\n",i,gMapAxisID[i]))
stanley1228 8:0adb0b96d630 254 pulse = dxl_read_word(gMapAxisID[i], PRESENT_POS);
stanley1228 8:0adb0b96d630 255 if(dxl_get_result()==COMM_RXSUCCESS)
stanley1228 8:0adb0b96d630 256 {
stanley1228 8:0adb0b96d630 257 pulse-=gr2m_offset_pulse[i];
stanley1228 8:0adb0b96d630 258 usRegHoldingBuf[DEF_INX_POSVAL1+i]=pulse;
stanley1228 8:0adb0b96d630 259 DBGMSG(("pulse%d=%d\n",i,pulse))
stanley1228 8:0adb0b96d630 260 }
stanley1228 8:0adb0b96d630 261
stanley1228 8:0adb0b96d630 262
stanley1228 6:e6e7a2ba9f65 263 }
stanley1228 8:0adb0b96d630 264 led_D7=0;
stanley1228 8:0adb0b96d630 265
stanley1228 8:0adb0b96d630 266
stanley1228 8:0adb0b96d630 267 //if(rt==0)
stanley1228 8:0adb0b96d630 268 //{
stanley1228 8:0adb0b96d630 269 // for(int i=0; i<MAX_AXIS_NUM;i++)
stanley1228 8:0adb0b96d630 270 // usRegHoldingBuf[DEF_INX_POSVAL1+i]=(unsigned int)pos_pus[i];
stanley1228 8:0adb0b96d630 271 //
stanley1228 8:0adb0b96d630 272 //}
stanley1228 8:0adb0b96d630 273 //else
stanley1228 8:0adb0b96d630 274 //{
stanley1228 8:0adb0b96d630 275 // usRegHoldingBuf[DEF_INX_ERR_STATUS]=rt;
stanley1228 8:0adb0b96d630 276 //}
stanley1228 6:e6e7a2ba9f65 277 //for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
stanley1228 6:e6e7a2ba9f65 278 //{
stanley1228 6:e6e7a2ba9f65 279 // DBGMSG(("X%d=%.2f,",getMapAxisNO(i),pos_deg[i]));
stanley1228 6:e6e7a2ba9f65 280 //}
stanley1228 6:e6e7a2ba9f65 281 //for(i=ID_AXIS1;i<=ID_AXIS7;i++)
stanley1228 6:e6e7a2ba9f65 282 //{
stanley1228 6:e6e7a2ba9f65 283 // pos = dxl_read_word(i, PRESENT_POS);
stanley1228 6:e6e7a2ba9f65 284 // if(dxl_get_result()!=COMM_RXSUCCESS)
stanley1228 6:e6e7a2ba9f65 285 // pos=-1;
stanley1228 6:e6e7a2ba9f65 286 //}
stanley1228 6:e6e7a2ba9f65 287 }
stanley1228 6:e6e7a2ba9f65 288
stanley1228 6:e6e7a2ba9f65 289 void ReadMotorInfo_Initial()
stanley1228 6:e6e7a2ba9f65 290 {
stanley1228 7:9127ccc07448 291 tReadMotorInfo.attach_us(&ReadMotorInfo,40000); //Read_pos may cost 10ms
stanley1228 6:e6e7a2ba9f65 292 }
stanley1228 6:e6e7a2ba9f65 293
stanley1228 4:8fc956bd0e78 294
stanley1228 4:8fc956bd0e78 295 int main( void )
stanley1228 4:8fc956bd0e78 296 {
stanley1228 4:8fc956bd0e78 297 unsigned int rt=0;
stanley1228 6:e6e7a2ba9f65 298 Modbus_Initial();
stanley1228 4:8fc956bd0e78 299
stanley1228 0:f147c77caac0 300 myled=1;//stanley
stanley1228 0:f147c77caac0 301
stanley1228 4:8fc956bd0e78 302
stanley1228 4:8fc956bd0e78 303 rt=dxl_initialize( 1, 1);
stanley1228 6:e6e7a2ba9f65 304
stanley1228 7:9127ccc07448 305 //ReadMotorInfo_Initial();
stanley1228 6:e6e7a2ba9f65 306
stanley1228 4:8fc956bd0e78 307 DBGMSG(("dxl_initialize rt=%d\n",rt))
stanley1228 4:8fc956bd0e78 308
stanley1228 4:8fc956bd0e78 309 while(1)
stanley1228 4:8fc956bd0e78 310 {
stanley1228 4:8fc956bd0e78 311
stanley1228 4:8fc956bd0e78 312 switch(usRegHoldingBuf[DEF_INX_OPMODE])
stanley1228 4:8fc956bd0e78 313 {
stanley1228 4:8fc956bd0e78 314
stanley1228 4:8fc956bd0e78 315 case JOG:
stanley1228 4:8fc956bd0e78 316 //usRegHoldingBuf[DEF_INX_STATE]=BUSY;
stanley1228 4:8fc956bd0e78 317 Jog_Motion();
stanley1228 4:8fc956bd0e78 318 //usRegHoldingBuf[DEF_INX_STATE]=DONE;
stanley1228 4:8fc956bd0e78 319 usRegHoldingBuf[DEF_INX_OPMODE]=IDLE;
stanley1228 4:8fc956bd0e78 320
stanley1228 4:8fc956bd0e78 321 break;
stanley1228 4:8fc956bd0e78 322 case IDLE:
stanley1228 5:77e9dc268338 323 break;
stanley1228 4:8fc956bd0e78 324 case P2P:
stanley1228 4:8fc956bd0e78 325 P2P_Motion();
stanley1228 4:8fc956bd0e78 326 usRegHoldingBuf[DEF_INX_OPMODE]=IDLE;
stanley1228 5:77e9dc268338 327 break;
stanley1228 4:8fc956bd0e78 328 case SEWING:
stanley1228 4:8fc956bd0e78 329 case LINE:
stanley1228 4:8fc956bd0e78 330
stanley1228 4:8fc956bd0e78 331
stanley1228 4:8fc956bd0e78 332
stanley1228 4:8fc956bd0e78 333 break;
stanley1228 4:8fc956bd0e78 334
stanley1228 4:8fc956bd0e78 335
stanley1228 4:8fc956bd0e78 336
stanley1228 4:8fc956bd0e78 337 }
stanley1228 7:9127ccc07448 338
stanley1228 8:0adb0b96d630 339 ReadMotorInfo();
stanley1228 7:9127ccc07448 340 wait_ms(20);
stanley1228 4:8fc956bd0e78 341
stanley1228 4:8fc956bd0e78 342 }
stanley1228 1:249f89a36223 343
stanley1228 0:f147c77caac0 344 }
stanley1228 0:f147c77caac0 345
stanley1228 0:f147c77caac0 346 eMBErrorCode
stanley1228 0:f147c77caac0 347 eMBRegInputCB( UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNRegs )
stanley1228 0:f147c77caac0 348 {
stanley1228 0:f147c77caac0 349 eMBErrorCode eStatus = MB_ENOERR;
stanley1228 0:f147c77caac0 350 int iRegIndex;
stanley1228 0:f147c77caac0 351
stanley1228 0:f147c77caac0 352 if( ( usAddress >= REG_INPUT_START )
stanley1228 0:f147c77caac0 353 && ( usAddress + usNRegs <= REG_INPUT_START + REG_INPUT_NREGS ) )
stanley1228 0:f147c77caac0 354 {
stanley1228 0:f147c77caac0 355 iRegIndex = ( int )( usAddress - usRegInputStart );
stanley1228 0:f147c77caac0 356 while( usNRegs > 0 )
stanley1228 0:f147c77caac0 357 {
stanley1228 0:f147c77caac0 358 *pucRegBuffer++ =
stanley1228 0:f147c77caac0 359 ( unsigned char )( usRegInputBuf[iRegIndex] >> 8 );
stanley1228 0:f147c77caac0 360 *pucRegBuffer++ =
stanley1228 0:f147c77caac0 361 ( unsigned char )( usRegInputBuf[iRegIndex] & 0xFF );
stanley1228 0:f147c77caac0 362 iRegIndex++;
stanley1228 0:f147c77caac0 363 usNRegs--;
stanley1228 0:f147c77caac0 364 }
stanley1228 0:f147c77caac0 365 }
stanley1228 0:f147c77caac0 366 else
stanley1228 0:f147c77caac0 367 {
stanley1228 0:f147c77caac0 368 eStatus = MB_ENOREG;
stanley1228 0:f147c77caac0 369 }
stanley1228 0:f147c77caac0 370
stanley1228 0:f147c77caac0 371 return eStatus;
stanley1228 0:f147c77caac0 372 }
stanley1228 0:f147c77caac0 373
stanley1228 0:f147c77caac0 374 eMBErrorCode
stanley1228 0:f147c77caac0 375 eMBRegHoldingCB( UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNRegs, eMBRegisterMode eMode ) //change variable to REG_HOLDING ,original is all input register
stanley1228 0:f147c77caac0 376 {
stanley1228 0:f147c77caac0 377 eMBErrorCode eStatus = MB_ENOERR;
stanley1228 0:f147c77caac0 378 int iRegIndex;
stanley1228 0:f147c77caac0 379
stanley1228 0:f147c77caac0 380 if (eMode == MB_REG_READ)
stanley1228 0:f147c77caac0 381 {
stanley1228 0:f147c77caac0 382 if( ( usAddress >= REG_HOLDING_START )
stanley1228 0:f147c77caac0 383 && ( usAddress + usNRegs <= REG_HOLDING_START + REG_HOLDING_NREGS ) )
stanley1228 0:f147c77caac0 384 {
stanley1228 0:f147c77caac0 385 iRegIndex = ( int )( usAddress - usRegHoldingStart );
stanley1228 0:f147c77caac0 386 while( usNRegs > 0 )
stanley1228 0:f147c77caac0 387 {
stanley1228 0:f147c77caac0 388 *pucRegBuffer++ =
stanley1228 0:f147c77caac0 389 ( unsigned char )( usRegHoldingBuf[iRegIndex] >> 8 );
stanley1228 0:f147c77caac0 390 *pucRegBuffer++ =
stanley1228 0:f147c77caac0 391 ( unsigned char )( usRegHoldingBuf[iRegIndex] & 0xFF );
stanley1228 0:f147c77caac0 392 iRegIndex++;
stanley1228 0:f147c77caac0 393 usNRegs--;
stanley1228 0:f147c77caac0 394 }
stanley1228 0:f147c77caac0 395 }
stanley1228 0:f147c77caac0 396 }
stanley1228 0:f147c77caac0 397
stanley1228 0:f147c77caac0 398 if (eMode == MB_REG_WRITE)
stanley1228 0:f147c77caac0 399 {
stanley1228 0:f147c77caac0 400 if( ( usAddress >= REG_HOLDING_START )
stanley1228 0:f147c77caac0 401 && ( usAddress + usNRegs <= REG_HOLDING_START + REG_HOLDING_NREGS ) )
stanley1228 0:f147c77caac0 402 {
stanley1228 0:f147c77caac0 403 iRegIndex = ( int )( usAddress - usRegHoldingStart );
stanley1228 0:f147c77caac0 404 while( usNRegs > 0 )
stanley1228 0:f147c77caac0 405 {
stanley1228 0:f147c77caac0 406 usRegHoldingBuf[iRegIndex] = ((unsigned int) *pucRegBuffer << 8) | ((unsigned int) *(pucRegBuffer+1));
stanley1228 0:f147c77caac0 407 pucRegBuffer+=2;
stanley1228 0:f147c77caac0 408 iRegIndex++;
stanley1228 0:f147c77caac0 409 usNRegs--;
stanley1228 0:f147c77caac0 410 }
stanley1228 0:f147c77caac0 411 }
stanley1228 0:f147c77caac0 412 }
stanley1228 0:f147c77caac0 413
stanley1228 0:f147c77caac0 414 return eStatus;
stanley1228 0:f147c77caac0 415 }
stanley1228 0:f147c77caac0 416
stanley1228 0:f147c77caac0 417
stanley1228 0:f147c77caac0 418 eMBErrorCode
stanley1228 0:f147c77caac0 419 eMBRegCoilsCB( UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNCoils,
stanley1228 0:f147c77caac0 420 eMBRegisterMode eMode )
stanley1228 0:f147c77caac0 421 {
stanley1228 0:f147c77caac0 422 return MB_ENOREG;
stanley1228 0:f147c77caac0 423 }
stanley1228 0:f147c77caac0 424
stanley1228 0:f147c77caac0 425 eMBErrorCode
stanley1228 0:f147c77caac0 426 eMBRegDiscreteCB( UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNDiscrete )
stanley1228 0:f147c77caac0 427 {
stanley1228 0:f147c77caac0 428 return MB_ENOREG;
stanley1228 0:f147c77caac0 429 }
stanley1228 0:f147c77caac0 430