/* Dynamixel.cpp - Ax-12+ Half Duplex USART Comunication Copyright (c) 2011 Savage Electronics. Created by Savage on 27/01/11. This library is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; either version 2.1 of the License, or (at your option) any later version. This library is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License along with this library; if not, write to the Free Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA ***************************************************************************** Modifications: 25/07/2011 - Eliminado la modificacion serial para ser modificada dentro del mismo Hardware Serial. 25/07/2011 - Modificado la funcion setBD() para aceptar todas la velocidades sin PDF. 25/07/2011 - Agregada la funcion de Rotacion Continua. 26/07/2011 - Agregada la funcion begin sin seteo de Direction_Pin. 25/07/2011 - Agregada la funcion Reset. 26/07/2011 - Agregada la funcion Reg_Write en move y moveSpeed. 26/07/2011 - Agregada la funcion Action. 13/12/2011 - Arreglado el manejo y envio de variables. 22/12/2011 - Compatible con la actualizacion Arduino 1.0. 10/01/2012 - Utilizacion de Macros y eliminacion codigo no necesario. 11/01/2012 - Agregadas las funciones: int setTempLimit(unsigned char ID, unsigned char Temperature); int setAngleLimit(unsigned char ID, int CWLimit, int CCWLimit); int setVoltageLimit(unsigned char ID, unsigned char DVoltage, unsigned char UVoltage); int setMaxTorque(unsigned char ID, int MaxTorque); int setSRL(unsigned char ID, unsigned char SRL); int setRDT(unsigned char ID, unsigned char RDT); int setLEDAlarm(unsigned char ID, unsigned char LEDAlarm); int setShutdownAlarm(unsigned char ID, unsigned char SALARM); int setCMargin(unsigned char ID, unsigned char CWCMargin, unsigned char CCWCMargin); int setCSlope(unsigned char ID, unsigned char CWCSlope, unsigned char CCWCSlope); 15/01/2012 - Agregadas las funciones: int setPunch(unsigned char ID, int Punch); int moving(unsigned char ID); int lockRegister(unsigned char ID); int RWStatus(unsigned char ID); int readSpeed(unsigned char ID); int readLoad(unsigned char ID); Bug Reports: 13/06/2013 - CSlope Function "Bad command Error", by filip aka. TODO: FUNCION SYNCWRITE. ***************************************************************************** Contact: savageelectronics@gmail.com Web: http://savageelectrtonics.blogspot.com/ Autor: Josue Alejandro Savage */ #ifndef DynamixelSerial1_h #define DynamixelSerial1_h // EEPROM AREA /////////////////////////////////////////////////////////// #define AX_MODEL_NUMBER_L 0 #define AX_MODEL_NUMBER_H 1 #define AX_VERSION 2 #define AX_ID 3 #define AX_BAUD_RATE 4 #define AX_RETURN_DELAY_TIME 5 #define AX_CW_ANGLE_LIMIT_L 6 #define AX_CW_ANGLE_LIMIT_H 7 #define AX_CCW_ANGLE_LIMIT_L 8 #define AX_CCW_ANGLE_LIMIT_H 9 #define AX_SYSTEM_DATA2 10 #define AX_LIMIT_TEMPERATURE 11 #define AX_DOWN_LIMIT_VOLTAGE 12 #define AX_UP_LIMIT_VOLTAGE 13 #define AX_MAX_TORQUE_L 14 #define AX_MAX_TORQUE_H 15 #define AX_RETURN_LEVEL 16 #define AX_ALARM_LED 17 #define AX_ALARM_SHUTDOWN 18 #define AX_OPERATING_MODE 19 #define AX_DOWN_CALIBRATION_L 20 #define AX_DOWN_CALIBRATION_H 21 #define AX_UP_CALIBRATION_L 22 #define AX_UP_CALIBRATION_H 23 // RAM AREA ////////////////////////////////////////////////////////////// #define AX_TORQUE_ENABLE 24 #define AX_LED 25 #define AX_CW_COMPLIANCE_MARGIN 26 #define AX_CCW_COMPLIANCE_MARGIN 27 #define AX_CW_COMPLIANCE_SLOPE 28 #define AX_CCW_COMPLIANCE_SLOPE 29 #define AX_GOAL_POSITION_L 30 #define AX_GOAL_POSITION_H 31 #define AX_GOAL_SPEED_L 32 #define AX_GOAL_SPEED_H 33 #define AX_TORQUE_LIMIT_L 34 #define AX_TORQUE_LIMIT_H 35 #define AX_PRESENT_POSITION_L 36 #define AX_PRESENT_POSITION_H 37 #define AX_PRESENT_SPEED_L 38 #define AX_PRESENT_SPEED_H 39 #define AX_PRESENT_LOAD_L 40 #define AX_PRESENT_LOAD_H 41 #define AX_PRESENT_VOLTAGE 42 #define AX_PRESENT_TEMPERATURE 43 #define AX_REGISTERED_INSTRUCTION 44 #define AX_PAUSE_TIME 45 #define AX_MOVING 46 #define AX_LOCK 47 #define AX_PUNCH_L 48 #define AX_PUNCH_H 49 // Status Return Levels /////////////////////////////////////////////////////////////// #define AX_RETURN_NONE 0 #define AX_RETURN_READ 1 #define AX_RETURN_ALL 2 // Instruction Set /////////////////////////////////////////////////////////////// #define AX_PING 1 #define AX_READ_DATA 2 #define AX_WRITE_DATA 3 #define AX_REG_WRITE 4 #define AX_ACTION 5 #define AX_RESET 6 #define AX_SYNC_WRITE 131 // Specials /////////////////////////////////////////////////////////////// #define OFF 0 #define ON 1 #define LEFT 0 #define RIGTH 1 #define AX_BYTE_READ 1 #define AX_BYTE_READ_POS 2 #define AX_RESET_LENGTH 2 #define AX_ACTION_LENGTH 2 #define AX_ID_LENGTH 4 #define AX_LR_LENGTH 4 #define AX_SRL_LENGTH 4 #define AX_RDT_LENGTH 4 #define AX_LEDALARM_LENGTH 4 #define AX_SALARM_LENGTH 4 #define AX_TL_LENGTH 4 #define AX_VL_LENGTH 6 #define AX_CM_LENGTH 6 #define AX_CS_LENGTH 5 #define AX_CCW_CW_LENGTH 8 #define AX_BD_LENGTH 4 #define AX_TEM_LENGTH 4 #define AX_MOVING_LENGTH 4 #define AX_RWS_LENGTH 4 #define AX_VOLT_LENGTH 4 #define AX_LED_LENGTH 4 #define AX_TORQUE_LENGTH 4 #define AX_POS_LENGTH 4 #define AX_GOAL_LENGTH 5 #define AX_MT_LENGTH 5 #define AX_PUNCH_LENGTH 5 #define AX_SPEED_LENGTH 5 #define AX_GOAL_SP_LENGTH 7 #define AX_ACTION_CHECKSUM 250 #define BROADCAST_ID 254 #define AX_START 255 #define AX_CCW_AL_L 255 #define AX_CCW_AL_H 3 #define TIME_OUT 10 // Este parametro depende de la velocidad de transmision #define TX_DELAY_TIME 400 // Este parametro depende de la velocidad de transmision - pero pueden ser cambiados para mayor velocidad. #define Tx_MODE 1 #define Rx_MODE 0 #define LOCK 1 #include class DynamixelClass { private: unsigned char Checksum; unsigned char Direction_Pin; unsigned char Time_Counter; unsigned char Incoming_Byte; unsigned char Position_High_Byte; unsigned char Position_Low_Byte; unsigned char Speed_High_Byte; unsigned char Speed_Low_Byte; unsigned char Load_High_Byte; unsigned char Load_Low_Byte; int Moving_Byte; int RWS_Byte; int Speed_Long_Byte; int Load_Long_Byte; int Position_Long_Byte; int Temperature_Byte; int Voltage_Byte; int Error_Byte; int read_error(void); public: void begin(long baud, unsigned char directionPin); void begin(long baud); void end(void); int reset(unsigned char ID); int ping(unsigned char ID); int setID(unsigned char ID, unsigned char newID); int setBD(unsigned char ID, long baud); int move(unsigned char ID, int Position); int moveSpeed(unsigned char ID, int Position, int Speed); int setEndless(unsigned char ID,bool Status); int turn(unsigned char ID, bool SIDE, int Speed); int moveRW(unsigned char ID, int Position); int moveSpeedRW(unsigned char ID, int Position, int Speed); void action(void); int setTempLimit(unsigned char ID, unsigned char Temperature); int setAngleLimit(unsigned char ID, int CWLimit, int CCWLimit); int setVoltageLimit(unsigned char ID, unsigned char DVoltage, unsigned char UVoltage); int setMaxTorque(unsigned char ID, int MaxTorque); int setSRL(unsigned char ID, unsigned char SRL); int setRDT(unsigned char ID, unsigned char RDT); int setLEDAlarm(unsigned char ID, unsigned char LEDAlarm); int setShutdownAlarm(unsigned char ID, unsigned char SALARM); int setCMargin(unsigned char ID, unsigned char CWCMargin, unsigned char CCWCMargin); int setCSlope(unsigned char ID, unsigned char CWCSlope, unsigned char CCWCSlope); int setPunch(unsigned char ID, int Punch); int moving(unsigned char ID); int lockRegister(unsigned char ID); int RWStatus(unsigned char ID); int readTemperature(unsigned char ID); int readVoltage(unsigned char ID); int readPosition(unsigned char ID); int readSpeed(unsigned char ID); int readLoad(unsigned char ID); int torqueStatus(unsigned char ID, bool Status); int ledStatus(unsigned char ID, bool Status); }; extern DynamixelClass Dynamixel; #endif