3D Printed Humanoid Robot – Robonoid – Design concept – 20180619

[20190107] Upload Links
[20181101] Printing
[20180621] Update Design concept
[20180619] Update Design concept
[20180607] Update Design concept
[20180604] Update Design concept
[20180430] Design concept

Humanoid Robot – Robonoid – Design concept – https://youtu.be/n-flpiyNw-M

https://youtu.be/WonvWjfs4Jc

3D Design Tool: SketchUp Pro

Robonoid is small sized bipedal walking robot The robot has 17 or 19, 24 freely moveable joints and servomotor in order to provide for a range of action and stable movements. Since it’s small in size, Robonoid can balance well and cope with basic movements such as walking and getting up. Also, intricate movements like roller skating and skateboarding are possible.

Robonoid is a wireless controllable robot You can control it by WiFi protocol through your PC and Smartphones. App for android and iOS are an especially intelligible UI. By using it, complicated operations can be controlled more easily.

Robonoid is a friendly robot Robonoid was named indicates a “simply shaped robot” that everyone imagines. Robonoid was designed by pursuing a simple appearance and simple functionality.

Robonoid Lineup

https://youtu.be/uXKarzPmxNc

PSY – 135.7mm(W) x 258.39mm(H) x 100mm(D) – 17DOF
Jack – 135.7mm(W) x 305.62mm(H) x – 92,48mm(D) – 22DOF
Gentleman – 135.7mm(W) x 341.22mm(H) x – 78.5mm(D) – 22DOF
Tony – 135.7mm(W) x 265.5mm(H) x – 100mm(D) – 18DOF
SpongwBob – 135.7mm(W) x 230.0mm(H) x 100mm(D) – 16DOF
Hudi – 135.7mm(W) x 251.4mm(H) x 87mm(D) – 19DOF
Gunmo – 135.7mm(W) x 259.3mm(H) x 78.5mm(D) – 19DOF – https://youtu.be/qIZJZDcRpVw
Nova – 135.7mm(W) x 282.8mm(H) x 100mm(D) – 17DOF – https://youtu.be/kzfyKRzp_9I
Hexapod S1 – DOM 352.83mm(WD) x 193.16mm(H) – 20DOF

Robonoid is a Plen/mini-Plan/RoboHero robot derivative designed at Zalophus DesignHouse. We love the Plen2 robot but its want to new design. This is our take on a new lower cost version of the Plen2 robot using MG90S/ES08MA-II/SG90 servo’s.

The 3D printing parts were inspired by the Plen2 components, but they were redrawn from SketchUp to use the inexpensive MG90S servo motors.

Electronic Parts

1 x WeMos D1 mini ESP8266 ESP-12
1 x PCA9685 16-channel, 12-bit PWM Fm+ I2C-bus Servo controller
1 x Shield Robonoid-20CH-R0a
1 x HC-SR04 Ultrasonic Distance Measurement Sensor
2 x 16340 or 18650 Battery Holder
1 x 2S 7.4V Lithium Battery Charger Protection Board
2 x 16340 or 18650 Batteries
2 x Snap-In Single ‘A’-‘AA’ Battery Contacts 209 [KEYSTONE ELECTRONICS CORP.]
2 x Snap-In Single ‘A’-‘AA’ Battery Contacts 228 [KEYSTONE ELECTRONICS CORP.]
1 x DC Jack and Battery Harness Cable
17/19 x MG90S Metal Gear Servo Motors

3D Printing Parts

WANT TO SUPPORT Zalophus’s DesignHouse?

The open source Humanoid Robot – Robonoid hardware and software is free and made with love. Please show your level of support with a voluntary donation.

Donate: https://www.paypal.com/cgi-bin/webscr?cmd=_s-xclick&hosted_button_id=RDN7ZGAVFS5UE

 

Setting up the Arduino IDE on Raspbian

 If you have used the Sleepy Pi setup script you DO NOT need to do this. The script has already set this up for you.

The following instructions relate to installation on the standard Raspberry Pi Operating System (OS) Raspbian. Following the principals shown here, it should be possible to apply these same modifications to other Raspberry OS’s.

The Arduino processor on the Sleepy Pi can be programmed directly from the Arduino IDE running on the Raspberry Pi.

The first step is to load the Arduino environment onto the Raspberry Pi. If you haven’t already done so, it’s a good idea to ensure that your Raspbian is up to date by opening up an LXTerminal window and executing the following:

Note the dist-upgrade will ensure that you have the latest versions of RPi.GPIO which will be required later.

Now install the Arduino IDE with:

(click Y to any dependencies)

Setting up the Serial Pins

The Arduino processor on the Sleepy Pi can be programmed directly from the Raspberry Pi using the serial GPIO lines on the RPi and another GPIO line to reset the the Arduino to allow automatic code upload. These pins are:

GPIO 14: TXD

GPIO 15: RXD

GPIO 22: Reset (see next section)

By default Raspbian has exclusive access to the serial pins to output status, debug data and logging in. We need to change that using the following steps.

Step 1: Disable Serial login

Wheezy

Raspbian allows you to login using the serial port. To use the Sleepy Pi we need to disable this. To do this, we need to edit /etc/inittab.

In an LXTerminal window type:

Scroll down the bottom and you will find the lines:

You need to comment the last line out (i.e. disabling it) with a “#” and save it, so that it results in:

Jessie and Raspberry Pi 3

Raspbian Jessie no longer has the /etc/inittab file and replaces it instead with a mechanism called systemd and you use a tool called systemctl to enable / disable services.

In a terminal type the following commands, for all Raspberry Pi’s except Raspberry Pi 3:

Instead, for Raspberry Pi 3:

On Jessie, you also need to enable the serial port in either rasp-config of in /boot/config.txt with:

and add the line:

Step 2. Disable Boot info

When Raspbian boots up it outputs boot information to the serial port and hence streams it to the Sleepy Pi (which is not particularly interested in it). To disable this we need to edit the /boot/cmdline.txt in LXTerminal:

It will look something like:

Delete the “console=serail0,115200″ parts so that you are left with:

This is the file I use:

Step 3. Link the Serial port to the Arduino IDE

NOTE: For Raspberry Pi 3 Omit Step 3 – /dev/ttyS0 is the default mapping not /dev/ttyAMA0 which is used for the Bluetooth.

The Arduino IDE wants to use the /dev/ttyS0 serial port, but we need to use the /dev/ttyAMA0 which is linked to the GPIO. In order to do this we need to create a permanent link that maps AMA0 to S0. To this we need to create a small file. We can do this in LXTerminal by the following sequence:

In the new file that it creates type the following:

Save this file as a new file called “80-sleepypi.rules” to: /etc/udev/rules.d/

This is the one I use:

Setting up the Reset (DTR) pin

The Sleepy Pi Arduino processor reset line in connected to GPIO 22. To automatically upload code from the Arduino IDE we need to pulse this line low to rest the Arduino and enter bootload mode.

On a normal Arduino system connected to a computer via a USB / serial cable the reset line is connected to the DTR line. To replicate this behavior on the Raspberry Pi we need to hack the AVRDude programming software. Dean Mao has detailed a great hack for this. He’s produced a modified version of Avrdude (avrdude-autoreset) and written a piece of python code (autoreset) that runs in the background and pulses the GPIO line when required.

Download the avrdude-rpi files from Github . Select “Download ZIP” file which will download the file into your /home/pidirectory.

NOTE: this is a subtly modified version of the original Dean Mao version so only use this version.

Open up a File Manager window from the bottom toolbar, locate the ZIP file in your /home/Pi directory and right-click and select xarchiver to extract the files to a folder in /home/Pi. These will extract into a folder named “avrdude-rpi-master“.

If you are using a terminal rather than the GUI, you can use the following from the command lines to download and unzip :

Next, copy the files to the appropriate places from a LXTerminal window using the following commands:

This renames your original avrdude, so that you have a backup and can replace it with the new one.

Then link the new avrdude-autoreset to avrdude with the ‘ln” command so that when something calls for avrdude, the new version runs instead.

Adding the Sleepy Pi to the Arduino environment

To enable the Sleepy Pi to be selected from the IDE you need to add a folder and file to your sketchbook. If it is a fresh install and you haven’t yet run the Arduino environment you’ll need to create a sketchbook folder (skip this step if it already exists).

In LXTerminal type:

to create folders “hardware” and “sleepy pi” in your Arduino sketchbook.

Download and copy the boards.txt file to the Sleepy Pi folder.

This file should look like this:

Reboot

And finally reboot your Raspberry Pi to complete and load all our changes. You can use:

Arduino Polar V Plotter

This Instructable will be about Polar Plotters.

There are a lot of different versions to build one, and i just wanna share how i built it.

The Software i use is called Makelangelo, and can be downloaded here:

http://www.makelangelo.com

If you have further questions or ideas to perfect my design, please feel free to comment 🙂

Watch it draw:

And the idea of the penholder, as video:

