herkulex servo control library (fixes velocityControl and positionControl)
Fork of herkulex by
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 + +//------------------------------------------------------------------------------