3D Printed Humanoid Robot – Robonoid – LineUp – 20180621

[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

 

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.