Step 1: GENERAL_ COMPONENTS BOM

BILL OF MATERIALS
(all prices like i paid on amazon)

2 stepper motors 2×25$

I used NEMA8 and NEMA17. Both work well, the 8beeing a quarter of the size of the 17.

If you want to get an idea for the numbers of NEMA steppers, its 1/10 of the distance between two nearby mounting holes. In inches.

So in a NEMA 8 motor, the mounting holes are placed on an 0,8″x0,8″ square. Pretty tiny.

Stepper motor drivers 30$ from adafruit or 8$ clone

As i used the Makelangelo firmware, i used an Adafruit Motorshield v1 (clone)
Makelangelo runs with(out modification) on an Uno with AMShield v1 and v2, and with a MEGA 2560 and the RAMPS 1.4 shield.
I tested the uno with v1 and the RAMPS setup, both work.

Micro servo or Solenoid 5$ each

Both work, the solenoid is the more elegant option, but needs some tweaks to the firmware.
You can take virtually any servo available.

On my penholder, i use a standard 5$ microservo, as you can see on some photos.

Belt/Chain 12$ (see dedicated step)

The cheaper and more elgant version is the belt.
I still used white pearlropes from home depot first, because i didnt know that 5m belt with 5 pulleys are 12$ on amazon.

Pens (see dedicated steps)

It sucks pens dry like nothing, so ballpens are the most efficient, copics the most expensive

Step 2: ELECTRONICS_ ADAFRUIT SHIELD ARDUINO

I used an Arduino (Genuino) UNO, but ordered a feather M0 with a stepperwing, to make it more compact.
There isnt any special thing to consider, just plug the shield to the arduino and give it the code 🙂

On thing i found out is, the L284 drivers get really really hot, so if they didnt came with heatsinks, better add some, or mount a fan like i did.

Probably both, depending on the motors/weight.

To mount the cooling fan i took a sheet of 3mm fibreglass and cut it with a jigsaw.
The fan is pretty strong and loud, i probably add a pot in the future to tune it down a bit.

Step 3: MECHANICS_ BELTS ROPES PEARL-ROPES

Basically there are three systems.

Beginning with the worst possible version:

A spindle and some rope.

The problem is, that with spooling of the rope, the spindle-diameter changes, so the calculation isnt correct anymore, which ends in wacky drawings.

A Pearlchain from window-shutters.

Very light and precise setup. I have built a big plotter for shop windows and liquid chalk that uses two 3m ropes.
The problem with the rope and the pearls is, that it needs special idler pulleys if you wanna use idler pulleys at all.

A belt from 3d printing spares

To me the best and most affordable version is a 5mm belt. As i wrote earlier i bought 5m of belt together with five teeth pulley for 12$ on amazon.
Its super precise, though i think it flexes more then the rope.

Step 4: MECHANICS_ STEPPERS MOUNTS IDLER-PULLEYS

The easiest and cleanest version is to just mount a teeth pulley on the motor and let it go.
Be sure to add some counterweight, to prevent slip.

If you want or need to make it more complex, i added some photos, that solve different problems i had at that time.

On the NEMA17 stepper photos:

One thing i always tried to do was to keep distance between the left and right idler pulley as wide as possible.
In a polar v plotter, one situation that is sub-optimal is, when one of the belts hangs down vertically.
Thats the position where the motors have the least control about what the pen is doing.

I tried to avoid that, by mounting the last idler pulleys wider then the canvas.

Next i had to mount a second idler to get the counterweight out of the line.

On the NEMA 8 photo:

This will be a small A3 version, that hangs on the wall, framed, and takes portrait of random people.
O the goal was to make it as flat as possible. As you can see there is one 90degree twist in the belt to get the motor parallel to the wall.
All idlers have double ball bearings and run supersmooth

Step 5: MECHANICS_ PEN GONDOLA

There are endless versions how to mount a pen and lift a pen hanging on two ropes.

The blue penholder on the photos is made from a segafredo coffeecan some makeblock parts and a servo.
The second (yellow) version is made for endless-lines drawings (without lifting) and is made of three ballbearings and three 3mm fibreglass parts.

If anyone is interested in the second design, i will get a 3d printer very soon, and can add a 3d file as soon as i tested/printed it on my printer.

Step 6: MECHANICS_ DRAWING SURFACES SIZES PAPER-FORMATS

I would say there are three classic surfaces for a plotter like that:

First the surface where the polar v plotter is the only plotter capeable to draw on:

Shopwindows/ windows in general

For drawing on windows, besides mirroring the graphics (which Makelangelo can do in the software) you need suction cups/glasslifters and liquid chalk pens.
Liquid chalk seems to be some sort of chemical substance that you can paint on windows with and easily wipe it of to clean the windows.
Its filled in a regular boardmarker sized pen.

Whiteboards

Also easily removable and a perfect surface to permanently mount a drawbot.
you can test directly on the whiteboard without wasting paper, and once it runs smooth, just use four magnets and put a paper on it.

Paper in general, obviously

The rougher the paper surface, the better.
The less preassure a pen needs to write, the better.

Even if the pen holder is heavy, it puts a very tiny force on the actual drawing surface.
You can always add force, by tilting the surface backwards.

On the big whiteboard plotter i mounted in my office i lifted the base of the whiteboard by 2″/5cm to get some angle.

Step 7: SOFTWARE_ MAKELANGELO

That’s pretty straight forward, just visit their site, and download the files.

Dont forget to contribute a bit, so they can keep on developing fine software like Makelangelo

http://www.makelangelo.com

The code, and java is well documented, so i wont discuss it in detail.

Step 8: GENERAL_ WHAT ABOUT …

Some ideas, what to do with a plotter like that, and how i use it.

On a whiteboard in the office
I work for a creative school and i got it in my office, to plot svg files graphic design students made, or print other stuff i would normally write on it, like xls sheets etc

With a laser-head on wooden doors, print foilage on hunterstands etc

With a spraycan on a housewall

With a felt tip on a dirty window

Arduino Self-Balancing Robot

Hello, everyone!

In this instructable, I’ll show you how to build a small self-balancing robot that can move around avoiding obstacles. This is a tiny robot measuring 4 inches wide and 4 inches tall and is based on the Arduino Pro Mini development board and the MPU6050 accelerometer-gyroscope module.

In the steps that follow, we will see how to interface the MPU6050 with Arduino, how to measure the angle of inclination of the robot, how to use PID to make the robot stay balanced. An ultrasonic rangefinder is also added to the robot which prevents it from banging into obstacles as it wanders around.

Parts List

I bought most of these parts from aliexpress but you can find them in any other electronics store as well.

1. Arduino Pro Mini

2. GY-521 module with MPU-6050

3. DRV8833 Pololu motor driver

4. 2, 5V boost converter

5. US-020 ultrasonic distance sensor

6. NCR18650 battery and holder

7. Pair of micro metal gear motors (N20, 6V, 200 rpm) and brackets

8. Pair of 42x19mm wheels

9. 3, Double-sided prototype PCB (4cm x 6cm)

10. 8, 25cm Nylon spacers and 4, nylon nuts

Apart from the above, you will need some cables, berg connectors and one on/off switch.

Step 1: A Bit of Theory

Let’s start with some fundamentals before getting our hands dirty.

The self-balancing robot is similar to an upside down pendulum. Unlike a normal pendulum which keeps on swinging once given a nudge, this inverted pendulum cannot stay balanced on its own. It will simply fall over. Then how do we balance it? Consider balancing a broomstick on our index finger which is a classic example of balancing an inverted pendulum. We move our finger in the direction in which the stick is falling. Similar is the case with a self-balancing robot, only that the robot will fall either forward or backward. Just like how we balance a stick on our finger, we balance the robot by driving its wheels in the direction in which it is falling. What we are trying to do here is to keep the center of gravity of the robot exactly above the pivot point.

To drive the motors we need some information on the state of the robot. We need to know the direction in which the robot is falling, how much the robot has tilted and the speed with which it is falling. All these information can be deduced from the readings obtained from MPU6050. We combine all these inputs and generate a signal which drives the motors and keeps the robot balanced.

Step 2: Let’s Start Building

We will first complete the circuitry and structure of the robot. The robot is built on three layers of perfboards that are spaced 25mm apart using nylon spacers. The bottom layer contains the two motors and the motor driver. The middle layer has the controller, the IMU, and the 5V boost regulator modules. The top most layer has the battery, an on/off switch and the ultrasonic distance sensor (we will install this towards the end once we get the robot to balance).

Before we begin to prototype on a perfboard we should have a clear picture about where each part should be placed. To make prototyping easy, it is always better to draw the physical layout of all the components and use this as a reference to place the components and route the jumpers on the perfboard. Once all the parts are placed and soldered, interconnect the three boards using nylon spacers.

