herkulex servo control library (fixes velocityControl and positionControl)
Fork of herkulex by
Diff: herkulex.cpp
- Revision:
- 6:1dacff31b77a
- Parent:
- 5:f737e5c70115
- Child:
- 7:fb5f919f2700
--- a/herkulex.cpp Mon Jan 14 16:24:05 2013 +0000 +++ b/herkulex.cpp Mon Jan 14 21:03:43 2013 +0000 @@ -28,7 +28,7 @@ #ifdef HERKULEX_DEBUG pc = new Serial(USBTX, USBRX); pc->baud(57600); - pc->printf("Herkulex Init!\n"); + pc->printf("\n\nHerkulex Init!\n"); #endif txd = new Serial(tx, NC); @@ -74,6 +74,27 @@ } //------------------------------------------------------------------------------ +void Herkulex::rxPacket(uint8_t packetSize, uint8_t* data) +{ + #ifdef HERKULEX_DEBUG + pc->printf("[RX]"); + #endif + + for (uint8_t i=0; i < packetSize; i++) + { + data[i] = rxd->getc(); + + #ifdef HERKULEX_DEBUG + pc->printf("%02X ",data[i]); + #endif + } + + #ifdef HERKULEX_DEBUG + pc->printf("\n"); + #endif +} + +//------------------------------------------------------------------------------ void Herkulex::clear(uint8_t id) { uint8_t txBuf[11]; @@ -113,7 +134,7 @@ txBuf[6] = 0; // Checksum2 txBuf[7] = RAM_TORQUE_CONTROL; // Address 52 txBuf[8] = BYTE1; // Length - txBuf[9] = cmdTorue; // Torque ON + txBuf[9] = cmdTorue; // Torque ON // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE // Checksum2 = (~Checksum1)&0xFE @@ -125,7 +146,7 @@ } //------------------------------------------------------------------------------ -void Herkulex::movePos(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED) +void Herkulex::positionControl(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED) { if (position > 1023) return; if (playtime > 255) return; @@ -155,7 +176,7 @@ } //------------------------------------------------------------------------------ -void Herkulex::turn(uint8_t id, int16_t speed, uint8_t setLED) +void Herkulex::velocityControl(uint8_t id, int16_t speed, uint8_t setLED) { if (speed > 1023 || speed < -1023) return; @@ -184,5 +205,116 @@ } //------------------------------------------------------------------------------ +int8_t Herkulex::getStatus(uint8_t id) +{ + uint8_t status; + uint8_t txBuf[7]; + + txBuf[0] = HEADER; // Packet Header (0xFF) + txBuf[1] = HEADER; // Packet Header (0xFF) + txBuf[2] = MIN_PACKET_SIZE; // Packet Size + txBuf[3] = id; // Servo ID + txBuf[4] = CMD_STAT; // Status Error, Status Detail request + // Check Sum1 and Check Sum2 + txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]) & 0xFE; + txBuf[6] = (~txBuf[5])&0xFE; + // send packet (mbed -> herkulex) + txPacket(7, txBuf); + + uint8_t rxBuf[9]; + rxPacket(9, rxBuf); + + // Checksum1 + uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]) & 0xFE; + if (chksum1 != rxBuf[5]) + { + #ifdef HERKULEX_DEBUG + pc->printf("Checksum1 fault\n"); + #endif + + return -1; + } + + // Checksum2 + uint8_t chksum2 = (~rxBuf[5]&0xFE); + if (chksum2 != rxBuf[6]) + { + #ifdef HERKULEX_DEBUG + pc->printf("Checksum2 fault\n"); + #endif + + return -1; + } + + status = rxBuf[7]; // Status Error + //status = rxBuf[8]; // Status Detail + + #ifdef HERKULEX_DEBUG + pc->printf("Status = %02X\n", status); + #endif + + return status; +} + +//------------------------------------------------------------------------------ +int16_t Herkulex::getPos(uint8_t id) +{ + uint16_t position = 0; + + uint8_t txBuf[9]; + + txBuf[0] = HEADER; // Packet Header (0xFF) + txBuf[1] = HEADER; // Packet Header (0xFF) + txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size + txBuf[3] = id; // Servo ID + txBuf[4] = CMD_RAM_READ; // Status Error, Status Detail request + txBuf[5] = 0; // Checksum1 + txBuf[6] = 0; // Checksum2 + txBuf[7] = RAM_CALIBRATED_POSITION; // Address 52 + txBuf[8] = BYTE2; // Address 52 and 53 + + // Check Sum1 and Check Sum2 + txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE; + txBuf[6] = (~txBuf[5])&0xFE; + + // send packet (mbed -> herkulex) + txPacket(9, txBuf); + + uint8_t rxBuf[13]; + rxPacket(13, rxBuf); + + // Checksum1 + uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]^rxBuf[12]) & 0xFE; + if (chksum1 != rxBuf[5]) + { + #ifdef HERKULEX_DEBUG + pc->printf("Checksum1 fault\n"); + #endif + + return -1; + } + + // Checksum2 + uint8_t chksum2 = (~rxBuf[5]&0xFE); + if (chksum2 != rxBuf[6]) + { + #ifdef HERKULEX_DEBUG + pc->printf("Checksum2 fault\n"); + #endif + + return -1; + } + + position = ((rxBuf[10]&0x03)<<8) | rxBuf[9]; + + #ifdef HERKULEX_DEBUG + pc->printf("position = %04X(%d)\n", position, position); + #endif + + return position; +} + +//------------------------------------------------------------------------------ +