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@1:874d1f42989c, 2013-01-14 (annotated)

Committer:
passionvirus
Date:
Mon Jan 14 06:46:31 2013 +0000
Revision:
1:874d1f42989c
Parent:
0:0eef242852bb
Child:
5:f737e5c70115
change licence

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 0:0eef242852bb 77 void Herkulex::setTorque(uint8_t id, uint8_t cmdTorue)
passionvirus 0:0eef242852bb 78 {
passionvirus 0:0eef242852bb 79 uint8_t txBuf[10];
passionvirus 0:0eef242852bb 80
passionvirus 0:0eef242852bb 81 txBuf[0] = HEADER; // Packet Header (0xFF)
passionvirus 0:0eef242852bb 82 txBuf[1] = HEADER; // Packet Header (0xFF)
passionvirus 0:0eef242852bb 83 txBuf[2] = MIN_PACKET_SIZE + 3; // Packet Size
passionvirus 0:0eef242852bb 84 txBuf[3] = id; // Servo ID
passionvirus 0:0eef242852bb 85 txBuf[4] = CMD_RAM_WRITE; // Command Ram Write (0x03)
passionvirus 0:0eef242852bb 86 txBuf[5] = 0; // Checksum1
passionvirus 0:0eef242852bb 87 txBuf[6] = 0; // Checksum2
passionvirus 0:0eef242852bb 88 txBuf[7] = RAM_TORQUE_CONTROL; // Address 52
passionvirus 0:0eef242852bb 89 txBuf[8] = BYTE1; // Length
passionvirus 0:0eef242852bb 90 txBuf[9] = cmdTorue; // Torque ON
passionvirus 0:0eef242852bb 91
passionvirus 0:0eef242852bb 92 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
passionvirus 0:0eef242852bb 93 // Checksum2 = (~Checksum1)&0xFE
passionvirus 0:0eef242852bb 94 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]) & 0xFE;
passionvirus 0:0eef242852bb 95 txBuf[6] = (~txBuf[5])&0xFE;
passionvirus 0:0eef242852bb 96
passionvirus 0:0eef242852bb 97 // send packet (mbed -> herkulex)
passionvirus 0:0eef242852bb 98 txPacket(10, txBuf);
passionvirus 0:0eef242852bb 99 }
passionvirus 0:0eef242852bb 100
passionvirus 0:0eef242852bb 101 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 102 void Herkulex::movePos(uint8_t id, uint16_t pos, uint8_t playtime, uint8_t setMode, uint8_t setLED)
passionvirus 0:0eef242852bb 103 {
passionvirus 0:0eef242852bb 104 if (pos > 1023) return;
passionvirus 0:0eef242852bb 105 if (playtime > 255) return;
passionvirus 0:0eef242852bb 106
passionvirus 0:0eef242852bb 107 uint8_t txBuf[12];
passionvirus 0:0eef242852bb 108
passionvirus 0:0eef242852bb 109 txBuf[0] = HEADER; // Packet Header (0xFF)
passionvirus 0:0eef242852bb 110 txBuf[1] = HEADER; // Packet Header (0xFF)
passionvirus 0:0eef242852bb 111 txBuf[2] = MIN_PACKET_SIZE + 5; // Packet Size
passionvirus 0:0eef242852bb 112 txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
passionvirus 0:0eef242852bb 113 txBuf[4] = CMD_S_JOG; // Command S JOG (0x06)
passionvirus 0:0eef242852bb 114 txBuf[5] = 0; // Checksum1
passionvirus 0:0eef242852bb 115 txBuf[6] = 0; // Checksum2
passionvirus 0:0eef242852bb 116 txBuf[7] = playtime; // Playtime
passionvirus 0:0eef242852bb 117 txBuf[8] = pos & 0x00FF; // JOG(LSB)
passionvirus 0:0eef242852bb 118 txBuf[9] =(pos & 0xFF00) >> 8; // JOG(MSB)
passionvirus 0:0eef242852bb 119 txBuf[10] = setMode | setLED; // Set mode and LED on/off
passionvirus 0:0eef242852bb 120 txBuf[11] = id; // Servo ID
passionvirus 0:0eef242852bb 121
passionvirus 0:0eef242852bb 122 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
passionvirus 0:0eef242852bb 123 // Checksum2 = (~Checksum1)&0xFE
passionvirus 0:0eef242852bb 124 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]) & 0xFE;
passionvirus 0:0eef242852bb 125 txBuf[6] = (~txBuf[5])&0xFE;
passionvirus 0:0eef242852bb 126
passionvirus 0:0eef242852bb 127 // send packet (mbed -> herkulex)
passionvirus 0:0eef242852bb 128 txPacket(12, txBuf);
passionvirus 0:0eef242852bb 129 }
passionvirus 0:0eef242852bb 130
passionvirus 0:0eef242852bb 131 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 132