MX28.cpp

00001 /* Copyright (c) 2012 Georgios Petrou, MIT License
00002  *
00003  * Permission is hereby granted, free of charge, to any person obtaining a copy of this software 
00004  * and associated documentation files (the "Software"), to deal in the Software without restriction, 
00005  * including without limitation the rights to use, copy, modify, merge, publish, distribute, 
00006  * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 
00007  * furnished to do so, subject to the following conditions:
00008  *
00009  * The above copyright notice and this permission notice shall be included in all copies or 
00010  * substantial portions of the Software.
00011  *
00012  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 
00013  * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 
00014  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 
00015  * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 
00016  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
00017  */
00018  
00019 #include "MX28.h"
00020 #include "mbed.h"
00021 
00022 #define DELAY_DIR 90 // unit in us
00023 
00024 DigitalOut dirc_rs485(PIN_RS485_DIRC);
00025 
00026 
00027 uint8_t MX28::CommunicatePacket(MX28_PROTOCOL_PACKET *packet)
00028 {    
00029     uint8_t currentParameter = 0;
00030     bool isWholePacket = false;    
00031     uint8_t encoderState = WAIT_ON_HEADER_0;
00032             
00033     packet->checkSum = Utilities::GetCheckSum((uint8_t*)&(packet->servoId), packet->length + 1);
00034 
00035     Timer timer;
00036     timer.start();
00037     
00038     while((timer.read_ms() < MX28_PROTOCOL_COMMAND_RESPONSE_TIMEOUT_MS) && (!isWholePacket))
00039     { 
00040         if(servoSerialHalfDuplex->writeable()) 
00041         {
00042 
00043             switch(encoderState)
00044             {
00045                 case WAIT_ON_HEADER_0:
00046                 {   
00047                     #ifdef MX28_DEBUG
00048                         pc->printf("Write: 0x%02X ", MX28_PROTOCOL_HEADER_0);
00049                     #endif
00050                     dirc_rs485=1;
00051                    // wait_us(DELAY_DIR);
00052 
00053                     servoSerialHalfDuplex->putc(MX28_PROTOCOL_HEADER_0);
00054                  //   dirc_rs485=0;
00055                 //    wait_us(DELAY_DIR);                    
00056                    
00057    
00058                     encoderState = WAIT_ON_HEADER_1;                
00059                     
00060                     break;
00061                 }
00062                 case WAIT_ON_HEADER_1:
00063                 {   
00064                     #ifdef MX28_DEBUG
00065                         pc->printf("0x%02X ", MX28_PROTOCOL_HEADER_1);
00066                     #endif
00067                     
00068                     dirc_rs485=1;
00069               //      wait_us(DELAY_DIR);
00070                     
00071                     servoSerialHalfDuplex->putc(MX28_PROTOCOL_HEADER_1);
00072               //      dirc_rs485=0;
00073               //      wait_us(DELAY_DIR);
00074                     
00075                     encoderState = WAIT_ON_SERVO_ID;
00076                     
00077                     break;   
00078                 } 
00079                 case WAIT_ON_SERVO_ID:
00080                 {   
00081                     #ifdef MX28_DEBUG
00082                         pc->printf("0x%02X ", packet->servoId);
00083                     #endif
00084                     dirc_rs485=1;
00085               //      wait_us(DELAY_DIR);
00086                     
00087                     servoSerialHalfDuplex->putc(packet->servoId);
00088               //      dirc_rs485=0;
00089               //      wait_us(DELAY_DIR);
00090                     
00091                     encoderState = WAIT_ON_LENGTH;
00092                    
00093                     break;
00094                 } 
00095                 case WAIT_ON_LENGTH:
00096                 {
00097                     #ifdef MX28_DEBUG
00098                         pc->printf("0x%02X ", packet->length);
00099                     #endif
00100                     dirc_rs485=1;
00101               //      wait_us(DELAY_DIR);
00102                     servoSerialHalfDuplex->putc(packet->length);
00103               //      dirc_rs485=0;
00104                     
00105                     encoderState = WAIT_ON_INSTRUCTION_ERROR_ID;
00106                     
00107                     break; 
00108                 }
00109                 case WAIT_ON_INSTRUCTION_ERROR_ID:
00110                 {
00111                     #ifdef MX28_DEBUG
00112                         pc->printf("0x%02X ", packet->instructionErrorId);
00113                     #endif
00114                     dirc_rs485=1;
00115               //      wait_us(DELAY_DIR);
00116                     
00117                     servoSerialHalfDuplex->putc(packet->instructionErrorId);
00118                     
00119                     if(packet->length > 2)
00120                         encoderState = WAIT_ON_PARAMETER;                    
00121                     else                   
00122                         encoderState = WAIT_ON_CHECK_SUM;
00123                     
00124                     break;    
00125                 }
00126                 case WAIT_ON_PARAMETER:
00127                 {                      
00128                     #ifdef MX28_DEBUG
00129                         pc->printf("0x%02X ", packet->parameter[currentParameter]);
00130                     #endif
00131                     dirc_rs485=1;
00132               //      wait_us(DELAY_DIR);
00133                     
00134                     servoSerialHalfDuplex->putc(packet->parameter[currentParameter]);
00135                                        
00136                     if(++currentParameter == packet->length - 2)                    
00137                         encoderState = WAIT_ON_CHECK_SUM;
00138                     
00139                     break;
00140                 }
00141                 case WAIT_ON_CHECK_SUM:
00142                 {      
00143                     #ifdef MX28_DEBUG
00144                         pc->printf("0x%02X\r\n", packet->checkSum);
00145                     #endif
00146                     dirc_rs485=1;
00147                //     wait_us(DELAY_DIR);
00148                     
00149                     servoSerialHalfDuplex->putc(packet->checkSum);              
00150                  
00151 
00152                     encoderState = WAIT_ON_HEADER_0; 
00153                     isWholePacket = true;
00154                     
00155                     break;
00156                 }                
00157             }
00158         }    
00159     }
00160     
00161     wait_us(DELAY_DIR);
00162     dirc_rs485=0;  
00163     
00164     #ifdef MX28_DEBUG
00165         pc->printf("Timer: %d ms\r\n", timer.read_ms());                      
00166     #endif    
00167     
00168     timer.stop();
00169     
00170 
00171     
00172     if(!isWholePacket)
00173     {
00174         #ifdef MX28_DEBUG
00175             pc->printf("Error: Write response timeout.\r\n");
00176         #endif
00177        
00178         return MX28_ERRBIT_WRITE_TIMEOUT;
00179     }
00180     
00181     if( packet->servoId == MX28_PROTOCOL_BROADCAST_ID || 
00182         packet->instructionErrorId == MX28_ACTION || 
00183         packet->instructionErrorId == MX28_SYNC_WRITE)
00184         
00185         return MX28_ERRBIT_NONE;   
00186     
00187     currentParameter = 0; 
00188     isWholePacket = false;    
00189     uint8_t decoderState = WAIT_ON_HEADER_0;
00190     
00191     timer.reset();
00192     timer.start();
00193     
00194     while((timer.read_ms() < MX28_PROTOCOL_COMMAND_RESPONSE_TIMEOUT_MS) && (!isWholePacket))
00195     {  
00196         if(servoSerialHalfDuplex->readable()) 
00197         {                        
00198             switch(decoderState)
00199             {
00200                 case WAIT_ON_HEADER_0:
00201                 {   
00202                     dirc_rs485=0;
00203                    // wait_us(DELAY_DIR);
00204                     uint8_t mx28ProtocolHeader0 = servoSerialHalfDuplex->getc(); 
00205                     
00206                     #ifdef MX28_DEBUG
00207                         pc->printf("Read: 0x%02X ", mx28ProtocolHeader0);
00208                     #endif
00209     
00210                     decoderState = WAIT_ON_HEADER_1;                
00211                     
00212                     break;
00213                 }
00214                 case WAIT_ON_HEADER_1:
00215                 {   
00216                     dirc_rs485=0;
00217                     //wait_us(DELAY_DIR);
00218                     uint8_t mx28ProtocolHeader1 = servoSerialHalfDuplex->getc(); 
00219                     
00220                     #ifdef MX28_DEBUG
00221                         pc->printf("0x%02X ", mx28ProtocolHeader1);
00222                     #endif
00223                     
00224                     decoderState = WAIT_ON_SERVO_ID;
00225                     
00226                     break;   
00227                 }
00228                 case WAIT_ON_SERVO_ID:
00229                 {   
00230                     dirc_rs485=0;
00231                     //wait_us(DELAY_DIR);
00232                     packet->servoId = servoSerialHalfDuplex->getc(); 
00233                     
00234                     #ifdef MX28_DEBUG
00235                         pc->printf("0x%02X ", packet->servoId);
00236                     #endif
00237                     
00238                     decoderState = WAIT_ON_LENGTH;
00239                     
00240                     break;
00241                 }                
00242                 case WAIT_ON_LENGTH:
00243                 {
00244                     dirc_rs485=0;
00245                  //   wait_us(DELAY_DIR);
00246                     packet->length = servoSerialHalfDuplex->getc();
00247                     
00248                     #ifdef MX28_DEBUG
00249                         pc->printf("0x%02X ", packet->length);
00250                     #endif
00251                                        
00252                     decoderState = WAIT_ON_INSTRUCTION_ERROR_ID;  
00253                                   
00254                     break;
00255                 } 
00256                 case WAIT_ON_INSTRUCTION_ERROR_ID:
00257                 {
00258                     dirc_rs485=0;
00259                   //  wait_us(DELAY_DIR);
00260                     packet->instructionErrorId = servoSerialHalfDuplex->getc();
00261                     
00262                     #ifdef MX28_DEBUG
00263                         pc->printf("0x%02X ", packet->instructionErrorId);
00264                     #endif
00265                                    
00266                     if(packet->length > 2)
00267                         decoderState = WAIT_ON_PARAMETER;                    
00268                     else                   
00269                         decoderState = WAIT_ON_CHECK_SUM;
00270                     
00271                     break;    
00272                 } 
00273                 case WAIT_ON_PARAMETER:
00274                 {    
00275                     dirc_rs485=0;
00276                //     wait_us(DELAY_DIR);
00277                     uint8_t parameter = servoSerialHalfDuplex->getc();               
00278                     packet->parameter[currentParameter] = parameter;
00279                     
00280                     #ifdef MX28_DEBUG
00281                         pc->printf("0x%02X ", parameter);
00282                     #endif
00283                     
00284                     if(++currentParameter == packet->length - 2)                    
00285                         decoderState = WAIT_ON_CHECK_SUM;
00286                    
00287                     break;
00288                 }
00289                 case WAIT_ON_CHECK_SUM:
00290                 {
00291                     dirc_rs485=0;
00292                //     wait_us(DELAY_DIR);
00293                     packet->checkSum = servoSerialHalfDuplex->getc();
00294                     
00295                     #ifdef MX28_DEBUG
00296                         pc->printf("sum =0x%02X\r\n", packet->checkSum);
00297                     #endif
00298                     
00299                     decoderState = WAIT_ON_HEADER_0; 
00300                     isWholePacket = true;
00301                     
00302                     break;
00303                 }
00304             }
00305         }    
00306     }
00307     
00308     #ifdef MX28_DEBUG
00309         pc->printf("Timer: %d ms\r\n", timer.read_ms());
00310     #endif
00311     
00312     timer.stop();
00313     
00314     if(!isWholePacket)
00315     {
00316         #ifdef MX28_DEBUG
00317             pc->printf("Error: Read response timeout\r\n");
00318         #endif
00319                
00320         return MX28_ERRBIT_READ_TIMEOUT;
00321     }   
00322    
00323     if(packet->instructionErrorId != MX28_ERRBIT_NONE)
00324     {
00325         #ifdef MX28_DEBUG
00326             if(packet->instructionErrorId & MX28_ERRBIT_VOLTAGE)
00327                 pc->printf("Error: Input voltage error\r\n");
00328             if(packet->instructionErrorId & MX28_ERRBIT_ANGLE)
00329                 pc->printf("Error: Angle limit error\r\n");
00330             if(packet->instructionErrorId & MX28_ERRBIT_OVERHEAT)
00331                 pc->printf("Error: Overheat error\r\n");
00332             if(packet->instructionErrorId & MX28_ERRBIT_RANGE)
00333                 pc->printf("Error: Out of range error\r\n");
00334             if(packet->instructionErrorId & MX28_ERRBIT_CHECKSUM)
00335                 pc->printf("Error: Checksum error\r\n");
00336             if(packet->instructionErrorId & MX28_ERRBIT_OVERLOAD)
00337                 pc->printf("Error: Overload error\r\n");
00338             if(packet->instructionErrorId & MX28_ERRBIT_INSTRUCTION)
00339                 pc->printf("Error: Instruction code error\r\n");            
00340         #endif         
00341         
00342         return packet->instructionErrorId;
00343     }
00344    
00345     if(packet->checkSum != Utilities::GetCheckSum((uint8_t*)&(packet->servoId), packet->length + 1))
00346     {
00347         #ifdef MX28_DEBUG
00348             pc->printf("Error: Master got wrong checksum\r\n");
00349         #endif   
00350             
00351         return MX28_ERRBIT_MASTER_CHECKSUM;
00352     }   
00353   
00354     return MX28_ERRBIT_NONE;        
00355 }
00356 
00357 uint8_t MX28::GetModelNumber(uint8_t servoId, uint16_t *modelNumber)
00358 {
00359     MX28_PROTOCOL_PACKET packet;
00360     
00361     packet.servoId = servoId;
00362     packet.length = 4;
00363     packet.instructionErrorId = MX28_READ_DATA;
00364     packet.parameter[0] = MX28_MODEL_NUMBER_L;
00365     packet.parameter[1] = 0x02;
00366     
00367     uint8_t status = CommunicatePacket(&packet);
00368     
00369     if(status == MX28_ERRBIT_NONE)
00370     {
00371         *modelNumber = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);
00372         
00373         #ifdef MX28_DEBUG
00374             pc->printf("Get model number: %hu\r\n", *modelNumber);
00375         #endif
00376     }
00377     
00378     return status;
00379 }
00380 
00381 uint8_t MX28::GetFirmwareVersion(uint8_t servoId, uint8_t *firmwareVersion)
00382 {
00383     MX28_PROTOCOL_PACKET packet;
00384     
00385     packet.servoId = servoId;
00386     packet.length = 4;
00387     packet.instructionErrorId = MX28_READ_DATA;
00388     packet.parameter[0] = MX28_VERSION;
00389     packet.parameter[1] = 0x01;
00390  
00391     uint8_t status = CommunicatePacket(&packet);
00392         
00393     if(status == MX28_ERRBIT_NONE)
00394     {
00395        *firmwareVersion = packet.parameter[0];        
00396         
00397         #ifdef MX28_DEBUG
00398             pc->printf("Get firmware version: 0x%02X\r\n", *firmwareVersion);
00399         #endif
00400      }
00401      
00402      return status;
00403 }
00404 
00405 uint8_t MX28::GetId(uint8_t servoId, uint8_t *id)
00406 {
00407     MX28_PROTOCOL_PACKET packet;
00408     
00409     packet.servoId = servoId;
00410     packet.length = 4;
00411     packet.instructionErrorId = MX28_READ_DATA;
00412     packet.parameter[0] = MX28_ID;
00413     packet.parameter[1] = 0x01;
00414  
00415     uint8_t status = CommunicatePacket(&packet);
00416         
00417     if(status == MX28_ERRBIT_NONE)
00418     {
00419         *id = packet.parameter[0];        
00420         
00421         #ifdef MX28_DEBUG
00422             pc->printf("Get id: 0x%02X\r\n", *id);
00423         #endif        
00424     }
00425     
00426     return status;
00427 }
00428 
00429 uint8_t MX28::SetId(uint8_t servoId, uint8_t id, bool isRegWrite) 
00430 {
00431     MX28_PROTOCOL_PACKET packet;
00432 
00433     packet.servoId = servoId;
00434     packet.length = 4;
00435     packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
00436     packet.parameter[0] = MX28_ID;
00437     packet.parameter[1] = id;
00438     
00439     #ifdef MX28_DEBUG
00440         pc->printf("Set id: 0x%02X\r\n", id);
00441     #endif
00442 
00443     return CommunicatePacket(&packet);  
00444 }
00445 
00446 uint8_t MX28::GetBaudRate(uint8_t servoId, int32_t *baudRate)
00447 {
00448     MX28_PROTOCOL_PACKET packet;
00449     
00450     packet.servoId = servoId;
00451     packet.length = 4;
00452     packet.instructionErrorId = MX28_READ_DATA;
00453     packet.parameter[0] = MX28_BAUD_RATE;
00454     packet.parameter[1] = 1;
00455  
00456     uint8_t status = CommunicatePacket(&packet);
00457         
00458     if(status == MX28_ERRBIT_NONE)
00459     {
00460         if(packet.parameter[0] < 0xFA)
00461             *baudRate = 2000000.0 / (double)(packet.parameter[0] + 1);
00462         else if(packet.parameter[0] == 0xFA)
00463             *baudRate = 2250000;
00464         else if(packet.parameter[0] == 0xFB)
00465             *baudRate = 2500000;
00466         else
00467             *baudRate = 3000000;          
00468        
00469         #ifdef MX28_DEBUG
00470             pc->printf("Get baud rate: %d\r\n", *baudRate);
00471         #endif
00472     } 
00473      
00474     return status;
00475 }
00476 
00477 uint8_t MX28::SetBaudRate(uint8_t servoId, int32_t baudRate, bool isRegWrite) 
00478 {
00479     MX28_PROTOCOL_PACKET packet;
00480     
00481     uint8_t divisor = 0x00;
00482     
00483     if(baudRate < 2250000)
00484         divisor = (uint8_t)(2000000.0 / (double)baudRate) - 1;
00485     else if(baudRate == 2250000)
00486         divisor = 0xFA;
00487     else if(baudRate == 2500000)
00488         divisor = 0xFB; 
00489     else          
00490         divisor = 0xFC; 
00491         
00492     packet.servoId = servoId;
00493     packet.length = 4;
00494     packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
00495     packet.parameter[0] = MX28_BAUD_RATE;
00496     packet.parameter[1] = divisor;
00497     
00498     #ifdef MX28_DEBUG
00499         pc->printf("Set baudrate: 0x%02X\r\n", divisor);
00500     #endif    
00501     
00502     return CommunicatePacket(&packet);  
00503 }
00504 
00505 uint8_t MX28::GetReturnDelayTime(uint8_t servoId, uint8_t *returnDelayTime)
00506 {
00507     MX28_PROTOCOL_PACKET packet;
00508     
00509     packet.servoId = servoId;
00510     packet.length = 4;
00511     packet.instructionErrorId = MX28_READ_DATA;
00512     packet.parameter[0] = MX28_RETURN_DELAY_TIME;
00513     packet.parameter[1] = 0x01;
00514  
00515     uint8_t status = CommunicatePacket(&packet);
00516         
00517     if(status == MX28_ERRBIT_NONE)
00518     {
00519         *returnDelayTime = packet.parameter[0];        
00520         
00521         #ifdef MX28_DEBUG
00522             pc->printf("Get return delay time: 0x%02X\r\n", *returnDelayTime);
00523         #endif
00524     }
00525     
00526     return status;
00527 }
00528 
00529 uint8_t MX28::SetReturnDelayTime(uint8_t servoId, uint8_t returnDelayTime, bool isRegWrite)
00530 {
00531     MX28_PROTOCOL_PACKET packet;
00532        
00533     packet.servoId = servoId;
00534     packet.length = 4;
00535     packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
00536     packet.parameter[0] = MX28_RETURN_DELAY_TIME;
00537     packet.parameter[1] = returnDelayTime;
00538     
00539     #ifdef MX28_DEBUG
00540         pc->printf("Set return delay time: 0x%02X\r\n", returnDelayTime);
00541     #endif
00542 
00543     return CommunicatePacket(&packet);  
00544 }
00545 
00546 uint8_t MX28::GetCWAngleLimit(uint8_t servoId, uint16_t *cwAngleLimit)
00547 {
00548     MX28_PROTOCOL_PACKET packet;
00549     
00550     packet.servoId = servoId;
00551     packet.length = 4;
00552     packet.instructionErrorId = MX28_READ_DATA;
00553     packet.parameter[0] = MX28_CW_ANGLE_LIMIT_L;
00554     packet.parameter[1] = 0x02;
00555  
00556     uint8_t status = CommunicatePacket(&packet);
00557         
00558     if(status == MX28_ERRBIT_NONE)
00559     {
00560         *cwAngleLimit = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);       
00561         
00562         #ifdef MX28_DEBUG
00563             pc->printf("Set CW angle limit: %hu\r\n", *cwAngleLimit);
00564         #endif
00565     }
00566     
00567     return status;
00568 }
00569 
00570 uint8_t MX28::SetCWAngleLimit(uint8_t servoId, uint16_t cwAngleLimit, bool isRegWrite)
00571 {
00572     MX28_PROTOCOL_PACKET packet;
00573        
00574     packet.servoId = servoId;
00575     packet.length = 5;
00576     packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
00577     packet.parameter[0] = MX28_CW_ANGLE_LIMIT_L;
00578     Utilities::ConvertInt16ToUInt8Array(cwAngleLimit, (uint8_t*)&(packet.parameter[1]));
00579     
00580     #ifdef MX28_DEBUG
00581         pc->printf("Set Cw angle limit: %hu\r\n", cwAngleLimit);
00582     #endif
00583 
00584     return CommunicatePacket(&packet);  
00585 }
00586 
00587 uint8_t MX28::GetCCWAngleLimit(uint8_t servoId, uint16_t *ccwAngleLimit)
00588 {
00589     MX28_PROTOCOL_PACKET packet;
00590     
00591     packet.servoId = servoId;
00592     packet.length = 4;
00593     packet.instructionErrorId = MX28_READ_DATA;
00594     packet.parameter[0] = MX28_CCW_ANGLE_LIMIT_L;
00595     packet.parameter[1] = 0x02;
00596  
00597     uint8_t status = CommunicatePacket(&packet);
00598         
00599     if(status == MX28_ERRBIT_NONE)
00600     {
00601         *ccwAngleLimit = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);       
00602         
00603         #ifdef MX28_DEBUG
00604             pc->printf("Get CCW angle limit: %hu\r\n", *ccwAngleLimit);
00605         #endif
00606     }
00607     
00608     return status;
00609 }
00610 
00611 uint8_t MX28::SetCCWAngleLimit(uint8_t servoId, uint16_t ccwAngleLimit, bool isRegWrite)
00612 {
00613     MX28_PROTOCOL_PACKET packet;
00614        
00615     packet.servoId = servoId;
00616     packet.length = 5;
00617     packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
00618     packet.parameter[0] = MX28_CCW_ANGLE_LIMIT_L;
00619     Utilities::ConvertUInt16ToUInt8Array(ccwAngleLimit, (uint8_t*)&(packet.parameter[1]));
00620     
00621     #ifdef MX28_DEBUG
00622         pc->printf("Set CCW angle limit: %hu\r\n", ccwAngleLimit);
00623     #endif
00624 
00625     return CommunicatePacket(&packet);  
00626 }
00627 
00628 uint8_t MX28::GetHighestTemperatureLimit(uint8_t servoId, uint8_t *highestTemperatureLimit)
00629 {
00630     MX28_PROTOCOL_PACKET packet;
00631     
00632     packet.servoId = servoId;
00633     packet.length = 4;
00634     packet.instructionErrorId = MX28_READ_DATA;
00635     packet.parameter[0] = MX28_UP_LIMIT_TEMPERATURE;
00636     packet.parameter[1] = 0x01;
00637  
00638     uint8_t status = CommunicatePacket(&packet);
00639         
00640     if(status == MX28_ERRBIT_NONE)
00641     {
00642         *highestTemperatureLimit = packet.parameter[0];       
00643         
00644         #ifdef MX28_DEBUG
00645             pc->printf("Get highest temperature limit: 0x%02X\r\n", *highestTemperatureLimit);
00646         #endif
00647     }
00648     
00649     return status;
00650 }
00651 
00652 uint8_t MX28::SetHighestTemperatureLimit(uint8_t servoId, uint8_t highestTemperatureLimit, bool isRegWrite)
00653 {
00654     MX28_PROTOCOL_PACKET packet;
00655        
00656     packet.servoId = servoId;
00657     packet.length = 4;
00658     packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
00659     packet.parameter[0] = MX28_UP_LIMIT_TEMPERATURE;
00660     packet.parameter[1] = highestTemperatureLimit;
00661     
00662     #ifdef MX28_DEBUG
00663         pc->printf("Set highest temperature limit: 0x%02X\r\n", highestTemperatureLimit);
00664     #endif
00665 
00666     return CommunicatePacket(&packet);
00667 }
00668 
00669 uint8_t MX28::GetLowestVoltageLimit(uint8_t servoId, uint8_t *lowestVoltageLimit)
00670 {
00671     MX28_PROTOCOL_PACKET packet;
00672     
00673     packet.servoId = servoId;
00674     packet.length = 4;
00675     packet.instructionErrorId = MX28_READ_DATA;
00676     packet.parameter[0] = MX28_DOWN_LIMIT_VOLTAGE;
00677     packet.parameter[1] = 0x01;
00678  
00679     uint8_t status = CommunicatePacket(&packet);
00680         
00681     if(status == MX28_ERRBIT_NONE)
00682     {
00683         *lowestVoltageLimit = packet.parameter[0];       
00684         
00685         #ifdef MX28_DEBUG
00686             pc->printf("Get lowest voltage limit: 0x%02X\r\n", *lowestVoltageLimit);
00687         #endif
00688     }
00689     
00690     return status;
00691 }
00692 
00693 uint8_t MX28::SetLowestVoltageLimit(uint8_t servoId, uint8_t lowestVoltageLimit, bool isRegWrite)
00694 {
00695     MX28_PROTOCOL_PACKET packet;
00696        
00697     packet.servoId = servoId;
00698     packet.length = 4;
00699     packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
00700     packet.parameter[0] = MX28_DOWN_LIMIT_VOLTAGE;
00701     packet.parameter[1] = lowestVoltageLimit;
00702     
00703     #ifdef MX28_DEBUG
00704         pc->printf("Set lowest voltage limit: 0x%02X\r\n", lowestVoltageLimit);
00705     #endif
00706 
00707     return CommunicatePacket(&packet);
00708 }
00709 
00710 uint8_t MX28::GetHighestVoltageLimit(uint8_t servoId, uint8_t *highestVoltageLimit)
00711 {
00712     MX28_PROTOCOL_PACKET packet;
00713     
00714     packet.servoId = servoId;
00715     packet.length = 4;
00716     packet.instructionErrorId = MX28_READ_DATA;
00717     packet.parameter[0] = MX28_UP_LIMIT_VOLTAGE;
00718     packet.parameter[1] = 0x01;
00719  
00720     uint8_t status = CommunicatePacket(&packet);
00721         
00722     if(status == MX28_ERRBIT_NONE)
00723     {
00724         *highestVoltageLimit = packet.parameter[0];       
00725         
00726         #ifdef MX28_DEBUG
00727             pc->printf("Get highest voltage limit: 0x%02X\r\n", *highestVoltageLimit);
00728         #endif
00729     }
00730     
00731     return status;
00732 } 
00733 
00734 uint8_t MX28::SetHighestVoltageLimit(uint8_t servoId, uint8_t highestVoltageLimit, bool isRegWrite)
00735 {
00736     MX28_PROTOCOL_PACKET packet;
00737        
00738     packet.servoId = servoId;
00739     packet.length = 4;
00740     packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
00741     packet.parameter[0] = MX28_UP_LIMIT_VOLTAGE;
00742     packet.parameter[1] = highestVoltageLimit;
00743     
00744     #ifdef MX28_DEBUG
00745         pc->printf("Set highest voltage limit: 0x%02X\r\n", highestVoltageLimit);
00746     #endif
00747 
00748     return CommunicatePacket(&packet);
00749 }
00750 
00751 uint8_t MX28::GetMaxTorque(uint8_t servoId, uint16_t *maxTorque)
00752 {
00753     MX28_PROTOCOL_PACKET packet;
00754     
00755     packet.servoId = servoId;
00756     packet.length = 4;
00757     packet.instructionErrorId = MX28_READ_DATA;
00758     packet.parameter[0] = MX28_MAX_TORQUE_L;
00759     packet.parameter[1] = 0x02;
00760  
00761     uint8_t status = CommunicatePacket(&packet);
00762         
00763     if(status == MX28_ERRBIT_NONE)
00764     {
00765         *maxTorque = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);       
00766         
00767         #ifdef MX28_DEBUG
00768             pc->printf("Get max torque: %hu\r\n", *maxTorque);
00769         #endif
00770     }
00771     
00772     return status;
00773 } 
00774 
00775 uint8_t MX28::SetMaxTorque(uint8_t servoId, uint16_t maxTorque, bool isRegWrite)
00776 {
00777     MX28_PROTOCOL_PACKET packet;
00778        
00779     packet.servoId = servoId;
00780     packet.length = 5;
00781     packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
00782     packet.parameter[0] = MX28_MAX_TORQUE_L;
00783     Utilities::ConvertUInt16ToUInt8Array(maxTorque, (uint8_t*)&(packet.parameter[1]));
00784     
00785     #ifdef MX28_DEBUG
00786         pc->printf("Set max torque: %hu\r\n", maxTorque);
00787     #endif
00788    
00789     return CommunicatePacket(&packet);  
00790 }
00791 
00792 uint8_t MX28::GetStatusReturnLevel(uint8_t servoId, uint8_t *statusReturnLevel)
00793 {
00794     MX28_PROTOCOL_PACKET packet;
00795     
00796     packet.servoId = servoId;
00797     packet.length = 4;
00798     packet.instructionErrorId = MX28_READ_DATA;
00799     packet.parameter[0] = MX28_STATUS_RETURN_LEVEL;
00800     packet.parameter[1] = 0x01;
00801  
00802     uint8_t status = CommunicatePacket(&packet);
00803         
00804     if(status == MX28_ERRBIT_NONE)
00805     {
00806         *statusReturnLevel = packet.parameter[0];        
00807         
00808         #ifdef MX28_DEBUG
00809             pc->printf("Get status return level: 0x%02X\r\n", *statusReturnLevel);
00810         #endif
00811     }
00812     
00813     return status;
00814 }
00815 
00816 uint8_t MX28::SetStatusReturnLevel(uint8_t servoId, uint8_t statusReturnLevel, bool isRegWrite)
00817 {
00818     MX28_PROTOCOL_PACKET packet;
00819 
00820     packet.servoId = servoId;
00821     packet.length = 4;
00822     packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
00823     packet.parameter[0] = MX28_STATUS_RETURN_LEVEL;
00824     packet.parameter[1] = statusReturnLevel;
00825     
00826     #ifdef MX28_DEBUG
00827         pc->printf("Set status return level: 0x%02X\r\n", statusReturnLevel);
00828     #endif
00829 
00830     return CommunicatePacket(&packet);  
00831 }
00832 
00833 uint8_t MX28::GetAlarmLED(uint8_t servoId, uint8_t *alarmLED)
00834 {
00835     MX28_PROTOCOL_PACKET packet;
00836     
00837     packet.servoId = servoId;
00838     packet.length = 4;
00839     packet.instructionErrorId = MX28_READ_DATA;
00840     packet.parameter[0] = MX28_ALARM_LED;
00841     packet.parameter[1] = 0x01;
00842  
00843     uint8_t status = CommunicatePacket(&packet);
00844         
00845     if(status == MX28_ERRBIT_NONE)
00846     {
00847         *alarmLED = packet.parameter[0];        
00848         
00849         #ifdef MX28_DEBUG
00850             pc->printf("Get alarm LED: 0x%02X\r\n", *alarmLED);
00851         #endif
00852     }
00853     
00854     return status;
00855 }
00856 
00857 uint8_t MX28::SetAlarmLED(uint8_t servoId, uint8_t alarmLED, bool isRegWrite)
00858 {
00859     MX28_PROTOCOL_PACKET packet;
00860 
00861     packet.servoId = servoId;
00862     packet.length = 4;
00863     packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
00864     packet.parameter[0] = MX28_ALARM_LED;
00865     packet.parameter[1] = alarmLED;
00866     
00867     #ifdef MX28_DEBUG
00868         pc->printf("Set alarm LED: 0x%02X\r\n", alarmLED);
00869     #endif
00870 
00871     return CommunicatePacket(&packet);  
00872 }
00873 
00874 uint8_t MX28::GetAlarmShutdown(uint8_t servoId, uint8_t *alarmShutdown)
00875 {
00876     MX28_PROTOCOL_PACKET packet;
00877     
00878     packet.servoId = servoId;
00879     packet.length = 4;
00880     packet.instructionErrorId = MX28_READ_DATA;
00881     packet.parameter[0] = MX28_ALARM_SHUTDOWN;
00882     packet.parameter[1] = 0x01;
00883  
00884     uint8_t status = CommunicatePacket(&packet);
00885         
00886     if(status == MX28_ERRBIT_NONE)
00887     {
00888         *alarmShutdown = packet.parameter[0];        
00889         
00890         #ifdef MX28_DEBUG
00891             pc->printf("Get alarm shutdown: 0x%02X\r\n", *alarmShutdown);
00892         #endif
00893     }
00894     
00895     return status;
00896 }
00897 
00898 uint8_t MX28::SetAlarmShutdown(uint8_t servoId, uint8_t alarmShutdown, bool isRegWrite)
00899 {
00900     MX28_PROTOCOL_PACKET packet;
00901 
00902     packet.servoId = servoId;
00903     packet.length = 4;
00904     packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
00905     packet.parameter[0] = MX28_ALARM_SHUTDOWN;
00906     packet.parameter[1] = alarmShutdown;
00907     
00908     #ifdef MX28_DEBUG
00909         pc->printf("Set alarm shutdown: 0x%02X\r\n", alarmShutdown);
00910     #endif
00911 
00912     return CommunicatePacket(&packet);  
00913 }
00914 
00915 uint8_t MX28::GetEnableTorque(uint8_t servoId, uint8_t *enableTorque)
00916 {
00917     MX28_PROTOCOL_PACKET packet;
00918     
00919     packet.servoId = servoId;
00920     packet.length = 4;
00921     packet.instructionErrorId = MX28_READ_DATA;
00922     packet.parameter[0] = MX28_TORQUE_ENABLE;
00923     packet.parameter[1] = 0x01;
00924  
00925     uint8_t status = CommunicatePacket(&packet);
00926         
00927     if(status == MX28_ERRBIT_NONE)
00928     {
00929         *enableTorque = packet.parameter[0];        
00930         
00931         #ifdef MX28_DEBUG
00932             pc->printf("Get enable torque: 0x%02X\r\n", *enableTorque);
00933         #endif
00934     }
00935     
00936     return status;
00937 }
00938 
00939 uint8_t MX28::SetEnableTorque(uint8_t servoId, uint8_t enableTorque, bool isRegWrite)
00940 {
00941     MX28_PROTOCOL_PACKET packet;
00942 
00943     packet.servoId = servoId;
00944     packet.length = 4;
00945     packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
00946     packet.parameter[0] = MX28_TORQUE_ENABLE;
00947     packet.parameter[1] = enableTorque;
00948     
00949     #ifdef MX28_DEBUG
00950         pc->printf("Set enable torque: 0x%02X\r\n", enableTorque);
00951     #endif
00952 
00953     return CommunicatePacket(&packet);  
00954 }
00955 
00956 uint8_t MX28::GetEnableLED(uint8_t servoId, uint8_t *enableLED)
00957 {
00958     MX28_PROTOCOL_PACKET packet;
00959     
00960     packet.servoId = servoId;
00961     packet.length = 4;
00962     packet.instructionErrorId = MX28_READ_DATA;
00963     packet.parameter[0] = MX28_LED_ENABLE;
00964     packet.parameter[1] = 0x01;
00965  
00966     uint8_t status = CommunicatePacket(&packet);
00967         
00968     if(status == MX28_ERRBIT_NONE)
00969     {
00970         *enableLED = packet.parameter[0];        
00971         
00972         #ifdef MX28_DEBUG
00973             pc->printf("Get enable LED: 0x%02X\r\n", *enableLED);
00974         #endif
00975     }
00976     
00977     return status;
00978 }
00979 
00980 uint8_t MX28::SetEnableLED(uint8_t servoId, uint8_t enableLED, bool isRegWrite)
00981 {
00982     MX28_PROTOCOL_PACKET packet;
00983 
00984     packet.servoId = servoId;
00985     packet.length = 4;
00986     packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
00987     packet.parameter[0] = MX28_LED_ENABLE;
00988     packet.parameter[1] = enableLED;
00989     
00990     #ifdef MX28_DEBUG
00991         pc->printf("Set enable LED: 0x%02X\r\n", enableLED);
00992     #endif
00993 
00994     return CommunicatePacket(&packet);  
00995 }
00996 
00997 uint8_t MX28::GetPGain(uint8_t servoId, uint8_t *pGain)
00998 {
00999     MX28_PROTOCOL_PACKET packet;
01000     
01001     packet.servoId = servoId;
01002     packet.length = 4;
01003     packet.instructionErrorId = MX28_READ_DATA;
01004     packet.parameter[0] = MX28_P_GAIN;
01005     packet.parameter[1] = 0x01;
01006  
01007     uint8_t status = CommunicatePacket(&packet);
01008         
01009     if(status == MX28_ERRBIT_NONE)
01010     {
01011         *pGain = packet.parameter[0];        
01012         
01013         #ifdef MX28_DEBUG
01014             pc->printf("Get P gain: 0x%02X\r\n", *pGain);
01015         #endif
01016     }
01017     
01018     return status;
01019 }
01020 
01021 uint8_t MX28::SetPGain(uint8_t servoId, uint8_t pGain, bool isRegWrite)
01022 {
01023     MX28_PROTOCOL_PACKET packet;
01024 
01025     packet.servoId = servoId;
01026     packet.length = 4;
01027     packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
01028     packet.parameter[0] = MX28_P_GAIN;
01029     packet.parameter[1] = pGain;
01030     
01031     #ifdef MX28_DEBUG
01032         pc->printf("Set P gain: 0x%02X\r\n", pGain);
01033     #endif
01034 
01035     return CommunicatePacket(&packet);  
01036 }
01037 
01038 uint8_t MX28::GetIGain(uint8_t servoId, uint8_t *iGain)
01039 {
01040     MX28_PROTOCOL_PACKET packet;
01041     
01042     packet.servoId = servoId;
01043     packet.length = 4;
01044     packet.instructionErrorId = MX28_READ_DATA;
01045     packet.parameter[0] = MX28_I_GAIN;
01046     packet.parameter[1] = 0x01;
01047  
01048     uint8_t status = CommunicatePacket(&packet);
01049         
01050     if(status == MX28_ERRBIT_NONE)
01051     {
01052         *iGain = packet.parameter[0];        
01053         
01054         #ifdef MX28_DEBUG
01055             pc->printf("Get I gain: 0x%02X\r\n", *iGain);
01056         #endif
01057     }
01058     
01059     return status;
01060 }
01061 
01062 uint8_t MX28::SetIGain(uint8_t servoId, uint8_t iGain, bool isRegWrite)
01063 {
01064     MX28_PROTOCOL_PACKET packet;
01065 
01066     packet.servoId = servoId;
01067     packet.length = 4;
01068     packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
01069     packet.parameter[0] = MX28_I_GAIN;
01070     packet.parameter[1] = iGain;
01071     
01072     #ifdef MX28_DEBUG
01073         pc->printf("Set I gain: 0x%02X\r\n", iGain);
01074     #endif
01075 
01076     return CommunicatePacket(&packet);  
01077 }
01078 
01079 uint8_t MX28::GetDGain(uint8_t servoId, uint8_t *dGain)
01080 {
01081     MX28_PROTOCOL_PACKET packet;
01082     
01083     packet.servoId = servoId;
01084     packet.length = 4;
01085     packet.instructionErrorId = MX28_READ_DATA;
01086     packet.parameter[0] = MX28_D_GAIN;
01087     packet.parameter[1] = 0x01;
01088  
01089     uint8_t status = CommunicatePacket(&packet);
01090         
01091     if(status == MX28_ERRBIT_NONE)
01092     {
01093         *dGain = packet.parameter[0];        
01094         
01095         #ifdef MX28_DEBUG
01096             pc->printf("Get D gain: 0x%02X\r\n", *dGain);
01097         #endif
01098     }
01099     
01100     return status;
01101 }
01102 
01103 uint8_t MX28::SetDGain(uint8_t servoId, uint8_t dGain, bool isRegWrite)
01104 {
01105     MX28_PROTOCOL_PACKET packet;
01106 
01107     packet.servoId = servoId;
01108     packet.length = 4;
01109     packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
01110     packet.parameter[0] = MX28_D_GAIN;
01111     packet.parameter[1] = dGain;
01112     
01113     #ifdef MX28_DEBUG
01114         pc->printf("Set D gain: 0x%02X\r\n", dGain);
01115     #endif
01116 
01117     return CommunicatePacket(&packet);  
01118 }
01119 
01120 uint8_t MX28::GetGoalPosition(uint8_t servoId, uint16_t *goalPosition)
01121 {
01122     MX28_PROTOCOL_PACKET packet;
01123     
01124     packet.servoId = servoId;
01125     packet.length = 4;
01126     packet.instructionErrorId = MX28_READ_DATA;
01127     packet.parameter[0] = MX28_GOAL_POSITION_L;
01128     packet.parameter[1] = 0x02;
01129  
01130     uint8_t status = CommunicatePacket(&packet);
01131         
01132     if(status == MX28_ERRBIT_NONE)
01133     {
01134         *goalPosition = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);       
01135         
01136         #ifdef MX28_DEBUG
01137             pc->printf("Get goal position: %hu\r\n", *goalPosition);
01138         #endif
01139     }
01140     
01141     return status;
01142 } 
01143 
01144 uint8_t MX28::SetGoalPosition(uint8_t servoId, uint16_t goalPosition, bool isRegWrite)
01145 {
01146     MX28_PROTOCOL_PACKET packet;
01147        
01148     packet.servoId = servoId;
01149     packet.length = 5;
01150     packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
01151     packet.parameter[0] = MX28_GOAL_POSITION_L;
01152     Utilities::ConvertUInt16ToUInt8Array(goalPosition, (uint8_t*)&(packet.parameter[1]));
01153     
01154     #ifdef MX28_DEBUG
01155         pc->printf("Set goal position: %hu\r\n", goalPosition);
01156     #endif
01157 
01158     return CommunicatePacket(&packet);  
01159 }
01160 
01161 uint8_t MX28::GetMovingSpeed(uint8_t servoId, uint16_t *movingSpeed)
01162 {
01163     MX28_PROTOCOL_PACKET packet;
01164     
01165     packet.servoId = servoId;
01166     packet.length = 4;
01167     packet.instructionErrorId = MX28_READ_DATA;
01168     packet.parameter[0] = MX28_MOVING_SPEED_L;
01169     packet.parameter[1] = 0x02;
01170  
01171     uint8_t status = CommunicatePacket(&packet);
01172         
01173     if(status == MX28_ERRBIT_NONE)
01174     {
01175         *movingSpeed = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);       
01176         
01177         #ifdef MX28_DEBUG
01178             pc->printf("Get moving speed: %hu\r\n", *movingSpeed);
01179         #endif
01180     }
01181     
01182     return status;
01183 } 
01184 
01185 uint8_t MX28::SetMovingSpeed(uint8_t servoId, uint16_t movingSpeed, bool isRegWrite)
01186 {
01187     MX28_PROTOCOL_PACKET packet;
01188        
01189     packet.servoId = servoId;
01190     packet.length = 5;
01191     packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
01192     packet.parameter[0] = MX28_MOVING_SPEED_L;
01193     Utilities::ConvertUInt16ToUInt8Array(movingSpeed, (uint8_t*)&(packet.parameter[1]));
01194     
01195     #ifdef MX28_DEBUG
01196         pc->printf("Set moving speed: %hu\r\n", movingSpeed);
01197     #endif
01198 
01199     return CommunicatePacket(&packet);  
01200 }
01201 
01202 uint8_t MX28::GetTorqueLimit(uint8_t servoId, uint16_t *torqueLimit)
01203 {
01204     MX28_PROTOCOL_PACKET packet;
01205     
01206     packet.servoId = servoId;
01207     packet.length = 4;
01208     packet.instructionErrorId = MX28_READ_DATA;
01209     packet.parameter[0] = MX28_TORQUE_LIMIT_L;
01210     packet.parameter[1] = 0x02;
01211  
01212     uint8_t status = CommunicatePacket(&packet);
01213         
01214     if(status == MX28_ERRBIT_NONE)
01215     {
01216         *torqueLimit = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);       
01217         
01218         #ifdef MX28_DEBUG
01219             pc->printf("Torque limit: %hu\r\n", *torqueLimit);
01220         #endif
01221     }
01222     
01223     return status;
01224 } 
01225 
01226 uint8_t MX28::SetTorqueLimit(uint8_t servoId, uint16_t torqueLimit, bool isRegWrite)
01227 {
01228     MX28_PROTOCOL_PACKET packet;
01229        
01230     packet.servoId = servoId;
01231     packet.length = 5;
01232     packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
01233     packet.parameter[0] = MX28_TORQUE_LIMIT_L;
01234     Utilities::ConvertUInt16ToUInt8Array(torqueLimit, (uint8_t*)&(packet.parameter[1]));
01235     
01236     #ifdef MX28_DEBUG
01237         pc->printf("Set torque limit: %hu\r\n", torqueLimit);
01238     #endif
01239 
01240     return CommunicatePacket(&packet);  
01241 }
01242 
01243 uint8_t MX28::GetPresentPosition(uint8_t servoId, uint16_t *presentPosition)
01244 {
01245     MX28_PROTOCOL_PACKET packet;
01246     
01247     packet.servoId = servoId;
01248     packet.length = 4;
01249     packet.instructionErrorId = MX28_READ_DATA;
01250     packet.parameter[0] = MX28_PRESENT_POSITION_L;
01251     packet.parameter[1] = 0x02;
01252  
01253     uint8_t status = CommunicatePacket(&packet);
01254         
01255     if(status == MX28_ERRBIT_NONE)
01256     {
01257         *presentPosition = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);       
01258         
01259         #ifdef MX28_DEBUG
01260             pc->printf("Get present position: %hu\r\n", *presentPosition);
01261         #endif
01262     }
01263     
01264     return status;
01265 } 
01266 
01267 uint8_t MX28::GetPresentSpeed(uint8_t servoId, uint16_t *presentSpeed)
01268 {
01269     MX28_PROTOCOL_PACKET packet;
01270     
01271     packet.servoId = servoId;
01272     packet.length = 4;
01273     packet.instructionErrorId = MX28_READ_DATA;
01274     packet.parameter[0] = MX28_PRESENT_SPEED_L;
01275     packet.parameter[1] = 0x02;
01276  
01277     uint8_t status = CommunicatePacket(&packet);
01278         
01279     if(status == MX28_ERRBIT_NONE)
01280     {
01281         *presentSpeed = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);       
01282         
01283         #ifdef MX28_DEBUG
01284             pc->printf("Get present speed: %hu\r\n", *presentSpeed);
01285         #endif
01286     }
01287     
01288     return status;
01289 } 
01290 
01291 uint8_t MX28::GetPresentLoad(uint8_t servoId, uint16_t *presentLoad)
01292 {
01293     MX28_PROTOCOL_PACKET packet;
01294     
01295     packet.servoId = servoId;
01296     packet.length = 4;
01297     packet.instructionErrorId = MX28_READ_DATA;
01298     packet.parameter[0] = MX28_PRESENT_LOAD_L;
01299     packet.parameter[1] = 0x02;
01300  
01301     uint8_t status = CommunicatePacket(&packet);
01302         
01303     if(status == MX28_ERRBIT_NONE)
01304     {
01305         *presentLoad = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);       
01306         
01307         #ifdef MX28_DEBUG
01308             pc->printf("Get present load: %hu\r\n", *presentLoad);
01309         #endif
01310     }
01311     
01312     return status;
01313 } 
01314 
01315 uint8_t MX28::GetPresentVoltage(uint8_t servoId, uint8_t *presentVoltage)
01316 {
01317     MX28_PROTOCOL_PACKET packet;
01318     
01319     packet.servoId = servoId;
01320     packet.length = 4;
01321     packet.instructionErrorId = MX28_READ_DATA;
01322     packet.parameter[0] = MX28_PRESENT_VOLTAGE;
01323     packet.parameter[1] = 0x01;
01324  
01325     uint8_t status = CommunicatePacket(&packet);
01326         
01327     if(status == MX28_ERRBIT_NONE)
01328     {
01329         *presentVoltage = packet.parameter[0];        
01330         
01331         #ifdef MX28_DEBUG
01332             pc->printf("Get present voltage: 0x%02X\r\n", *presentVoltage);
01333         #endif
01334     }
01335     
01336     return status;
01337 }
01338 
01339 uint8_t MX28::GetPresentTemperature(uint8_t servoId, uint8_t *presentTemperature)
01340 {
01341     MX28_PROTOCOL_PACKET packet;
01342     
01343     packet.servoId = servoId;
01344     packet.length = 4;
01345     packet.instructionErrorId = MX28_READ_DATA;
01346     packet.parameter[0] = MX28_PRESENT_TEMPERATURE;
01347     packet.parameter[1] = 0x01;
01348  
01349     uint8_t status = CommunicatePacket(&packet);
01350         
01351     if(status == MX28_ERRBIT_NONE)
01352     {
01353         *presentTemperature = packet.parameter[0];        
01354         
01355         #ifdef MX28_DEBUG
01356             pc->printf("Get present temperature: 0x%02X\r\n", *presentTemperature);
01357         #endif
01358     }
01359     
01360     return status;
01361 }
01362 
01363 uint8_t MX28::GetIsRegistered(uint8_t servoId, uint8_t *registered)
01364 {
01365     MX28_PROTOCOL_PACKET packet;
01366     
01367     packet.servoId = servoId;
01368     packet.length = 4;
01369     packet.instructionErrorId = MX28_READ_DATA;
01370     packet.parameter[0] = MX28_PRESENT_TEMPERATURE;
01371     packet.parameter[1] = 0x01;
01372  
01373     uint8_t status = CommunicatePacket(&packet);
01374         
01375     if(status == MX28_ERRBIT_NONE)
01376     {
01377         *registered = packet.parameter[0];        
01378         
01379         #ifdef MX28_DEBUG
01380             pc->printf("Get is registered: 0x%02X\r\n", *registered);
01381         #endif
01382     }
01383     
01384     return status;
01385 }
01386 
01387 uint8_t MX28::GetIsMoving(uint8_t servoId, uint8_t *moving)
01388 {
01389     MX28_PROTOCOL_PACKET packet;
01390     
01391     packet.servoId = servoId;
01392     packet.length = 4;
01393     packet.instructionErrorId = MX28_READ_DATA;
01394     packet.parameter[0] = MX28_PRESENT_TEMPERATURE;
01395     packet.parameter[1] = 0x01;
01396  
01397     uint8_t status = CommunicatePacket(&packet);
01398         
01399     if(status == MX28_ERRBIT_NONE)
01400     {
01401         *moving = packet.parameter[0];        
01402         
01403         #ifdef MX28_DEBUG
01404             pc->printf("Get is moving: 0x%02X\r\n", *moving);
01405         #endif
01406     }
01407     
01408     return status;
01409 }
01410 
01411 uint8_t MX28::GetIsLocked(uint8_t servoId, uint8_t *isLocked)
01412 {
01413     MX28_PROTOCOL_PACKET packet;
01414     
01415     packet.servoId = servoId;
01416     packet.length = 4;
01417     packet.instructionErrorId = MX28_READ_DATA;
01418     packet.parameter[0] = MX28_LOCK;
01419     packet.parameter[1] = 0x01;
01420  
01421     uint8_t status = CommunicatePacket(&packet);
01422         
01423     if(status == MX28_ERRBIT_NONE)
01424     {
01425         *isLocked = packet.parameter[0];        
01426         
01427         #ifdef MX28_DEBUG
01428             pc->printf("Get is locked: 0x%02X\r\n", *isLocked);
01429         #endif
01430     }
01431     
01432     return status;
01433 }
01434 
01435 uint8_t MX28::SetIsLocked(uint8_t servoId, uint8_t isLocked, bool isRegWrite)
01436 {
01437     MX28_PROTOCOL_PACKET packet;
01438 
01439     packet.servoId = servoId;
01440     packet.length = 4;
01441     packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
01442     packet.parameter[0] = MX28_LOCK;
01443     packet.parameter[1] = isLocked;
01444     
01445     #ifdef MX28_DEBUG
01446         pc->printf("Set is locked: 0x%02X\r\n", isLocked);
01447     #endif
01448 
01449     return CommunicatePacket(&packet);  
01450 }
01451 
01452 uint8_t MX28::GetPunch(uint8_t servoId, uint16_t *punch)
01453 {
01454     MX28_PROTOCOL_PACKET packet;
01455     
01456     packet.servoId = servoId;
01457     packet.length = 4;
01458     packet.instructionErrorId = MX28_READ_DATA;
01459     packet.parameter[0] = MX28_PUNCH_L;
01460     packet.parameter[1] = 0x02;
01461  
01462     uint8_t status = CommunicatePacket(&packet);
01463         
01464     if(status == MX28_ERRBIT_NONE)
01465     {
01466         *punch = Utilities::ConvertUInt8ArrayToUInt16(packet.parameter);       
01467         
01468         #ifdef MX28_DEBUG
01469             pc->printf("Get punch: %hu\r\n", *punch);
01470         #endif
01471     }
01472     
01473     return status;
01474 }
01475 
01476 uint8_t MX28::SetPunch(uint8_t servoId, uint16_t punch, bool isRegWrite)
01477 {
01478     MX28_PROTOCOL_PACKET packet;
01479        
01480     packet.servoId = servoId;
01481     packet.length = 5;
01482     packet.instructionErrorId = (isRegWrite == true) ? MX28_REG_WRITE : MX28_WRITE_DATA;
01483     packet.parameter[0] = MX28_PUNCH_L;
01484     Utilities::ConvertUInt16ToUInt8Array(punch, (uint8_t*)&(packet.parameter[1]));
01485     
01486     #ifdef MX28_DEBUG
01487         pc->printf("Set punch: %hu\r\n", punch);
01488     #endif
01489 
01490     return CommunicatePacket(&packet);  
01491 }
01492 
01493 uint8_t MX28::Ping(uint8_t servoId)
01494 {
01495     MX28_PROTOCOL_PACKET packet;
01496        
01497     packet.servoId = servoId;
01498     packet.length = 2;
01499     packet.instructionErrorId = MX28_PING;
01500         
01501     #ifdef MX28_DEBUG
01502         pc->printf("Ping\r\n");
01503     #endif
01504 
01505     return CommunicatePacket(&packet);  
01506 }
01507 
01508 uint8_t MX28::Reset(uint8_t servoId)
01509 {
01510     MX28_PROTOCOL_PACKET packet;
01511        
01512     packet.servoId = servoId;
01513     packet.length = 2;
01514     packet.instructionErrorId = MX28_RESET;
01515         
01516     #ifdef MX28_DEBUG
01517         pc->printf("Reset\r\n");
01518     #endif
01519 
01520     return CommunicatePacket(&packet);  
01521 }
01522 
01523 uint8_t MX28::Action(uint8_t servoId)
01524 {
01525     MX28_PROTOCOL_PACKET packet;
01526        
01527     packet.servoId = servoId;
01528     packet.length = 2;
01529     packet.instructionErrorId =  MX28_ACTION;
01530         
01531     #ifdef MX28_DEBUG
01532         pc->printf("Action\r\n");
01533     #endif
01534 
01535     return CommunicatePacket(&packet);  
01536 }
01537 
01538 MX28::MX28(PinName tx, PinName rx, int baudRate)
01539 {
01540     
01541     
01542     #ifdef MX28_DEBUG
01543         pc = new Serial(USBTX, USBRX);
01544         pc->baud(115200);
01545         pc->printf("\033[2J");
01546     #endif
01547     //pc->printf("baud1= %d\r\n",baudRate);
01548     //servoSerialHalfDuplex = new SerialHalfDuplex(tx, rx);
01549     servoSerialHalfDuplex = new iSerial(tx, rx,NULL,1000,100);
01550     //pc->printf("baud2= %d\r\n",baudRate);
01551     servoSerialHalfDuplex->baud(baudRate);    
01552     //servoSerialHalfDuplex->baud(1000000);    
01553     //servoSerialHalfDuplex->printf("a");
01554 }
01555 
01556 MX28::~MX28()
01557 {
01558     #ifdef MX28_DEBUG
01559         if(pc != NULL)           
01560             delete pc;        
01561     #endif
01562     
01563     if(servoSerialHalfDuplex != NULL)
01564         delete servoSerialHalfDuplex;
01565 }