Diff: herkulex.h

Revision:

0:0eef242852bb

Child:

1:874d1f42989c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/herkulex.h	Mon Jan 14 06:06:30 2013 +0000
@@ -0,0 +1,231 @@
+//------------------------------------------------------------------------------
+/* 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.
+ */
+//------------------------------------------------------------------------------
+#ifndef MBED_HERKULEX_H
+#define MBED_HERKULEX_H
+
+//------------------------------------------------------------------------------
+#include "mbed.h"
+
+//------------------------------------------------------------------------------
+#define HERKULEX_DEBUG
+
+//------------------------------------------------------------------------------
+// Herkulex ROM Register
+#define ROM_MODEL_NO1                       0
+#define ROM_MODEL_NO2                       1
+#define ROM_VERSION1                        2
+#define ROM_VERSION2                        3
+#define ROM_BAUD_RATE                       4
+#define ROM_RESERVED5                       5
+#define ROM_ID                              6
+#define ROM_ACK_POLICY                      7
+#define ROM_ALARM_LED_POLICY                8
+#define ROM_TORQUE_POLICY                   9
+#define ROM_RESERVED10                      10
+#define ROM_MAX_TEMPERATURE                 11
+#define ROM_MIN_VOLTAGE                     12
+#define ROM_MAX_VOLTAGE                     13
+#define ROM_ACCELERATION_RATIO              14
+#define ROM_MAX_ACCELERATION_TIME           15
+#define ROM_DEAD_ZONE                       16
+#define ROM_SATURATOR_OFFSET                17
+#define ROM_SATURATOR_SLOPE                 18  // 2Byte
+#define ROM_PWM_OFFSET                      20
+#define ROM_MIN_PWM                         21
+#define ROM_MAX_PWM                         22  // 2Byte
+#define ROM_OVERLOAD_PWM_THRESHOLD          24  // 2Byte
+#define ROM_MIN_POSITION                    26  // 2Byte
+#define ROM_MAX_POSITION                    28  // 2Byte
+#define ROM_POSITION_KP                     30  // 2Byte
+#define ROM_POSITION_KD                     32  // 2Byte
+#define ROM_POSITION_KI                     34  // 2Byte
+#define ROM_POSITION_FEEDFORWARD_1ST_GAIN   36  // 2Byte
+#define ROM_POSITION FEEDFORWARD_2ND_GAIN   38  // 2Byte
+#define ROM_RESERVED40                      40  // 2Byte
+#define ROM_RESERVED42                      42  // 2Byte
+#define ROM_LED_BLINK_PERIOD                44
+#define ROM_ADC_FAULT_CHECK_PERIOD          45
+#define ROM_PACKET_GARBAGE_CHECK_PERIOD     46
+#define ROM_STOP_DETECTION_PERIOD           47
+#define ROM_OVERLOAD_DETECTION_PERIOD       48   
+#define ROM_STOP_THRESHOLD                  49
+#define ROM_INPOSITION_MARGIN               50
+#define ROM_RESERVED51                      51
+#define ROM_RESERVED52                      52
+#define ROM_CALIBRATION_DIFFERENCE          53
+
+//------------------------------------------------------------------------------
+// Herkulex RAM Register
+#define RAM_ID                              0
+#define RAM_ACK_POLICY                      1
+#define RAM_ALARM_LED_POLICY                2
+#define RAM_TORQUE_POLICY                   3
+#define RAM_RESERVED4                       4
+#define RAM_MAX_TEMPERATURE                 5
+#define RAM_MIN_VOLTAGE                     6
+#define RAM_MAX_VOLTAGE                     7
+#define RAM_ACCELERATION_RATIO              8
+#define RAM_MAX_ACCELERATION                9
+#define RAM_DEAD_ZONE                       10
+#define RAM_SATURATOR_OFFSET                11
+#define RAM_SATURATOR_SLOPE                 12 // 2Byte
+#define RAM_PWM_OFFSET                      14
+#define RAM_MIN_PWM                         15
+#define RAM_MAX_PWM                         16 // 2Byte
+#define RAM_OVERLOAD_PWM_THRESHOLD          18 // 2Byte
+#define RAM_MIN_POSITION                    20 // 2Byte
+#define RAM_MAX_POSITION                    22 // 2Byte
+#define RAM_POSITION_KP                     24 // 2Byte
+#define RAM_POSITION_KD                     26 // 2Byte
+#define RAM_POSITION_KI                     28 // 2Byte
+#define RAM_POSITION_FEEDFORWARD_1ST_GAIN   30 // 2Byte
+#define RAM_POSITION_FEEDFORWARD 2ND GAIN   32 // 2Byte
+#define RAM_RESERVED34                      34 // 2Byte
+#define RAM_RESERVED36                      36 // 2Byte
+#define RAM_LED_BLINK_PERIOD                38
+#define RAM_ADC_FAULT_DETECTION_PERIOD      39
+#define RAM_PACKET_GARBAGE_DETECTION_PERIOD 40
+#define RAM_STOP_DETECTION_PERIOD           41
+#define RAM_OVERLOAD_DETECTION_PERIOD       42
+#define RAM_STOP_THRESHOLD                  43
+#define RAM_INPOSITION_MARGIN               44
+#define RAM_RESERVED45                      45
+#define RAM_RESERVED46                      46
+#define RAM_CALIBRATION_DIFFERENCE          47
+#define RAM_STATUS_ERROR                    48
+#define RAM_STATUS_DETAIL                   49
+#define RAM_RESERVED50                      50
+#define RAM_RESERVED51                      51 
+#define RAM_TORQUE_CONTROL                  52
+#define RAM_LED_CONTROL                     53
+#define RAM_VOLTAGE                         54
+#define RAM_TEMPERATURE                     55
+#define RAM_CURRENT_CONTROL_MODE            56
+#define RAM_TICK                            57
+#define RAM_CALIBRATED_POSITION             58 // 2Byte
+#define RAM_ABSOLUTE_POSITION               60 // 2Byte
+#define RAM_DIFFERENTIAL_POSITION           62 // 2Byte
+#define RAM_PWM                             64 // 2Byte
+#define RAM_RESERVED66                      66 // 2Byte
+#define RAM_ABSOLUTE_GOAL_POSITION          68 // 2Byte
+#define RAM_ABSOLUTE_DESIRED_TRAJECTORY_POSITION    70 // 2Byte
+#define RAM_DESIRED_VELOCITY                72 // 2Byte
+
+//------------------------------------------------------------------------------
+// Request Packet [To Servo Module] 
+#define CMD_ROM_WRITE  0x01    // Write Length number of values to EEP Register Address
+#define CMD_ROM_READ   0x02    // Request Length number of values from EEP Register Address
+#define CMD_RAM_WRITE  0x03    // Write Length number of values to RAM Register Address
+#define CMD_RAM_READ   0x04    // Request Lenght number of values from RAM Register Address
+#define CMD_I_JOG      0x05    // Able to send JOG command to maximum 43 servos (operate timing of individual Servo)
+#define CMD_S_JOG      0x06    // Able to send JOG command to maximum 53 servos (operate simultaneously at same time)
+#define CMD_STAT       0x07    // Status Error, Status Detail request
+#define CMD_ROLLBACK   0x08    // Change all EEP Regsters to Factory Default value
+#define CMD_REBOOT     0x09    // Request Reboot
+
+//------------------------------------------------------------------------------
+// ACK Packet [To Controller(ACK)]
+#define CMD_ACK_MASK   0x40    // ACK Packet CMD is Request Packet CMD + 0x40
+
+#define CMD_EEP_WRITE_ACK   (CMD_EEP_WRITE|CMD_ACK_MASK)
+#define CMD_EEP_READ_ACK    (CMD_EEP_READ|CMD_ACK_MASK)
+#define CMD_RAM_WRITE_ACK   (CMD_RAM_WRITE|CMD_ACK_MASK)
+#define CMD_RAM_READ_ACK    (CMD_RAM_READ|CMD_ACK_MASK)
+#define CMD_I_JOG_ACK       (CMD_I_JOG|CMD_ACK_MASK)
+#define CMD_S_JOG_ACK       (CMD_S_JOG|CMD_ACK_MASK)
+#define CMD_STAT_ACK        (CMD_STAT|CMD_ACK_MASK)
+#define CMD_ROLLBACK_ACK    (CMD_ROLLBACK|CMD_ACK_MASK)
+#define CMD_REBOOT_ACK      (CMD_REBOOT|CMD_ACK_MASK)
+
+//------------------------------------------------------------------------------
+// Header
+#define HEADER                              0xFF
+
+// Size
+#define MIN_PACKET_SIZE                     7
+#define MIN_ACK_PACKET_SIZE                 9
+#define WRITE_PACKET_SIZE                   13
+#define MAX_PACKET_SIZE                     223
+#define MAX_DATA_SIZE                       (MAX_PACKET_SIZE-MIN_PACKET_SIZE)
+
+// ID
+#define MAX_PID                             0xFD
+#define DEFAULT_ID                          0xFD
+#define MAX_ID                              0xFD
+#define BROADCAST_ID                        0xFE
+
+// Checksum
+#define CHKSUM_MASK                         0xFE
+
+// Torque CMD
+#define TORQUE_FREE                         0x00
+#define BREAK ON                            0x40
+#define TORQUE_ON                           0x60
+
+// Register Size
+#define BYTE1                               1
+#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
+
+
+//------------------------------------------------------------------------------
+
+class Herkulex
+{
+public:
+
+    Herkulex(PinName tx, PinName rx, uint32_t baudRate);
+    ~Herkulex(); 
+
+    void txPacket(uint8_t packetSize, uint8_t* data);
+    
+    void setTorque(uint8_t id, uint8_t cmdTorue);
+    
+    void movePos(uint8_t id, uint16_t pos, uint8_t playtime, uint8_t setMode, uint8_t setLED);
+            
+private :
+
+    Serial *pc;
+    Serial *txd, *rxd;
+};
+
+//------------------------------------------------------------------------------
+#endif  // MBED_HERKULEX_H
+
+//------------------------------------------------------------------------------