Yoonseok Pyo / herkulex

herkulex servo control library

Dependents:   HerkuleX-HelloWorld

This herkulex library is based on DongBu Robot documentation and protocol.

http://dasarobot.com/guide/herkulexeng.pdf

/media/uploads/passionvirus/mbedandherkulex_i.png /media/uploads/passionvirus/range.png

herkulex.cpp@0:0eef242852bb, 2013-01-14 (annotated)

Committer:
passionvirus
Date:
Mon Jan 14 06:06:30 2013 +0000
Revision:
0:0eef242852bb
Child:
1:874d1f42989c
new lib for herkulex servo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
passionvirus 0:0eef242852bb 1 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 2 /* herkulex servo library for mbed
passionvirus 0:0eef242852bb 3 *
passionvirus 0:0eef242852bb 4 * Copyright (c) 2012-2013, Yoonseok Pyo
passionvirus 0:0eef242852bb 5 * All rights reserved.
passionvirus 0:0eef242852bb 6 *
passionvirus 0:0eef242852bb 7 * New BSD License
passionvirus 0:0eef242852bb 8 *
passionvirus 0:0eef242852bb 9 * Redistribution and use in source and binary forms, with or without
passionvirus 0:0eef242852bb 10 * modification, are permitted provided that the following conditions are met:
passionvirus 0:0eef242852bb 11 * * Redistributions of source code must retain the above copyright
passionvirus 0:0eef242852bb 12 * notice, this list of conditions and the following disclaimer.
passionvirus 0:0eef242852bb 13 * * Redistributions in binary form must reproduce the above copyright
passionvirus 0:0eef242852bb 14 * notice, this list of conditions and the following disclaimer in the
passionvirus 0:0eef242852bb 15 * documentation and/or other materials provided with the distribution.
passionvirus 0:0eef242852bb 16 * * Neither the name of the <organization> nor the
passionvirus 0:0eef242852bb 17 * names of its contributors may be used to endorse or promote products
passionvirus 0:0eef242852bb 18 * derived from this software without specific prior written permission.
passionvirus 0:0eef242852bb 19 *
passionvirus 0:0eef242852bb 20 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
passionvirus 0:0eef242852bb 21 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
passionvirus 0:0eef242852bb 22 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
passionvirus 0:0eef242852bb 23 * DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
passionvirus 0:0eef242852bb 24 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
passionvirus 0:0eef242852bb 25 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
passionvirus 0:0eef242852bb 26 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
passionvirus 0:0eef242852bb 27 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
passionvirus 0:0eef242852bb 28 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
passionvirus 0:0eef242852bb 29 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
passionvirus 0:0eef242852bb 30 */
passionvirus 0:0eef242852bb 31 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 32 #include "mbed.h"
passionvirus 0:0eef242852bb 33 #include "herkulex.h"
passionvirus 0:0eef242852bb 34
passionvirus 0:0eef242852bb 35 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 36 Herkulex::Herkulex(PinName tx, PinName rx, uint32_t baudRate)
passionvirus 0:0eef242852bb 37 {
passionvirus 0:0eef242852bb 38 #ifdef HERKULEX_DEBUG
passionvirus 0:0eef242852bb 39 pc = new Serial(USBTX, USBRX);
passionvirus 0:0eef242852bb 40 pc->baud(57600);
passionvirus 0:0eef242852bb 41 pc->printf("Herkulex Init!\n");
passionvirus 0:0eef242852bb 42 #endif
passionvirus 0:0eef242852bb 43
passionvirus 0:0eef242852bb 44 txd = new Serial(tx, NC);
passionvirus 0:0eef242852bb 45 rxd = new Serial(NC, rx);
passionvirus 0:0eef242852bb 46
passionvirus 0:0eef242852bb 47 txd->baud(baudRate);
passionvirus 0:0eef242852bb 48 rxd->baud(baudRate);
passionvirus 0:0eef242852bb 49 }
passionvirus 0:0eef242852bb 50
passionvirus 0:0eef242852bb 51 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 52 Herkulex::~Herkulex()
passionvirus 0:0eef242852bb 53 {
passionvirus 0:0eef242852bb 54 #ifdef HERKULEX_DEBUG
passionvirus 0:0eef242852bb 55 if(pc != NULL)
passionvirus 0:0eef242852bb 56 delete pc;
passionvirus 0:0eef242852bb 57 #endif
passionvirus 0:0eef242852bb 58
passionvirus 0:0eef242852bb 59 if(txd != NULL)
passionvirus 0:0eef242852bb 60 delete txd;
passionvirus 0:0eef242852bb 61 if(rxd != NULL)
passionvirus 0:0eef242852bb 62 delete rxd;
passionvirus 0:0eef242852bb 63 }
passionvirus 0:0eef242852bb 64
passionvirus 0:0eef242852bb 65 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 66 void Herkulex::txPacket(uint8_t packetSize, uint8_t* data)
passionvirus 0:0eef242852bb 67 {
passionvirus 0:0eef242852bb 68 #ifdef HERKULEX_DEBUG
passionvirus 0:0eef242852bb 69 pc->printf("[TX]");
passionvirus 0:0eef242852bb 70 #endif
passionvirus 0:0eef242852bb 71
passionvirus 0:0eef242852bb 72 for(uint8_t i = 0; i < packetSize ; i++)
passionvirus 0:0eef242852bb 73 {
passionvirus 0:0eef242852bb 74 #ifdef HERKULEX_DEBUG
passionvirus 0:0eef242852bb 75 pc->printf("%02X ",data[i]);
passionvirus 0:0eef242852bb 76 #endif
passionvirus 0:0eef242852bb 77
passionvirus 0:0eef242852bb 78 txd->putc(data[i]);
passionvirus 0:0eef242852bb 79 }
passionvirus 0:0eef242852bb 80
passionvirus 0:0eef242852bb 81 #ifdef HERKULEX_DEBUG
passionvirus 0:0eef242852bb 82 pc->printf("\n");
passionvirus 0:0eef242852bb 83 #endif
passionvirus 0:0eef242852bb 84 }
passionvirus 0:0eef242852bb 85
passionvirus 0:0eef242852bb 86 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 87 void Herkulex::setTorque(uint8_t id, uint8_t cmdTorue)
passionvirus 0:0eef242852bb 88 {
passionvirus 0:0eef242852bb 89 uint8_t txBuf[10];
passionvirus 0:0eef242852bb 90
passionvirus 0:0eef242852bb 91 txBuf[0] = HEADER; // Packet Header (0xFF)
passionvirus 0:0eef242852bb 92 txBuf[1] = HEADER; // Packet Header (0xFF)
passionvirus 0:0eef242852bb 93 txBuf[2] = MIN_PACKET_SIZE + 3; // Packet Size
passionvirus 0:0eef242852bb 94 txBuf[3] = id; // Servo ID
passionvirus 0:0eef242852bb 95 txBuf[4] = CMD_RAM_WRITE; // Command Ram Write (0x03)
passionvirus 0:0eef242852bb 96 txBuf[5] = 0; // Checksum1
passionvirus 0:0eef242852bb 97 txBuf[6] = 0; // Checksum2
passionvirus 0:0eef242852bb 98 txBuf[7] = RAM_TORQUE_CONTROL; // Address 52
passionvirus 0:0eef242852bb 99 txBuf[8] = BYTE1; // Length
passionvirus 0:0eef242852bb 100 txBuf[9] = cmdTorue; // Torque ON
passionvirus 0:0eef242852bb 101
passionvirus 0:0eef242852bb 102 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
passionvirus 0:0eef242852bb 103 // Checksum2 = (~Checksum1)&0xFE
passionvirus 0:0eef242852bb 104 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]) & 0xFE;
passionvirus 0:0eef242852bb 105 txBuf[6] = (~txBuf[5])&0xFE;
passionvirus 0:0eef242852bb 106
passionvirus 0:0eef242852bb 107 // send packet (mbed -> herkulex)
passionvirus 0:0eef242852bb 108 txPacket(10, txBuf);
passionvirus 0:0eef242852bb 109 }
passionvirus 0:0eef242852bb 110
passionvirus 0:0eef242852bb 111 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 112 void Herkulex::movePos(uint8_t id, uint16_t pos, uint8_t playtime, uint8_t setMode, uint8_t setLED)
passionvirus 0:0eef242852bb 113 {
passionvirus 0:0eef242852bb 114 if (pos > 1023) return;
passionvirus 0:0eef242852bb 115 if (playtime > 255) return;
passionvirus 0:0eef242852bb 116
passionvirus 0:0eef242852bb 117 uint8_t txBuf[12];
passionvirus 0:0eef242852bb 118
passionvirus 0:0eef242852bb 119 txBuf[0] = HEADER; // Packet Header (0xFF)
passionvirus 0:0eef242852bb 120 txBuf[1] = HEADER; // Packet Header (0xFF)
passionvirus 0:0eef242852bb 121 txBuf[2] = MIN_PACKET_SIZE + 5; // Packet Size
passionvirus 0:0eef242852bb 122 txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
passionvirus 0:0eef242852bb 123 txBuf[4] = CMD_S_JOG; // Command S JOG (0x06)
passionvirus 0:0eef242852bb 124 txBuf[5] = 0; // Checksum1
passionvirus 0:0eef242852bb 125 txBuf[6] = 0; // Checksum2
passionvirus 0:0eef242852bb 126 txBuf[7] = playtime; // Playtime
passionvirus 0:0eef242852bb 127 txBuf[8] = pos & 0x00FF; // JOG(LSB)
passionvirus 0:0eef242852bb 128 txBuf[9] =(pos & 0xFF00) >> 8; // JOG(MSB)
passionvirus 0:0eef242852bb 129 txBuf[10] = setMode | setLED; // Set mode and LED on/off
passionvirus 0:0eef242852bb 130 txBuf[11] = id; // Servo ID
passionvirus 0:0eef242852bb 131
passionvirus 0:0eef242852bb 132 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
passionvirus 0:0eef242852bb 133 // Checksum2 = (~Checksum1)&0xFE
passionvirus 0:0eef242852bb 134 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]) & 0xFE;
passionvirus 0:0eef242852bb 135 txBuf[6] = (~txBuf[5])&0xFE;
passionvirus 0:0eef242852bb 136
passionvirus 0:0eef242852bb 137 // send packet (mbed -> herkulex)
passionvirus 0:0eef242852bb 138 txPacket(12, txBuf);
passionvirus 0:0eef242852bb 139 }
passionvirus 0:0eef242852bb 140
passionvirus 0:0eef242852bb 141 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 142