herkulex.cpp

00001 //------------------------------------------------------------------------------
00002 /* herkulex servo library for mbed
00003  *
00004  * Copyright (c) 2012-2013 Yoonseok Pyo, MIT License
00005  *
00006  * Permission is hereby granted, free of charge, to any person obtaining a copy of this software 
00007  * and associated documentation files (the "Software"), to deal in the Software without restriction, 
00008  * including without limitation the rights to use, copy, modify, merge, publish, distribute, 
00009  * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 
00010  * furnished to do so, subject to the following conditions:
00011  *
00012  * The above copyright notice and this permission notice shall be included in all copies or 
00013  * substantial portions of the Software.
00014  *
00015  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 
00016  * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 
00017  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 
00018  * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 
00019  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
00020  */
00021 //------------------------------------------------------------------------------
00022 #include "mbed.h"
00023 #include "herkulex.h"
00024 
00025 //------------------------------------------------------------------------------
00026 Herkulex::Herkulex(PinName tx, PinName rx, uint32_t baudRate)
00027 {
00028     #ifdef HERKULEX_DEBUG
00029         pc = new Serial(USBTX, USBRX);
00030         pc->baud(57600);
00031         pc->printf("\n\nHerkulex Init!\n");
00032     #endif
00033     
00034     txd = new Serial(tx, NC);
00035     rxd = new Serial(NC, rx);
00036     
00037     txd->baud(baudRate);
00038     rxd->baud(baudRate);
00039 }
00040 
00041 //------------------------------------------------------------------------------
00042 Herkulex::~Herkulex()
00043 {
00044     #ifdef HERKULEX_DEBUG
00045         if(pc != NULL)
00046             delete pc;
00047     #endif
00048     
00049     if(txd != NULL)
00050         delete txd;
00051     if(rxd != NULL)
00052         delete rxd;
00053 }
00054 
00055 //------------------------------------------------------------------------------
00056 void Herkulex::txPacket(uint8_t packetSize, uint8_t* data)
00057 {
00058     #ifdef HERKULEX_DEBUG
00059         pc->printf("[TX]");
00060     #endif
00061     
00062     for(uint8_t i = 0; i < packetSize ; i++) 
00063     {
00064         #ifdef HERKULEX_DEBUG
00065             pc->printf("%02X ",data[i]);
00066         #endif
00067         
00068         txd->putc(data[i]);
00069     }
00070     
00071     #ifdef HERKULEX_DEBUG
00072         pc->printf("\n");
00073     #endif
00074 }
00075 
00076 //------------------------------------------------------------------------------
00077 void Herkulex::rxPacket(uint8_t packetSize, uint8_t* data)
00078 {
00079     #ifdef HERKULEX_DEBUG
00080         pc->printf("[RX]");
00081     #endif
00082        
00083     for (uint8_t i=0; i < packetSize; i++) 
00084     {   
00085         data[i] = rxd->getc();
00086         
00087         #ifdef HERKULEX_DEBUG
00088             pc->printf("%02X ",data[i]);
00089         #endif
00090     }      
00091     
00092     #ifdef HERKULEX_DEBUG
00093         pc->printf("\n");
00094     #endif
00095 }
00096 
00097 //------------------------------------------------------------------------------
00098 void Herkulex::clear(uint8_t id)
00099 {
00100     uint8_t txBuf[11];
00101     
00102     txBuf[0] = HEADER;              // Packet Header (0xFF)
00103     txBuf[1] = HEADER;              // Packet Header (0xFF)
00104     txBuf[2] = MIN_PACKET_SIZE + 4; // Packet Size
00105     txBuf[3] = id;                  // Servo ID
00106     txBuf[4] = CMD_RAM_WRITE;       // Command Ram Write (0x03)
00107     txBuf[5] = 0;                   // Checksum1
00108     txBuf[6] = 0;                   // Checksum2
00109     txBuf[7] = RAM_STATUS_ERROR;    // Address 48
00110     txBuf[8] = BYTE2;               // Length
00111     txBuf[9] = 0;                   // Clear RAM_STATUS_ERROR
00112     txBuf[10]= 0;                   // Clear RAM_STATUS_DETAIL
00113     
00114     // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
00115     // Checksum2 = (~Checksum1)&0xFE
00116     txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]) & 0xFE;
00117     txBuf[6] = (~txBuf[5])&0xFE;
00118 
00119     // send packet (mbed -> herkulex)
00120     txPacket(11, txBuf);
00121 }
00122 
00123 //------------------------------------------------------------------------------
00124 void Herkulex::setTorque(uint8_t id, uint8_t cmdTorue)
00125 {
00126     uint8_t txBuf[10];
00127     
00128     txBuf[0] = HEADER;              // Packet Header (0xFF)
00129     txBuf[1] = HEADER;              // Packet Header (0xFF)
00130     txBuf[2] = MIN_PACKET_SIZE + 3; // Packet Size
00131     txBuf[3] = id;                  // Servo ID
00132     txBuf[4] = CMD_RAM_WRITE;       // Command Ram Write (0x03)
00133     txBuf[5] = 0;                   // Checksum1
00134     txBuf[6] = 0;                   // Checksum2
00135     txBuf[7] = RAM_TORQUE_CONTROL;  // Address 52
00136     txBuf[8] = BYTE1;               // Length
00137     txBuf[9] = cmdTorue;            // Torque ON
00138 
00139     // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
00140     // Checksum2 = (~Checksum1)&0xFE
00141     txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]) & 0xFE;
00142     txBuf[6] = (~txBuf[5])&0xFE;
00143 
00144     // send packet (mbed -> herkulex)
00145     txPacket(10, txBuf);
00146 }
00147 
00148 //------------------------------------------------------------------------------
00149 void Herkulex::positionControl(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED)
00150 {
00151     if (position > 1023) return;
00152     if (playtime > 255) return;
00153     
00154     uint8_t txBuf[12];
00155     
00156     txBuf[0]  = HEADER;                 // Packet Header (0xFF)
00157     txBuf[1]  = HEADER;                 // Packet Header (0xFF)
00158     txBuf[2]  = MIN_PACKET_SIZE + 5;    // Packet Size
00159     txBuf[3]  = MAX_PID;                // pID is total number of servos in the network (0 ~ 253)
00160     txBuf[4]  = CMD_S_JOG;              // Command S JOG (0x06)
00161     txBuf[5]  = 0;                      // Checksum1
00162     txBuf[6]  = 0;                      // Checksum2
00163     txBuf[7]  = playtime;               // Playtime
00164     txBuf[8]  = position & 0x00FF;      // Position (LSB, Least Significant Bit)
00165     txBuf[9]  =(position & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
00166     txBuf[10] = POS_MODE | setLED;      // Pos Mode and LED on/off
00167     txBuf[11] = id;                     // Servo ID
00168     
00169     // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
00170     // Checksum2 = (~Checksum1)&0xFE
00171     txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]) & 0xFE;
00172     txBuf[6] = (~txBuf[5])&0xFE;
00173 
00174     // send packet (mbed -> herkulex)
00175     txPacket(12, txBuf);
00176 }
00177 
00178 //------------------------------------------------------------------------------
00179 void Herkulex::velocityControl(uint8_t id, int16_t speed, uint8_t setLED)
00180 {
00181     if (speed > 1023 || speed < -1023) return;
00182     
00183     uint8_t txBuf[12];
00184     
00185     txBuf[0]  = HEADER;                 // Packet Header (0xFF)
00186     txBuf[1]  = HEADER;                 // Packet Header (0xFF)
00187     txBuf[2]  = MIN_PACKET_SIZE + 5;    // Packet Size
00188     txBuf[3]  = MAX_PID;                // pID is total number of servos in the network (0 ~ 253)
00189     txBuf[4]  = CMD_S_JOG;              // Command S JOG (0x06)
00190     txBuf[5]  = 0;                      // Checksum1
00191     txBuf[6]  = 0;                      // Checksum2
00192     txBuf[7]  = 0;                      // Playtime, unmeaningful in turn mode
00193     txBuf[8]  = speed & 0x00FF;         // Speed (LSB, Least Significant Bit)
00194     txBuf[9]  =(speed & 0xFF00) >> 8;   // Speed (MSB, Most Significanct Bit)
00195     txBuf[10] = TURN_MODE | setLED;     // Turn Mode and LED on/off
00196     txBuf[11] = id;                     // Servo ID
00197     
00198     // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
00199     // Checksum2 = (~Checksum1)&0xFE
00200     txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]) & 0xFE;
00201     txBuf[6] = (~txBuf[5])&0xFE;
00202 
00203     // send packet (mbed -> herkulex)
00204     txPacket(12, txBuf);
00205 }
00206 
00207 //------------------------------------------------------------------------------
00208 int8_t Herkulex::getStatus(uint8_t id) 
00209 {
00210     uint8_t status;
00211     uint8_t txBuf[7];
00212     
00213     txBuf[0] = HEADER;                  // Packet Header (0xFF)
00214     txBuf[1] = HEADER;                  // Packet Header (0xFF)
00215     txBuf[2] = MIN_PACKET_SIZE;         // Packet Size
00216     txBuf[3] = id;                      // Servo ID
00217     txBuf[4] = CMD_STAT;                // Status Error, Status Detail request
00218 
00219     // Check Sum1 and Check Sum2
00220     txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]) & 0xFE;
00221     txBuf[6] = (~txBuf[5])&0xFE;
00222 
00223     // send packet (mbed -> herkulex)
00224     txPacket(7, txBuf);
00225     
00226     uint8_t rxBuf[9];
00227     rxPacket(9, rxBuf);
00228 
00229     // Checksum1
00230     uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]) & 0xFE;    
00231     if (chksum1 != rxBuf[5])
00232     {
00233         #ifdef HERKULEX_DEBUG
00234             pc->printf("Checksum1 fault\n");
00235         #endif
00236         
00237         return -1;
00238     }
00239     
00240     // Checksum2
00241     uint8_t chksum2 = (~rxBuf[5]&0xFE);
00242     if (chksum2 != rxBuf[6])
00243     {
00244         #ifdef HERKULEX_DEBUG
00245             pc->printf("Checksum2 fault\n");
00246         #endif
00247         
00248         return -1;
00249     }
00250 
00251     status = rxBuf[7];  // Status Error
00252   //status = rxBuf[8];  // Status Detail
00253     
00254     #ifdef HERKULEX_DEBUG
00255         pc->printf("Status = %02X\n", status);
00256     #endif
00257     
00258     return status;
00259 }
00260 
00261 //------------------------------------------------------------------------------
00262 int16_t Herkulex::getPos(uint8_t id) 
00263 {
00264     uint16_t position = 0;
00265     
00266     uint8_t txBuf[9];
00267     
00268     txBuf[0] = HEADER;                  // Packet Header (0xFF)
00269     txBuf[1] = HEADER;                  // Packet Header (0xFF)
00270     txBuf[2] = MIN_PACKET_SIZE + 2;     // Packet Size
00271     txBuf[3] = id;                      // Servo ID
00272     txBuf[4] = CMD_RAM_READ;            // Status Error, Status Detail request
00273     txBuf[5] = 0;                       // Checksum1
00274     txBuf[6] = 0;                       // Checksum2    
00275     txBuf[7] = RAM_CALIBRATED_POSITION; // Address 52
00276     txBuf[8] = BYTE2;                   // Address 52 and 53      
00277 
00278     // Check Sum1 and Check Sum2
00279     txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE;
00280     txBuf[6] = (~txBuf[5])&0xFE;
00281 
00282     // send packet (mbed -> herkulex)
00283     txPacket(9, txBuf);
00284     
00285     uint8_t rxBuf[13];
00286     rxPacket(13, rxBuf);
00287 
00288     // Checksum1
00289     uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]^rxBuf[12]) & 0xFE;    
00290     if (chksum1 != rxBuf[5])
00291     {
00292         #ifdef HERKULEX_DEBUG
00293             pc->printf("Checksum1 fault\n");
00294         #endif
00295         
00296         return -1;
00297     }
00298     
00299     // Checksum2
00300     uint8_t chksum2 = (~rxBuf[5]&0xFE);
00301     if (chksum2 != rxBuf[6])
00302     {
00303         #ifdef HERKULEX_DEBUG
00304             pc->printf("Checksum2 fault\n");
00305         #endif
00306         
00307         return -1;
00308     }
00309 
00310     position = ((rxBuf[10]&0x03)<<8) | rxBuf[9];
00311     
00312     #ifdef HERKULEX_DEBUG
00313         pc->printf("position = %04X(%d)\n", position, position);
00314     #endif
00315     
00316     return position;
00317 }
00318 
00319 //------------------------------------------------------------------------------
00320