123
Embed:
(wiki syntax)
Show/hide line numbers
dynamixel.h
00001 // Dynamixel SDK platform independent header 00002 #ifndef _DYNAMIXEL_HEADER 00003 #define _DYNAMIXEL_HEADER 00004 00005 00006 #ifdef __cplusplus 00007 extern "C" { 00008 #endif 00009 00010 00011 ///////////// device control methods //////////////////////// 00012 int dxl_initialize( int devIndex, int baudnum ); 00013 void dxl_terminate(void); 00014 00015 00016 ///////////// set/get packet methods ////////////////////////// 00017 void dxl_set_txpacket_id( int id ); 00018 #define BROADCAST_ID (254) 00019 00020 void dxl_set_txpacket_instruction( int instruction ); 00021 #define INST_PING (1) 00022 #define INST_READ (2) 00023 #define INST_WRITE (3) 00024 #define INST_REG_WRITE (4) 00025 #define INST_ACTION (5) 00026 #define INST_RESET (6) 00027 #define INST_SYNC_WRITE (131) 00028 00029 #define MAXNUM_TXPARAM (150) 00030 void dxl_set_txpacket_parameter( int index, int value ); 00031 void dxl_set_txpacket_length( int length ); 00032 00033 int dxl_get_rxpacket_error( int errbit ); 00034 #define ERRBIT_VOLTAGE (1) 00035 #define ERRBIT_ANGLE (2) 00036 #define ERRBIT_OVERHEAT (4) 00037 #define ERRBIT_RANGE (8) 00038 #define ERRBIT_CHECKSUM (16) 00039 #define ERRBIT_OVERLOAD (32) 00040 #define ERRBIT_INSTRUCTION (64) 00041 00042 #define MAXNUM_RXPARAM (60) 00043 int dxl_get_rxpacket_parameter( int index ); 00044 int dxl_get_rxpacket_length(void); 00045 00046 // utility for value 00047 int dxl_makeword( int lowbyte, int highbyte ); 00048 int dxl_get_lowbyte( int word ); 00049 int dxl_get_highbyte( int word ); 00050 00051 00052 ////////// packet communication methods /////////////////////// 00053 void dxl_tx_packet(void); 00054 void dxl_rx_packet(void); 00055 void dxl_txrx_packet(void); 00056 00057 int dxl_get_result(void); 00058 #define COMM_TXSUCCESS (0) 00059 #define COMM_RXSUCCESS (1) 00060 #define COMM_TXFAIL (2) 00061 #define COMM_RXFAIL (3) 00062 #define COMM_TXERROR (4) 00063 #define COMM_RXWAITING (5) 00064 #define COMM_RXTIMEOUT (6) 00065 #define COMM_RXCORRUPT (7) 00066 00067 00068 //MX64 Register stanley/// 00069 #define ID_ADDRESS 3 00070 #define GOAL_POSITION 30 00071 #define PRESENT_POS 36 00072 #define RETURN_DELAY_TIME 5 00073 #define CW_ANGLE_LIMIT_L 6 00074 #define CW_ANGLE_LIMIT_H 7 00075 #define CCW_ANGLE_LIMIT_L 8 00076 #define CCW_ANGLE_LIMIT_H 9 00077 #define MAX_TORQUE 14 00078 #define MULTITURN_OFFSET 20 00079 #define GOAL_POSITION 30 00080 #define GOAL_SPEED 32 00081 #define TORQUE_LIMIT 34 00082 #define PRESENT_POS 36 00083 00084 #define STILL_MOVING 46 00085 00086 00087 #define ADDRESS_LED 25 00088 00089 00090 //////////// high communication methods /////////////////////// 00091 void dxl_ping( int id ); 00092 int dxl_read_byte( int id, int address ); 00093 void dxl_write_byte( int id, int address, int value ); 00094 int dxl_read_word( short int id, short int address ); 00095 void dxl_write_word( short int id, short int address, short int value ); 00096 int syncWrite_u16base(unsigned short int start_addr, unsigned short int data_length, unsigned short int *param, unsigned short int param_length); // WORD(16bit) syncwrite() for DXL 00097 void setPosition(int ServoID, int Position, int Speed);//stanley 00098 00099 00100 00101 00102 00103 #ifdef __cplusplus 00104 } 00105 #endif 00106 00107 #endif
Generated on Thu Jul 14 2022 17:32:04 by
1.7.2