Акклерометр arduino с серводвигателем

Вот окончательная форма моего проекта. Я добавил функцию отсоединения двигателя, так как он все еще немного дрожит.

#include <SoftwareSerial.h>
#include <Servo.h>

const int MPU_addr=0x68;  // MPU-6050 I2C address
int16_t AcX,AcY,AcZ,GyX,GyY,GyZ;

SoftwareSerial BTSerial(3, 2);  // TX, RX

Servo servoMini;
Servo servoUp;
Servo servoDown;

const byte pinMini = 6;
const byte pinUp = 9;
const byte pinDown = 10;

// Timings for each event.
const unsigned long ACC_INTERVAL = 50;
const unsigned long INTERVAL = 1000;
const unsigned long INTERVAL_FIN = 2000;
const unsigned long GRIPPER_INTERVAL = 2000;
const unsigned long GRIPPER_INTERVAL_FIN= 3000;
const unsigned long LED_INTERVAL = 250;

// Keep track of timestamps for each event.
unsigned long prev_led_timestamp;
unsigned long prev_acc_timestamp;
unsigned long servo_timestamp;
unsigned long servo_timestamp_fin;
unsigned long gripper_timestamp;
unsigned long gripper_timestamp_fin;

// Flags to indicate completion of "just once" events.
bool servo = false;
bool servo_fin = false;
bool gripper = false;
bool gripper_fin = false;

// State of blinking LED.
bool led_state = false;

void readAcc(){ 
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true);  // request a total of 14 registers   // 14bit??
  
  AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  //AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  //AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  //GyX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  //GyY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  //GyZ=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

  BTSerial.println(AcX);   // 16bit
  //Serial.println(AcX);
}

void setup(){
  //Serial.begin(9600);
  BTSerial.begin(9600);

  // For blink without delay.
  pinMode(LED_BUILTIN, OUTPUT);

  // I2C
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);

  // servo
  servoMini.attach(pinMini);
  servoUp.attach(pinUp);
  servoDown.attach(pinDown);

  // Perform first move.
  servoMini.write(145);
  servoUp.write(180);
  servoDown.write(0);

  // Initialise all timestamps with the same value with a single call to millis().
  prev_led_timestamp =
  prev_acc_timestamp =
  servo_timestamp = servo_timestamp_fin =
  gripper_timestamp = gripper_timestamp_fin = millis();
}

void loop(){  
  unsigned long current_timestamp = millis();

  // TASK 1: Read acc.
  if (current_timestamp - prev_acc_timestamp >= ACC_INTERVAL){
    // Read acc.
    readAcc();
    prev_acc_timestamp += ACC_INTERVAL;
  }

  // TASK 2: Just once.
  if (!servo && current_timestamp - servo_timestamp >= INTERVAL)  {
    //Serial.print("Running just once... ");
    servoUp.attach(pinUp);
    servoDown.attach(pinDown);
    servoUp.write(60);
    servoDown.write(120);
    // retract speed control
    servo = true;
    //Serial.println("Done.");
  }
  if (!servo_fin && current_timestamp - servo_timestamp_fin >= INTERVAL_FIN){
    servoUp.detach();
    servoDown.detach();
    servo_fin = true;
  }

  // TASK 3: Also just once.
  if (!gripper && current_timestamp - gripper_timestamp >= GRIPPER_INTERVAL)  {
    // Perform third move.
    //Serial.print("Running also just once... ");
    servoMini.write(180);
    gripper = true;
    //Serial.println("Done.");
  }
  if (!gripper_fin && current_timestamp - gripper_timestamp_fin >= GRIPPER_INTERVAL_FIN){
    servoMini.detach();
    gripper_fin = true;
  }

  // TASK 4: Blink without delay.
  if (current_timestamp - prev_led_timestamp >= LED_INTERVAL){
    // Toggle LED state.
    led_state = !led_state;
    digitalWrite(LED_BUILTIN, led_state);
    prev_led_timestamp += LED_INTERVAL;
  }
}