├── .gitignore ├── LICENSE ├── README.md ├── examples ├── lx16aOneWireESPExample │ └── lx16aOneWireESPExample.ino ├── lx16aOneWireTeensyExample │ └── lx16aOneWireTeensyExample.ino ├── lx16aServoExample │ └── lx16aServoExample.ino ├── lx16aServo_Calibrate_Motors │ └── lx16aServo_Calibrate_Motors.ino ├── lx16aServo_Motor_Mode │ └── lx16aServo_Motor_Mode.ino ├── lx16aServo_Multiple_Servos │ └── lx16aServo_Multiple_Servos.ino └── lx16aSetServoID │ └── lx16aSetServoID.ino ├── library.properties ├── lx-16a LewanSoul Bus Servo Communication Protocol.pdf └── src ├── lx16a-servo.cpp └── lx16a-servo.h /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Thorsten von Eicken 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # LX-16A, LX-224, HTS-35H and LX-15D Servo Library 2 | 3 | Simple Arduino library to operate LX-16A, LX-224, HTS-35H and LX-15D serial servos. 4 | 5 | This library sends simple commands to LewanSoul LX-16A, LX-224, HTS-35H and Hiwonder LX-15D serial bus servos. 6 | It is designed for the Arduino framework and uses the more common 3-pin configuration (TX, RX, direction). 7 | 8 | Using the BusLinker v2.2 IS NOT RECOMMENDED, it is not compatible with this library. 9 | 10 | # Electrical 11 | 12 | The LX-* servos all use a 3.3v driven bi directional asynchronus serial. It is similar to UART, but uses both signals on one pin. Because of this, the Master TX line has to be connected only while transmitting. The correct way to do this is a buffer chip 74HC126. 13 | 14 | https://www.digikey.com/product-detail/en/texas-instruments/SN74HC126N/296-8221-5-ND 15 | 16 | This library uses an IO pin passed to the begin() method to flag when the master is transmitting on the bus. When the flag is de-asserted, then the bus is freed for a motor to transmit, with the Masters UART TX line held in high-impedance. 17 | 18 | **LX-224 Pinout** 19 | 20 | 21 | 22 | ## Detailed Example instructions 23 | 24 | How to Wire up an ItsyBitsy to the LewanSoul Bus motors 25 | 26 | Here are detailed instructions for how to wire up the Lewansoul motors using the $0.43 chip linked above. https://github.com/Hephaestus-Arm/HephaestusArm2/blob/0.1.1/electronics.md#2-setting-up-the-board 27 | 28 | ## Generic instructions 29 | 30 | ``` 31 | MCU RX -> Direct Connection -> LX-* Serial Pin 32 | MCU TX -> 74HC126 A 33 | MCU Flag GPIO -> 74HC126 OE 34 | 74HC126 Y -> LX-* Serial Pin 35 | 6v-7.5v -> LX-* Power (center) pin 36 | GND -> LX-* GND Pin 37 | ``` 38 | 39 | The MCU Rx pin always listens, and hears its own bytes comming in. This library clears out the incomming bytes and will hang if it could not hear itself talking. 40 | 41 | ### ESP32 One Pin Mode 42 | 43 | The esp32 microcontrollers have the ability to run the serial Tx in Open Drain mode. This is detected by the library and done automatically. This means the pins for tx and rx are the same pin. 44 | 45 | ``` 46 | MCU RX/TX -> Direct Connection -> LX-16a Serial Pin 47 | 6v-7.5v -> LX-16a Power (center) pin 48 | GND -> LX-16a GND Pin 49 | ``` 50 | ### Other Arduino Quick and dirty/ Hacky one motor setup 51 | 52 | This wireing configuration will get you up and running fast. You will get a few failed commands and errored bytes using this method. It is usefull to quickly test a servo on a new system. 53 | 54 | ``` 55 | MCU RX -> Direct Connection -> LX-16a Serial Pin 56 | MCU TX -> 1K Ohm resistor -> LX-16a Serial Pin 57 | 6v-7.5v -> LX-16a Power (center) pin 58 | GND -> LX-16a GND Pin 59 | ``` 60 | 61 | ## ESP32 toolchain 62 | 63 | This library was writen for the 1.0.4 toolchain. It also works with the 2.0.0 toolchain for newer processors. 64 | 65 | Changes in the toolchain moving forward have broken the one-pin mode and cause you to need to use the buffer chip, or back-date the toolchain. 66 | 67 | ## ESP32 Board & PlatformIO Environment Configuration 68 | 69 | These drivers have been written to work with earlier versions of the ESP32 toolchains. Problems with reading from and writing to the servos were encountered with newer toolchain versions and newer ESP32 dev boards. Two dev boards were found to work with the environment setup shown below. These boards are the NodeMCU-32s and the ESP32 DevKitC-32e. 70 | 71 | ``` 72 | [env:nodemcu-32s] 73 | platform = espressif32@2.0.0 74 | board = nodemcu-32s 75 | framework = arduino 76 | monitor_speed = 115200 77 | lib_deps = 78 | madhephaestus/lx16a-servo@^0.9.3 79 | ``` 80 | 81 | It should be noted that running the ESP32 DevKitC-32e with its intended platform/toolchain (espressif32@4.0.0 and onwards) did not work with these servo drivers. 82 | 83 | -------------------------------------------------------------------------------- /examples/lx16aOneWireESPExample/lx16aOneWireESPExample.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | LX16ABus servoBus; 4 | LX16AServo servo(&servoBus, 1); 5 | 6 | void setup() { 7 | Serial.begin(115200); 8 | Serial.println("Beginning Servo Example"); 9 | servoBus.beginOnePinMode(&Serial2,33); 10 | servoBus.debug(true); 11 | servoBus.retry=0; 12 | } 13 | 14 | void loop() { 15 | int divisor =4; 16 | 17 | for (int x = -9000; x < 9000; x+=1000) { 18 | long start = millis(); 19 | int angle = x; 20 | int16_t pos = 0; 21 | pos = servo.pos_read(); 22 | // Serial.printf("\n\nPosition at %d -> %s\n", pos, 23 | // servo.isCommandOk() ? "OK" : "\n\nERR!!\n\n"); 24 | 25 | //do { 26 | servo.move_time(angle, 10*divisor); 27 | //} while (!servo.isCommandOk()); 28 | // Serial.printf("Move to %d -> %s\n", angle, 29 | // servo.isCommandOk() ? "OK" : "\n\nERR!!\n\n"); 30 | // Serial.println("Voltage = " + String(servo.vin())); 31 | // Serial.println("Temp = " + String(servo.temp())); 32 | // Serial.println("ID = " + String(servo.id_read())); 33 | // Serial.println("Motor Mode = " + String(servo.readIsMotorMode())); 34 | long took = millis()-start; 35 | long time = (10*divisor)-took; 36 | if(time>0) 37 | delay(time); 38 | else{ 39 | Serial.println("Real Time broken, took: "+String(took)); 40 | } 41 | } 42 | 43 | servo.move_time(-9000, 3000); 44 | delay(4000); 45 | } 46 | 47 | -------------------------------------------------------------------------------- /examples/lx16aOneWireTeensyExample/lx16aOneWireTeensyExample.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | LX16ABus servoBus; 4 | LX16AServo servo(&servoBus, 5); 5 | 6 | void setup() { 7 | Serial.begin(115200); 8 | Serial.println("Beginning Servo Example"); 9 | servoBus.beginOnePinMode(&Serial2, 8); 10 | servoBus.debug(true); 11 | servoBus.retry=0; 12 | } 13 | 14 | void loop() { 15 | int divisor =4; 16 | 17 | for (int x = -9000; x < 9000; x+=1000) { 18 | long start = millis(); 19 | int angle = x; 20 | int16_t pos = 0; 21 | pos = servo.pos_read(); 22 | // Serial.printf("\n\nPosition at %d -> %s\n", pos, 23 | // servo.isCommandOk() ? "OK" : "\n\nERR!!\n\n"); 24 | 25 | //do { 26 | servo.move_time(angle, 10*divisor); 27 | //} while (!servo.isCommandOk()); 28 | // Serial.printf("Move to %d -> %s\n", angle, 29 | // servo.isCommandOk() ? "OK" : "\n\nERR!!\n\n"); 30 | // Serial.println("Voltage = " + String(servo.vin())); 31 | // Serial.println("Temp = " + String(servo.temp())); 32 | // Serial.println("ID = " + String(servo.id_read())); 33 | // Serial.println("Motor Mode = " + String(servo.readIsMotorMode())); 34 | long took = millis()-start; 35 | long time = (10*divisor)-took; 36 | if(time>0) 37 | delay(time); 38 | else{ 39 | Serial.println("Real Time broken, took: "+String(took)); 40 | } 41 | } 42 | 43 | servo.move_time(-9000, 3000); 44 | delay(4000); 45 | } 46 | -------------------------------------------------------------------------------- /examples/lx16aServoExample/lx16aServoExample.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | LX16ABus servoBus; 4 | LX16AServo servo(&servoBus, 1); 5 | 6 | void setup() { 7 | servoBus.begin(&Serial1, 8 | 1,// on TX pin 1 9 | 2);// use pin 2 as the TX flag for buffer 10 | Serial.begin(115200); 11 | servoBus.debug(true); 12 | Serial.println("Beginning Servo Example"); 13 | } 14 | 15 | void loop() { 16 | int divisor =4; 17 | for (int i = 0; i < 1000/divisor; i++) { 18 | long start = millis(); 19 | uint16_t angle = i * 24*divisor; 20 | int16_t pos = 0; 21 | pos = servo.pos_read(); 22 | Serial.printf("\n\nPosition at %d -> %s\n", pos, 23 | servo.isCommandOk() ? "OK" : "\n\nERR!!\n\n"); 24 | 25 | do { 26 | servo.move_time(angle, 10*divisor); 27 | } while (!servo.isCommandOk()); 28 | Serial.printf("Move to %d -> %s\n", angle, 29 | servo.isCommandOk() ? "OK" : "\n\nERR!!\n\n"); 30 | Serial.println("Voltage = " + String(servo.vin())); 31 | Serial.println("Temp = " + String(servo.temp())); 32 | Serial.println("ID = " + String(servo.id_read())); 33 | Serial.println("Motor Mode = " + String(servo.readIsMotorMode())); 34 | long took = millis()-start; 35 | long time = (10*divisor)-took; 36 | if(time>0) 37 | delay(time); 38 | else{ 39 | Serial.println("Real Time broken, took: "+String(took)); 40 | } 41 | } 42 | 43 | servo.move_time(0, 3000); 44 | delay(3000); 45 | } 46 | -------------------------------------------------------------------------------- /examples/lx16aServo_Calibrate_Motors/lx16aServo_Calibrate_Motors.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | LX16ABus servoBus; 4 | LX16AServo servo(&servoBus, 1); 5 | LX16AServo servo2(&servoBus, 2); 6 | LX16AServo servo3(&servoBus, 3); 7 | int16_t startingAngles []= {-9000,0,9000}; 8 | bool calibrationDone = false; 9 | #define HOME_SWITCH_PIN 32 10 | int divisor = 3; 11 | void setup() { 12 | while(!Serial); 13 | delay(1000); 14 | Serial.begin(115200); 15 | Serial.println("Starting"); 16 | servoBus.begin(&Serial1, 1, // on TX pin 1 17 | 2); // use pin 2 as the TX flag for buffer 18 | 19 | servoBus.retry = 2; // enforce synchronous real time 20 | //servoBus.debug(true); 21 | Serial.println("Beginning Coordinated Servo Example"); 22 | pinMode(HOME_SWITCH_PIN, INPUT_PULLUP); 23 | servo.disable(); 24 | servo2.disable(); 25 | servo3.disable(); 26 | } 27 | // 40ms trajectory planning loop seems the most stable 28 | void interp(int i){ 29 | long start = millis(); 30 | servoBus.move_sync_start(); 31 | int32_t pos = servo.pos_read(); 32 | int32_t pos2 = servo2.pos_read(); 33 | int32_t pos3 = servo3.pos_read(); 34 | 35 | int32_t angle = (i * 9 * divisor)-4500; 36 | int timingOffset = millis()-start; 37 | servo2.move_time_and_wait_for_sync(angle+4500, 10 * divisor+timingOffset); 38 | servo3.move_time_and_wait_for_sync(-4500-(angle), 10 * divisor+timingOffset); 39 | servo.move_time_and_wait_for_sync(angle*2, 10 * divisor+timingOffset); 40 | 41 | 42 | 43 | //if(abs(pos2-pos)>100){ 44 | // Serial.printf("\n\nPosition at %d, %d and %d-> %s\n", pos, pos2,pos3, 45 | // servo.isCommandOk() ? "OK" : "\n\nERR!!\n\n"); 46 | 47 | // Serial.printf("Move to %d -> %s\n", angle, 48 | // servo.isCommandOk() ? "OK" : "\n\nERR!!\n\n"); 49 | //} 50 | long took = millis() - start; 51 | 52 | long time = (10 * divisor) - took; 53 | if (time > 0) 54 | delay(time); 55 | else { 56 | Serial.println("Real Time broken, took: " + String(took)); 57 | } 58 | } 59 | void loop() { 60 | bool HOME = !digitalRead(HOME_SWITCH_PIN); 61 | if(!calibrationDone){ 62 | 63 | Serial.println("Homing pin "+String(HOME)); 64 | 65 | if(HOME){ 66 | Serial.println("Calibrating 1"); 67 | servo.calibrate(startingAngles[0],-9000,9000); 68 | Serial.println("Calibrating 2"); 69 | servo2.calibrate(startingAngles[1],-2512,10000); 70 | Serial.println("Calibrating 3"); 71 | servo3.calibrate(startingAngles[2],-9000,9000); 72 | 73 | Serial.println("Limits read"); 74 | } 75 | Serial.println("Read 1"); 76 | int32_t pos = servo.pos_read(); 77 | Serial.println("Read 2"); 78 | int32_t pos2 = servo2.pos_read(); 79 | Serial.println("Read 3"); 80 | int32_t pos3 = servo3.pos_read(); 81 | Serial.printf("\n\nPosition at %d, %d and %d-> %s\n", pos, pos2, pos3, 82 | servo.isCommandOk() ? "OK" : "\n\nERR!!\n\n"); 83 | if(pos==startingAngles[0]&& 84 | pos2==startingAngles[1]&& 85 | pos3==startingAngles[2]&& 86 | HOME){ 87 | calibrationDone=true; 88 | Serial.println("Calibration Done!"); 89 | servo.move_time(0, 2000); 90 | delay(2000); 91 | servo2.move_time_and_wait_for_sync(0, 2000); 92 | servo3.move_time_and_wait_for_sync(0, 2000); 93 | servo.move_time_and_wait_for_sync(-9000, 2000); 94 | servoBus.move_sync_start(); 95 | delay(2000); 96 | } 97 | } 98 | 99 | if(!calibrationDone){ 100 | delay(50); 101 | return; 102 | } 103 | if(HOME){ 104 | int32_t pos = servo.pos_read(); 105 | int32_t pos2 = servo2.pos_read(); 106 | int32_t pos3 = servo3.pos_read(); 107 | Serial.printf("\n\nMAX at %d, %d and %d", servo.getMaxCentDegrees(), servo2.getMaxCentDegrees(), servo3.getMaxCentDegrees()); 108 | Serial.printf("\nPos at %d, %d and %d", pos, pos2, pos3); 109 | Serial.printf("\nmin at %d, %d and %d", servo.getMinCentDegrees(), servo2.getMinCentDegrees(), servo3.getMinCentDegrees()); 110 | // 111 | delay(50); 112 | return; 113 | } 114 | 115 | for (int i = 0; i < 1000 / divisor; i++) { 116 | interp( i); 117 | } 118 | Serial.println("Interpolated Set pos done, not long set"); 119 | 120 | for (int i = 1000 / divisor; i >0; i--) { 121 | interp( i); 122 | } 123 | Serial.println("Loop resetting"); 124 | } 125 | 126 | 127 | -------------------------------------------------------------------------------- /examples/lx16aServo_Motor_Mode/lx16aServo_Motor_Mode.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | LX16ABus servoBus; 4 | LX16AServo servo(&servoBus, 1); 5 | void setup() { 6 | servoBus.begin(&Serial1, 7 | 1,// on TX pin 1 8 | 2);// use pin 2 as the TX flag for buffer 9 | Serial.begin(115200); 10 | } 11 | 12 | void loop() { 13 | for (int i = -10; i < 10; i++) { 14 | int16_t pos = 0; 15 | pos = servo.pos_read(); 16 | Serial.printf("\n\nPosition at %d -> %s\n", pos, 17 | servo.isCommandOk() ? "OK" : "\n\nERR!!\n\n"); 18 | 19 | int16_t angle = i * 100; 20 | 21 | 22 | servo.motor_mode(angle); 23 | 24 | Serial.printf("Move to %d -> %s\n", angle, 25 | servo.isCommandOk() ? "OK" : "\n\nERR!!\n\n"); 26 | Serial.println("Voltage = " + String(servo.vin())); 27 | Serial.println("Temp = " + String(servo.temp())); 28 | Serial.println("ID = " + String(servo.id_read())); 29 | delay(500); 30 | } 31 | servo.move_time(0, 500); 32 | delay(5000); 33 | } 34 | -------------------------------------------------------------------------------- /examples/lx16aServo_Multiple_Servos/lx16aServo_Multiple_Servos.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | LX16ABus servoBus; 4 | LX16AServo servo(&servoBus, 1); 5 | LX16AServo servo2(&servoBus, 2); 6 | LX16AServo servo3(&servoBus, 3); 7 | void setup() { 8 | servoBus.begin(&Serial1, 1, // on TX pin 1 9 | 2); // use pin 2 as the TX flag for buffer 10 | Serial.begin(115200); 11 | servoBus.retry = 1; // enforce synchronous real time 12 | servoBus.debug(true); 13 | Serial.println("Beginning Coordinated Servo Example"); 14 | 15 | } 16 | // 40ms trajectory planning loop seems the most stable 17 | 18 | void loop() { 19 | int divisor = 3; 20 | for (int i = 0; i < 1000 / divisor; i++) { 21 | long start = millis(); 22 | int16_t pos = 0; 23 | pos = servo.pos_read(); 24 | int16_t pos2 = servo2.pos_read(); 25 | int16_t pos3 = servo3.pos_read(); 26 | 27 | uint16_t angle = i * 24 * divisor; 28 | 29 | servo2.move_time_and_wait_for_sync(angle, 10 * divisor); 30 | servo3.move_time_and_wait_for_sync(angle, 10 * divisor); 31 | servo.move_time_and_wait_for_sync(angle, 10 * divisor); 32 | 33 | servoBus.move_sync_start(); 34 | 35 | //if(abs(pos2-pos)>100){ 36 | Serial.printf("\n\nPosition at %d and %d-> %s\n", pos, pos2, 37 | servo.isCommandOk() ? "OK" : "\n\nERR!!\n\n"); 38 | Serial.printf("Move to %d -> %s\n", angle, 39 | servo.isCommandOk() ? "OK" : "\n\nERR!!\n\n"); 40 | //} 41 | long took = millis() - start; 42 | 43 | long time = (10 * divisor) - took; 44 | if (time > 0) 45 | delay(time); 46 | else { 47 | Serial.println("Real Time broken, took: " + String(took)); 48 | } 49 | } 50 | Serial.println("Interpolated Set pos done, not long set"); 51 | 52 | servoBus.retry = 5; // These commands must arrive 53 | servo.move_time(0, 10000); 54 | servo2.move_time(0, 10000); 55 | servo3.move_time(0, 10000); 56 | servoBus.retry = 0; // Back to low latency mode 57 | for (int i = 0; i < 1000 / divisor; i++) { 58 | long start = millis(); 59 | int16_t pos = 0; 60 | pos = servo.pos_read(); 61 | int16_t pos2 = servo2.pos_read(); 62 | int16_t pos3 = servo3.pos_read(); 63 | 64 | Serial.printf("\n\nPosition at %d and %d\n", pos, pos2); 65 | 66 | Serial.println("Voltage = " + String(servo.vin())); 67 | Serial.println("Temp = " + String(servo.temp())); 68 | 69 | long took = millis() - start; 70 | 71 | long time = (10 * divisor) - took; 72 | if (time > 0) 73 | delay(time); 74 | else { 75 | Serial.println("Real Time broken, took: " + String(took)); 76 | } 77 | } 78 | Serial.println("Loop resetting"); 79 | } 80 | -------------------------------------------------------------------------------- /examples/lx16aSetServoID/lx16aSetServoID.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | LX16ABus servoBus; 4 | LX16AServo servo(&servoBus, LX16A_BROADCAST_ID);// send these commands to any motor on the bus 5 | int id =3; 6 | void setup() { 7 | servoBus.begin(&Serial1, 8 | 4,// on TX pin 4 9 | 16);// use pin 16 as the TX flag for buffer 10 | Serial.begin(115200); 11 | } 12 | 13 | void loop() { 14 | // Set any motor plugged in to ID 3 15 | // this INO acts as an auto-provisioner for any motor plugged in 16 | servo.id_write(id); 17 | Serial.println("Attempting set to ID "+String (id)); 18 | int read=servo.id_read(); 19 | if(read!=id || read==0){ 20 | Serial.println("\r\nERROR ID set failed"); 21 | delay(500); 22 | }else{ 23 | Serial.println("\r\nCurrent ID is now "+String(read)); 24 | } 25 | delay(200); 26 | 27 | } 28 | 29 | -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=lx16a-servo 2 | version=0.9.3 3 | author=Kevin Harrington,Thorsten von Eicken 4 | maintainer=Kevin Harrington 5 | sentence=Simple Arduino library to operate ALX-16A, LX-224, HTS-35H and LX-15D serial servos. 6 | paragraph=This library sends simple commands to LewanSoul LX-16A, LX-224, HTS-35H and LX-15D serial bus servos. It is designed for the ESP32 Arduino framework and uses a single pin to interface to the servos as opposed to the more common 3-pin configuration (TX, RX, direction). 7 | category=Device Control 8 | url=https://github.com/madhephaestus/lx16a-servo 9 | architectures=* 10 | includes=lx16a-servo.h 11 | 12 | -------------------------------------------------------------------------------- /lx-16a LewanSoul Bus Servo Communication Protocol.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/madhephaestus/lx16a-servo/24fe55ecf9b52f57460e0725df565d981ab86695/lx-16a LewanSoul Bus Servo Communication Protocol.pdf -------------------------------------------------------------------------------- /src/lx16a-servo.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | // write a command with the provided parameters 5 | // returns true if the command was written without conflict onto the bus 6 | bool LX16ABus::write_no_retry(uint8_t cmd, const uint8_t *params, int param_cnt, 7 | uint8_t MYID) { 8 | if (param_cnt < 0 || param_cnt > 4) 9 | return false; 10 | 11 | // prepare packet in a buffer 12 | int buflen = 6 + param_cnt; 13 | uint8_t buf[buflen]; 14 | uint8_t ret[buflen]; 15 | buf[0] = 0x55; 16 | buf[1] = 0x55; 17 | buf[2] = MYID; 18 | buf[3] = buflen - 3; 19 | buf[4] = cmd; 20 | for (int i = 0; i < param_cnt; i++) 21 | buf[5 + i] = params[i]; 22 | uint8_t cksum = 0; 23 | for (int i = 2; i < buflen - 1; i++) 24 | cksum += buf[i]; 25 | buf[buflen - 1] = ~cksum; 26 | 27 | // clear input buffer 28 | int junkCount = 0; 29 | delayMicroseconds(timeus(2)); 30 | while (available()) { 31 | if (junkCount == 0) { 32 | // if (_debug) 33 | // Serial.print("\n\t\t[ "); 34 | } 35 | // if (_debug) 36 | // Serial.print(" " + String(read()) + ", "); 37 | // else 38 | read(); 39 | delayMicroseconds(timeus(3)); 40 | junkCount++; 41 | } 42 | 43 | // if (_debug && junkCount!=0) { 44 | // Serial.print("]\n "); 45 | // Serial.println( 46 | // "\t\t Junk bytes = " + String(junkCount) + " id " + String(MYID) 47 | // + " cmd " + String(cmd) + " last cmd " 48 | // + String(lastCommand)); 49 | // } 50 | lastCommand = cmd; 51 | // send command packet 52 | uint32_t t0 = millis(); 53 | if(!singlePinMode) 54 | if (myTXFlagGPIO >= 0) { 55 | digitalWrite(myTXFlagGPIO, 1); 56 | } 57 | delayMicroseconds(timeus(1)); 58 | write(buf, buflen); 59 | delayMicroseconds(timeus(1)); 60 | if(!singlePinMode) 61 | if (myTXFlagGPIO >= 0) { 62 | digitalWrite(myTXFlagGPIO, 0); 63 | } 64 | // expect to read back command by virtue of single-pin loop-back 65 | uint32_t tout = time(buflen+4) + 4; // 2ms margin 66 | int got = 0; 67 | bool ok = true; 68 | if(!singlePinMode){ 69 | if (_deepDebug) 70 | Serial.println("RCV: "); 71 | while ((got < buflen) && ((millis() - t0) < tout)) { 72 | if (available()) { 73 | ret[got] = read(); 74 | if (ret[got] != buf[got]) { 75 | ok = false; 76 | } 77 | got++; 78 | } 79 | } 80 | if (got 7) { 137 | if (_debug) 138 | Serial.println(" ERR (len)\n"); 139 | return false; 140 | } 141 | len = ch + 3; 142 | if (len > param_len + 6) { 143 | if (_debug) 144 | Serial.println( 145 | " ERR (param_len) got " + String(len) 146 | + " expected " 147 | + String((param_len + 6))); 148 | return false; 149 | } 150 | break; 151 | case 4: 152 | if (ch != cmd) { 153 | if (_debug) 154 | Serial.println(" ERR (cmd)\n"); 155 | return false; 156 | } 157 | break; 158 | default: 159 | if (got == len - 1) { 160 | if ((uint8_t) ch == (uint8_t) ~sum) { 161 | //if (_deepDebug) 162 | // Serial.println(" OK\n"); 163 | return true; 164 | } else { 165 | if (_debug) 166 | Serial.println(" ERR (cksum!="+String(~sum)+")\n"); 167 | return false; 168 | } 169 | } 170 | if (got - 5 > param_len) { 171 | if (_debug) 172 | Serial.println(" ERR (cksum)\n"); 173 | return false; 174 | } 175 | params[got - 5] = ch; 176 | } 177 | if (got > 1) 178 | sum += ch; 179 | got++; 180 | } 181 | } 182 | if (_debug){ 183 | long done = millis(); 184 | Serial.println( 185 | "Read TIMEOUT Expected " + String(len) + " got " + String(got) 186 | 187 | + " on cmd: " + String(cmd) + " id " + String(MYID) 188 | +" took "+String(done-t0)); 189 | } 190 | return false; 191 | } 192 | // read sends a command to the servo and reads back the response into the params buffer. 193 | // returns true if everything checks out correctly. 194 | bool LX16ABus::read_no_retry(uint8_t cmd, uint8_t *params, int param_len, 195 | uint8_t MYID) { 196 | // send the read command 197 | bool ok = write(cmd, NULL, 0, MYID); 198 | if (!ok) { 199 | if (_debug) 200 | Serial.println( 201 | "Command of read failed on cmd: " + String(cmd) + " id " 202 | + String(MYID)); 203 | return false; 204 | } 205 | 206 | return rcv(cmd, params, param_len, MYID); 207 | } 208 | -------------------------------------------------------------------------------- /src/lx16a-servo.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #define LX16A_BROADCAST_ID 0xFE 4 | #define LX16A_SERVO_MOVE_TIME_WRITE 1 5 | #define LX16A_SERVO_MOVE_TIME_READ 2 6 | #define LX16A_SERVO_MOVE_TIME_WAIT_WRITE 7 7 | #define LX16A_SERVO_MOVE_TIME_WAIT_READ 8 8 | #define LX16A_SERVO_MOVE_START 11 9 | #define LX16A_SERVO_MOVE_STOP 12 10 | #define LX16A_SERVO_ID_WRITE 13 11 | #define LX16A_SERVO_ID_READ 14 12 | #define LX16A_SERVO_ANGLE_OFFSET_ADJUST 17 13 | #define LX16A_SERVO_ANGLE_OFFSET_WRITE 18 14 | #define LX16A_SERVO_ANGLE_OFFSET_READ 19 15 | #define LX16A_SERVO_ANGLE_LIMIT_WRITE 20 16 | #define LX16A_SERVO_ANGLE_LIMIT_READ 21 17 | #define LX16A_SERVO_VIN_LIMIT_WRITE 22 18 | #define LX16A_SERVO_VIN_LIMIT_READ 23 19 | #define LX16A_SERVO_TEMP_MAX_LIMIT_WRITE 24 20 | #define LX16A_SERVO_TEMP_MAX_LIMIT_READ 25 21 | #define LX16A_SERVO_TEMP_READ 26 22 | #define LX16A_SERVO_VIN_READ 27 23 | #define LX16A_SERVO_POS_READ 28 24 | #define LX16A_SERVO_OR_MOTOR_MODE_WRITE 29 25 | #define LX16A_SERVO_OR_MOTOR_MODE_READ 30 26 | #define LX16A_SERVO_LOAD_OR_UNLOAD_WRITE 31 27 | #define LX16A_SERVO_LOAD_OR_UNLOAD_READ 32 28 | #define LX16A_SERVO_LED_CTRL_WRITE 33 29 | #define LX16A_SERVO_LED_CTRL_READ 34 30 | #define LX16A_SERVO_LED_ERROR_WRITE 35 31 | #define LX16A_SERVO_LED_ERROR_READ 36 32 | 33 | class LX16ABus { 34 | private: 35 | int myTXFlagGPIO = -1; 36 | int myTXPin=-1; 37 | int lastCommand=0; 38 | void setTXFlag(int flag) { 39 | myTXFlagGPIO = flag; 40 | if (myTXFlagGPIO >= 0) { 41 | pinMode(myTXFlagGPIO, OUTPUT); 42 | digitalWrite(myTXFlagGPIO, 0); 43 | } 44 | } 45 | 46 | public: 47 | 48 | bool _debug = false; 49 | bool _deepDebug = false; 50 | bool singlePinMode=false; 51 | LX16ABus() { 52 | } 53 | 54 | // debug enables/disables debug printf's for this servo 55 | void debug(bool on) { 56 | _debug = on; 57 | 58 | } 59 | void beginOnePinMode(HardwareSerial * port, int tXrXpin){ 60 | _port = port; 61 | myTXPin=tXrXpin; 62 | singlePinMode=true; 63 | 64 | #if defined ARDUINO_ARCH_ESP32 65 | port->begin(_baud, SERIAL_8N1,myTXPin,myTXPin); 66 | pinMode(myTXPin, OUTPUT|PULLUP); 67 | #elif defined(CORE_TEENSY) 68 | pinMode(myTXPin, OUTPUT_OPENDRAIN); 69 | port->begin(_baud, SERIAL_8N1 | SERIAL_HALF_DUPLEX); 70 | port->setTX(myTXPin, true); 71 | 72 | #endif 73 | delay(3); 74 | while (port->available()) 75 | port->read(); 76 | } 77 | void begin(HardwareSerial * port, int tXpin, int TXFlagGPIO = -1) { 78 | _port = port; 79 | myTXPin=tXpin; 80 | #if defined ARDUINO_ARCH_ESP32 81 | port->begin(_baud); 82 | pinMode(myTXPin, OUTPUT|PULLUP); 83 | #elif defined(CORE_TEENSY) 84 | pinMode(myTXPin, OUTPUT_OPENDRAIN); 85 | port->begin(_baud, SERIAL_8N1); 86 | port->setTX(myTXPin, true); 87 | 88 | #else 89 | pinMode(myTXPin, OUTPUT); 90 | port->begin(_baud, SERIAL_8N1); 91 | #endif 92 | delay(3); 93 | while (port->available()) 94 | port->read(); 95 | if(TXFlagGPIO>=0)setTXFlag(TXFlagGPIO); 96 | 97 | } 98 | 99 | 100 | // time returns the number of ms to TX/RX n characters 101 | uint32_t time(uint32_t n) { 102 | return n * 10 * 1000 / _baud; // 10 bits per char 103 | } 104 | uint32_t timeus(uint32_t n) { 105 | return n * 10 * 1000000 / _baud; // 10 bits per char 106 | } 107 | // methods passed through to Serial 108 | bool available() { 109 | return _port->available(); 110 | } 111 | int read() { 112 | return _port->read(); 113 | } 114 | void write(const uint8_t *buf, int buflen) { 115 | 116 | #if defined ARDUINO_ARCH_ESP32 117 | pinMode(myTXPin, OUTPUT|PULLUP); 118 | #elif defined(CORE_TEENSY) 119 | _port->setTX(myTXPin, false); 120 | #endif 121 | // _port->write(buf, buflen); 122 | // _port->flush(); 123 | if(!singlePinMode){ 124 | delayMicroseconds(10); 125 | for(int i=0;iwrite(buf[i]); 127 | _port->flush(); 128 | } 129 | } 130 | if(singlePinMode){ 131 | _port->write(buf, buflen); 132 | _port->flush(); 133 | } 134 | #if defined ARDUINO_ARCH_ESP32 135 | if(singlePinMode) 136 | pinMode(myTXPin, INPUT|PULLUP); 137 | else 138 | pinMode(myTXPin, OUTPUT|PULLUP); 139 | #elif defined(CORE_TEENSY) 140 | _port->setTX(myTXPin, true); 141 | #endif 142 | 143 | 144 | } 145 | int retry = 3; 146 | void setRetryCount(int count) { 147 | retry = count; 148 | } 149 | // write a command with the provided parameters 150 | // returns true if the command was written without conflict onto the bus 151 | bool write_no_retry(uint8_t cmd, const uint8_t *params, int param_cnt, 152 | uint8_t MYID); 153 | 154 | // read sends a command to the servo and reads back the response into the params buffer. 155 | // returns true if everything checks out correctly. 156 | bool read_no_retry(uint8_t cmd, uint8_t *params, int param_len, 157 | uint8_t MYID); 158 | // read sends a command to the servo and reads back the response into the params buffer. 159 | // returns true if everything checks out correctly. 160 | bool rcv(uint8_t cmd, uint8_t *params, int param_len, 161 | uint8_t MYID); 162 | // write a command with the provided parameters 163 | // returns true if the command was written without conflict onto the bus 164 | bool write(uint8_t cmd, const uint8_t *params, int param_cnt, 165 | uint8_t MYID) { 166 | if (retry == 0) { 167 | return write_no_retry(cmd, params, param_cnt, MYID); 168 | } else { 169 | for (int i = 0; i < retry; i++) { 170 | bool ok = write_no_retry(cmd, params, param_cnt, MYID); 171 | if (ok) 172 | return true; 173 | } 174 | } 175 | return false; 176 | } 177 | 178 | // read sends a command to the servo and reads back the response into the params buffer. 179 | // returns true if everything checks out correctly. 180 | bool read(uint8_t cmd, uint8_t *params, int param_len, uint8_t MYID) { 181 | if (retry == 0) { 182 | return read_no_retry(cmd, params, param_len, MYID); 183 | } else { 184 | for (int i = 0; i < retry; i++) { 185 | bool ok = read_no_retry(cmd, params, param_len, MYID); 186 | if (ok) 187 | return true; 188 | } 189 | } 190 | return false; 191 | } 192 | 193 | HardwareSerial * _port = NULL; 194 | int _baud = 115200; 195 | /** 196 | * Command name: SERVO_LOAD_OR_UNLOAD_WRITE 197 | Command value: 31 Length: 4 198 | Parameter 1: Whether the internal motor of the servo is unloaded power-down 199 | or not, the range 0 or 1, 0 represents the unloading power down, and the servo 200 | has no torque output. 1 represents the loaded motor, then the servo has a 201 | torque output, the default value is 0. 202 | @return sucess 203 | */ 204 | bool disableAll() { 205 | uint8_t params[] = { 0 }; 206 | return write(LX16A_SERVO_LOAD_OR_UNLOAD_WRITE, params, 1, 207 | LX16A_BROADCAST_ID); 208 | } 209 | /** 210 | * Command name: SERVO_LOAD_OR_UNLOAD_WRITE 211 | Command value: 31 Length: 4 212 | Parameter 1: Whether the internal motor of the servo is unloaded power-down 213 | or not, the range 0 or 1, 0 represents the unloading power down, and the servo 214 | has no torque output. 1 represents the loaded motor, then the servo has a 215 | torque output, the default value is 0. 216 | @return sucess 217 | */ 218 | bool enableAll() { 219 | uint8_t params[] = { 1 }; 220 | return write(LX16A_SERVO_LOAD_OR_UNLOAD_WRITE, params, 1, 221 | LX16A_BROADCAST_ID); 222 | } 223 | /** 224 | * Command name: SERVO_MOVE_START Command value: 11 225 | Length: 3 226 | With the use of command SERVO_MOVE_TIME_WAIT_WRITE, described in 227 | point 3 228 | @return sucess 229 | */ 230 | bool move_sync_start() { 231 | uint8_t params[1]; 232 | return write(LX16A_SERVO_MOVE_START, params, 1, 233 | LX16A_BROADCAST_ID); 234 | } 235 | /** 236 | * Command name: SERVO_MOVE_STOP Command value: 12 237 | Length: 3 238 | When the command arrives at the servo, it will stop running 239 | This is sent to all servos at once 240 | */ 241 | void stopAll() { 242 | uint8_t params[1]; 243 | write(LX16A_SERVO_MOVE_STOP, params, 1, 244 | LX16A_BROADCAST_ID); 245 | } 246 | // id_read returns the ID of the servo, useful if the id is 0xfe, which is broadcast... 247 | uint8_t id_read() { 248 | uint8_t params[1]; 249 | if (!read(LX16A_SERVO_ID_READ, params, 1, LX16A_BROADCAST_ID)) { 250 | return 0; 251 | } 252 | return params[0]; 253 | 254 | } 255 | 256 | // id_write sets the id of the servo, updates the object's id if write appears successful 257 | void id_write(uint8_t id) { 258 | uint8_t params[] = { id }; 259 | write(LX16A_SERVO_ID_WRITE, params, 1, LX16A_BROADCAST_ID); 260 | } 261 | }; 262 | 263 | class LX16AServo { 264 | private: 265 | bool commandOK = true; 266 | int32_t lastKnownGoodPosition = 0; 267 | bool isMotorMode = false; 268 | bool isInitialized = false; 269 | //private: 270 | LX16ABus * _bus; 271 | 272 | 273 | public: 274 | int32_t staticOffset =0; 275 | int32_t maxCentDegrees =240000; 276 | int32_t minCentDegrees =0; 277 | 278 | uint8_t _id = LX16A_BROADCAST_ID; 279 | LX16AServo(LX16ABus * bus, int id) : 280 | _bus(bus), _id(id) { 281 | } 282 | /** 283 | * Set the current position as a specific angle. 284 | * 285 | * This function will first read the current position, then 286 | * calculate an offset to be applied to all position reads and writes. 287 | * The offset will make convert the current position to the value passed in 288 | * 289 | * This is a function to be called when the motor hits a limit switch 290 | * and load in a specific value when the limit is reached 291 | * 292 | */ 293 | bool calibrate(int32_t currentAngleCentDegrees,int32_t min_angle_cent_deg, int32_t max_angle_cent_deg){ 294 | if(min_angle_cent_deg>=max_angle_cent_deg){ 295 | Serial.println("Min can not be greater than max for "+String(_id)+" halting"); 296 | while(1); 297 | } 298 | int32_t current; 299 | initialize(); 300 | do{ 301 | pos_read(); 302 | current=pos_read_cached(); 303 | if(!isCommandOk()){ 304 | Serial.println("Calibration read FAILED! on index "+String(_id)); 305 | 306 | } 307 | }while(!isCommandOk());// this is a calibration and can not be allowed to fail 308 | 309 | staticOffset=currentAngleCentDegrees-current; 310 | int32_t min_angle_in_Ticks = (min_angle_cent_deg-staticOffset) / 24; 311 | int32_t max_angle_in_Ticks = (max_angle_cent_deg-staticOffset) / 24; 312 | int32_t currentTicks = current/24; 313 | int32_t angularOffset =1450; 314 | int32_t angularOffsetTicks =angularOffset/24; 315 | if(min_angle_in_Ticks<0||max_angle_in_Ticks>1000){ 316 | int32_t theoretivalMinError = currentAngleCentDegrees-min_angle_cent_deg; 317 | int32_t theoretivalMinErrorTicks = theoretivalMinError/24; 318 | int32_t newSetpointTicks = theoretivalMinErrorTicks+angularOffsetTicks; 319 | int32_t newAngle = (newSetpointTicks*24)+staticOffset; 320 | Serial.println("ERROR! bounds of servo ID "+String(_id)+" can not be outside hardware limit"); 321 | Serial.println("\tlower "+String(min_angle_in_Ticks)+" ticks (Must be > 0)"); 322 | Serial.println("\tcurrent "+String(currentTicks)+" ticks "+String(pos_read_cached()+staticOffset)+"deg"); 323 | Serial.println("\tupper "+String(max_angle_in_Ticks)+" ticks (Must be < 1000)"); 324 | Serial.println("\terror "+String(theoretivalMinErrorTicks)+" ticks "); 325 | Serial.println("\tnewset "+String(newSetpointTicks)+" ticks "); 326 | Serial.println("\tnewset deg "+String(newAngle)+" centDeg "); 327 | 328 | min_angle_in_Ticks=0; 329 | max_angle_in_Ticks=1000; 330 | minCentDegrees= (min_angle_in_Ticks*24)+staticOffset; 331 | maxCentDegrees= ((max_angle_in_Ticks)*24)+staticOffset; 332 | setLimitsTicks(min_angle_in_Ticks,max_angle_in_Ticks); 333 | move_time(newAngle,0); 334 | delay(500); 335 | return false; 336 | }else{ 337 | Serial.println("\nBounds of servo ID "+String(_id)+" ok!"); 338 | Serial.println("\tlower "+String(min_angle_in_Ticks)+" ticks "); 339 | Serial.println("\tcurrent "+String(currentTicks)+" ticks "); 340 | Serial.println("\tupper "+String(max_angle_in_Ticks)+" ticks "); 341 | } 342 | setLimitsTicks(min_angle_in_Ticks,max_angle_in_Ticks); 343 | minCentDegrees= (min_angle_in_Ticks*24)+staticOffset; 344 | maxCentDegrees= ((max_angle_in_Ticks)*24)+staticOffset; 345 | if(abs(min_angle_cent_deg-minCentDegrees)>24) 346 | Serial.println("FAULT Min angle desired was "+String(min_angle_cent_deg)+" got "+String(minCentDegrees)); 347 | if(abs(max_angle_cent_deg-maxCentDegrees)>24) 348 | Serial.println("FAULT max angle desired was "+String(max_angle_cent_deg)+" got "+String(maxCentDegrees)); 349 | return true; 350 | } 351 | void setLimitsTicks(int32_t lower,int32_t upper){ 352 | if(lower<0) 353 | lower=0; 354 | if(upper>1000) 355 | upper=1000; 356 | for(int i=0;i<2;i++){ 357 | uint8_t params[] = { (uint8_t) lower, (uint8_t) (lower 358 | >> 8),(uint8_t) upper, (uint8_t) (upper >> 8) }; 359 | commandOK = _bus->write(LX16A_SERVO_ANGLE_LIMIT_WRITE, params, 360 | 4, _id); 361 | 362 | if(isCommandOk()){ 363 | Serial.println("Set Limit MotorID:"+String(_id)+" Min set "+String(((lower*24)+staticOffset))+" max = "+String((upper*24)+staticOffset)); 364 | return; 365 | } 366 | if(_bus->_debug) 367 | Serial.println("Set Limits Failed ID "+String(_id)+" Lower "+String(lower)+" upper: "+String(upper)+" Retry #"+String(i)); 368 | 369 | } 370 | } 371 | int32_t getMinCentDegrees(){ 372 | return minCentDegrees; 373 | } 374 | int32_t getMaxCentDegrees(){ 375 | return maxCentDegrees; 376 | } 377 | bool isCommandOk() { 378 | return commandOK; 379 | } 380 | void initialize() { 381 | if (isInitialized) { 382 | return; 383 | } 384 | isInitialized = true; 385 | motor_mode(0); 386 | pos_read(); 387 | readLimits(); 388 | delay(1); 389 | } 390 | 391 | void readLimits(){ 392 | uint8_t params[4]; 393 | int numFail=0; 394 | do{ 395 | if (!_bus->read(LX16A_SERVO_ANGLE_LIMIT_READ, params, 4, _id)) { 396 | commandOK = false; 397 | if(_bus->_debug){ 398 | Serial.println("ERROR reading limits #"+String(_id)); 399 | } 400 | } else { 401 | commandOK = true; 402 | int lowicks = (params[0] | ((uint16_t) params[1] << 8)); 403 | int highticks = (params[2] | ((uint16_t) params[3] << 8)); 404 | 405 | minCentDegrees = (lowicks * 24) + staticOffset; 406 | maxCentDegrees = (highticks * 24) + staticOffset; 407 | if (minCentDegrees > maxCentDegrees) { 408 | Serial.println( 409 | "ERR MotorID:" + String(_id) + " Min set " 410 | + String(minCentDegrees) + " max = " 411 | + String(maxCentDegrees) + " Min ticks " 412 | + String(lowicks) + " max ticks = " 413 | + String(highticks)); 414 | 415 | maxCentDegrees = 24000; 416 | minCentDegrees = 0; 417 | } else 418 | Serial.println( 419 | "MotorID:" + String(_id) + " Min set " 420 | + String(minCentDegrees) + " max = " 421 | + String(maxCentDegrees)); 422 | } 423 | }while(!isCommandOk()&&numFail++<3);// this is a calibration and can not be allowed to fail 424 | } 425 | 426 | /** 427 | * Length: 7 428 | Parameter 1: lower 8 bits of angle value 429 | Parameter 2: higher 8 bits of angle value.range 0~100. corresponding to the 430 | servo angle of 0 ~ 240 °, that means the minimum angle of the servo can be 431 | varied is 0.24 degree. 432 | Parameter 3: lower 8 bits of time value 433 | Parameter 4: higher 8 bits of time value. the range of time is 0~30000ms. 434 | When the command is sent to servo, the servo will be rotated from current 435 | angle to parameter angle at uniform speed within param 436 | */ 437 | void move_time(int32_t angle, uint16_t time) { 438 | initialize(); 439 | if(angle> maxCentDegrees){ 440 | 441 | Serial.println("ERROR Capped set at max "+String(maxCentDegrees)+" attempted "+String(angle)); 442 | angle=maxCentDegrees; 443 | } 444 | if(angle1000){ 452 | angle=1000; 453 | } 454 | if(angle<0){ 455 | angle=0; 456 | } 457 | Serial.println("Setting ticks "+String(angle)+" on ID "+String(_id)); 458 | uint8_t params[] = { (uint8_t) angle, (uint8_t) (angle >> 8), 459 | (uint8_t) time, (uint8_t) (time >> 8) }; 460 | commandOK = _bus->write(LX16A_SERVO_MOVE_TIME_WRITE, params, 4, _id); 461 | 462 | } 463 | /** 464 | * Command name: SERVO_MOVE_TIME_WAIT_WRITE 465 | Command value: 7 466 | Length : 7 467 | Parameter1: lower 8 bits of preset angle 468 | Parameter2: higher 8 bits of preset angle. range 0~100. corresponding to the 469 | servo angle of 0 ~ 240 °. that means the minimum angle of the servo can be 470 | varied is 0.24 degree. 471 | Parameter3: lower 8 bits of preset time 472 | Parameter3: higher 8 bits of preset time. the range of time is 0~30000ms. 473 | The function of this command is similar to this 474 | “SERVO_MOVE_TIME_WRITE” command in the first point. But the difference 475 | is that the servo will not immediately turn when the command arrives at the 476 | servo,the servo will be rotated from current angle to parameter angle at unifor 477 | m speed within parameter time until the command name SERVO_MOVE_ST 478 | ART sent to servo(command value of 11) 479 | , then the servo will be rotate 480 | */ 481 | void move_time_and_wait_for_sync(int32_t angle, uint16_t time) { 482 | initialize(); 483 | if(angle> maxCentDegrees){ 484 | angle=maxCentDegrees; 485 | Serial.println("ERROR Capped set at max "+String(maxCentDegrees)); 486 | } 487 | if(angle_debug) 494 | // Serial.println("Setting motor to "+String(angle)); 495 | angle = (angle-staticOffset) / 24; 496 | uint8_t params[] = { (uint8_t) angle, (uint8_t) (angle >> 8), 497 | (uint8_t) time, (uint8_t) (time >> 8) }; 498 | commandOK = _bus->write(LX16A_SERVO_MOVE_TIME_WAIT_WRITE, params, 4, 499 | _id); 500 | 501 | } 502 | 503 | /** 504 | * Command name: SERVO_MOVE_STOP Command value: 12 505 | Length: 3 506 | When the command arrives at the servo, it will stop running 507 | */ 508 | void stop() { 509 | uint8_t params[1]; 510 | commandOK = _bus->write(LX16A_SERVO_MOVE_STOP, params, 1, _id); 511 | } 512 | 513 | /** 514 | * Command name: SERVO_LOAD_OR_UNLOAD_WRITE 515 | Command value: 31 Length: 4 516 | Parameter 1: Whether the internal motor of the servo is unloaded power-down 517 | or not, the range 0 or 1, 0 represents the unloading power down, and the servo 518 | has no torque output. 1 represents the loaded motor, then the servo has a 519 | torque output, the default value is 0. 520 | */ 521 | void disable() { 522 | uint8_t params[] = { 0 }; 523 | commandOK = _bus->write(LX16A_SERVO_LOAD_OR_UNLOAD_WRITE, params, 1, _id); 524 | } 525 | /** 526 | * Command name: SERVO_LOAD_OR_UNLOAD_WRITE 527 | Command value: 31 Length: 4 528 | Parameter 1: Whether the internal motor of the servo is unloaded power-down 529 | or not, the range 0 or 1, 0 represents the unloading power down, and the servo 530 | has no torque output. 1 represents the loaded motor, then the servo has a 531 | torque output, the default value is 0. 532 | */ 533 | void enable() { 534 | uint8_t params[] = { 1 }; 535 | commandOK = _bus->write(LX16A_SERVO_LOAD_OR_UNLOAD_WRITE, params, 1, _id); 536 | } 537 | 538 | /** 539 | * Command name: SERVO_OR_MOTOR_MODE_WRITE 540 | Command value: 29 541 | Length: 7 542 | Parameter 1: Servo mode, range 0 or 1, 0 for position control mode, 1 for 543 | motor control mode, default 0, 544 | Parameter 2: null value 545 | Parameter 3: lower 8 bits of rotation speed value 546 | Parameter 4: higher 8 bits of rotation speed value. range -1000~1000, 547 | Only in the motor control mode is valid, control the motor speed, the value of 548 | the negative value represents the reverse, positive value represents the 549 | forward rotation. Write mode and speed do not support power-down save. 550 | Note: Since the rotation speed is the “signed short int” type of data, it is forced 551 | to convert the data to convert the data to “unsigned short int “type of data before sending the 552 | command packet. 553 | */ 554 | void motor_mode(int16_t speed) { 555 | bool isMotorMode_tmp = speed != 0; 556 | uint8_t params[] = { (uint8_t) (isMotorMode_tmp ? 1 : 0), 0, 557 | (uint8_t) speed, (uint8_t) (speed >> 8) }; 558 | commandOK = _bus->write(LX16A_SERVO_OR_MOTOR_MODE_WRITE, params, 4, 559 | _id); 560 | if (commandOK) 561 | isMotorMode = isMotorMode_tmp; 562 | } 563 | // angle_adjust sets the position angle offset in centi-degrees (-3000..3000) 564 | void angle_offset_save() { 565 | uint8_t params[1]; 566 | _bus-> write(LX16A_SERVO_ANGLE_OFFSET_WRITE, params, 1, 567 | _id); 568 | } 569 | // angle_adjust sets the position angle offset in centi-degrees (-3000..3000) 570 | void angle_offset_adjust(int16_t angle) { 571 | int32_t tmp = (int32_t) angle; 572 | 573 | uint8_t params[] = { (uint8_t) tmp}; 574 | commandOK = _bus->write(LX16A_SERVO_ANGLE_OFFSET_ADJUST, params, 1, 575 | _id); 576 | } 577 | // angle_adjust sets the position angle offset in centi-degrees (-3000..3000) 578 | int16_t read_angle_offset() { 579 | uint8_t params[1]; 580 | if (!_bus->read(LX16A_SERVO_ANGLE_OFFSET_READ, params, 1, _id)) { 581 | commandOK = false; 582 | return 0; 583 | } 584 | commandOK = true; 585 | return params[0] ; 586 | } 587 | 588 | 589 | // pos_read returns the servo position in centi-degrees (0..24000) 590 | int32_t pos_read() { 591 | initialize(); 592 | uint8_t params[3]; 593 | if (!_bus->read(LX16A_SERVO_POS_READ, params, 2, _id)) { 594 | if(_bus->_debug) 595 | Serial.print("Position Read failed "+String(_id)+"\n\n"); 596 | commandOK = false; 597 | return pos_read_cached()+staticOffset; 598 | } 599 | commandOK = true; 600 | lastKnownGoodPosition = ((int16_t) params[0] 601 | | ((int16_t) params[1] << 8)) * 24; 602 | return pos_read_cached()+staticOffset; 603 | } 604 | /** 605 | * Get the cached position from the most recent read 606 | */ 607 | int32_t pos_read_cached() { 608 | return lastKnownGoodPosition; 609 | } 610 | 611 | // id_read returns the ID of the servo, useful if the id is 0xfe, which is broadcast... 612 | uint8_t id_read() { 613 | uint8_t params[1]; 614 | if (!_bus->read(LX16A_SERVO_ID_READ, params, 1, LX16A_BROADCAST_ID)) { 615 | commandOK = false; 616 | return 0; 617 | } 618 | commandOK = true; 619 | return params[0]; 620 | 621 | } // id_read returns the ID of the servo using its ID for verification 622 | uint8_t id_verify() { 623 | uint8_t params[1]; 624 | if (!_bus->read(LX16A_SERVO_ID_READ, params, 1, _id)) { 625 | commandOK = false; 626 | return 0; 627 | } 628 | commandOK = true; 629 | return params[0]; 630 | 631 | } 632 | 633 | // id_write sets the id of the servo, updates the object's id if write appears successful 634 | void id_write(uint8_t id) { 635 | uint8_t params[] = { id }; 636 | bool ok = _bus->write(LX16A_SERVO_ID_WRITE, params, 1, LX16A_BROADCAST_ID); 637 | if (ok && _id != LX16A_BROADCAST_ID) 638 | _id = id; 639 | commandOK = ok; 640 | } 641 | /** 642 | * Command name: SERVO_OR_MOTOR_MODE_READ 643 | Command value: 30 Length: 3 644 | Read the relative values of the servo, for the details of the command package 645 | that the servo returns to host computer, please refer to the description of Table 646 | 4 below. 647 | */ 648 | bool readIsMotorMode() { 649 | 650 | uint8_t params[4]; 651 | if (!_bus->read(LX16A_SERVO_OR_MOTOR_MODE_READ, params, 4, _id)) { 652 | commandOK = false; 653 | return false; 654 | } 655 | commandOK = true; 656 | isMotorMode = params[0] == 1; 657 | return isMotorMode; 658 | } 659 | 660 | 661 | // temp_read returns the servo temperature in centigrade 662 | uint8_t temp() { 663 | uint8_t params[1]; 664 | if (!_bus->read(LX16A_SERVO_TEMP_READ, params, 1, _id)) { 665 | commandOK = false; 666 | return 0; 667 | } 668 | commandOK = true; 669 | return params[0]; 670 | } 671 | 672 | // vin_read returns the servo input voltage in millivolts 673 | uint16_t vin() { 674 | uint8_t params[2]; 675 | if (!_bus->read(LX16A_SERVO_VIN_READ, params, 2, _id)) { 676 | commandOK = false; 677 | return 0; 678 | } 679 | commandOK = true; 680 | return params[0] | ((uint16_t) params[1] << 8); 681 | } 682 | 683 | }; 684 | 685 | --------------------------------------------------------------------------------