You might have noticed that I’ve used two separate voltage regulator modules for driving the motors and the controller even though they both require a 5V source. This is very important. In my first design, I used a single 5V boost regulator to power up the controller as well as the motors. When I switched on the robot, the program freezes intermittently. This was due to the noise generated from the motor circuit acting upon the controller and the IMU. This was effectively eliminated by separating the voltage regulator to the controller and the motor and adding a 10uF capacitor at the motor power supply terminals.

Step 3: Measuring Angle of Inclination Using Accelerometer

The MPU6050 has a 3-axis accelerometer and a 3-axis gyroscope. The accelerometer measures acceleration along the three axes and the gyroscope measures angular rate about the three axes. To measure the angle of inclination of the robot we need acceleration values along y and z-axes. The atan2(y,z)function gives the angle in radians between the positive z-axis of a plane and the point given by the coordinates (z,y) on that plane, with positive sign for counter-clockwise angles (right half-plane, y > 0), and negative sign for clockwise angles (left half-plane, y < 0). We use this library written by Jeff Rowberg to read the data from MPU6050. Upload the code given below and see how the angle of inclination varies.

#include “Wire.h”
#include “I2Cdev.h” #include “MPU6050.h” #include “math.h”

MPU6050 mpu;

int16_t accY, accZ; float accAngle;

void setup() { mpu.initialize(); Serial.begin(9600); }

void loop() { accZ = mpu.getAccelerationZ(); accY = mpu.getAccelerationY(); accAngle = atan2(accY, accZ)*RAD_TO_DEG; if(isnan(accAngle)); else Serial.println(accAngle); }

Try moving the robot forward and backward while keeping it tilted at some fixed angle. You will observe that the angle shown in your serial monitor suddenly changes. This is due to the horizontal component of acceleration interfering with the acceleration values of y and z-axes.

Step 4: Measuring Angle of Inclination Using Gyroscope

The 3-axis gyroscope of MPU6050 measures angular rate (rotational velocity) along the three axes. For our self-balancing robot, the angular velocity along the x-axis alone is sufficient to measure the rate of fall of the robot.

In the code given below, we read the gyro value about the x-axis, convert it to degrees per second and then multiply it with the loop time to obtain the change in angle. We add this to the previous angle to obtain the current angle.

#include “Wire.h”
#include “I2Cdev.h” #include “MPU6050.h”

MPU6050 mpu;

int16_t gyroX, gyroRate; float gyroAngle=0; unsigned long currTime, prevTime=0, loopTime;

void setup() { mpu.initialize(); Serial.begin(9600); }

void loop() { currTime = millis(); loopTime = currTime – prevTime; prevTime = currTime; gyroX = mpu.getRotationX(); gyroRate = map(gyroX, -32768, 32767, -250, 250); gyroAngle = gyroAngle + (float)gyroRate*loopTime/1000; Serial.println(gyroAngle); }

The position of the MPU6050 when the program starts running is the zero inclination point. The angle of inclination will be measured with respect to this point.

Keep the robot steady at a fixed angle and you will observe that the angle will gradually increase or decrease. It won’t stay steady. This is due to the drift which is inherent to the gyroscope.

In the code given above, loop time is calculated using the millis() function which is built into the Arduino IDE. In later steps, we will be using timer interrupts to create precise sampling intervals. This sampling period will also be used in generating the output using a PID controller.

Step 5: Combining the Results With a Complementary Filter

Google defines complementary as “combining in such a way as to enhance or emphasize the qualities of each other or another”.

We have two measurements of the angle from two different sources. The measurement from accelerometer gets affected by sudden horizontal movements and the measurement from gyroscope gradually drifts away from actual value. In other words, the accelerometer reading gets affected by short duration signals and the gyroscope reading by long duration signals. These readings are, in a way, complementary to each other. Combine them both using a Complementary Filter and we get a stable, accurate measurement of the angle. The complementary filter is essentially a high pass filter acting on the gyroscope and a low pass filter acting on the accelerometer to filter out the drift and noise from the measurement.

 currentAngle = 0.9934 * (previousAngle + gyroAngle) + 0.0066 * (accAngle)

0.9934 and 0.0066 are filter coefficients for a filter time constant of 0.75s. The low pass filter allows any signal longer than this duration to pass through it and the high pass filter allows any signal shorter than this duration to pass through. The response of the filter can be tweaked by picking the correct time constant. Lowering the time constant will allow more horizontal acceleration to pass through.

Eliminating accelerometer and gyroscope offset errors
Download and run the code given in this page to calibrate the MPU6050’s offsets. Any error due to offset can be eliminated by defining the offset values in the setup() routine as shown below.

mpu.setYAccelOffset(1593);
mpu.setZAccelOffset(963); mpu.setXGyroOffset(40);

Step 6: PID Control for Generating Output

PID stands for Proportional, Integral, and Derivative. Each of these terms provides a unique response to our self-balancing robot.

The proportional term, as its name suggests, generates a response that is proportional to the error. For our system, the error is the angle of inclination of the robot.

The integral term generates a response based on the accumulated error. This is essentially the sum of all the errors multiplied by the sampling period. This is a response based on the behavior of the system in past.

The derivative term is proportional to the derivative of the error. This is the difference between the current error and the previous error divided by the sampling period. This acts as a predictive term that responds to how the robot might behave in the next sampling loop.

Multiplying each of these terms by their corresponding constants (i.e, Kp, Ki and Kd) and summing the result, we generate the output which is then sent as command to drive the motor.

Step 7: Tuning the PID Constants

1. Set Ki and Kd to zero and gradually increase Kp so that the robot starts to oscillate about the zero position.

2. Increase Ki so that the response of the robot is faster when it is out of balance. Ki should be large enough so that the angle of inclination does not increase. The robot should come back to zero position if it is inclined.

3. Increase Kd so as to reduce the oscillations. The overshoots should also be reduced by now.

4. Repeat the above steps by fine tuning each parameter to achieve the best result.

Step 8: Adding the Distance Sensor

The ultrasonic distance sensor that I’ve used is the US-020. It has four pins namely Vcc, Trig, Echo, and Gnd. It is powered by a 5V source. The trigger and echo pins are respectively connected to digital pins 9 and 8 of Arduino. We will be using the NewPing library to get the distance value from the sensor. We will read the distance once every 100 milliseconds and if the value is between 0 and 20cm, we will command the robot to perform a rotation. This should be sufficient to steer the robot away from the obstacle.

Step 9: The Complete Code

#include "Wire.h"

#include “I2Cdev.h” #include “MPU6050.h” #include “math.h” #include <NewPing.h>

#define leftMotorPWMPin 6 #define leftMotorDirPin 7 #define rightMotorPWMPin 5 #define rightMotorDirPin 4

#define TRIGGER_PIN 9 #define ECHO_PIN 8 #define MAX_DISTANCE 75

#define Kp 40 #define Kd 0.05 #define Ki 40 #define sampleTime 0.005 #define targetAngle -2.5

MPU6050 mpu; NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

int16_t accY, accZ, gyroX; volatile int motorPower, gyroRate; volatile float accAngle, gyroAngle, currentAngle, prevAngle=0, error, prevError=0, errorSum=0; volatile byte count=0; int distanceCm;

void setMotors(int leftMotorSpeed, int rightMotorSpeed) { if(leftMotorSpeed >= 0) { analogWrite(leftMotorPWMPin, leftMotorSpeed); digitalWrite(leftMotorDirPin, LOW); } else { analogWrite(leftMotorPWMPin, 255 + leftMotorSpeed); digitalWrite(leftMotorDirPin, HIGH); } if(rightMotorSpeed >= 0) { analogWrite(rightMotorPWMPin, rightMotorSpeed); digitalWrite(rightMotorDirPin, LOW); } else { analogWrite(rightMotorPWMPin, 255 + rightMotorSpeed); digitalWrite(rightMotorDirPin, HIGH); } }

