herkulex servo control library (fixes velocityControl and positionControl)
Fork of herkulex by
Diff: herkulex.cpp
- Revision:
- 0:0eef242852bb
- Child:
- 1:874d1f42989c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/herkulex.cpp Mon Jan 14 06:06:30 2013 +0000 @@ -0,0 +1,142 @@ +//------------------------------------------------------------------------------ +/* 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); +} + +//------------------------------------------------------------------------------ +