herkulex servo control library (fixes velocityControl and positionControl)
Fork of herkulex by
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?
User | Revision | Line number | New 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 |