While I will admit that smart servos are a niche for those that cannot code their own PWM + rotary encoder + PID. It is even harder for those that cannot code their own motion control algorithms (RPM, ac/de-celeration, hold stiffness, etc); which is me.
With the help of @phillip1, I think there is some really useful code to share that brings Lynxmotion LSS smart servos into the ESPHome world. And while not entirely exhaustive, it is a solid foundation to add any missing feature from the communication protocol.
Why a Smart Servo?
Well letâs cover the benefits;
- far simpler to deploy with existing libraries
- can be daisy chained
- highly configurable out of the box
Now I understand that they cost 10X the price of analogue servo. Thatâs what sales are for!
Lynxmotion Servo Control
The LSS Communication Protocol is actually quite ESPHome friendly by using simple text parsing and no CRC/checksumâs. We can not only control the direction, speed and holding power; but have live feedback of current values! This is benefitial to ensure that HA is in sync w/ the servoâs current state.
In addition to âknowning stateâ, you can control behavior beyond just movement. You can control whether the server âholdsâ or resists outside forces. Or if it should go âlimpâ and allow free movement. All of a sudden, your servo isnât controlling something, it is measuring!
With a single servo up and running, youâll want to add a second. This is where the below code becomes more useful. As long as you follow the naming convention, all state will be captured without additional coding.
YAML Naming Convention
Prefix: servo# where # is an identifier digit from 1 to 9. ID 0 is reserved as a default for all servos and should be avoided
Name: _NAME where NAME is the description matching a configuration parameter including;
- led
- gyre
- state
- degree
- degree_per_second
- angular_acceleration
- angular_deceleration
- hold_mode
- limp_mode
The Code
The .h
This solution leverages a Custom UART Text Sensor with one small tweak. Returning on the â*â character which is indicative of a response from a servo.
if (readch > 0) {
switch (readch) {
case '\n': // Ignore new-lines
break;
case '\r': // Return on CR
rpos = pos;
pos = 0; // Reset position index ready for next time
return rpos;
case '*': // Return on LSS servo response
rpos = pos;
pos = 0; // Reset position index ready for next time
return rpos;
default:
if (pos < len-1) {
buffer[pos++] = readch;
buffer[pos] = 0;
}
}
}
YAML
I have included a two (2) servo example. By following the naming convention you can easily add 3-9 servos with no added C++ lambdaâs necessary to capture state info.
esphome:
includes:
- uart_read_line_sensor_lss.h
logger:
logs:
text_sensor: INFO
switch:
- platform: template
id: servo_query_state
name: servo_query_state
optimistic: true
assumed_state : OFF
- platform: template
id: servo1_gyre
name: servo1_gyre
optimistic: true
assumed_state : ON
turn_on_action:
- uart.write:
id: uart_bus
data: "#1CG1\r"
turn_off_action:
- uart.write:
id: uart_bus
data: "#1CG-1\r"
- platform: template
id: servo2_gyre
name: servo2_gyre
optimistic: true
assumed_state : ON
turn_on_action:
- uart.write:
id: uart_bus
data: "#2CG1\r"
turn_off_action:
- uart.write:
id: uart_bus
data: "#2CG-1\r"
- platform: template
id: servo1_hold_mode
name: servo1_hold_mode
optimistic: true
assumed_state : ON
turn_on_action:
- uart.write:
id: uart_bus
data: "#1H\r"
turn_off_action:
- uart.write:
id: uart_bus
data: "#1L\r"
- platform: template
id: servo2_hold_mode
name: servo2_hold_mode
optimistic: true
assumed_state : ON
turn_on_action:
- uart.write:
id: uart_bus
data: "#2H\r"
turn_off_action:
- uart.write:
id: uart_bus
data: "#2L\r"
- platform: template
id: servo1_limp_mode
name: servo1_limp_mode
optimistic: true
assumed_state : true
turn_on_action:
- uart.write:
id: uart_bus
data: "#1L\r"
turn_off_action:
- uart.write:
id: uart_bus
data: "#1H\r"
- platform: template
id: servo2_limp_mode
name: servo2_limp_mode
optimistic: true
assumed_state : true
turn_on_action:
- uart.write:
id: uart_bus
data: "#2L\r"
turn_off_action:
- uart.write:
id: uart_bus
data: "#2H\r"
- platform: template
id: servo1_led
name: servo1_led
optimistic: true
turn_on_action:
- uart.write:
id: uart_bus
data: "#1CLED5\r"
- delay: 500ms
- uart.write:
id: uart_bus
data: "#1QLED\r"
turn_off_action:
- uart.write:
id: uart_bus
data: "#1CLED0\r"
- delay: 500ms
- uart.write:
id: uart_bus
data: "#1QLED\r"
- platform: template
id: servo2_led
name: servo2_led
optimistic: true
turn_on_action:
- uart.write:
id: uart_bus
data: "#2CLED6\r"
- delay: 500ms
- uart.write:
id: uart_bus
data: "#2QLED\r"
turn_off_action:
- uart.write:
id: uart_bus
data: "#2CLED0\r"
- delay: 500ms
- uart.write:
id: uart_bus
data: "#2QLED\r"
button:
- platform: template
name: servo1_soft_reset
on_press:
- uart.write:
id: uart_bus
data: "#1RESET\r"
- platform: template
name: servo2_soft_reset
on_press:
- uart.write:
id: uart_bus
data: "#2RESET\r"
uart:
- id: uart_bus
tx_pin: GPIO18
rx_pin: GPIO5
baud_rate: 115200
number:
- platform: template
id: servo1_degree_per_second
name: servo1_degree_per_second
step: 1
min_value: 1
max_value: 600
optimistic: true
restore_value: true
initial_value: 360
mode: box
- platform: template
id: servo2_degree_per_second
name: servo2_degree_per_second
step: 1
min_value: 1
max_value: 600
optimistic: true
restore_value: true
initial_value: 360
mode: box
- platform: template
id: servo1_degree
name: servo1_degree
step: 0.1
min_value: -90
max_value: 90
mode: box
set_action:
- uart.write:
id: uart_bus
data: !lambda
std::string D = "#1D" + to_string(int(x*10)) + "SD" + to_string(int(id(servo1_degree_per_second).state*10)) +"\r";
return std::vector<uint8_t>(D.begin(), D.end());
- delay: 1s
- uart.write:
id: uart_bus
data: "#1QD\r"
- platform: template
id: servo2_degree
name: servo2_degree
step: 0.1
min_value: -33
max_value: 110
mode: box
set_action:
- uart.write:
id: uart_bus
data: !lambda
std::string D = "#2D" + to_string(int(x*10)) + "SD" + to_string(int(id(servo2_degree_per_second).state*10)) +"\r";
return std::vector<uint8_t>(D.begin(), D.end());
- delay: 1s
- uart.write:
id: uart_bus
data: "#2QD\r"
- platform: template
id: servo1_angular_acceleration
name: servo1_angular_acceleration
step: 1
min_value: 1
max_value: 360
# optimistic: true
restore_value: true
initial_value: 100
mode: box
set_action:
- uart.write:
id: uart_bus
data: !lambda
std::string CAA = "#1CAA" + to_string(int(x*10)) +"\r";
return std::vector<uint8_t>(CAA.begin(), CAA.end());
- delay: 1s
- uart.write:
id: uart_bus
data: "#1QAA\r"
- platform: template
id: servo2_angular_acceleration
name: servo2_angular_acceleration
step: 1
min_value: 1
max_value: 360
# optimistic: true
restore_value: true
initial_value: 100
mode: box
set_action:
- uart.write:
id: uart_bus
data: !lambda
std::string CAA = "#2CAA" + to_string(int(x*10)) +"\r";
return std::vector<uint8_t>(CAA.begin(), CAA.end());
- delay: 1s
- uart.write:
id: uart_bus
data: "#2QAA\r"
- platform: template
id: servo1_angular_deceleration
name: servo1_angular_deceleration
step: 1
min_value: 1
max_value: 360
# optimistic: true
restore_value: true
initial_value: 100
mode: box
set_action:
- uart.write:
id: uart_bus
data: !lambda
std::string CAD = "#1CAD" + to_string(int(x*10)) +"\r";
return std::vector<uint8_t>(CAD.begin(), CAD.end());
- delay: 1s
- uart.write:
id: uart_bus
data: "#1QAD\r"
- platform: template
id: servo2_angular_deceleration
name: servo2_angular_deceleration
step: 1
min_value: 1
max_value: 360
# optimistic: true
restore_value: true
initial_value: 100
mode: box
set_action:
- uart.write:
id: uart_bus
data: !lambda
std::string CAD = "#2CAD" + to_string(int(x*10)) +"\r";
return std::vector<uint8_t>(CAD.begin(), CAD.end());
- delay: 1s
- uart.write:
id: uart_bus
data: "#2QAD\r"
interval:
- interval: 1s
then:
- uart.write:
id: uart_bus
data: "#1Q\r"
- uart.write:
id: uart_bus
data: "#2Q\r"
- if:
condition:
switch.is_on: servo1_limp_mode
then:
- uart.write:
id: uart_bus
data: "#1QD\r"
- if:
condition:
switch.is_on: servo2_limp_mode
then:
- uart.write:
id: uart_bus
data: "#2QD\r"
text_sensor:
- platform: template
name: servo1_state
id: servo1_state
# internal: true
- platform: template
name: servo2_state
id: servo2_state
# internal: true
- platform: custom
lambda: |-
auto my_custom_sensor = new UartReadLineSensor(id(uart_bus));
App.register_component(my_custom_sensor);
return {my_custom_sensor};
text_sensors:
id: "servo_readline"
internal: true
on_value:
lambda: |-
auto publishNumber = [](std::string idx, std::string sensor, float resp) {
auto sens = App.get_numbers();
for(int i = 0; i < sens.size(); i++) {
auto name = sens[i]->get_name();
auto servo = "servo" + to_string(idx);
if(name.size() > 7 && name.substr(0, 6) == servo) {
if(name.substr(7) == sensor) {
sens[i]->publish_state(resp);
}
}
}
};
auto publishSwitch = [](std::string idx, std::string sensor, int state) {
auto sens = App.get_switches();
for(int i = 0; i < sens.size(); i++) {
auto name = sens[i]->get_name();
auto servo = "servo" + to_string(idx);
if(name.size() > 7 && name.substr(0, 6) == servo) {
if(name.substr(7) == sensor) {
sens[i]->publish_state(state);
}
}
}
};
auto publishTextSensor = [](std::string idx, std::string sensor, std::string stringstate) {
auto sens = App.get_text_sensors();
for(int i = 0; i < sens.size(); i++) {
auto name = sens[i]->get_name();
auto servo = "servo" + to_string(idx);
if(name.size() > 7 && name.substr(0, 6) == servo) {
if(name.substr(7) == sensor) {
sens[i]->publish_state(stringstate);
}
}
}
};
auto ss = [](int pos, int len) {
std::string line = id(servo_readline).state;
return line.substr(pos, len);
};
std::string sid = ss(0,1);
// Degree
if (ss(1,3) == "QD-") {
float QDR = parse_number<float>(ss(3,5)).value();
publishNumber(sid,"degree",(QDR/10));
} else if (ss(1,2) == "QD") {
float QDR = parse_number<float>(ss(3,4)).value();
publishNumber(sid,"degree",(QDR/10));
}
// Angular Acceleration
else if (ss(1,3) == "QAA") {
float QAAR = parse_number<float>(ss(4,4)).value();
publishNumber(sid,"angular_acceleration",(QAAR/10));
}
// Angular Deceleration
else if (ss(1,2) == "QAD") {
float QADR = parse_number<float>(ss(4,4)).value();
publishNumber(sid,"angular_deceleration",(QADR/10));
}
// LED
else if (ss(1,4) == "QLED") {
publishSwitch(sid,"led",(int)(ss(5,1) != "0"));
}
// Unknown Mode
else if (ss(1,2) == "Q0") {
publishTextSensor(sid,"state","Unknown");
}
// Limp Mode
else if (ss(1,2) == "Q1") {
publishSwitch(sid,"hold_mode",0);
publishSwitch(sid,"limp_mode",1);
publishTextSensor(sid,"state","Limp");
}
// Hold Mode
else if (ss(1,2) == "Q6") {
publishSwitch(sid,"hold_mode",1);
publishSwitch(sid,"limp_mode",0);
publishTextSensor(sid,"state","Hold");
}
// Stuck Mode
else if (ss(1,2) == "Q8") {
publishTextSensor(sid,"state","Stuck");
}
// Blocked Mode
else if (ss(1,2) == "Q9") {
publishTextSensor(sid,"state","Blocked");
}
// Safe Mode
else if (ss(1,3) == "Q10") {
publishTextSensor(sid,"state","SafeMode");
}
// Gyre Counter-Clockwise
else if (ss(1,3) == "QG-") {
publishSwitch(sid,"gyre",0);
}
// Gyre Clockwise
else if (ss(1,3) == "QG1") {
publishSwitch(sid,"gyre",1);
}