XL320_Driver.cpp

00001 #include "XL320_Driver.h"
00002 
00003 XL320::XL320(uint32_t baud, PinName tx, PinName rx): sXL320(tx, rx)
00004 {
00005     baudrate = baud;
00006     sXL320.baud(baudrate);
00007 
00008     iPacket[0] = 0xFF; //Header
00009     iPacket[1] = 0xFF; //Header
00010     iPacket[2] = 0xFD; //Header
00011     iPacket[3] = 0x00; //Reserved
00012 }
00013 
00014 /***** XL320 Instruction Method *****/
00015 void XL320::iRead(uint8_t id, uint8_t length, uint16_t address)
00016 {
00017     //Creates instruction packet to read data
00018     iPacket[4] = id;
00019     iPacket[5] = 0x07;
00020     iPacket[6] = 0x00;
00021     iPacket[7] = READ_DATA; //Instruction
00022     iPacket[8] = SHIFT_TO_LSB(address);
00023     iPacket[9] = SHIFT_TO_MSB(address);
00024     iPacket[10] = SHIFT_TO_LSB(length);
00025     iPacket[11] = SHIFT_TO_MSB(length);
00026 
00027     uint16_t CRC = update_crc(0, iPacket, 12);
00028     iPacket[12] = SHIFT_TO_LSB(CRC);
00029     iPacket[13] = SHIFT_TO_MSB(CRC);
00030 
00031     iPacketLength = 14;
00032 }
00033 
00034 void XL320::iWrite(uint8_t id, uint16_t address, uint8_t param[], uint8_t paramLength)
00035 {
00036     //Creates instruction packet to write action
00037     int i;
00038 
00039     iPacket[4] = id;            //PacketID
00040     iPacket[5] = (5 + paramLength);  //Packet Lenght LEN_L
00041     iPacket[6] = 0x00;          //Packet Lenght LEN_H
00042     iPacket[7] = WRITE_DATA;    //Instruction
00043     iPacket[8] = SHIFT_TO_LSB(address);
00044     iPacket[9] = SHIFT_TO_MSB(address);
00045     for(i = 10; i < 10 + paramLength ; i++) iPacket[i] = param[i - 10];
00046 
00047     uint16_t CRC = update_crc(0, iPacket, 10 + paramLength);
00048     iPacket[i] = SHIFT_TO_LSB(CRC);
00049     iPacket[i + 1] = SHIFT_TO_MSB(CRC);
00050 
00051     iPacketLength = i + 2;
00052 }
00053 
00054 /***** Dynamixel Protocol 2.0 Methods *****/
00055 void XL320::sendIPacket()
00056 {
00057     float timeOut = 12.0 * (float) ((float) iPacketLength) / ((float) baudrate); //12 = 10 (start + 8 bits + stop) + 2 (gives a bit more time)
00058 
00059     //Send packet
00060     for(int i = 0; i < iPacketLength; i++) sXL320.putc(iPacket[i]);
00061 
00062     //Waits for sending before doing something else
00063     wait(timeOut);
00064 }
00065 
00066 void XL320::getRPacket()
00067 {
00068     int i = 0;
00069     float timeOut = 12.0 * (float) ((float) rPacketLength) / ((float) baudrate); //12 = 10 (start + 8 bits + stop) + 2 (gives a bit more time)
00070     Timer t;
00071 
00072     t.start();
00073     while((i < rPacketLength) && (t.read() <= timeOut)) {
00074         if (sXL320.readable()) {
00075             rPacket[i] = sXL320.getc();
00076             //pc.putc(rPacket[i]); //Prints packets
00077             i++;
00078         }
00079     }
00080     t.stop();
00081 
00082     if (t.read() >= timeOut) rPacket[8] = 0x80; //Creates an error code
00083 }
00084 
00085 /***** Generic functions *****/
00086 void XL320::SetSomething(uint8_t id, uint16_t address, uint8_t param[], uint8_t paramLength)
00087 {
00088     iWrite(id, address, param, paramLength);
00089     rPacketLength = 11;
00090 
00091     sendIPacket();
00092     if (id != 0xFE) getRPacket(); //Broadcast does not returns packets
00093     else rPacket[8] = 0x00; //No error because no rPacket in broadcast
00094 
00095     switch(rPacket[8]) {
00096         case 0x00 : //No error
00097             break;
00098 
00099         case 0x80 :
00100             pc.printf("Missed status packet\r\n");
00101             break;
00102 
00103         default :   //Error occurred
00104             pc.printf("Error %d in status packet\r\n", rPacket[8]);
00105             break;
00106     }
00107 }
00108 
00109 uint8_t XL320::GetSomething(uint8_t id, uint16_t address)
00110 {
00111     iRead(id, 1, address);
00112     rPacketLength = 12;
00113 
00114     sendIPacket();
00115     getRPacket();
00116 
00117     switch(rPacket[8]) {
00118         case 0x00 : //No error
00119             return rPacket[9];
00120 
00121         case 0x80 :
00122             pc.printf("Missed status packet\r\n");
00123             return rPacket[8];
00124 
00125         default :   //Error occurred
00126             pc.printf("Error %d in status packet\r\n", rPacket[8]);
00127             return rPacket[8]; //Return error status
00128     }
00129 }
00130 
00131 uint16_t XL320::GetSomethingElse(uint8_t id, uint16_t address)
00132 {
00133     iRead(id, 2, address);
00134     rPacketLength = 13;
00135 
00136     sendIPacket();
00137     getRPacket();
00138 
00139     switch(rPacket[8]) {
00140         case 0x00 : //No error
00141             return (uint16_t)rPacket[9] | (((uint16_t)rPacket[10]<<8)&0xFF00);
00142 
00143         case 0x80 :
00144             pc.printf("Missed status packet\r\n");
00145             return (uint16_t)rPacket[8];
00146 
00147         default :   //Error occurred
00148             pc.printf("Error %d in status packet\r\n", rPacket[8]);
00149             return (uint16_t)rPacket[8]; //Return error status
00150     }
00151 }
00152 
00153 ////// EEPROM ACCESS METHODS //////
00154 
00155 /***** XL320 Network Parameter *****/
00156 void XL320::SetID(uint8_t id, uint8_t newId)
00157 {
00158     uint8_t parameter[1];
00159     parameter[0] = newId;
00160 
00161     SetSomething(id, XL_ID, parameter, 1);
00162     wait_ms(200);   //Waits for the value to be written in EEPROM
00163 }
00164 
00165 uint8_t XL320::GetID(uint8_t id)
00166 {
00167     return GetSomething(id, XL_ID);
00168 }
00169 
00170 void XL320::SetBaudRate(uint8_t id, uint8_t baudrt)
00171 {
00172     uint8_t parameter[1];
00173     parameter[0] = baudrt;
00174 
00175     SetSomething(id, BAUD_RATE, parameter, 1);
00176 
00177     switch (baudrt) {
00178         case 0:
00179             sXL320.baud(9600);
00180             break;
00181         case 1:
00182             sXL320.baud(57600);
00183             break;
00184         case 2:
00185             sXL320.baud(115200);
00186             break;
00187         case 3:
00188             sXL320.baud(1000000);
00189             break;
00190     }
00191 
00192     wait_ms(200);   //Waits for the value to be written in EEPROM
00193 }
00194 
00195 uint8_t XL320::GetBaudRate(uint8_t id)
00196 {
00197     return GetSomething(id, BAUD_RATE);
00198 }
00199 
00200 void XL320::SetRetDelTime(uint8_t id,uint8_t time)
00201 {
00202     uint8_t parameter[1];
00203     parameter[0] = time;
00204 
00205     SetSomething(id, RETURN_DELAY_TIME, parameter, 1);
00206     wait_ms(200);   //Waits for the value to be written in EEPROM
00207 }
00208 
00209 uint8_t XL320::GetRetDelTime(uint8_t id)
00210 {
00211     return GetSomething(id, RETURN_DELAY_TIME);
00212 }
00213 
00214 /***** XL320 Motor Setting *****/
00215 void XL320::SetCWAngLim(uint8_t id, uint16_t angle)
00216 {
00217     uint8_t parameter[2];
00218     parameter[0] = SHIFT_TO_LSB(angle);
00219     parameter[1] = SHIFT_TO_MSB(angle);
00220 
00221     SetSomething(id, CW_ANGLE_LIMIT, parameter, 2);
00222     wait_ms(200);   //Waits for the value to be written in EEPROM
00223 }
00224 
00225 uint16_t XL320::GetCWAngLim(uint8_t id)
00226 {
00227     return GetSomethingElse(id, CW_ANGLE_LIMIT);
00228 }
00229 
00230 void XL320::SetCCWAngLim(uint8_t id, uint16_t angle)
00231 {
00232     uint8_t parameter[2];
00233     parameter[0] = SHIFT_TO_LSB(angle);
00234     parameter[1] = SHIFT_TO_MSB(angle);
00235 
00236     SetSomething(id, CCW_ANGLE_LIMIT, parameter, 2);
00237     wait_ms(200);   //Waits for the value to be written in EEPROM
00238 }
00239 
00240 uint16_t XL320::GetCCWAngLim(uint8_t id)
00241 {
00242     return GetSomethingElse(id, CCW_ANGLE_LIMIT);
00243 }
00244 
00245 void XL320::SetContMode(uint8_t id, uint8_t mode)
00246 {
00247     uint8_t parameter[1];
00248     parameter[0] = mode;
00249 
00250     SetSomething(id, CONTROL_MODE, parameter, 1);
00251     wait_ms(200);   //Waits for the value to be written in EEPROM
00252 }
00253 
00254 uint8_t XL320::GetContMode(uint8_t id)
00255 {
00256     return GetSomething(id, CONTROL_MODE);
00257 }
00258 
00259 void XL320::SetTempLim(uint8_t id, uint8_t temp)
00260 {
00261     uint8_t parameter[1];
00262     parameter[0] = temp;
00263 
00264     SetSomething(id, LIMIT_TEMPERATURE, parameter, 1);
00265     wait_ms(200);   //Waits for the value to be written in EEPROM
00266 }
00267 
00268 uint8_t XL320::GetTempLim(uint8_t id)
00269 {
00270     return GetSomething(id, LIMIT_TEMPERATURE);
00271 }
00272 
00273 void XL320::SetLowVoltLim(uint8_t id, uint8_t volt)
00274 {
00275     uint8_t parameter[1];
00276     parameter[0] = volt;
00277 
00278     SetSomething(id, LOWER_LIMIT_VOLTAGE, parameter, 1);
00279     wait_ms(200);   //Waits for the value to be written in EEPROM
00280 }
00281 
00282 uint8_t XL320::GetLowVoltLim(uint8_t id)
00283 {
00284     return GetSomething(id, LOWER_LIMIT_VOLTAGE);
00285 }
00286 
00287 void XL320::SetUpVoltLim(uint8_t id, uint8_t volt)
00288 {
00289     uint8_t parameter[1];
00290     parameter[0] = volt;
00291 
00292     SetSomething(id, UPPER_LIMIT_VOLTAGE, parameter, 1);
00293     wait_ms(200);   //Waits for the value to be written in EEPROM
00294 }
00295 
00296 uint8_t XL320::GetUpVoltLim(uint8_t id)
00297 {
00298     return GetSomething(id, UPPER_LIMIT_VOLTAGE);
00299 }
00300 
00301 void XL320::SetMaxTorq(uint8_t id, uint16_t torque)
00302 {
00303     uint8_t parameter[2];
00304     parameter[0] = SHIFT_TO_LSB(torque);
00305     parameter[1] = SHIFT_TO_MSB(torque);
00306 
00307     SetSomething(id, MAX_TORQUE, parameter, 2);
00308     wait_ms(200);   //Waits for the value to be written in EEPROM
00309 }
00310 
00311 uint16_t XL320::GetMaxTorq(uint8_t id)
00312 {
00313     return GetSomethingElse(id, MAX_TORQUE);
00314 }
00315 
00316 void XL320::SetRetLev(uint8_t id, uint8_t level)
00317 {
00318     uint8_t parameter[1];
00319     parameter[0] = level;
00320 
00321     SetSomething(id, RETURN_LEVEL, parameter, 1);
00322     wait_ms(200);   //Waits for the value to be written in EEPROM
00323 }
00324 
00325 uint8_t XL320::GetRetLev(uint8_t id)
00326 {
00327     return GetSomething(id, RETURN_LEVEL);
00328 }
00329 
00330 void XL320::SetAlarmShut(uint8_t id, uint8_t alarm)
00331 {
00332     uint8_t parameter[1];
00333     parameter[0] = alarm;
00334 
00335     SetSomething(id, ALARM_SHUTDOWN, parameter, 1);
00336     wait_ms(200);   //Waits for the value to be written in EEPROM
00337 }
00338 
00339 uint8_t XL320::GetAlarmShut(uint8_t id)
00340 {
00341     return GetSomething(id, ALARM_SHUTDOWN);
00342 }
00343 
00344 ////// RAM ACCESS METHODS //////
00345 
00346 /***** XL320 On/Off *****/
00347 void XL320::SetTorqueEn(uint8_t id, uint8_t enable)
00348 {
00349     uint8_t parameter[1];
00350     parameter[0] = enable;
00351 
00352     SetSomething(id, TORQUE_ENABLE, parameter, 1);
00353 }
00354 
00355 uint8_t XL320::GetTorqueEn(uint8_t id)
00356 {
00357     return GetSomething(id, TORQUE_ENABLE);
00358 }
00359 
00360 void XL320::TurnOnLED(uint8_t id, uint8_t led)
00361 {
00362     uint8_t parameter[1];
00363     parameter[0] = led;
00364 
00365     SetSomething(id, LED, parameter, 1);
00366 }
00367 
00368 uint8_t XL320::GetStatusLED(uint8_t id)
00369 {
00370     return GetSomething(id, LED);
00371 }
00372 
00373 /***** XL320 Motor Control *****/
00374 void XL320::SetDGain(uint8_t id, uint8_t d_cons)
00375 {
00376     uint8_t parameter[1];
00377     parameter[0] = d_cons;
00378 
00379     SetSomething(id, D_GAIN, parameter, 1);
00380 }
00381 
00382 uint8_t XL320::GetDGain(uint8_t id)
00383 {
00384     return GetSomething(id, D_GAIN);
00385 }
00386 
00387 void XL320::SetIGain(uint8_t id, uint8_t i_cons)
00388 {
00389     uint8_t parameter[1];
00390     parameter[0] = i_cons;
00391 
00392     SetSomething(id, I_GAIN, parameter, 1);
00393 }
00394 
00395 uint8_t XL320::GetIGain(uint8_t id)
00396 {
00397     return GetSomething(id, I_GAIN);
00398 }
00399 
00400 void XL320::SetPGain(uint8_t id, uint8_t p_cons)
00401 {
00402     uint8_t parameter[1];
00403     parameter[0] = p_cons;
00404 
00405     SetSomething(id, P_GAIN, parameter, 1);
00406 }
00407 
00408 uint8_t XL320::GetPGain(uint8_t id)
00409 {
00410     return GetSomething(id, P_GAIN);
00411 }
00412 
00413 void XL320::SetGoalPos(uint8_t id, uint16_t position)
00414 {
00415     uint8_t parameter[2];
00416     parameter[0] = SHIFT_TO_LSB(position);
00417     parameter[1] = SHIFT_TO_MSB(position);
00418 
00419     SetSomething(id, GOAL_POSITION, parameter, 2);
00420 }
00421 
00422 uint16_t XL320::GetGoalPos(uint8_t id)
00423 {
00424     return GetSomethingElse(id, GOAL_POSITION);
00425 }
00426 
00427 void XL320::SetGoalVel(uint8_t id, uint16_t velocity)
00428 {
00429     uint8_t parameter[2];
00430     parameter[0] = SHIFT_TO_LSB(velocity);
00431     parameter[1] = SHIFT_TO_MSB(velocity);
00432 
00433     SetSomething(id, GOAL_SPEED, parameter, 2);
00434 }
00435 
00436 uint16_t XL320::GetGoalVel(uint8_t id)
00437 {
00438     return GetSomethingElse(id, GOAL_SPEED);
00439 }
00440 
00441 void XL320::SetGoalTorq(uint8_t id, uint16_t torque)
00442 {
00443     uint8_t parameter[2];
00444     parameter[0] = SHIFT_TO_LSB(torque);
00445     parameter[1] = SHIFT_TO_MSB(torque);
00446 
00447     SetSomething(id, GOAL_TORQUE, parameter, 2);
00448 }
00449 
00450 uint16_t XL320::GetGoalTorq(uint8_t id)
00451 {
00452     return GetSomethingElse(id, GOAL_TORQUE);
00453 }
00454 
00455 /***** XL320 Feedback *****/
00456 uint16_t XL320::GetPresentPos(uint8_t id)
00457 {
00458     return GetSomethingElse(id, PRESENT_POSITION);
00459 }
00460 
00461 uint16_t XL320::GetPresentSpeed(uint8_t id)
00462 {
00463     return GetSomethingElse(id, PRESENT_SPEED);
00464 }
00465 
00466 uint16_t XL320::GetPresentLoad(uint8_t id)
00467 {
00468     return GetSomethingElse(id, PRESENT_LOAD);
00469 }
00470 
00471 uint8_t XL320::GetPresentVolt(uint8_t id)
00472 {
00473     return GetSomething(id, PRESENT_VOLTAGE);
00474 }
00475 
00476 uint8_t XL320::GetPresentTemp(uint8_t id)
00477 {
00478     return GetSomething(id, PRESENT_TEMPERATURE);
00479 }
00480 
00481 /***** XL320 Status *****/
00482 uint8_t XL320::GetRegInst(uint8_t id)
00483 {
00484     return GetSomething(id, REGISTERED_INSTRUCTION);
00485 }
00486 
00487 uint8_t XL320::GetMoving(uint8_t id)
00488 {
00489     return GetSomething(id, MOVING);
00490 }
00491 
00492 uint8_t XL320::GetHWErr(uint8_t id)
00493 {
00494     return GetSomething(id, HARDWARE_ERROR_STATUS);
00495 }
00496 
00497 void XL320::SetPunch(uint8_t id, uint16_t punch)
00498 {
00499     uint8_t parameter[2];
00500     parameter[0] = SHIFT_TO_LSB(punch);
00501     parameter[1] = SHIFT_TO_MSB(punch);
00502 
00503     SetSomething(id, PUNCH, parameter, 2);
00504 }
00505 
00506 uint16_t XL320::GetPunch(uint8_t id)
00507 {
00508     return GetSomethingElse(id, PUNCH);
00509 }
00510 
00511 /***** XL320 Instruction Method *****/
00512 void XL320::FactoryReset(uint8_t id, uint8_t option)
00513 {
00514     iPacket[4] = id;
00515     iPacket[5] = 0x04;
00516     iPacket[6] = 0x00;
00517     iPacket[7] = FACTORY_RESET; //Instruction
00518     iPacket[8] = option;
00519 
00520     uint16_t CRC = update_crc(0, iPacket, 9);
00521     iPacket[9] = SHIFT_TO_LSB(CRC);
00522     iPacket[10] = SHIFT_TO_MSB(CRC);
00523 
00524     iPacketLength = 11;
00525     rPacketLength = 11;
00526 
00527     sendIPacket();
00528     getRPacket();
00529 }
00530 
00531 void XL320::Ping(uint8_t id)
00532 {
00533     iPacket[4] = id;
00534     iPacket[5] = 0x03;
00535     iPacket[6] = 0x00;
00536     iPacket[7] = PING; //Instruction
00537 
00538     uint16_t CRC = update_crc(0, iPacket, 9);
00539     iPacket[8] = SHIFT_TO_LSB(CRC);
00540     iPacket[9] = SHIFT_TO_MSB(CRC);
00541 
00542     iPacketLength = 10;
00543     rPacketLength = 14;
00544 
00545     sendIPacket();
00546     getRPacket();
00547 }
00548 
00549 /***** Calculating CRC Method *****/
00550 unsigned short XL320::update_crc(unsigned short crc_accum, unsigned char *data_blk_ptr, unsigned short data_blk_size)
00551 {
00552     unsigned short i, j;
00553     unsigned short crc_table[256] = {
00554         0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
00555         0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022,
00556         0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 0x8077, 0x0072,
00557         0x0050, 0x8055, 0x805F, 0x005A, 0x804B, 0x004E, 0x0044, 0x8041,
00558         0x80C3, 0x00C6, 0x00CC, 0x80C9, 0x00D8, 0x80DD, 0x80D7, 0x00D2,
00559         0x00F0, 0x80F5, 0x80FF, 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1,
00560         0x00A0, 0x80A5, 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1,
00561         0x8093, 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082,
00562         0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, 0x0192,
00563         0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, 0x01A4, 0x81A1,
00564         0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, 0x01FE, 0x01F4, 0x81F1,
00565         0x81D3, 0x01D6, 0x01DC, 0x81D9, 0x01C8, 0x81CD, 0x81C7, 0x01C2,
00566         0x0140, 0x8145, 0x814F, 0x014A, 0x815B, 0x015E, 0x0154, 0x8151,
00567         0x8173, 0x0176, 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162,
00568         0x8123, 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132,
00569         0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 0x8101,
00570         0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, 0x8317, 0x0312,
00571         0x0330, 0x8335, 0x833F, 0x033A, 0x832B, 0x032E, 0x0324, 0x8321,
00572         0x0360, 0x8365, 0x836F, 0x036A, 0x837B, 0x037E, 0x0374, 0x8371,
00573         0x8353, 0x0356, 0x035C, 0x8359, 0x0348, 0x834D, 0x8347, 0x0342,
00574         0x03C0, 0x83C5, 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1,
00575         0x83F3, 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2,
00576         0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, 0x03B2,
00577         0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, 0x0384, 0x8381,
00578         0x0280, 0x8285, 0x828F, 0x028A, 0x829B, 0x029E, 0x0294, 0x8291,
00579         0x82B3, 0x02B6, 0x02BC, 0x82B9, 0x02A8, 0x82AD, 0x82A7, 0x02A2,
00580         0x82E3, 0x02E6, 0x02EC, 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2,
00581         0x02D0, 0x82D5, 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1,
00582         0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252,
00583         0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261,
00584         0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231,
00585         0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202
00586     };
00587 
00588     for(j = 0; j < data_blk_size; j++) {
00589         i = ((unsigned short)(crc_accum >> 8) ^ data_blk_ptr[j]) & 0xFF;
00590         crc_accum = (crc_accum << 8) ^ crc_table[i];
00591     }
00592     return crc_accum;
00593 }
00594 
00595 XL320::~XL320() {};