void init_PID() { // initialize Timer1 cli(); // disable global interrupts TCCR1A = 0; // set entire TCCR1A register to 0 TCCR1B = 0; // same for TCCR1B // set compare match register to set sample time 5ms OCR1A = 9999; // turn on CTC mode TCCR1B |= (1 << WGM12); // Set CS11 bit for prescaling by 8 TCCR1B |= (1 << CS11); // enable timer compare interrupt TIMSK1 |= (1 << OCIE1A); sei(); // enable global interrupts }

void setup() { // set the motor control and PWM pins to output mode pinMode(leftMotorPWMPin, OUTPUT); pinMode(leftMotorDirPin, OUTPUT); pinMode(rightMotorPWMPin, OUTPUT); pinMode(rightMotorDirPin, OUTPUT); // set the status LED to output mode pinMode(13, OUTPUT); // initialize the MPU6050 and set offset values mpu.initialize(); mpu.setYAccelOffset(1593); mpu.setZAccelOffset(963); mpu.setXGyroOffset(40); // initialize PID sampling loop init_PID(); }

void loop() { // read acceleration and gyroscope values accY = mpu.getAccelerationY(); accZ = mpu.getAccelerationZ(); gyroX = mpu.getRotationX(); // set motor power after constraining it motorPower = constrain(motorPower, -255, 255); setMotors(motorPower, motorPower); // measure distance every 100 milliseconds if((count%20) == 0){ distanceCm = sonar.ping_cm(); } if((distanceCm < 20) && (distanceCm != 0)) { setMotors(-motorPower, motorPower); } } // The ISR will be called every 5 milliseconds ISR(TIMER1_COMPA_vect) { // calculate the angle of inclination accAngle = atan2(accY, accZ)*RAD_TO_DEG; gyroRate = map(gyroX, -32768, 32767, -250, 250); gyroAngle = (float)gyroRate*sampleTime; currentAngle = 0.9934*(prevAngle + gyroAngle) + 0.0066*(accAngle); error = currentAngle – targetAngle; errorSum = errorSum + error; errorSum = constrain(errorSum, -300, 300); //calculate output from P, I and D values motorPower = Kp*(error) + Ki*(errorSum)*sampleTime – Kd*(currentAngle-prevAngle)/sampleTime; prevAngle = currentAngle; // toggle the led on pin13 every second count++; if(count == 200) { count = 0; digitalWrite(13, !digitalRead(13)); } }

Step 10: Final Thoughts

Spending a bit more time on tweaking the PID constants would give us a better result. The size of our robot also limits the level of stability we can achieve. It is easier to build a full-sized balancing robot than it is to build a small one like ours. Still, I guess, our robot does a pretty decent job in balancing on various surfaces as shown in the video.

That’s it for now.

Thanks for your time. Don’t forget to leave your thoughts in the comments section.

Self Balancing Robot

First of all I want to apologize for my English, if you don’t understand something, please, ask.

I know that a self-balancing robot is not new, but when i started this project i found a lot of information, but never in the same site, i had to search a lot to join all information in a single project. Becouse of that i’m making this instrucctable, to show you all the information i get, with all detail, to make that robot.

This project is for all of you that like’s to make robots but don’t have many things, and by things i mean time, money and robotics knowledge. In this project i’m gonna show you the easiest way to do a simple, cheap and useless two wheels self-balancing robot.

I explain the materials and electronics used in the project, how and where to buy or create it and i’m gonna tell you my experience and tips along the way to create this project.

Step 1: Materials

The materials i used for this projects were the cheapest i could get, but there are even cheaper. Principally i buy from two places: DX, a Chinese online store with lots of very cheap electronic (arduino, drivers, sensors,…) and free shipping (that’s a good point); and Robot-Italy, an Italian store specialized in kits for robotics.

From Robot-Italy i get the chassis from a kit for a 3 wheeled robot and the battery, a LiPo of 1300mAh

Chassis:

http://www.robot-italy.com/en/magician-chassis-kit…

Batery:

http://www.robot-italy.com/en/fullpower-batteria-l…

You can buy the Arduino form both stores, in Robot-Italy have the official version (23€) and in DX have the Chinese version (10€), in not gonna open a debate but i used both and they just work fine:

http://www.robot-italy.com/en/arduino-uno-r3.html

http://www.dx.com/p/uno-r3-development-board-micro…

The last two thing left are the IMU sensor and the motor driver, both bought from DX:

Motor driver:

http://www.dx.com/p/l298n-stepper-motor-driver-con…

IMU sensor:

http://www.dx.com/p/gy-80-bmp085-9-axis-magnetic-a…

Making a price summary:

Arduino UNO ——————–> 10-20 €

IMU sensor ——————–> 15 €

Motor driver ——————–> 4 €

Chassis ——————–> 19 €

Battery ——————–> 10 €

______________________________________

TOTAL : 58-68 €

I used materials as cheap if i could but you can use whatever you have, i saw people using servo motors and stepper motors with a good result. This motor driver maybe is much bigger than the needed one, with an L293 it can work, you can make your own chassis and use other type of sensors.

Step 2: Phisics

The physics for this robot are simple, the robot stand in two points lined, the wheel, and i tends to fall and lose his verticality, the movement of the wheel in the direction of the falling rises the robot for recover the vertical position.

A Segway-type vehicle is a classic inverted pendulum control problem that is solvable in two degrees of freedom for the simplest models. The vehicle attempts to correct for an induced lean angle by moving forward or backwards, and the goal is to return itself to vertical. Or at least not fall over.

For that objective we have two things to do, in one hand we have to measure the angle of inclination (Roll) that have the vehicle, and in the other hand we have to control the motors for going forward or backwards to make that angle 0, maintaining his verticality.

Angle Measurement:

For measure the angle we have two sensors, accelerometer and gyroscope, both have his advantages and disadvantages. The accelerometer can measure the force of the gravity, and with that information we can obtain the angle of the robot, the problem of the accelerometer is that it can also measure the rest of the forces the vehicle is someted, so it has lot of error and noise. The gyroscope measure the angular velocity, so if we integrate this measure we can obtain the angle the robot is moved, the problem of this measure is that is not perfect and the integration has a deviation, that means that in short time the measure is so good, but for long time terms the angle will deviate much form the real angle.

Those problems can be resolved be the combination of both sensors, that’s called sensor fusion, and there are a lot of methods to combine it. In this project i try two of them: Kalman Filter, and complementary filter.

  • The Kalman filter is an algorithm very extended in robotics, and offers a good result with low computational cost. There is a library for arduino that implements this method, but if you want to learn more about that method or implement it by yourself look at this page.
  • The Complementary filter is a combination of two or more filters that combines the information from different sources and gets the best value you want. It can be implement in only one line of code .For more information visit this page.

angle = A * (angle + gyro * dt) + (1 – A) * accel;

where A is normally equals to 0.98.

First i tried to use Kalman filter but i don’t obtain good results, my angle was calculated with a little delay and it affect the control. The Kalman filter has three variables you can change based on the parameter of your sensor, and varying this you can obtain better result, i tried to change that values, but i don’t get better results so i decided to implement the complementary filter, so much easier and it have less computational cost. The complementary filter works fine for me.

Step 3: Chasis

For create the main structure of the robot i used the kit previously mentioned, this kit contains a simple plastic chassis with some nuts and screws, two wheels with two motors, one battery socket, one caster wheel, and even 2 little wheels for encoders. The last time i checked the price was 19€.

Tip: If you like to make your own chassis with wood, aluminium, or other materials and if you have old DC motors and wheels you can save some money.

This kit is prepared to make a 3 wheel vehicle but we gonna change the plans a little to adapt it to our project.

From the kit there are part i don’t use, like the caster wheel, two motor fastenings and some nuts and screws. I put the two motor in the lower part of the structure and closed it with the two grat plastic parts, keeping it together with the screws.

The electronics and the battery creates a tower in the upper part of the structure, i used Meccano to build the tower where the PCBs and the battery were located, but you can use other materials, like plastic, wood metal, or even only tape surrounding everywhere, like a gigant ball of tape.

And Voila! chassis done, not so difficult for the moment, let’s see next step.

Tip: When you build your robot you have to try to put the mass center the higher you can, putting heavy things in the upper part of the robot, like batteries. Remember that the more height of the center of mass the more stability the robot will have.

Step 4: Electronics

The electronics we are going to use in the project are simply three, an arduino UNO (you can use whatever arduino you have, doesn’t matter if isn’t arduino UNO), a motor driver, in this case a L298, and finally an IMU.

We use a commercial motor driver based on the chip L298, maybe much powerful that we need for these motors but i have it and it works fine. If you want to make you own DC motor driver you can use some transistors and make a H-bridge or use a L293, cheap and easy to use, there is an instructable where you can find information how to do it.

For the IMU i used the cheapest 10DOF (10 Degrees Of Freedom) i find, the chinese GY-80 with 3-axis accelerometer, 3-axis gyroscope, magnetometer, barometer and temperature sensors. We use only accelerometer and gyro so you can save money buying another IMU, like the MPU-6050, a 6DOF IMU for only 3.63€!!!!, or accelerometer and gyro for separate.

The IMU is connected to the arduino using I2C bus (If you want to lerarn more about I2C look this instructable), so we need 2 wires for communication (SDA and SCL) and 2 wires for power, it use 3.3V so we need 3.3V wire and GND.

The motor driver take power directly form the battery so don’t have to connect arduino’s power to it (i mean the 5V form the arduino), but we need 6 wires to control it, 3 for each motor, one for send the PWM signal for control the motor velocity, and for indicate the direction we want the motor to spin.

Tip: Try to position the IMU sensor (or accelerometer) in the line of the axis of the motors because if you locate the IMU far form this you can obtain much error in the accelerometer measure, remember that it measure linear acceleration, if you locate it to a distance R from the axis when the robot falls form vertical the acceleration of the accelerometer is the gravity plus R*dAngle/dt that means that it introduce an error in the measurement.

Step 5: Code

I’m not going to explain every single line of code for the project (i commented the code, if you download it i think u will have no problems to understand it), but i’m gonna show you how i organize it.

The code has 4 files: one the main code, a second one for the motors, the third is for the PID, and the last one is for the sensor code.

In the main code first i initialize the entire robot: pins, sensors, communications, … Then i calculate the error of the sensors. This part it’s very important because in this part we take the initial angle and we make it zero, it means that the sensor have an initial deviation, when we place the robot vertically the sensor don’t show that the angle is zero, instead send a deviation angle, this initial angle is used to subtract it from the posterior measurements of the sensors, to obtain the real angle. So when we initiate the robot we have to maintain it vertically until it starts to move the wheels.

The next part of code is the loop where we take the sensor values every 10 millisecond, that mean the frequency of sampling is 100Hz (you can use whatever frequency , but remember that very low and very high frequencies could not work), and we calculate the angle of the robot using, in this case, the complementary filter previously explained. We have the angle, now we can use that information to control our motors, this uses an intermediate PID, the simplest way to control things efficiently, there is an arduino library for the PID but is simple to implement it, you can code it in no more than 10-20 lines of code.

In order to use the accelerometer, in this case the ADXL345, we have to use its libraries. I used the next adafruit libraries: Adafruit_ADXL345 library and Adafruit_Sensor library.

And that’s all, simple code for simple robot, but it woks fine for me. You can implement so many more things if you want, like LCD screen, more sensors, better control, … That the magic of robots, you make one and improve it as much as you want.

Some of you have troubles using the code, i uploaded a single file with the entire project (Balacing_single_file).

Link to the google drive folder:

https://drive.google.com/folderview?id=0B7kBdG1oQk…

Step 6: Tests

I made a lot of test in the long way for this project, first i prove the motors: direction, velocity, … Then the sensors and the sensor fusion, that was a lot of time for find the right way to use it, i made a simple processing program (included in the code file) to show the values of the sensors graphically:

That help me to understand and to get the right form to take the real angle using Kalman or complementary filter.

At the end i prove the robot itself, the first prove was no as expected but it seems like we can achieve it

After some PID adjust and some code cleaning i reach the goal, maintain the robot vertically all the time, and even recover from pushing it with a little force. As you can see the robot walks with no control, drifting around without sense, but always vertical, that’s we wanted (for now).

Step 7: Improvements

There are some much improvements to do with this robot, this is the first step of many more:

  1. The first i want to implement is the position recovery, i don’t want my robot to walk around the room like a zombie although my cat like it, not me :D. For that we need encoders for measure the movement of the wheel and use it for bring the robot back to the initial position.
  2. Control the movement of the robot, forward, backward and turning , that is easy, the only thing we have to do is change the angle we want the robots stay then the gravity will do his work and the robot will move in the direction of the angle, then we put the angle to zero again and the robot stops. For turning we have to put some offset in the motor velocity, for turning right we subtract the offset to the velocity in right wheel and sum it to the velocity of the left wheel.
  3. Adding a WiFi shield to control it via internet.
  4. Implementation with Raspberry Pi to allow the robot to use a camera.
  5. Implementation of a camera and artificial vision for the robot.
  6. Use a ball instead of wheels, so hard, but we’ll try.

Balancing Robot

The Arduino SDK  https://www.arduino.cc/en/Main/Software has been updated since I last worked on this, and the latest version gave me this error when compiling.
This was due to where the libraries were.

libraries\MPU6050\xxxI2Cdev.cpp.o (symbol from plugin): In function `I2Cdev::I2Cdev()’:
(.text+0x0): multiple definition of `I2Cdev::I2Cdev()’

You won’t get an error if you put the header files in with the program file like so

But if like me you prefer to keep your libraries in the Arduino Libraries folder
and add them using the Include Library tab like this

Then
because the header file MPU6050_6Axis_MotionApps20.h already includes (#include”I2Cdev.h”)

#ifndef _MPU6050_6AXIS_MOTIONAPPS20_H_
#define _MPU6050_6AXIS_MOTIONAPPS20_H_
#include “I2Cdev.h”
#include “helper_3dmath.h”
// MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board
#define MPU6050_INCLUDE_DMP_MOTIONAPPS20

#include “MPU6050.h”

you may see the above error. The compiler looks for I2Cdev.h in the MPU6050 folder
but its not there! So copy I2Cdev.cpp & I2Cdev.h from I2Cdev Folder to
the MPU6050 Folder then add libraries….

Shopping list of objects you will need.
1.  Compatible Nano V3.0 – ATmega328

2.  L298N DC Stepper Motor Dual H Bridge

3. MPU-6050 3 Axis Gyroscope And Accelerometer
To calibrate your MPU-6050 watch this  https://www.youtube.com/watch?v=nlXqIe9-R7s  not a little helpful

4. SYB-170  Breadboard

5. Smart Car Robot Wheels

6.  10V 3900uf Capaictor

7. Ferrite core noise filters 25mm long 5mm core hole

You will need the I2Cdev & MPU6050  files available here  https://github.com/jrowberg/i2cdevlib/tree/master/Arduino
courtesy Jeff Rowberg

//   Most of this code is other peoples read below!!
// the bits I have hacked are indicated thusly//********************************************************************************************************************************************************
// MY STUFF

// i found PlyAlex  https://www.youtube.com/watch?v=nlXqIe9-R7s  not a little helpful
//
// if you get the   ERROR NO LIBRARY FOUND   edit sketch book location in File-> Preferences
// default location of sketch book is not where you think it is.

//   Most of this code is other peoples read below!!
// the bits I have hacked are indicated thusly

//********************************************************************************************************************************************************
// MY STUFF

// i found PlyAlex  https://www.youtube.com/watch?v=nlXqIe9-R7s  not a little helpful
//
// if you get the   ERROR NO LIBRARY FOUND   edit sketch book location in File-> Preferences
// default location of sketch book is not where you think it is.

//*********************************************************************************************************************************************************

// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
//
// Changelog:
//      2013-05-08 – added seamless Fastwire support
//                 – added note about gyro calibration
//      2012-06-21 – added note about Arduino 1.0.1 + Leonardo compatibility error
//      2012-06-20 – improved FIFO overflow handling and simplified read process
//      2012-06-19 – completely rearranged DMP initialization code and simplification
//      2012-06-13 – pull gyro and accel data from FIFO packet instead of reading directly
//      2012-06-09 – fix broken FIFO read sequence and change interrupt detection to RISING
//      2012-06-05 – add gravity-compensated initial reference frame acceleration output
//                 – add 3D math helper file to DMP6 example sketch
//                 – add Euler output and Yaw/Pitch/Roll output formats
//      2012-06-04 – remove accel offset clearing for better results (thanks Sungon Lee)
//      2012-06-01 – fixed gyro sensitivity to be 2000 deg/sec instead of 250
//      2012-05-30 – basic DMP initialization working

/* ============================================
I2Cdev device library code is placed under the MIT license
Copyright (c) 2012 Jeff Rowberg

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the “Software”), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
===============================================
*/

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include “I2Cdev.h”

#include “MPU6050_6Axis_MotionApps20.h”
//#include “MPU6050.h” // not necessary if using MotionApps include file

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include “Wire.h”
#endif

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <– use for AD0 high

/* =========================================================================
NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
depends on the MPU-6050’s INT pin being connected to the Arduino’s
external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
digital I/O pin 2.
* ========================================================================= */

/* =========================================================================
NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
when using Serial.write(buf, len). The Teapot output uses this method.
The solution requires a modification to the Arduino USBAPI.h file, which
is fortunately simple, but annoying. This will be fixed in the next IDE
release. For more info, see these links:

http://arduino.cc/forum/index.php/topic,109987.0.html
http://code.google.com/p/arduino/issues/detail?id=958
* ========================================================================= */

// uncomment “OUTPUT_READABLE_QUATERNION” if you want to see the actual
// quaternion components in a [w, x, y, z] format (not best for parsing
// on a remote host such as Processing or something though)
//#define OUTPUT_READABLE_QUATERNION

// uncomment “OUTPUT_READABLE_EULER” if you want to see Euler angles
// (in degrees) calculated from the quaternions coming from the FIFO.
// Note that Euler angles suffer from gimbal lock (for more info, see
// http://en.wikipedia.org/wiki/Gimbal_lock)
//#define OUTPUT_READABLE_EULER

// uncomment “OUTPUT_READABLE_YAWPITCHROLL” if you want to see the yaw/
// pitch/roll angles (in degrees) calculated from the quaternions coming
// from the FIFO. Note this also requires gravity vector calculations.
// Also note that yaw/pitch/roll angles suffer from gimbal lock (for
// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
#define OUTPUT_READABLE_YAWPITCHROLL

// uncomment “OUTPUT_READABLE_REALACCEL” if you want to see acceleration
// components with gravity removed. This acceleration reference frame is
// not compensated for orientation, so +X is always +X according to the
// sensor, just without the effects of gravity. If you want acceleration
// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
//#define OUTPUT_READABLE_REALACCEL

// uncomment “OUTPUT_READABLE_WORLDACCEL” if you want to see acceleration
// components with gravity removed and adjusted for the world frame of
// reference (yaw is relative to initial orientation, since no magnetometer
// is present in this case). Could be quite handy in some cases.
//#define OUTPUT_READABLE_WORLDACCEL

// uncomment “OUTPUT_TEAPOT” if you want output that matches the
// format used for the InvenSense teapot demo
//#define OUTPUT_TEAPOT

#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;

// MPU control/status vars
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

// orientation/motion vars
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

// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { ‘$’, 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, ‘\r’, ‘\n’ };
//*************************************************************************************************************************************************************************
//                                                 MY Variables

float OldP = 0;         // Previous value used to calculate change in P //  DELTA P
float P = 0;            //  Proportional component
float I = 0;            //  Integral        just the sum of P over time
float OldI = 0;         //  previous value of I for calculation of Delta I
float D = 0;            //  Differential       D = P – OldP
float bp = -60;         // balance point
float pwm = 0;          // value of Pulse Width Modulation  to ENA ENB
long a = 0;             // L298N to IN 1 to 4
long b = 0;             //
const int PinR1 = 5;    //  arduino  pin 5 to l298  pin IN4
const int PinR2 = 6;    //  arduino  pin 6 to l298  pin IN3
const int PinL1 = 7;    //  arduino  pin 7 to l298  pin IN1
const int PinL2 = 8;    //  arduino  pin 8 to l298  pin IN2
const int PwmR  = 9;    //  arduino  pin 9 to l298  pin ENB
const int PwmL  = 10;   //  arduino  pin 10 to l298  pin ENA
//****************************************************************************************************************************************************

// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}

// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================

void setup() {
//*********************************************************************************************************************************************************
// arduino to l298 pins hopefully self explanatory
pinMode(PinR1,OUTPUT);
pinMode(PinR2,OUTPUT);
pinMode(PinL1,OUTPUT);
pinMode(PinL2,OUTPUT);
//**********************************************************************************************************************************************************
// join I2C bus (I2Cdev library doesn’t do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif

// initialize serial communication
// (115200 chosen because it is required for Teapot Demo output, but it’s
// really up to you depending on your project)
Serial.begin(57600);
while (!Serial); // wait for Leonardo enumeration, others continue immediately

// NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
// Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
// the baud timing being too misaligned with processor ticks. You must use
// 38400 or slower in these cases, or use some kind of external separate
// crystal solution for the UART timer.

// initialize device
Serial.println(F(“Initializing I2C devices…”));
mpu.initialize();

// verify connection
Serial.println(F(“Testing device connections…”));
Serial.println(mpu.testConnection() ? F(“MPU6050 connection successful”) : F(“MPU6050 connection failed”));

//****************************************************************************************************************************************
//     Commented out so that program dose not wait for imput,but gets straight to work
/*
// wait for ready
Serial.println(F(“\nSend any character to begin DMP programming and demo: “));
while (Serial.available() && Serial.read()); // empty buffer
while (!Serial.available());                 // wait for data
while (Serial.available() && Serial.read()); // empty buffer again
*/
//*****************************************************************************************************************************************
// load and configure the DMP
Serial.println(F(“Initializing DMP…”));
devStatus = mpu.dmpInitialize();

// supply your own gyro offsets here, scaled for min sensitivity
//****************************************************************************************************************************************
//  use calibration program to get your own values
mpu.setXGyroOffset(44);//(220);
mpu.setYGyroOffset(-21);//(76);
mpu.setZGyroOffset(-30);//(-85);
mpu.setXAccelOffset(-1875);//(1788); // 1688 factory default for my test chip
mpu.setYAccelOffset(-1426);
mpu.setZAccelOffset(2215);
//****************************************************************************************************************************************
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// turn on the DMP, now that it’s ready
Serial.println(F(“Enabling DMP…”));
mpu.setDMPEnabled(true);

// enable Arduino interrupt detection
Serial.println(F(“Enabling interrupt detection (Arduino external interrupt 0)…”));
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();

// set our DMP Ready flag so the main loop() function knows it’s okay to use it
Serial.println(F(“DMP ready! Waiting for first interrupt…”));
dmpReady = true;

// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it’s going to break, usually the code will be 1)
Serial.print(F(“DMP Initialization failed (code “));
Serial.print(devStatus);
Serial.println(F(“)”));
}

// configure LED for output
pinMode(LED_PIN, OUTPUT);

}

// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================

void loop() {
// if programming failed, don’t try to do anything
if (!dmpReady) return;

// wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize) {
// other program behavior stuff here

}

// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();

// get current FIFO count
fifoCount = mpu.getFIFOCount();

// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
Serial.println(F(“FIFO overflow!”));

// otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (mpuIntStatus & 0x02) {
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);

// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;

#ifdef OUTPUT_READABLE_YAWPITCHROLL
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

//*********************************************************************************************************************************************************************
//                                                                      My Code
// PID control based on Pseudocode from https://en.wikipedia.org/wiki/PID_controller
// and the balance point idea from https://www.youtube.com/user/jmhrvy1947

OldP =P;                     // save value of P
P = (ypr[2] * 1000) + bp;    // update P from MPU add bp to correct for balance
OldI = I;                    // save old I
I = I + (P * 0.05) ;
I = I + ((I – OldI)*2  );     // calulate new I
if( I >  250 ) I =  250;           // LIMIT  Stop I building up too high
if( I < -250 ) I = -250;           // or too low value
D = P – OldP;                      //  D differential   change in P
pwm = ( P * 1 ) + ( I  ) + ( D * 10 ) ; // P I D

a = 0;
b = 0;
if(pwm < 0){
a = 0;
b = 1;
bp = bp – 0.01;
digitalWrite(13, 0);
}
if(pwm > 0){
a = 1;
b = 0;
bp = bp + 0.01;
digitalWrite(13, 1);
}
/////////////////////////////
// remove sign from PWM as – value has no meaning
pwm  = abs(pwm);
if ( pwm < 0) pwm = 0;
if ( pwm > 255) pwm = 255;

if(abs(ypr[2]) < abs(1.1)){
analogWrite(PwmR, pwm);
digitalWrite(PinR1, a);
digitalWrite(PinR2 ,b);

analogWrite(PwmL ,pwm);
digitalWrite(PinL1 ,a);
digitalWrite(PinL2 ,b);
}
else{
analogWrite(PwmR , 0);
analogWrite(PwmL , 0);
I = 0;
bp = -98;
delay(1000);
}

//********************************************************************************************************************************************************
#endif
}
}

