(Tiểu luận) project on automatic control two wheeled self balancing robot

  Ministry of Education and Training Ho Chi Minh City University of Technology and Education Faculty of Electrical and Electronics Engineering Department of Automatic Control - ⸙∆⸙ - Project on Automatic Control Two-Wheeled Self-Balancing Robot LECTURER : STUDENT’S NAME  : VU VAN PHONG  TRAN TUAN HIEU  –  20151275 NGUYEN DINH HOAN - 20151277 Ho Chi Minh City, JANUARY th 2022 h     Contents CHAPTER 1: OVERVIEW OF SELF-BALACED VEHICLES .1 1.1 what is two wheels self-balancing? 1.2 Models of self-balancing wheels in labrotary 1.2.1 nBot 1.2.2 JOE 1.2.3 NXTway-GS 1.3 Some actual products are based on self-balancing wheels robot model 1.3.1 Segway Personal Transporter 1.3.2 Winglet Toyota 1.3.3 Iswing Toyota CHAPTER 2: THEORETICAL BASIC 2.1 PID controller 2.2 Modeling and symbols 2.3 Kinetic model of DC motor 10 2.4 Two wheels self-balancing robot 11 2.5 Simulation 15 2.5.1 System parameter declaration 15 2.5.2 System stability investigation 15 2.5.3 System controlability investigation 17 2.5.4 System observability investigation 18 2.5.5 controller construction 19 CHAPTER 3: SYSTEM DESIGN 22 3.1 Hardware 22 3.1.1 Arduino nano 22 3.1.2 CNC shield V4 24 3.1.3 Driver A4988 25 3.1.4 Stepper motor size 42 (1.8º) 25 3.1.5 GY-521 MPU 6050 angel sensor .26 3.1.6 65mm wheels 28 3.1.7 Battery 28   h   3.2 Control alogrithm flowchart 29 3.3 Software design 29 3.3.1 Main program 29 3.3.2 sensor library program .33 3.4 Realistic model 37 3.5 Realistic testing 38 3.5.1 Standard PID 38 3.5.2 Kp increased/decreased 39 3.5.3 Ki increased/decreased .41 3.5.4 Kd increased/decreased 42 CHAPTER 4: CONCLUSION AND DEVELOPMENT 44 4.1 Results 44 4.2 Limtitations and development directions 44 4.2.1 Limitations 44 4.2.2 Development directions 44 REFERENCES 45   h 37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.33.44.55.54.78.655.43.22.2.4.55.2237.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.66     CHAPTER 1: OVERVIEW OF SELF-BALACED VEHICLES 1.1 what is two wheels self-balancing? In the field of automation - automatic control in general and cybernetics in  particular, the inverted pendulum model is one of the typical research objects and is characterized by the unstable dynamic characteristics of the model, so the control controlling this object in fact poses as a challenge The research results of the basic inverted pendulum model, such as the car pendulum model, the rotating inverted pendulum, etc can be applied and inherited to other similar models but have more practical application such as rocket model, self-balancing two-wheeled vehicle model, etc., thereby overcoming the inherent disadvantages of classic two- or three-wheeled robots Classic two- or three-wheeled robots, which consist of a drive wheel and a free wheel (or whatever else) to support the robot's weight If a lot of weight is placed on the rudder, the robot will be unstable and prone to falling, and if put on many tail wheels, the two main wheels will lose their ability to grip Many robot designs can move well on flat terrain but cannot move up and down on convex or inclined terrain When moving up a hill, the robot's weight is placed on the rear of the vehicle, causing it to lose its ability to grip and slip 3-wheel robot moves on flat terrain, the weight is divided equally between the rudder and the small guide wheel Figure 1) 3-wheeled robot moves on flat terrain 3-wheel robot when going downhill, gravity on the rear wheel makes the car can capsized h 37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.99 37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.33.44.55.54.78.655.43.22.2.4.55.2237.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.66     Figure 2) Robot with wheels when going downhill 3-wheel robot when going uphill, the weight is put on the front wheel, making the friction force to help the car stick on the road surface is not guaranteed Figure 3) 3-wheel robot when going uphill In contrast, coaxial two-wheeled robots balance very flexibly when moving maneuvering over complex terrain, even though the robot itself is an unstable system determined When the robot moves on steep terrain, it automatically tilts forward and holds for the weight to be transferred to the two main wheels Similarly, when moving downhill, it lean back and keep the center of gravity on the main wheel So there is never present The vehicle's center of gravity falls outside the wheel support area, which can cause a rollover h 37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.99 37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.33.44.55.54.78.655.43.22.2.4.55.2237.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.66     Figure 4) 2-wheel robot moves on different terrains in a direction that preserves balance 1.2 Models of self-balancing wheels in labrotary 1.2.1 nBot Robot nBot built by David P.Anderson The principle of controlling nBot is as follows: the wheels will run in the direction where the upper part of the robot is about to fall, if the wheels can be steered in a way that keeps the robot's center of gravity, the robot will be balanced The control process uses signals from two sensors: the sensor for the tilt angle of the robot body relative to the direction of gravity and the encoder mounted on the wheel to measure the robot's position This signal forms four variables: robot body tilt angle, tilt angular velocity, robot position and robot speed; These variables are calculated into the motor control voltage for the robot h 37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.99 37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.33.44.55.54.78.655.43.22.2.4.55.2237.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.66     Figure 5) nBot 1.2.2 JOE JOE was created in 2002 by the industrial electronics laboratory of the Federal Institute of Technology Lausanne, Switzerland Its shape consists of two coaxial wheels, each wheel is attached to a DC motor, this robot can move Ushaped rotation The control system consists of two separate "state space" controllers, controlling the motor to keep the system balance State information is provided  by two optical encoders and two sensors: angular accelerometer and gyro The JOE is controlled by an RC remote controller The central controller and signal processing unit is a digital signal processing board (DSP) developed  by the group itself and by the Federal Institute, in conjunction with XILINC's FPGA h 37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.99 37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.33.44.55.54.78.655.43.22.2.4.55.2237.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.66     Figure 6) JOE 1.2.3 NXTway-GS Figure 7) NXTway-GS by LEGO MINDSTORMS 1.3 Some actual products are based on self-balancing wheels robot model 1.3.1 Segway Personal Transporter Segway PT (short for Segway Personal Transporter), commonly known as Segway for short, is a two-wheeled personal transport vehicle, operating on a self balancing mechanism invented by Dean Kamen This vehicle is manufactured by Segway Inc in the state of New Hampshire, USA The word "Segway" is h 37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.99 37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.33.44.55.54.78.655.43.22.2.4.55.2237.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.66      pronounced akin to "segue" (an original Italian word meaning "to move gently") The outstanding feature of the Segway is the self-balancing mechanism thanks to the computer system, engine and gyroscope located inside the vehicle, which helps the vehicle, even with only one axis of movement with two wheels, but always in a state of balance Equally, the user only needs to lean forward or  backward to control the car to go forward or backward Figure 8) Segway Personal Transporter   1.3.2 Winglet Toyota The Winglet, developed by Toyota and introduced in 2008, has a base the size of an A3 sheet of paper The vehicle is based on a self-balancing 2-wheel model and has sensors to recognize the movement of the driver, thereby giving commands to the vehicle to operate according to the wishes of the driver h 37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.99 37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.33.44.55.54.78.655.43.22.2.4.55.2237.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.66     Figure 9) Winglet Toyota 1.3.3 Iswing Toyota Figure 10) Iswing Toyota Iswing is known as one of the boldest ideas of the auto industry in recent times Making its debut at the 2005 Tokyo Motor Show, the Iswing is a symbol of the future of personal vehicles because it is far different from conventional cars The movement, control, communication between people and vehicles are all human For example, when placing the seat in the "minimum body opening" mode, the driver's eyes will be level with the person standing, so it is easy to talk h 37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.99 37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.33.44.55.54.78.655.43.22.2.4.55.2237.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.66     else PORTD &= 0b11011111;  if (Count_timer1 > Count_BOT1) {  Count_timer1 = 0;  if (Dir_M1 > 0) Step1++; else if (Dir_M1 < 0) Step1 ; }  }  if (Dir_M2 != 0) {  Count_timer2++; if (Count_timer2 Count_BOT2) {  Count_timer2 = 0; if (Dir_M2 > 0) Step2++; else if (Dir_M2 < 0) Step2 ; }  }  if (Dir_M3 != 0) {  Count_timer3++; if (Count_timer3 Count_BOT3) {  Count_timer3 = 0; if (Dir_M3 > 0) Step3++; else if (Dir_M3 < 0) Step3 ; }  }  }   Speed function:  V=60/(x*3200*20*10^(-6)) rpm Example: X=1 => V=937.5 rpm X=10 => V=93.75 rpm X=400 => V=2.3 rpm void Speed_L(int16_t x) {  if (x < 0) {  Dir_M1 = -1; PORTD &= 0b11111011; } else if (x > 0) {  Dir_M1 = 1; PORTD |= 0b00000100; } else Dir_M1 = 0; Count_BOT1 = abs(x); Count_TOP1 = Count_BOT1 / 2; 31 h 37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.99 37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.33.44.55.54.78.655.43.22.2.4.55.2237.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.66     }  void Speed_R(int16_t x) {  if (x < 0) {  Dir_M2 = -1; PORTD &= 0b11110111; } else if (x > 0) {  Dir_M2 = 1; PORTD |= 0b00001000; } else Dir_M2 = 0; Count_BOT2 = abs(x); Count_TOP2 = Count_BOT2 / 2; }   Main function:  Set up: void setup() {  mpu6050.init(0x68); Serial.begin(9600);  //Khai báo Serial  pin_INI();  //Khai báo PIN Arduino đấu nối DRIVER A4988  timer_INI();  //Khai báo TIMER2  delay(500); loop_timer = micros() + 4000; }   Read angle: void loop() {  offset = 0.5; float AngleY = mpu6050.getYAngle(); input = AngleY + offset;  PID and speed calculation: i += input; output = kp * input + kd * i + kd * (input - input_last); input_last = input; if (output > -5 && output < 5) output = 0; output = constrain(output, -400, 400); outputL = output; 32 h 37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.99 37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.33.44.55.54.78.655.43.22.2.4.55.2237.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.66     outputR = output; if (output > 0) {  M_L = 405 - (1 / (outputL + 9)) * 5500; M_R = 405 - (1 / (outputR + 9)) * 5500; } else if (output < 0) {  M_L = -405 - (1 / (outputL - 9)) * 5500; M_R = -405 - (1 / (outputR - 9)) * 5500; } else {  M_L = 0; M_R = 0; }  if (M_L > 0) motorL = 400 - M_L; else if (M_L < 0) motorL = -400 - M_L; else motorL = 0; if (M_R > 0) motorR = 400 - M_R; else if (M_R < 0) motorR = -400 - M_R; else motorR = 0; Speed_L(motorL); Speed_R(motorR); while (loop_timer > micros()); loop_timer += 4000; 3.3.2 sensor library program #ifndef _SAIGONTECH_MPU6050_H_  #define _SAIGONTECH_MPU6050_H_  #include "Wire.h"  #define  RAD2DEG  57.295779  class SMPU6050 {  public:  SMPU6050() {  }; void init(int address) {  this->i2cAddress = address; this->gyroXOffset = 0; this->gyroYOffset = 0; this->gyroZOffset = 0; this->xAngle = 0; 33 h 37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.99 37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.33.44.55.54.78.655.43.22.2.4.55.2237.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.66     this->yAngle = 0; this->zAngle = 0; this->accX = 0; this->accY = 0; this->prevMillis = millis(); Wire.begin(); Wire.beginTransmission(this->i2cAddress); Wire.write(0x6B);  Wire.write(0);  Wire.endTransmission(true); Wire.beginTransmission(this->i2cAddress); Wire.write(0x19);  Wire.write(0);  Wire.endTransmission(true); Wire.beginTransmission(this->i2cAddress); Wire.write(0x1B);  Wire.write(0);  Wire.endTransmission(true); Wire.beginTransmission(this->i2cAddress); Wire.write(0x1C);  Wire.write(0);  Wire.endTransmission(true); }  void calibrate(int times) {  long gyroXTotal = 0, gyroYTotal = 0, gyroZTotal = 0; int count = 0; int gyroRawX, gyroRawY, gyroRawZ; for (int i = 0; i < times; i++) {  Wire.beginTransmission(this->i2cAddress); Wire.write(0x43);  Wire.endTransmission(false); Wire.requestFrom(this->i2cAddress, 6, true); gyroRawX = Wire.read() readAngles(); return this->yAngle; }; double getZAngle() {  this->readAngles(); return this->zAngle; }; double getXAcc() {  this->readAngles(); return this->accX; }; double getYAcc() {  this->readAngles(); return this->accY; }; void getXYZAngles(double &x, double &y, double &z) {  this->readAngles(); x = xAngle; y = yAngle; z = zAngle; }  private:  int i2cAddress; double accX, accY, gyroX, gyroY, gyroZ; double gyroXOffset, gyroYOffset, gyroZOffset; double xAngle, yAngle, zAngle; unsigned long prevMillis; void readAngles() {  if (millis() - this->prevMillis < 3)  35 h 37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.99 37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.C.33.44.55.54.78.655.43.22.2.4.55.2237.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.55.77.77.99.44.45.67.22.55.77.C.37.99.44.45.67.22.66     return; int accRawX, accRawY, accRawZ, gyroRawX, gyroRawY, gyroRawZ; Wire.beginTransmission(this->i2cAddress); Wire.write(0x3B); Wire.endTransmission(false); Wire.requestFrom(this->i2cAddress, 14, true); accRawX = Wire.read()