Documente Academic
Documente Profesional
Documente Cultură
Manuela Beltrán University. Sebastian Balaguera.Tatiana Rocha Laboratory project (Design Applied to Biomedical Engineering)
Abstract—This project is based on development of a self The objective of this project is double, first put the creativity
balanced two wheeled robot. In particular, the focus is on the and skill of the engineer in the development and
electro-mechanical mechanisms & control algorithms implementation of a circuit that allows to carry out
required to enable the robot to perceive and act in real time. mechanical and digital system to control a self balance robot
This research is aimed to design and implement Proportional to control the speed and balance controlled by a DC stepper
Integral Derivative (PID) controller on Two-wheeled self- motor and a gyroscope. The second one is to consolidate and
balance robot. The PID is used for the purpose of balancing apply the basic programming concepts for the creation and
the robot to stand still at upright position despite implementation of a self-balancing code including the
disturbances to follow the desired trajectory smoothly. consideration for the PID gains that allows the visualization of
This report discusses the theoretical considerations made at effectiveness of the proposed controller for stabilization.
the start of the project, the steps that were taken to
implement the self-balancing code including the II. DEVELOPMENT
consideration for the PID gains were tuned until the optimum 1. Flowchart (Self balancing Robot)
values are achieved, the analysis of the final results, the
interpretation of the final results, Therefore the experimental
results show the effectiveness of the proposed controller for
stabilization.
I. INTRODUCTION
The real world surrounding the application of technology are
large systems that interact according to the type of power
supply, the variable parameter and according to the
magnitude of measurement.
In this branch of technology mixes multiple disciplines in this
case medicine and electronics can find the applied design;
which is to design, plan and develop systems with desired
behaviors in the implementation of control systems based on
mathematical modeling.
The control objective of the inverted pendulum is to swing up
the pendulum hinged on the moving cart by a motor from
stable position to the zero state and to keep the pendulum in
vertically state despite disturbances
If the base on which the robot stand is not stable or the
platform is not balanced, the robot tend to falling off from
the vertical axis. This time a gyro chip is needed to provide
the PID controller about the angular position of the base of
the self balancing robot. A self balancing algorithm is
programmed into the controller and the controller drives the
motors either clockwise or anticlockwise directions to
balance itself.
2
Manuela Beltrán University. Sebastian Balaguera.Tatiana Rocha Laboratory project (Design Applied to Biomedical Engineering)
long gyro_yaw_calibration_value,
gyro_pitch_calibration_value;
The wheel diameter is 5,1mm , so with every pulse the wheel TWBR = 12;
is travelling 0,2mm , at 800 pulses the wheel has travelled
160.2mm . By changing the time between the pulses , the
number of pulses per second can be changed . As a result, to
travel distance per time can be changed.
.
Now that we know how a stepper motor properly works and The main objective here knowing that we are implementin a
has some kind of “Linear” behavior we can adapt only one set stepper motor is to create a variable pulse for controlling the
of PID values, we can now explain the code: stepper motors a timer is created that will execute a piece of
First we define the wire library to communicate with the code (subroutine) every 20us; This subroutine is called
MPU6050 then the address must be loaded with the I2C TIMER2_COMPA_vect. According to the datashhet this can
library as well. We begin by setting the variables for the PID be done since the Arduino pro mini have an 8 bit has a
values , The balance Value given by the MPU 6050 and some counter in which we can manipulate the timing , so if we use
globar variables that will be used later on the code the following table :
#include <Wire.h>
int gyro_address = 0x68;
int acc_calibration_value = 80;
//Various settings
float pid_p_gain = 15;
In which establishes that if the registers CS22, CS21 and
float pid_i_gain = 1.5;
CS200 are in 011 the output of this counter will set it as 8 . As
float pid_d_gain = 30;
a result the 16MHZ clock is divided by 8 and a clock pulse is
2MHz , and dividing 1 by 2 MHz we get a clock time of 0,5us.
float turning_speed = 30;
6
Manuela Beltrán University. Sebastian Balaguera.Tatiana Rocha Laboratory project (Design Applied to Biomedical Engineering)
Now we can calculate the compare value to trigger the PORTD |= 0b00100000;
interrupt routine every 20 us by dividing 20us by 0,5us , the throttle_right_motor_memory *= -1;
result is 40 , Since the timer starts at 0 the compare value is }
39. else PORTD &= 0b11011111;
. }
else if(throttle_counter_right_motor == 1)PORTD |=
0b00010000; else if(throttle_counter_right_motor ==
2)PORTD &= 0b11101111; }
gyro_yaw_calibration_value += Wire.read()<<8|Wire.read();
gyro_pitch_calibration_value += Wire.read()<<8|
Wire.read(); delayMicroseconds(3700);
}
gyro_pitch_calibration_value /= 500;
gyro_yaw_calibration_value /= 500;
loop_timer = micros() + 4000; Wire.beginTransmission(gyro_address);
Wire.write(0x43);
Now it is time to measure the angle , First the raw Wire.endTransmission();
accelerometer Data is pulled from the MPU 6050 , The Wire.requestFrom(gyro_address, 4);
balance calibration value is added and the result is limited to
plus or minus 8200 to prevent a division by “0”. gyro_yaw_data_raw = Wire.read()<<8|Wire.read();
gyro_pitch_data_raw = Wire.read()<<8|Wire.read();
//Angle calculations
Wire.beginTransmission(gyro_address); gyro_pitch_data_raw -= gyro_pitch_calibration_value;
Wire.write(0x3F);
Wire.endTransmission(); angle_gyro += gyro_pitch_data_raw * 0.000031;
Wire.requestFrom(gyro_address, 2);
accelerometer_data_raw = Wire.read()<<8|Wire.read(); By adding this travelled angle by angle gyro variables the
accelerometer_data_raw += acc_calibration_value; actual angle is updated to the new position, in short ; in the
if(accelerometer_data_raw > 8200)accelerometer_data_raw start moment of the angle gets updated to new position so at
= 8200; the start the angle gyro variable is set to accelerometer
if(accelerometer_data_raw < -8200)accelerometer_data_raw variable and according to that goes forward or backward with
= -8200; the motors.
Is now possible to calculate the angle of the robot when using
the “Asin” function, , The accelerometer value is incremented Basically for the PID Calculations , we have two inputs the
and the funtion does when the angle of the robot is current actual angle for the robot and the desired angle .
increasing , With this function the angle of the robot can be When the robot is not moving the desired angle is at 0
calculated. degrees. By measuring the difference between the actual
The result is 57,296 is used becuase the asin function is in angle of the robot and the desired angle the PID calculates an
radians and not in degrees . This part will be the responsible output that is used for controlling the stepper motors
to handle the start of the robot :
The balancing robot is angle driven. First the difference
angle_acc = asin((float)accelerometer_data_raw/8200.0)* between the desired angel (setpoint) and actual angle
57.296; (process value) is calculated. The self_balance_pid_setpoint
if(start == 0 && angle_acc > -0.5&& angle_acc < 0.5){ variable is automatically changed to make sure that the robot
angle_gyro = angle_acc; stays balanced all the time, This is how the PID algorithm is
start = 1; made and has comments on each line for self explanation.
} .
//The (pid_setpoint - pid_output * 0.015) part functions as a
Considering that the robot starts when slowly is put into an brake function.
upright position; every loop that is 255 times per second. The pid_error_temp = angle_gyro - self_balance_pid_setpoint -
angle of the robot is measured with only the acceloremeter . pid_setpoint;
And if it is between (-0,5,0,5) degrees of the balancing point . if(pid_output > 10 || pid_output < -10)pid_error_temp +=
The angle Gyro variable is set to the current angle and start pid_output * 0.015 ;
variable is set to 1 to indicate that the robot is activated.
Next, the yaw and pitch are variables from the MPU6050 , //Calculate the I-controller value and add it to the
and the calibration of the offset is subtracted ; according to pid_i_mem variable
the datasheet of the MPU6050 we can see if the value output pid_i_mem += pid_i_gain * pid_error_temp;
is 131 the gyro output is 1 degree per second however //Limit the I-controller to the maximum controller output
according to the code , the output is set to ¼ degree per 4 ms if(pid_i_mem > 400)pid_i_mem = 400;
or 255 so the next equation is presented: else if(pid_i_mem < -400)pid_i_mem = -400;
//Calculate the PID output value
8
Manuela Beltrán University. Sebastian Balaguera.Tatiana Rocha Laboratory project (Design Applied to Biomedical Engineering)
pid_output = pid_p_gain * pid_error_temp + pid_i_mem + if(pid_setpoint > -2.5)pid_setpoint -= 0.05;
pid_d_gain * (pid_error_temp - pid_last_d_error);
//Limit the PI-controller to the maximum controller output if(pid_output > max_target_speed * -1)pid_setpoint -= 0.005;
//If the robot tips over or the start variable is zero or the }
battery is empty //Slowly reduce the setpoint to zero if no foreward or
if(angle_gyro > 30 || angle_gyro < -30 || start == 0 || backward command is given
low_bat == 1){ //If the PID setpoint is larger then 0.5 reduce the
//Set the PID controller output to 0 so the motors stop setpoint with 0.05 every loop
moving if(!(received_byte & B00001100)){
pid_output = 0; if(pid_setpoint > 0.5)pid_setpoint -=0.05;
//Reset the I-controller memory else if(pid_setpoint < -0.5)pid_setpoint +=0.05;
pid_i_mem = 0; else pid_setpoint = 0;
//Set the start variable to 0 }
start = 0;
//Reset the self_balance_pid_setpoint variable The self balancing point is adjusted when there is not forward
self_balance_pid_setpoint = 0; or backwards movement from the transmitter. This way the
} robot will always find it's balancing point
Now we have to see the how control calculations are done, //If the setpoint is zero degrees
all the lines of the code are commented in order to //Increase the self_balance_pid_setpoint if the robot is still
understand what the code does : moving forewards
if(pid_setpoint == 0){
//Control calculations if(pid_output < 0)self_balance_pid_setpoint += 0.0015;
//Copy the controller output to the pid_output_left variable
for the left motor and right motor //Decrease the self_balance_pid_setpoint if the robot is still
pid_output_left = pid_output; moving backwards
pid_output_right = pid_output;
//If the first bit of the receive byte is set change the left and if(pid_output > 0)self_balance_pid_setpoint -= 0.0015;
right variable to turn the robot to the left
if(received_byte & B00000001){ }
//Increase left / Decrease right
pid_output_left += turning_speed To finish the last part of the algorithm , basically it calculates
pid_output_right -= turning_speed; the pause times for the stepper controller
V. REFERENCIAS
[1] Anadigm, Inc., PID Control Customer Presentation,
PR080200-0004, Agosto 2002.
http://www.anadigm.com/_doc/FPAA Solutions for PID
ControlPID Control.pdf. [Citado en 2005]
[2] Ethesis.nitrkl.ac.in, 2019. [Online]. Available:
http://ethesis.nitrkl.ac.in/2213/1/Inverted_Pendulum.pd
f. [Accessed: 14- May- 2019].
[3] M. author:, "Self-Balancing Robot", Instructables, 2019.
[Online]. Available: 2. Program (Arduino)
a. (see Anexes in the Zip)