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