herkulex servo control library
Fork of herkulex by
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?
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 | 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 |