Self-Balancing Two-Wheel Robot Is Controlled By Android

ANDROID APK

apk download link:

ROBOT  PARTS

-Plastic Tire Wheel with DC 3-6V Gear Motor for Robot 65*27MM   x2

-UNO R3 MEGA328P ATMEGA16U2 Development Board   x1
-Plastic Box For Robotic Body   x2

-Pololu TB6612FNG Dual Motor Driver
-MPU6050 6 Axis Acceleration and Gyro Sensor
-HC-05 Bluetooth Module
-Samsung ICR18650-22F 2200mAh(Green)Battery

Circuit  Diagram

ARDUINO CODES

arduino code download link:
denge_robot

Kod listesi

Franko – Arduino Self Balancing Robot

My latest Arduino DIY project is a self balancing robot.

Arduino Uno R3 microcontroller board is used for this project. MPU6050, an 6DOF IMU (with accelerometer and gyroscope) is used to get the angle and L298N motor controller board controls the 2 motors.

The motors are 12V 122rpm and the wheels are 8cm in diameter.

Robot size is 17x10x30 cm. Three plexiglas plates are used to hold the frame and parts. Standard PID controller is used to control the robot. To tune the PID controller parameters 3 potentiometers are used.

You may find the Arduino sketch and libs on my GitHub repo: https://github.com/lukagabric/Franko

