Aiuto Balancebot

Come faccio a stabilizzare meglio il balancebot tramite il codice per far muovere i motori di pochissimo avanti ed indietro e quindi renderlo più stabile invece che farlo muovere per 20 centimetri avanti ed indietro. Vorrei rimanesse praticamente sul posto
Vi allego il codice per farvi un'idea

#include <Wire.h>
#include <Kalman.h>
#include <I2Cdev.h>

Kalman kalmanY;


int16_t accX, accY, accZ;
int16_t tempRaw;
int16_t gyroX, gyroY, gyroZ;

float accYangle;
float gyroYangle;
float kalAngleY;

unsigned long timer;
uint8_t i2cData[14];
float CurrentAngle;

const int AIN1 = 3;
const int AIN2 = 9;
const int BIN1 = 10; 
const int BIN2 = 11;


int speed;

const float Kp = 4; 
const float Ki = 1;
const float Kd = 1;
float pTerm, iTerm, dTerm, integrated_error, last_error, error;
const float K = 1.9*1.12;
#define   GUARD_GAIN   10.0

#define runEvery(t) for (static typeof(t) _lasttime;(typeof(t))((typeof(t))millis() - _lasttime) > (t);_lasttime += (t))

const uint8_t IMUAddress = 0x68;
const uint16_t I2C_TIMEOUT = 1000;

uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) {
  return i2cWrite(registerAddress, &data, 1, sendStop);
}

uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop) {
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  Wire.write(data, length);
  uint8_t rcode = Wire.endTransmission(sendStop);
  if (rcode) {
    Serial.print(F("i2cWrite failed: "));
    Serial.println(rcode);
  }
  return rcode;
}

uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) {
  uint32_t timeOutTimer;
  Wire.beginTransmission(IMUAddress);
  Wire.write(registerAddress);
  uint8_t rcode = Wire.endTransmission(false);
  if (rcode) {
    Serial.print(F("i2cRead failed: "));
    Serial.println(rcode);
    return rcode;
  }
  Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true);
  for (uint8_t i = 0; i < nbytes; i++) {
    if (Wire.available())
      data[i] = Wire.read();
    else {
      timeOutTimer = micros();
      while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available());
      if (Wire.available())
        data[i] = Wire.read();
      else {
        Serial.println(F("i2cRead timeout"));
        return 5;
      }
    }
  }
  return 0;
}

void setup() {  
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);
  Serial.begin(57600);
  Wire.begin();
  i2cData[0] = 7;
  i2cData[1] = 0x00;
  i2cData[2] = 0x00;
  i2cData[3] = 0x00;
  while(i2cWrite(0x19,i2cData,4,false));
  while(i2cWrite(0x6B,0x01,true));

    while(i2cRead(0x75,i2cData,1));
  if(i2cData[0] != 0x68) {
    Serial.print(F("Error reading sensor"));
    while(1);
  }

  delay(100);
  
  while(i2cRead(0x3B,i2cData,6));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;

  kalmanY.setAngle(accYangle);
  gyroYangle = accYangle;
  timer = micros();
}

void loop() {
  runEvery(25)
  {
    dof();
    if (CurrentAngle <= 180.2 && CurrentAngle >= 179.8)
    {
      stop();
    }
    else{
    if (CurrentAngle < 230 && CurrentAngle > 130)
    {
    Pid();
    Motors();
    }
    else
    {
      stop();
    }
  }
  }
  Serial.print("Accelerometer: ");
  Serial.print("X = "); Serial.print(accX);
  Serial.print(" | Y = "); Serial.print(accY);
  Serial.print(" | Z = "); Serial.println(accZ);
  Serial.print("Temperature: "); Serial.print(tempRaw/340.00+36.53); Serial.println(" C "); 
  Serial.print("Gyroscope: ");
  Serial.print("X = "); Serial.print(gyroX);
  Serial.print(" | Y = "); Serial.print(gyroY);
  Serial.print(" | Z = "); Serial.println(gyroZ);
  Serial.println(" ");
  delay(400);
}

void Motors(){
   if (speed > 0)
  { 
    //forward 
    analogWrite(AIN1, speed);
    analogWrite(AIN2, 0);
    analogWrite(BIN1, speed);
    analogWrite(BIN2, 0);
  }
  else
  { 
    // backward
    speed = map(speed,0,-255,0,255);
    analogWrite(AIN1, 0);
    analogWrite(AIN2, speed);
    analogWrite(BIN1, 0);
    analogWrite(BIN2, speed);
  }
}

void stop()
{
  analogWrite(AIN1, 0);
  analogWrite(AIN2, 0);
  analogWrite(BIN1, 0);
  analogWrite(BIN2, 0);
}

void Pid(){
  error = 180 - CurrentAngle;
  pTerm = Kp * error;
  integrated_error += error;
  iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
  dTerm = Kd * (error - last_error);
  last_error = error;
  speed = constrain(K*(pTerm + iTerm + dTerm), -255, 255);
}

void dof()
{
  while(i2cRead(0x3B,i2cData,14));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  tempRaw = ((i2cData[6] << 8) | i2cData[7]);  
  gyroX = ((i2cData[8] << 8) | i2cData[9]);
  gyroY = ((i2cData[10] << 8) | i2cData[11]);
  gyroZ = ((i2cData[12] << 8) | i2cData[13]);
  accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
  double gyroYrate = (double)gyroY/131.0;
  CurrentAngle = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);
  timer = micros();
}