Qassim University ‫جامعـة القصيـم‬

Faculty of Engineering ‫كليه الهندسة‬

Mechanical Engineering
Dept. ‫قسم الهندسة‬
‫الميكانيكية‬

ME 465
System Dynamics and Automatic Control.
Semester 432

Plate angle control

by

No Name ID
1 Abdullah Saleh Nasser 381126354
2 Naif Almasaabi 381126404

Instructor

Prof. Hanafy M. Omar

Date of submission: 22/5/2022

 Table of Contents
Objectives: .................................................................................................................... 4
Introduction: ................................................................................................................ 4
System Components: ................................................................................................... 4
1-Arduino board ..................................................................................................... 4
2- servo motor ........................................................................................................ 5
3-sensor .................................................................................................................. 6
4- breadboard and wires ......................................................................................... 7
Mathematical model: ................................................................................................. 11
Result and discussion ................................................................................................. 12
Conclusion .................................................................................................................. 15
 Table of figures

Figure 1-Arduino board component............................................................................... 5

Figure 2:servo motor ...................................................................................................... 5
Figure 3: MPU6050. ...................................................................................................... 6
Figure 4:breadboard ....................................................................................................... 7
Figure 5: jumper wires ................................................................................................... 7
Figure 6: Feedback controller loop using a PID controller.......................................... 11
Figure 7: Dc servo motor ............................................................................................. 11
Figure 8: The system without using a PID controller .................................................. 12
Figure 9:Root locus ...................................................................................................... 13
Figure 10: Bode diagram. ............................................................................................ 13
Figure 11: The system using a PID controller. ............................................................ 14
Objectives:
To build a patient transfer trolley with automatic balance and test the
effects of different PD changing on system performance.

Introduction:

In this project, a control system will be build that includes the automatic
balancing of a patient transfer trolley with a feedback system, utilizing all
of the theoretical information obtained. An actuator (Servo Motor), an
angle sensor (MPU6050), an Arduino board, wiring, and the physical
model are all included in the system, the system operates by the sensor
detecting the angle disturbance which reference is 0, delivering the
feedback to the PID controller to implement the appropriate parameters,
and then sending the final values to the Servo motor to balance the system.

System Components:
1- Arduino board.
2- Servo Motor.
3- MPU 6050 Sensor
4- Breadboard

1-Arduino board

Arduino boards can take inputs - such as light from a sensor, a finger on a
button, or a Twitter message - and convert them to outputs - such as turning
on an LED, triggering a motor, or publishing anything online. By providing
a set of instructions to the board's microcontroller, you may tell it what to
do. [1]
 Figure 1-Arduino board component

2- servo motor

Figure 2:servo motor

Specifications
 Model SG90
 Operating speed: 0.1sec/60degree
 No-load current: 350 mA
 Operating Voltage: 4.8.0 V
 Stall Torque: 1.8 Kg/cm
 Stall Current: 7 A
 Insulation resistance: 20 M Ω
 Encoder Operating Voltage: 5 V
 Weight: 9g
 Temperature range:0C~+55C

3-sensor

Figure 3: MPU6050.

MPU6050 is a three-axis accelerometer and three-axis gyroscope Micro

Electro-mechanical system (MEMS). It assists us in determining velocity,
orientation, acceleration, displacement, and other motion-related
characteristics.
4- breadboard and wires

Figure 4:breadboard

A breadboard allows for easy and quick creation of temporary electronic

circuits or to carry out experiments with circuit design.

Figure 5: jumper wires

jumper wire is normally used to interconnect the components of a

breadboard or other prototype or test circuit

Arduino code (1):

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include <Servo.h>
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif

MPU6050 mpu;
Servo myservo;

#define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards

#define servo_pin 4
bool blinkState = false;

bool dmpReady = false; // set true if DMP init was successful

uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 =
success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

Quaternion q; // [w, x, y, z] quaternion container

VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor
measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor
measurements
VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and
gravity vector

uint8_t teapotPacket[14] = { '$', 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0x00, 0x00,

'\r', '\n' };

volatile bool mpuInterrupt = false; // indicates whether MPU interrupt

pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}

