Vertical correction system constructed with 28BYJ-48 motor and MPU6050

source code:

#include <Stepper.h>

#include <Wire.h>

#include <SoftwareSerial.h>

SoftwareSerial mySerial(3, 4); // 软串口定义

const int stepsPerRevolution = 2048;

Stepper myStepper(stepsPerRevolution, 8, 10, 9, 11);

int targetSteps =0;

int mpuable=0;

void motordirection(bool clockwise);

void read_mpu_6050_data();

void setup_mpu_6050_registers();

//陀螺仪用的变量

int gyro_x, gyro_y, gyro_z;

long acc_x, acc_y, acc_z, acc_total_vector;

int temperature;

long gyro_x_cal, gyro_y_cal, gyro_z_cal;

long loop_timer;

int lcd_loop_counter;

float angle_pitch, angle_roll;

int angle_pitch_buffer, angle_roll_buffer;

boolean set_gyro_angles;

float angle_roll_acc, angle_pitch_acc;

float angle_pitch_output, angle_roll_output;

float pitch_difference=0;

float pen_pitch=0;

float phone=0;

int count=0;

int count2=0;

 float pitch_diffence_start=0;

 bool truth=true;

 float pitch_diffence=0;

void setup() {

  myStepper.setSpeed(10);

  Wire.begin();                                                        //Start I2C as master

  Serial.begin(57600);    

  mySerial.begin(9600);                                           //Use only for debugging

  pinMode(13, OUTPUT);                                                 //Set output 13 (LED) as output

  setup_mpu_6050_registers();                                          //Setup the registers of the MPU-6050 (500dfs / +/-8g) and start the gyro

  digitalWrite(13, HIGH);

  delay(1500);                                                         //Delay 1.5 second to display the text

                                     

  Serial.print("Calibrating gyro");                                    

  for (int cal_int = 0; cal_int < 2000 ; cal_int ++){                  //Run this code 2000 times

    if(cal_int % 125 == 0)Serial.print(".");                              //Print a dot on the LCD every 125 readings

    read_mpu_6050_data();                                              //Read the raw acc and gyro data from the MPU-6050

    gyro_x_cal += gyro_x;                                              //Add the gyro x-axis offset to the gyro_x_cal variable

    gyro_y_cal += gyro_y;                                              //Add the gyro y-axis offset to the gyro_y_cal variable

    gyro_z_cal += gyro_z;                                              //Add the gyro z-axis offset to the gyro_z_cal variable

    delay(3);                                                          //Delay 3us to simulate the 250Hz program loop

  }

  gyro_x_cal /= 2000;                                                  //Divide the gyro_x_cal variable by 2000 to get the avarage offset

  gyro_y_cal /= 2000;                                                  //Divide the gyro_y_cal variable by 2000 to get the avarage offset

  gyro_z_cal /= 2000;                                                  //Divide the gyro_z_cal variable by 2000 to get the avarage offset




  digitalWrite(13, LOW);                                               //All done, turn the LED off



  loop_timer = micros();  



}



void loop()

{

  //电机模块

  if (mySerial.available()) { // 如果有数据可读

     byte hexbyte = mySerial.read(); // 读取单个字节

    uint16_t hexdegree = hexbyte; // 将字节转换为无符号整数

    uint16_t decdegree = hexdegree; // 直接转换为十进制数

    targetSteps = decdegree / 360.0 * stepsPerRevolution; // 计算目标步数

     if (decdegree==1)

    {

    mpuable=decdegree;

    decdegree=0;

    }

    if (decdegree==2)//停止矫正

    {

    mpuable=0;

    decdegree=0;

    }

    if (decdegree==3)//逆时针转动

    {

    truth=false;

    decdegree=0;

    }

    if (decdegree==4)

    {

    truth=true;

    decdegree=0;

    }

    motordirection(truth);

   

    myStepper.step(targetSteps);

  }  

    // 陀螺仪测算俯仰角模块、

    if(mpuable==1){

    read_mpu_6050_data();                                                //开启校正



  gyro_x -= gyro_x_cal;                                                //Subtract the offset calibration value from the raw gyro_x value

  gyro_y -= gyro_y_cal;                                                //Subtract the offset calibration value from the raw gyro_y value

  gyro_z -= gyro_z_cal;                                                //Subtract the offset calibration value from the raw gyro_z value



  //Gyro angle calculations

  //0.0000611 = 1 / (250Hz / 65.5)

  angle_pitch += gyro_x * 0.0000611;                                   //Calculate the traveled pitch angle and add this to the angle_pitch variable

  angle_roll += gyro_y * 0.0000611;                                    //Calculate the traveled roll angle and add this to the angle_roll variable



  //0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians

  angle_pitch += angle_roll * sin(gyro_z * 0.000001066);               //If the IMU has yawed transfer the roll angle to the pitch angel

  angle_roll -= angle_pitch * sin(gyro_z * 0.000001066);               //If the IMU has yawed transfer the pitch angle to the roll angel



  //Accelerometer angle calculations

  acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z));  //Calculate the total accelerometer vector

  //57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians

  angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296;       //Calculate the pitch angle

  angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296;       //Calculate the roll angle



  //Place the MPU-6050 spirit level and note the values in the following two lines for calibration

  angle_pitch_acc -= 0.0;                                              //Accelerometer calibration value for pitch

  angle_roll_acc -= 0.0;                                               //Accelerometer calibration value for roll



  if(set_gyro_angles){                                                 //If the IMU is already started

    angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004;     //Correct the drift of the gyro pitch angle with the accelerometer pitch angle

    angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004;        //Correct the drift of the gyro roll angle with the accelerometer roll angle

  }

  else{                                                                //At first start

    angle_pitch = angle_pitch_acc;                                     //Set the gyro pitch angle equal to the accelerometer pitch angle

    angle_roll = angle_roll_acc;                                       //Set the gyro roll angle equal to the accelerometer roll angle

    set_gyro_angles = true;                                            //Set the IMU started flag

  }



  //To dampen the pitch and roll angles a complementary filter is used

  angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1;   //Take 90% of the output pitch value and add 10% of the raw pitch value

  angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1;      //Take 90% of the output roll value and add 10% of the raw roll value

