Электро корректор ПТФ

[Виталтй]

Нужна помощь в корректировке программы, программа работает уже около года, привода работают, при нажатии на кнопку энкодера, правой сервой можно корректировать туманку, настраивать горизонт, потому что даже на одинаковых сервах, по разному нарезаны шлицы, и рычаги одеваются не идеально одинаково, и имеют смещение, для этого и нужна настройка одной сервой. В этом и кроется вся проблема, допустим если верхний лимит установлен 180гр, левая серва стоит на 90гр, а правая чтоб выровнять горизонт стоит на 100гр, то когда правая серва достигнет лимит, она встанет, а левая еще продолжит двигаться пока не достигнет лимита в 180гр, от этого возникает рассинхрон птф в крайних положениях, тоже самое и вниз, если левая упрется в лимит, то правая еще нет, и она продолжит двигаться даже если левая уже будет стоять, мне нужна какая то динамическая корректировка, т.е если левая серва стоит 90гр, а правая 100гр, то когда левая достигнет лимита в 180гр, на правой будет 190гр, если левая уйдет в низ условно на 0 гр, то на правой будет 10гр, а не 0, и птф всегда будут в горизонте, я не программист, мне нужна именно помощь, а не просто сказать добавь это или это

#include <Servo.h>
#include <EEPROM.h>
#include <GyverEncoder.h>
#define MAX_DELTA_LEFT 30 // макс.наклон левой фары относительно правой
#define MIN_DELTA_LEFT -20 // min .наклон левой фары относительно правой
#define MAX_LIMIT_SERVO 150 //максимальная высота подъёма фары
#define MIN_LIMIT_SERVO 55 // минимальная высота опускания фары
#define PIN_RIGHT_SERVO 6 // пин сервы правой фары
#define PIN_LED_RIGHT 8 // пин светодиода индикатора работы с правой фарой
#define PIN_LEFT_SERVO 7 // пин сервы левой фары
#define PIN_LED_LEFT 9 // пин светодиода индикатора работы с левой фарой
#define SW A0
#define DT A1
#define CLK A2
#define ADDR_POS 0
#define ADDR_DELTA 1
Encoder enc_all(CLK, DT, SW);
Servo l_servo, r_servo;
uint8_t position_all,position_left;
int8_t delta_left;
void calc_positon_left(void){
position_left=uint8_t(int(position_all)+int(delta_left));
if(position_left>MAX_LIMIT_SERVO)position_left=MAX_LIMIT_SERVO;
if(position_left<MIN_LIMIT_SERVO)position_left=MIN_LIMIT_SERVO;
}
void led_left_on_right_off(void){
if(!digitalRead(PIN_LED_LEFT))digitalWrite(PIN_LED_LEFT,HIGH);
if(digitalRead(PIN_LED_RIGHT))digitalWrite(PIN_LED_RIGHT,LOW);
}
void led_all_on(void){
if(!digitalRead(PIN_LED_LEFT))digitalWrite(PIN_LED_LEFT,HIGH);
if(!digitalRead(PIN_LED_RIGHT))digitalWrite(PIN_LED_RIGHT,HIGH);
}
void set_left_servo(void){
calc_positon_left();
l_servo.write(position_left); // левого -/-
}
void set_all_servo(void){
r_servo.write(position_all); // установка правого сервоприводов
calc_positon_left();
l_servo.write(position_left); // левого -/-
}
void setup() {

Serial.begin(115200);
pinMode(PIN_LED_RIGHT, OUTPUT);
pinMode(PIN_LED_LEFT, OUTPUT);
pinMode(LED_BUILTIN, OUTPUT);
//подключение сервы левой фары
l_servo.attach(PIN_LEFT_SERVO);
// настройки сервы правой фары
r_servo.attach(PIN_RIGHT_SERVO);
position_all=EEPROM.read(ADDR_POS);
uint8_t temp_d=EEPROM.read(ADDR_DELTA);
delta_left=int8_t(temp_d);
if(position_all>MAX_LIMIT_SERVO||position_all<MIN_LIMIT_SERVO||delta_left<MIN_DELTA_LEFT||delta_left>MAX_DELTA_LEFT){
position_all=MIN_LIMIT_SERVO;
delta_left=0;
EEPROM.write(ADDR_POS,position_all);
EEPROM.write(ADDR_DELTA,delta_left);
}
set_all_servo(); // первоначальная установка сервоприводов
led_all_on();
}
void loop() {
enc_all.tick();
if(enc_all.isTurn()){
uint8_t old_pos=position_all;
int8_t old_delta=delta_left;
if(enc_all.isPress()){
led_left_on_right_off();
}else{
led_all_on();
}
if(enc_all.isRightH()){
if(delta_left<MAX_DELTA_LEFT) delta_left++;
Serial.print("delta left ++ ");
Serial.println(delta_left);
}else{
if(enc_all.isRight()){
if(position_all<MAX_LIMIT_SERVO) position_all++;
Serial.print("all ++ ");
Serial.println(position_all);
}
}
if(enc_all.isLeftH()){
if(delta_left>MIN_DELTA_LEFT) delta_left--;
Serial.print("delta left -- ");
Serial.println(delta_left);
}else{
if(enc_all.isLeft()){
if(position_all>MIN_LIMIT_SERVO)position_all--;
Serial.print("all -- ");
Serial.println(position_all);
}
}
if(old_delta!=delta_left){
EEPROM.write(ADDR_DELTA,delta_left);
set_left_servo();
}
if(old_pos!=position_all){
EEPROM.write(ADDR_POS,position_all); // если были изменения сохраняем данные в энерго независимую память
set_all_servo();
}
}
}