Genath N
herkulex servo control library
Fork of
herkulex
by 
Yoonseok Pyo
Diff: herkulex.cpp
- Revision:
- 7:0d98883391a2
- Parent:
- 6:1dacff31b77a
--- a/herkulex.cpp Mon Jan 14 21:03:43 2013 +0000
+++ b/herkulex.cpp Thu Jun 21 10:28:43 2018 +0000
@@ -3,102 +3,96 @@
*
* Copyright (c) 2012-2013 Yoonseok Pyo, MIT License
*
- * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
- * and associated documentation files (the "Software"), to deal in the Software without restriction,
- * including without limitation the rights to use, copy, modify, merge, publish, distribute,
- * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+ * and associated documentation files (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge, publish, distribute,
+ * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
- * The above copyright notice and this permission notice shall be included in all copies or
+ * The above copyright notice and this permission notice shall be included in all copies or
* substantial portions of the Software.
*
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
- * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
- * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
- * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+ * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
//------------------------------------------------------------------------------
#include "mbed.h"
#include "herkulex.h"
+Serial *pc;
+
//------------------------------------------------------------------------------
Herkulex::Herkulex(PinName tx, PinName rx, uint32_t baudRate)
{
- #ifdef HERKULEX_DEBUG
- pc = new Serial(USBTX, USBRX);
- pc->baud(57600);
- pc->printf("\n\nHerkulex Init!\n");
- #endif
-
- txd = new Serial(tx, NC);
- rxd = new Serial(NC, rx);
-
+#ifdef HERKULEX_DEBUG
+ pc = new Serial(USBTX, USBRX);
+ pc->baud(115200); //57600);
+ pc->printf("\n\r\n\rHerkulex Init!\n\r");
+#endif
+
+ txd = new RawSerial(tx, rx);
+
txd->baud(baudRate);
- rxd->baud(baudRate);
}
//------------------------------------------------------------------------------
Herkulex::~Herkulex()
{
- #ifdef HERKULEX_DEBUG
- if(pc != NULL)
- delete pc;
- #endif
-
+#ifdef HERKULEX_DEBUG
+ if(pc != NULL)
+ delete pc;
+#endif
+
if(txd != NULL)
delete txd;
- if(rxd != NULL)
- delete rxd;
}
//------------------------------------------------------------------------------
void Herkulex::txPacket(uint8_t packetSize, uint8_t* data)
{
- #ifdef HERKULEX_DEBUG
- pc->printf("[TX]");
- #endif
-
- for(uint8_t i = 0; i < packetSize ; i++)
- {
- #ifdef HERKULEX_DEBUG
- pc->printf("%02X ",data[i]);
- #endif
-
+#ifdef HERKULEX_DEBUG
+ pc->printf("[TX]");
+#endif
+
+ for(uint8_t i = 0; i < packetSize ; i++) {
+#ifdef HERKULEX_DEBUG
+ pc->printf("%02X ",data[i]);
+#endif
+
txd->putc(data[i]);
}
-
- #ifdef HERKULEX_DEBUG
- pc->printf("\n");
- #endif
+
+#ifdef HERKULEX_DEBUG
+ pc->printf("\n\r");
+#endif
}
//------------------------------------------------------------------------------
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
+#ifdef HERKULEX_DEBUG
+ pc->printf("[RX]");
+#endif
+
+ for (uint8_t i=0; i < packetSize; i++) {
+ data[i] = txd->getc();
+ }
+#ifdef HERKULEX_DEBUG
+ for (uint8_t i=0; i < packetSize; i++) {
+ pc->printf("%02X ",data[i]);
+ }
+ pc->printf("\n\r");
+#endif
}
//------------------------------------------------------------------------------
void Herkulex::clear(uint8_t id)
{
uint8_t txBuf[11];
-
+
txBuf[0] = HEADER; // Packet Header (0xFF)
txBuf[1] = HEADER; // Packet Header (0xFF)
txBuf[2] = MIN_PACKET_SIZE + 4; // Packet Size
@@ -110,7 +104,7 @@
txBuf[8] = BYTE2; // Length
txBuf[9] = 0; // Clear RAM_STATUS_ERROR
txBuf[10]= 0; // Clear RAM_STATUS_DETAIL
-
+
// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
// Checksum2 = (~Checksum1)&0xFE
txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]) & 0xFE;
@@ -121,10 +115,35 @@
}
//------------------------------------------------------------------------------
-void Herkulex::setTorque(uint8_t id, uint8_t cmdTorue)
+void Herkulex::leds(uint8_t id, uint8_t val)
{
uint8_t txBuf[10];
-
+
+ txBuf[0] = HEADER; // Packet Header (0xFF)
+ txBuf[1] = HEADER; // Packet Header (0xFF)
+ txBuf[2] = MIN_PACKET_SIZE + 3; // Packet Size
+ txBuf[3] = id; // Servo ID
+ txBuf[4] = CMD_RAM_WRITE; // Command Ram Write (0x03)
+ txBuf[5] = 0; // Checksum1
+ txBuf[6] = 0; // Checksum2
+ txBuf[7] = RAM_LED_CONTROL; // Address 48
+ txBuf[8] = BYTE1; // Length
+ txBuf[9] = val;
+
+ // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
+ // Checksum2 = (~Checksum1)&0xFE
+ txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]) & 0xFE;
+ txBuf[6] = (~txBuf[5])&0xFE;
+
+ // send packet (mbed -> herkulex)
+ txPacket(10, txBuf);
+}
+
+//------------------------------------------------------------------------------
+void Herkulex::setTorque(uint8_t id, uint8_t cmdTrue)
+{
+ uint8_t txBuf[10];
+
txBuf[0] = HEADER; // Packet Header (0xFF)
txBuf[1] = HEADER; // Packet Header (0xFF)
txBuf[2] = MIN_PACKET_SIZE + 3; // Packet Size
@@ -134,7 +153,7 @@
txBuf[6] = 0; // Checksum2
txBuf[7] = RAM_TORQUE_CONTROL; // Address 52
txBuf[8] = BYTE1; // Length
- txBuf[9] = cmdTorue; // Torque ON
+ txBuf[9] = cmdTrue; // Torque ON
// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
// Checksum2 = (~Checksum1)&0xFE
@@ -150,13 +169,13 @@
{
if (position > 1023) return;
if (playtime > 255) return;
-
+
uint8_t txBuf[12];
-
+
txBuf[0] = HEADER; // Packet Header (0xFF)
txBuf[1] = HEADER; // Packet Header (0xFF)
txBuf[2] = MIN_PACKET_SIZE + 5; // Packet Size
- txBuf[3] = MAX_PID; // pID is total number of servos in the network (0 ~ 253)
+ txBuf[3] = id; //MAX_PID; // pID is total number of servos in the network (0 ~ 253)
txBuf[4] = CMD_S_JOG; // Command S JOG (0x06)
txBuf[5] = 0; // Checksum1
txBuf[6] = 0; // Checksum2
@@ -165,7 +184,7 @@
txBuf[9] =(position & 0xFF00) >> 8;// position (MSB, Most Significanct Bit)
txBuf[10] = POS_MODE | setLED; // Pos Mode and LED on/off
txBuf[11] = id; // Servo ID
-
+
// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
// Checksum2 = (~Checksum1)&0xFE
txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]) & 0xFE;
@@ -179,9 +198,9 @@
void Herkulex::velocityControl(uint8_t id, int16_t speed, uint8_t setLED)
{
if (speed > 1023 || speed < -1023) return;
-
+
uint8_t txBuf[12];
-
+
txBuf[0] = HEADER; // Packet Header (0xFF)
txBuf[1] = HEADER; // Packet Header (0xFF)
txBuf[2] = MIN_PACKET_SIZE + 5; // Packet Size
@@ -194,7 +213,7 @@
txBuf[9] =(speed & 0xFF00) >> 8; // Speed (MSB, Most Significanct Bit)
txBuf[10] = TURN_MODE | setLED; // Turn Mode and LED on/off
txBuf[11] = id; // Servo ID
-
+
// Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
// Checksum2 = (~Checksum1)&0xFE
txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]) & 0xFE;
@@ -205,11 +224,11 @@
}
//------------------------------------------------------------------------------
-int8_t Herkulex::getStatus(uint8_t id)
+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
@@ -222,58 +241,56 @@
// 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
-
+ 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\r");
+#endif
+
return -1;
}
-
+
// Checksum2
uint8_t chksum2 = (~rxBuf[5]&0xFE);
- if (chksum2 != rxBuf[6])
- {
- #ifdef HERKULEX_DEBUG
- pc->printf("Checksum2 fault\n");
- #endif
-
+ if (chksum2 != rxBuf[6]) {
+#ifdef HERKULEX_DEBUG
+ pc->printf("Checksum2 fault\n\r");
+#endif
+
return -1;
}
status = rxBuf[7]; // Status Error
- //status = rxBuf[8]; // Status Detail
-
- #ifdef HERKULEX_DEBUG
- pc->printf("Status = %02X\n", status);
- #endif
-
+ //status = rxBuf[8]; // Status Detail
+
+#ifdef HERKULEX_DEBUG
+ pc->printf("Status = %02X\n\r", status);
+#endif
+
return status;
}
//------------------------------------------------------------------------------
-int16_t Herkulex::getPos(uint8_t id)
+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[6] = 0; // Checksum2
txBuf[7] = RAM_CALIBRATED_POSITION; // Address 52
- txBuf[8] = BYTE2; // Address 52 and 53
+ 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;
@@ -281,38 +298,36 @@
// 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
-
+ 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\r");
+#endif
+
return -1;
}
-
+
// Checksum2
uint8_t chksum2 = (~rxBuf[5]&0xFE);
- if (chksum2 != rxBuf[6])
- {
- #ifdef HERKULEX_DEBUG
- pc->printf("Checksum2 fault\n");
- #endif
-
+ if (chksum2 != rxBuf[6]) {
+#ifdef HERKULEX_DEBUG
+ pc->printf("Checksum2 fault\n\r");
+#endif
+
return -1;
}
position = ((rxBuf[10]&0x03)<<8) | rxBuf[9];
-
- #ifdef HERKULEX_DEBUG
- pc->printf("position = %04X(%d)\n", position, position);
- #endif
-
+
+#ifdef HERKULEX_DEBUG
+ pc->printf("position = %04X(%d)\n\r", position, position);
+#endif
+
return position;
}