Projects

Inverted Pendulum

Proportional Integral
Derivative Controller

Constructed and programmed a bi-wheeled robot to balance itself upon the positive Z-axis by using Proportional Integral Derivative (PID) control

Control Loop Schematic:

Circuit Schematic:

Parts List

 

Name

Manufacturer

Part Number

Purpose

2 x NEMA 17 Stepper Motor

StepperOnline

17HS16-2004S 

Moves the wheels on the robots 

2 x 100mm Wheels

N/A

N/A

Allows the robot to move around and recover from pushes

1 x MPU 6050

FTCBlock 

GY-521

Calculates the angle of the robot’s frame relative to the ground

1 x Arduino Nano

N/A

N/A

Calculates the changes in the wheels to make according to the internal PID and the Gyro readings

Initialization

 

int gyro_address = 0x68;          //MPU-6050 I2C address
int acc_calibration_value = 1000; //Enter the accelerometer calibration value

//Various settings
float pid_p_gain = 40;            //Gain setting for the P-controller
float pid_i_gain = 1;           //Gain setting for the I-controller
float pid_d_gain = 40;            //Gain setting for the D-controller
float turning_speed = 30;
float max_target_speed = 150;

////////////////////////////////////////////////////////////////////////
//Declaring global variables
////////////////////////////////////////////////////////////////////////
byte start, received_byte, low_bat;

int left_motor, throttle_left_motor, throttle_counter_left_motor, throttle_left_motor_memory;
int right_motor, throttle_right_motor, throttle_counter_right_motor, throttle_right_motor_memory;
int battery_voltage;
int receive_counter;
int gyro_pitch_data_raw, gyro_yaw_data_raw, accelerometer_data_raw;

long gyro_yaw_calibration_value, gyro_pitch_calibration_value;

unsigned long loop_timer;

float angle_gyro, angle_acc, angle, self_balance_pid_setpoint;
float pid_error_temp, pid_i_mem, pid_setpoint, gyro_input, pid_output, pid_last_d_error;
float pid_output_left, pid_output_right;

PID Calculations

 

pid_error_temp = angle_gyro – self_balance_pid_setpoint – pid_setpoint;
if(pid_output > 10 || pid_output < -10)
  pid_error_temp += pid_output * 0.015 ;

pid_i_mem += pid_i_gain * pid_error_temp;
if(pid_i_mem > 400)
  pid_i_mem = 400;
else if(pid_i_mem < -400)
  pid_i_mem = -400;
//Calculate the PID output value
pid_output = pid_p_gain * pid_error_temp + pid_i_mem + pid_d_gain * (pid_error_temp – pid_last_d_error);
if(pid_output > 400)
  pid_output = 400;
else if(pid_output < -400)
  pid_output = -400;

pid_last_d_error = pid_error_temp;                     //Store the error for the next loop

if(pid_output < 5 && pid_output > -5)
  pid_output = 0;   //Create a dead-band to stop the motors when the robot is balanced

if(angle_gyro > 30 || angle_gyro < -30 || start == 0 || low_bat == 1){
  //If the robot tips over or the start variable is zero or the battery is empty
  pid_output = 0;        //Set the PID controller output to 0 so the motors stop moving
  pid_i_mem = 0;       //Reset the I-controller memory
  start = 0;                 //Set the start variable to 0
  self_balance_pid_setpoint = 0;
  }