Another Ambassador Moment: 2 Wheel Self Balancing Robot!

2021-06-04 | By Tyler Marston

License: None Arduino

In this project we are going to make our very own self balancing robot! To start we are going to need a few parts:

(2) DC Gearbox motors
(1) Pololu TB6612FNG Dual Motor Driver
(1) Battery Pack
(1) 5v/3v Voltage Regulator
(1) Adafruit BNO055 Sensor
(1) Arduino Nano

The DC motors, Pololu TB6612FNG Dual Motor Driver and Adafruit BNO055 Sensor can be substituted with other similar parts, but I found these easy to work with.

All these parts need to be mounted onto a base that will equally distribute the weight. Using 3D modelling I designed this base. This base uses a clamping system to keep the motors perfectly aligned. There are many other bases that can work, but I found this base to be the best out of all the other options for my design. https://drive.google.com/file/d/1av8xvcs-HdmivYuFUwW5VuyzkBoZJLYQ/view?usp=sharing

Project%20Base

The next step is to assemble your circuit. The first step is to set up the Pololu TB6612FNG Dual Motor Driver. This motor driver allows me to independently control the speed of each motor using an outside voltage and a PWM signal from the Arduino Nano. The outside voltage will be supplied from the battery pack with a regulator to keep the voltage at 5v. Using the table below, I designed a segment of code that will correctly drive the motor. One thing to note, if your motors are spinning the opposite direction, you will need to reverse the polarity of the motor.

Motor%20Driver%20Data%20Sheet%20Table

Motor%20Driver

Once the motors are properly working, the next step is to attach the BNO055 IMU Sensor. This sensor has an Arduino library and can be used to read position, orientation, velocity and more. I used the euler angle data of the sensor to determine if the system is level or not. As seen in the code, there is an initial segment that sets the setpoint for the system before it starts. This is to eliminate all error from the sensor. Below is the setup of the circuit.

IMU

Circuit%20Design

Now that our robot is all set up, it is time to program its control system. I used a PID control system as it is easy to program and understand A PID controller or proportional-integral-derivative controller is a feedback control system that is used in many scenarios where a target value is trying to be achieved. As shown in the diagram, the error, e(t), is used to calculate a new output, and the new error is fed back to the beginning to form a continuous system. While I can make PID controllers by hand, to simplify our code I implemented the PID_v1 library. This library allows us to simply set values of our Kp, Ki and Kd and then functions from the library handle the math.

PID%20Controller

There are many different ways to tune a PID system. One method of tuning that worked well in my previous projects was the Zeigler Nichols Method. This method relies on ratios between value that causes oscillations and the period at which those oscillations occur. To begin, I was required to set both Kd and Ki to zero. Then, starting at 5, I steadily increase Kp until I observe steady oscillations. To track our incoming data, I left our cable from the computer attached to the robot. From the trails, I first found steady oscillations at a Kp of 29.6, which is designated as Ku. Next, I graphed these oscillations in MATLAB to find a period of 292 milliseconds, designated as Tu. Finally, to get our Ki, Kp and Kd values I use the table below. From this table I calculated Kp=17.76, Ki=121.64 & Kd=0.648. Upon first try, our system worked very well with the cable attached. Unfortunately, once the cable was removed from the system, I experienced a bit of trouble. After concluding the error was due to finding the values with a resistive cable, I had to reevaluate our system. To solve this issue, the group recalculated Ku with the cable being held in the least resistive position possible. The new values I achieved was a Ku of 38 and a Tu of 270 milliseconds. Again, using the table I found Kp=22.8, Ki=165.81 & Kd=0.783. This time when the cable was removed the robot successfully balanced itself.

Tuning%20Method%20Table

This is the MATLAB code used to find Tu

Copy Code

%% House Cleaning
clc;        % Clear the command window.
close all;  % Close all figures.
clear;      % Erase all existing variables.
workspace;  % Make sure the workspace panel is showing.

%% Read and store data
data = readmatrix('tilt-oscillations.xlsx'); % Read data from file

time = data(:, 1);   % Separate time data
tilt = data(:, 2);   % Seperate angle data

%% Normalize time
time = time - min(time); % Make time start from zero

%% Plot
figure(1)
plot(time, tilt)
grid on
xlabel('Time (ms)')
ylabel('Angle (degrees)')
title('Oscillations')

This is the final Arduino Code that was used in my project

Copy Code

#include <Wire.h>
#include <PID_v1.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>

/* PINS */
#define AIN1 2
#define AIN2 3
#define PWMA 5
#define BIN1 7
#define BIN2 8
#define PWMB 6
#define STBY 9

/* ALIAS */
#define LEFT 0
#define RIGHT 1
#define FORWARD 1
#define BACKWARD 0

/* VARIABLES */
double y_input, y_output, y_sp;
const int Ts = 10; // Sample Rate [ms]

/* OBJECTS */
Adafruit_BNO055 IMU = Adafruit_BNO055(55, 0x28);
PID BalancePID(&y_input, &y_output, &y_sp, 22.8, 165.81, 0.783, DIRECT);

void SpinMotor(int motor, int direction, int speed) {
  if (motor == LEFT) {
    digitalWrite(AIN1, direction);
    digitalWrite(AIN2, !direction);
    analogWrite(PWMA, speed);
  }
  else {
    digitalWrite(BIN1, direction);
    digitalWrite(BIN2, !direction);
    analogWrite(PWMB, speed);
  }
}

void forward(int speed) {
  SpinMotor(LEFT, FORWARD, speed);
  SpinMotor(RIGHT, FORWARD, speed);
}
void backward(int speed) {
  SpinMotor(LEFT, BACKWARD, speed);
  SpinMotor(RIGHT, BACKWARD, speed);
}

void getPosition() {
  sensors_event_t data;
  IMU.getEvent(&data, Adafruit_BNO055::VECTOR_EULER);

  y_input = data.orientation.z;
  y_input = map(y_input, -90.0, 90.0, 0.0, 180.0);
}

void setup() {
  Serial.begin(115200); // Set baud rate
  while (!Serial); // Wait for serial port to connect.

  pinMode(PWMA, OUTPUT);
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(PWMB, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);
  pinMode(STBY, OUTPUT);

  BalancePID.SetMode(AUTOMATIC);
  BalancePID.SetOutputLimits(-255, 255);
  BalancePID.SetSampleTime(Ts);

  if (!IMU.begin(IMU.OPERATION_MODE_IMUPLUS)) {
    Serial.println("Ooops, no BNO055 detected...");
    while (true);
  }
  delay(1000);
  getPosition();
  y_sp = y_input;
  digitalWrite(STBY, HIGH);
}

void loop() {
  getPosition();
  BalancePID.Compute();
   if (y_output < 0)
    backward(abs(y_output));
  else
    forward(abs(y_output));
}

Add all DigiKey Parts to Cart

Have questions or comments? Continue the conversation on TechForum, DigiKey's online community and technical resource.

Visit TechForum

Arduino | 3D Printing | Raspberry Pi

Another Ambassador Moment: 2 Wheel Self Balancing Robot!

2021-06-04 | By Tyler Marston

信息

帮助

联系我们

关注我们