PRISMA Lab
mbedos senza corrente
Embed:
(wiki syntax)
Show/hide line numbers
MX.cpp
00001 00002 #include "MX.h" 00003 00004 MX::MX(){ 00005 } 00006 00007 MX::MX(int* broadcastID, int Nmotor, int MotorBaud, UARTSerial_half* pCommLayer){ 00008 _MotorBaud = MotorBaud; 00009 _pCommLayer = pCommLayer; 00010 _broadcastID = broadcastID ; 00011 _Nmotor = Nmotor; 00012 } 00013 00014 00015 int MX::SyncTorqueEnable(bool enableVal[], int ID) { 00016 00017 int Nmotor = _Nmotor; 00018 char data[1*Nmotor]; 00019 00020 for (int i=0 ; i<Nmotor ; i++){ 00021 data[i] = enableVal[i]; 00022 } 00023 00024 #if SYNC_TORQUE_ENABLE_DEBUG 00025 printf("\n SYNC TORQUE ENABLE\n"); 00026 for (int i=0 ; i<Nmotor ; i++){ 00027 printf("\n Torque Enable value for ID[%d] = %d",_broadcastID[i], enableVal[i]); 00028 } 00029 #endif 00030 00031 int rVal = SyncSendPacket(MX_REG_TORQUE_ENABLE, 1, data, ID); 00032 00033 return(rVal); 00034 } 00035 00036 00037 int MX::SyncSetBaud(int MotorBaud[], int ID) { 00038 00039 int Nmotor = _Nmotor; 00040 char data[1*Nmotor]; 00041 00042 for (int i=0 ; i<Nmotor ; i++){ 00043 data[i] = MotorBaud[i]; 00044 } 00045 00046 #if SYNC_SET_BAUD_DEBUG 00047 printf("\n SYNC_SET_BAUD_DEBUG\n"); 00048 for (int i=0 ; i<Nmotor ; i++){ 00049 printf("Set Baud rate value to: 0x%02x\n",MotorBaud[i]); 00050 } 00051 printf( "* 0x00 = 9,600 bps \n" 00052 "* 0x01 = 57,600 bps (DEFAULT)\n" 00053 "* 0x02 = 115,200 bps \n" 00054 "* 0x03 = 1 Mbps \n" 00055 "* 0x04 = 2 Mbps \n" 00056 "* 0x05 = 3 Mbps \n" 00057 "* 0x06 = 4 Mbps \n" 00058 "* 0x06 = 4,5 Mbps \n"); 00059 for (int i=0 ; i<Nmotor ; i++){ 00060 printf("\n Operating mode value for ID[%d] = %d",_broadcastID[i], MotorBaud[i]); 00061 } 00062 #endif 00063 00064 int rVal = SyncSendPacket(MX_REG_OPERATING_MODE, 1, data, ID); 00065 00066 return(rVal); 00067 } 00068 00069 00070 00071 int MX::SyncSetGoal(float degrees, int ID) { 00072 00073 float goal = (degrees) * (float)(4095/360); 00074 00075 goal = goal+2048; 00076 char data[4]; 00077 00078 for (int j = 0 ; j < 4 ; j++){ 00079 data[j] = ((int)goal >> (j*8) ) & 0xff; // top 8 bits 00080 } 00081 00082 #if SYNC_SETGOAL_DEBUG 00083 printf("\n SYNC SET GOAL\n "); 00084 printf("\nGOAL CHOSEN FOR EACH MOTOR\n"); 00085 printf("goal[ID]: value(0-4095) - value(0 - 360)\n"); 00086 printf("goal[%d]: %f - %.02f\n",ID,goal,degrees); 00087 printf("\nDATA TO SET FOR EACH MOTOR (entire buffer RAW values) \n"); 00088 for (int j = 0 ; j < 4; j++){ 00089 printf("data[%d]: %02x\n",j+(4),data[j+(4)]); //debug data 00090 } 00091 #endif 00092 00093 int rVal = SyncSendPacket(MX_REG_GOAL_POSITION, 4, data, ID); 00094 00095 return(rVal); 00096 } 00097 00098 int MX::SyncSetGoal(float degrees[]) { 00099 00100 char data[4*_Nmotor]; //4 is dimension in bytes of instruction 00101 int goal[_Nmotor]; 00102 00103 for (int i=0 ; i<_Nmotor ; i++){ //set goal array with goal in RAW(uint32) values from DEGREES(float) 00104 goal[i] = (degrees[i]) * (float)(4095/360); 00105 goal[i] = goal[i]+2048; 00106 00107 } 00108 00109 for (int i=0 ; i<_Nmotor ; i++){ //set data array in 4 bytes sequence (split RAW(uint32) in 4x bytes(uint8) 0-255 00110 for (int j = 0 ; j < 4; j++){ 00111 data[j+(i*4)] = (goal[i] >> (j*8) ) & 0xff; // top 8 bits 00112 } 00113 } 00114 00115 #if SYNC_SETGOAL_DEBUG 00116 printf("\n SYNC SET GOAL\n "); 00117 printf("\nGOAL CHOSEN FOR EACH MOTOR\n"); 00118 printf("goal[ID]: value(0-4095) - value(0 - 360)\n"); 00119 for (int i=0; i<_Nmotor ; i++){ 00120 printf("goal[%d]: %d - %.02f\n",_broadcastID[i],goal[i],degrees[i]); 00121 } 00122 printf("\nDATA TO SET FOR EACH MOTOR (entire buffer RAW values) \n"); 00123 for (int i=0 ; i<_Nmotor ; i++){ //set data array in 4 bytes sequence (split RAW(uint32) in 4x bytes(uint8) 00124 for (int j = 0 ; j < 4; j++){ 00125 printf("data[%d]: %02x\n",j+(i*4),data[j+(i*4)]); //debug data 00126 } 00127 printf("\n"); 00128 } 00129 #endif 00130 00131 // write the packet, and return the error code 00132 int rVal = SyncSendPacket(MX_REG_GOAL_POSITION, 4, data); 00133 00134 return(rVal); 00135 } 00136 00137 00138 int MX::SyncOperatingMode(int mode[], int ID) { 00139 00140 int Nmotor = _Nmotor; 00141 char data[1*Nmotor]; 00142 00143 for (int i=0 ; i<Nmotor ; i++){ 00144 data[i] = mode[i]; 00145 } 00146 00147 #if SYNC_OPERATING_MODE_DEBUG 00148 printf("\n SYNC OPERATING MODE DEBUG\n"); 00149 for (int i=0 ; i<Nmotor ; i++){ 00150 printf("Set Operating Mode value to: 0x%02x\n",mode[i]); 00151 } 00152 printf( "* 0x00 = Current Control Mode\n" 00153 "* 0x01 = Velocity Control Mode\n" 00154 "* 0x03 = Position Control Mode (DEFAULT)\n" 00155 "* 0x04 = Extended Position Control Mode(Multi-turn)\n" 00156 "* 0x05 = Current-based Position Control Mode\n" 00157 "* 0x16 = PWM Control Mode (Voltage Control Mode)\n"); 00158 for (int i=0 ; i<Nmotor ; i++){ 00159 printf("\n Operating mode value for ID[%d] = %d",_broadcastID[i], mode[i]); 00160 } 00161 #endif 00162 00163 int rVal = SyncSendPacket(MX_REG_OPERATING_MODE, 1, data, ID); 00164 00165 return(rVal); 00166 } 00167 00168 00169 int MX::SyncCurrentLimit(float mAmpere, int ID) { 00170 00171 uint8_t byte = 2; //2 is dimension in bytes of instruction 00172 float goal = (mAmpere) * (float)(1941/6521.76); 00173 char data[byte]; 00174 00175 for (int j = 0 ; j < byte ; j++){ 00176 data[j] = ((int)goal >> (j*8) ) & 0xff; // top 8 bits 00177 } 00178 00179 #if SYNC_CURRENT_LIMIT_DEBUG 00180 printf("\n SYNC CURRENT LIMIT DEBUG\n "); 00181 printf("\nCURRENT LIMIT CHOSEN FOR EACH MOTOR\n"); 00182 printf("Current limit[ID]: value(0-1941) - value(0mA - 6521.76mA)\n"); 00183 printf("Current[%d]: %f - %.02f\n",ID,goal,mAmpere); 00184 printf("\nDATA TO SET FOR EACH MOTOR (entire buffer RAW values) \n"); 00185 for (int j = 0 ; j < byte; j++){ 00186 printf("data[%d]: %02x\n",j+(byte),data[j+(byte)]); //debug data 00187 } 00188 #endif 00189 00190 int rVal = SyncSendPacket(MX_REG_CURRENT_LIMIT, byte, data, ID); 00191 00192 return(rVal); 00193 } 00194 00195 00196 int MX::SyncCurrentLimit(float mAmpere[]) { 00197 00198 uint8_t byte = 2; //2 is dimension in bytes of instruction 00199 char data[byte*_Nmotor]; 00200 int goal[_Nmotor]; 00201 00202 for (int i=0 ; i<_Nmotor ; i++){ //set goal array with goal in RAW(uint32) values from DEGREES(float) 00203 goal[i] = (mAmpere[i]) * (float)(1941/6521.76); 00204 } 00205 00206 for (int i=0 ; i<_Nmotor ; i++){ //set data array in 2 bytes sequence (split RAW(uint32) in 2*bytes(uint8) 00207 for (int j = 0 ; j < byte; j++){ 00208 data[j+(i*byte)] = (goal[i] >> (j*8) ) & 0xff; // top 8 bits 00209 } 00210 } 00211 00212 #if SYNC_CURRENT_LIMIT_DEBUG 00213 printf("\n SYNC CURRENT LIMIT DEBUG\n "); 00214 printf("\nCURRENT LIMIT CHOSEN FOR EACH MOTOR\n"); 00215 printf("Current limit[ID]: value(0-1941) - value(0mA - 6521.76mA)\n"); 00216 for (int i=0; i<_Nmotor ; i++){ 00217 printf("goal[%d]: %d - %.02f\n",_broadcastID[i],goal[i],mAmpere[i]); 00218 } 00219 printf("\nDATA TO SET FOR EACH MOTOR (entire buffer RAW values) \n"); 00220 for (int i=0 ; i<_Nmotor ; i++){ //set data array in 2 bytes sequence (split RAW(uint32) in 2*bytes(uint8) 00221 for (int j = 0 ; j < byte; j++){ 00222 printf("data[%d]: %02x\n",j+(i*byte),data[j+(i*byte)]); //debug data 00223 } 00224 printf("\n"); 00225 } 00226 #endif 00227 00228 // write the packet, and return the error code 00229 int rVal = SyncSendPacket(MX_REG_CURRENT_LIMIT, byte, data); 00230 00231 return(rVal); 00232 } 00233 00234 00235 int MX::SyncGoalCurrent(float mAmpere, int ID) { 00236 00237 uint8_t byte = 2; //2 is dimension in bytes of instruction 00238 float goal = (mAmpere) * (float)(1941/6521.76); 00239 char data[byte]; 00240 00241 for (int j = 0 ; j < byte ; j++){ 00242 data[j] = ((int)goal >> (j*8) ) & 0xff; // top 8 bits 00243 } 00244 00245 #if SYNC_GOAL_CURRENT_DEBUG 00246 printf("\n SYNC GOAL CURRENT\n "); 00247 printf("\nGOAL CHOSEN FOR EACH MOTOR\n"); 00248 printf("goal[ID]: value(-1941-1941) - value(-6521.76mA - 6521.76mA)\n"); 00249 printf("goal[%d]: %f - %.02f\n",ID,goal,mAmpere); 00250 printf("\nDATA TO SET FOR EACH MOTOR (entire buffer RAW values) \n"); 00251 for (int j = 0 ; j < byte; j++){ 00252 printf("data[%d]: %02x\n",j+(byte),data[j+(byte)]); //debug data 00253 } 00254 #endif 00255 00256 int rVal = SyncSendPacket(MX_REG_GOAL_CURRENT, byte, data, ID); 00257 00258 return(rVal); 00259 } 00260 00261 00262 int MX::SyncGoalCurrent(float mAmpere[]) { 00263 00264 uint8_t byte = 2; //2 is dimension in bytes of instruction 00265 char data[byte*_Nmotor]; 00266 int goal[_Nmotor]; 00267 00268 for (int i=0 ; i<_Nmotor ; i++){ //set goal array with goal in RAW(uint32) values from DEGREES(float) 00269 goal[i] = (mAmpere[i]) * (float)(1941/6521.76); 00270 } 00271 00272 for (int i=0 ; i<_Nmotor ; i++){ //set data array in 2 bytes sequence (split RAW(uint32) in 2*bytes(uint8) 00273 for (int j = 0 ; j < byte; j++){ 00274 data[j+(i*byte)] = (goal[i] >> (j*8) ) & 0xff; // top 8 bits 00275 } 00276 } 00277 00278 #if SYNC_GOAL_CURRENT_DEBUG 00279 printf("\n SYNC GOAL CURRENT\n "); 00280 printf("\nGOAL CHOSEN FOR EACH MOTOR\n"); 00281 printf("goal[ID]: value(0-1941) - value(-6521.76mA - 6521.76mA)\n"); 00282 for (int i=0; i<_Nmotor ; i++){ 00283 printf("goal[%d]: %d - %.02f\n",_broadcastID[i],goal[i],mAmpere[i]); 00284 } 00285 printf("\nDATA TO SET FOR EACH MOTOR (entire buffer RAW values) \n"); 00286 for (int i=0 ; i<_Nmotor ; i++){ //set data array in 2 bytes sequence (split RAW(uint32) in 2*bytes(uint8) 00287 for (int j = 0 ; j < byte; j++){ 00288 printf("data[%d]: %02x\n",j+(i*byte),data[j+(i*byte)]); //debug data 00289 } 00290 printf("\n"); 00291 } 00292 #endif 00293 00294 // write the packet, and return the error code 00295 int rVal = SyncSendPacket(MX_REG_GOAL_CURRENT, byte, data); 00296 00297 return(rVal); 00298 } 00299 00300 00301 int MX::SyncGoalPWM(float values[], int ID) { 00302 00303 uint8_t byte = 2; //2 is dimension in bytes of instruction 00304 char data[byte*_Nmotor]; 00305 int goal[_Nmotor]; 00306 00307 for (int i=0 ; i<_Nmotor ; i++){ //set goal array with goal in RAW(uint32) values from DEGREES(float) 00308 goal[i] = (values[i]) * (float)(885/100); 00309 } 00310 00311 for (int i=0 ; i<_Nmotor ; i++){ //set data array in 2 bytes sequence (split RAW(uint32) in 2*bytes(uint8) 00312 for (int j = 0 ; j < byte; j++){ 00313 data[j+(i*byte)] = (goal[i] >> (j*8) ) & 0xff; // top 8 bits 00314 } 00315 } 00316 // write the packet, and return the error code 00317 int rVal = SyncSendPacket(MX_REG_GOAL_PWM, byte, data, ID); 00318 00319 return(rVal); 00320 } 00321 00322 00323 00324 int MX::SyncProfileAccel(float profileValAcc[]) { 00325 00326 char data[4*_Nmotor]; //4 is dimension in bytes of instruction 00327 int goal[_Nmotor]; 00328 00329 for (int i=0 ; i<_Nmotor ; i++){ //set goal array with goal in RAW(uint32) values from DEGREES(float) 00330 goal[i] = (profileValAcc[i]); 00331 } 00332 00333 for (int i=0 ; i<_Nmotor ; i++){ //set data array in 4 bytes sequence (split RAW(uint32) in 4x bytes(uint8) 0-255 00334 for (int j = 0 ; j < 4; j++){ 00335 data[j+(i*4)] = (goal[i] >> (j*8) ) & 0xff; // top 8 bits 00336 } 00337 } 00338 00339 #if SYNC_SETGOAL_DEBUG 00340 printf("\n##########################################"); 00341 printf("\n SYNC SET PROFILE ACCELERATION \n "); 00342 printf("\nACCELLERATION CHOSEN FOR EACH MOTOR\n"); 00343 printf("Acceleration[ID]: value(0-32767) \n"); 00344 for (int i=0; i<_Nmotor ; i++){ 00345 printf("goal[%d] : %d ",_broadcastID[i],goal[i]); 00346 } 00347 printf("\nDATA TO SET FOR EACH MOTOR (entire buffer RAW values) \n"); 00348 for (int i=0 ; i<_Nmotor ; i++){ //set data array in 4 bytes sequence (split RAW(uint32) in 4x bytes(uint8) 00349 for (int j = 0 ; j < 4; j++){ 00350 printf("data[%d]: %02x\n",j+(i*4),data[j+(i*4)]); //debug data 00351 } 00352 printf("\n"); 00353 } 00354 #endif 00355 00356 // write the packet, and return the error code 00357 int rVal = SyncSendPacket( MX_REG_PROFILE_ACCELER , 4, data); 00358 00359 return(rVal); 00360 } 00361 00362 00363 int MX::SyncProfileVel(float profileValueVel[]) { 00364 00365 char data[4*_Nmotor]; //4 is dimension in bytes of instruction 00366 int goal[_Nmotor]; 00367 00368 for (int i=0 ; i<_Nmotor ; i++){ //set goal array with goal in RAW(uint32) values from DEGREES(float) 00369 goal[i] = (profileValueVel[i]); 00370 } 00371 00372 for (int i=0 ; i<_Nmotor ; i++){ //set data array in 4 bytes sequence (split RAW(uint32) in 4x bytes(uint8) 0-255 00373 for (int j = 0 ; j < 4; j++){ 00374 data[j+(i*4)] = (goal[i] >> (j*8) ) & 0xff; // top 8 bits 00375 } 00376 } 00377 00378 #if SYNC_SETGOAL_DEBUG 00379 printf("\n##########################################"); 00380 printf("\n SYNC SET PROFILE VELOCITY \n "); 00381 printf("\nVELOCITY CHOSEN FOR EACH MOTOR\n"); 00382 printf("Velocity[ID]: value(0-44) \n"); 00383 for (int i=0; i<_Nmotor ; i++){ 00384 printf("goal[%d] : %d ",_broadcastID[i],goal[i]); 00385 } 00386 printf("\nDATA TO SET FOR EACH MOTOR (entire buffer RAW values) \n"); 00387 for (int i=0 ; i<_Nmotor ; i++){ //set data array in 4 bytes sequence (split RAW(uint32) in 4x bytes(uint8) 00388 for (int j = 0 ; j < 4; j++){ 00389 printf("data[%d]: %02x\n",j+(i*4),data[j+(i*4)]); //debug data 00390 } 00391 printf("\n"); 00392 } 00393 #endif 00394 00395 // write the packet, and return the error code 00396 int rVal = SyncSendPacket( MX_REG_PROFILE_VELOCITY , 4, data); 00397 00398 return(rVal); 00399 } 00400 00401 void MX::SyncGetPosition(float* angle) { 00402 int bytes = 4; 00403 int NumberOfMotor = _Nmotor; 00404 00405 char data[(11+bytes)*NumberOfMotor]; 00406 00407 int32_t position[NumberOfMotor]; 00408 float ScaleFactor = (float)360/4095; 00409 00410 00411 SyncReadPacket(MX_REG_PRESENT_POSITION, bytes,data); 00412 00413 00414 for (int i=0 ;i<_Nmotor ;i++){ 00415 position[i] = (int)data[12+(11+bytes)*i] << 24; 00416 position[i] |= (int)data[11+(11+bytes)*i] << 16; 00417 position[i] |= (int)data[10+(11+bytes)*i] << 8; 00418 position[i] |= (int)data[9 +(11+bytes)*i]; 00419 } 00420 00421 for (int i=0 ;i<_Nmotor ;i++){ 00422 // angle(degree) obtained from position(0 - 4095) 00423 position[i] = position[i]-2048; 00424 angle[i] = (float)position[i]*ScaleFactor; 00425 } 00426 00427 #if SYNC_GET_POSITION_DEBUG 00428 for (int i=0 ;i<_Nmotor ;i++){ 00429 printf("\nGet RAW current data from ID: %d\n",_broadcastID[i]); 00430 printf(" Data[%d]: 0x%02x\n",(9 +(11+bytes)*i),data[9 +(11+bytes)*i]); 00431 printf(" Data[%d]: 0x%02x\n",(10 +(11+bytes)*i),data[10 +(11+bytes)*i]); 00432 printf(" Data[%d]: 0x%02x\n",(11 +(11+bytes)*i),data[11 +(11+bytes)*i]); 00433 printf(" Data[%d]: 0x%02x\n",(12 +(11+bytes)*i),data[12 +(11+bytes)*i]); 00434 printf("Converted position (0 - 4095): %d\n",position[i]); 00435 printf("Converted angle %f\n\n",angle[i]); 00436 } 00437 #endif 00438 00439 //return angle; 00440 } 00441 00442 void MX::SyncGetCurrent(float* presentCurrent) { 00443 00444 printf("\n##########################################"); 00445 printf("\n SYNC GET CURRENT "); 00446 00447 int bytes = 4; 00448 char data[(11+bytes)*_Nmotor]; 00449 int32_t current[bytes]; 00450 00451 //float presentCurrent[NumberOfMotor] ; 00452 float scaleFactor = (float)3.36; // 3.36mA is a unit for scale from 0 to 1941 in DEC 00453 00454 00455 //char Status[(11+bytes)*_Nmotor]; (11+bytes) 00456 SyncReadPacket(MX_REG_PRESENT_CURRENT, bytes, data); 00457 00458 for (int i=0 ;i<_Nmotor ;i++){ 00459 current[i] = (int)data[12+(11+bytes)*i] << 24; 00460 current[i] |= (int)data[11+(11+bytes)*i] << 16; 00461 current[i] |= (int)data[10+(11+bytes)*i] << 8; 00462 current[i] |= (int)data[9 +(11+bytes)*i]; 00463 } 00464 00465 for (int i=0 ;i<_Nmotor ;i++){ 00466 if ((int)data[10+(11+bytes)*i] == 255){ 00467 current[i] = current[i] - (float)65535; 00468 } 00469 // PresentCurrent (mA) obtained from current (0 - 1941) 00470 presentCurrent[i] = (float)current[i]*scaleFactor; 00471 } 00472 00473 #if SYNC_GET_CURRENT_DEBUG 00474 for (int i=0 ;i<_Nmotor ;i++){ 00475 printf("\nGet RAW current data from ID: %d\n",_broadcastID[i]); 00476 printf(" Data[%d]: 0x%02x\n",(9 +(11+bytes)*i),data[9 +(11+bytes)*i]); 00477 printf(" Data[%d]: 0x%02x\n",(10 +(11+bytes)*i),data[10 +(11+bytes)*i]); 00478 printf(" Data[%d]: 0x%02x\n",(11 +(11+bytes)*i),data[11 +(11+bytes)*i]); 00479 printf(" Data[%d]: 0x%02x\n",(12 +(11+bytes)*i),data[12 +(11+bytes)*i]); 00480 printf("Converted Current (0 - 1941): %d\n",current[i]); 00481 printf("Converted Present Current %f mA\n\n",presentCurrent[i]); 00482 } 00483 #endif 00484 } 00485 00486 00487 int MX::SyncSendPacket(int start, int bytes, char* data, int ID) { 00488 00489 int Nmotor ; 00490 if (ID == -1){ 00491 Nmotor = _Nmotor; 00492 } 00493 else{ 00494 Nmotor = 1; 00495 } 00496 00497 char TxBuf[12+((bytes+1)*Nmotor)+2]; 00498 char Status[11]; 00499 int TxBufSize = sizeof(TxBuf); 00500 00501 // initialization of vector TxBuf 00502 for (int i=0; i<TxBufSize ; i++){ 00503 TxBuf[i] = 0x00; 00504 } 00505 00506 // Inizialization 00507 Status[8]=0x00; //The error is set to zero 00508 // Build the TxPacket first in RAM, then we'll send in one go 00509 TxBuf[0] = 0xff; //H1 00510 TxBuf[1] = 0xff; //H2 00511 TxBuf[2] = 0xfd; //H3 00512 TxBuf[3] = 0x00; //Reserved 00513 TxBuf[4] = 0xfe; //Broadcast ID 00514 // Length 00515 TxBuf[5] = (7+( (bytes+1)*Nmotor) ) & 0xff; //packet length low 8 = inst(1bytes)+ param(instr.bytes+1)+CRC(2bytes) 00516 TxBuf[6] = (7+( (bytes+1)*Nmotor) ) >> 8; //packet length high 00517 // Instruction 00518 TxBuf[7] = 0x83; //Sync Write 00519 // Parameters 00520 TxBuf[8] = start & 0xff; //Low order byte from starting address 00521 TxBuf[9] = start >> 8; //High order byte from starting address 00522 // Bytes lenght needed for instruction 00523 TxBuf[10] = bytes & 0xff; //Low order byte from starting address 00524 TxBuf[11] = bytes >> 8; //High order byte from starting address 00525 // data 00526 // Motor parameters for each motors 00527 00528 #if SYNC_SENDPACKET_DEBUG 00529 printf("\nDEBUG DI SyncSendPacket"); 00530 printf("\nTxBuf length : %d\n",TxBufSize ); 00531 printf("\nTxBuf before data and CRC adding:\n "); 00532 for (int i=0; i<TxBufSize ; i++){ 00533 printf("\nTxBuf[%d] = %x ",i,TxBuf[i]); 00534 } 00535 printf("\n\nData to add"); 00536 for (int i=0; i<(Nmotor*bytes) ; i++){ 00537 printf("\ndata[%d] = %02x ",i,data[i]); 00538 } 00539 #endif 00540 00541 for (int i=0; i<Nmotor ; i++){ 00542 // t is a index of TxBuf 00543 int t = 12 +((bytes+1)*i); // (bytes+1) => instruction dim + 1 (for ID) * i (for each motor) 00544 00545 if (Nmotor == 1){ 00546 #if SYNC_SENDPACKET_DEBUG 00547 printf("\nSINGLE MOTOR "); 00548 #endif 00549 TxBuf[t] = ID; 00550 }else{ 00551 #if SYNC_SENDPACKET_DEBUG 00552 printf("\n\nMULTI MOTOR "); 00553 printf("\nID: %d", _broadcastID[i]); 00554 #endif 00555 TxBuf[t] = _broadcastID[i]; 00556 } 00557 00558 for (int j = 0; j < bytes ; j++){ 00559 //Little endian 00560 TxBuf[t+j+1] = data[j+(i*(bytes))]; //(first byte is a low order byte, second byte are High order byte) 00561 #if SYNC_SENDPACKET_DEBUG 00562 printf("\nTxBuf[%d] = data[%d] = %x ",t+j+1,j+(i*bytes),data[j+(i*bytes)]); 00563 #endif 00564 } 00565 } 00566 // CRC // 00567 uint16_t crc16 ; 00568 int dataSize = TxBufSize-2; 00569 crc16 = update_crc(0, TxBuf, dataSize); 00570 00571 TxBuf[TxBufSize-2] = (crc16 & 0x00FF); 00572 TxBuf[TxBufSize-1] = (crc16>>8) & 0x00FF; 00573 //printf("\nCRC 1 : %02x CRC 1 : %02x ",TxBuf[14+((bytes)*Nmotor)],TxBuf[15+((bytes)*Nmotor)]); 00574 00575 #if SYNC_SENDPACKET_DEBUG_PACKETONLY 00576 printf("\n\n Complete Packet to send of %d elements\n", TxBufSize); 00577 for (int i=0; i<TxBufSize ; i++){ 00578 printf("\nTxBuf[%d] = %02x ",i,TxBuf[i]); 00579 } 00580 printf("\n SYNC SEND PACKET \n"); 00581 for (int i=0; i<TxBufSize ; i++){ 00582 printf("|%02x",TxBuf[i]); 00583 } 00584 printf("\n \n"); 00585 #endif 00586 00587 // Build the TxPacket first in RAM, then we'll send in one go 00588 _pCommLayer->flush(); 00589 _pCommLayer->write(TxBuf,TxBufSize); // UART-write 00590 00591 return(Status[8]); // return error code 00592 } 00593 00594 00595 void MX::SyncReadPacket(int start, int bytes, char* data) { 00596 00597 char TxBuf[12+(_Nmotor)+2]; 00598 char Status[(11+bytes)*_Nmotor]; 00599 int TxBufSize = sizeof(TxBuf); 00600 //char data[(11+bytes)*_Nmotor]; 00601 Status[8] = 0x00; // The error is set to zero 00602 00603 // Build the TxPacket (FIXED !) first in RAM, then we'll send in one go 00604 //Header 00605 TxBuf[0] = 0xff; //H1 00606 TxBuf[1] = 0xff; //H2 00607 TxBuf[2] = 0xfd; //H3 00608 // Reserved 00609 TxBuf[3] = 0x00; 00610 // ID 00611 TxBuf[4] = 0xfe; // Broadcast 00612 // Lenght 7 bytes = inst(1)+ param(4 + numbers of motors)+CRC(2) 00613 TxBuf[5] = 7+(_Nmotor) & 0xff; // packet length low 00614 TxBuf[6] = (7+(_Nmotor)) >> 8; // packet length high 00615 // Instruction 00616 TxBuf[7] = 0x82; 00617 // Param 00618 TxBuf[8] = start & 0xFF; // Address 00619 TxBuf[9] = start >> 8; 00620 TxBuf[10] = bytes & 0xFF;; // Lenght of reply 00621 TxBuf[11] = bytes >> 8; 00622 for (int i=0; i<_Nmotor ;i++){ 00623 TxBuf[12+i] = _broadcastID[i]; 00624 } 00625 // CRC 00626 00627 uint16_t crc16 ; 00628 int dataSize = TxBufSize-2; 00629 crc16 = update_crc(0, TxBuf, dataSize); 00630 TxBuf[TxBufSize-2] = (crc16 & 0x00FF); 00631 TxBuf[TxBufSize-1] = (crc16>>8) & 0x00FF; 00632 00633 #if SYNC_READPACKET_DEBUG_PACKETONLY 00634 printf("\n\n Complete Packet to send of %d elements\n", TxBufSize); 00635 for (int i=0; i<TxBufSize ; i++){ 00636 printf("\nTxBuf[%d] = %02x ",i,TxBuf[i]); 00637 } 00638 printf("\n SYNC READ PACKET \n"); 00639 for (int i=0; i<TxBufSize ; i++){ 00640 printf("|%02x",TxBuf[i]); 00641 } 00642 printf("\n \n"); 00643 #endif 00644 00645 _pCommLayer->flush(); 00646 _pCommLayer->write(TxBuf,TxBufSize); // UART-write 00647 00648 wait_us(200); 00649 00650 _pCommLayer->read_timeout(Status, (11+bytes)*_Nmotor, 2.0); 00651 00652 00653 00654 // Take data from 8th cell of status array 00655 // first byte contain error code, for this we use (bytes+1) 00656 for (int i=0; i<(11+bytes)*_Nmotor ; i++){ 00657 data[i] = Status[i]; 00658 } 00659 00660 00661 00662 #if SYNC_READPACKET_DEBUG_PACKETONLY 00663 printf("\n SYNC-READ (entire buffer RAW values)\n"); 00664 for (int i=0; i<(11+bytes)*_Nmotor ; i++){ 00665 printf("Status[%d]:%02x\n",i,Status[i]); 00666 } 00667 #endif 00668 00669 //return(data); // return error code 00670 } 00671 00672 00673 /////////////////////////////////////////////////////////////////////////////// 00674 ////////////////////////SINGLE OPERATIONS////////////////////////////////////// 00675 /////////////////////////////////////////////////////////////////////////////// 00676 00677 00678 int MX::SetGoalSpeed(int speed, int flags) { 00679 00680 char reg_flag = 0; 00681 char data[4]; 00682 00683 // set the flag is only the register bit is set in the flag 00684 if (flags == 0x2) { 00685 reg_flag = 1; 00686 } 00687 00688 int goal = (speed) ; 00689 00690 data[0] = goal & 0xff; // bottom 8 bits 00691 data[1] = goal >> 8 & 0xff; // top 8 bits 00692 data[2] = goal >> 16 & 0xff; // top 8 bits 00693 data[3] = goal >> 24 & 0xff; // top 8 bits 00694 00695 // write the packet, return the error code 00696 int rVal = sendPacket( MX_REG_GOAL_VELOCITY, 4, data, reg_flag); 00697 00698 if (flags == 1) { 00699 // block until it comes to a halt 00700 while (isMoving()) {} 00701 } 00702 00703 #if SETGOAL_SPEED_DEBUG 00704 int16_t sp = speed * float(234.27/1023); //rev/min 00705 printf("\nSetGoal to: %d Velocity in [rev/min]%d \n" ,speed,sp); 00706 printf("Goal L: 0x%02x ; H: 0x%02x\n",data[0],data[1]); 00707 #endif 00708 00709 return(rVal); 00710 } 00711 00712 00713 int MX::SetGoal(float degrees, int flags) { 00714 00715 char reg_flag = 0; 00716 char data[4]; 00717 00718 // set the flag is only the register bit is set in the flag 00719 if (flags == 0x2) { 00720 reg_flag = 1; 00721 } 00722 00723 int goal = (degrees) * (float)(4095/360); 00724 00725 data[0] = goal & 0xff; // bottom 8 bits 00726 data[1] = goal >> 8 & 0xff; // top 8 bits 00727 data[2] = goal >> 16 & 0xff; // top 8 bits 00728 data[3] = goal >> 24 & 0xff; // top 8 bits 00729 00730 // write the packet, return the error code 00731 int rVal = sendPacket( MX_REG_GOAL_POSITION, 4, data, reg_flag ); 00732 00733 if (flags == 1) { 00734 // block until it comes to a halt 00735 while (isMoving()) {} 00736 } 00737 00738 #if SETGOAL_DEBUG 00739 printf("\n SetGoal to: %d in degree is: %f\n",goal,degrees); 00740 printf(" Goal L: 0x%02x ; H: 0x%02x\n",data[0],data[1]); 00741 #endif 00742 return(rVal); 00743 } 00744 00745 // if flag[0] is set, were blocking 00746 // if flag[1] is set, we're registering 00747 // they are mutually exclusive operations 00748 00749 int MX::OperatingMode(int mode) { 00750 00751 char data[1]; 00752 data[0] = mode; // bottom 8 bits 00753 00754 #if OPERATING_MODE_DEBUG 00755 printf("\nOPERATING_MODE_DEBUG\n"); 00756 printf("Set Operating Mode value to: 0x%02x\n",mode); 00757 printf( "* 0x00 = Current Control Mode\n" 00758 "* 0x01 = Velocity Control Mode\n" 00759 "* 0x03 = Position Control Mode (DEFAULT)\n" 00760 "* 0x04 = Extended Position Control Mode(Multi-turn)\n" 00761 "* 0x05 = Current-based Position Control Mode\n" 00762 "* 0x16 = PWM Control Mode (Voltage Control Mode)\n"); 00763 #endif 00764 00765 //sendPacket(Motor ID, Address, Lenght, data); 00766 //return give back a error code of sendPacket function 00767 return (sendPacket( MX_REG_OPERATING_MODE, 1, data)); 00768 } 00769 00770 00771 // return 1 is the servo is still in flight 00772 bool MX::isMoving(void) { 00773 00774 bool data[1]; 00775 data[0] = 1; 00776 data[0] = readPacket( MX_REG_MOVING,1 ); 00777 return(data[0]); 00778 } 00779 00780 00781 int MX::motorSetBaud (int MotorBaud) { 00782 00783 char data[1]; 00784 data[0] = MotorBaud; 00785 00786 #if SETBAUD_DEBUG 00787 printf("\nSTATUS Packet - SETBAUD_DEBUG\n"); 00788 printf("Set Baud rate value to: 0x%02x\n",MotorBaud); 00789 printf( "* 0x00 = 9,600 bps \n" 00790 "* 0x01 = 57,600 bps (DEFAULT)\n" 00791 "* 0x02 = 115,200 bps \n" 00792 "* 0x03 = 1 Mbps \n" 00793 "* 0x04 = 2 Mbps \n" 00794 "* 0x05 = 3 Mbps \n" 00795 "* 0x06 = 4 Mbps \n" 00796 "* 0x06 = 4,5 Mbps \n"); 00797 #endif 00798 00799 //sendPacket(Motor ID, Address, Lenght, data); 00800 //return give back a error code of sendPacket function 00801 return (sendPacket( MX_REG_BAUD, 1, data)); 00802 } 00803 00804 void MX::Reboot() { 00805 00806 char TxBuf[10]; 00807 char Status[11]; 00808 Status[8]=0x00; //The error is set to zero 00809 00810 // Build the TxPacket first in RAM, then we'll send in one go 00811 TxBuf[0] = 0xff; //H1 00812 TxBuf[1] = 0xff; //H2 00813 TxBuf[2] = 0xfd; //H3 00814 TxBuf[3] = 0x00; //Reserved 00815 TxBuf[4] = _ID; //ID 00816 // packet Length 00817 TxBuf[5] = 0x03; //packet length low 00818 TxBuf[6] = 0x00; //packet length high 00819 // Instruction 00820 TxBuf[7] = 0x08; // Instruction to reboot Device 00821 /*****************CRC***********************/ 00822 uint16_t crc16 ; 00823 int dataSize = sizeof(TxBuf)-2; 00824 crc16 = update_crc(0, TxBuf, dataSize /*5+bytes*/); 00825 TxBuf[8] = (crc16 & 0x00FF); 00826 TxBuf[9] = (crc16>>8) & 0x00FF; 00827 // Build the TxPacket first in RAM, then we'll send in one go 00828 _pCommLayer->flush(); 00829 _pCommLayer->write(TxBuf,10); // UART-write 00830 _pCommLayer->read_timeout(Status, 11, 2.0); 00831 00832 #if REBOOT_ENABLE_DEBUG 00833 printf("\n Reboot Motor: (%d)",enableVal); 00834 #endif 00835 } 00836 00837 00838 int MX::TorqueEnable(bool enableVal) { 00839 00840 char data[1]; 00841 data[0] = enableVal; 00842 00843 #if TORQUE_ENABLE_DEBUG 00844 printf("\n Setting Torque Enable value: (%d)",enableVal); 00845 #endif 00846 00847 return (sendPacket( MX_REG_TORQUE_ENABLE, 1, data)); 00848 } 00849 00850 00851 float MX::GetPosition(void) { 00852 printf("\nGET POSITION "); 00853 char* data; 00854 int32_t position; 00855 float angle = 0; 00856 float ScaleFactor = (float)360/4095; 00857 00858 data = readPacket( MX_REG_PRESENT_POSITION, 4); 00859 position = (uint32_t)data[3] << 24; 00860 position |= (uint32_t)data[2] << 16; 00861 position |= (uint32_t)data[1] << 8; 00862 position |= (uint32_t)data[0]; 00863 // angle(degree) obtained from position(0 - 4095) 00864 angle = (float)position*ScaleFactor; 00865 00866 #if GETPOSITION_DEBUG 00867 printf("\nGetPosition from ID: %d\n",_ID); 00868 for (uint16_t i=0; i<4 ; i++) { 00869 printf(" Data[%d] : 0x%02x\n",(i+1),data[i]); 00870 } 00871 printf(" Read position (0 - 4095): %d\n",position); 00872 printf(" Converted angle %f\n",angle); 00873 #endif 00874 return (angle); 00875 } 00876 00877 00878 int MX::sendPacket( int start, int bytes, char* data, int flag) { 00879 00880 char TxBuf[12+bytes]; 00881 char Status[11]; 00882 Status[8]=0x00; //The error is set to zero 00883 00884 // Build the TxPacket first in RAM, then we'll send in one go 00885 TxBuf[0] = 0xff; //H1 00886 TxBuf[1] = 0xff; //H2 00887 TxBuf[2] = 0xfd; //H3 00888 TxBuf[3] = 0x00; //Reserved 00889 TxBuf[4] = _ID; //ID 00890 // packet Length 00891 TxBuf[5] = (5+bytes) & 0xff; //packet length low 00892 TxBuf[6] = (5+bytes) >> 8; //packet length high 00893 // Instruction 00894 if (flag == 1) { 00895 TxBuf[7]=0x04; 00896 } else { 00897 TxBuf[7]=0x03; 00898 } 00899 // Start Address 00900 TxBuf[8] = start & 0xFF; 00901 TxBuf[9] = start >> 8; 00902 // data 00903 for (int i = bytes-1; i>=0 ; i--) { //little endian 00904 TxBuf[10+i] = data[i]; 00905 } 00906 /*****************CRC***********************/ 00907 uint16_t crc16 ; 00908 int dataSize = sizeof(TxBuf)-2; 00909 crc16 = update_crc(0, TxBuf, dataSize /*5+bytes*/); 00910 00911 TxBuf[10+bytes] = (crc16 & 0x00FF); 00912 TxBuf[11+bytes] = (crc16>>8) & 0x00FF; 00913 // Build the TxPacket first in RAM, then we'll send in one go 00914 _pCommLayer->flush(); 00915 _pCommLayer->write(TxBuf,bytes+12); // UART-write 00916 _pCommLayer->read_timeout(Status, 11, 2.0); 00917 00918 00919 #if SENDPACKET_DEBUG 00920 printf("\nWRITE input parameters:\n"); 00921 printf(" (ID: %d, Address: 0x%x, Bytes: %d, flag: %d)\n",_ID,start,bytes,flag); 00922 printf(" Data written:\n"); 00923 for (int i=0; i < bytes ; i++) { 00924 printf(" Data[%d]: 0x%x\n",i,data[i]); 00925 } 00926 #endif 00927 00928 #if SENDPACKET_DEBUG_INSTRUCTION_PKT 00929 printf("\nINSTRUCTIONS Packet - WRITE_DEBUG \n"); 00930 printf(" H1 : 0x%02x\n",TxBuf[0]); 00931 printf(" H2 : 0x%02x\n",TxBuf[1]); 00932 printf(" H3 : 0x%02x\n",TxBuf[2]); 00933 printf(" Reserved : 0x%02x\n",TxBuf[3]); 00934 printf(" ID : 0x%02x\n",TxBuf[4]); 00935 printf(" Length L : 0x%02x\n",TxBuf[5]); 00936 printf(" Length H : 0x%02x\n",TxBuf[6]); 00937 printf(" Instruction : 0x%02x\n",TxBuf[7]); 00938 printf(" Cmd L : 0x%02x\n",TxBuf[8]); 00939 printf(" Cmd H : 0x%02x\n",TxBuf[9]); 00940 for (uint16_t i=0; i<bytes ; i++) { 00941 printf(" Data : 0x%02x\n",TxBuf[10+i]); 00942 } 00943 printf(" CRC1 : 0x%02x\n",TxBuf[10+bytes]); 00944 printf(" CRC2 : 0x%02x\n",TxBuf[11+bytes]); 00945 printf(" TxBuf = "); 00946 for (int i = 0; i < sizeof(TxBuf) ; i++) { 00947 printf(" [0x%02x]",TxBuf[i]); 00948 } 00949 printf("\n"); 00950 #endif 00951 00952 #if SENDPACKET_DEBUG_STATUS_PKT 00953 printf("\nSTATUS Packet - WRITE_DEBUG\n"); 00954 printf(" H1 : 0x%02x\n",Status[0]); 00955 printf(" H2 : 0x%02x\n",Status[1]); 00956 printf(" H3 : 0x%02x\n",Status[2]); 00957 printf(" Reserved : 0x%02x\n",Status[3]); 00958 printf(" ID : 0x%02x\n",Status[4]); 00959 printf(" Length L : 0x%02x\n",Status[5]); 00960 printf(" Length H : 0x%02x\n",Status[6]); 00961 printf(" Instruction : 0x%02x\n",Status[7]); 00962 printf(" Error : 0x%02x\n",Status[8]); 00963 printf(" CRC1 : 0x%02x\n",Status[9]); 00964 printf(" CRC2 : 0x%02x\n",Status[10]); 00965 printf(" Status = "); 00966 for (int i = 0; i < sizeof(Status) ; i++) { 00967 printf(" [0x%02x]",Status[i]); 00968 } 00969 printf("\n"); 00970 #endif 00971 00972 return(Status[8]); // return error code 00973 } 00974 00975 00976 00977 char* MX::readPacket(int start, int bytes, int flag) { 00978 00979 char TxBuf[14]; 00980 char Status[11+bytes]; 00981 char* data; 00982 Status[8] = 0x00; //The error is set to zero 00983 00984 // Build the TxPacket (FIXED !) first in RAM, then we'll send in one go 00985 TxBuf[0] = 0xff; //H1 00986 TxBuf[1] = 0xff; //H2 00987 TxBuf[2] = 0xfd; //H3 00988 TxBuf[3] = 0x00; //Reserved 00989 TxBuf[4] = _ID; //ID 00990 TxBuf[5] = 0x07; //packet length low 00991 TxBuf[6] = 0x00; //packet length high 00992 TxBuf[7] = 0x02; // Instruction 00993 TxBuf[8] = start & 0xFF; // Start Address 00994 TxBuf[9] = start >> 8; 00995 // N param 00996 TxBuf[10] = 0x04; 00997 TxBuf[11] = 0x00; 00998 /*****************CRC***********************/ 00999 uint16_t crc16 ; 01000 int dataSize = sizeof(TxBuf)-2; 01001 crc16 = update_crc(0, TxBuf, dataSize); 01002 TxBuf[12] = (crc16 & 0x00FF); 01003 TxBuf[13] = (crc16>>8) & 0x00FF; 01004 01005 _pCommLayer->flush(); 01006 _pCommLayer->write(TxBuf,sizeof(TxBuf)); // UART-write 01007 wait(0.0002); 01008 _pCommLayer->read_timeout(Status, 11+bytes, 2.0); 01009 01010 01011 #if MX_READPACKET_DEBUG 01012 printf("\nREAD input parameters:\n"); 01013 printf(" (ID: %d, Address: 0x%x, Bytes: %d, flag: %d)\n",_ID,start,bytes,flag); 01014 printf(" Data written:\n"); 01015 for (int i=0; i < bytes ; i++) { 01016 printf(" Data[%d]: 0x%x\n",i,data[i]); 01017 } 01018 #endif 01019 01020 #if MX_READPACKET_DEBUG_INSTRUCTION_PKT 01021 printf("\nINSTRUCTIONS Packet - READ_DEBUG \n"); 01022 printf(" H1 - Header : 0x%02x\n",TxBuf[0]); 01023 printf(" H2 - Header : 0x%02x\n",TxBuf[1]); 01024 printf(" H3 - Header : 0x%02x\n",TxBuf[2]); 01025 printf(" RESERVED : 0x%02x\n",TxBuf[3]); 01026 printf(" ID : 0x%02x\n",TxBuf[4]); 01027 printf(" Length L : 0x%02x\n",TxBuf[5]); 01028 printf(" Length H : 0x%02x\n",TxBuf[6]); 01029 printf(" Instruction : 0x%02x\n",TxBuf[7]); 01030 printf(" Param1 - Cmd L : 0x%02x\n",TxBuf[8]); 01031 printf(" Param2 - Cmd H : 0x%02x\n",TxBuf[9]); 01032 printf(" Param3 : 0x%02x\n",TxBuf[10]); 01033 printf(" Param4 : 0x%02x\n",TxBuf[11]); 01034 printf(" CRC1 : 0x%02x\n",TxBuf[12]); 01035 printf(" CRC2 : 0x%02x\n",TxBuf[13]); 01036 printf(" TxBuf = "); 01037 for (int i = 0; i < 14 ; i++) { 01038 printf(" [0x%02x]",TxBuf[i]); 01039 } 01040 printf("\n\n"); 01041 #endif 01042 01043 #if MX_READPACKET_DEBUG_STATUS_PKT 01044 printf("\nSTATUS Packet - READ_DEBUG \n"); 01045 printf(" H1 - Header : 0x%02x\n",Status[0]); 01046 printf(" H2 - Header : 0x%02x\n",Status[1]); 01047 printf(" H3 - Header : 0x%02x\n",Status[2]); 01048 printf(" Reserved : 0x%02x\n",Status[3]); 01049 printf(" ID : 0x%02x\n",Status[4]); 01050 printf(" Length L : 0x%02x\n",Status[5]); 01051 printf(" Length H : 0x%02x\n",Status[6]); 01052 printf(" Instruction : 0x%02x\n",Status[7]); 01053 printf(" Error : 0x%02x\n",Status[8]); 01054 for (uint16_t i=0; i<bytes ; i++) { 01055 printf(" Param%d : 0x%02x\n",(i+1),Status[9+i]); 01056 } 01057 printf(" Crc1 : 0x%02x\n",Status[13]); 01058 printf(" Crc2 : 0x%02x\n",Status[14]); 01059 printf(" Status = "); 01060 for (int i = 0; i < sizeof(Status) ; i++) { 01061 printf(" [0x%02x]",Status[i]); 01062 } 01063 printf("\n"); 01064 #endif 01065 for (uint16_t i=0; i<bytes ; i++) { 01066 data[i] = Status[9+i]; 01067 } 01068 return(data); // return error code 01069 } 01070 01071 unsigned short MX::update_crc(unsigned short crc_accum, char *data_blk_ptr, unsigned short data_blk_size) 01072 { 01073 unsigned short i, j; 01074 unsigned short crc_table[256] = { 01075 0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, 01076 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022, 01077 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 0x8077, 0x0072, 01078 0x0050, 0x8055, 0x805F, 0x005A, 0x804B, 0x004E, 0x0044, 0x8041, 01079 0x80C3, 0x00C6, 0x00CC, 0x80C9, 0x00D8, 0x80DD, 0x80D7, 0x00D2, 01080 0x00F0, 0x80F5, 0x80FF, 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1, 01081 0x00A0, 0x80A5, 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1, 01082 0x8093, 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082, 01083 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, 0x0192, 01084 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, 0x01A4, 0x81A1, 01085 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, 0x01FE, 0x01F4, 0x81F1, 01086 0x81D3, 0x01D6, 0x01DC, 0x81D9, 0x01C8, 0x81CD, 0x81C7, 0x01C2, 01087 0x0140, 0x8145, 0x814F, 0x014A, 0x815B, 0x015E, 0x0154, 0x8151, 01088 0x8173, 0x0176, 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162, 01089 0x8123, 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132, 01090 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 0x8101, 01091 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, 0x8317, 0x0312, 01092 0x0330, 0x8335, 0x833F, 0x033A, 0x832B, 0x032E, 0x0324, 0x8321, 01093 0x0360, 0x8365, 0x836F, 0x036A, 0x837B, 0x037E, 0x0374, 0x8371, 01094 0x8353, 0x0356, 0x035C, 0x8359, 0x0348, 0x834D, 0x8347, 0x0342, 01095 0x03C0, 0x83C5, 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1, 01096 0x83F3, 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2, 01097 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, 0x03B2, 01098 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, 0x0384, 0x8381, 01099 0x0280, 0x8285, 0x828F, 0x028A, 0x829B, 0x029E, 0x0294, 0x8291, 01100 0x82B3, 0x02B6, 0x02BC, 0x82B9, 0x02A8, 0x82AD, 0x82A7, 0x02A2, 01101 0x82E3, 0x02E6, 0x02EC, 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2, 01102 0x02D0, 0x82D5, 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1, 01103 0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252, 01104 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261, 01105 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231, 01106 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202 01107 }; 01108 for(j = 0; j < data_blk_size; j++) 01109 { 01110 i = ((unsigned short)(crc_accum >> 8) ^ data_blk_ptr[j]) & 0xFF; 01111 crc_accum = (crc_accum << 8) ^ crc_table[i]; 01112 } 01113 return crc_accum; 01114 }
Generated on Mon Jul 18 2022 22:37:28 by
1.7.2