## TerraControl – with NodeMCU Webserver

UPDATE 3: Version 1.3

I’m working on making the code as user friendly as possible but it’s going to be a long run. I’d like to have separate html and css files but I’m still figuring it out. For now, few changes…:

• ! ! ! included mDNS protocol, so from now on you don’t need IP address to connect to your NodeMCU, simply put terracontrol.local in your browser and you are done (you have to be on the same network, of course)
• separate file for setting up the variables (setting.h). Unzip the file to your Projects folder, when you open the *.ino file, setting.h should be opened as well.

UPDATE 2: Version 1.2

• improved graph displaying range
• new values in graphs are moved to the end of array, not starting from the beginning again
• improved light setting – it is now unlimited (ON time can now be later than OFF time)
• code for manual defining your own server is in one place and commented by default (i.e. it is on automatic setting)
• clearer information in serial monitor
• unified function for min/max values in array
• new function for printing out minute values

UPDATE 1: Please see the version 1.1 I got the graphs and statistics working! Well, sort of…the range is still not as I want it to be, but at least now it is correctly displaying min and max. Plus new mouseover feature for the individual values in the graph.

After my first attempt to create controlled terrarium using Arduino board I got my hands on NodeMCU 12E board and I knew it was going to be a big step up!

I took me a few days before I began to understand how this board works (thanks to a lot of instructables here and google of course) and the possibilities it had. It think I’m on the right path to create exactly what I was dreaming about for several years…

So what is TerraControl v1.2 capable of?

1. 2 automatically controlled relays (light timer and heating)
2. 2 manually controlled relays (fan, second heating)
3. GMT time change
4. Simple graphs with highest/lowest temperature/humidity over the last 24 hours
5. Monitoring temperature and humidity in terrarium
6. All accessible and adjustable through webserver using HTML and CSS

What is necessary:

• given the nature of NodeMCU board (its output is only 3.3v) you will either have to buy 3.3V relay board, or modify 5v board, or buy I2C logic converter module – for example – $0.9 • 5V source (I’m using older usb charger) • wires • solder • case/box • Arduino IDE ## Step 1: Getting the Parts Together Connecting these parts is easy, just look at the source code and keep in mind that GPIO’s of the NodeMCU board is different from the actual boards description (as seen on the trird picture): //Define sensor pins #define SENSOR_IN 15 //D8 #define SENSOR_IN_Type DHT22 //Define Relay pins #define relayLight 5 //D1 #define relayHeat 4 //D2 #define relayFan 12 //D6 #define relayHeat2 14 //D5 i.e. DHT sensor pin goes to D8 (board’s D3, D4, D8 can’t be used as output but can be used as input), and the relay pins accordingly to the code. Remember, if you are using 5V relay, you need to modify the relay board or use I2C logic converter. ! ! ! IMPORTANT! When uploading the code to the board, you have to disconnect the DHT sensor, otherwise you will get an error when attempting to upload ! ! ! All parts can be powered with 5v power adapter ## Step 2: Setup and Customization Before we upload the code, there are few things that needs to be set up in setting.h: //You WiFi AP const char ssid[] = “SSID Name”; // insert your WiFi AP name const char pass[] = “password”; // insert your WiFi password // T E R R A R I U M S E T T I N G float tempMin = 24; // temperature in Celsius for switching the heating ON float tempMax = 30; // temperature in Celsius for switching the heating OFF int humMin = 50; // minimum humidity in % int humMax = 70; // maximum humidity in % // hour and minute for light to go ON int lightOn_hour = 7; int lightOn_min = 0; // hour and minute for light to go OFF int lightOff_hour = 20; int lightOff_min = 30; // Central European Time (1 for winter time) int timeZone = 2; Uncomment the following part of the code if you know how to define your server manually or just run the code and get addresses from the serial monitor. /*— UNCOMMENT THIS FOR MANUAL SETTING — IPAddress ip(192, 168, 0, 111); //Node static IP IPAddress gateway(192, 168, 0, 1); IPAddress subnet(255, 255, 255, 0); WiFi.config(ip, gateway, subnet); */ All done? Great, let’s move on… ## Step 3: Alwas ON/OFF Relay Connection One thing I wanted was the relay board to be used as little as possible. As you probably know, relays have two possible ways of connection: ON when not used and ON when used. So I connected the light and heating to “ON when not used” (heating is almost always ON and lights are ON for about 13-14 hours every day) and fan and heating 2 to “ON when used” (I barely need to use one of them). That is why the code for the same function is using different values: if (heatVal == 1) { client.println(“ON”); } else { client.println(“OFF”); } AND if (heat2Val == 1) { client.println(“OFF”); } else { client.println(“ON”); } You can of course modify the code according to your needs. Now just connect the DHT sensor and let’s look at the result! ## Step 4: Webserver When you open the webserver you will see simple page with all information about your terrarium and some features: • Light ON/OFF time can be adjusted (step are: 1 hour for hour setting and 5 min for minute setting). At the moment ON time has to be earlier that OFF time (ON 22:30 and OFF 0:30 will not work – yet) – fixed in version 1.2 • Temperature setting (steps are 0.5 degree Celsius) • Manualy turn ON/OFF other two relays – Fan and Heat 2 and adjust timezone when the time changes if needed, change your timezones in following part of the code: if (request.indexOf(“/TIMEZONE_SWITCH”) != -1) { if (timeZone == 1) { timeZone = 2; setSyncProvider(getNtpTime); } else { timeZone = 1; setSyncProvider(getNtpTime); } } • Webserver is using automatic time synchronization ## Step 5: Disclosure I know that the HTML and CSS code could be much more simple and the coding is not really user friendly but for the moment it works as it is supposed to (only the graphs are not very accurate but I’m still working on them) and I will get to these points when I start working on version number two. I have already decided to use external power supply (in this version I just stripped the 5v adapter and soldered it inside the box) and I also want the power cables to be more accessible and easier to connect/disconnect. I hope you guys (and your pets) will appreciate this instructable, if you do, please leave a short comment. And of course, suggestions are more than welcome! Thank you ## 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 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
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
/*
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)…”));
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…”));

// 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

// 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;

// 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
}
}

ANDROID APK

## 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

## ARDUINO CODES

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:

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

wtih

VEMF=keωVEMF=keω

equation of motion for the motor:

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

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

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

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:

Following is the wiring diagram:

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.

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).

#### 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):

$u(t)=K_pe(t)+K_i\int_{0}^{t}e(\tau)d\tau+K_{d}\frac{d&space;}{dt}e(t)$

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.

#### 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).

$\theta_{acc}(t)=tan^{-1}\frac{A_x(t)}{A_z(t)}=sin^{-1}\frac{A_x(t)}{g}$

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.

$\theta_{gyro}(t)=\int_{t_0}^{t_2}G(t)dt+\theta_{gyro}(t_0)$

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:

$0.98*\theta_{acc}(t)+0.02*\theta_{gyro}(t)$

### 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
• 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;

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
{
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
mpuIntStatus = mpu.getIntStatus();

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

// 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

// 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!

## Your Arduino Balancing Robot (YABR)

Your Arduino Balancing Robot (YABR) is a self-balancing robot that you can build yourself as a school project or as a fun project with your kids. It might look simple but there is a lot that you can learn from building this self-balancing robot.

In contrast to most self-balancing robots, this one uses stepper motors instead of regular DC motors. The main reason is that stepper motors are precise and have no performance loss when the battery voltage drops. One pulse is always an exact amount of motion. Regular DC motors can have mechanical friction and electric resistance differences. This can cause performance differences. As a result the robot will not move in a straight line.

The total cost to build this robot is approximately \$80 if you use the hardware list below. This includes a battery, Nunchuck, charger, stepper motors, etc.

Click to see the full image

If you encounter any problems during the build or setup please check the Q&A page first. Most questions are already answered in detail.

## Step 1 – Software

YABR.zip (version 1.1)

## Step 2 – Hardware

To build this robot you need hardware. I made the following list for convenience purpose alone. You are free to get your own hardware from different sources. But this is the hardware that I used/ordered:

If the 35mm stepper motors are out of stock. You could also use these stepper motors. Please note that these are 42mm in stead of 35mm. So you have to modify the frame.

Most of the following parts can be found in your local electronics store. But in case you don’t have an electronics store nearby I will put links here:

And finally you need an old inner tube and two sheets of plywood. I used 2.5mm and 12.5mm sheets.

## Step 3 – Tools

And of course you need some simple tools like a soldering iron, screwdrivers, a fretsaw, compact drill, etc.

## Step 4 – The build

Watch the YABR hardware build video and build the robot according to the video and the schematic that is included in the complete software package.

Detailed pictures of my own YABR balancing robot can be found in the media section of this project page.

### 4.1 The diode and resistors

The resistor R1 on the schematic is needed for uploading a program to the Arduino. The TXD output of the transceiver is forced high or low. As a result the FTDI programmer cannot change this output anymore and you will get an upload error. By adding this resistor the FTDI programmer can change the voltage on the RX-pin of the Arduino despite the state the transceiver output and the program is uploaded without any problems.

The other two resistors (R2 and R3) form a voltage divider. Meaning that the 12.6 volt of the battery minus the 0.6 volt voltage drop over the diode is divided by 2.5. Resulting in a 4.8 volt on the analog input when the battery fully charged. In the main program this analog input will be used to protect the battery. This is because lipo batteries can be damaged when the voltage drops below 3 volt per cell.

The diode D1 protects all the electronics against reversed polarity. So when you accidentally reverse the connections of the battery the components won’t go up in smoke.

### 4.2 The MPU-6050 gyro/accelerometer

The only gyro / accelerometer that is supported by the YABR software is the MPU-6050. This is because the self-level feature requires an accelerometer and a gyro as I explain in these two videos:

The orientation of the gyro is important. Make sure to mount the gyro in the exact same orientation as shown on this picture. Otherwise the YABR software cannot calculate the correct angle and the robot will not work.

### 4.3 Hardware test

Please note that you are soldering the wires on the back side of the PCB. The schematic is drawn facing the components from the front. So everything is mirrored and you need to double check all the connections before you connect any power to the PCB.

With everything in place it’s time to connect the FTDI programmer to the Arduino pro mini. Don’t connect the battery yet. If the LED’s don’t lit up there is a short circuit in the wiring and you need to disconnect the FTDI programmer as soon as possible. Normally the Arduino pro mini is already programmed with the blink sketch so the LED on the Arduino should start to flash.

To check if the gyro is connected correctly and to check the balancing point of the robot you need to upload the “hardware-check” program that you can find in the software package that you downloaded earlier.

Test the balancing angle of the robot and fix it in that position on a stand as I showed in the video. Upload the “hardware-check” program, open the serial monitor and set the baud rate to 9600.

After uploading the program, open the serial monitor and set the baud rate to 9600.

The program will check if there is any I2C device is connected and if this is a MPU-6050. If everything is working as expected the program will output several raw gyro values on the screen. These are just examples and your values may be different. Note down the balance value that you see in the serial output. You will need it later in the main program.

### 4.4 Limit the motor current

Next thing on the to do list is to set the stepper controllers to the correct drive current. If the motor current is set to high the stepper controllers will heat up and they might get damaged.

First set the potentiometer at the same position as shown in the picture below. Now always be careful when connecting the lipo battery for the first time. A short circuit can cause high currents, heat, sparks, and burns.

A good alternative is to use a small DC fuse like the one below. This fuse will blow if the wiring on the back side of the pcb holds a short circuit.

The easiest and safest way to set the correct current and to check if the wiring is correctly connected is by measuring the current in the power supply wires with a bench power supply. The power supply is limited to 500milli amps. This is an extra safety feature. If there is a short circuit the power supply will limit the current to 500 milli amps and the wiring will be fine.

First connect one motor. With the potentiometer it’s possible to set the current between 100 and 150mA. This is more than enough to get good performance. Do the same with the other stepper controller.

If you don’t have the possibility to measure the current just set the potentiometer in this position and feel if the stepper driver doesn’t get to hot. But it’s always best to measure the current.

### 3.5 The remote control

If you open the Nunchuck you can note the wire colors that are connected to the various pins as you can see on the schematic. The Nunchuck works on 3.3V and you can use the 3.3V output of the Arduino Uno to power the Nunchuck.

The 5V output can be used for the transceiver. Again, connect the wires as shown on the schematic to get it to work.

To check if the Nunchuck is connected correctly you need to upload the “hardware-check” program that you can find in the software package that you downloaded earlier. After uploading the program, open the serial monitor and set the baud rate to 9600.

The program will check if there is any I2C device is connected and if this is a Nunchuck. If everything is working as expected the program will output several raw joystick values on the screen.

## Step 5 – Upload the software

And finally you can upload the YABR-remote program to the Arduino Uno and the YABR-robot program to the Arduino pro mini.

The YABR-remote program does not need any modification. In the robot program you need to change the accelerometer calibration value to the value that was shown by the hardware test program when the robot was in it’s balancing position.

To start the robot you need to power up the remote. Lay the robot on it’s back and switch on the power. The LED will blink indicating that the gyro is calibrating. When the blinking stops you can slowly lift the robot and it will automatically start to balance itself.

And basically that’s it. You can now control the robot with the Nunchuck.