Parts:
1 x Arduino Uno R3
1 x MPU6050 (IMU)
1 x L298N Motor Driver Controller
2 x 12V 122 RPM Gear Motors + 80mm Wheels
3 x 10k Potentiometer

Two-wheel self-balancing robot

After my first fast made self balancing robot, I want to better understand it and especially to try different control methods. For me it is necessary to simulate the whole stuff on computer (robot + control law).

The first step is therefore to model the robot with mathematical equations. Because of the complexity of the model I do make some assumption and simplification.

The model is separated into three part: the DC motor, the wheel and the inverted pendulum

The DC motor

electrical model:

motor

Va=Ldidt+VEMF+iRVa=Ldidt+VEMF+iR

wtih

VEMF=keωVEMF=keω

equation of motion for the motor:rotor

Irω˙=τmτaτfIrω˙=τm−τa−τf

with

τm=kmiτm=kmi
τf=kfωτf=kfω

If the motor inductance and friction torque are neglected, then

dwdt=kmkeIrRω+kmIrRVa1Irτadwdt=−kmkeIrRω+kmIrRVa−1Irτa

in state space form:

[θ˙ω˙]=[001kmkeIrR][θω]+[0kmIrR01Ir][θ˙ω˙]=[010−kmkeIrR][θω]+[00kmIrR−1Ir]

