herkulex servo control library (fixes velocityControl and positionControl)
Fork of herkulex by
herkulex.cpp
- Committer:
- passionvirus
- Date:
- 2013-01-14
- Revision:
- 5:f737e5c70115
- Parent:
- 1:874d1f42989c
- Child:
- 6:1dacff31b77a
File content as of revision 5:f737e5c70115:
//------------------------------------------------------------------------------ /* herkulex servo library for mbed * * Copyright (c) 2012-2013 Yoonseok Pyo, MIT License * * Permission is hereby granted, free of charge, to any person obtaining a copy of this software * and associated documentation files (the "Software"), to deal in the Software without restriction, * including without limitation the rights to use, copy, modify, merge, publish, distribute, * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: * * The above copyright notice and this permission notice shall be included in all copies or * substantial portions of the Software. * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ //------------------------------------------------------------------------------ #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::clear(uint8_t id) { uint8_t txBuf[11]; txBuf[0] = HEADER; // Packet Header (0xFF) txBuf[1] = HEADER; // Packet Header (0xFF) txBuf[2] = MIN_PACKET_SIZE + 4; // 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_STATUS_ERROR; // Address 48 txBuf[8] = BYTE2; // Length txBuf[9] = 0; // Clear RAM_STATUS_ERROR txBuf[10]= 0; // Clear RAM_STATUS_DETAIL // 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]) & 0xFE; txBuf[6] = (~txBuf[5])&0xFE; // send packet (mbed -> herkulex) txPacket(11, txBuf); } //------------------------------------------------------------------------------ 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 position, uint8_t playtime, uint8_t setLED) { if (position > 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] = position & 0x00FF; // Position (LSB, Least Significant Bit) txBuf[9] =(position & 0xFF00) >> 8;// position (MSB, Most Significanct Bit) txBuf[10] = POS_MODE | setLED; // Pos 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); } //------------------------------------------------------------------------------ void Herkulex::turn(uint8_t id, int16_t speed, uint8_t setLED) { if (speed > 1023 || speed < -1023) 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] = 0; // Playtime, unmeaningful in turn mode txBuf[8] = speed & 0x00FF; // Speed (LSB, Least Significant Bit) txBuf[9] =(speed & 0xFF00) >> 8; // Speed (MSB, Most Significanct Bit) txBuf[10] = TURN_MODE | setLED; // Turn 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); } //------------------------------------------------------------------------------