void setup() {
myservo.attach(servo_pin);
myservo.write(0);
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if
having compilation difficulties
#elif I2CDEV_IMPLEMENTATION ==
I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
 Serial.begin(115200);
while (!Serial); // wait for Leonardo enumeration, others continue
immediately

Serial.println(F("Initializing I2C devices..."));

mpu.initialize();
pinMode(INTERRUPT_PIN, INPUT);

Serial.println(F("Testing device connections..."));

Serial.println(mpu.testConnection() ? F("MPU6050 connection
successful") : F("MPU6050 connection failed"));

Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();

// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

if (devStatus == 0) {
mpu.CalibrateAccel(6);
mpu.CalibrateGyro(6);
mpu.PrintActiveOffsets();
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);

Serial.print(F("Enabling interrupt detection (Arduino external interrupt

"));
Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
Serial.println(F(")..."));
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN),
dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();

Serial.println(F("DMP ready! Waiting for first interrupt..."));

dmpReady = true;

packetSize = mpu.dmpGetFIFOPacketSize();
} else {
Serial.print(F("DMP Initialization failed (code "));
 Serial.print(devStatus);
Serial.println(F(")"));
}
}

void loop() {
if (!dmpReady) return;

while (!mpuInterrupt && fifoCount < packetSize) {

if (mpuInterrupt && fifoCount < packetSize) {
// try to get out of the infinite loop
fifoCount = mpu.getFIFOCount();
}
}

mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();

fifoCount = mpu.getFIFOCount();
if (fifoCount < packetSize) {
}
else if ((mpuIntStatus & (0x01 <<
MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) {
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
} else if (mpuIntStatus & (0x01 <<
MPU6050_INTERRUPT_DMP_INT_BIT)) {

while (fifoCount >= packetSize) { // Lets catch up to NOW, someone

is using the dreaded delay()!
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
}

mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Serial.print("YAW: ");
Serial.print(ypr[0] * 180 / M_PI);
Serial.print("\t");
Serial.print("PITCH:");
Serial.print(ypr[1] * 180 / M_PI);
Serial.print("\t");
 Serial.print("ROLL: ");
Serial.println(ypr[2] * 180 / M_PI);

myservo.write(90 + ((ypr[2] * 180 / M_PI)* -1));

Serial.print("Servo Pos: "); Serial.println(90 + ((ypr[2] * 180 / M_PI)*
-1));
Serial.println();
}
}

Mathematical model:

In the figure below is the feedback controller loop

Ref angle PID DC Output angle

+ Controll
- Servo
er Motor

Sensor
Figure 6: Feedback controller loop using a PID controller

Dc-Servo motor’s model

In the figure below is the free body diagram of the servo motor

Figure 7: Dc servo motor

Equations:
𝑑𝑖𝑎
𝐿𝑎 + 𝑅 𝑖𝑎 + 𝑒𝑏 = 𝑒𝑖
𝑑𝑡
𝑑∅
𝑒𝑏 = 𝐾𝑏
𝑑𝑡
𝑇𝑚 = 𝐾𝑡 𝑖𝑎
∅𝑚
𝑇𝐿 = 𝑇
∅ 𝑚
𝐽𝐿 ∅ = 𝑇𝐿

Transfer Function:
∅(𝑠) 𝐾𝑡
=
𝑉(𝑠) (𝑅𝑎 + 𝑠𝐿𝑎 )𝐽𝑠 2 + 𝐾𝑏 𝐾𝑡 𝑠

Result and discussion

If the team run the simulations of the system without using a PID
controller in matlab we will get the result shown in figure 8 below.

Figure 8: The system without using a PID controller

The response behavior shown above is for first order system, thus the
obtained transfer function for the system is:
𝜃(𝑠) 𝐾
=
𝑉(𝑠) 𝑆(𝑆+𝑎)
From the response plot we will find the needed parameter for the system.
Root Locus
2.5

1.5

0.5
Imaginary Axis

-0.5

-1

-1.5

-2

-2.5
-4.5 -4 -3.5 -3 -2.5 -2 -1.5 -1 -0.5 0 0.5
Real Axis
Figure 9:Root locus

Bode Diagram
80

60
Magnitude (dB)

40

20

-20

-40
-90
Phase (deg)

-135

-180
-1 0 1 2
10 10 10 10
Frequency (rad/sec)
Figure 10: Bode diagram.
 After applying the controller Kp=10 ,Ki=0 ,Kd=0

Figure 11: The system using a PID controller.

Figure 8 shows that the team worked without using a PID controller at
first, and it is clear that the system has a long rise time and a small steady
state error. By using the P from the PID controller, as shown in figure 11,
the system will be faster and have a lower steady state error than the case
without a PID controller.
Conclusion
The team desired to have a final model with better results and finish with
a complete model during this project, but due to MPU6050 library issues
that made the servo motor not move, the team was unable to achieve the
planned goals. That said however, working on this project and attempting
to build the control was a great experience, and the team wishes to have a
better experience in the upcoming projects.
Reference

https://www.theengineeringprojects.com/2018/06/introduction-to-
arduino-nano.html

https://arabic.alibaba.com/product-detail/GY-521-MPU-6050-MPU6050-
Module-62475468526.html

https://www.jsumo.com/sg90-micro-servo-motor

https://www.adafruit.com/product/64

https://www.sparkfun.com/products/11026