File content as of revision 0:0eef242852bb:

//------------------------------------------------------------------------------
/* herkulex servo library for mbed
 *
 * Copyright (c) 2012-2013, Yoonseok Pyo
 * All rights reserved.
 *
 * New BSD License
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the <organization> nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
//------------------------------------------------------------------------------
#include "mbed.h"
#include "herkulex.h"

//------------------------------------------------------------------------------
Herkulex::Herkulex(PinName tx, PinName rx, uint32_t baudRate)
{
    #ifdef HERKULEX_DEBUG
        pc = new Serial(USBTX, USBRX);
        pc->baud(57600);
        pc->printf("Herkulex Init!\n");
    #endif
    
    txd = new Serial(tx, NC);
    rxd = new Serial(NC, rx);
    
    txd->baud(baudRate);
    rxd->baud(baudRate);
}

//------------------------------------------------------------------------------
Herkulex::~Herkulex()
{
    #ifdef HERKULEX_DEBUG
        if(pc != NULL)
            delete pc;
    #endif
    
    if(txd != NULL)
        delete txd;
    if(rxd != NULL)
        delete rxd;
}

//------------------------------------------------------------------------------
void Herkulex::txPacket(uint8_t packetSize, uint8_t* data)
{
    #ifdef HERKULEX_DEBUG
        pc->printf("[TX]");
    #endif
    
    for(uint8_t i = 0; i < packetSize ; i++) 
    {
        #ifdef HERKULEX_DEBUG
            pc->printf("%02X ",data[i]);
        #endif
        
        txd->putc(data[i]);
    }
    
    #ifdef HERKULEX_DEBUG
        pc->printf("\n");
    #endif
}

//------------------------------------------------------------------------------
void Herkulex::setTorque(uint8_t id, uint8_t cmdTorue)
{
    uint8_t txBuf[10];
    
    txBuf[0] = HEADER;              // Packet Header (0xFF)
    txBuf[1] = HEADER;              // Packet Header (0xFF)
    txBuf[2] = MIN_PACKET_SIZE + 3; // Packet Size
    txBuf[3] = id;                  // Servo ID
    txBuf[4] = CMD_RAM_WRITE;       // Command Ram Write (0x03)
    txBuf[5] = 0;                   // Checksum1
    txBuf[6] = 0;                   // Checksum2
    txBuf[7] = RAM_TORQUE_CONTROL;  // Address 52
    txBuf[8] = BYTE1;               // Length
    txBuf[9] = cmdTorue;           // Torque ON

    // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
    // Checksum2 = (~Checksum1)&0xFE
    txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]) & 0xFE;
    txBuf[6] = (~txBuf[5])&0xFE;

    // send packet (mbed -> herkulex)
    txPacket(10, txBuf);
}

//------------------------------------------------------------------------------
void Herkulex::movePos(uint8_t id, uint16_t pos, uint8_t playtime, uint8_t setMode, uint8_t setLED)
{
    if (pos > 1023) return;
    if (playtime > 255) return;
    
    uint8_t txBuf[12];
    
    txBuf[0]  = HEADER;                 // Packet Header (0xFF)
    txBuf[1]  = HEADER;                 // Packet Header (0xFF)
    txBuf[2]  = MIN_PACKET_SIZE + 5;    // Packet Size
    txBuf[3]  = MAX_PID;                // pID is total number of servos in the network (0 ~ 253)
    txBuf[4]  = CMD_S_JOG;              // Command S JOG (0x06)
    txBuf[5]  = 0;                      // Checksum1
    txBuf[6]  = 0;                      // Checksum2
    txBuf[7]  = playtime;               // Playtime
    txBuf[8]  = pos & 0x00FF;           // JOG(LSB)
    txBuf[9]  =(pos & 0xFF00) >> 8;     // JOG(MSB)
    txBuf[10] = setMode | setLED;       // Set mode and LED on/off
    txBuf[11] = id;                     // Servo ID
    
    // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
    // Checksum2 = (~Checksum1)&0xFE
    txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]) & 0xFE;
    txBuf[6] = (~txBuf[5])&0xFE;

    // send packet (mbed -> herkulex)
    txPacket(12, txBuf);
}

//------------------------------------------------------------------------------