PID Controllo con 2 motori

Buongiorno sto cercando di programmare un controllo pid per due motori che hanno dei target diversi per ognuno di essi, ho un problema che quando uno dei due target viene raggiunto da uno dei due motori, l'altro motore inizia a rallentare prima di raggiungere la posizione desiderata finche non la raggiunge. In teoria dovrebbe avere velocità quasi sempre costante e solo in prossimità del target iniziare a rallentare. Sembra un problema di sincronizzazione delle velocità, ho provato a fare di tutto ma non riesco a risolvere. Grazie in anticipo se qualcuno sa come risolvere.

Questo è lo sketch:

#include link rimosso ;
#include
const unsigned int IN1 = 7;
const unsigned int IN2 = 8;
const unsigned int EN = 9;

const unsigned int IN3 = 11;
const unsigned int IN4 = 6;
const unsigned int ENB = 10;

// pin encoder with interrupt
#define encoder0PinA 4
#define encoder0PinB 3
volatile int pinAstateCurrent = LOW; // Current state of Pin A
volatile int pinAStateLast = pinAstateCurrent; // Last read value of Pin A

#define encoder1PinA 5
#define encoder1PinB 2
volatile int pinBstateCurrent = LOW; // Current state of Pin B
volatile int pinBStateLast = pinBstateCurrent; // Last read value of Pin B

int pos=0;
int posB = 0;
//for control with pid
long prevT =0;
float eprev=0;
float eintegral = 0;

long prevT_b =0;
float eprev_b=0;
float eintegral_b = 0;

//L298N motorA(EN, IN1, IN2);
//L298N motorB(ENB, IN3, IN4);
L298NX2 motors(EN, IN1, IN2, ENB, IN3, IN4);
void setup() {
Serial.begin(9600);
//start controller

pinMode(encoder0PinA, INPUT);
pinMode(encoder0PinB, INPUT);
pinMode(encoder1PinA, INPUT);
pinMode(encoder1PinB, INPUT);

attachInterrupt(digitalPinToInterrupt(encoder0PinB),readEncoderA,CHANGE);
attachInterrupt(digitalPinToInterrupt(encoder1PinB),readEncoderB,CHANGE);

}

void loop() {

Motor1();
motor2();

}

void readEncoderA(){
pinAstateCurrent = digitalRead(encoder0PinA); // Read the current state of Pin A

// If there is a minimal movement of 1 step
if ((pinAStateLast == LOW) && (pinAstateCurrent == HIGH)) {

if (digitalRead(encoder0PinB) == HIGH) { // If Pin B is HIGH
pos++;
} else {
pos--;
}

}

pinAStateLast = pinAstateCurrent;
}

void readEncoderB(){
pinBstateCurrent = digitalRead(encoder1PinA); // Read the current state of Pin A

// If there is a minimal movement of 1 step
if ((pinBStateLast == LOW) && (pinBstateCurrent == HIGH)) {

if (digitalRead(encoder1PinB) == HIGH) { // If Pin B is HIGH
posB++;
} else {
posB--;
}

}

pinBStateLast = pinBstateCurrent;
}

