Diff: herkulex.cpp

Revision:

0:0eef242852bb

Child:

1:874d1f42989c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/herkulex.cpp	Mon Jan 14 06:06:30 2013 +0000
@@ -0,0 +1,142 @@
+//------------------------------------------------------------------------------
+/* herkulex servo library for mbed
+ *
+ * Copyright (c) 2012-2013, Yoonseok Pyo
+ * All rights reserved.
+ *
+ * New BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the <organization> nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+//------------------------------------------------------------------------------
+#include "mbed.h"
+#include "herkulex.h"
+
+//------------------------------------------------------------------------------
+Herkulex::Herkulex(PinName tx, PinName rx, uint32_t baudRate)
+{
+    #ifdef HERKULEX_DEBUG
+        pc = new Serial(USBTX, USBRX);
+        pc->baud(57600);
+        pc->printf("Herkulex Init!\n");
+    #endif
+    
+    txd = new Serial(tx, NC);
+    rxd = new Serial(NC, rx);
+    
+    txd->baud(baudRate);
+    rxd->baud(baudRate);
+}
+
+//------------------------------------------------------------------------------
+Herkulex::~Herkulex()
+{
+    #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
+        
+        txd->putc(data[i]);
+    }
+    
+    #ifdef HERKULEX_DEBUG
+        pc->printf("\n");
+    #endif
+}
+
+//------------------------------------------------------------------------------
+void Herkulex::setTorque(uint8_t id, uint8_t cmdTorue)
+{
+    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_TORQUE_CONTROL;  // Address 52
+    txBuf[8] = BYTE1;               // Length
+    txBuf[9] = cmdTorue;           // Torque ON
+
+    // 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::movePos(uint8_t id, uint16_t pos, uint8_t playtime, uint8_t setMode, uint8_t setLED)
+{
+    if (pos > 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[4]  = CMD_S_JOG;              // Command S JOG (0x06)
+    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[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);
+}
+
+//------------------------------------------------------------------------------
+