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@5:f737e5c70115, 2013-01-14 (annotated)

Committer:
passionvirus
Date:
Mon Jan 14 16:24:05 2013 +0000
Revision:
5:f737e5c70115
Parent:
1:874d1f42989c
Child:
6:1dacff31b77a
Chage example code

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 1:874d1f42989c 4 * Copyright (c) 2012-2013 Yoonseok Pyo, MIT License
passionvirus 0:0eef242852bb 5 *
passionvirus 1:874d1f42989c 6 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
passionvirus 1:874d1f42989c 7 * and associated documentation files (the "Software"), to deal in the Software without restriction,
passionvirus 1:874d1f42989c 8 * including without limitation the rights to use, copy, modify, merge, publish, distribute,
passionvirus 1:874d1f42989c 9 * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
passionvirus 1:874d1f42989c 10 * furnished to do so, subject to the following conditions:
passionvirus 0:0eef242852bb 11 *
passionvirus 1:874d1f42989c 12 * The above copyright notice and this permission notice shall be included in all copies or
passionvirus 1:874d1f42989c 13 * substantial portions of the Software.
passionvirus 0:0eef242852bb 14 *
passionvirus 1:874d1f42989c 15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
passionvirus 1:874d1f42989c 16 * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
passionvirus 1:874d1f42989c 17 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
passionvirus 1:874d1f42989c 18 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
passionvirus 1:874d1f42989c 19 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
passionvirus 0:0eef242852bb 20 */
passionvirus 0:0eef242852bb 21 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 22 #include "mbed.h"
passionvirus 0:0eef242852bb 23 #include "herkulex.h"
passionvirus 0:0eef242852bb 24
passionvirus 0:0eef242852bb 25 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 26 Herkulex::Herkulex(PinName tx, PinName rx, uint32_t baudRate)
passionvirus 0:0eef242852bb 27 {
passionvirus 0:0eef242852bb 28 #ifdef HERKULEX_DEBUG
passionvirus 0:0eef242852bb 29 pc = new Serial(USBTX, USBRX);
passionvirus 0:0eef242852bb 30 pc->baud(57600);
passionvirus 0:0eef242852bb 31 pc->printf("Herkulex Init!\n");
passionvirus 0:0eef242852bb 32 #endif
passionvirus 0:0eef242852bb 33
passionvirus 0:0eef242852bb 34 txd = new Serial(tx, NC);
passionvirus 0:0eef242852bb 35 rxd = new Serial(NC, rx);
passionvirus 0:0eef242852bb 36
passionvirus 0:0eef242852bb 37 txd->baud(baudRate);
passionvirus 0:0eef242852bb 38 rxd->baud(baudRate);
passionvirus 0:0eef242852bb 39 }
passionvirus 0:0eef242852bb 40
passionvirus 0:0eef242852bb 41 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 42 Herkulex::~Herkulex()
passionvirus 0:0eef242852bb 43 {
passionvirus 0:0eef242852bb 44 #ifdef HERKULEX_DEBUG
passionvirus 0:0eef242852bb 45 if(pc != NULL)
passionvirus 0:0eef242852bb 46 delete pc;
passionvirus 0:0eef242852bb 47 #endif
passionvirus 0:0eef242852bb 48
passionvirus 0:0eef242852bb 49 if(txd != NULL)
passionvirus 0:0eef242852bb 50 delete txd;
passionvirus 0:0eef242852bb 51 if(rxd != NULL)
passionvirus 0:0eef242852bb 52 delete rxd;
passionvirus 0:0eef242852bb 53 }
passionvirus 0:0eef242852bb 54
passionvirus 0:0eef242852bb 55 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 56 void Herkulex::txPacket(uint8_t packetSize, uint8_t* data)
passionvirus 0:0eef242852bb 57 {
passionvirus 0:0eef242852bb 58 #ifdef HERKULEX_DEBUG
passionvirus 0:0eef242852bb 59 pc->printf("[TX]");
passionvirus 0:0eef242852bb 60 #endif
passionvirus 0:0eef242852bb 61
passionvirus 0:0eef242852bb 62 for(uint8_t i = 0; i < packetSize ; i++)
passionvirus 0:0eef242852bb 63 {
passionvirus 0:0eef242852bb 64 #ifdef HERKULEX_DEBUG
passionvirus 0:0eef242852bb 65 pc->printf("%02X ",data[i]);
passionvirus 0:0eef242852bb 66 #endif
passionvirus 0:0eef242852bb 67
passionvirus 0:0eef242852bb 68 txd->putc(data[i]);
passionvirus 0:0eef242852bb 69 }
passionvirus 0:0eef242852bb 70
passionvirus 0:0eef242852bb 71 #ifdef HERKULEX_DEBUG
passionvirus 0:0eef242852bb 72 pc->printf("\n");
passionvirus 0:0eef242852bb 73 #endif
passionvirus 0:0eef242852bb 74 }
passionvirus 0:0eef242852bb 75
passionvirus 0:0eef242852bb 76 //------------------------------------------------------------------------------
passionvirus 5:f737e5c70115 77 void Herkulex::clear(uint8_t id)
passionvirus 5:f737e5c70115 78 {
passionvirus 5:f737e5c70115 79 uint8_t txBuf[11];
passionvirus 5:f737e5c70115 80
passionvirus 5:f737e5c70115 81 txBuf[0] = HEADER; // Packet Header (0xFF)
passionvirus 5:f737e5c70115 82 txBuf[1] = HEADER; // Packet Header (0xFF)
passionvirus 5:f737e5c70115 83 txBuf[2] = MIN_PACKET_SIZE + 4; // Packet Size
passionvirus 5:f737e5c70115 84 txBuf[3] = id; // Servo ID
passionvirus 5:f737e5c70115 85 txBuf[4] = CMD_RAM_WRITE; // Command Ram Write (0x03)
passionvirus 5:f737e5c70115 86 txBuf[5] = 0; // Checksum1
passionvirus 5:f737e5c70115 87 txBuf[6] = 0; // Checksum2
passionvirus 5:f737e5c70115 88 txBuf[7] = RAM_STATUS_ERROR; // Address 48
passionvirus 5:f737e5c70115 89 txBuf[8] = BYTE2; // Length
passionvirus 5:f737e5c70115 90 txBuf[9] = 0; // Clear RAM_STATUS_ERROR
passionvirus 5:f737e5c70115 91 txBuf[10]= 0; // Clear RAM_STATUS_DETAIL
passionvirus 5:f737e5c70115 92
passionvirus 5:f737e5c70115 93 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
passionvirus 5:f737e5c70115 94 // Checksum2 = (~Checksum1)&0xFE
passionvirus 5:f737e5c70115 95 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]) & 0xFE;
passionvirus 5:f737e5c70115 96 txBuf[6] = (~txBuf[5])&0xFE;
passionvirus 5:f737e5c70115 97
passionvirus 5:f737e5c70115 98 // send packet (mbed -> herkulex)
passionvirus 5:f737e5c70115 99 txPacket(11, txBuf);
passionvirus 5:f737e5c70115 100 }
passionvirus 5:f737e5c70115 101
passionvirus 5:f737e5c70115 102 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 103 void Herkulex::setTorque(uint8_t id, uint8_t cmdTorue)
passionvirus 0:0eef242852bb 104 {
passionvirus 0:0eef242852bb 105 uint8_t txBuf[10];
passionvirus 0:0eef242852bb 106
passionvirus 0:0eef242852bb 107 txBuf[0] = HEADER; // Packet Header (0xFF)
passionvirus 0:0eef242852bb 108 txBuf[1] = HEADER; // Packet Header (0xFF)
passionvirus 0:0eef242852bb 109 txBuf[2] = MIN_PACKET_SIZE + 3; // Packet Size
passionvirus 0:0eef242852bb 110 txBuf[3] = id; // Servo ID
passionvirus 0:0eef242852bb 111 txBuf[4] = CMD_RAM_WRITE; // Command Ram Write (0x03)
passionvirus 0:0eef242852bb 112 txBuf[5] = 0; // Checksum1
passionvirus 0:0eef242852bb 113 txBuf[6] = 0; // Checksum2
passionvirus 0:0eef242852bb 114 txBuf[7] = RAM_TORQUE_CONTROL; // Address 52
passionvirus 0:0eef242852bb 115 txBuf[8] = BYTE1; // Length
passionvirus 0:0eef242852bb 116 txBuf[9] = cmdTorue; // Torque ON
passionvirus 0:0eef242852bb 117
passionvirus 0:0eef242852bb 118 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
passionvirus 0:0eef242852bb 119 // Checksum2 = (~Checksum1)&0xFE
passionvirus 0:0eef242852bb 120 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]) & 0xFE;
passionvirus 0:0eef242852bb 121 txBuf[6] = (~txBuf[5])&0xFE;
passionvirus 0:0eef242852bb 122
passionvirus 0:0eef242852bb 123 // send packet (mbed -> herkulex)
passionvirus 0:0eef242852bb 124 txPacket(10, txBuf);
passionvirus 0:0eef242852bb 125 }
passionvirus 0:0eef242852bb 126
passionvirus 0:0eef242852bb 127 //------------------------------------------------------------------------------
passionvirus 5:f737e5c70115 128 void Herkulex::movePos(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED)
passionvirus 0:0eef242852bb 129 {
passionvirus 5:f737e5c70115 130 if (position > 1023) return;
passionvirus 0:0eef242852bb 131 if (playtime > 255) return;
passionvirus 0:0eef242852bb 132
passionvirus 0:0eef242852bb 133 uint8_t txBuf[12];
passionvirus 0:0eef242852bb 134
passionvirus 0:0eef242852bb 135 txBuf[0] = HEADER; // Packet Header (0xFF)
passionvirus 0:0eef242852bb 136 txBuf[1] = HEADER; // Packet Header (0xFF)
passionvirus 0:0eef242852bb 137 txBuf[2] = MIN_PACKET_SIZE + 5; // Packet Size
passionvirus 0:0eef242852bb 138 txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
passionvirus 0:0eef242852bb 139 txBuf[4] = CMD_S_JOG; // Command S JOG (0x06)
passionvirus 0:0eef242852bb 140 txBuf[5] = 0; // Checksum1
passionvirus 0:0eef242852bb 141 txBuf[6] = 0; // Checksum2
passionvirus 0:0eef242852bb 142 txBuf[7] = playtime; // Playtime
passionvirus 5:f737e5c70115 143 txBuf[8] = position & 0x00FF; // Position (LSB, Least Significant Bit)
passionvirus 5:f737e5c70115 144 txBuf[9] =(position & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
passionvirus 5:f737e5c70115 145 txBuf[10] = POS_MODE | setLED; // Pos Mode and LED on/off
passionvirus 5:f737e5c70115 146 txBuf[11] = id; // Servo ID
passionvirus 5:f737e5c70115 147
passionvirus 5:f737e5c70115 148 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
passionvirus 5:f737e5c70115 149 // Checksum2 = (~Checksum1)&0xFE
passionvirus 5:f737e5c70115 150 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]) & 0xFE;
passionvirus 5:f737e5c70115 151 txBuf[6] = (~txBuf[5])&0xFE;
passionvirus 5:f737e5c70115 152
passionvirus 5:f737e5c70115 153 // send packet (mbed -> herkulex)
passionvirus 5:f737e5c70115 154 txPacket(12, txBuf);
passionvirus 5:f737e5c70115 155 }
passionvirus 5:f737e5c70115 156
passionvirus 5:f737e5c70115 157 //------------------------------------------------------------------------------
passionvirus 5:f737e5c70115 158 void Herkulex::turn(uint8_t id, int16_t speed, uint8_t setLED)
passionvirus 5:f737e5c70115 159 {
passionvirus 5:f737e5c70115 160 if (speed > 1023 || speed < -1023) return;
passionvirus 5:f737e5c70115 161
passionvirus 5:f737e5c70115 162 uint8_t txBuf[12];
passionvirus 5:f737e5c70115 163
passionvirus 5:f737e5c70115 164 txBuf[0] = HEADER; // Packet Header (0xFF)
passionvirus 5:f737e5c70115 165 txBuf[1] = HEADER; // Packet Header (0xFF)
passionvirus 5:f737e5c70115 166 txBuf[2] = MIN_PACKET_SIZE + 5; // Packet Size
passionvirus 5:f737e5c70115 167 txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
passionvirus 5:f737e5c70115 168 txBuf[4] = CMD_S_JOG; // Command S JOG (0x06)
passionvirus 5:f737e5c70115 169 txBuf[5] = 0; // Checksum1
passionvirus 5:f737e5c70115 170 txBuf[6] = 0; // Checksum2
passionvirus 5:f737e5c70115 171 txBuf[7] = 0; // Playtime, unmeaningful in turn mode
passionvirus 5:f737e5c70115 172 txBuf[8] = speed & 0x00FF; // Speed (LSB, Least Significant Bit)
passionvirus 5:f737e5c70115 173 txBuf[9] =(speed & 0xFF00) >> 8; // Speed (MSB, Most Significanct Bit)
passionvirus 5:f737e5c70115 174 txBuf[10] = TURN_MODE | setLED; // Turn Mode and LED on/off
passionvirus 0:0eef242852bb 175 txBuf[11] = id; // Servo ID
passionvirus 0:0eef242852bb 176
passionvirus 0:0eef242852bb 177 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
passionvirus 0:0eef242852bb 178 // Checksum2 = (~Checksum1)&0xFE
passionvirus 0:0eef242852bb 179 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]) & 0xFE;
passionvirus 0:0eef242852bb 180 txBuf[6] = (~txBuf[5])&0xFE;
passionvirus 0:0eef242852bb 181
passionvirus 0:0eef242852bb 182 // send packet (mbed -> herkulex)
passionvirus 0:0eef242852bb 183 txPacket(12, txBuf);
passionvirus 0:0eef242852bb 184 }
passionvirus 0:0eef242852bb 185
passionvirus 0:0eef242852bb 186 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 187
passionvirus 5:f737e5c70115 188