├── images ├── Max485Schematic.jpg └── DynamixelArduino_bb.png ├── library.properties ├── README.adoc ├── src ├── DynamixelAxConfig.h ├── DynamixelMxConfig.h ├── DynamixelAx.h ├── DynamixelAx.cpp ├── DynamixelMx.h ├── DynamixelMx.cpp ├── DynamixelConfig.h ├── Dynamixel.h └── Dynamixel.cpp ├── keywords.txt └── examples ├── DynamixelAxMonitorBlock └── DynamixelAxMonitorBlock.ino ├── DynamixelMxMonitorBlock └── DynamixelMxMonitorBlock.ino └── DynamixelMxMonitorNoBlock └── DynamixelMxMonitorNoBlock.ino /images/Max485Schematic.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akira215/DynamixelArduino/HEAD/images/Max485Schematic.jpg -------------------------------------------------------------------------------- /images/DynamixelArduino_bb.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/akira215/DynamixelArduino/HEAD/images/DynamixelArduino_bb.png -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=DynamixelArduino 2 | version=1.2.0 3 | author=Akira 4 | maintainer=Akira 5 | sentence=Use to communicate with Dynamixel servos. 6 | paragraph=This library is divided in 3 classes : dxl, dxlAx, dxlMx. As Dynamixel use half duplex communication, a halfDuplexSerial object should be passed as a constructor argument. 7 | category=Communication 8 | url=https://github.com/akira215/DynamixelArduino 9 | architectures=* 10 | -------------------------------------------------------------------------------- /README.adoc: -------------------------------------------------------------------------------- 1 | = DynamixelArduino Library for Arduino = 2 | 3 | This library implement all the required operation to drive Dynamixel servo.T his library is divided in 3 classes : dxl, dxlAx, dxlMx. As Dynamixel use half duplex communication, a halfDuplexSerial object should be passed as a constructor argument. 4 | This could be achieved using the halfDuplexSerial lib (soft, usig only one wire, or hard, using Tx, Rx and dir pin) 5 | Limitations: 6 | - Baudrate is limited 57 600bps ~ 115 200bps for 16Mhz Arduino board with softHalfDuplexSerial, and ~ 400 000bps for 16Mhz Arduino board with hardHalfDuplexSerial 7 | - This library is non blocking, i.e. when a write or a read command occured, it will wait NOT the answer of the servo. The return value is a bool (true when transmit succeed, false if an error occured during transmit) 8 | 9 | You can customize servo control by editing the DynamixelConfig.h file. 10 | The librarie has been written and tested with AX-12 servo & MX-64R, it should work with other Dynamixel servos. 11 | 12 | For more information about this library please visit us at 13 | https://github.com/akira215/DynamixelArduino 14 | 15 | == License == 16 | 17 | Copyright (c) Arduino LLC. All right reserved. 18 | 19 | This library is free software; you can redistribute it and/or 20 | modify it under the terms of the GNU Lesser General Public 21 | License as published by the Free Software Foundation; either 22 | version 2.1 of the License, or (at your option) any later version. 23 | 24 | This library is distributed in the hope that it will be useful, 25 | but WITHOUT ANY WARRANTY; without even the implied warranty of 26 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 27 | Lesser General Public License for more details. 28 | 29 | You should have received a copy of the GNU Lesser General Public 30 | License along with this library; if not, write to the Free Software 31 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 32 | -------------------------------------------------------------------------------- /src/DynamixelAxConfig.h: -------------------------------------------------------------------------------- 1 | /* 2 | DynamixelAxConfig.h 3 | Written by Akira 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | 19 | ***************************************************************************** 20 | Decription: 21 | This file contains all definition to drive Dynamixel servos (actually AX servo and most MX functions will work). 22 | You should modify the value according to your servo specification. Take care that invalid address or command can lead to unpredictible results 23 | Please visit http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm for AX actuators description 24 | and http://support.robotis.com/en/product/dynamixel/dxl_communication.htm to understand Dynamixel communication protocol 25 | */ 26 | 27 | #ifndef DynamixelAxConfig_h 28 | #define DynamixelAxConfig_h 29 | 30 | 31 | /****************************************************************************** 32 | * Servo registers (ADDRESS) 33 | ******************************************************************************/ 34 | 35 | #define DXL_ADD_CW_COMPLIANCE_MARGIN (0x1A) 36 | #define DXL_ADD_CCW_COMPLIANCE_MARGIN (0x1B) 37 | #define DXL_ADD_CW_COMPLIANCE_SLOPE (0x1C) 38 | #define DXL_ADD_CCW_COMPLIANCE_SLOPE (0x1D) 39 | 40 | /**************************************************************/ 41 | 42 | #endif // DynamixelAxConfig_h 43 | -------------------------------------------------------------------------------- /src/DynamixelMxConfig.h: -------------------------------------------------------------------------------- 1 | /* 2 | DynamixelMxConfig.h 3 | Written by Akira 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | 19 | ***************************************************************************** 20 | Decription: 21 | This file contains all definition to drive Dynamixel servos (actually MX servos). 22 | You should modify the value according to your servo specification. 23 | Please visit http://support.robotis.com/en/product/dynamixel/mx_series/mx-64.htm for MX actuators description 24 | and http://support.robotis.com/en/product/dynamixel/dxl_communication.htm to understand Dynamixel communication protocol 25 | */ 26 | 27 | #ifndef DynamixelMxConfig_h 28 | #define DynamixelMxConfig_h 29 | 30 | /****************************************************************************** 31 | * Servo registers (ADDRESS) 32 | ******************************************************************************/ 33 | 34 | #define DXL_ADD_MULTITURN_OFFSET (0x14) 35 | #define DXL_ADD_RESOLUTION_DIVIDER (0x16) 36 | #define DXL_ADD_D_GAIN (0x1A) 37 | #define DXL_ADD_I_GAIN (0x1B) 38 | #define DXL_ADD_P_GAIN (0x1C) 39 | #define DXL_ADD_CURRENT (0X44) 40 | #define DXL_ADD_TORQUE_CONTROL_ENABLE (0X46) 41 | #define DXL_ADD_GOAL_TORQUE (0X47) 42 | #define DXL_ADD_GOAL_ACCELERATION (0X49) 43 | 44 | /**************************************************************/ 45 | 46 | #endif // DynamixelMxConfig_h 47 | -------------------------------------------------------------------------------- /src/DynamixelAx.h: -------------------------------------------------------------------------------- 1 | /* 2 | DynamixelAx.h 3 | Written by Akira 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | 19 | ***************************************************************************** 20 | Decription: 21 | This library implement all the required operation to drive Dynamixel servo, 22 | using only one wire. 23 | It required the softHalfDuplexSerial lib 24 | Limitations: 25 | - The pin should support change interrupts (see halfDuplexSerial lib) 26 | - Baudrate is limited (57 600bps ~ 115 200bps for 16Mhz Arduino board) 27 | - 5V Arduino Board (I didn't try with 3.3V board as I didn't have one) 28 | - This library is non blocking, i.e. when a write or a read command occured, it will NOT wait the answer of the servo 29 | 30 | For more information, please visit : 31 | https://github.com/akira215/DynamixelArduino 32 | */ 33 | #ifndef DynamixelAx_h 34 | #define DynamixelAx_h 35 | 36 | #include 37 | #include 38 | #include 39 | 40 | 41 | /****************************************************************************** 42 | * Definitions 43 | ******************************************************************************/ 44 | 45 | class dxlAx : public dxl 46 | { 47 | public: 48 | // public methods 49 | dxlAx(halfDuplexSerial* port); 50 | ~dxlAx(); 51 | 52 | bool setCWComplianceMargin(const byte ID, const byte margin); 53 | bool readCWComplianceMargin(const byte ID); 54 | 55 | bool setCCWComplianceMargin(const byte ID, const byte margin); 56 | bool readCCWComplianceMargin(const byte ID); 57 | 58 | bool setCWComplianceSlope(const byte ID, const byte slope); 59 | bool readCWComplianceSlope(const byte ID); 60 | 61 | bool setCCWComplianceSlope(const byte ID, const byte slope); 62 | bool readCCWComplianceSlope(const byte ID); 63 | 64 | }; 65 | 66 | #endif // DynamixelAx_h 67 | -------------------------------------------------------------------------------- /src/DynamixelAx.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | DynamixelAx.cpp 3 | Written by Akira 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | 19 | ***************************************************************************** 20 | Decription: 21 | This library implement all the required operation to drive Dynamixel servo, 22 | using only one wire. 23 | It required the softHalfDuplexSerial lib 24 | Limitations: 25 | - The pin should support change interrupts (see halfDuplexSerial lib) 26 | - Baudrate is limited (57 600bps ~ 115 200bps for 16Mhz Arduino board) 27 | - 5V Arduino Board (I didn't try with 3.3V board as I didn't have one) 28 | - This library is non blocking, i.e. when a write or a read command occured, it will NOT wait the answer of the servo 29 | 30 | For more information, please visit : 31 | https://github.com/akira215/DynamixelArduino 32 | */ 33 | 34 | 35 | // 36 | // Includes 37 | // 38 | #include 39 | 40 | // 41 | // Constructor 42 | // 43 | dxlAx::dxlAx(halfDuplexSerial* port) : 44 | dxl(port) 45 | { 46 | } 47 | 48 | // 49 | // Destructor 50 | // 51 | dxlAx::~dxlAx() 52 | {} 53 | 54 | // 55 | // RAM commands 56 | // 57 | 58 | 59 | bool dxlAx::setCWComplianceMargin(const byte ID, const byte margin) 60 | { return sendDxlWrite(ID, DXL_ADD_CW_COMPLIANCE_MARGIN , &margin, 1 );} 61 | bool dxlAx::readCWComplianceMargin(const byte ID) 62 | { return sendDxlRead(ID, DXL_ADD_CW_COMPLIANCE_MARGIN , 1); } 63 | 64 | bool dxlAx::setCCWComplianceMargin(const byte ID, const byte margin) 65 | { return sendDxlWrite(ID, DXL_ADD_CCW_COMPLIANCE_MARGIN , &margin, 1 );} 66 | bool dxlAx::readCCWComplianceMargin(const byte ID) 67 | { return sendDxlRead(ID, DXL_ADD_CCW_COMPLIANCE_MARGIN , 1); } 68 | 69 | bool dxlAx::setCWComplianceSlope(const byte ID, const byte slope) 70 | { return sendDxlWrite(ID, DXL_ADD_CW_COMPLIANCE_SLOPE , &slope, 1 );} 71 | bool dxlAx::readCWComplianceSlope(const byte ID) 72 | { return sendDxlRead(ID, DXL_ADD_CW_COMPLIANCE_SLOPE , 1); } 73 | 74 | bool dxlAx::setCCWComplianceSlope(const byte ID, const byte slope) 75 | { return sendDxlWrite(ID, DXL_ADD_CCW_COMPLIANCE_SLOPE , &slope, 1 );} 76 | bool dxlAx::readCCWComplianceSlope(const byte ID) 77 | { return sendDxlRead(ID, DXL_ADD_CCW_COMPLIANCE_SLOPE , 1); } 78 | 79 | -------------------------------------------------------------------------------- /src/DynamixelMx.h: -------------------------------------------------------------------------------- 1 | /* 2 | DynamixelMx.h 3 | Written by Akira 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | 19 | ***************************************************************************** 20 | Decription: 21 | This library implement all the required operation to drive Dynamixel MX servo, 22 | 23 | Limitations: 24 | - This library is non blocking, i.e. when a write or a read command occured, it will NOT wait the answer of the servo 25 | - Baudrate tested up to 400 000bps on 16Mhz Arduino board (Mega 2560), with MAX485 component 26 | 27 | documentation for MX actuator is here: 28 | http://support.robotis.com/beta/en/product/dynamixel/mx_series/mx-64.htm 29 | */ 30 | #ifndef DynamixelMx_h 31 | #define DynamixelMx_h 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | /****************************************************************************** 38 | * Definitions 39 | ******************************************************************************/ 40 | 41 | class dxlMx : public dxl 42 | { 43 | public: 44 | 45 | // public methods 46 | dxlMx(halfDuplexSerial* port); 47 | ~dxlMx(); 48 | 49 | // 50 | // EEPROM commands 51 | // 52 | 53 | bool setMultiturnOffset(const byte ID, const short Offset); 54 | bool readMultiturnOffset(const byte ID); 55 | 56 | bool setResolutionDivider(const byte ID, const byte Resolution); 57 | bool readResolutionDivider(const byte ID); 58 | 59 | // 60 | // RAM commands 61 | // 62 | 63 | bool setDGain(const byte ID, const byte gain); 64 | bool readDGain(const byte ID); 65 | 66 | bool setIGain(const byte ID, const byte gain); 67 | bool readIGain(const byte ID); 68 | 69 | bool setPGain(const byte ID, const byte gain); 70 | bool readPGain(const byte ID); 71 | 72 | bool setCurrent(const byte ID, const unsigned short current); 73 | bool readCurrent(const byte ID); 74 | 75 | bool setTorqueControlEnable(const byte ID, const bool Enable); 76 | bool isTorqueControlEnable(const byte ID); 77 | 78 | bool setGoalTorque(const byte ID, const unsigned short Torque); 79 | bool readGoalTorque(const byte ID); 80 | 81 | bool setGoalAcceleration(const byte ID, const byte Acceleration); 82 | bool readGoalAcceleration(const byte ID); 83 | 84 | }; 85 | 86 | #endif // DynamixelMx_h 87 | -------------------------------------------------------------------------------- /src/DynamixelMx.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | DynamixelMx.cpp 3 | written by Akira 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | 19 | ***************************************************************************** 20 | Decription: 21 | This library implement all the required operation to drive Dynamixel MX servo, 22 | 23 | Limitations: 24 | - This library is blocking, i.e. when a write or a read command occured, it will wait the answer of the servo 25 | - Baudrate tested up to 400 000bps on 16Mhz Arduino board (Mega 2560), with MAX485 component 26 | 27 | documentation for MX actuator is here: 28 | http://support.robotis.com/beta/en/product/dynamixel/mx_series/mx-64.htm 29 | */ 30 | 31 | 32 | // 33 | // Includes 34 | // 35 | #include 36 | 37 | //////////////////////////////////////////////////////////////////////////////////////////////////// 38 | 39 | // 40 | // Constructor 41 | // 42 | dxlMx::dxlMx(halfDuplexSerial* port) : 43 | dxl(port) 44 | { 45 | } 46 | 47 | // 48 | // Destructor 49 | // 50 | dxlMx::~dxlMx() 51 | {} 52 | 53 | // 54 | // EEPROM commands 55 | // 56 | 57 | bool dxlMx::setMultiturnOffset(const byte ID, const short Offset) 58 | { return sendDxlWrite(ID, DXL_ADD_MULTITURN_OFFSET, (const byte*) &Offset, 2 );} 59 | bool dxlMx::readMultiturnOffset(const byte ID) 60 | { return sendDxlRead(ID, DXL_ADD_MULTITURN_OFFSET , 2); } 61 | 62 | bool dxlMx::setResolutionDivider(const byte ID, const byte Resolution) 63 | { return sendDxlWrite(ID, DXL_ADD_RESOLUTION_DIVIDER , &Resolution, 1 );} 64 | bool dxlMx::readResolutionDivider(const byte ID) 65 | { return sendDxlRead(ID, DXL_ADD_RESOLUTION_DIVIDER , 1); } 66 | 67 | // 68 | // RAM commands 69 | // 70 | 71 | bool dxlMx::setDGain(const byte ID, const byte gain) 72 | { return sendDxlWrite(ID, DXL_ADD_D_GAIN , &gain, 1 );} 73 | bool dxlMx::readDGain(const byte ID) 74 | { return sendDxlRead(ID, DXL_ADD_D_GAIN , 1); } 75 | 76 | bool dxlMx::setIGain(const byte ID, const byte gain) 77 | { return sendDxlWrite(ID, DXL_ADD_I_GAIN , &gain, 1 );} 78 | bool dxlMx::readIGain(const byte ID) 79 | { return sendDxlRead(ID, DXL_ADD_I_GAIN , 1); } 80 | 81 | bool dxlMx::setPGain(const byte ID, const byte gain) 82 | { return sendDxlWrite(ID, DXL_ADD_P_GAIN , &gain, 1 );} 83 | bool dxlMx::readPGain(const byte ID) 84 | { return sendDxlRead(ID, DXL_ADD_P_GAIN , 1); } 85 | 86 | bool dxlMx::setCurrent(const byte ID, const unsigned short current) 87 | { return sendDxlWrite(ID, DXL_ADD_CURRENT ,(const byte*) ¤t, 2 );} 88 | bool dxlMx::readCurrent(const byte ID) 89 | { return sendDxlRead(ID, DXL_ADD_CURRENT , 2); } 90 | 91 | bool dxlMx::setTorqueControlEnable(const byte ID, const bool Enable) 92 | { return sendDxlWrite(ID, DXL_ADD_TORQUE_CONTROL_ENABLE, (const byte*) &Enable, 1 );} 93 | bool dxlMx::isTorqueControlEnable(const byte ID) 94 | { return sendDxlRead(ID, DXL_ADD_TORQUE_CONTROL_ENABLE , 1); } 95 | 96 | bool dxlMx::setGoalTorque(const byte ID, const unsigned short Torque) 97 | { return sendDxlWrite(ID, DXL_ADD_GOAL_TORQUE ,(const byte*) &Torque, 2 );} 98 | bool dxlMx::readGoalTorque(const byte ID) 99 | { return sendDxlRead(ID, DXL_ADD_GOAL_TORQUE , 2); } 100 | 101 | bool dxlMx::setGoalAcceleration(const byte ID, const byte Acceleration) 102 | { return sendDxlWrite(ID, DXL_ADD_GOAL_ACCELERATION , &Acceleration, 1 );} 103 | bool dxlMx::readGoalAcceleration(const byte ID) 104 | { return sendDxlRead(ID, DXL_ADD_GOAL_ACCELERATION , 1); } 105 | 106 | 107 | -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map For Keyboard 3 | ####################################### 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | 9 | dxl KEYWORD1 10 | dxlAx KEYWORD1 11 | dxlMx KEYWORD1 12 | 13 | ####################################### 14 | # Methods and Functions (KEYWORD2) 15 | ####################################### 16 | 17 | ping KEYWORD2 18 | action KEYWORD2 19 | reset KEYWORD2 20 | reboot KEYWORD2 21 | readModelNumber KEYWORD2 22 | readFirmware KEYWORD2 23 | setId KEYWORD2 24 | readId KEYWORD2 25 | setBaudRate KEYWORD2 26 | readBaudRate KEYWORD2 27 | setReturnDelayTime KEYWORD2 28 | readReturnDelayTime KEYWORD2 29 | setCWAngleLimit KEYWORD2 30 | setCCWAngleLimit KEYWORD2 31 | readCWAngleLimit KEYWORD2 32 | readCCWAngleLimit KEYWORD2 33 | setMaxTemperature KEYWORD2 34 | readMaxTemperature KEYWORD2 35 | setMinVoltage KEYWORD2 36 | readMinVoltage KEYWORD2 37 | setMaxVoltage KEYWORD2 38 | readMaxVoltage KEYWORD2 39 | setMaxTorque KEYWORD2 40 | readMaxTorque KEYWORD2 41 | setStatusReturnLevel KEYWORD2 42 | readStatusReturnLevel KEYWORD2 43 | setAlarmLed KEYWORD2 44 | readAlarmLed KEYWORD2 45 | setAlarmShutdown KEYWORD2 46 | readAlarmShutdown KEYWORD2 47 | setMultiturnOffset KEYWORD2 48 | readMultiturnOffset KEYWORD2 49 | setResolutionDivider KEYWORD2 50 | readResolutionDivider KEYWORD2 51 | setTorqueEnable KEYWORD2 52 | readTorqueEnable KEYWORD2 53 | setLedEnable KEYWORD2 54 | readLedEnable KEYWORD2 55 | setDGain KEYWORD2 56 | readDGain KEYWORD2 57 | setIGain KEYWORD2 58 | readIGain KEYWORD2 59 | setPGain KEYWORD2 60 | readPGain KEYWORD2 61 | setCWComplianceMargin KEYWORD2 62 | readCWComplianceMargin KEYWORD2 63 | setCCWComplianceMargin KEYWORD2 64 | readCCWComplianceMargin KEYWORD2 65 | setCWComplianceSlope KEYWORD2 66 | readCWComplianceSlope KEYWORD2 67 | setCCWComplianceSlope KEYWORD2 68 | readCCWComplianceSlope KEYWORD2 69 | setGoalPosition KEYWORD2 70 | setGoalPositionAtSpeed KEYWORD2 71 | setMovingSpeed KEYWORD2 72 | setTorqueLimit KEYWORD2 73 | readPresentPosition KEYWORD2 74 | readPresentSpeed KEYWORD2 75 | readPresentLoad KEYWORD2 76 | readVoltage KEYWORD2 77 | readTemperature KEYWORD2 78 | isRegistered KEYWORD2 79 | isMoving KEYWORD2 80 | setEEPROMLock KEYWORD2 81 | isEEPROMLock KEYWORD2 82 | setPunch KEYWORD2 83 | readPunch KEYWORD2 84 | setCurrent KEYWORD2 85 | readCurrent KEYWORD2 86 | setTorqueControlEnable KEYWORD2 87 | isTorqueControlEnable KEYWORD2 88 | setGoalTorque KEYWORD2 89 | readGoalTorque KEYWORD2 90 | setGoalAcceleration KEYWORD2 91 | readGoalAcceleration KEYWORD2 92 | sendDxlCommand KEYWORD2 93 | sendDxlWrite KEYWORD2 94 | sendDxlRegData KEYWORD2 95 | sendDxlRead KEYWORD2 96 | dxlDataReady KEYWORD2 97 | readDxlError KEYWORD2 98 | readDxlResult KEYWORD2 99 | isBusy KEYWORD2 100 | setDxlTimeout KEYWORD2 101 | readDxlTimeout KEYWORD2 102 | setDxlReturnDelayTime KEYWORD2 103 | readDxlReturnDelayTime KEYWORD2 104 | 105 | ####################################### 106 | # Constants (LITERAL1) 107 | ####################################### 108 | 109 | DXL_ERR_SUCCESS LITERAL1 110 | DXL_ERR_VOLTAGE LITERAL1 111 | DXL_ERR_ANGLE LITERAL1 112 | DXL_ERR_OVERHEATING LITERAL1 113 | DXL_ERR_RANGE LITERAL1 114 | DXL_ERR_TX_CHECKSUM LITERAL1 115 | DXL_ERR_OVERLOAD LITERAL1 116 | DXL_ERR_INSTRUCTION LITERAL1 117 | DXL_ERR_TX_FAIL LITERAL1 118 | DXL_ERR_RX_FAIL LITERAL1 119 | DXL_ERR_TX_ERROR LITERAL1 120 | DXL_ERR_RX_LENGTH LITERAL1 121 | DXL_ERR_RX_TIMEOUT LITERAL1 122 | DXL_ERR_RX_CORRUPT LITERAL1 123 | DXL_ERR_ID LITERAL1 124 | DXL_START LITERAL1 125 | DXL_RETURN_DELAY_TIME LITERAL1 126 | DXL_PING LITERAL1 127 | DXL_READ_DATA LITERAL1 128 | DXL_WRITE_DATA LITERAL1 129 | DXL_REG_WRITE LITERAL1 130 | DXL_ACTION LITERAL1 131 | DXL_RESET LITERAL1 132 | DXL_REBOOT LITERAL1 133 | DXL_SYNC_READ LITERAL1 134 | DXL_SYNC_WRITE LITERAL1 135 | DXL_ADD_MODEL_NUMBER LITERAL1 136 | DXL_ADD_FIRMWARE LITERAL1 137 | DXL_ADD_ID LITERAL1 138 | DXL_ADD_BAUDRATE LITERAL1 139 | DXL_ADD_RETURN_DELAY_TIME LITERAL1 140 | DXL_ADD_CW_ANGLE_LIMIT LITERAL1 141 | DXL_ADD_CCW_ANGLE_LIMIT LITERAL1 142 | DXL_ADD_MAX_TEMPERATURE LITERAL1 143 | DXL_ADD_MIN_VOLTAGE LITERAL1 144 | DXL_ADD_MAX_VOLTAGE LITERAL1 145 | DXL_ADD_MAX_TORQUE LITERAL1 146 | DXL_ADD_STATUS_RETURN_LEVEL LITERAL1 147 | DXL_ADD_ALARM_LED LITERAL1 148 | DXL_ADD_ALARM_SHUTDOWN LITERAL1 149 | DXL_ADD_MULTITURN_OFFSET LITERAL1 150 | DXL_ADD_RESOLUTION_DIVIDER LITERAL1 151 | DXL_ADD_TORQUE_ENABLE LITERAL1 152 | DXL_ADD_LED LITERAL1 153 | DXL_ADD_D_GAIN LITERAL1 154 | DXL_ADD_I_GAIN LITERAL1 155 | DXL_ADD_P_GAIN LITERAL1 156 | DXL_ADD_CW_COMPLIANCE_MARGIN LITERAL1 157 | DXL_ADD_CCW_COMPLIANCE_MARGIN LITERAL1 158 | DXL_ADD_CW_COMPLIANCE_SLOPE LITERAL1 159 | DXL_ADD_CCW_COMPLIANCE_SLOPE LITERAL1 160 | DXL_ADD_GOAL_POSITION LITERAL1 161 | DXL_ADD_MOVING_SPEED LITERAL1 162 | DXL_ADD_TORQUE_LIMIT LITERAL1 163 | DXL_ADD_PRESENT_POSITION LITERAL1 164 | DXL_ADD_PRESENT_SPEED LITERAL1 165 | DXL_ADD_PRESENT_LOAD LITERAL1 166 | DXL_ADD_PRESENT_VOLTAGE LITERAL1 167 | DXL_ADD_PRESENT_TEMPERATURE LITERAL1 168 | DXL_ADD_REGISTERED LITERAL1 169 | DXL_ADD_MOVING LITERAL1 170 | DXL_ADD_LOCK LITERAL1 171 | DXL_ADD_PUNCH LITERAL1 172 | DXL_ADD_CURRENT LITERAL1 173 | DXL_ADD_TORQUE_CONTROL_ENABLE LITERAL1 174 | DXL_ADD_GOAL_TORQUE LITERAL1 175 | DXL_ADD_GOAL_ACCELERATION LITERAL1 176 | -------------------------------------------------------------------------------- /src/DynamixelConfig.h: -------------------------------------------------------------------------------- 1 | /* 2 | DynamixelConfig.h 3 | Written by Akira 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | 19 | ***************************************************************************** 20 | Decription: 21 | This file contains all definition to drive Dynamixel servos. 22 | You should modify the value according to your servo specification. 23 | Please visit http://support.robotis.com/en/product/dynamixel/dxl_communication.htm to understand Dynamixel communication protocol 24 | */ 25 | 26 | #ifndef DynamixelConfig_h 27 | #define DynamixelConfig_h 28 | 29 | /****************************************************************************** 30 | * Errors 31 | ******************************************************************************/ 32 | 33 | // lsb concern harware error (servo side) 34 | // msb concern software error (PC side) 35 | 36 | #define DXL_ERR_SUCCESS (0x00) // No header found during transmit 37 | 38 | #define DXL_ERR_VOLTAGE (0x01) // When the applied voltage is out of the range of operating voltage set in the Control table, 39 | #define DXL_ERR_ANGLE (0x02) // When Goal Position is written out of the range from CW Angle Limit to CCW Angle Limit 40 | #define DXL_ERR_OVERHEATING (0x04) // When internal temperature of Dynamixel is out of the range of operating temperature set in the Control table 41 | #define DXL_ERR_RANGE (0x08) // When a command is out of the range for use 42 | #define DXL_ERR_TX_CHECKSUM (0x10) // When the Checksum of the transmitted Instruction Packet is incorrect 43 | #define DXL_ERR_OVERLOAD (0x20) // When the current load cannot be controlled by the set Torque 44 | #define DXL_ERR_INSTRUCTION (0x40) // In case of sending an undefined instruction or delivering the action command without the reg_write command 45 | 46 | #define DXL_ERR_TX_FAIL (0x100) // No header found during transmit 47 | #define DXL_ERR_RX_FAIL (0x200) // No header found during receive 48 | #define DXL_ERR_TX_ERROR (0x400) 49 | #define DXL_ERR_RX_LENGTH (0x800) // The length of the response doesn't match 50 | #define DXL_ERR_RX_TIMEOUT (0x1000) // Time out during receive operation 51 | #define DXL_ERR_RX_CORRUPT (0x2000) // Checksum error 52 | #define DXL_ERR_ID (0x4000) // Wrong ID response after instruction packet 53 | 54 | /****************************************************************************** 55 | * Communication protocol 56 | ******************************************************************************/ 57 | 58 | #define DXL_START (0xFF) 59 | #define DXL_RETURN_DELAY_TIME (800) // us, servo return delay time register : 2usec per data value 60 | 61 | #define DXL_PING (0x01) 62 | #define DXL_READ_DATA (0x02) 63 | #define DXL_WRITE_DATA (0x03) 64 | #define DXL_REG_WRITE (0x04) 65 | #define DXL_ACTION (0x05) 66 | #define DXL_RESET (0x06) 67 | #define DXL_REBOOT (0x08) 68 | #define DXL_SYNC_READ (0x82) 69 | #define DXL_SYNC_WRITE (0x83) 70 | 71 | /****************************************************************************** 72 | * Servo registers (ADDRESS) 73 | ******************************************************************************/ 74 | 75 | #define DXL_ADD_MODEL_NUMBER (0x00) 76 | #define DXL_ADD_FIRMWARE (0x02) 77 | #define DXL_ADD_ID (0x03) 78 | #define DXL_ADD_BAUDRATE (0x04) 79 | #define DXL_ADD_RETURN_DELAY_TIME (0x05) 80 | #define DXL_ADD_CW_ANGLE_LIMIT (0x06) 81 | #define DXL_ADD_CCW_ANGLE_LIMIT (0x08) 82 | #define DXL_ADD_MAX_TEMPERATURE (0x0B) 83 | #define DXL_ADD_MIN_VOLTAGE (0x0C) 84 | #define DXL_ADD_MAX_VOLTAGE (0x0D) 85 | #define DXL_ADD_MAX_TORQUE (0x0E) 86 | #define DXL_ADD_STATUS_RETURN_LEVEL (0x10) 87 | #define DXL_ADD_ALARM_LED (0x11) 88 | #define DXL_ADD_ALARM_SHUTDOWN (0x12) 89 | 90 | 91 | #define DXL_ADD_TORQUE_ENABLE (0x18) 92 | #define DXL_ADD_LED (0x19) 93 | #define DXL_ADD_GOAL_POSITION (0x1E) 94 | #define DXL_ADD_MOVING_SPEED (0X20) 95 | #define DXL_ADD_TORQUE_LIMIT (0X22) 96 | #define DXL_ADD_PRESENT_POSITION (0X24) 97 | #define DXL_ADD_PRESENT_SPEED (0X26) 98 | #define DXL_ADD_PRESENT_LOAD (0X28) 99 | #define DXL_ADD_PRESENT_VOLTAGE (0X2A) 100 | #define DXL_ADD_PRESENT_TEMPERATURE (0x2B) 101 | #define DXL_ADD_REGISTERED (0X2C) 102 | #define DXL_ADD_MOVING (0X2E) 103 | #define DXL_ADD_LOCK (0X2F) 104 | #define DXL_ADD_PUNCH (0X30) 105 | 106 | 107 | /**************************************************************/ 108 | 109 | #endif // DynamixelConfig_h 110 | -------------------------------------------------------------------------------- /src/Dynamixel.h: -------------------------------------------------------------------------------- 1 | /* 2 | Dynamixel.h 3 | Written by Akira 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | 19 | ***************************************************************************** 20 | Decription: 21 | This library implement all the required operation to drive Dynamixel servo, 22 | Please visit http://support.robotis.com/en/product/dynamixel/dxl_communication.htm to understand Dynamixel communication protocol 23 | 24 | Limitations: 25 | - Baudrate is limited 57 600bps ~ 115 200bps for 16Mhz Arduino board with softHalfDuplexSerial, and ~ 400 000bps for 16Mhz Arduino board with hardHalfDuplexSerial 26 | - This library is non blocking, i.e. when a write or a read command occured, it will NOT wait the answer of the servo 27 | */ 28 | #ifndef Dynamixel_h 29 | #define Dynamixel_h 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | 36 | /****************************************************************************** 37 | * Definitions 38 | ******************************************************************************/ 39 | 40 | class dxl 41 | { 42 | private: 43 | halfDuplexSerial* _port; // Serial port to be used to communicate with dynamixel 44 | unsigned short _error; // Store the last error 45 | byte _nByteToBeRead; // Used to check when Rx packet is complete 46 | byte _currentId; // Used to check if the rx packet comes from the correct servo 47 | unsigned long _timeout; // Store Rx timeout (default 1000ms) 48 | unsigned long _currentTimeout; // To be compared with millis(); 49 | unsigned long _returnDelayTime; // Store the return time of Dynamixel servo (dfault 1000 µs) 50 | unsigned long _currentReturnDelayTime; // this variable assume that dxlDataReady();is called periodically 51 | unsigned short _dxlResult; // Dxl read will be stored here 52 | 53 | // This is the only function to write on the serial port 54 | bool writeRaw( const byte *sentence, const byte nByteToBeWritten); 55 | unsigned short readDxlData(); // the only function to read on serial port 56 | 57 | public: 58 | 59 | // public methods 60 | dxl(halfDuplexSerial* port); 61 | ~dxl(); 62 | void begin(const unsigned long speed); 63 | 64 | bool ping(const byte ID); // ping the servo 65 | bool action(const byte ID); // Excecute action written in REG_WRITE servo register. A status packet is receive, although the documentation don't mention it 66 | bool reset(const byte ID); // Reset the servo to factory default setting 67 | bool reboot(const byte ID); // Reboot the servo (not supported by mx servos) 68 | 69 | // 70 | // EEPROM commands 71 | // 72 | 73 | bool readModelNumber(const byte ID); 74 | bool readFirmware(const byte ID); 75 | 76 | bool setId(const byte ID, const byte newID); 77 | bool readId(const byte ID); // Is this really usefull ? 78 | 79 | bool setBaudRate(const byte ID, const byte baudRate); 80 | bool readBaudRate(const byte ID); 81 | 82 | bool setReturnDelayTime(const byte ID, const byte returnDelayTime); 83 | bool readReturnDelayTime(const byte ID); 84 | 85 | bool setCWAngleLimit(const byte ID, const unsigned short angle); 86 | bool setCCWAngleLimit(const byte ID, const unsigned short angle); 87 | 88 | bool readCWAngleLimit(const byte ID); 89 | bool readCCWAngleLimit(const byte ID); 90 | 91 | bool setMaxTemperature(const byte ID, const byte maxTemperature); 92 | bool readMaxTemperature(const byte ID); 93 | 94 | bool setMinVoltage(const byte ID, const byte minVoltage); 95 | bool readMinVoltage(const byte ID); 96 | 97 | bool setMaxVoltage(const byte ID, const byte maxVoltage); 98 | bool readMaxVoltage(const byte ID); 99 | 100 | bool setMaxTorque(const byte ID, const unsigned short maxTorque); 101 | bool readMaxTorque(const byte ID); 102 | 103 | bool setStatusReturnLevel(const byte ID, const byte Status); 104 | bool readStatusReturnLevel(const byte ID); 105 | 106 | bool setAlarmLed(const byte ID, const byte Status); 107 | bool readAlarmLed(const byte ID); 108 | 109 | bool setAlarmShutdown(const byte ID, const byte Status); 110 | bool readAlarmShutdown(const byte ID); 111 | 112 | // 113 | // RAM commands 114 | // 115 | bool setTorqueEnable(const byte ID, const bool Status); 116 | bool readTorqueEnable(const byte ID); 117 | 118 | bool setLedEnable(const byte ID, const bool Status); 119 | bool readLedEnable(const byte ID); 120 | 121 | bool setGoalPosition(const byte ID, const short Position); 122 | bool setGoalPositionAtSpeed(const byte ID, const short Position, const short Speed); 123 | bool setMovingSpeed(const byte ID, const short Speed); 124 | bool setTorqueLimit(const byte ID, const short torque); 125 | 126 | bool readPresentPosition(const byte ID); 127 | bool readPresentSpeed(const byte ID); 128 | bool readPresentLoad(const byte ID); 129 | 130 | bool readVoltage(const byte ID); 131 | bool readTemperature(const byte ID); 132 | bool isRegistered(const byte ID); 133 | bool isMoving(const byte ID); 134 | 135 | bool setEEPROMLock(const byte ID, const bool lock); 136 | bool isEEPROMLock(const byte ID); 137 | 138 | bool setPunch(const byte ID, const unsigned short current); 139 | bool readPunch(const byte ID); 140 | 141 | // Low level public methods 142 | 143 | bool sendDxlCommand(const byte ID, const byte dxlCommand); 144 | bool sendDxlWrite(const byte ID, const byte dxlAddress, const byte *params, const byte nByteToBeWritten); 145 | bool sendDxlRegData(const byte ID, const byte dxlAddress, const byte *params, const byte nByteToBeWritten); 146 | bool sendDxlRead(const byte ID, const byte dxlAddress, const byte nByteToBeRead); 147 | bool dxlDataReady(); 148 | unsigned short readDxlError(); // return the error or DXL_ERR_SUCCESS of the last operation 149 | unsigned short readDxlResult(); // return the value read of the last operation 150 | 151 | bool isBusy(); 152 | void setDxlTimeout(const unsigned long timeout) { _timeout = timeout; } 153 | unsigned long readDxlTimeout() { return _timeout; } 154 | void setDxlReturnDelayTime(const unsigned long returnDelayTime) { _returnDelayTime = returnDelayTime; } 155 | unsigned long readDxlReturnDelayTime() { return _returnDelayTime; } 156 | 157 | }; 158 | 159 | #endif // Dynamixel_h 160 | -------------------------------------------------------------------------------- /examples/DynamixelAxMonitorBlock/DynamixelAxMonitorBlock.ino: -------------------------------------------------------------------------------- 1 | /* 2 | DynamixelMxMonitorBlock.ino 3 | written by Akira 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | 19 | ***************************************************************************** 20 | Decription: 21 | This library implement all the required operation to drive Dynamixel servo, 22 | Please visit http://support.robotis.com/en/product/dynamixel/dxl_communication.htm to understand Dynamixel communication protocol 23 | 24 | Hardware: 25 | - This example use softHalfDuplexSerial library, check that the connected data pin support change interrupt 26 | - Pin 8 is used in this example to communicate with dynamixel servos (only one wire) 27 | - Check that the board is properly grounded with dynamixel power supply. 28 | - Please check the servo baudrate, it has been set to 57 600bps here. 29 | - Take care that the defaut servo ID is 1. 30 | In this example, we will wait after each Dynamixel command the answer of the servo. 31 | The code is simplier than in non blocking implemantation, but it will block any other code (except interrupt) during this time. 32 | */ 33 | #include 34 | #include 35 | 36 | softHalfDuplexSerial port(8); // data pin 8 37 | dxlAx dxlCom(&port); 38 | 39 | String _readString; // Input string from serial monitor 40 | bool _strComplete = false; 41 | int _id = 1; // Default Dynamixel servo ID 42 | 43 | 44 | void printServoId(String msg); 45 | void printDxlResult(); 46 | void printDxlError(unsigned short dxlError); 47 | 48 | void setup() { 49 | 50 | // Open serial communications and wait for port to open (PC communication) 51 | Serial.begin(57600); 52 | while (!Serial) { 53 | ; // wait for serial port to connect. Needed for native USB port only 54 | } 55 | 56 | Serial.println("Starting COM!"); 57 | 58 | dxlCom.begin(57600); 59 | 60 | } 61 | 62 | ///////////////////////////////////////////////////////////////////////////////////// 63 | 64 | void loop() 65 | { 66 | while (Serial.available()) 67 | { 68 | char inputChar = Serial.read(); //gets one byte from serial buffer 69 | _readString += inputChar; //makes the string readString 70 | 71 | if (inputChar=='\n') 72 | _strComplete = true; 73 | 74 | } 75 | 76 | if (_strComplete) 77 | { 78 | _strComplete = false; 79 | if (_readString.startsWith("ID")) 80 | { 81 | _readString.remove(0, 2); 82 | _id = _readString.toInt(); //convert readString into a number 83 | printServoId("Communicating with"); 84 | Serial.println(_id); 85 | } 86 | else if (_readString.startsWith("ping")) 87 | { 88 | printServoId("Ping"); 89 | dxlCom.ping(_id); 90 | printDxlResult(); 91 | } 92 | else if (_readString.startsWith("action")) 93 | { 94 | dxlCom.isRegistered(_id); 95 | while(!dxlCom.dxlDataReady()); // waiting the answer of servo 96 | printDxlError(dxlCom.readDxlError()); 97 | if (dxlCom.readDxlResult()) // if it is registred 98 | { 99 | printServoId("Execute reg command in"); 100 | while(dxlCom.isBusy()); // waiting the status return delay time 101 | dxlCom.action(_id); 102 | printDxlResult(); 103 | } 104 | else 105 | { 106 | printServoId("No reg command in"); 107 | Serial.println(); 108 | } 109 | } 110 | else if (_readString.startsWith("reboot")) 111 | { 112 | printServoId("Reboot (not supported by MX)"); 113 | dxlCom.reboot(_id); 114 | printDxlResult(); 115 | } 116 | else if (_readString.startsWith("model")) 117 | { 118 | printServoId("Model number of"); 119 | dxlCom.readModelNumber(_id); 120 | printDxlResult(); 121 | } 122 | else if (_readString.startsWith("firmware")) 123 | { 124 | printServoId("Firmware number of"); 125 | dxlCom.readFirmware(_id); 126 | printDxlResult(); 127 | } 128 | else if (_readString.startsWith("setID")) 129 | { 130 | _readString.remove(0, 5); 131 | int newID = _readString.toInt(); //convert readString into a number 132 | printServoId("Setting"); 133 | Serial.print(_id); 134 | Serial.print(" to "); 135 | Serial.print(newID); 136 | Serial.print(" : "); 137 | dxlCom.setId(_id, newID); 138 | printDxlResult(); 139 | } 140 | else if (_readString.startsWith("led")) 141 | { 142 | _readString.remove(0, 3); 143 | bool Status = _readString.toInt(); //convert readString into a number 144 | printServoId("Changing led status of "); 145 | dxlCom.setLedEnable(_id,Status); 146 | printDxlResult(); 147 | } 148 | else if (_readString.startsWith("move")) 149 | { 150 | _readString.remove(0, 4); 151 | unsigned short Position = _readString.toInt(); //convert readString into a number 152 | printServoId("Moving "); 153 | dxlCom.setGoalPosition(_id,Position); 154 | printDxlResult(); 155 | 156 | bool isMoving = true; 157 | 158 | while (isMoving) 159 | { 160 | unsigned short error = DXL_ERR_SUCCESS; 161 | while(dxlCom.isBusy()); // waiting the status return delay time 162 | dxlCom.readPresentPosition(_id); 163 | Serial.print("Pos : "); 164 | while(!dxlCom.dxlDataReady()); // waiting the answer of servo 165 | error = dxlCom.readDxlError(); 166 | if(error!=DXL_ERR_SUCCESS) // readDxlResult should always be called before readDxlData 167 | printDxlError(error); 168 | Serial.println(dxlCom.readDxlResult()); 169 | while(dxlCom.isBusy()); // waiting the status return delay time (for testing if it is moving) 170 | dxlCom.isMoving(_id); 171 | while(!dxlCom.dxlDataReady()); // waiting the answer of servo 172 | error = dxlCom.readDxlError(); 173 | if(error!=DXL_ERR_SUCCESS) // readDxlResult should always be called before readDxlData 174 | printDxlError(error); 175 | isMoving = dxlCom.readDxlResult(); 176 | } 177 | 178 | } 179 | else if (_readString.startsWith("speed")) 180 | { 181 | _readString.remove(0, 5); 182 | unsigned short Speed = _readString.toInt(); //convert readString into a number 183 | printServoId("Set speed of "); 184 | dxlCom.setMovingSpeed(_id,Speed); 185 | printDxlResult(); 186 | } 187 | else if (_readString.startsWith("torque")) 188 | { 189 | _readString.remove(0, 6); 190 | unsigned short torque = _readString.toInt(); //convert readString into a number 191 | printServoId("Set torque of "); 192 | dxlCom.setTorqueLimit(_id,torque); 193 | printDxlResult(); 194 | } 195 | else if (_readString.startsWith("voltage")) 196 | { 197 | printServoId("Voltage (to be divided by 10) of"); 198 | dxlCom.readVoltage(_id); 199 | printDxlResult(); 200 | } 201 | else if (_readString.startsWith("temperature")) 202 | { 203 | printServoId("Temperature of"); 204 | dxlCom.readTemperature(_id); 205 | printDxlResult(); 206 | } 207 | else if (_readString.startsWith("regmove")) 208 | { 209 | _readString.remove(0, 7); 210 | unsigned short Position = _readString.toInt(); //convert readString into a number 211 | printServoId("Write command (type 'action' to execute) in REG register of "); 212 | dxlCom.sendDxlRegData(_id, DXL_ADD_GOAL_POSITION, (const byte*) &Position, 2 ); 213 | printDxlResult(); 214 | } 215 | 216 | _readString=""; //empty for next input 217 | } 218 | 219 | } 220 | 221 | ///////////////////////////////////////////////////////////////////////////////////// 222 | 223 | void printDxlResult() 224 | { 225 | while(!dxlCom.dxlDataReady()); // waiting the answer of servo 226 | printDxlError(dxlCom.readDxlError()); 227 | Serial.println(dxlCom.readDxlResult()); 228 | } 229 | 230 | void printServoId(String msg) 231 | { 232 | Serial.print(msg); 233 | Serial.print(" servo ID "); 234 | Serial.print(_id); 235 | Serial.print(" - "); 236 | } 237 | 238 | void printDxlError(unsigned short dxlError) 239 | { 240 | // after any operation error can be retrieve using dx::readDxlResult() (i.e. after read or write operation) 241 | if(dxlError == DXL_ERR_SUCCESS) 242 | Serial.println("OK"); 243 | else 244 | { 245 | if (dxlError & DXL_ERR_VOLTAGE) 246 | Serial.print("voltage out of range-"); 247 | if (dxlError & DXL_ERR_ANGLE) 248 | Serial.print("angle out of range-"); 249 | if (dxlError & DXL_ERR_OVERHEATING) 250 | Serial.print("overheating-"); 251 | if (dxlError & DXL_ERR_RANGE) 252 | Serial.print("cmd out of range-"); 253 | if (dxlError & DXL_ERR_TX_CHECKSUM) 254 | Serial.print("Tx CRC invalid-"); 255 | if (dxlError & DXL_ERR_OVERLOAD ) 256 | Serial.print("overload-"); 257 | if (dxlError & DXL_ERR_INSTRUCTION ) 258 | Serial.print("undefined instruction-"); 259 | if (dxlError & DXL_ERR_TX_FAIL ) 260 | Serial.print("Tx No header-"); 261 | if (dxlError & DXL_ERR_RX_FAIL ) 262 | Serial.print("Rx No header-"); 263 | if (dxlError & DXL_ERR_TX_ERROR ) 264 | Serial.print("Tx error-"); 265 | if (dxlError & DXL_ERR_RX_LENGTH ) 266 | Serial.print("Rx length invalid-"); // Not implemented yet 267 | if (dxlError & DXL_ERR_RX_TIMEOUT) 268 | Serial.print("timeout-"); 269 | if (dxlError & DXL_ERR_RX_CORRUPT) 270 | Serial.print("Rx CRC invalid-"); 271 | if (dxlError & DXL_ERR_ID ) 272 | Serial.print("Wrong ID answered-"); // ?? Hardware issue 273 | Serial.println(); 274 | } 275 | } 276 | -------------------------------------------------------------------------------- /examples/DynamixelMxMonitorBlock/DynamixelMxMonitorBlock.ino: -------------------------------------------------------------------------------- 1 | /* 2 | DynamixelMxMonitorBlock.ino 3 | written by Akira 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | 19 | ***************************************************************************** 20 | Decription: 21 | This library implement all the required operation to drive Dynamixel servo, 22 | Please visit http://support.robotis.com/en/product/dynamixel/dxl_communication.htm to understand Dynamixel communication protocol 23 | 24 | Hardware: 25 | - This example use Serial1 port, so an Arduino with this port is required. UART Tx & Rx pin should be respectively connected to MAX485 DI & RO pin. 26 | - Pin 53 is used to switch between Tx & Rx mode. It should be connected to !RE & DE simultaneously. 27 | - Please check the servo baudrate, it has been set to 400 000bps here. 28 | - Take care that the defaut servo ID is 2. 29 | In this example, we will wait after each Dynamixel command the answer of the servo. 30 | The code is simplier than in non blocking implemantation, but it will block any other code (except interrupt) during this time. 31 | */ 32 | #include 33 | #include 34 | 35 | dxlMx dxlCom(&hdSerial1); // using Serial1 (Tx 18, Rx 19) 36 | 37 | String _readString; // Input string from serial monitor 38 | bool _strComplete = false; 39 | int _id = 2; 40 | 41 | 42 | void printServoId(String msg); 43 | void printDxlResult(); 44 | void printDxlError(unsigned short dxlError); 45 | 46 | void setup() { 47 | 48 | // Open serial communications and wait for port to open (PC communication) 49 | Serial.begin(57600); 50 | while (!Serial) { 51 | ; // wait for serial port to connect. Needed for native USB port only 52 | } 53 | 54 | Serial.println("Starting COM!"); 55 | 56 | pinMode(19,INPUT_PULLUP); // To be replaced by hardware pull up 57 | 58 | #if !defined (__SAM3X8E__) 59 | hdSerial1.setDirPin(53); // dirPin pin 53, 60 | #endif 61 | 62 | 63 | dxlCom.begin(400000); 64 | 65 | } 66 | 67 | ///////////////////////////////////////////////////////////////////////////////////// 68 | 69 | void loop() 70 | { 71 | while (Serial.available()) 72 | { 73 | char inputChar = Serial.read(); //gets one byte from serial buffer 74 | _readString += inputChar; //makes the string readString 75 | 76 | if (inputChar=='\n') 77 | _strComplete = true; 78 | 79 | } 80 | 81 | if (_strComplete) 82 | { 83 | _strComplete = false; 84 | if (_readString.startsWith("ID")) 85 | { 86 | _readString.remove(0, 2); 87 | _id = _readString.toInt(); //convert readString into a number 88 | printServoId("Communicating with"); 89 | Serial.println(_id); 90 | } 91 | else if (_readString.startsWith("ping")) 92 | { 93 | printServoId("Ping"); 94 | dxlCom.ping(_id); 95 | printDxlResult(); 96 | } 97 | else if (_readString.startsWith("action")) 98 | { 99 | dxlCom.isRegistered(_id); 100 | while(!dxlCom.dxlDataReady()); // waiting the answer of servo 101 | printDxlError(dxlCom.readDxlError()); 102 | if (dxlCom.readDxlResult()) // if it is registred 103 | { 104 | printServoId("Execute reg command in"); 105 | while(dxlCom.isBusy()); // waiting the status return delay time 106 | dxlCom.action(_id); 107 | printDxlResult(); 108 | } 109 | else 110 | { 111 | printServoId("No reg command in"); 112 | Serial.println(); 113 | } 114 | } 115 | else if (_readString.startsWith("reboot")) 116 | { 117 | printServoId("Reboot (not supported by MX)"); 118 | dxlCom.reboot(_id); 119 | printDxlResult(); 120 | } 121 | else if (_readString.startsWith("model")) 122 | { 123 | printServoId("Model number of"); 124 | dxlCom.readModelNumber(_id); 125 | printDxlResult(); 126 | } 127 | else if (_readString.startsWith("firmware")) 128 | { 129 | printServoId("Firmware number of"); 130 | dxlCom.readFirmware(_id); 131 | printDxlResult(); 132 | } 133 | else if (_readString.startsWith("setID")) 134 | { 135 | _readString.remove(0, 5); 136 | int newID = _readString.toInt(); //convert readString into a number 137 | printServoId("Setting"); 138 | Serial.print(_id); 139 | Serial.print(" to "); 140 | Serial.print(newID); 141 | Serial.print(" : "); 142 | dxlCom.setId(_id, newID); 143 | printDxlResult(); 144 | } 145 | else if (_readString.startsWith("led")) 146 | { 147 | _readString.remove(0, 3); 148 | bool Status = _readString.toInt(); //convert readString into a number 149 | printServoId("Changing led status of "); 150 | dxlCom.setLedEnable(_id,Status); 151 | printDxlResult(); 152 | } 153 | else if (_readString.startsWith("move")) 154 | { 155 | _readString.remove(0, 4); 156 | unsigned short Position = _readString.toInt(); //convert readString into a number 157 | printServoId("Moving "); 158 | dxlCom.setGoalPosition(_id,Position); 159 | printDxlResult(); 160 | 161 | bool isMoving = true; 162 | 163 | while (isMoving) 164 | { 165 | unsigned short error = DXL_ERR_SUCCESS; 166 | while(dxlCom.isBusy()); // waiting the status return delay time 167 | dxlCom.readPresentPosition(_id); 168 | Serial.print("Pos : "); 169 | while(!dxlCom.dxlDataReady()); // waiting the answer of servo 170 | error = dxlCom.readDxlError(); 171 | if(error!=DXL_ERR_SUCCESS) // readDxlResult should always be called before readDxlData 172 | printDxlError(error); 173 | Serial.println(dxlCom.readDxlResult()); 174 | while(dxlCom.isBusy()); // waiting the status return delay time (for testing if it is moving) 175 | dxlCom.isMoving(_id); 176 | while(!dxlCom.dxlDataReady()); // waiting the answer of servo 177 | error = dxlCom.readDxlError(); 178 | if(error!=DXL_ERR_SUCCESS) // readDxlResult should always be called before readDxlData 179 | printDxlError(error); 180 | isMoving = dxlCom.readDxlResult(); 181 | } 182 | 183 | } 184 | else if (_readString.startsWith("speed")) 185 | { 186 | _readString.remove(0, 5); 187 | unsigned short Speed = _readString.toInt(); //convert readString into a number 188 | printServoId("Set speed of "); 189 | dxlCom.setMovingSpeed(_id,Speed); 190 | printDxlResult(); 191 | } 192 | else if (_readString.startsWith("torque")) 193 | { 194 | _readString.remove(0, 6); 195 | unsigned short torque = _readString.toInt(); //convert readString into a number 196 | printServoId("Set torque of "); 197 | dxlCom.setTorqueLimit(_id,torque); 198 | printDxlResult(); 199 | } 200 | else if (_readString.startsWith("voltage")) 201 | { 202 | printServoId("Voltage (to be divided by 10) of"); 203 | dxlCom.readVoltage(_id); 204 | printDxlResult(); 205 | } 206 | else if (_readString.startsWith("temperature")) 207 | { 208 | printServoId("Temperature of"); 209 | dxlCom.readTemperature(_id); 210 | printDxlResult(); 211 | } 212 | else if (_readString.startsWith("regmove")) 213 | { 214 | _readString.remove(0, 7); 215 | unsigned short Position = _readString.toInt(); //convert readString into a number 216 | printServoId("Write command (type 'action' to execute) in REG register of "); 217 | dxlCom.sendDxlRegData(_id, DXL_ADD_GOAL_POSITION, (const byte*) &Position, 2 ); 218 | printDxlResult(); 219 | } 220 | 221 | _readString=""; //empty for next input 222 | } 223 | 224 | } 225 | 226 | ///////////////////////////////////////////////////////////////////////////////////// 227 | 228 | void printDxlResult() 229 | { 230 | while(!dxlCom.dxlDataReady()); // waiting the answer of servo 231 | printDxlError(dxlCom.readDxlError()); 232 | Serial.println(dxlCom.readDxlResult()); 233 | } 234 | 235 | void printServoId(String msg) 236 | { 237 | Serial.print(msg); 238 | Serial.print(" servo ID "); 239 | Serial.print(_id); 240 | Serial.print(" - "); 241 | } 242 | 243 | void printDxlError(unsigned short dxlError) 244 | { 245 | // after any operation error can be retrieve using dx::readDxlResult() (i.e. after read or write operation) 246 | if(dxlError == DXL_ERR_SUCCESS) 247 | Serial.println("OK"); 248 | else 249 | { 250 | if (dxlError & DXL_ERR_VOLTAGE) 251 | Serial.print("voltage out of range-"); 252 | if (dxlError & DXL_ERR_ANGLE) 253 | Serial.print("angle out of range-"); 254 | if (dxlError & DXL_ERR_OVERHEATING) 255 | Serial.print("overheating-"); 256 | if (dxlError & DXL_ERR_RANGE) 257 | Serial.print("cmd out of range-"); 258 | if (dxlError & DXL_ERR_TX_CHECKSUM) 259 | Serial.print("Tx CRC invalid-"); 260 | if (dxlError & DXL_ERR_OVERLOAD ) 261 | Serial.print("overload-"); 262 | if (dxlError & DXL_ERR_INSTRUCTION ) 263 | Serial.print("undefined instruction-"); 264 | if (dxlError & DXL_ERR_TX_FAIL ) 265 | Serial.print("Tx No header-"); 266 | if (dxlError & DXL_ERR_RX_FAIL ) 267 | Serial.print("Rx No header-"); 268 | if (dxlError & DXL_ERR_TX_ERROR ) 269 | Serial.print("Tx error-"); 270 | if (dxlError & DXL_ERR_RX_LENGTH ) 271 | Serial.print("Rx length invalid-"); // Not implemented yet 272 | if (dxlError & DXL_ERR_RX_TIMEOUT) 273 | Serial.print("timeout-"); 274 | if (dxlError & DXL_ERR_RX_CORRUPT) 275 | Serial.print("Rx CRC invalid-"); 276 | if (dxlError & DXL_ERR_ID ) 277 | Serial.print("Wrong ID answered-"); // ?? Hardware issue 278 | Serial.println(); 279 | } 280 | } 281 | -------------------------------------------------------------------------------- /examples/DynamixelMxMonitorNoBlock/DynamixelMxMonitorNoBlock.ino: -------------------------------------------------------------------------------- 1 | /* 2 | DynamixelMxMonitorNoBlock.ino 3 | written by Akira 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | 19 | ***************************************************************************** 20 | Decription: 21 | This library implement all the required operation to drive Dynamixel servo, 22 | Please visit http://support.robotis.com/en/product/dynamixel/dxl_communication.htm to understand Dynamixel communication protocol 23 | 24 | Hardware: 25 | - This example use Serial1 port, so an Arduino with this port is required. UART Tx & Rx pin should be respectively connected to MAX485 DI & RO pin. 26 | - Pin 53 is used to switch between Tx & Rx mode. It should be connected to !RE & DE simultaneously. 27 | - Please check the servo baudrate, it has been set to 400 000bps here. 28 | - Take care that the defaut servo ID is 2. 29 | */ 30 | #include 31 | #include 32 | 33 | #define TRACKING_IDLE 0 34 | #define TRACKING_MOVING 1 35 | #define TRACKING_POSITION 2 36 | 37 | dxlMx dxlCom(&hdSerial1); // using Serial1 (Tx 18, Rx 19) 38 | 39 | String _readString; // Input string from serial monitor 40 | bool _strComplete = false; 41 | int _id = 2; 42 | bool _trackDxlPosition = false; 43 | byte _trackingState = TRACKING_IDLE; 44 | bool _action = false; 45 | 46 | void printServoId(String msg); 47 | void processUserInput(); 48 | void processDxlData(); 49 | void trackPositon(); 50 | 51 | 52 | void setup() { 53 | 54 | // Open serial communications and wait for port to open (PC communication) 55 | Serial.begin(57600); 56 | while (!Serial) { 57 | ; // wait for serial port to connect. Needed for native USB port only 58 | } 59 | 60 | Serial.println("Starting COM!"); 61 | 62 | pinMode(19,INPUT_PULLUP); // To be replaced by hardware pull up 63 | 64 | // On Arduino Due, only the corresponding USART RTS pin is used to drive the controller 65 | #if !defined (__SAM3X8E__) 66 | hdSerial1.setDirPin(53); // dirPin pin 53, 67 | #endif 68 | 69 | dxlCom.begin(400000); 70 | 71 | } 72 | 73 | ///////////////////////////////////////////////////////////////////////////////////// 74 | 75 | void loop() 76 | { 77 | processUserInput(); 78 | processDxlData(); 79 | } 80 | 81 | ///////////////////////////////////////////////////////////////////////////////////// 82 | 83 | void processDxlData() 84 | { 85 | if (dxlCom.dxlDataReady()) 86 | { 87 | if(_trackDxlPosition) 88 | trackPosition(); 89 | else if (_action) 90 | action(); 91 | else 92 | { 93 | //unsigned short dxlError = dxlCom.readDxlError(); 94 | if(dxlCom.readDxlError() == DXL_ERR_SUCCESS) 95 | { 96 | Serial.print("Rx Status OK - "); 97 | Serial.println(dxlCom.readDxlResult()); 98 | } 99 | else 100 | printDxlError(dxlCom.readDxlError()); 101 | } 102 | 103 | 104 | } // dxlCom.dxlDataReady() 105 | } 106 | 107 | void trackPosition() 108 | { 109 | if(dxlCom.isBusy()) 110 | return; 111 | 112 | //unsigned short dxlError = dxlCom.readDxlError(); 113 | if(dxlCom.readDxlError() == DXL_ERR_SUCCESS) 114 | { 115 | switch (_trackingState) 116 | { 117 | case TRACKING_IDLE: 118 | dxlCom.isMoving(_id); 119 | _trackingState = TRACKING_MOVING; 120 | break; 121 | 122 | case TRACKING_MOVING: 123 | if(dxlCom.readDxlResult()) // dxl is moving 124 | { 125 | dxlCom.readPresentPosition(_id); 126 | _trackingState = TRACKING_POSITION; 127 | } 128 | else 129 | { 130 | _trackDxlPosition = false; 131 | _trackingState = TRACKING_IDLE; 132 | } 133 | break; 134 | 135 | case TRACKING_POSITION: 136 | Serial.print("Pos : "); Serial.println(dxlCom.readDxlResult()); 137 | dxlCom.isMoving(_id); 138 | _trackingState = TRACKING_MOVING; 139 | break; 140 | } //switch (_trackingState) 141 | } // if(dxlError == DXL_ERR_SUCCESS) 142 | else 143 | printDxlError(dxlCom.readDxlError()); 144 | } 145 | 146 | void action() 147 | { 148 | if(dxlCom.isBusy()) 149 | return; 150 | 151 | if(dxlCom.readDxlError() == DXL_ERR_SUCCESS) 152 | if(dxlCom.readDxlResult()) // dxl is registred 153 | { 154 | printServoId("Execute reg command in"); 155 | dxlCom.action(_id); 156 | _action = false; // no need to check the result of action 157 | } 158 | else 159 | { 160 | printServoId("No reg command in"); 161 | Serial.println(); 162 | } 163 | else 164 | printDxlError(dxlCom.readDxlError()); 165 | } 166 | 167 | void processUserInput() 168 | { 169 | while (Serial.available()) 170 | { 171 | char inputChar = Serial.read(); //gets one byte from serial buffer 172 | _readString += inputChar; //makes the string readString 173 | 174 | if (inputChar=='\n') 175 | _strComplete = true; 176 | 177 | } 178 | 179 | if (_strComplete) 180 | { 181 | _strComplete = false; 182 | if (_readString.startsWith("ID")) 183 | { 184 | _readString.remove(0, 2); 185 | _id = _readString.toInt(); //convert readString into a number 186 | printServoId("Communicating with"); 187 | Serial.println(_id); 188 | } 189 | else if (_readString.startsWith("ping")) 190 | { 191 | printServoId("Ping"); 192 | dxlCom.ping(_id); 193 | } 194 | else if (_readString.startsWith("action")) 195 | { 196 | _action = dxlCom.isRegistered(_id); // return true if transmit succeed 197 | } 198 | else if (_readString.startsWith("reboot")) 199 | { 200 | printServoId("Reboot (not supported by MX)"); 201 | dxlCom.reboot(_id); 202 | } 203 | else if (_readString.startsWith("model")) 204 | { 205 | printServoId("Model number of"); 206 | dxlCom.readModelNumber(_id); 207 | } 208 | else if (_readString.startsWith("firmware")) 209 | { 210 | printServoId("Firmware number of"); 211 | dxlCom.readFirmware(_id); 212 | } 213 | else if (_readString.startsWith("setID")) 214 | { 215 | _readString.remove(0, 5); 216 | int newID = _readString.toInt(); //convert readString into a number 217 | printServoId("Setting"); 218 | Serial.print(_id); 219 | Serial.print(" to "); 220 | Serial.print(newID); 221 | Serial.print(" : "); 222 | dxlCom.setId(_id, newID); 223 | } 224 | else if (_readString.startsWith("led")) 225 | { 226 | _readString.remove(0, 3); 227 | bool Status = _readString.toInt(); //convert readString into a number 228 | printServoId("Changing led status of "); 229 | dxlCom.setLedEnable(_id,Status); 230 | } 231 | else if (_readString.startsWith("move")) 232 | { 233 | _readString.remove(0, 4); 234 | unsigned short Position = _readString.toInt(); //convert readString into a number 235 | printServoId("Moving "); 236 | dxlCom.setGoalPosition(_id,Position); 237 | _trackDxlPosition = true; 238 | } 239 | else if (_readString.startsWith("speed")) 240 | { 241 | _readString.remove(0, 5); 242 | unsigned short Speed = _readString.toInt(); //convert readString into a number 243 | printServoId("Set speed of "); 244 | dxlCom.setMovingSpeed(_id,Speed); 245 | } 246 | else if (_readString.startsWith("torque")) 247 | { 248 | _readString.remove(0, 6); 249 | unsigned short torque = _readString.toInt(); //convert readString into a number 250 | printServoId("Set torque of "); 251 | dxlCom.setTorqueLimit(_id,torque); 252 | } 253 | else if (_readString.startsWith("voltage")) 254 | { 255 | printServoId("Voltage (to be divided by 10) of"); 256 | dxlCom.readVoltage(_id); 257 | } 258 | else if (_readString.startsWith("temperature")) 259 | { 260 | printServoId("Temperature of"); 261 | dxlCom.readTemperature(_id); 262 | } 263 | else if (_readString.startsWith("regmove")) 264 | { 265 | _readString.remove(0, 7); 266 | unsigned short Position = _readString.toInt(); //convert readString into a number 267 | printServoId("Write command (type 'action' to execute) in REG register of "); 268 | dxlCom.sendDxlRegData(_id, DXL_ADD_GOAL_POSITION, (const byte*) &Position, 2 ); 269 | } 270 | 271 | _readString=""; //empty for next input 272 | } 273 | } 274 | void printServoId(String msg) 275 | { 276 | Serial.print(msg); 277 | Serial.print(" servo ID "); 278 | Serial.print(_id); 279 | Serial.print(" - "); 280 | } 281 | 282 | void printDxlError(unsigned short dxlError) 283 | { 284 | // after any operation error can be retrieve using dx::readDxlResult() (i.e. after read or write operation) 285 | if(dxlError == DXL_ERR_SUCCESS) 286 | Serial.println("OK"); 287 | else 288 | { 289 | if (dxlError & DXL_ERR_VOLTAGE) 290 | Serial.print("voltage out of range-"); 291 | if (dxlError & DXL_ERR_ANGLE) 292 | Serial.print("angle out of range-"); 293 | if (dxlError & DXL_ERR_OVERHEATING) 294 | Serial.print("overheating-"); 295 | if (dxlError & DXL_ERR_RANGE) 296 | Serial.print("cmd out of range-"); 297 | if (dxlError & DXL_ERR_TX_CHECKSUM) 298 | Serial.print("Tx CRC invalid-"); 299 | if (dxlError & DXL_ERR_OVERLOAD ) 300 | Serial.print("overload-"); 301 | if (dxlError & DXL_ERR_INSTRUCTION ) 302 | Serial.print("undefined instruction-"); 303 | if (dxlError & DXL_ERR_TX_FAIL ) 304 | Serial.print("Tx No header-"); 305 | if (dxlError & DXL_ERR_RX_FAIL ) 306 | Serial.print("Rx No header-"); 307 | if (dxlError & DXL_ERR_TX_ERROR ) 308 | Serial.print("Tx error-"); 309 | if (dxlError & DXL_ERR_RX_LENGTH ) 310 | Serial.print("Rx length invalid-"); // Not implemented yet 311 | if (dxlError & DXL_ERR_RX_TIMEOUT) 312 | Serial.print("timeout-"); 313 | if (dxlError & DXL_ERR_RX_CORRUPT) 314 | Serial.print("Rx CRC invalid-"); 315 | if (dxlError & DXL_ERR_ID ) 316 | Serial.print("Wrong ID answered-"); // ?? Hardware issue 317 | Serial.println(); 318 | } 319 | } 320 | -------------------------------------------------------------------------------- /src/Dynamixel.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Dynamixel.cpp 3 | written by Akira 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | 19 | ***************************************************************************** 20 | Decription: 21 | This library implement all the required operation to drive Dynamixel servo, 22 | Please visit http://support.robotis.com/en/product/dynamixel/dxl_communication.htm to understand Dynamixel communication protocol 23 | 24 | Limitations: 25 | - Baudrate is limited 57 600bps ~ 115 200bps for 16Mhz Arduino board with softHalfDuplexSerial, and ~ 400 000bps for 16Mhz Arduino board with hardHalfDuplexSerial 26 | - This library is non blocking, i.e. when a write or a read command occured, it will NOT wait the answer of the servo 27 | */ 28 | 29 | // 30 | // Includes 31 | // 32 | #include 33 | 34 | //////////////////////////////////////////////////////////////////////////////////////////////////// 35 | 36 | // 37 | // Constructor 38 | // 39 | dxl::dxl(halfDuplexSerial* port) : 40 | _port(port), _error(0), _nByteToBeRead(0), 41 | _currentId(0), _timeout(1000), _currentTimeout(0), 42 | _returnDelayTime(2000), _currentReturnDelayTime(0), _dxlResult(0) 43 | { 44 | } 45 | 46 | // 47 | // Destructor 48 | // 49 | dxl::~dxl() 50 | { 51 | _port->end(); 52 | } 53 | 54 | void dxl::begin(const unsigned long speed) 55 | { 56 | _port->begin(speed); 57 | } 58 | 59 | // 60 | // Dxl Commands 61 | // 62 | bool dxl::ping(const byte ID) 63 | { return (dxl::sendDxlCommand(ID, DXL_PING)); } 64 | 65 | bool dxl::action(const byte ID) 66 | { return (dxl::sendDxlCommand(ID, DXL_ACTION)); } 67 | 68 | bool dxl::reset(const byte ID) 69 | { return (dxl::sendDxlCommand(ID, DXL_RESET)); } 70 | 71 | bool dxl::reboot(const byte ID) 72 | { return (dxl::sendDxlCommand(ID, DXL_REBOOT)); } 73 | 74 | 75 | // 76 | // EEPROM commands 77 | // 78 | 79 | bool dxl::readModelNumber(const byte ID) 80 | { return sendDxlRead(ID, DXL_ADD_MODEL_NUMBER , 2); } 81 | 82 | bool dxl::readFirmware(const byte ID) 83 | { return sendDxlRead(ID, DXL_ADD_FIRMWARE , 1); } 84 | 85 | bool dxl::setId(const byte ID, const byte newID) 86 | { return sendDxlWrite(ID, DXL_ADD_ID, &newID, 1 );} 87 | bool dxl::readId(const byte ID) 88 | { return sendDxlRead(ID, DXL_ADD_ID , 1); } 89 | 90 | bool dxl::setBaudRate(const byte ID, const byte baudRate) 91 | { return sendDxlWrite(ID, DXL_ADD_BAUDRATE, &baudRate, 1 );} 92 | bool dxl::readBaudRate(const byte ID) 93 | { return sendDxlRead(ID, DXL_ADD_BAUDRATE , 1); } 94 | 95 | bool dxl::setReturnDelayTime(const byte ID, const byte returnDelayTime) 96 | { return sendDxlWrite(ID, DXL_ADD_RETURN_DELAY_TIME , &returnDelayTime, 1 );} 97 | bool dxl::readReturnDelayTime(const byte ID) 98 | { return sendDxlRead(ID, DXL_ADD_RETURN_DELAY_TIME , 1); } 99 | 100 | bool dxl::setCWAngleLimit(const byte ID, const unsigned short angle) 101 | { return sendDxlWrite(ID, DXL_ADD_CW_ANGLE_LIMIT ,(const byte*) &angle, 2 );} 102 | bool dxl::setCCWAngleLimit(const byte ID, const unsigned short angle) 103 | { return sendDxlWrite(ID, DXL_ADD_CCW_ANGLE_LIMIT , (const byte*) &angle, 2 );} 104 | 105 | bool dxl::readCWAngleLimit(const byte ID) 106 | { return sendDxlRead(ID, DXL_ADD_CW_ANGLE_LIMIT , 2); } 107 | bool dxl::readCCWAngleLimit(const byte ID) 108 | { return sendDxlRead(ID, DXL_ADD_CCW_ANGLE_LIMIT , 2); } 109 | 110 | bool dxl::setMaxTemperature(const byte ID, const byte maxTemperature) 111 | { return sendDxlWrite(ID, DXL_ADD_MAX_TEMPERATURE , &maxTemperature, 1 );} 112 | bool dxl::readMaxTemperature(const byte ID) 113 | { return sendDxlRead(ID, DXL_ADD_MAX_TEMPERATURE , 1); } 114 | 115 | bool dxl::setMinVoltage(const byte ID, const byte minVoltage) 116 | { return sendDxlWrite(ID, DXL_ADD_MIN_VOLTAGE , &minVoltage, 1 );} 117 | bool dxl::readMinVoltage(const byte ID) 118 | { return sendDxlRead(ID, DXL_ADD_MIN_VOLTAGE , 1); } 119 | 120 | bool dxl::setMaxVoltage(const byte ID, const byte maxVoltage) 121 | { return sendDxlWrite(ID, DXL_ADD_MAX_VOLTAGE , &maxVoltage, 1 );} 122 | bool dxl::readMaxVoltage(const byte ID) 123 | { return sendDxlRead(ID, DXL_ADD_MAX_VOLTAGE , 1); } 124 | 125 | bool dxl::setMaxTorque(const byte ID, const unsigned short maxTorque) 126 | { return sendDxlWrite(ID, DXL_ADD_MAX_TORQUE ,(const byte*) &maxTorque, 2 );} 127 | bool dxl::readMaxTorque(const byte ID) 128 | { return sendDxlRead(ID, DXL_ADD_MAX_TORQUE , 2); } 129 | 130 | bool dxl::setStatusReturnLevel(const byte ID, const byte Status) 131 | { return sendDxlWrite(ID, DXL_ADD_STATUS_RETURN_LEVEL , &Status, 1 );} 132 | bool dxl::readStatusReturnLevel(const byte ID) 133 | { return sendDxlRead(ID, DXL_ADD_STATUS_RETURN_LEVEL , 1); } 134 | 135 | bool dxl::setAlarmLed(const byte ID, const byte Status) 136 | { return sendDxlWrite(ID, DXL_ADD_ALARM_LED , &Status, 1 );} 137 | bool dxl::readAlarmLed(const byte ID) 138 | { return sendDxlRead(ID, DXL_ADD_ALARM_LED , 1); } 139 | 140 | bool dxl::setAlarmShutdown(const byte ID, const byte Status) 141 | { return sendDxlWrite(ID, DXL_ADD_ALARM_SHUTDOWN , &Status, 1 );} 142 | bool dxl::readAlarmShutdown(const byte ID) 143 | { return sendDxlRead(ID, DXL_ADD_ALARM_SHUTDOWN , 1); } 144 | 145 | 146 | // 147 | // RAM commands 148 | // 149 | bool dxl::setTorqueEnable(const byte ID, const bool Status) 150 | { return sendDxlWrite(ID, DXL_ADD_TORQUE_ENABLE, (const byte*) &Status, 1 );} 151 | bool dxl::readTorqueEnable(const byte ID) 152 | { return sendDxlRead(ID, DXL_ADD_TORQUE_ENABLE , 1); } 153 | 154 | bool dxl::setLedEnable(const byte ID, const bool Status) 155 | { return sendDxlWrite(ID, DXL_ADD_LED, (const byte*) &Status, 1 );} 156 | bool dxl::readLedEnable(const byte ID ) 157 | { return sendDxlRead(ID, DXL_ADD_LED , 1); } 158 | 159 | bool dxl::setGoalPosition(const byte ID, const short Position) 160 | { return sendDxlWrite(ID, DXL_ADD_GOAL_POSITION, (const byte*) &Position, 2 );} 161 | 162 | bool dxl::setGoalPositionAtSpeed(const byte ID, const short Position, const short Speed) 163 | { 164 | const short params[] = { Position, Speed }; 165 | return sendDxlWrite(ID, DXL_ADD_GOAL_POSITION, (const byte*) params, 4 ); 166 | } 167 | 168 | bool dxl::setMovingSpeed(const byte ID, const short Speed) 169 | { return sendDxlWrite(ID, DXL_ADD_MOVING_SPEED, (const byte*) &Speed, 2 );} 170 | 171 | bool dxl::setTorqueLimit(const byte ID, const short torque) 172 | { return sendDxlWrite(ID, DXL_ADD_TORQUE_LIMIT, (const byte*) &torque, 2 );} 173 | 174 | bool dxl::readPresentPosition(const byte ID) 175 | { return sendDxlRead(ID, DXL_ADD_PRESENT_POSITION , 2); } 176 | 177 | bool dxl::readPresentSpeed(const byte ID) 178 | { return sendDxlRead(ID, DXL_ADD_PRESENT_SPEED , 2); } 179 | 180 | bool dxl::readPresentLoad(const byte ID) 181 | { return sendDxlRead(ID, DXL_ADD_PRESENT_LOAD , 2); } 182 | 183 | bool dxl::readVoltage(const byte ID) 184 | { return sendDxlRead(ID, DXL_ADD_PRESENT_VOLTAGE , 1); } 185 | 186 | bool dxl::readTemperature(const byte ID) 187 | { return sendDxlRead(ID, DXL_ADD_PRESENT_TEMPERATURE, 1); } 188 | 189 | bool dxl::isRegistered(const byte ID) 190 | { return sendDxlRead(ID, DXL_ADD_REGISTERED , 1); } 191 | 192 | bool dxl::isMoving(const byte ID) 193 | { return sendDxlRead(ID, DXL_ADD_MOVING , 1); } 194 | 195 | bool dxl::setEEPROMLock(const byte ID, const bool lock) 196 | { return sendDxlWrite(ID, DXL_ADD_LOCK, (const byte*) &lock, 1 );} 197 | bool dxl::isEEPROMLock(const byte ID) 198 | { return sendDxlRead(ID, DXL_ADD_LOCK , 1); } 199 | 200 | bool dxl::setPunch(const byte ID, const unsigned short current) 201 | { return sendDxlWrite(ID, DXL_ADD_PUNCH ,(const byte*) ¤t, 2 );} 202 | bool dxl::readPunch(const byte ID) 203 | { return sendDxlRead(ID, DXL_ADD_PUNCH , 2); } 204 | 205 | 206 | // 207 | // Low Level Public Methods 208 | // 209 | 210 | bool dxl::sendDxlCommand(const byte ID, const byte dxlCommand) 211 | { 212 | byte sentence[6]; 213 | byte Checksum = (~(ID + 0x02 + dxlCommand )) & 0xFF; 214 | 215 | sentence[0] = sentence[1] = DXL_START; // 2 start bytes 216 | sentence[2] = ID; // Servo ID 217 | sentence[3] = 0x02; // length 218 | sentence[4] = dxlCommand; // dxl instruction 219 | sentence[5] = Checksum; // Checksum 220 | 221 | if(writeRaw(sentence, 6)) 222 | { 223 | _nByteToBeRead = 6; 224 | _currentId = ID; 225 | return true; 226 | } 227 | 228 | _nByteToBeRead = 0; 229 | _currentId = 0; 230 | return false; 231 | 232 | } 233 | 234 | 235 | bool dxl::sendDxlRegData(const byte ID, const byte dxlAddress, const byte *params, const byte nByteToBeWritten ) 236 | { 237 | byte i; 238 | const byte txDataLength = nByteToBeWritten + 3 ; // length is "number of parameters N +2" 239 | byte sentence[7 + nByteToBeWritten]; 240 | byte Checksum = 0; 241 | 242 | for (i = 0; i < nByteToBeWritten; i++) 243 | Checksum += params[i]; 244 | 245 | Checksum = (~(ID + txDataLength + DXL_REG_WRITE + dxlAddress + Checksum )) & 0xFF; 246 | 247 | sentence[0] = sentence[1] = DXL_START; // 2 start bytes 248 | sentence[2] = ID; // Servo ID 249 | sentence[3] = txDataLength; // length 250 | sentence[4] = DXL_REG_WRITE; // write instruction 251 | sentence[5] = dxlAddress; // adress to start 252 | 253 | for (i = 0; i < nByteToBeWritten; i++) // write params ( first one is address) 254 | sentence[i+6] = params[i]; 255 | 256 | sentence[i+6] = Checksum; 257 | 258 | if(writeRaw(sentence, txDataLength + 4)) 259 | { 260 | _nByteToBeRead = 6; 261 | _currentId = ID; 262 | return true; 263 | } 264 | 265 | _nByteToBeRead = 0; 266 | _currentId = 0; 267 | return false; 268 | 269 | } 270 | 271 | bool dxl::sendDxlWrite(const byte ID, const byte dxlAddress, const byte *params, const byte nByteToBeWritten) 272 | { 273 | byte i; 274 | const byte txDataLength = nByteToBeWritten + 3 ; // length is "number of parameters N +2" 275 | byte sentence[7 + nByteToBeWritten]; 276 | byte Checksum = 0; 277 | 278 | for (i = 0; i < nByteToBeWritten; i++) 279 | Checksum += params[i]; 280 | 281 | Checksum = (~(ID + txDataLength + DXL_WRITE_DATA + dxlAddress + Checksum )) & 0xFF; 282 | 283 | sentence[0] = sentence[1] = DXL_START; // 2 start bytes 284 | sentence[2] = ID; // Servo ID 285 | sentence[3] = txDataLength; // length 286 | sentence[4] = DXL_WRITE_DATA; // write instruction 287 | sentence[5] = dxlAddress; // adress to start 288 | 289 | for (i = 0; i < nByteToBeWritten; i++) // write params ( first one is address) 290 | sentence[i+6] = params[i]; 291 | 292 | sentence[i+6] = Checksum; 293 | 294 | if(writeRaw(sentence, txDataLength + 4)) 295 | { 296 | _nByteToBeRead = 6; 297 | _currentId = ID; 298 | return true; 299 | } 300 | 301 | _nByteToBeRead = 0; 302 | _currentId = 0; 303 | return false; 304 | 305 | } 306 | 307 | bool dxl::sendDxlRead(const byte ID, const byte dxlAddress, const byte nByteToBeRead) 308 | { 309 | 310 | byte Checksum = (~(ID + 4 + DXL_READ_DATA + dxlAddress + nByteToBeRead)) & 0xFF; 311 | byte sentence[] ={ DXL_START, DXL_START, ID, 0x04, DXL_READ_DATA, dxlAddress, nByteToBeRead, Checksum}; 312 | 313 | if(writeRaw(sentence, 8)) 314 | { 315 | _nByteToBeRead = 6 + nByteToBeRead; 316 | _currentId = ID; 317 | return true; 318 | } 319 | 320 | _nByteToBeRead = 0; 321 | _currentId = 0; 322 | return false; 323 | 324 | } 325 | 326 | bool dxl::isBusy() 327 | { 328 | if(micros() < _currentReturnDelayTime) 329 | return true; 330 | else 331 | return false; // We are waiting some answer 332 | } 333 | 334 | bool dxl::dxlDataReady() 335 | { 336 | if(_nByteToBeRead == 0) 337 | return false; 338 | 339 | if(_port->available()>=_nByteToBeRead) 340 | { 341 | if(_currentReturnDelayTime == 0) 342 | _currentReturnDelayTime = _returnDelayTime + micros(); 343 | 344 | return true; 345 | } 346 | 347 | if(millis()>=_currentTimeout ) 348 | { 349 | _error |= DXL_ERR_RX_TIMEOUT; 350 | return true; 351 | } 352 | 353 | return false; 354 | } 355 | 356 | unsigned short dxl::readDxlResult() 357 | { 358 | if(_nByteToBeRead == 0) 359 | return _dxlResult; 360 | else 361 | return readDxlData(); 362 | } 363 | 364 | unsigned short dxl::readDxlError() 365 | { 366 | if(_nByteToBeRead != 0) 367 | readDxlData(); 368 | 369 | return _error; 370 | } 371 | 372 | 373 | 374 | // 375 | // Private Methods 376 | // 377 | 378 | // The only function to write on serial port 379 | bool dxl::writeRaw( const byte *sentence, const byte nByteToBeWritten) 380 | { 381 | while(_port->available() > 0) 382 | _port->read(); // clear the Rx buffer 383 | 384 | _error = DXL_ERR_SUCCESS; // purge error 385 | _dxlResult = 0; // purge result 386 | 387 | if(_port-> write(sentence, nByteToBeWritten) == nByteToBeWritten) 388 | { 389 | _currentTimeout = millis() + _timeout; 390 | _currentReturnDelayTime = 0; 391 | return true; 392 | } 393 | else 394 | { 395 | _currentTimeout = 0; 396 | return false; 397 | } 398 | } 399 | 400 | // The only function to read on serial port 401 | unsigned short dxl::readDxlData() 402 | { 403 | if(_port->available()<_nByteToBeRead) 404 | { 405 | while(_port->available() > 0) 406 | _port->read(); // clear the Rx buffer 407 | _nByteToBeRead = 0; 408 | _currentTimeout = 0; 409 | _currentId = 0; 410 | return _error; 411 | } 412 | 413 | byte check = 0; // used to compute checksum 414 | byte header[5]; // 2 start byte + ID + length + Error 415 | byte nDataBytes = _nByteToBeRead - 6; 416 | byte result[nDataBytes]; // where the data will be saved 417 | 418 | // Read the incoming bytes 419 | header[0] = _port->read(); // 2 Starts Bytes 420 | header[1] = _port->read(); 421 | 422 | header[2] = _port->read(); // Servo ID 423 | header[3] = _port->read(); // Length 424 | header[4] = _port->read(); // Error 425 | 426 | for (byte i = 0; i < nDataBytes; i++) 427 | { 428 | result[i] = _port->read(); 429 | check += result[i]; 430 | } 431 | 432 | byte Checksum = _port->read(); // Checksum 433 | 434 | if (!( ( header[0] == DXL_START) & ( header[1] == DXL_START) )) 435 | _error |= DXL_ERR_RX_FAIL; // No header found 436 | 437 | if (header[2]!=_currentId) 438 | _error |=DXL_ERR_ID; // The response comes from another Servo ! 439 | 440 | if (header[3]!= nDataBytes + 2) 441 | { 442 | nDataBytes = header[3] - 2; 443 | _error |=DXL_ERR_RX_LENGTH; // Servo send an error we push it. To catch it error mask should be used 444 | } 445 | 446 | if (header[4]!=0) 447 | _error |=header[4]; // Servo send an error we push it. To catch it error mask should be used 448 | 449 | 450 | if (Checksum != ((~(header[2] + header[3] + header[4] + check )) & 0xFF )) 451 | _error |=DXL_ERR_RX_CORRUPT; // Checksum error 452 | 453 | if(nDataBytes == 0) 454 | _dxlResult = 0; 455 | else if(nDataBytes == 1) 456 | _dxlResult = (unsigned short) result[0]; 457 | else if(nDataBytes == 2) 458 | _dxlResult = (result[1] << 8) | result[0]; // (msb << 8) | lsb 459 | 460 | _nByteToBeRead = 0; 461 | _currentTimeout = 0; 462 | _currentId = 0; 463 | 464 | return _dxlResult ; // Returns the error, if OK, DXL_ERR_SUCCESS 465 | } 466 | 467 | --------------------------------------------------------------------------------