The inputs to the motor is the applied voltage and applied torque on the rotor.

The wheel

text4756-4

in horizontal (x direction):

Mwx¨=NfNMwx¨=Nf−N

rotation equation around the center of the wheel:

Iwθω¨=τmNfrIwθω¨=τm−Nfr

by using motor dynamic equations, one can get

Nf=Iwrωw˙kmkerRωw+kmrRVaNf=−Iwrωw˙−kmkerRωw+kmrRVa

with

ωw=θw˙ωw=θw˙

then

Mwx¨=Iwrωw˙kmkerRωw+kmrRVaNMwx¨=−Iwrωw˙−kmkerRωw+kmrRVa−N

Above equation is valid for both wheels.

With following equations, one can translate the angular rotate to linear motion:

rωw˙=rθw¨=x¨rωw˙=rθw¨=x¨
rωw=rθw˙=x˙rωw=rθw˙=x˙

then

Mwx¨=Iwr2x¨kmker2Rx˙+kmrRVaNMwx¨=−Iwr2x¨−kmker2Rx˙+kmrRVa−N

After rearrangement, one has:

(Mw+Iwr2)x¨=kmker2Rx˙+kmrRVaN(Mw+Iwr2)x¨=−kmker2Rx˙+kmrRVa−N

The inverted pendulum

pendulum

horizontal:

2N+mlθp˙2sinθp=mlθp¨cosθp+mx¨2N+mlθp˙2sinθp=mlθp¨cosθp+mx¨

The sum of forces perpendicular to the pendulum:

2Ncosθp2Psinθp+mgsinθp=mlθp¨+mx¨cosθp2Ncosθp−2Psinθp+mgsinθp=mlθp¨+mx¨cosθp

The sum of motions around the center of mass of pendulum:

2τm2lNcosθp+2lPsinθp=Ipθp¨−2τm−2lNcosθp+2lPsinθp=Ipθp¨

with

τm=kmkeRω+kmRVa=kmkeRx˙r+kmRVaτm=−kmkeRω+kmRVa=−kmkeRx˙r+kmRVa

one has:

2kmkeRx˙r2kmRVa2lNcosθp+2lPsinθp=Ipθp¨2kmkeRx˙r−2kmRVa−2lNcosθp+2lPsinθp=Ipθp¨

then

2kmkeRx˙r2kmRVa+mglsinθp=Ipθp¨+ml2θp¨+mlx¨cosθp2kmkeRx˙r−2kmRVa+mglsinθp=Ipθp¨+ml2θp¨+mlx¨cosθp

then

Ipθp¨2kmkeRx˙r+2kmRVa+ml2θp¨mglsinθp=mlx¨cosθpIpθp¨−2kmkeRx˙r+2kmRVa+ml2θp¨−mglsinθp=−mlx¨cosθp

and

2(Mw+Iwr2)x¨+mlθp¨cosθp+mx¨=2kmker2Rx˙+2kmrRVa+mlθp˙2sinθp2(Mw+Iwr2)x¨+mlθp¨cosθp+mx¨=−2kmker2Rx˙+2kmrRVa+mlθp˙2sinθp

after arrangement, one can get the following state space expression:

⎡⎣⎢⎢⎢⎢x˙x¨θp˙θp¨⎤⎦⎥⎥⎥⎥=⎡⎣⎢⎢⎢⎢⎢000012kmke(T+rml)r2Rα02kmke(ml+rS)r2Rα0m2gl2α0mglSα0010⎤⎦⎥⎥⎥⎥⎥⎡⎣⎢⎢⎢⎢xx˙θpθp˙⎤⎦⎥⎥⎥⎥+⎡⎣⎢⎢⎢⎢⎢02km(rml+T)rRα02km(ml+rS)rRα⎤⎦⎥⎥⎥⎥⎥Va[x˙x¨θp˙θp¨]=[01000−2kmke(T+rml)r2Rα−m2gl2α0000102kmke(ml+rS)r2RαmglSα0][xx˙θpθp˙]+[02km(rml+T)rRα0−2km(ml+rS)rRα]Va

with

S=2Mw+2Iwr2+mS=2Mw+2Iwr2+m
T=Ip+ml2T=Ip+ml2
α=STm2l2α=ST−m2l2

On my first two-wheel self-balancing robot, I didn’t use the encoder for speed measurement.
So it can basically stand still and balance its weight with small movements of its two
wheels simultaneously. I preferred that the robot would work very soon without much effort
and then I could improve it by adding speed control, RC, etc.

1.Hardware

It’s not difficult to gather all the parts. Here is my list:
1.glass fiber board
2.control board (arduino uno)
3.sensor board (MPU6050 gyroscope + accelerameter)
4.two-channel motor driver (L298N)
5.two geared DC motors (25GA370)
6.two toy wheels and fixation kit

Following is my assembly diagram:

assembly

Following is the wiring diagram:

wiring_SBR

In L298N, each motor is controlled by a full bridge circuit (or H-bridge) composed of two legs. Following figure is a simplified diagram coming from wikipedia. S1 and S2 are always controlled in complementary manner. When S1 closes, S2 opens. S1 and S2 close simultaneously is not allowed since that creates a short circuit of the power supply. Idem for S3 and S4.

H-bridge

When S1 and S4 close simultaneously, the motor rotates in one direction. When S3 and S2 close simultaneously, the motor rotates in the other direction. I use PWM control to set the motors’ speed. the switching frequency is about 1kHz which is the default value of Arduino module.

After the wiring, the robot looks like:

2.Control

Then comes the control part. The following figure shows the control diagram. Only the inner
loop in blue is implemented this time. I use it to regulate the tilt angle of the robot. As
I want the robot stands straight, I simply set the reference angle (REF_ANGLE) to 0. The
gyro+acc sensor gives the information needed to calculate the actual tilt angle. The error
between the reference and the measurement is used by the PID controller to give the command
to the robot (the two wheel motors).

loop

2.1 PID controller

The PID controller is a complexe topic. With google you can find tons of material about it.
Fortunately it is relatively simple to implement it in a microcontroller. With some basic
understanding and tuning techniques, one can make it work very easily.

The general mathematical form of the PID controller is (extracted from wikipedia):

Here is a simple software loop that implements a PID algorithm (extracted from wikipedia):

previous_error = 0
integral = 0 
start:
  error = setpoint - measured_value
  integral = integral + error*dt
  derivative = (error - previous_error)/dt
  output = Kp*error + Ki*integral + Kd*derivative
  previous_error = error
  wait(dt)
  goto start

The purpose of the PID controller is to generate a suitable command to the system (the self
balancing robot in this case) so that the system is stable and reactive. There are some
creteria of both the static and dynamic performance. But they are out of the scope. The PID
controller uses three parameters to do the job, the proportional (Kp), integral (Ki),
derivative(Kd) parameter. The big Kp makes the tilt angle error small and the reaction fast. The
Ki makes the static angle error dispear. This is important since I want the robot stands
straight (tilt angle=0) all the time. I choose to put Kd=0 because Ki and Kp achieve
perfectly my desired performance. So in fact I use a PI controller. For the PI parameter
tuning, I follow Ziegler-Nichols method that you can find on wikipedia. Following table is
copied from wikipedia. It works well for me.

PID_ZN

2.2 Tilt angle measurement

A sensor combining a gyroscope and an accelerometer is used to measure the tilt angle. The sensor don’t give the angle directly. Some calculation is needed. I use MP6050 as sensor. You can check its datasheet for more information.

The accelerometer gives the acceleration (unit g) in three dimension. If the robot is perpendicular to the earth (z axis for example), you should have only Z axis which is not zero. If you have another axis not equaling zero besides Z axis in static state, it means the robot has a none zero tilt angle. And you can get easily the tilt angle based on these two axis acceleration. However using accelerometer to get the tilt angle in this way is valid only for static state since we suppose that only gravity acts on the robot. When the two wheels act a force on the robot, the accelerometer can measure the resulting acceleration as well! In consequence, the measures are very noised. The good point is it is accurate in long period of time (or in DC after filtering).

The gyroscope measures the angle speed (unit deg/s) in three dimension. In order to get the angle, we need to do the integration of it. It is accurate dynamically (in short period). But the integration action accumulates the error of measurement, which makes the result drift in long period of time.

I use a complementary filter to combine the two sensor measurements. It is simple and needs little microcontroller calculation. It works well for me (so far). One can implement kalman filter for optimal control. But the knowledge of the system model and more calculation capability are required. It needs definitely more effort. I use following complementary filter to get the tilt angle estimation:

3.Software

The sensor communicates to the microcontroller through I2C bus. By using the WIRE library of arduino, it is easy to read the data. I use analogWrite function to set PWM duty cycle for the motor driver L298N. The command (PID controller’s output) is a value between -255 and 255. Positive sign means the robot moves forward. 255 represents the max speed (100% duty cycle for the first half bridge). I use anti-windup code to prevent the command to be higher than 255 (or lower than -255) which is the limit of PWM register (8 bits) max value.

