├── .dir-locals.el ├── .github ├── ISSUE_TEMPLATE │ ├── bug-report-or-feature-request.md │ └── config.yml └── workflows │ └── ci.yaml ├── .gitignore ├── .gitlab-ci.yml ├── LICENSE.txt ├── README.md ├── XYZrobotServo.cpp ├── XYZrobotServo.h ├── arm_animation.txt ├── examples ├── BlinkAll │ └── BlinkAll.ino ├── ChangeBaud │ └── ChangeBaud.ino ├── ChangeId │ └── ChangeId.ino ├── Compliance │ └── Compliance.ino ├── DetectServos │ └── DetectServos.ino ├── PlayAnimation │ └── PlayAnimation.ino ├── ReadEverything │ └── ReadEverything.ino ├── Rollback │ └── Rollback.ino ├── SetPosition │ └── SetPosition.ino └── SetSpeed │ └── SetSpeed.ino ├── keywords.txt ├── library.properties └── tests └── LibraryTest1 └── LibraryTest1.ino /.dir-locals.el: -------------------------------------------------------------------------------- 1 | ; Settings for the Emacs text editor. 2 | 3 | ( 4 | (c-default-style . "BSD") 5 | (c-mode . ((c-basic-offset . 2) (tab-width . 2) (indent-tabs-mode . nil))) 6 | (nil . ((fill-column . 80))) 7 | ("examples" . ( 8 | (nil . ((c-basic-offset . 2) (tab-width . 8) (fill-column . 65))) 9 | )) 10 | ) 11 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/bug-report-or-feature-request.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report or feature request 3 | about: Did you find a specific bug in the code for this project? Do you want to request 4 | a new feature? Please open an issue! 5 | title: '' 6 | labels: '' 7 | assignees: '' 8 | 9 | --- 10 | 11 | 12 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/config.yml: -------------------------------------------------------------------------------- 1 | blank_issues_enabled: false 2 | contact_links: 3 | - name: Pololu Forum 4 | url: https://forum.pololu.com/ 5 | about: Do you need help getting started? Can't get this code to work at all? Having problems with electronics? Please post on our forum! 6 | -------------------------------------------------------------------------------- /.github/workflows/ci.yaml: -------------------------------------------------------------------------------- 1 | name: "CI" 2 | on: 3 | pull_request: 4 | push: 5 | jobs: 6 | ci: 7 | runs-on: ubuntu-20.04 8 | steps: 9 | - name: Checkout this repository 10 | uses: actions/checkout@v2.3.4 11 | - name: Cache for arduino-ci 12 | uses: actions/cache@v2.1.3 13 | with: 14 | path: | 15 | ~/.arduino15 16 | key: ${{ runner.os }}-arduino 17 | - name: Install nix 18 | uses: cachix/install-nix-action@v12 19 | - run: nix-shell -I nixpkgs=channel:nixpkgs-unstable -p arduino-ci --run "arduino-ci" 20 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | /docs/ 2 | /out/ 3 | -------------------------------------------------------------------------------- /.gitlab-ci.yml: -------------------------------------------------------------------------------- 1 | image: $CI_REGISTRY_IMAGE/nixos/nix:2.3.6 2 | 3 | stages: 4 | - ci 5 | 6 | ci: 7 | stage: ci 8 | tags: 9 | - nix 10 | script: 11 | - nix-shell -I nixpkgs=channel:nixpkgs-unstable -p arduino-ci --run "arduino-ci" 12 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2017-2020 Pololu Corporation (www.pololu.com) 2 | 3 | Permission is hereby granted, free of charge, to any person 4 | obtaining a copy of this software and associated documentation 5 | files (the "Software"), to deal in the Software without 6 | restriction, including without limitation the rights to use, 7 | copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the 9 | Software is furnished to do so, subject to the following 10 | conditions: 11 | 12 | The above copyright notice and this permission notice shall be 13 | included in all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 16 | EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES 17 | OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 18 | NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 19 | HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 20 | WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR 22 | OTHER DEALINGS IN THE SOFTWARE. 23 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # XYZrobotServo: Pololu's Arduino library for the XYZrobot Smart Servo A1-16 2 | 3 | Version: 1.1.0
4 | Release date: 2017-11-17
5 | [www.pololu.com](https://www.pololu.com/) 6 | 7 | ## Summary 8 | 9 | This is a library for the Arduino IDE that helps interface with the [A1-16 smart servo][servo] from XYZrobot over serial. It works with the following products: 10 | 11 | - [XYZrobot Smart Servo A1-16][servo] 12 | - [XYZrobot 6 DOF Robotic Arm Kit][arm] 13 | - [XYZrobot Bolide Y-01 Advanced Humanoid Robot DIY Kit][robot] 14 | 15 | ## Supported platforms 16 | 17 | This library works on any Arduino-compatible board, but it works best on boards 18 | based on the ATmega32U4, such as our [A-Star 32U4 controllers][a-star], [Arduino 19 | Leonardo][leo], and [Arduino Micro][micro] due to the issues explained in the 20 | "Issues with receiving data" section below. 21 | 22 | ## Issues with receiving data 23 | 24 | This library can be used without reading any data back from the servos (see the 25 | SetPosition, and SetSpeed examples). However, if you want to read data from a 26 | servo—such as its speed, position, or current consumption—then there 27 | are some issues to consider. 28 | 29 | **Receiving data with SoftwareSerial:** On platforms that do not have a free 30 | hardware serial port, such as the Arduino Uno and most other ATmega328P-based 31 | Arduino-compatible boards, the examples in this library use the the 32 | SoftwareSerial library to send and receive serial data. Unfortunately, the 33 | SoftwareSerial library cannot reliably receive data at 115200 baud, and that is 34 | the default baud rate used by the servos. 35 | 36 | To get your sketch to receive data reliably with SoftwareSerial, you will have 37 | to lower the baud rate of the servos to 57600 or lower using this library's 38 | ChangeBaud example. 39 | 40 | Sending data with SoftwareSerial is not a problem. 41 | 42 | **Enabling a pull-up on RX:** To receive data, a pull-up is needed on your 43 | board's RX line because the servos do not pull their TX lines high while idle. 44 | Without a pull-up, the RX line would be floating and your system would receive 45 | junk bytes before the expected response packet from the servo. 46 | 47 | If you are using SoftwareSerial, the RX pull-up is probably enabled 48 | automatically, but it depends on what version of the Arduino IDE you are using 49 | and what board you are using. If you are using hardware serial, the pull-up is 50 | probably not enabled automatically, so you will either need to add a line of 51 | code to enable it, or add an external pull-up resistor to your setup. 52 | 53 | To enable a pull-up resistor on your RX pin, run this line of code after setting 54 | up the serial port: 55 | 56 | pinMode(rxPinNumber, INPUT_PULLUP); 57 | 58 | The `rxPinNumber` in the code above should be the number of your RX pin, for 59 | example `0`. 60 | 61 | If you are using the hardware serial port on an ATmega32U4-based board, this 62 | library's examples enable the pull-up on pin 0 for you. 63 | 64 | ## Connecting the hardware 65 | 66 | You will need to connect an appropriate power source to your servo or servos. 67 | See the datasheet for your servo for information on the voltage and current 68 | requirements of the power supply, and information about which pins to connect it 69 | to. 70 | 71 | To control the servo from your microcontroller board, you must connect the GND 72 | of the microcontroller board to the GND pin of the servo. You must also connect 73 | your board's TX line to the RX line of the servo. If you want to receive data 74 | from the servo, you must connect your board's RX line to the TX line of the 75 | servo. 76 | 77 | The example sketches for this library use a hardware serial port on your Arduino 78 | if one is available: if your Arduino environment defines 79 | `SERIAL_PORT_HARDWARE_OPEN`, the examples will use that port. Otherwise, it 80 | uses SoftwareSerial on pins 10 (RX) and 11 (TX). Therefore, the serial pins to 81 | use depend on which board you are using. 82 | 83 | | Microcontroller Board | Hardware serial? | MCU RX pin | MCU TX pin | 84 | |-----------------------|------------------|------------|------------| 85 | | A-Star 32U4 | Yes | 0 | 1 | 86 | | Arduino Leonardo | Yes | 0 | 1 | 87 | | Arduino Micro | Yes | 0 | 1 | 88 | | Arduino Mega 2560 | Yes | 19 | 18 | 89 | | Arduino Due | Yes | 19** | 18 | 90 | | Arduino Uno | No | 10 | 11 | 91 | | Arduino Yun | No | 10 | 11 | 92 | 93 | ** The Due's serial port is 3.3 V, so you should not connect it directly to 94 | the servo's 5 V TX line. You could use a voltage divider or level shifter. 95 | 96 | 97 | ## Installing this library 98 | 99 | If you are using version 1.6.2 or later of the [Arduino software (IDE)][ide], 100 | you can use the Library Manager to install this library: 101 | 102 | 1. In the Arduino IDE, open the "Sketch" menu, select "Include Library", then 103 | "Manage Libraries...". 104 | 2. Search for "XYZrobotServo". 105 | 3. Click the XYZrobotServo entry in the list. 106 | 4. Click "Install". 107 | 108 | If this does not work, you can manually install the library: 109 | 110 | 1. Download the 111 | [latest release archive from GitHub](https://github.com/pololu/xyzrobot-servo-arduino/releases) 112 | and decompress it. 113 | 2. Rename the folder "xyzrobot-servo-arduino-xxxx" to "XYZrobotServo". 114 | 3. Drag the "XYZrobotServo" folder into the "libraries" directory inside your 115 | Arduino sketchbook directory. You can view your sketchbook location by 116 | opening the "File" menu and selecting "Preferences" in the Arduino IDE. If 117 | there is not already a "libraries" folder in that location, you should make 118 | the folder yourself. 119 | 4. After installing the library, restart the Arduino IDE. 120 | 121 | 122 | ## Finding the examples 123 | 124 | Several example sketches are available that show how to use the library. You can 125 | access them from the Arduino IDE by opening the "File" menu, selecting 126 | "Examples", and then selecting "XYZrobotServo". If you cannot find these 127 | examples, the library was probably installed incorrectly and you should retry 128 | the installation instructions above. 129 | 130 | 131 | ## Detecting servos 132 | 133 | The first example we recommend running is the DetectServos example, which will 134 | detect all the servos that are connected to your board by attempting to read 135 | status information from them. If your servos are powered and connected 136 | correctly, it determines the baud rate, ID, and ACK policy of each servo, and 137 | prints it out to the Serial Monitor in the Arduino IDE. 138 | 139 | However, if you have not connected your board's RX line, this example will not 140 | work, so you should skip this step. 141 | 142 | 143 | ## Configuring your servos 144 | 145 | To make sure that this library and its examples can communicate properly with 146 | each of your servos, you might need to change some of the configuration options 147 | in the servo's non-volatile EEPROM memory. 148 | 149 | - **Baud rate:** This library's examples use 115200 baud, which is the default 150 | baud rate used by the servos. If your servo is using a different baud rate, you 151 | should either reconfigure your servo or change the baud rate used in the 152 | examples. 153 | - **Servo ID:** We recommend setting the IDs of your servos to a consecutive 154 | sequence starting at 1. If you have the [robot arm][arm], the IDs are probably set 155 | correctly already, so you don't need to worry about this. 156 | - **ACK policy:** The *ACK policy* setting determines which commands the servo 157 | responds to. This library and most of its examples assume that the servos are 158 | using their default ACK policy of 1, which means the servos only respond to the 159 | EEPROM Read, RAM Read, and STAT commands. If your servos are using a different 160 | ACK policy, that could cause issues. 161 | 162 | You should run the DetectServos example describe above before attempting to 163 | reconfigure any of these parameters. If you need to change a servo's ID, see 164 | the ChangeId example. If you need to change a servo's baud rate, see the 165 | ChangeBaud example. We do not yet have an example showing how to change the ACK 166 | policy. 167 | 168 | 169 | ## Getting your servos to move 170 | 171 | After you have configured your servos, you should try running a simple example 172 | to make sure you can move your servos. If you have the [XYZrobot arm][arm], you 173 | should run the RobotArmTest example. If you have a standalone servo, you should 174 | run the SetPosition or SetSpeed example. 175 | 176 | 177 | ## Documentation 178 | 179 | For complete documentation of this library, see the comments in `XYZrobotServo.h`. 180 | 181 | [servo]: https://www.pololu.com/product/3400 182 | [arm]: https://www.pololu.com/product/2743 183 | [robot]: https://www.pololu.com/product/2734 184 | [a-star]: https://www.pololu.com/category/149/a-star-programmable-controllers 185 | [leo]: https://www.pololu.com/product/2192 186 | [micro]: https://www.pololu.com/product/2188 187 | 188 | ## Version history 189 | 190 | * 1.1.0 (2017-11-17): 191 | - Added the BlinkAll example. 192 | - Better support for the Arduino Uno in the examples: 193 | - Fixed all examples that use the Serial Monitor to initialize its baud 194 | rate to 115200 instead of leaving it uninitialized. 195 | - Fixed the ChangeBaud example so that it does not try to read data at 115200 196 | baud using software serial, since that is unreliable. 197 | - Fixed the ChangeBaud and ChangeId examples to work better on boards 198 | that automatically reset when the Serial Monitor is opened. 199 | These examples now require user input from the serial monitor. 200 | - This release contains no changes to the actual library code. 201 | * 1.0.0 (2017-10-13): Original release. 202 | -------------------------------------------------------------------------------- /XYZrobotServo.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #define CMD_EEPROM_WRITE 0x01 4 | #define CMD_EEPROM_READ 0x02 5 | #define CMD_RAM_WRITE 0x03 6 | #define CMD_RAM_READ 0x04 7 | #define CMD_I_JOG 0x05 8 | #define CMD_S_JOG 0x06 9 | #define CMD_STAT 0x07 10 | #define CMD_ROLLBACK 0x08 11 | #define CMD_REBOOT 0x09 12 | 13 | #define SET_POSITION_CONTROL 0 14 | #define SET_SPEED_CONTROL 1 15 | #define SET_TORQUE_OFF 2 16 | #define SET_POSITION_CONTROL_SERVO_ON 3 17 | 18 | XYZrobotServo::XYZrobotServo(Stream & stream, uint8_t id) 19 | { 20 | this->stream = &stream; 21 | this->id = id; 22 | this->lastError = XYZrobotServoError::None; 23 | } 24 | 25 | void XYZrobotServo::eepromWrite(uint8_t startAddress, const uint8_t * data, uint8_t dataSize) 26 | { 27 | memoryWrite(CMD_EEPROM_WRITE, startAddress, data, dataSize); 28 | } 29 | 30 | void XYZrobotServo::eepromRead(uint8_t startAddress, uint8_t * data, uint8_t dataSize) 31 | { 32 | memoryRead(CMD_EEPROM_READ, startAddress, data, dataSize); 33 | } 34 | 35 | void XYZrobotServo::ramWrite(uint8_t startAddress, const uint8_t * data, uint8_t dataSize) 36 | { 37 | memoryWrite(CMD_RAM_WRITE, startAddress, data, dataSize); 38 | } 39 | 40 | void XYZrobotServo::ramRead(uint8_t startAddress, uint8_t * data, uint8_t dataSize) 41 | { 42 | memoryRead(CMD_RAM_READ, startAddress, data, dataSize); 43 | } 44 | 45 | void XYZrobotServo::writeBaudRateEeprom(XYZrobotServoBaudRate baud) 46 | { 47 | uint8_t b = (uint8_t)baud; 48 | memoryWrite(CMD_EEPROM_WRITE, 5, &b, 1); 49 | } 50 | 51 | XYZrobotServoBaudRate XYZrobotServo::readBaudRateEeprom() 52 | { 53 | uint8_t b = 0; 54 | memoryRead(CMD_EEPROM_READ, 5, &b, 1); 55 | return (XYZrobotServoBaudRate)b; 56 | } 57 | 58 | void XYZrobotServo::writeIdEeprom(uint8_t id) 59 | { 60 | memoryWrite(CMD_EEPROM_WRITE, 6, &id, 1); 61 | } 62 | 63 | uint8_t XYZrobotServo::readIdEeprom() 64 | { 65 | uint8_t id = 0; 66 | memoryRead(CMD_EEPROM_READ, 6, &id, 1); 67 | return id; 68 | } 69 | 70 | void XYZrobotServo::writeIdRam(uint8_t id) 71 | { 72 | memoryWrite(CMD_RAM_WRITE, 0, &id, 1); 73 | } 74 | 75 | void XYZrobotServo::writeAckPolicyEeprom(XYZrobotServoAckPolicy policy) 76 | { 77 | uint8_t p = (uint8_t)policy; 78 | eepromWrite(7, &p, 1); 79 | } 80 | 81 | XYZrobotServoAckPolicy XYZrobotServo::readAckPolicyEeprom() 82 | { 83 | uint8_t result = 0; 84 | eepromRead(7, &result, 1); 85 | return (XYZrobotServoAckPolicy)result; 86 | } 87 | 88 | void XYZrobotServo::writeAckPolicyRam(XYZrobotServoAckPolicy policy) 89 | { 90 | uint8_t p = (uint8_t)policy; 91 | ramWrite(1, &p, 1); 92 | } 93 | 94 | void XYZrobotServo::writeAlarmLedPolicyRam(uint8_t policy) 95 | { 96 | ramWrite(2, &policy, 1); 97 | } 98 | 99 | void XYZrobotServo::writeSpdctrlPolicyRam(XYZrobotServoSpdctrlPolicy policy) 100 | { 101 | uint8_t p = (uint8_t)policy; 102 | ramWrite(4, &p, 1); 103 | } 104 | 105 | void XYZrobotServo::writeMaxPwmRam(uint16_t value) 106 | { 107 | ramWrite(16, (uint8_t *)&value, 2); 108 | } 109 | 110 | void XYZrobotServo::writeLedControl(uint8_t control) 111 | { 112 | ramWrite(53, &control, 1); 113 | } 114 | 115 | XYZrobotServoAckPolicy XYZrobotServo::readAckPolicyRam() 116 | { 117 | uint8_t result = 0; 118 | ramRead(1, &result, 1); 119 | return (XYZrobotServoAckPolicy)result; 120 | } 121 | 122 | XYZrobotServoStatus XYZrobotServo::readStatus() 123 | { 124 | flushRead(); 125 | 126 | XYZrobotServoStatus status; 127 | sendRequest(CMD_STAT, NULL, 0); 128 | readAck(CMD_STAT, (uint8_t *)&status, 10); 129 | return status; 130 | } 131 | 132 | void XYZrobotServo::setPosition(uint16_t position, uint8_t playtime) 133 | { 134 | sendIJog(position, SET_POSITION_CONTROL, playtime); 135 | } 136 | 137 | void XYZrobotServo::setSpeed(int16_t speed, uint8_t playtime) 138 | { 139 | sendIJog(speed, SET_SPEED_CONTROL, playtime); 140 | } 141 | 142 | void XYZrobotServo::torqueOff() 143 | { 144 | sendIJog(0, SET_TORQUE_OFF, 0); 145 | } 146 | 147 | void XYZrobotServo::rollback() 148 | { 149 | sendRequest(CMD_ROLLBACK, NULL, 0); 150 | } 151 | 152 | void XYZrobotServo::reboot() 153 | { 154 | sendRequest(CMD_REBOOT, NULL, 0); 155 | } 156 | 157 | void XYZrobotServo::flushRead() 158 | { 159 | while(stream->available()) { stream->read(); } 160 | } 161 | 162 | void XYZrobotServo::sendRequest(uint8_t cmd, 163 | const uint8_t * data1, uint8_t data1Size, 164 | const uint8_t * data2, uint8_t data2Size) 165 | { 166 | uint8_t header[7]; 167 | 168 | uint8_t size = data1Size + data2Size + sizeof(header); 169 | 170 | uint8_t checksum = size ^ id ^ cmd; 171 | for (uint8_t i = 0; i < data1Size; i++) { checksum ^= data1[i]; } 172 | for (uint8_t i = 0; i < data2Size; i++) { checksum ^= data2[i]; } 173 | 174 | header[0] = 0xFF; 175 | header[1] = 0xFF; 176 | header[2] = size; 177 | header[3] = id; 178 | header[4] = cmd; 179 | header[5] = checksum & 0xFE; 180 | header[6] = ~checksum & 0xFE; 181 | 182 | stream->write(header, sizeof(header)); 183 | if (data1Size) { stream->write(data1, data1Size); } 184 | if (data2Size) { stream->write(data2, data2Size); } 185 | 186 | lastError = XYZrobotServoError::None; 187 | } 188 | 189 | void XYZrobotServo::readAck(uint8_t cmd, 190 | uint8_t * data1, uint8_t data1Size, 191 | uint8_t * data2, uint8_t data2Size) 192 | { 193 | // The CMD byte for an acknowledgment always has bit 6 set. 194 | cmd |= 0x40; 195 | 196 | uint8_t header[7]; 197 | 198 | uint8_t size = sizeof(header) + data1Size + data2Size; 199 | 200 | uint8_t byteCount = stream->readBytes(header, sizeof(header)); 201 | if (byteCount != sizeof(header)) 202 | { 203 | lastError = XYZrobotServoError::HeaderTimeout; 204 | return; 205 | } 206 | 207 | if (header[0] != 0xFF) 208 | { 209 | lastError = XYZrobotServoError::HeaderByte1Wrong; 210 | return; 211 | } 212 | 213 | if (header[1] != 0xFF) 214 | { 215 | lastError = XYZrobotServoError::HeaderByte2Wrong; 216 | return; 217 | } 218 | 219 | if (header[3] != id) 220 | { 221 | lastError = XYZrobotServoError::IdWrong; 222 | return; 223 | } 224 | 225 | if (header[4] != cmd) 226 | { 227 | lastError = XYZrobotServoError::CmdWrong; 228 | return; 229 | } 230 | 231 | if (header[2] != size) 232 | { 233 | lastError = XYZrobotServoError::SizeWrong; 234 | return; 235 | } 236 | 237 | if (data1Size) 238 | { 239 | byteCount = stream->readBytes(data1, data1Size); 240 | if (byteCount != data1Size) 241 | { 242 | lastError = XYZrobotServoError::Data1Timeout; 243 | return; 244 | } 245 | } 246 | 247 | if (data2Size) 248 | { 249 | byteCount = stream->readBytes(data2, data2Size); 250 | if (byteCount != data2Size) 251 | { 252 | lastError = XYZrobotServoError::Data2Timeout; 253 | return; 254 | } 255 | } 256 | 257 | uint8_t checksum = size ^ id ^ cmd; 258 | for (uint8_t i = 0; i < data1Size; i++) { checksum ^= data1[i]; } 259 | for (uint8_t i = 0; i < data2Size; i++) { checksum ^= data2[i]; } 260 | 261 | if (header[5] != (checksum & 0xFE)) 262 | { 263 | lastError = XYZrobotServoError::Checksum1Wrong; 264 | return; 265 | } 266 | 267 | if (header[6] != (~checksum & 0xFE)) 268 | { 269 | lastError = XYZrobotServoError::Checksum2Wrong; 270 | return; 271 | } 272 | 273 | lastError = XYZrobotServoError::None; 274 | } 275 | 276 | void XYZrobotServo::memoryWrite(uint8_t cmd, uint8_t startAddress, 277 | const uint8_t * data, uint8_t dataSize) 278 | { 279 | uint8_t request[2]; 280 | request[0] = startAddress; 281 | request[1] = dataSize; 282 | 283 | sendRequest(cmd, request, sizeof(request), data, dataSize); 284 | } 285 | 286 | void XYZrobotServo::memoryRead(uint8_t cmd, uint8_t startAddress, 287 | uint8_t * data, uint8_t dataSize) 288 | { 289 | flushRead(); 290 | 291 | uint8_t request[2]; 292 | request[0] = startAddress; 293 | request[1] = dataSize; 294 | sendRequest(cmd, request, sizeof(request)); 295 | 296 | uint8_t response[4]; 297 | readAck(cmd, response, 4, data, dataSize); 298 | if (getLastError()) { return; } 299 | 300 | // Despite what the A1-16 datasheet says, the first two bytes of the response 301 | // tend to 0, and the start address and data size come after that. 302 | 303 | if (response[2] != request[0]) 304 | { 305 | lastError = XYZrobotServoError::ReadOffsetWrong; 306 | return; 307 | } 308 | 309 | if (response[3] != request[1]) 310 | { 311 | lastError = XYZrobotServoError::ReadLengthWrong; 312 | return; 313 | } 314 | } 315 | 316 | void XYZrobotServo::sendIJog(uint16_t goal, uint8_t type, uint8_t playtime) 317 | { 318 | uint8_t data[5]; 319 | data[0] = goal & 0xFF; 320 | data[1] = goal >> 8 & 0xFF; 321 | data[2] = type; 322 | data[3] = id; 323 | data[4] = playtime; 324 | sendRequest(CMD_I_JOG, data, sizeof(data)); 325 | } 326 | -------------------------------------------------------------------------------- /XYZrobotServo.h: -------------------------------------------------------------------------------- 1 | // Copyright (C) Pololu Corporation. See LICENSE.txt for details. 2 | 3 | #pragma once 4 | 5 | #include 6 | 7 | /// The possible communication errors that can happen when reading the 8 | /// acknowledgment packet from a servo. 9 | enum class XYZrobotServoError 10 | { 11 | /// No error. 12 | None = 0, 13 | 14 | /// There was a timeout waiting to receive the 7-byte acknowledgment header. 15 | HeaderTimeout = 1, 16 | 17 | /// The first byte of received header was not 0xFF. 18 | HeaderByte1Wrong = 2, 19 | 20 | /// The second byte of the received header was not 0xFF. 21 | HeaderByte2Wrong = 3, 22 | 23 | /// The ID byte in the received header was wrong. 24 | IdWrong = 4, 25 | 26 | /// The CMD bytes in the received header was wrong. 27 | CmdWrong = 5, 28 | 29 | /// The size byte in the received header was wrong. 30 | SizeWrong = 6, 31 | 32 | /// There was a timeout reading the first expected block of data in the 33 | /// acknowledgment. 34 | Data1Timeout = 7, 35 | 36 | /// There was a timeout reading the second expected block of data in the 37 | /// acknowledgment. 38 | Data2Timeout = 8, 39 | 40 | /// The first byte of the checksum was wrong. 41 | Checksum1Wrong = 9, 42 | 43 | /// The second byte of the checksum was wrong. 44 | Checksum2Wrong = 10, 45 | 46 | /// The offset byte returned by an EEPROM Read or RAM Read command was wrong. 47 | ReadOffsetWrong = 16, 48 | 49 | /// The length byte returned by an EEPROM Read or RAM Read command was wrong. 50 | ReadLengthWrong = 17, 51 | }; 52 | 53 | enum class XYZrobotServoBaudRate 54 | { 55 | B9600 = 1, 56 | B19200 = 2, 57 | B57600 = 6, 58 | B115200 = 12, 59 | }; 60 | 61 | static inline uint32_t XYZrobotServoBaudRateToInt(XYZrobotServoBaudRate baud) 62 | { 63 | switch (baud) 64 | { 65 | case XYZrobotServoBaudRate::B9600: return 9600; 66 | case XYZrobotServoBaudRate::B19200: return 19200; 67 | case XYZrobotServoBaudRate::B57600: return 57600; 68 | case XYZrobotServoBaudRate::B115200: return 115200; 69 | default: return 0; 70 | } 71 | } 72 | 73 | /// The possible values for the ACK_Policy parameter stored in the servo's 74 | /// EEPROM and RAM. This parameter determins which commands the servo will send 75 | /// an acknowledgment response for. 76 | enum class XYZrobotServoAckPolicy 77 | { 78 | // The servo only responds to STAT commands. 79 | OnlyStat = 0, 80 | 81 | // The servo only responds to STAT, EEPROM Read, and RAM Read commands. 82 | OnlyReadAndStat = 1, 83 | 84 | // The servo responds to all commands. 85 | All = 2, 86 | }; 87 | 88 | enum class XYZrobotServoSpdctrlPolicy 89 | { 90 | OpenLoop = 0, 91 | CloseLoop = 1, 92 | }; 93 | 94 | /// This struct represents the data returned by a STAT command. 95 | struct XYZrobotServoStatus 96 | { 97 | uint8_t statusError; 98 | uint8_t statusDetail; 99 | uint16_t pwm; 100 | uint16_t posRef; 101 | uint16_t position; 102 | uint16_t iBus; 103 | } __attribute__((packed)); 104 | 105 | class XYZrobotServo { 106 | public: 107 | XYZrobotServo(Stream &, uint8_t id); 108 | 109 | /// Writes data from the specified buffer to the servo's EEPROM. 110 | /// 111 | /// After running this command, we recommend delaying for 10 ms per data byte 112 | /// before sending the next command to this servo, since writing to EEPROM 113 | /// takes some time and the servo cannot receive more commands until it is 114 | /// done. 115 | void eepromWrite(uint8_t startAddress, const uint8_t *, uint8_t dataSize); 116 | 117 | /// Reads data from the servo's EEPROM and stores it in the specified buffer. 118 | /// 119 | /// The data size should be 35 or less: otherwise the A1-16 seems to return a 120 | /// response with an invalid CRC. 121 | void eepromRead(uint8_t startAddress, uint8_t * data, uint8_t dataSize); 122 | 123 | /// Writes data from the specified buffer to the servo's RAM. 124 | void ramWrite(uint8_t startAddress, const uint8_t *, uint8_t dataSize); 125 | 126 | /// Reads data from the servo's RAM and stores it in the specified buffer. 127 | /// 128 | /// The data size should be 35 or less: otherwise the A1-16 seems to return a 129 | /// response with an invalid CRC. 130 | void ramRead(uint8_t startAddress, uint8_t * data, uint8_t dataSize); 131 | 132 | /// Write the Baud_Rate parameter byte in EEPROM, which determines which baud 133 | /// rate the servo uses on its serial interface. 134 | /// 135 | /// After running this command, we recommend delaying for 10 ms before sending 136 | /// the next command to this servo, since writing to EEPROM takes some time 137 | /// and the servo cannot receive more commands until it is done. 138 | void writeBaudRateEeprom(XYZrobotServoBaudRate baudRate); 139 | 140 | /// Reads the Baud_Rate parameter byte in EEPROM, which determines which baud 141 | /// rate the servo uses on its serial interface. 142 | XYZrobotServoBaudRate readBaudRateEeprom(); 143 | 144 | /// Write the sID parameter byte in EEPROM, which determines which ID the 145 | /// servo uses on its serial interface. 146 | /// 147 | /// After running this command, we recommend delaying for 10 ms before sending 148 | /// the next command to this servo, since writing to EEPROM takes some time 149 | /// and the servo cannot receive more commands until it is done. 150 | void writeIdEeprom(uint8_t id); 151 | 152 | /// Reads the sID parameter byte in EEPROM, which determines which ID the 153 | /// servo uses on its serial interface. 154 | uint8_t readIdEeprom(); 155 | 156 | /// Write the sID parameter byte in RAM, which determines which ID the 157 | /// servo uses on its serial interface. 158 | /// 159 | /// Write the ACK_Policy parameter byte in RAM. 160 | void writeIdRam(uint8_t id); 161 | 162 | /// Write the ACK_Policy parameter byte in EEPROM. 163 | /// 164 | /// After running this command, we recommend delaying for 10 ms before sending 165 | /// the next command to this servo, since writing to EEPROM takes some time 166 | /// and the servo cannot receive more commands until it is done. 167 | void writeAckPolicyEeprom(XYZrobotServoAckPolicy); 168 | 169 | /// Read the ACK_Policy parameter byte in EEPROM. 170 | XYZrobotServoAckPolicy readAckPolicyEeprom(); 171 | 172 | /// Write the ACK_Policy parameter byte in RAM. 173 | void writeAckPolicyRam(XYZrobotServoAckPolicy); 174 | 175 | /// Write the Alarm_LED_Policy byte in RAM. This controls which LEDs on the 176 | /// servo are controlled by the user and which are controlled by the system. 177 | /// 178 | /// A 0 bit means the LED is controlled by the system, and a 1 bit means the 179 | /// LED is controlled by the user. 180 | /// 181 | /// - Bit 0: White LED 182 | /// - Bit 1: Blue LED 183 | /// - Bit 2: Green LED 184 | /// - Bit 3: Red LED 185 | /// 186 | /// To control user LEDs, see writeLedControl(). 187 | void writeAlarmLedPolicyRam(uint8_t); 188 | 189 | /// Sets the SPDctrl_Policy variable in RAM. 190 | void writeSpdctrlPolicyRam(XYZrobotServoSpdctrlPolicy policy); 191 | 192 | /// Sets the maximum PWM value in RAM. 193 | /// 194 | /// This should be a number between 0 and 1023 that indicates how strong the 195 | /// servo should be allowed to drive its motor, with 1023 corresponding to 196 | /// 100%. 197 | void writeMaxPwmRam(uint16_t value); 198 | 199 | /// After calling writeAlarmLedPolicyRam(), you can use this to control any 200 | /// LEDs that are configured as user LED. 201 | /// 202 | /// - Bit 0: White LED 203 | /// - Bit 1: Blue LED 204 | /// - Bit 2: Green LED 205 | /// - Bit 3: Red LED 206 | void writeLedControl(uint8_t); 207 | 208 | /// Read the ACK_Policy parameter byte in RAM. 209 | XYZrobotServoAckPolicy readAckPolicyRam(); 210 | 211 | /// Sends a STAT command to the servo and returns the results. 212 | XYZrobotServoStatus readStatus(); 213 | 214 | /// Uses a STAT command to read the current PWM duty cycle. 215 | /// 216 | /// See readStatus(). 217 | uint16_t readPwm() { return readStatus().pwm; } 218 | 219 | /// Uses a STAT command to read the servo position. 220 | /// 221 | /// See readStatus(). 222 | uint16_t readPosition() { return readStatus().position; } 223 | 224 | /// Uses a STAT command to read the servo position goal. 225 | /// 226 | /// If the servo has no position goal, this is just its current measured 227 | /// position. 228 | /// 229 | /// See readStatus(). 230 | uint16_t readPosRef() { return readStatus().posRef; } 231 | 232 | /// Uses a STAT command to read the bus current. 233 | /// 234 | /// See readStatus(). 235 | uint16_t readIBus() { return readStatus().iBus; } 236 | 237 | /// Sends an I-JOG command to set the target/goal position for this servo. 238 | /// 239 | /// The position should be a number between 0 and 1023. 240 | /// 241 | /// The playtime should the desired time for the movement to take, in units of 242 | /// 10 ms. For example, a playtime of 50 would correspond to 500 ms or 0.5 243 | /// seconds. 244 | void setPosition(uint16_t position, uint8_t playtime = 0); 245 | 246 | /// Sends an I-JOG command to set the speed for this servo. 247 | /// 248 | /// The speed should be a number between -1023 and 1023. 249 | /// 250 | /// A value of 0 causes an abrupt stop. Non-zero values generally cause the 251 | /// servo to smoothly ramp its speed up or down to the specified value. 252 | /// 253 | /// The playtime specifies how long this command will last, in units of 10 ms. 254 | /// A value of 0 makes it last indefinitely. 255 | /// 256 | /// See writeSpeedControlPolicyRam(). 257 | void setSpeed(int16_t speed, uint8_t playtime = 0); 258 | 259 | /// Sends an I-JOG command to turn off the servo's motor. 260 | /// 261 | /// Note that this command interacts badly with the A1-16 servo's speed 262 | /// ramping feature. If you are in speed control mode and then send this 263 | /// command, the servo will still remember what speed it was using before it 264 | /// received the torqueOff() command. If you later send a setSpeed() command 265 | /// with a non-zero speed, it will ramp up or down to that speed, starting 266 | /// from the remembered speed instead of starting from zero. If you encounter 267 | /// this issue, you can call `setSpeed(0)` immediately before the next 268 | /// non-zero setSpeed() command to solve it. 269 | void torqueOff(); 270 | 271 | // Resets all parameters in EEPROM to their default values. 272 | // 273 | // After running this command, we recommend delaying for 2500 ms before 274 | // sending the next command to this servo, since it takes the servo a while to 275 | // change its parameters. 276 | void rollback(); 277 | 278 | // Resets the servo. 279 | // 280 | // After running this command, we recommend delaying for 2500 ms before 281 | // sending the next command to this servo, since it takes the servo a while to 282 | // restart. 283 | void reboot(); 284 | 285 | /// Returns the communication error from the last command. The return value 286 | /// will be 0 if there was no error and non-zero if there was an error. The 287 | /// return value will be one of the values of the XYZrobotServoError enum. 288 | uint8_t getLastError() const { return (uint8_t)lastError; } 289 | 290 | /// Get the servo ID assigned to this object. 291 | uint8_t getId() const { return id; } 292 | 293 | private: 294 | void flushRead(); 295 | 296 | void sendRequest(uint8_t cmd, 297 | const uint8_t * data1, uint8_t data1Size, 298 | const uint8_t * data2 = NULL, uint8_t data2Size = 0); 299 | 300 | void readAck(uint8_t cmd, 301 | uint8_t * data1, uint8_t data1Size, 302 | uint8_t * data2 = NULL, uint8_t data2Size = 0); 303 | 304 | void memoryWrite(uint8_t cmd, uint8_t startAddress, const uint8_t * data, uint8_t dataSize); 305 | 306 | void memoryRead(uint8_t cmd, uint8_t startAddress, uint8_t * data, uint8_t dataSize); 307 | 308 | void sendIJog(uint16_t goal, uint8_t type, uint8_t playtime); 309 | 310 | XYZrobotServoError lastError; 311 | 312 | uint8_t id; 313 | 314 | Stream * stream; 315 | }; 316 | 317 | 318 | -------------------------------------------------------------------------------- /arm_animation.txt: -------------------------------------------------------------------------------- 1 | Animation data we use at Pololu to test the PlayAnimation sketch: 2 | 3 | WARNING: Your arm might be constructed differently than ours, 4 | so if you try to use this, it might damage your arm. 5 | 6 | { 1000, 521, 535, 526, 507, 236, 662 }, 7 | { 1000, 466, 575, 559, 539, 236, 662 }, 8 | { 1000, 586, 567, 540, 512, 236, 662 }, 9 | { 1000, 595, 563, 696, 542, 236, 662 }, 10 | { 1000, 514, 533, 556, 463, 236, 662 }, 11 | 12 | -------------------------------------------------------------------------------- /examples/BlinkAll/BlinkAll.ino: -------------------------------------------------------------------------------- 1 | // This sketch shows how to blink the LEDs on all of the smart 2 | // servos attached to your board. 3 | // 4 | // Note that this sketch will leave your servo in a state where 5 | // the LEDs are controlled by the serial interface instead of 6 | // indicating the servo' status. To get your servos back to 7 | // normal after running this sketch, you will probably want to 8 | // power cycle them. 9 | // 10 | // This sketch only writes data to the servos; it does not 11 | // receive anything. 12 | 13 | #include 14 | 15 | // On boards with a hardware serial port available for use, use 16 | // that port. For other boards, create a SoftwareSerial object 17 | // using pin 10 to receive (RX) and pin 11 to transmit (TX). 18 | #ifdef SERIAL_PORT_HARDWARE_OPEN 19 | #define servoSerial SERIAL_PORT_HARDWARE_OPEN 20 | #else 21 | #include 22 | SoftwareSerial servoSerial(10, 11); 23 | #endif 24 | 25 | // Define a servo object that uses the broadcast address, so 26 | // commands that it sends will go to all the servos. 27 | XYZrobotServo servo(servoSerial, 254); 28 | 29 | void setup() 30 | { 31 | // Turn on the serial port and set its baud rate. 32 | servoSerial.begin(115200); 33 | } 34 | 35 | void setAllLedsAtThisBaudRate(uint8_t color) 36 | { 37 | delay(100); 38 | 39 | // Make all the LEDs be user-controlled. 40 | servo.writeAlarmLedPolicyRam(0b1111); 41 | 42 | // Turn on the specified LEDs. 43 | servo.writeLedControl(color); 44 | } 45 | 46 | void setAllLeds(uint8_t color) 47 | { 48 | servoSerial.begin(9600); 49 | setAllLedsAtThisBaudRate(color); 50 | 51 | servoSerial.begin(19200); 52 | setAllLedsAtThisBaudRate(color); 53 | 54 | servoSerial.begin(57600); 55 | setAllLedsAtThisBaudRate(color); 56 | 57 | servoSerial.begin(115200); 58 | setAllLedsAtThisBaudRate(color); 59 | } 60 | 61 | void loop() 62 | { 63 | delay(2500); 64 | 65 | // Try to make all the LEDs blue. 66 | setAllLeds(0b0010); 67 | 68 | delay(1000); 69 | 70 | // Try to make all the LEDs green. 71 | setAllLeds(0b0100); 72 | } 73 | -------------------------------------------------------------------------------- /examples/ChangeBaud/ChangeBaud.ino: -------------------------------------------------------------------------------- 1 | // This example shows how to change the baud rate of a servo. 2 | // 3 | // To use this sketch: 4 | // 5 | // 1) Change the servoId, servoBaudOld, and servoBaudNew 6 | // parameters below. 7 | // 2) Upload the sketch to your board. 8 | // 3) Open the Serial Monitor and set its baud rate to 115200. 9 | // 4) In the input box, type "y" and click "Send". 10 | 11 | #include 12 | 13 | // Change this to be the ID of the servo. 14 | const uint8_t servoId = 1; 15 | 16 | // Change this to be the baud rate that the servo currently uses. 17 | // The options are 9600, 19200, 57600, or 115200. 18 | const XYZrobotServoBaudRate servoBaudOld = XYZrobotServoBaudRate::B115200; 19 | 20 | // Change this to be the baud rate that you want the servo to use. 21 | const XYZrobotServoBaudRate servoBaudNew = XYZrobotServoBaudRate::B115200; 22 | 23 | // On boards with a hardware serial port available for use, use 24 | // that port. For other boards, create a SoftwareSerial object 25 | // using pin 10 to receive (RX) and pin 11 to transmit (TX). 26 | #ifdef SERIAL_PORT_HARDWARE_OPEN 27 | #define servoSerial SERIAL_PORT_HARDWARE_OPEN 28 | const bool usingSoftwareSerial = false; 29 | #else 30 | #include 31 | SoftwareSerial servoSerial(10, 11); 32 | const bool usingSoftwareSerial = true; 33 | #endif 34 | 35 | XYZrobotServo servo(servoSerial, servoId); 36 | 37 | bool success; 38 | char errorMessage[256]; 39 | 40 | void useOldBaudRate() 41 | { 42 | servoSerial.begin(XYZrobotServoBaudRateToInt(servoBaudOld)); 43 | delay(10); 44 | } 45 | 46 | void useNewBaudRate() 47 | { 48 | servoSerial.begin(XYZrobotServoBaudRateToInt(servoBaudNew)); 49 | delay(10); 50 | } 51 | 52 | void tryToChangeBaud() 53 | { 54 | success = false; 55 | errorMessage[0] = 0; 56 | 57 | // We can't reliably receive data at the old baud rate if we 58 | // are using software serial and the old baud rate is 115200. 59 | const bool canReceiveAtOldBaud = !(usingSoftwareSerial && 60 | servoBaudOld == XYZrobotServoBaudRate::B115200); 61 | 62 | // Switch to the old baud rate. 63 | useOldBaudRate(); 64 | 65 | // Make sure we can communicate with the servo using the old 66 | // baud rate. 67 | if (canReceiveAtOldBaud) 68 | { 69 | servo.readStatus(); 70 | if (servo.getLastError()) 71 | { 72 | sprintf_P(errorMessage, 73 | PSTR("Could not communicate with the servo: code %d."), 74 | servo.getLastError()); 75 | return; 76 | } 77 | } 78 | 79 | // Set the ACK policy to its default so we can later read back 80 | // values from EEPROM. 81 | servo.writeAckPolicyRam(XYZrobotServoAckPolicy::OnlyReadAndStat); 82 | 83 | // Make sure there is not another servo using the new baud rate 84 | // and same ID, because then changing the baud rate would cause 85 | // a conflict and it could be hard to fix. 86 | useNewBaudRate(); 87 | servo.readStatus(); 88 | if (servo.getLastError() != (uint8_t)XYZrobotServoError::HeaderTimeout) 89 | { 90 | sprintf_P(errorMessage, 91 | PSTR("There was already a servo at the new baud rate: code %d."), 92 | servo.getLastError()); 93 | return; 94 | } 95 | 96 | // Change the baud rate in EEPROM. (It is not in RAM.) 97 | useOldBaudRate(); 98 | servo.writeBaudRateEeprom(servoBaudNew); 99 | delay(20); 100 | 101 | // Make sure the baud rate in EEPROM is correct. 102 | if (canReceiveAtOldBaud) 103 | { 104 | XYZrobotServoBaudRate baudFromEeprom = servo.readBaudRateEeprom(); 105 | if (servo.getLastError()) 106 | { 107 | sprintf_P(errorMessage, 108 | PSTR("Failed to read baud rate from EEPROM: code %d"), 109 | servo.getLastError()); 110 | return; 111 | } 112 | if (baudFromEeprom != servoBaudNew) 113 | { 114 | sprintf_P(errorMessage, 115 | PSTR("The baud rate in EEPROM is incorrect: %d"), 116 | (uint8_t)baudFromEeprom); 117 | return; 118 | } 119 | } 120 | 121 | // Restart the servo so it can use its new baud rate. 122 | servo.reboot(); 123 | delay(2500); 124 | 125 | // Make sure the servo is responding at the new baud rate. 126 | useNewBaudRate(); 127 | servo.readStatus(); 128 | if (servo.getLastError()) 129 | { 130 | sprintf_P(errorMessage, 131 | PSTR("The servo did not respond at its new baud rate: code %d"), 132 | servo.getLastError()); 133 | return; 134 | } 135 | 136 | success = true; 137 | } 138 | 139 | // Prompt the user to send 'y' and wait until they do it. 140 | void waitForUserInput() 141 | { 142 | uint16_t lastPromptTime = 0; 143 | while (true) 144 | { 145 | if ((uint16_t)(millis() - lastPromptTime) >= 2000) 146 | { 147 | Serial.println(F("Send \"y\" to change the baud rate.")); 148 | lastPromptTime = millis(); 149 | } 150 | 151 | if (Serial.read() == 'y') { return; } 152 | } 153 | } 154 | 155 | void setup() 156 | { 157 | Serial.begin(115200); 158 | 159 | servoSerial.setTimeout(20); 160 | 161 | // To receive data, a pull-up is needed on the RX line because 162 | // the servos do not pull the line high while idle. If you are 163 | // using SoftwareSerial, the pull-up is probably enabled 164 | // already. If you are using the hardware serial port on an 165 | // ATmega32U4-based board, we know the RX pin must be pin 0 so 166 | // we enable its pull-up here. For other cases, you should add 167 | // code below to enable the pull-up on your board's RX line. 168 | #if defined(SERIAL_PORT_HARDWARE_OPEN) && defined(__AVR_ATmega32U4__) 169 | pinMode(0, INPUT_PULLUP); 170 | #endif 171 | 172 | } 173 | 174 | void loop() 175 | { 176 | waitForUserInput(); 177 | 178 | Serial.println(F("Trying to change the baud rate...")); 179 | 180 | tryToChangeBaud(); 181 | 182 | if (success) 183 | { 184 | Serial.println(F("Successfully changed the servo's baud rate.")); 185 | while (true) { } 186 | } 187 | else 188 | { 189 | Serial.print(F("Error: ")); 190 | Serial.println(errorMessage); 191 | } 192 | } 193 | -------------------------------------------------------------------------------- /examples/ChangeId/ChangeId.ino: -------------------------------------------------------------------------------- 1 | // This example shows how to change the ID number of a servo. 2 | // 3 | // To use this sketch: 4 | // 5 | // 1) Change the servoIdOld and servoIdNew parameters below. 6 | // 2) If necessary, change the baud rate on the 7 | // servoSerial.begin() line in in setup() to match the 8 | // baud rate used by your servos. 9 | // 3) Open the Serial Monitor and set its baud rate to 115200. 10 | // 4) In the input box, type "y" and click "Send". 11 | 12 | // Change this to be ID that the servo currently has. 13 | const uint8_t servoIdOld = 1; 14 | 15 | // Change this to be the ID that you want the servo to have. 16 | const uint8_t servoIdNew = 1; 17 | 18 | #include 19 | 20 | // On boards with a hardware serial port available for use, use 21 | // that port. For other boards, create a SoftwareSerial object 22 | // using pin 10 to receive (RX) and pin 11 to transmit (TX). 23 | #ifdef SERIAL_PORT_HARDWARE_OPEN 24 | #define servoSerial SERIAL_PORT_HARDWARE_OPEN 25 | #else 26 | #include 27 | SoftwareSerial servoSerial(10, 11); 28 | #endif 29 | 30 | XYZrobotServo servoOld(servoSerial, servoIdOld); 31 | XYZrobotServo servoNew(servoSerial, servoIdNew); 32 | 33 | bool success; 34 | char errorMessage[256]; 35 | 36 | void tryToChangeId() 37 | { 38 | success = false; 39 | errorMessage[0] = 0; 40 | 41 | // Make sure we can communicate with the servo with its current ID. 42 | servoOld.readStatus(); 43 | if (servoOld.getLastError()) 44 | { 45 | sprintf_P(errorMessage, 46 | PSTR("Could not communicate with the servo: code %d."), 47 | servoOld.getLastError()); 48 | return; 49 | } 50 | 51 | // Set the ACK policy to its default so we can later read back 52 | // values from EEPROM. 53 | servoOld.writeAckPolicyRam(XYZrobotServoAckPolicy::OnlyReadAndStat); 54 | 55 | // Make sure there is not another servo using the new ID, 56 | // because then changing the ID would cause an ID conflict and 57 | // it could be hard to fix. 58 | servoNew.readStatus(); 59 | if (servoNew.getLastError() != (uint8_t)XYZrobotServoError::HeaderTimeout) 60 | { 61 | sprintf_P(errorMessage, 62 | PSTR("There was already a servo at the new ID: code %d."), 63 | servoNew.getLastError()); 64 | return; 65 | } 66 | 67 | // Change the ID in EEPROM and RAM. 68 | servoOld.writeIdEeprom(servoIdNew); 69 | delay(10); 70 | servoOld.writeIdRam(servoIdNew); 71 | delay(10); 72 | 73 | // Make sure the servo is responding to the new ID. 74 | servoNew.readStatus(); 75 | if (servoNew.getLastError()) 76 | { 77 | sprintf_P(errorMessage, 78 | PSTR("The servo did not respond at its new ID: code %d"), 79 | servoNew.getLastError()); 80 | return; 81 | } 82 | 83 | // Make sure the ID in EEPROM is correct. 84 | uint8_t idFromEeprom = servoNew.readIdEeprom(); 85 | if (servoNew.getLastError()) 86 | { 87 | sprintf_P(errorMessage, 88 | PSTR("Failed to read ID from EEPROM: code %d"), 89 | servoNew.getLastError()); 90 | return; 91 | } 92 | if (idFromEeprom != servoIdNew) 93 | { 94 | sprintf_P(errorMessage, 95 | PSTR("The ID in EEPROM is incorrect: %d"), 96 | idFromEeprom); 97 | return; 98 | } 99 | 100 | success = true; 101 | } 102 | 103 | // Prompt the user to send 'y' and wait until they do it. 104 | void waitForUserInput() 105 | { 106 | uint16_t lastPromptTime = 0; 107 | while (true) 108 | { 109 | if ((uint16_t)(millis() - lastPromptTime) >= 2000) 110 | { 111 | Serial.println(F("Send \"y\" to change the ID.")); 112 | lastPromptTime = millis(); 113 | } 114 | 115 | if (Serial.read() == 'y') { return; } 116 | } 117 | } 118 | 119 | void setup() 120 | { 121 | Serial.begin(115200); 122 | 123 | // Turn on the serial port and set its baud rate. 124 | servoSerial.begin(115200); 125 | servoSerial.setTimeout(20); 126 | 127 | // To receive data, a pull-up is needed on the RX line because 128 | // the servos do not pull the line high while idle. If you are 129 | // using SoftwareSerial, the pull-up is probably enabled 130 | // already. If you are using the hardware serial port on an 131 | // ATmega32U4-based board, we know the RX pin must be pin 0 so 132 | // we enable its pull-up here. For other cases, you should add 133 | // code below to enable the pull-up on your board's RX line. 134 | #if defined(SERIAL_PORT_HARDWARE_OPEN) && defined(__AVR_ATmega32U4__) 135 | pinMode(0, INPUT_PULLUP); 136 | #endif 137 | } 138 | 139 | void loop() 140 | { 141 | waitForUserInput(); 142 | 143 | Serial.println(F("Trying to change the ID...")); 144 | 145 | tryToChangeId(); 146 | 147 | if (success) 148 | { 149 | Serial.println(F("Successfully changed the servo's ID.")); 150 | while (true) { } 151 | } 152 | else 153 | { 154 | Serial.print(F("Error: ")); 155 | Serial.println(errorMessage); 156 | } 157 | 158 | delay(2000); 159 | } 160 | -------------------------------------------------------------------------------- /examples/Compliance/Compliance.ino: -------------------------------------------------------------------------------- 1 | // This example shows how to make the XYZrobot 6 DOF robotic arm 2 | // hold itself up, while complying when you try to move it by 3 | // hand. 4 | // 5 | // Note that this sketch changes the Max_PWM parameter in the RAM 6 | // of your servos. If your servos are not responding very well 7 | // after running this sketch, you might need to power cycle them 8 | // to make them reload the Max_PWM parameter from EEPROM. 9 | // 10 | // If you send an 'f' character with the Serial Monitor, this 11 | // sketch will print the positions of all the servos, separated 12 | // by commas. These numbers can then be copied into another 13 | // program to be frames in an animation. 14 | // 15 | // You can use this sketch with other arrangements of A1-16 16 | // servos that are not the robot arm, but you might need to 17 | // change the part of the code that defines SERVO_COUNT and sets 18 | // up the XYZrobotServo objects. 19 | 20 | #include 21 | 22 | // This is the maximum PWM value, out of 1023, to use when 23 | // driving the servos. Setting it lower makes it easier to 24 | // manipulate the arm by hand, but if you set it too low, the arm 25 | // will not be able to hold itself up. 26 | const uint16_t maxPwm = 80; 27 | 28 | // This is how much the arm's position measurement has to differ 29 | // from its target position before this sketch adjusts its target 30 | // position. If you set it too low, then the deflections caused 31 | // by gravity will be large enough to make the arm move, and it 32 | // will not hold itself up. If you set it to high, it will be 33 | // hard to accurately position the arm. 34 | const uint16_t servoHysteresis = 5; 35 | 36 | // On boards with a hardware serial port available for use, use 37 | // that port. For other boards, create a SoftwareSerial object 38 | // using pin 10 to receive (RX) and pin 11 to transmit (TX). 39 | #ifdef SERIAL_PORT_HARDWARE_OPEN 40 | #define servoSerial SERIAL_PORT_HARDWARE_OPEN 41 | #else 42 | #include 43 | SoftwareSerial servoSerial(10, 11); 44 | #endif 45 | 46 | #define SERVO_COUNT 6 47 | 48 | XYZrobotServo servo1(servoSerial, 1); 49 | XYZrobotServo servo2(servoSerial, 2); 50 | XYZrobotServo servo3(servoSerial, 3); 51 | XYZrobotServo servo4(servoSerial, 4); 52 | XYZrobotServo servo5(servoSerial, 5); 53 | XYZrobotServo servo6(servoSerial, 6); 54 | 55 | XYZrobotServo * servos[SERVO_COUNT] = { 56 | &servo1, &servo2, &servo3, &servo4, &servo5, &servo6 57 | }; 58 | 59 | void setup() 60 | { 61 | Serial.begin(115200); 62 | 63 | servoSerial.begin(115200); 64 | servoSerial.setTimeout(20); 65 | 66 | // To receive data, a pull-up is needed on the RX line because 67 | // the servos do not pull the line high while idle. If you are 68 | // using SoftwareSerial, the pull-up is probably enabled 69 | // already. If you are using the hardware serial port on an 70 | // ATmega32U4-based board, we know the RX pin must be pin 0 so 71 | // we enable its pull-up here. For other cases, you should add 72 | // code below to enable the pull-up on your board's RX line. 73 | #if defined(SERIAL_PORT_HARDWARE_OPEN) && defined(__AVR_ATmega32U4__) 74 | pinMode(0, INPUT_PULLUP); 75 | #endif 76 | } 77 | 78 | bool updateServo(XYZrobotServo & servo) 79 | { 80 | XYZrobotServoStatus status = servo.readStatus(); 81 | if (servo.getLastError()) 82 | { 83 | return false; 84 | } 85 | 86 | // If posRef (the position that the servo is trying to 87 | // maintain) is more than servoHysteresis away from the current 88 | // measured position, then the servo is probably being 89 | // manipulated by hand, so move posRef. 90 | int16_t newPosRefSigned = constrain((int16_t)status.posRef, 91 | (int16_t)(status.position - servoHysteresis), 92 | (int16_t)(status.position + servoHysteresis)); 93 | 94 | // Convert posRef back to an unsigned number, handling the case 95 | // where it is negative. 96 | uint16_t newPosRef = newPosRefSigned < 0 ? 0 : newPosRefSigned; 97 | 98 | servo.setPosition(newPosRef); 99 | 100 | servo.writeMaxPwmRam(maxPwm); 101 | 102 | return true; 103 | } 104 | 105 | void updateServos() 106 | { 107 | for (uint8_t i = 1; i < SERVO_COUNT; i++) 108 | { 109 | bool success = updateServo(*servos[i]); 110 | if (!success) 111 | { 112 | Serial.print(F("Error: Failed to communicate with servo ")); 113 | Serial.println(servos[i]->getId()); 114 | break; 115 | } 116 | } 117 | } 118 | 119 | void handleSerialCommands() 120 | { 121 | int input = Serial.read(); 122 | 123 | // If we receive an 'f' from the serial monitor, print the 124 | // current servo positions so they can be used to make an 125 | // animation. 126 | if (input == 'f') 127 | { 128 | for (uint8_t i = 0; i < SERVO_COUNT; i++) 129 | { 130 | uint16_t posRef = servos[i]->readPosRef(); 131 | Serial.print(posRef); 132 | if (i + 1 != SERVO_COUNT) 133 | { 134 | Serial.print(F(", ")); 135 | } 136 | } 137 | Serial.println(); 138 | } 139 | } 140 | 141 | 142 | void loop() 143 | { 144 | updateServos(); 145 | 146 | handleSerialCommands(); 147 | 148 | delay(20); 149 | } 150 | -------------------------------------------------------------------------------- /examples/DetectServos/DetectServos.ino: -------------------------------------------------------------------------------- 1 | // This sketch searches for smart servos and prints information 2 | // about them that will be useful for communicating with them. 3 | // 4 | // Also, when it detects a servo, it makes the servo turn its LED 5 | // magenta for one second. Since servos are detected in order by 6 | // increasing ID number, the blinking LED can help you verify 7 | // that your servos have the correct IDs. 8 | 9 | #include 10 | 11 | // On boards with a hardware serial port available for use, use 12 | // that port. For other boards, create a SoftwareSerial object 13 | // using pin 10 to receive (RX) and pin 11 to transmit (TX). 14 | #ifdef SERIAL_PORT_HARDWARE_OPEN 15 | #define servoSerial SERIAL_PORT_HARDWARE_OPEN 16 | #else 17 | #include 18 | SoftwareSerial servoSerial(10, 11); 19 | #endif 20 | 21 | // By default, only search for servos with IDs from 1 to 20, to 22 | // save time. If you think you might have a servo with a higher 23 | // ID, you can change this to 255. 24 | const uint8_t maxId = 20; 25 | 26 | void setup() 27 | { 28 | Serial.begin(115200); 29 | 30 | // Turn on the serial port and set its baud rate. 31 | servoSerial.begin(115200); 32 | servoSerial.setTimeout(20); 33 | 34 | // To receive data, a pull-up is needed on the RX line because 35 | // the servos do not pull the line high while idle. If you are 36 | // using SoftwareSerial, the pull-up is probably enabled 37 | // already. If you are using the hardware serial port on an 38 | // ATmega32U4-based board, we know the RX pin must be pin 0 so 39 | // we enable its pull-up here. For other cases, you should add 40 | // code below to enable the pull-up on your board's RX line. 41 | #if defined(SERIAL_PORT_HARDWARE_OPEN) && defined(__AVR_ATmega32U4__) 42 | pinMode(0, INPUT_PULLUP); 43 | #endif 44 | } 45 | 46 | void blinkServoLed(XYZrobotServo & servo) 47 | { 48 | // Make all the LEDs be user-controlled. 49 | servo.writeAlarmLedPolicyRam(0b1111); 50 | 51 | // Turn on the red and blue LEDs to make magenta. 52 | servo.writeLedControl(0b1010); 53 | 54 | delay(1000); 55 | 56 | // Turn on the white LED while turning the others off, and 57 | // restore control to the system. Both the writeLedControl and 58 | // the delay commands seem to be necessary to get the LED to 59 | // change back to white. 60 | servo.writeLedControl(0b0001); 61 | delay(10); 62 | servo.writeAlarmLedPolicyRam(0); 63 | } 64 | 65 | void detectServo(uint8_t id) 66 | { 67 | XYZrobotServo servo(servoSerial, id); 68 | 69 | // Try to read the status from the servo to see if it is there. 70 | servo.readStatus(); 71 | if (servo.getLastError()) 72 | { 73 | if (servo.getLastError() == (uint8_t)XYZrobotServoError::HeaderTimeout) 74 | { 75 | // This is the error we get if there was no response at 76 | // all. Most of the IDs will have this error, so don't 77 | // print anything. 78 | } 79 | else 80 | { 81 | Serial.print(F("ID ")); 82 | Serial.print(id); 83 | Serial.print(F(": error ")); 84 | Serial.println(servo.getLastError()); 85 | } 86 | return; 87 | } 88 | 89 | // We successfully detected the servo. 90 | Serial.print(F("ID ")); 91 | Serial.print(id); 92 | Serial.println(F(": detected servo")); 93 | 94 | // Make the servo's LED shine magenta for one second. You can 95 | // comment this out if you want to speed up this sketch. 96 | blinkServoLed(servo); 97 | 98 | // Print some other information that will be useful when 99 | // communicating with it or troubleshooting issues. 100 | 101 | XYZrobotServoAckPolicy ackPolicy = servo.readAckPolicyRam(); 102 | Serial.print(F(" ACK policy: ")); 103 | Serial.println((uint8_t)ackPolicy); 104 | 105 | XYZrobotServoAckPolicy ackPolicyEeprom = servo.readAckPolicyEeprom(); 106 | if (ackPolicyEeprom != ackPolicy) 107 | { 108 | Serial.print(F(" ACK policy (EEPROM): ")); 109 | Serial.println((uint8_t)ackPolicyEeprom); 110 | } 111 | 112 | uint8_t versionInfo[4] = { 0, 0, 0, 0 }; 113 | servo.eepromRead(0, versionInfo, sizeof(versionInfo)); 114 | Serial.print(F(" Version info: ")); 115 | Serial.print(versionInfo[0]); // Model_No 116 | Serial.print(','); 117 | Serial.print(versionInfo[1]); // Year 118 | Serial.print('-'); 119 | Serial.print(versionInfo[2] & 0xF); // Month 120 | Serial.print('-'); 121 | Serial.print(versionInfo[3]); // Day 122 | Serial.print(','); 123 | Serial.println(versionInfo[2] >> 4 & 0xF); // Firmware version 124 | 125 | } 126 | 127 | void detectServos(uint32_t baudRate) 128 | { 129 | servoSerial.begin(baudRate); 130 | 131 | Serial.print(F("Detecting servos at ")); 132 | Serial.print(baudRate); 133 | Serial.println(F(" baud...")); 134 | delay(10); 135 | 136 | for (uint16_t id = 1; id <= maxId; id++) 137 | { 138 | detectServo(id); 139 | } 140 | } 141 | 142 | void loop() 143 | { 144 | delay(2500); 145 | 146 | // Try each of the four baud rates supported by the A1-16 servo. 147 | detectServos(9600); 148 | detectServos(19200); 149 | detectServos(57600); 150 | detectServos(115200); 151 | } 152 | -------------------------------------------------------------------------------- /examples/PlayAnimation/PlayAnimation.ino: -------------------------------------------------------------------------------- 1 | // This example shows how to play an animation consisting of 2 | // pre-recorded sets of servo positions on the XYZrobot 6 DOF 3 | // Robot Arm Kit. 4 | // 5 | // The animation is empty by default; you need to add frames to 6 | // it to make it work. You can use the Compliance example to get 7 | // the position data for the frames of the animation and then 8 | // copy that data into this sketch. 9 | // 10 | // You can use this sketch with other arrangements of A1-16 11 | // servos that are not the robot arm, but you might need to 12 | // change the part of the code that defines SERVO_COUNT and sets 13 | // up the XYZrobotServo objects. 14 | 15 | #include 16 | 17 | // On boards with a hardware serial port available for use, use 18 | // that port. For other boards, create a SoftwareSerial object 19 | // using pin 10 to receive (RX) and pin 11 to transmit (TX). 20 | #ifdef SERIAL_PORT_HARDWARE_OPEN 21 | #define servoSerial SERIAL_PORT_HARDWARE_OPEN 22 | #else 23 | #include 24 | SoftwareSerial servoSerial(10, 11); 25 | #endif 26 | 27 | #define SERVO_COUNT 6 28 | 29 | XYZrobotServo servo1(servoSerial, 1); 30 | XYZrobotServo servo2(servoSerial, 2); 31 | XYZrobotServo servo3(servoSerial, 3); 32 | XYZrobotServo servo4(servoSerial, 4); 33 | XYZrobotServo servo5(servoSerial, 5); 34 | XYZrobotServo servo6(servoSerial, 6); 35 | 36 | XYZrobotServo * servos[SERVO_COUNT] = { 37 | &servo1, &servo2, &servo3, &servo4, &servo5, &servo6 38 | }; 39 | 40 | typedef uint16_t Frame[1 + SERVO_COUNT]; 41 | 42 | #define END_FRAME { 0xFFFF } 43 | 44 | const Frame animation[] = { 45 | // To make your servos move, you need to add animation frames here. 46 | // Each frame is of the form: 47 | // 48 | // { DURATION, POSITION1, POSITION2, POSITION3, ... }, 49 | // 50 | // For example, if you had 3 servos, the following frame would 51 | // last for 1300 ms and put the first servo in position 333 52 | // while putting the second servo in position 555: 53 | // 54 | // { 1300, 333, 555 }, 55 | // 56 | // You can use the Compliance example to get position data that 57 | // can be pasted into your frames, but don't forget to add the 58 | // duration (in milliseconds) to the beginning. 59 | 60 | END_FRAME // Our loop uses this to detect the end. 61 | }; 62 | 63 | void setup() 64 | { 65 | servoSerial.begin(115200); 66 | servoSerial.setTimeout(20); 67 | 68 | // To receive data, a pull-up is needed on the RX line because 69 | // the servos do not pull the line high while idle. If you are 70 | // using SoftwareSerial, the pull-up is probably enabled 71 | // already. If you are using the hardware serial port on an 72 | // ATmega32U4-based board, we know the RX pin must be pin 0 so 73 | // we enable its pull-up here. For other cases, you should add 74 | // code below to enable the pull-up on your board's RX line. 75 | #if defined(SERIAL_PORT_HARDWARE_OPEN) && defined(__AVR_ATmega32U4__) 76 | pinMode(0, INPUT_PULLUP); 77 | #endif 78 | } 79 | 80 | void loop() 81 | { 82 | delay(2500); 83 | 84 | // The playtime is a time in units of 10 ms that specifies how 85 | // long the servos should take to move to the desired position. 86 | // You can change it to 0 if you want the movements to be as 87 | // fast as possible. 88 | uint16_t playtime = 25; 89 | 90 | // Loop over each from of the animation. 91 | for (const Frame * frame = animation; ; frame++) 92 | { 93 | uint16_t duration = (*frame)[0]; 94 | 95 | // Break out of the loop if this is the end frame. 96 | if (duration == 0xFFFF) { break; } 97 | 98 | // Go to the next frame if the duration of this frame is 0. 99 | if (duration == 0) { continue; } 100 | 101 | // Set the positions of each servo using the data in the frame. 102 | for (uint8_t i = 0; i < SERVO_COUNT; i++) 103 | { 104 | servos[i]->setPosition((*frame)[1 + i], playtime); 105 | } 106 | 107 | delay(duration); 108 | } 109 | } 110 | -------------------------------------------------------------------------------- /examples/ReadEverything/ReadEverything.ino: -------------------------------------------------------------------------------- 1 | // This example reads all the information from a smart servo and 2 | // prints it to the serial monitor. 3 | 4 | #include 5 | 6 | // On boards with a hardware serial port available for use, use 7 | // that port. For other boards, create a SoftwareSerial object 8 | // using pin 10 to receive (RX) and pin 11 to transmit (TX). 9 | #ifdef SERIAL_PORT_HARDWARE_OPEN 10 | #define servoSerial SERIAL_PORT_HARDWARE_OPEN 11 | #else 12 | #include 13 | SoftwareSerial servoSerial(10, 11); 14 | #endif 15 | 16 | const uint8_t servoId = 1; 17 | 18 | XYZrobotServo servo(servoSerial, servoId); 19 | 20 | void setup() 21 | { 22 | Serial.begin(115200); 23 | 24 | // Turn on the serial port and set its baud rate. 25 | servoSerial.begin(115200); 26 | servoSerial.setTimeout(20); 27 | 28 | // To receive data, a pull-up is needed on the RX line because 29 | // the servos do not pull the line high while idle. If you are 30 | // using SoftwareSerial, the pull-up is probably enabled 31 | // already. If you are using the hardware serial port on an 32 | // ATmega32U4-based board, we know the RX pin must be pin 0 so 33 | // we enable its pull-up here. For other cases, you should add 34 | // code below to enable the pull-up on your board's RX line. 35 | #if defined(SERIAL_PORT_HARDWARE_OPEN) && defined(__AVR_ATmega32U4__) 36 | pinMode(0, INPUT_PULLUP); 37 | #endif 38 | } 39 | 40 | void readEverything(XYZrobotServo & servo) 41 | { 42 | XYZrobotServoStatus status = servo.readStatus(); 43 | if (servo.getLastError()) 44 | { 45 | Serial.print(F("error reading status: ")); 46 | Serial.println(servo.getLastError()); 47 | } 48 | else 49 | { 50 | Serial.println(F("status:")); 51 | Serial.print(F(" statusError: 0x")); 52 | Serial.println(status.statusError, HEX); 53 | Serial.print(F(" statusDetail: 0x")); 54 | Serial.println(status.statusDetail, HEX); 55 | Serial.print(F(" pwm: ")); 56 | Serial.println(status.pwm); 57 | Serial.print(F(" posRef: ")); 58 | Serial.println(status.posRef); 59 | Serial.print(F(" position: ")); 60 | Serial.println(status.position); 61 | Serial.print(F(" iBus: ")); 62 | Serial.println(status.iBus); 63 | } 64 | 65 | uint8_t ram[80]; 66 | servo.ramRead(0, ram, 30); 67 | if (!servo.getLastError()) { servo.ramRead(30, ram + 30, 30); } 68 | if (!servo.getLastError()) { servo.ramRead(60, ram + 60, 20); } 69 | if (servo.getLastError()) 70 | { 71 | Serial.print(F("error reading RAM: ")); 72 | Serial.println(servo.getLastError()); 73 | } 74 | else 75 | { 76 | Serial.println(F("RAM:")); 77 | Serial.print(F(" sID: ")); 78 | Serial.println(ram[0]); 79 | Serial.print(F(" ACK_Policy: ")); 80 | Serial.println(ram[1]); 81 | Serial.print(F(" Alarm_LED_Policy: ")); 82 | Serial.println(ram[2]); 83 | Serial.print(F(" Torque_Policy: ")); 84 | Serial.println(ram[3]); 85 | Serial.print(F(" SPDctrl_Policy: ")); 86 | Serial.println(ram[4]); 87 | Serial.print(F(" Max_Temperature: ")); 88 | Serial.println(ram[5]); 89 | Serial.print(F(" Min_Voltage: ")); 90 | Serial.println(ram[6]); 91 | Serial.print(F(" Max_Voltage: ")); 92 | Serial.println(ram[7]); 93 | Serial.print(F(" Acceleration_Ratio: ")); 94 | Serial.println(ram[8]); 95 | Serial.print(F(" Max_Wheel_Ref_Position: ")); 96 | Serial.println(ram[12] + (ram[13] << 8)); 97 | Serial.print(F(" Max_PWM: ")); 98 | Serial.println(ram[16] + (ram[17] << 8)); 99 | Serial.print(F(" Overload_Threshold: ")); 100 | Serial.println(ram[18] + (ram[19] << 8)); 101 | Serial.print(F(" Min_Position: ")); 102 | Serial.println(ram[20] + (ram[21] << 8)); 103 | Serial.print(F(" Max_Position: ")); 104 | Serial.println(ram[22] + (ram[23] << 8)); 105 | Serial.print(F(" Position_Kp: ")); 106 | Serial.println(ram[24] + (ram[25] << 8)); 107 | Serial.print(F(" Position_Kd: ")); 108 | Serial.println(ram[26] + (ram[27] << 8)); 109 | Serial.print(F(" Position_Ki: ")); 110 | Serial.println(ram[28] + (ram[29] << 8)); 111 | Serial.print(F(" Close_to_Open_Ref_Position: ")); 112 | Serial.println(ram[30] + (ram[31] << 8)); 113 | Serial.print(F(" Open_to_Close_Ref_Position: ")); 114 | Serial.println(ram[32] + (ram[33] << 8)); 115 | Serial.print(F(" Ramp_Speed: ")); 116 | Serial.println(ram[36] + (ram[37] << 8)); 117 | Serial.print(F(" LED_Blink_Period: ")); 118 | Serial.println(ram[38]); 119 | Serial.print(F(" Packet_Timeout_Detection_Period: ")); 120 | Serial.println(ram[40]); 121 | Serial.print(F(" Overload_Detection_Period: ")); 122 | Serial.println(ram[42]); 123 | Serial.print(F(" Inposition_Margin: ")); 124 | Serial.println(ram[44]); 125 | Serial.print(F(" Over_Voltage_Detection_Period: ")); 126 | Serial.println(ram[45]); 127 | Serial.print(F(" Over_Temperature_Detection_Period: ")); 128 | Serial.println(ram[46]); 129 | Serial.print(F(" Calibration_Difference: ")); 130 | Serial.println(ram[47]); 131 | Serial.print(F(" Status_Error: ")); 132 | Serial.println(ram[48]); 133 | Serial.print(F(" Status_Detail: ")); 134 | Serial.println(ram[49]); 135 | Serial.print(F(" LED_Control: ")); 136 | Serial.println(ram[53]); 137 | Serial.print(F(" Voltage: ")); 138 | Serial.println(ram[54]); 139 | Serial.print(F(" Temperature: ")); 140 | Serial.println(ram[55]); 141 | Serial.print(F(" Current_Control_Mode: ")); 142 | Serial.println(ram[56]); 143 | Serial.print(F(" Tick: ")); 144 | Serial.println(ram[57]); 145 | Serial.print(F(" Joint_Position: ")); 146 | Serial.println(ram[60] + (ram[61] << 8)); 147 | Serial.print(F(" PWM_Output_Duty: ")); 148 | Serial.println(ram[64] + (ram[65] << 8)); 149 | Serial.print(F(" Bus_Current: ")); 150 | Serial.println(ram[66] + (ram[67] << 8)); 151 | Serial.print(F(" Position_Goal: ")); 152 | Serial.println(ram[68] + (ram[69] << 8)); 153 | Serial.print(F(" Position_Ref: ")); 154 | Serial.println(ram[70] + (ram[71] << 8)); 155 | Serial.print(F(" Omega_Goal: ")); 156 | Serial.println(ram[72] + (ram[73] << 8)); 157 | Serial.print(F(" Omega_Ref: ")); 158 | Serial.println(ram[74] + (ram[75] << 8)); 159 | Serial.print(F(" Requested_Counts: ")); 160 | Serial.println(ram[76] + (ram[77] << 8)); 161 | Serial.print(F(" ACK_Counts: ")); 162 | Serial.println(ram[78] + (ram[79] << 8)); 163 | } 164 | 165 | uint8_t eeprom[54]; 166 | servo.eepromRead(0, eeprom, 30); 167 | if (!servo.getLastError()) { servo.eepromRead(30, eeprom + 30, 24); } 168 | if (servo.getLastError()) 169 | { 170 | Serial.print(F("error reading EEPROM: ")); 171 | Serial.println(servo.getLastError()); 172 | } 173 | else 174 | { 175 | Serial.println(F("EEPROM:")); 176 | Serial.print(F(" Model_No: ")); 177 | Serial.println(eeprom[0]); 178 | Serial.print(F(" Date: ")); 179 | Serial.print(eeprom[1]); // Year 180 | Serial.print('-'); 181 | Serial.print(eeprom[2] & 0xF); // Month 182 | Serial.print('-'); 183 | Serial.println(eeprom[3]); // Day 184 | Serial.print(F(" Firmware_Version: ")); 185 | Serial.println(eeprom[2] >> 4 & 0xF); 186 | Serial.print(F(" Baud_Rate: ")); 187 | Serial.println(eeprom[5]); 188 | Serial.print(F(" sID: ")); 189 | Serial.println(eeprom[6]); 190 | Serial.print(F(" ACK_Policy: ")); 191 | Serial.println(eeprom[7]); 192 | Serial.print(F(" Alarm_LED_Policy: ")); 193 | Serial.println(eeprom[8]); 194 | Serial.print(F(" Torque_Policy: ")); 195 | Serial.println(eeprom[9]); 196 | Serial.print(F(" SPDctrl_Policy: ")); 197 | Serial.println(eeprom[10]); 198 | Serial.print(F(" Max_Temperature: ")); 199 | Serial.println(eeprom[11]); 200 | Serial.print(F(" Min_Voltage: ")); 201 | Serial.println(eeprom[12]); 202 | Serial.print(F(" Max_Voltage: ")); 203 | Serial.println(eeprom[13]); 204 | Serial.print(F(" Acceleration_Ratio: ")); 205 | Serial.println(eeprom[14]); 206 | Serial.print(F(" Max_Wheel_Ref_Position: ")); 207 | Serial.println(eeprom[18] + (eeprom[19] << 8)); 208 | Serial.print(F(" Max_PWM: ")); 209 | Serial.println(eeprom[22] + (eeprom[23] << 8)); 210 | Serial.print(F(" Overload_Threshold: ")); 211 | Serial.println(eeprom[24] + (eeprom[25] << 8)); 212 | Serial.print(F(" Min_Position: ")); 213 | Serial.println(eeprom[26] + (eeprom[27] << 8)); 214 | Serial.print(F(" Max_Position: ")); 215 | Serial.println(eeprom[28] + (eeprom[29] << 8)); 216 | Serial.print(F(" Position_Kp: ")); 217 | Serial.println(eeprom[30] + (eeprom[31] << 8)); 218 | Serial.print(F(" Position_Kd: ")); 219 | Serial.println(eeprom[32] + (eeprom[33] << 8)); 220 | Serial.print(F(" Position_Ki: ")); 221 | Serial.println(eeprom[34] + (eeprom[35] << 8)); 222 | Serial.print(F(" Close_to_Open_Ref_Position: ")); 223 | Serial.println(eeprom[36] + (eeprom[37] << 8)); 224 | Serial.print(F(" Open_to_Close_Ref_Position: ")); 225 | Serial.println(eeprom[38] + (eeprom[39] << 8)); 226 | Serial.print(F(" Ramp_Speed: ")); 227 | Serial.println(eeprom[42] + (eeprom[43] << 8)); 228 | Serial.print(F(" LED_Blink_Period: ")); 229 | Serial.println(eeprom[44]); 230 | Serial.print(F(" Packet_Timeout_Detection_Period: ")); 231 | Serial.println(eeprom[46]); 232 | Serial.print(F(" Overload_Detection_Period: ")); 233 | Serial.println(eeprom[48]); 234 | Serial.print(F(" Inposition_Margin: ")); 235 | Serial.println(eeprom[50]); 236 | Serial.print(F(" Over_Voltage_Detection_Period: ")); 237 | Serial.println(eeprom[51]); 238 | Serial.print(F(" Over_Temperature_Detection_Period: ")); 239 | Serial.println(eeprom[52]); 240 | Serial.print(F(" Calibration_Difference: ")); 241 | Serial.println(eeprom[53]); 242 | } 243 | 244 | Serial.println(); 245 | } 246 | 247 | void loop() 248 | { 249 | delay(4000); 250 | readEverything(servo); 251 | } 252 | -------------------------------------------------------------------------------- /examples/Rollback/Rollback.ino: -------------------------------------------------------------------------------- 1 | // This example shows how to send the Rollback command to reset a 2 | // servo's settings in EEPROM to their defaults. 3 | 4 | #include 5 | 6 | // On boards with a hardware serial port available for use, use 7 | // that port. For other boards, create a SoftwareSerial object 8 | // using pin 10 to receive (RX) and pin 11 to transmit (TX). 9 | #ifdef SERIAL_PORT_HARDWARE_OPEN 10 | #define servoSerial SERIAL_PORT_HARDWARE_OPEN 11 | #else 12 | #include 13 | SoftwareSerial servoSerial(10, 11); 14 | #endif 15 | 16 | // Set up a servo object, specifying what serial port to use and 17 | // what ID number to use. 18 | // 19 | // WARNING: If you change the ID number below to something other 20 | // than 1, make sure there are no other servos in your system 21 | // with ID 1. Otherwise, you could cause an ID conflict, because 22 | // the servo you roll back will change its ID to 1. 23 | XYZrobotServo servo(servoSerial, 1); 24 | 25 | void setup() 26 | { 27 | // Turn on the serial port and set its baud rate. 28 | servoSerial.begin(115200); 29 | 30 | delay(2500); 31 | 32 | servo.rollback(); 33 | } 34 | 35 | void loop() 36 | { 37 | // Nothing to do here. 38 | } 39 | -------------------------------------------------------------------------------- /examples/SetPosition/SetPosition.ino: -------------------------------------------------------------------------------- 1 | // This sketch shows how to move a servo back and forth between 2 | // two different position. 3 | // 4 | // Positions are represented as numbers between 0 and 1023. When 5 | // you set a position, you can also specify the playtime, which 6 | // is how long you want the movement to take, in units of 10 ms. 7 | // 8 | // This sketch only writes data to the servos; it does not 9 | // receive anything. 10 | 11 | #include 12 | 13 | // On boards with a hardware serial port available for use, use 14 | // that port. For other boards, create a SoftwareSerial object 15 | // using pin 10 to receive (RX) and pin 11 to transmit (TX). 16 | #ifdef SERIAL_PORT_HARDWARE_OPEN 17 | #define servoSerial SERIAL_PORT_HARDWARE_OPEN 18 | #else 19 | #include 20 | SoftwareSerial servoSerial(10, 11); 21 | #endif 22 | 23 | // Set up a servo object, specifying what serial port to use and 24 | // what ID number to use. 25 | // 26 | // WARNING: Only change the ID number below to a servo that can 27 | // rotate freely without damaging anything. 28 | XYZrobotServo servo(servoSerial, 128); 29 | 30 | const uint8_t playtime = 75; 31 | 32 | void setup() 33 | { 34 | // Turn on the serial port and set its baud rate. 35 | servoSerial.begin(115200); 36 | } 37 | 38 | void loop() 39 | { 40 | delay(2500); 41 | servo.setPosition(475, playtime); 42 | delay(2500); 43 | servo.setPosition(525, playtime); 44 | } 45 | -------------------------------------------------------------------------------- /examples/SetSpeed/SetSpeed.ino: -------------------------------------------------------------------------------- 1 | // This sketch shows how to set the speed of a servo using open 2 | // loop speed control. 3 | // 4 | // Speeds are represented as numbers between -1023 and 1023. 5 | // Setting the speed to 0 results in abrupt deceleration. 6 | // 7 | // This sketch only writes data to the servos; it does not 8 | // receive anything. 9 | 10 | #include 11 | 12 | // On boards with a hardware serial port available for use, use 13 | // that port. For other boards, create a SoftwareSerial object 14 | // using pin 10 to receive (RX) and pin 11 to transmit (TX). 15 | #ifdef SERIAL_PORT_HARDWARE_OPEN 16 | #define servoSerial SERIAL_PORT_HARDWARE_OPEN 17 | #else 18 | #include 19 | SoftwareSerial servoSerial(10, 11); 20 | #endif 21 | 22 | // Set up a servo object, specifying what serial port to use and 23 | // what ID number to use. 24 | // 25 | // WARNING: Only change the ID number below to a servo that can 26 | // rotate freely without damaging anything. 27 | XYZrobotServo servo(servoSerial, 128); 28 | 29 | void setup() 30 | { 31 | // Turn on the serial port and set its baud rate. 32 | servoSerial.begin(115200); 33 | } 34 | 35 | void loop() 36 | { 37 | delay(2500); 38 | 39 | // Move the servo output counter-clockwise for some time, 40 | // ramping up to the specified speed. 41 | servo.setSpeed(400); 42 | delay(2000); 43 | 44 | // Set the speed to 0 to make the servo stop abruptly. 45 | servo.setSpeed(0); 46 | delay(1000); 47 | 48 | // Move the servo output clockwise for some time, ramping up to 49 | // the specified speed. 50 | servo.setSpeed(-400); 51 | delay(1000); 52 | 53 | // Set the speed to -1 to make the servo smoothly ramp down to 54 | // a speed that is effectively zero. 55 | servo.setSpeed(-1); 56 | } 57 | -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | XYZrobotServoError KEYWORD1 2 | XYZrobotServoBaudRate KEYWORD1 3 | XYZrobotServoBaudRateToInt KEYWORD2 4 | XYZrobotServoAckPolicy KEYWORD1 5 | XYZrobotServoSpdctrlPolicy KEYWORD1 6 | XYZrobotServoStatus KEYWORD1 7 | XYZrobotServo KEYWORD1 8 | eepromWrite KEYWORD2 9 | eepromRead KEYWORD2 10 | ramWrite KEYWORD2 11 | ramRead KEYWORD2 12 | writeBaudRateEeprom KEYWORD2 13 | readBaudRateEeprom KEYWORD2 14 | writeIdEeprom KEYWORD2 15 | readIdEeprom KEYWORD2 16 | writeIdRam KEYWORD2 17 | writeAckPolicyEeprom KEYWORD2 18 | readAckPolicyEeprom KEYWORD2 19 | writeAckPolicyRam KEYWORD2 20 | writeAlarmLedPolicyRam KEYWORD2 21 | writeSpdctrlPolicyRam KEYWORD2 22 | writeMaxPwmRam KEYWORD2 23 | writeLedControl KEYWORD2 24 | readAckPolicyRam KEYWORD2 25 | readStatus KEYWORD2 26 | readPwm KEYWORD2 27 | readPosition KEYWORD2 28 | readPosRef KEYWORD2 29 | readIBus KEYWORD2 30 | setPosition KEYWORD2 31 | setSpeed KEYWORD2 32 | torqueOff KEYWORD2 33 | rollback KEYWORD2 34 | reboot KEYWORD2 35 | getLastError KEYWORD2 36 | getId KEYWORD2 37 | -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=XYZrobotServo 2 | version=1.1.0 3 | author=Pololu 4 | maintainer=Pololu 5 | sentence=Pololu's Arduino library for the XYZrobot Smart Servo A1-16 6 | paragraph=This library helps interface with the A1-16 smart servo from XYZrobot over serial. 7 | category=Device Control 8 | url=https://github.com/pololu/xyzrobot-servo-arduino 9 | architectures=* 10 | includes=XYZrobotServo.h 11 | -------------------------------------------------------------------------------- /tests/LibraryTest1/LibraryTest1.ino: -------------------------------------------------------------------------------- 1 | // This example tests some features of the library that are not used in other 2 | // examples. It is mainly intended for developers of the library. 3 | // 4 | // This example will change your servo's ACK_Policy parameter in EEPROM. 5 | 6 | #include 7 | 8 | // On boards with a hardware serial port available for use, use that port. For 9 | // other boards, create a SoftwareSerial object using pin 10 to receive (RX) and 10 | // pin 11 to transmit (TX). 11 | #ifdef SERIAL_PORT_HARDWARE_OPEN 12 | #define servoSerial SERIAL_PORT_HARDWARE_OPEN 13 | #else 14 | #include 15 | SoftwareSerial servoSerial(10, 11); 16 | #endif 17 | 18 | XYZrobotServo servo(servoSerial, 1); 19 | 20 | void runTest() 21 | { 22 | // Get the status to make sure the servo is there. 23 | servo.readStatus(); 24 | if (servo.getLastError()) 25 | { 26 | Serial.print(F("Error reading status: ")); 27 | Serial.println(servo.getLastError()); 28 | return; 29 | } 30 | 31 | // Make sure we can read and write the ACK_Policy in RAM. Use extra delays to 32 | // make sure the unneeded ACKs are done before the next command. 33 | XYZrobotServoAckPolicy policy; 34 | servo.writeAckPolicyRam(XYZrobotServoAckPolicy::All); 35 | delay(10); 36 | policy = servo.readAckPolicyRam(); 37 | if (policy != XYZrobotServoAckPolicy::All) 38 | { 39 | Serial.print(F("Error: RAM ACK_Policy is wrong (1): ")); 40 | Serial.println((uint8_t)policy); 41 | return; 42 | } 43 | servo.writeAckPolicyRam(XYZrobotServoAckPolicy::OnlyReadAndStat); 44 | delay(10); 45 | policy = servo.readAckPolicyRam(); 46 | if (policy != XYZrobotServoAckPolicy::OnlyReadAndStat) 47 | { 48 | Serial.print(F("Error: RAM ACK_Policy is wrong (2): ")); 49 | Serial.println((uint8_t)policy); 50 | return; 51 | } 52 | 53 | // Make sure we can read and write ACK_Policy in EEPROM. 54 | // Extra delays are added because the servo cannot receive commands until it 55 | // is done writing to EEPROM. 56 | servo.writeAckPolicyEeprom(XYZrobotServoAckPolicy::All); 57 | delay(10); 58 | policy = servo.readAckPolicyEeprom(); 59 | if (policy != XYZrobotServoAckPolicy::All) 60 | { 61 | Serial.print(F("Error: EEPROM ACK_Policy is wrong (1): ")); 62 | Serial.println((uint8_t)policy); 63 | return; 64 | } 65 | servo.writeAckPolicyEeprom(XYZrobotServoAckPolicy::OnlyReadAndStat); 66 | delay(10); 67 | policy = servo.readAckPolicyEeprom(); 68 | if (policy != XYZrobotServoAckPolicy::OnlyReadAndStat) 69 | { 70 | Serial.print(F("Error: EEPROM ACK_Policy is wrong (2): ")); 71 | Serial.println((uint8_t)policy); 72 | return; 73 | } 74 | 75 | // Make sure reboot works: we set ACK_Policy in RAM to All and then expect it 76 | // to get rolled back. 77 | servo.writeAckPolicyRam(XYZrobotServoAckPolicy::All); 78 | delay(10); 79 | Serial.println(F("Rebooting servo...")); 80 | servo.reboot(); 81 | delay(2500); 82 | policy = servo.readAckPolicyRam(); 83 | if (policy != XYZrobotServoAckPolicy::OnlyReadAndStat) 84 | { 85 | Serial.print(F("Error: RAM ACK_Policy is wrong after reboot: ")); 86 | Serial.println((uint8_t)policy); 87 | return; 88 | } 89 | 90 | // These lines appear commented out in the SetSpeed example. 91 | // Just make sure they can compile. 92 | if (0) 93 | { 94 | servo.writeSpdctrlPolicyRam(XYZrobotServoSpdctrlPolicy::OpenLoop); 95 | servo.writeSpdctrlPolicyRam(XYZrobotServoSpdctrlPolicy::CloseLoop); 96 | } 97 | 98 | Serial.println(F("Test passed.")); 99 | } 100 | 101 | void setup() 102 | { 103 | // Turn on the serial port and set its baud rate. 104 | servoSerial.begin(115200); 105 | servoSerial.setTimeout(10); 106 | 107 | // To receive data, a pull-up is needed on the RX line because the servos do 108 | // not pull the line high while idle. If you are using SoftwareSerial, the 109 | // pull-up is probably enabled already. If you are using the hardware serial 110 | // port on an ATmega32U4-based board, we know the RX pin must be pin 0 so we 111 | // enable its pull-up here. For other cases, you should add code below to 112 | // enable the pull-up on your board's RX line. 113 | #if defined(SERIAL_PORT_HARDWARE_OPEN) && defined(__AVR_ATmega32U4__) 114 | digitalWrite(0, HIGH); 115 | #endif 116 | } 117 | 118 | void loop() 119 | { 120 | delay(2500); 121 | runTest(); 122 | } 123 | 124 | --------------------------------------------------------------------------------