/*

  //手机屏幕发送模块

  float phone_pitch=angle_pitch_output;

  Serial.println(" phone_pitch:"); // 向软串口发送数据

  Serial.println( phone_pitch); // 向软串口发送数据

*/



//笔接收模块

  pen_pitch=angle_pitch_output;

  if (Serial.available()) { // 如果有数据可读

  float phone_pitch = Serial.parseFloat(); // 读取浮点数数据

  float pitch_diffence_false=pen_pitch-phone_pitch;



  //笔矫正模块

   



    if (!isnan(phone_pitch))

    {

      if(count<=0)

      {

        pitch_diffence_start=fabs(pitch_diffence_false);

      count=1;        

      }

    }



  pitch_diffence=fabs(pitch_diffence_false);

  Serial.print("pitch_diffence: ");

  Serial.println(pitch_diffence);

   int i=pitch_diffence;

   if(pitch_diffence==90)

   {//电机停转

   }

   else

   {

     if(count2==0)

     {

     myStepper.step(10);

     count2=1;

     }

     if(abs(pitch_diffence-90)>abs(90-pitch_diffence_start))//判断方向

     {

       motordirection(false);

       while (i<90) {

     myStepper.step(5);

     i++;

  }

  while (i>90) {

     myStepper.step(5);

     i--;

  }

  pitch_diffence=90;

       }

                   

    if(abs(pitch_diffence-90)<abs(90-pitch_diffence_start))//判断方向

     {

       motordirection(true);

       while (i<90) {

     myStepper.step(5);

     i++;

  }

  while (i>90) {

     myStepper.step(5);

     i--;

  }

  pitch_diffence=90;

    }

  }

   

  }

  }  

  while(micros() - loop_timer < 4000);                                 //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop

  loop_timer = micros();    

   





}



void read_mpu_6050_data(){                                             //Subroutine for reading the raw gyro and accelerometer data

  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050

  Wire.write(0x3B);                                                    //Send the requested starting register

  Wire.endTransmission();                                              //End the transmission

  Wire.requestFrom(0x68,14);                                           //Request 14 bytes from the MPU-6050

  while(Wire.available() < 14);                                        //Wait until all the bytes are received

  acc_x = Wire.read()<<8|Wire.read();                                  //Add the low and high byte to the acc_x variable

  acc_y = Wire.read()<<8|Wire.read();                                  //Add the low and high byte to the acc_y variable

  acc_z = Wire.read()<<8|Wire.read();                                  //Add the low and high byte to the acc_z variable

  temperature = Wire.read()<<8|Wire.read();                            //Add the low and high byte to the temperature variable

  gyro_x = Wire.read()<<8|Wire.read();                                 //Add the low and high byte to the gyro_x variable

  gyro_y = Wire.read()<<8|Wire.read();                                 //Add the low and high byte to the gyro_y variable

  gyro_z = Wire.read()<<8|Wire.read();                                 //Add the low and high byte to the gyro_z variable



}



void setup_mpu_6050_registers(){

  //Activate the MPU-6050

  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050

  Wire.write(0x6B);                                                    //Send the requested starting register

  Wire.write(0x00);                                                    //Set the requested starting register

  Wire.endTransmission();                                              //End the transmission

  //Configure the accelerometer (+/-8g)

  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050

  Wire.write(0x1C);                                                    //Send the requested starting register

  Wire.write(0x10);                                                    //Set the requested starting register

  Wire.endTransmission();                                              //End the transmission

  //Configure the gyro (500dps full scale)

  Wire.beginTransmission(0x68);                                        //Start communicating with the MPU-6050

  Wire.write(0x1B);                                                    //Send the requested starting register

  Wire.write(0x08);                                                    //Set the requested starting register

  Wire.endTransmission();                                              //End the transmission

}

void motordirection(bool clockwise) {

  if (clockwise) {

    targetSteps=targetSteps;

  } else {

     targetSteps=-targetSteps;

  }

}

summary:

This code use a special algroithm to count the angle by the MPU6050,we usually called this "Gyroscope-based orientation estimation algorithm"，it use Rotation matrix  ，Quaternion ，Euler angles，Gyroscope，Integration，Kalman filter，and so on.To be honest,these knowledge is so complexity that I can't Expour clearly.I try to find a awesome figure and copy his code.So this article stay here wait me to learn clearly and speak again.