The calibration of the sensor should be done generally. I calibrated the gyroscope because I see big static offset. My accelerometer’s offset is not obvious. I use 10ms as sampling time.

The total code has only about 50 lines which I believe possible to be optimized. It’s simple, right? :)

4.Conclusion

The robot resists relatively well the disturbance from both sides. The stability and dynamic response are satisfactory. The next step is to install an encoder on each motor, to make the robot possible to move and to add a remote control. Following is a video of it standing still.

 

How to Build an Arduino Self-Balancing Robot

By Roland Pelayo

Ever wonder how Segways work? This tutorial will show you how to build an Arduino self-balancing robot that balances itself — just like a Segway!

How Does Balancing Work?

To keep the robot balanced, the motors must counteract the fall of the robot. This action requires a feedback and a correcting element. The feedback element is the MPU6050 gyroscope + accelerometer, which gives both acceleration and rotation in all three axis (MPU6050 I2C basics) which is used by the Arduino to know the current orientation of the robot. The correcting element is the motor and wheel combination.

Required Materials

  • Arduino (UNO or Nano can be used. I used Nano for this project.)
  • MPU6050 gyro+accelerometer breakout board.
  • L298N driver module
  • 2 x Geared DC Motor+wheels
  • Three platforms (PCB, acrylic, or thick plastic)
  • Posts to hold the platforms
  • Jumper wires
  • Headers
  • Battery Pack

 

Connection Diagram

Complete Fritzing diagram

Connect the MPU6050 to the Arduino first and test the connection using the codes in this IMU interfacing tutorial.  If data is now displayed on the serial monitor, you’re good to go! Proceed to connect the rest of the components as shown above.

The L298N module can provide the +5V needed by the Arduino as long as its input voltage is +7 V or greater.  However, I chose to have separate power sources for the motor and the circuit for isolation. Note that if you are planning to use a supply voltage of more than +12V for the L298N module, you need to remove the jumper just above the +12V input.

Building the Robot

Robot frame (made mostly of acrylic slab) with two geared dc motors

Main circuit board consisting of an Arduino Nano and MPU6050

L298N motor driver module

Geared DC motor with wheel

 

The self-balancing robot is essentially an inverted pendulum. It can be balanced better if the center of mass is higher relative to the wheel axles. A higher center of mass means a higher mass moment of inertia which corresponds to lower angular acceleration (slower fall). This is why I’ve placed the battery pack on top. The height of the robot, however, was chosen based on the availability of materials.

Completed self-balancing robot. At the top are six Ni-Cd batteries for powering the circuit board. In between the motors is a 9V battery for the motor driver.

 

More Self-balancing Theories

In control theory, keeping some variable (in this case, the position of the robot) steady needs a special controller called a PID: P for proportional, I for integral, and D for derivative. Each of these parameters has “gains” normally called Kp, Ki, and Kd.

PID provides correction between the desired value (or input) and the actual value (or output). The difference between the input and the output is called “error”. The PID controller reduces the error to the smallest value possible by continually adjusting the output. In our Arduino self-balancing robot, the input (which is the desired tilt, in degrees) is set by software. The MPU6050 reads the current tilt of the robot and feeds it to the PID algorithm which performs calculations to control the motor and keep the robot in the upright position.

PID requires that the gains Kp, Ki, and Kd values be “tuned” to optimal values. Engineers use software like MATLAB to compute these values automatically. Unfortunately, we can’t use MATLAB in our case because it would further complicate the project. We will tune the PID values manually instead. I’ve outlined the steps on how to do this:

    1. Make Kp, Ki, and Kd equal to zero.
    2. Adjust Kp. Too little Kp will make the robot fall over (not enough correction). Too much Kp will make the robot go back and forth wildly. A good enough Kp will make the robot slightly go back and forth (or oscillate a little).
    3. Once the Kp is set, adjust Kd. A good Kd value will lessen the oscillations until the robot is almost steady.  Also, the right amount of Kd will keep the robot standing even if pushed.
    4. Lastly, set the Ki. The robot will oscillate when turned on even if the Kp and Kd are set but will stabilize in time. The correct Ki value will shorten the time it takes for the robot to stabilize.

 

Arduino Self-balancing Robot Code

I needed four external libraries to make this Arduino self-balancing robot work. The PID library makes it easy to calculate the P, I, and D values. The LMotorController library is used for driving the two motors with the L298N module. The I2Cdev library and MPU6050_6_Axis_MotionApps20 library are for reading data from the MPU6050. You can download the code including the libraries in this repository.

#include <PID_v1.h>
#include <LMotorController.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
 #include "Wire.h"
#endif

#define MIN_ABS_SPEED 20

MPU6050 mpu;

// MPU control/status vars
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

// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorFloat gravity; // [x, y, z] gravity vector
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector

//PID
double originalSetpoint = 173;
double setpoint = originalSetpoint;
double movingAngleOffset = 0.1;
double input, output;

//adjust these values to fit your own design
double Kp = 50;   
double Kd = 1.4;
double Ki = 60;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

double motorSpeedFactorLeft = 0.6;
double motorSpeedFactorRight = 0.5;
//MOTOR CONTROLLER
int ENA = 5;
int IN1 = 6;
int IN2 = 7;
int IN3 = 8;
int IN4 = 9;
int ENB = 10;
LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, motorSpeedFactorLeft, motorSpeedFactorRight);

volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady()
{
 mpuInterrupt = true;
}


void setup()
{
 // join I2C bus (I2Cdev library doesn't do this automatically)
 #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
 Wire.begin();
 TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
 #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
 Fastwire::setup(400, true);
 #endif

 mpu.initialize();

 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

 // make sure it worked (returns 0 if so)
 if (devStatus == 0)
 {
 // turn on the DMP, now that it's ready
 mpu.setDMPEnabled(true);

 // enable Arduino interrupt detection
 attachInterrupt(0, dmpDataReady, RISING);
 mpuIntStatus = mpu.getIntStatus();

 // set our DMP Ready flag so the main loop() function knows it's okay to use it
 dmpReady = true;

 // get expected DMP packet size for later comparison
 packetSize = mpu.dmpGetFIFOPacketSize();
 
 //setup PID
 pid.SetMode(AUTOMATIC);
 pid.SetSampleTime(10);
 pid.SetOutputLimits(-255, 255); 
 }
 else
 {
 // ERROR!
 // 1 = initial memory load failed
 // 2 = DMP configuration updates failed
 // (if it's going to break, usually the code will be 1)
 Serial.print(F("DMP Initialization failed (code "));
 Serial.print(devStatus);
 Serial.println(F(")"));
 }
}


void loop()
{
 // if programming failed, don't try to do anything
 if (!dmpReady) return;

 // wait for MPU interrupt or extra packet(s) available
 while (!mpuInterrupt && fifoCount < packetSize)
 {
 //no mpu data - performing PID calculations and output to motors 
 pid.Compute();
 motorController.move(output, MIN_ABS_SPEED);
 
 }

 // reset interrupt flag and get INT_STATUS byte
 mpuInterrupt = false;
 mpuIntStatus = mpu.getIntStatus();

 // get current FIFO count
 fifoCount = mpu.getFIFOCount();

 // check for overflow (this should never happen unless our code is too inefficient)
 if ((mpuIntStatus & 0x10) || fifoCount == 1024)
 {
 // reset so we can continue cleanly
 mpu.resetFIFO();
 Serial.println(F("FIFO overflow!"));

 // otherwise, check for DMP data ready interrupt (this should happen frequently)
 }
 else if (mpuIntStatus & 0x02)
 {
 // wait for correct available data length, should be a VERY short wait
 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

 // read a packet from FIFO
 mpu.getFIFOBytes(fifoBuffer, packetSize);
 
 // track FIFO count here in case there is > 1 packet available
 // (this lets us immediately read more without waiting for an interrupt)
 fifoCount -= packetSize;

 mpu.dmpGetQuaternion(&q, fifoBuffer);
 mpu.dmpGetGravity(&gravity, &q);
 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
 input = ypr[1] * 180/M_PI + 180;
 }
}

My Kp, Ki, Kd values may or may not work you. If it doesn’t, then follow the steps I outlined above. Notice that the input tilt in my code is set to 173 degrees. You can change this value if you’d like but take note that this is the tilt angle to which the robot must be maintained.

Also, if your motors are too fast, you can adjust the motorSpeedFactorLeft and motorSpeedFactorRight values.

Video

Here’s the self-balancing robot in action!

What’s Next?

An improvement for this self-balancing robot would be controlling it remotely. Unfortunately, I can’t make this robot do that unless I make it ultra stable using dc motors with an encoder. An encoder would update the current speed of the motor which I can use to make the robot steadier via a second PID loop. I might do that next time so stay tuned!