Yoonseok Pyo
herkulex servo control library
Dependents: HerkuleX-HelloWorld
This herkulex library is based on DongBu Robot documentation and protocol.
http://dasarobot.com/guide/herkulexeng.pdf
Revision 5:f737e5c70115, committed 2013-01-14
- Comitter:
- passionvirus
- Date:
- Mon Jan 14 16:24:05 2013 +0000
- Parent:
- 4:e6873d42df32
- Child:
- 6:1dacff31b77a
- Commit message:
- Chage example code
Changed in this revision
| herkulex.cpp | Show annotated file Show diff for this revision Revisions of this file |
| herkulex.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/herkulex.cpp Mon Jan 14 07:25:00 2013 +0000
+++ b/herkulex.cpp Mon Jan 14 16:24:05 2013 +0000
@@ -74,6 +74,32 @@
}
//------------------------------------------------------------------------------
+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
+ 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_STATUS_ERROR; // Address 48
+ 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;
+ txBuf[6] = (~txBuf[5])&0xFE;
+
+ // send packet (mbed -> herkulex)
+ txPacket(11, txBuf);
+}
+
+//------------------------------------------------------------------------------
void Herkulex::setTorque(uint8_t id, uint8_t cmdTorue)
{
uint8_t txBuf[10];
@@ -99,9 +125,9 @@
}
//------------------------------------------------------------------------------
-void Herkulex::movePos(uint8_t id, uint16_t pos, uint8_t playtime, uint8_t setMode, uint8_t setLED)
+void Herkulex::movePos(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED)
{
- if (pos > 1023) return;
+ if (position > 1023) return;
if (playtime > 255) return;
uint8_t txBuf[12];
@@ -114,9 +140,38 @@
txBuf[5] = 0; // Checksum1
txBuf[6] = 0; // Checksum2
txBuf[7] = playtime; // Playtime
- txBuf[8] = pos & 0x00FF; // JOG(LSB)
- txBuf[9] =(pos & 0xFF00) >> 8; // JOG(MSB)
- txBuf[10] = setMode | setLED; // Set mode and LED on/off
+ txBuf[8] = position & 0x00FF; // Position (LSB, Least Significant Bit)
+ 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;
+ txBuf[6] = (~txBuf[5])&0xFE;
+
+ // send packet (mbed -> herkulex)
+ txPacket(12, txBuf);
+}
+
+//------------------------------------------------------------------------------
+void Herkulex::turn(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
+ txBuf[3] = 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
+ txBuf[7] = 0; // Playtime, unmeaningful in turn mode
+ txBuf[8] = speed & 0x00FF; // Speed (LSB, Least Significant Bit)
+ 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
@@ -130,3 +185,4 @@
//------------------------------------------------------------------------------
+
--- a/herkulex.h Mon Jan 14 07:25:00 2013 +0000
+++ b/herkulex.h Mon Jan 14 16:24:05 2013 +0000
@@ -186,12 +186,12 @@
#define BYTE2 2
// Jog Set CMD
-#define SET_STOP 0x01
-#define SET_MODE_POS 0x00
-#define SET_MODE_TURN 0x02
-#define SET_LED_GREEN_ON 0x04
-#define SET_LED_BLUE_ON 0x08
-#define SET_LED_RED_ON 0x10
+#define STOP 0x01
+#define POS_MODE 0x00
+#define TURN_MODE 0x02
+#define GLED_ON 0x04
+#define BLED_ON 0x08
+#define RLED_ON 0x10
//------------------------------------------------------------------------------
/** herkulex Servo control class, based packet protocol on a serial
@@ -245,7 +245,13 @@
*/
void txPacket(uint8_t packetSize, uint8_t* data);
- /** Set Torque setting
+ /** Clear error status
+ *
+ * @param id The herkulex servo ID.
+ */
+ void clear(uint8_t id);
+
+ /** Set torque setting
*
* @param id The herkulex servo ID.
* @param cmdTorue The Command for setting of torque (TORQUE_FREE 0x00,BREAK_ON 0x40, TORQUE_ON 0x60)
@@ -255,13 +261,21 @@
/** move position
*
* @param id The herkulex servo ID.
- * @param pos The goal position of herkulex servo.
+ * @param position The goal position of herkulex servo.
* @param playtime Time to target position.
- * @param setMode The mode of position control or infinite turn (SET_MODE_POS 0x00, SET_MODE_TURN 0x02)
* @param setLED Select LED and on/off controll (SET_LED_GREEN_ON 0x00,SET_LED_BLUE_ON 0x08, SET_LED_RED_ON 0x10)
*/
- void movePos(uint8_t id, uint16_t pos, uint8_t playtime, uint8_t setMode, uint8_t setLED);
-
+ void movePos(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED);
+
+ /** turn
+ *
+ * @param id The herkulex servo ID.
+ * @param speed The goal position of herkulex servo.
+ * @param setLED Select LED and on/off controll (SET_LED_GREEN_ON 0x00,SET_LED_BLUE_ON 0x08, SET_LED_RED_ON 0x10)
+ */
+ void turn(uint8_t id, int16_t speed,uint8_t setLED);
+
+
private :
/** PC serial connection used in debug mode.
HerkuleX