herkulex servo control library (fixes velocityControl and positionControl)
Fork of herkulex by
herkulex.cpp
- Committer:
- passionvirus
- Date:
- 2013-01-14
- Revision:
- 0:0eef242852bb
- Child:
- 1:874d1f42989c
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); } //------------------------------------------------------------------------------