void Motor1(){
int targetA=-500*sin(prevT/1e6);//valore dell'encoder come posizione finale;

// val prof kp = 5 , ki = 1 , kd = 0.01;
//PID constants
float kp = 1; //costante proportional
float kd = 0.025; //costante derivate
float ki = 0;//costante integral

/* val predefiniti
float kp = 1; //costante proportional
float kd = 0; //costante derivate
float ki = 0;//costante integral
*/

//time difference
long currT = micros();

float deltaT = ((float)(currT-prevT)) link rimosso ;
prevT = currT;




//error
int e = pos-targetA;




//derivate
float dedt = (e-eprev)/(deltaT);

//integral
eintegral = eintegral + e*deltaT;

//control signal
float u = kp*e + kd*dedt + ki*eintegral;

//motor power
float pwr = fabs(u);

link rimosso ("PWR : ");
link rimosso ln(pwr);

if(pwr>255){ //controllo velocità forse 400
pwr = 255;

}
if(pwr<30){ //controllo velocità forse 400
//pwr = 30;

}

//motor direction
int dir = 1;
if(u<0){
dir = -1;
}




//signal the motor speed
ATOMIC_BLOCK(ATOMIC_RESTORESTATE){

link rimosso (pwr);
link rimosso (pwr_b);

if(dir==1){
link rimosso ();
digitalWrite(7, LOW); // Impostazione della direzione “avanti” del motore collegato al canale A
digitalWrite(8, HIGH); // Disattivazione del brake del motore per il canale A
analogWrite(9,pwr);
// Serial.println("AVANTI_A");
}
if(dir==-1){
link rimosso ();
digitalWrite(7, HIGH); // Impostazione della direzione “avanti” del motore collegato al canale A
digitalWrite(8, LOW); // Disattivazione del brake del motore per il canale A
analogWrite(9,pwr);
link rimosso ln("INDIETRO_A");
}
if(pwr == 0){
link rimosso ();
digitalWrite(7, LOW); // Impostazione della direzione “avanti” del motore collegato al canale A
digitalWrite(8, LOW);
link rimosso ln("STOP_A");
}

}

// store previous error
eprev = e;




Serial.print(targetA);
Serial.print(" ");
Serial.print(pos);
Serial.println();

}

void motor2(){

int targetB=750*sin(prevT/1e6); //valore dell'encoder come posizione finale;
// val prof kp = 5 , ki = 1 , kd = 0.01;
//PID constant
float kp_b = 1; //costante proportional
float kd_b = 0.025; //costante derivate
float ki_b = 0;//costante integral
/* val predefiniti
float kp = 1; //costante proportional
float kd = 0; //costante derivate
float ki = 0;//costante integral
*/

//time difference

long currT_b = micros();




float deltaT_b = ((float)(currT_b-prevT_b)) link rimosso ;
prevT_b = currT_b;

//error

int e_b = posB-targetB;

//derivate

float dedt_b = (e_b-eprev_b)/(deltaT_b);

//integral

eintegral_b = eintegral_b + e_b*deltaT_b;

//control signal

float u_b = kp_b*e_b + kd_b*dedt_b + ki_b*eintegral_b;

//motor power

float pwr_b = fabs(u_b);
link rimosso ("PWR : ");
link rimosso ln(pwr);

if(pwr_b>255){ //controllo velocità forse 400
pwr_b = 255;

}
if(pwr_b<30){ //controllo velocità forse 400
pwr_b = 30;

}




int dir_b = 1;
if(u_b<0){
dir_b = -1;
}

//signal the motor speed
ATOMIC_BLOCK(ATOMIC_RESTORESTATE){

link rimosso (pwr);
link rimosso (pwr_b);

if(dir_b==1){
link rimosso ();
digitalWrite(11, LOW); // Impostazione della direzione “avanti” del motore collegato al canale A
digitalWrite(6, HIGH); // Disattivazione del brake del motore per il canale A
analogWrite(10, pwr_b); //motoreB
link rimosso ln("AVANTI_B");
}
if(dir_b==-1){
link rimosso ();
digitalWrite(11, HIGH); // Impostazione della direzione “avanti” del motore collegato al canale A
digitalWrite(6, LOW); // Disattivazione del brake del motore per il canale A
analogWrite(10, pwr_b); //motoreB
link rimosso ln("INDIETRO_B");
}
if(pwr_b == 0){
link rimosso ();
digitalWrite(11, LOW); // Impostazione della direzione “avanti” del motore collegato al canale A
digitalWrite(6, LOW); // Disattivazione del brake del motore per il canale A
analogWrite(10, pwr_b); //motoreB
link rimosso ln("STOP_B");
}
}

// store previous error

eprev_b = e_b;




Serial.print(targetB);
Serial.print(" ");
Serial.print(posB);
Serial.print(" ");
Serial.println();

}