herkulex servo control library
Fork of herkulex by
herkulex.h
- Committer:
- passionvirus
- Date:
- 2013-01-14
- Revision:
- 0:0eef242852bb
- Child:
- 1:874d1f42989c
File content as of revision 0:0eef242852bb:
//------------------------------------------------------------------------------ /* 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 //------------------------------------------------------------------------------