Problema PID non funzionante

[littlebanana]

Ciao a tutti, sto attualmente costruendo un robottino per delle gare nazionali.....Volevo dotarlo di pid dato che sarebbe molto utile per compiere sterzate di 90 gradi senza superarli....Però sto avendo non pochi problemi a farlo funzionare e avrei bisogno di qualcuno che se ne intenda di PID che come input ha un bno055......allego qui il codice che ho provato a fare e che funziona ma malissimo......Grazie mille.

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <Servo.h>

Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28, &Wire);

float target = 0;
double error = 0;
double proportional = 0;
double integral = 0;
double derivative = 0;
double last_error = 0;

double dt,last_time=0;

double Kp = 5;
double Ki = 0;
double Kd = 0;

int mtrSpd = 60;

float angle = 0;

void setup() {
  Serial.begin(9600);
  pinMode(22, OUTPUT);
  pinMode(23, OUTPUT);
  pinMode(24, OUTPUT);
  pinMode(25, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);

  pinMode(26, OUTPUT);
  pinMode(27, OUTPUT);
  pinMode(28, OUTPUT);
  pinMode(29, OUTPUT);
  pinMode(11, OUTPUT);
  pinMode(12, OUTPUT);

  Wire.begin();
  bno.begin();

  digitalWrite(22, LOW); //motore RL
  digitalWrite(23, HIGH);
  digitalWrite(24, HIGH); //motore RR
  digitalWrite(25, LOW);
  digitalWrite(26, HIGH); //motore RL
  digitalWrite(27, LOW);
  digitalWrite(28, HIGH); //motore RR
  digitalWrite(29, LOW);

  TCCR1A = 0b10100001;
  TCCR1B = 0b00001101;
  OCR1A = mtrSpd;
  OCR1B = mtrSpd;

  TCCR2A = 0b10100011;
  TCCR2B = 0b00000111;
  OCR2A = mtrSpd;
  OCR2B = mtrSpd;

  bno.setExtCrystalUse(true);
}

void loop() {
  double now = millis();
  dt = (now - last_time)/1000.00;
  last_time=now;

  sensors_event_t event;
  bno.getEvent(&event);
  float angle_input = event.orientation.x;
  
  
  error = target - angle_input;
  proportional = error;
  integral = integral + (error*dt);
  derivative = (error - last_error)/dt;
  
  angle = (proportional * Kp) + (integral * Ki) + (derivative * Kd);
  
  Serial.print(target);
  Serial.print(",");
  Serial.print(angle_input);
  Serial.print(",");
  Serial.print(angle);
  Serial.print("\n");
  // if (angle > 255) {
  //   angle=255;
  // }else if (angle < -255) {
  //   angle=-255;
  // }
 
  if (angle > 0) {
    OCR2B = mtrSpd - abs(angle);
    OCR1A = mtrSpd - abs(angle);
    OCR2A = mtrSpd + abs(angle);
    OCR1B = mtrSpd - abs(angle);

  } else if (angle < 0) {
    OCR2B= mtrSpd + abs(angle);
    OCR1A = mtrSpd + abs(angle);
    OCR2A = mtrSpd - abs(angle);
    OCR1B = mtrSpd - abs(angle);
  }
  
  last_error = error;
  delay(10); 
}