In my first project Idea, I wanted to make a Shopbot assistant that would follow you around, so the you can have access to all the tools you need near you. I found out a lot of people are doing some variation of this idea. Well after so many months doing the Fabacademy, I wanted to really push myself to make a good final project.
I have always admired Self-balancing vehicles, they are so futuristic. I have been wanting to make one for quite some time. I got into this project mainly because I wanted to learn a bit more about gyroscopic stabilization and how to properly implement a PID loop to control a motion.
Electronics are not my strong suit, but I want to learn them. In todays Digital world being a mechanical engineer alone won't help. Hence I plan to confront my daemons and to hopefully gain control over them.
What I plan this project will be, is a learning experience. I want to learn how to build robots that can take data from sensors and process them using a control loop and implement a real-time correcting output. My learning objectives are numerous. i want this to be a starting point for some cool new projects i'm planning on doing in the future, like Gimbals for cameras, Monowheel bikes, Hoverboards, Segways etc.
I'll be experimenting with a lot of separate subsystems for my project, but I know the point is not to do tiny parts of a lot of systems, but make a complete system from start to finish. I would be happy if I could make a small working prototype.
Hence my aim would be to split my project into subsystems, so that i can work on finishing one module at a time. There is a lot of component for the system I'm building, I'll work as fast as I can to finish on time.
Lets get down to business.
A self-balancing robot is like an inverted pendulum, unlike a normal pendulum which oscillates back and forth when a force is applied. An inverted pendulum cannot balance it self. An example of this would be, trying to balance a broom stick on your fingers. You can't stay in one place, you have to move your finger back and forth. This is exactly what the vehicle is doing, its moving back and forth so that the center of gravity will always remain under the wheels.
To drive the motors we need some information on the state of the robot. We need to know the direction the robot is falling, how much the robot has tilted and the speed with which it is falling.
To get this information we will be using a combination of gyroscopic sensor and an accelerometer. The sensor I will be using is MPU6050. We combine all these inputs and generate a signal which drives the motors and keeps the robot balanced.
Measuring Angle of Inclination
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. The sensor communicated with the board using I2C protocol. The board comes with SDA, and SCL pin outs.
To get this data from the MPU6050.We use this library written by Jeff Rowberg to read the data from MPU6050.
You will also need to install the I2C development library
I first started by going through an example code and understand what all the functions are doing. The example code 'MPU6050_DMP6' is inside your library. The code is a bit long, It took me a while to understand most of it. It gives multiple outputs angles like QUATERNION, EULER Angles (Gimbal), YAW PITCH ROLL.
To measure the angle of inclination of the robot we need acceleration values along y and z-axes. The mpu.getAccelerationZ() function will retrieve the acceleration data from the corresponding registers. And 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, and negative sign for clockwise angles.
If you try moving the sensor back and forth, you can see that the angle reading changes, this is because of the horizontal component of acceleration interfering with the acceleration values of y and z-axes. Hence Accelerometer alone can't be used for reliable angle measurement.
To balance the robot, we don't just need the angle the robot is at, we also need to know the rate at which it is falling. The 3-axis gyroscope of MPU6050 measures angular rate (rotational velocity) along the three axes.
We read the gyro value about the X-axis, convert them to degrees/sec and then multiply it with the loop time to obtain the change in the angle.
This is simple physics, we have the angular velocity of the robot from the gyro sensor. We then multiply that value with the loop time to get the change in angle. Then we add that value into the previous angle to the get the current angle of the robot.
In order to do this we need to know the loop time. Hence we use millis() to tell us the time taken from the start of the program. Then loop time would be our,Current Time - Previous Time. Then we use the mpu.getRotationX() to get the angular velocity about X-axis.
Now our gyro holds the data in a 16bit register and we will be using the map() to map it to a range between (-250,250). Then GyroAngle is equal to the previous angle plus the GyroRate times the loop time in seconds.
When you keep your robot steady for a while, you will begin to notice that the angle is deviating slightly. This drift is an inherent problem with the gyro. Hence you cannot use the gyro Alone to measure the angle.
We can't use the Accelerometer or the Gyro alone to measure the angle. Hence we will use them together.
There are many ways in which we can combine the outputs of the gyro and the Accelerometer. This is where Digital filters come in. I've read through a couple and I found that the Complementary filter is the one best suited for our job.
We have two angle measurements from two different sensors. 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. 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
Lets look at the filters a bit more closely.
A low-Pass Filter only lets through long term changes, therby filtering out short term fluctuations. One way to do that is to force the changes to build up little by little in subsequent times through the program loop.
In code:
angle = (0.98)*angle + (0.02)*accX;
If, for example, the angle starts at zero and the accelerometer reading suddenly jumps to 10º, the angle estimate changes like this in subsequent iterations:
0.20º, 0.40º, 0.59º, 0.78º, 0.96º, 1.14º, 1.32º, 1.49º, 1.66º, 1.83º
If the sensor stays at 10º, the angle estimate will rise until it levels out at that value. The time it takes to reach the full value depends on both the filter constants (0.98 and 0.02 in the example) and the sample rate of the loop (dt).
High-Pass Filter is just the opposite of this. It allows short duration signals to pass, while filtering out signals that are steady over time.
Time constantThe time constant of a filter is the relative duration of signal it will act on. For a low-pass filter, signals much longer than the time constant pass through unaltered while signals shorter than the time constant are filtered out. The opposite is true for a high-pass filter. filter.
If this filter were running in a loop that executes 100 times per second, the time constant for both the low-pass and the high-pass filter would be 0.49sec
This defines where the boundary between trusting the gyroscope and trusting the accelerometer. For time periods shorter than half a second, the gyroscope integration takes precedence and the noisy horizontal accelerations are filtered out. For time periods longer than half a second, the accelerometer average is given more weighting than the gyroscope, which may have drifted by this time.
For testing purposes I use an arduino to make sure that my code works, but I plan on migrating to a new board base on the satshakit.
I made this board after my previous attempt at satshakit during my Inputs week. I have detailed all the problems I faced with it such as the lack of an ISP header and FTDI cable and Extra power headers. Hence in my second version I decided to add all that.
The original satshakit is very well routed so I'm using part of the routing and adding my extras on top of it. I used a couple of zero Ohm resistors to hop over some lines.
I've added the the ISP header and the FTDI header in the schematic and connected to all the corresponding pins.
Placing the components on the board. I didn't want to change the original layout hence I decided to move all the extra components to one side. This put me in a Dilemma, now the only way I can connect the traces is by hopping over the existing ones.
I added a some zero ohms resistors to help me hop over traces. Manually routing the board was hard. I spend a lot of time deciding where to place all the components and how to orient them so that I can connect them without having to hop over lines.
In the end I had to squeeze two traces inside some resistors to get the whole board routed. I had used a trace width of 12mil which made it possible. now my only concern was that if the traces would peel off while milling.
This is the final routed board. I added a few extra power headers so that I can power other boards connected to this board. Now I proceeded to mill the board.
Milling was done with a 1/64"bit with Modella using Fabmodules. The traces took around one hour to mill.
The traces came out good, none of the paths peeled off, there was some slight lift in one of the trace going to the MCU. But its was small and I can manage with it.
I ran the cut file with a 1/32" bit on Modella using fab modules.
Now comes the tricky part, soldering the components.
I started with the MCU, thankfully I had done it before hence I could solder it properly. The MCU is a little tricky, everyone has their own technique to solder it. Some apply soldering paste on the pads, place the MCU on top and heat it. What I do is a bit different.
I've detailed it before in my Electronics production week, but I'll brief it here. I first start by applying some solder on one of the pads, I then orient the MCU and then Place it on the pads so that it properly seated (check alignment on all the pads). Then I simple heat the pad with the solder on it and then the MCU is fixed. Then I go around soldering the rest of the legs and come back and properly solder the first one. This has worked well for me so far, I'll be using it until I find a better technique.
In a similar way I soldered the rest of the components to the board.
The board looks awesome, but I have to test it to make sure its working.
Testing the Board took a lot longer than I wanted it to. I first started by connecting the ISP header to my PC using the programmer. The Power light lit up and I was happy. But I was unable to program my board. Every time it would say 'Initialization failed'. This happens because of improper connection between the board and the computer. Hence I double checked all the connections and made sure they were good. But still not working.
I went back to my eagle file and started going through my traces one by one to made sure I hadn't made a mistake in connecting them. They were all good. Then I again went back to my board and re-soldered all the connections just to make sure. Still not working.
Now I gave my board and my design file to my instructor and asked him to check if I had made any mistake. He checked the connections and said they were fine. Then we tried programming it with other setting like changing the clock, now the program burned successfully when the clock was set to 8Mhz. I don't know why. Then we changed the settings back to 20Mhz external and flashed it once again. This time also it programmed successfully.
But the problem was not finished, now the error comes up intermittently.
I finally found out that I could program the chip only when BOD was disabled (Brown-out Condition).
BOD is a safety feature that resets the Micro controller in case the input voltage dips below the nominal level for reliable operation. I guess there might be a drop in the voltage somewhere between the ISP header and the VCC of the MCU. I've tried everything to fix this including running jumper cables from the ISP header to the VCC of the MCU. The problem is now intermittent. One of my of my friend, Ganadev Prajapathy also faced a similar problem, where he could not get the bluetooth serial communicaition to work when he powered hid board from the laptop. I don't know if my board is reliable enough to be put on my robot. The trouble shooting is really eating into the time I have for the project. I'll try it and see what happens.
The data from the sensor is used to derive the angle and the PID will generate a PWM signal to drive the motors. But the MCU cannot drive the motors themselves, thats where the motor driver circuit comes into play.
The speed of a DC motor is given by the following equation
N = V - Ia Ra / kØ
Hence we can change the speed of a motor by changing its effective voltage at the Armature. And the way we change the voltage in a digital circuit is by turning it ON&OFF rapidly.
The duty cycle is the percentage of power needed and the length of the time the output is high, determines how much voltage is outputted. The pwm needs 2 variables to be able to work out the on and off time, these are the cycle length/time period and the duty cycle.
You can generate a simple PWM signal by turning the Output on and off with a slight delay in between. But in this way the duty cycle is very hard to control. You can alternatively use a Potentiometer to vary the duty cycle of the PWM signal. You can use the Analog Read function to read the value of voltage from the Potentiometer and then map it on to PWM signal.
v_out= map(POT_PIN, 0, 1024, 0, 255) //gets the pot value from 0-1024 and turns it into a value from 0-255
I want to get a good understanding of how to control a motor using PWM, hence I started from the basics, using a MOSFET as the motor driver.
MOSFET stands for metal oxide semiconductor field effect transistor. It is capable of voltage gain and signal power gain. They come under a class of devices called Field Effect transistors (FET). It has three terminals a source, a drain, and a Gate. The way it works is that there is a channel between the source and the drain, but this channel activates only when a positive or negative voltage is applied (depending on doping). In digital Electronics it can be used as a switch.
My plan is simple. Generate a PWM wave using a micro-controller and then give it to the gate of the MOSFET, which is connected between the motor and the Power supply.
My plan is simple. Generate a PWM wave using a micro-controller and then give it to the gate of the MOSFET, which is connected between the motor and the Power supply.
Since all we have in our lab are SMD components, I soldered some wires to the terminals of the MOSFET. Then using a bread board and an Arduino as MCU I began my experiment.
There were some bugs with my setup. Initially when I connected the MOSFET to the power supply, leaving the gate open, and I turned on the power The motor started spinning. Now this might be some leakage current that might have gone through somehow. I spend some time trying to fix the issue and after rewiring it. The problem was gone. This time I had inverted the terminals of the MOSFET, which connects to the power supply.
I connected a potentiometer to the board so that I can control the PWM signal without changing the code. I used the Analog.Read() to read the value of the POT and then map it to the PWD signal.
As you can see the setup works, The motor changes speed depending on the PWM signal, but its a bit crude. I cannot make it reliable enough to be put on my project. The MOSFET needes acertain range of voltage so that it can operate well. I would also need to add some 10uF capacitors to prevent noise in the line.
Next I wanted to try the most common method, using a motor driver IC. The IC I have chosen is the L293D because it is cheap and easily available and had a simple circuitry. I had a Atmega board with an :293D attached, Hence I decided to test the PWM code on the board to see how the motors would respond to it.
The motors responded well, you can hear the high pitched whine of the motors when you stop them. But I noticed that there were some values near Zero during which the motors would not move at all. The PWM value changes from 0 to 255 and back to 0. So somewhere near the Zero range there is no motion happening. This is called Motor Slack , the lowest range of PWM values which does not result in any motion. This will become important when we are planning to use PID to control the motors.
I liked the L293D IC, so I'll be making my motor driver board with it.
The idea is simple, I'll use my Main controller board to read data from the IMU, process thee data and then send out the PWM signals into the Motor driver. The Motor driver would then amplify those signals and send to the motor.
The schematic is really simple. The IC has two Input pins and two output pins and an Enable pin per motor. Its a dual H-bridge so it can drive two motors.
I've added extra LEDs on the output lines of the motors so that I would be able to visualize the signals going to the motors. This is unnecessary, but I did this so that it would help me during the debugging process.
Once the schematic was done, its time to arrange the board.
Arranging the board was a lot harder than I thought. There are lines which cross over other lines and the output lines to the motor have to be thick in order to withstand the current.
Finally I settled on an arrangement that looked neat. Then my next task began, Routing the board.
Routing the board was my biggest challenge. The traces going to the motor had to be thick enough to handle the current. I first started with a 40mil wide trace to route all the connections, carefully leaving enough space on both sides.
Then I began selectively increasing the trace widths using the wrench tool to about 100mils.
Then I manually drew the outside cut line so as to follow the contour. This would give a unique look to the board.
This is the final board fully routed ready for milling. I had given terminal points instead of Header pins because I wanted a semi-permanent method of connecting the wires. I could not rely on the simple press-fit connections of the Header. They might come loose anytime.
This is the trace file ready for milling.
This is the outside cut lines with the hole for drilling the terminals. The next job is to mill the board in Modella.
I started milling the board using a 1/64bit with Modella by generating the machine code using Fabmodules. But a strange thing happened. I noticed that the Machine was going quite fast than normal. It was cutting curves really fast. and before I could do anything. The bit snapped. The tip of the mill was gone.
I double checked all the settings in fabmodules, but all where in the default settings. I asked my instructed about what happened and he said sometimes this machine is known to behave abnormally. So I turned off the machine, shut-down the computer and turned everything on again. This time it worked normally.
The trace came out well now its time for the outer cut.
The outer cut was done with a 1/32"bit using default setting in Modella.
The final cut board ready for soldering.
Soldering was easier since some of the components where through hole, but the problem was that my board was a one sided board. Hence I could solder only on the top. This is fine for SMD components but tricky for some through hole ones. Hence I had to solder the terminal blocks upside down.
I tested the board with a simple fade program connecting the PWM signal to the Enable pin, and the LED lit up like a Christmas tree. The board works, I'm happy.
I'm making a small base to test the code and learn how to implement PID. With it I can iron out all the bugs in the code and then start making the bigger version of the robot.
Halfway through my testing phase I realized I will run out of time if I keep testing my code on a simple base. The problem is that how much ever work I do on the small test base, like setting the sensor offsets and PWM motor power, I will have to do all the work again on my actual Robot. So it doesn't make much sense to do it, I don't have the time. Hence I've shifted into making my actual robot body.
I've gone with a minimal design. I believe in simplicity and I also needed access into my boards, since I'm still in my testing phase. Hence I've opted of a simple 3-tiered design.
The bottom layer will have the motor and the Accelerometer, I'll tell you why the sensor is on the bottom later. Then on top of it I have my battery and Motor driver. And above that I've put my MCU board. I've left an another tier on top of the MCU, so that my Robot can be used to carry something on it head.
I started my design work in SolidWorks, The base will be a rectangular plate of 150x100 dimension and will have holes in it to allow the ribs to be connected.
From previous experience I know the laser kerf to be around 0.3mm. Hence I designed my holes to be smaller so that I can get a good Interference fit.
I designed the Ribs to be slightly less angular, I wanted some curves in my robot body. I didn't want it to be a perfectly rectangular box. This is the bottom connector with a length of about 50mm. This section will come in the bottom and the top.
This is the Mid connector, the lenght of it is about 100mm. This will become the middle tier, which will house the Motor driver, MCU Board. The image is my first version of the Rib, I have later changed it so that The holes are a bit farther apart. If I keep them too close, There is less material to support it. The final rib can be seen in the last image.
I've switched from SolidWorks to Rhino for the last part of the design, since what I plan to do can be easily done in Rhino. This is the Middle Plate. It has holes of two dimensions. One pair is kept farther apart so that the There will be enough material in between to support the structure.
This is the final laser cut ready file. Here you can see the modification I made to the Mid connector, I've expanded its teeth so as to fit the hole dimension. Once all the parts are ready, I joined the vectors, changed the print colour and started cutting in the laser cutter.
The wood is 4mm plywood and the laser settings where P : 100, S: 1.2, ppm : Auto The cuts came out beautifully. I've test cut a few pieces first to check the tolerance and the fit.
This is the bottom and the Top plate of the body.
This is the smaller rib which will come in the Bottom and the Top tiers. Here I've test cut some pieces to see that they sit well in the hole. They fit nice and snug.
Similarly this is the longer Ribs which come in the middle tier.
The parts are ready for assembly.
I Mounted the motor to the base plate using an Angle bracket and some screws.
I haven't made something to hold the sensor in place so for testing I used a piece of PVC foam board and some screws to securely hold the sensor.
Now the bottom tier is done and I went on to fit the rest of the body to see how everything fits together.
The joints are nice and tight. I sanded down a few of the ribs so that I can remove them easily, since I'll need access to the insides during testing.
For testing and debugging the code I'll be using the Arduino Uno board. So that I can narrow down the possible problems. And later once I've swept out all the bugs in the code, I'll switch to my Fabkit.
The IMU sensor which we are using needs to be calibrated so that we can remove the zero error . The zero error is when the sensor is kept perfectly level but thinks its slightly angled. You can calibrate the sensor either manually or using a script. The way to calibrate it is by giving offsets to counteract the inherent changes in the chip.
You can find this in code as
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788);
Each chip is different, hence you have to calibrate them individually.
Let's try manual first, You need to place the sensor on a perfectly horizontal table and run the Example code in the MPU6050.h library to get the raw data from the sensor. Leave it on for about 5-10 mins so that the temperature fluctuations does not affect it. The sensor will output the raw data in the serial monitor. Take the average of some of the readings for each term and give them as offsets - with the opposite sign - in the code and run it again. Do this till you get close to zero on all the offsets.
I did this for sometime and couldn't get satisfactory results as the offsets keep changing each time. Then I looked online and found a calibrating code which will do the same.
Resource for Calibrating the MPU6050What it does is that it takes the average of all the values of each terms and calculates the offset. Let the code run till it converges and give you the offset value you need to put into your code and run the same script again.
This may not work for everyone and it may not be perfect, but if you edit the script, you can print the offsets as they are being calculated. so even if the code does not converge you will get a ball park value of where you offsets should be.
For me after two attempts it successfully converged and gave me the offset values for my sensor. I ran the code again with the offsets and all the values where pretty close to zero.
At the heart of every balancing robot is a robust PID control loop. A proportional–integral–derivative controller (PID controller) is a control loop feedback mechanism. The controller continuously calculates an error value e(t) as the difference between a desired setpoint and a measured process variable and applies a correction based on proportional, integral, and derivative terms.
PID does three functions,
1. Handle the present, through the proportional term.
2. Recover from the past, using the integral term.
3. Anticipate the future, through the derivative term.
Lets take a look at what each term does.
The P-Term The P-term takes into account the present error and makes an effort in proportion to how far the error is from the setpoint. However as it approaches closer to the setpoint the error becomes too small to have an effect on the controller, hence there will always be a steady-state error, which appears as an offset from the setpoint.
However, a large value of the proportional term can cause the system to reach the setpoint, but it will make the system more unstable with oscillations and overshoots. Thus it behaves more like an ON/OFF controller. This is why P-controller alone is not sufficient. Therefore, it is usually accompanied by an I-term.
In a balancing Robot the error is the angle of the robot compared to the vertical axis.
The I-Term tries to balance the difference in time spend by the proportional value on both sides of the setpoint. For example, if the controller spends some time at 95% of the setpoint, the I-term will push the controller to 105% for the same amount of time. Thus there is an overshoot in the system. This will be a problem for sensitive systems, but PI controller is better due to its simplicity. It is important to make sure that the Integral Windup is taken care of. The integral windup refers to the saturated values of the integral term in either direction.
The PID controller cannot exceed certain limits, Hence the higher and the lower limits of the integral terms must be set in the program, such that beyond those limits the PID loop will saturate to its corresponding values.
In our balancing robot, the limiting value for PWM control of the motor is from 0 to 255. hence the output of the PID is constrained within the limits of -255 to 255 , negative for the opposite direction.
The D-Term Takes the derivate of the error of the system at every point. Thus, it tries to predict the future operation of the controller. The purpose of this term is to check how the output variable moves without overshooting the setpoint. The D-Term acts on the PI terms by counteracting them.
In case of our Balancing Robot, The error is the Angle and the derivate of that will give angular velocity. But we have a gyro sensor which gives us angular velocity directly. The gyro sensor responds faster than the PID loop can calculate, Hence we can skip the calculation of the derivate term and replace it with the GyroRate about X-axis.
Improving the Beginner’s PID (Highly Recommended)
Control Guru
PID for Dummies
Self balancing robot (Instructables)
Self balancing revisited (Instructables)
My code has gone through several Iterations. From getting a basic PID loop working to changing the PID constants through serial commands.
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "math.h"
#define EnableA 5 //PWM of Motor A
#define EnableB 6 //PWM of Motor B
#define MotorA1 4 //Direction Control of Motors A and B
#define MotorA2 3
#define MotorB1 8
#define MotorB2 9
#define Motor_Slack 45 //Compensate for the Dead zone in the PWM range
#define Kp 40 //proportional constant
#define Kd -2 //Derivative constant
#define Ki 10 //Integral constant
#define sampleTime 0.005 //sampling time 5 micro seconds
#define target_Angle -2 //The angle to keep the robot.
#define P_inc 1 // inc/dec value for PID tuning
#define D_inc 1 // inc/dec value for PID tuning
#define I_inc 1 // inc/dec value for PID tuning
#define Angle_inc .5 // inc/dec value for PID tuning
#define MS_inc 1
#define offbalance 30
#define PID_min -255
#define PID_max 255
#define Motor_min -100
#define Motor_max 100
#define runEvery(t) for (static long _lasttime;\
(uint16_t)((uint16_t)millis() - _lasttime) >= (t);\
_lasttime += (t))
uint32_t timer;
MPU6050 mpu;
int16_t accX, accZ, gyroY; //16bit integer
volatile int motorPower, gyroRate, pidout, kp, ki, kd;
volatile float accAngle, gyroAngle, currentAngle, prevAngle=0, error, prevError=0, errorSum=0;
volatile byte count=0;
float targetAngle, MotorSlack;
//Set left and right motors with the motor power obtained from PID.
void motorspeed(int MotorAspeed, int MotorBspeed) {
// Motor A control
if (MotorAspeed >= 0)
{
digitalWrite(MotorA1, HIGH);
digitalWrite(MotorA2, LOW);
}
else
{
digitalWrite(MotorA2, HIGH);
digitalWrite(MotorA1, LOW);
}
analogWrite(EnableA, abs(MotorAspeed));
// Motor B control
if (MotorBspeed >= 0)
{
digitalWrite(MotorB2, HIGH);
digitalWrite(MotorB1, LOW);
}
else
{
digitalWrite(MotorB1, HIGH);
digitalWrite(MotorB2, LOW);
}
analogWrite(EnableB, abs(MotorBspeed));
}
void angle()
{
accX = mpu.getAccelerationX();
accZ = mpu.getAccelerationZ();
gyroY = mpu.getRotationY();
accAngle = atan2(accX, accZ)*RAD_TO_DEG;
gyroRate = map(gyroY, -32768, 32767, -250, 250);
gyroAngle = (float)gyroRate*sampleTime;
currentAngle = 0.9934*(prevAngle + gyroAngle) + 0.0066*(accAngle); //Complimentary Filter
prevAngle = currentAngle;
}
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 PID()
{
error = currentAngle - targetAngle;
errorSum = errorSum + error;
errorSum = constrain(errorSum, -300, 300); // Prevent Integral windup
//calculate output from P, I and D values
// pidout = Kp*(error) + Ki*(errorSum)*sampleTime + Kd *gyroRate;
pidout = kp*(error) + ki*(errorSum)*sampleTime + kd *gyroRate;
pidout = constrain(pidout, PID_min, PID_max); //constrain the PWM output
motorPower = map(pidout, PID_min, PID_max, Motor_min, Motor_max);
//Check for off balance conditon
/* if(currentAngle > offbalance)
motorPower = 0; //turn off the motors
*/
// Compensate for DC motor "dead" zone
if (motorPower > 0) motorPower = motorPower + MotorSlack;
if (motorPower < 0) motorPower = motorPower - MotorSlack;
}
void BalancePID() {
Serial.print ("kp");
Serial.print (kp);
Serial.print ("ki");
Serial.print (ki);
Serial.print ("kd");
Serial.println (kd);
}
void Aerror() {
Serial.print ("Target Angle");
Serial.println (targetAngle);
}
void Mslack() {
Serial.print ("MotorSlack");
Serial.print (MotorSlack);
}
void ReceiveData ()
{
if (Serial.available())
{
char cmd = Serial.read();
// Implement BT Commands
switch (cmd) {
case 'B' : // Return PID control values
BalancePID();
break;
case 'P' : // Increment P multiplier
kp = kp + P_inc;
BalancePID();
break;
case 'p' : // Decriment P multiplier
kp = kp - P_inc;
BalancePID();
break;
case 'I' : // Increment I multiplier
ki = ki + I_inc;
BalancePID();
break;
case 'i' : // Decriment I multiplier
ki = ki - I_inc;
BalancePID();
break;
case 'D' : // Increment D multiplier
kd = kd + D_inc;
BalancePID();
break;
case 'd' : // Decriment D multiplier
kd = kd - D_inc;
BalancePID();
break;
case 'A' : // Return Roll Error value
Aerror();
break;
case 'S' : // Return Motor Slack value
Mslack();
break;
case 'a' : // Increment roll error
targetAngle = targetAngle + Angle_inc;
Aerror();
break;
case 'b' : // Decrement roll error
targetAngle = targetAngle - Angle_inc;
Aerror();
break;
case 's' : // Increment motor slack
MotorSlack = MotorSlack + MS_inc;
Mslack();
break;
case 't' : // Increment motor slack
if (MotorSlack >= 1) MotorSlack = MotorSlack - MS_inc;
Mslack();
break;
default :
Serial.println (cmd);
}
cmd = ' ' ; // clear BT receive string
}
}
void setup() {
Serial.begin(9600);
// set the motor control and PWM pins to output mode
pinMode(EnableA, OUTPUT);
pinMode(EnableB, OUTPUT);
pinMode(MotorA1, OUTPUT);
pinMode(MotorA2, OUTPUT);
pinMode(MotorB1, OUTPUT);
pinMode(MotorB2, OUTPUT);
digitalWrite(EnableA, HIGH);
digitalWrite(EnableB, HIGH);
digitalWrite(MotorA1, HIGH);
digitalWrite(MotorA2, HIGH);
digitalWrite(MotorB1, HIGH);
digitalWrite(MotorB2, HIGH);
// set the status LED to output mode
pinMode(13, OUTPUT);
// initialize the MPU6050 and set offset values
mpu.initialize();
mpu.setXAccelOffset(-3638);
mpu.setYAccelOffset(-667);
mpu.setZAccelOffset(591);
mpu.setXGyroOffset(217);
mpu.setYGyroOffset(18);
mpu.setZGyroOffset(-2);
init_PID(); // initialize PID sampling loop
kp=Kp;
ki=Ki;
kd=Kd;
targetAngle = target_Angle ;
MotorSlack = Motor_Slack;
}
void loop() {
runEvery(5){
angle();
}
ReceiveData();
motorspeed(motorPower, motorPower);
}
ISR (TIMER1_COMPA_vect)
{
PID();
count++;
if(count == 200) {
count = 0;
digitalWrite(13, !digitalRead(13));
}
}
This is the code I wrote after a lot of Iterations, and it also has the possibility of tuning the PID constants through Serial commands. You can tune it either through the Serial Monitor or via bluetooth.
I will add all the codes and the design files as a package the end of this page.
Till now I've been working on all the subsystems for the robot, now its time to bring it together. The synergy between all the subsystems is what keeps the robot balanced.
Most resources I've read suggests an intuition led trial and error method for tuning the parameters. So I decided I would give it a try and tune the parameters of the robot with my intuition. How hard could it be? all I have to do is change the values of 3 parameters till the robot balances.
Well, I was wrong. This was a lot harder than I imagined. I've gone through hours of reading many tutorials and putting all the code into practice. The problem is that there is no set standard for what PID values a system might need, It varies from one system to the other. I've spend days on getting it to work.
A common method in tuning a PID loop is the the Zeigler-Nichols method. A simple adaptation of which goes like this.
1.Start with a low proportional and no integral or derivative.
2.Double the proportional until it begins to oscillate, then halve it.
3.Implement a small integral.
4.Double the integral until it starts oscillating, then halve it.
This will get the constants close to where they need to be for fine tuning.
With this information I began my tuning attempts.
The first thing to do was to set up a test rig so that I can test the code on the robot safely. My test rig is simple, a scrap piece of wood fixed to the side of the bench, with a string hanging from the top.
I put a loop through the robot and hung it on the cable.
With that my test rig is done. Time to sit down and tune the PID controller.
Honestly, what happened in the next six days is a blur. I've tried many possible combination of values, I've followed the tuning method by increasing the P-value till the robot oscillates, then increase the I-term till the oscillations stop. Well, they didn't. No matter what value of the PID terms I give the robot kept going back and forth.
There where a lot of problems which I did not understand in the beginning. Which only became apparent after I plotted the values. Problems like,
There was a lot of delay in the response of the robot to the change in angle. With a slight change the robot would not move at all but once it leans to one side it give full power back and then full power in reverse. So in effect its like a forced oscillation.
And also when I changed the angle quickly the robot first went back in the opposite direction before going in the correct direction, the effect of which was the robot would fall over on one side and try to get up and fall over on the other side.
The robot seems to be running full power in one direction and then switch to the other direction. Far from where I want it to be.
What follows is my weeks of attempt at trying to tune the PID controller manually by changing the various constants.
The only term that had an effect that I could observe was the D-term. The D-term would resist any sudden change in motion. As you can see above, I though I was getting close but unfortunately I was not. Remember the problem I mentioned about the robot first moving in the opposite direction and then in the correct direction, well what is happening is that the robot is moving in the opposite direction first and the D-term is holding it back but this cause the robot to change the angle in the other direction and then again the wheels goes in the opposite direction and the D-term holds it back. This creates a forced oscillation that locks up the wheels in both direction.
I had not realized this for a long time and I kept playing with the values thinking I was getting close. Well thankfully I figured it out later when I plotted the data.
After a lot of time spend observing the robot and adjusting the PID parameters, my instructors told me to plot the output so that I can see what is happening.
I began searching online for a tool to help me plot the data, and finally found a feature in Arduino called Serial Plotter Its an awesome feature and I can do it without much change to the code.
I began experimenting with the serial plotter feature to see what kind of output I'm getting. I've uploaded some of the graphs I got, I'll try and explain what is is happening in them
This is the motor output(blue) vs the angle(red), you can see that there is some lag between them, that might be why my robot seems to run back and forth trying to catch up to the angle.
This is the output vs the angle with a very large differential term, you can see how quickly the motor power moves back and forth. Here the robot was just shaking back and forth.
In this plot you can see the large oscillations that happen when I place the robot on the floor. The motor power is a bigger version of the angle.
Before I talked about Integral Windup It is the saturation of PID values because of a large error build up. I also talked about why you need to constrain your PID output. In this graph you can see the effect of not constraining the PID values.
I've constrained the motor output(orange) within the range of -150 to 150 and I have not limited the PID values(green). You can see the PID rises to levels which is not possible for the system. The controller thinks that the system has infinite potential, and will try to reach that point, but depending on the system, you will have to set limits for the PID to operate.
In the derivate term of the PID we calculate the derivative by measuring the change in angle divided by time. But for out Balancing robot there is a better way to do this. We have a gyro sensor which can give as the rate of change of the angle directly. So all we have to do is replace the derivate term with the gyro rate about that axis.
The gyro output is faster and more responsive and we can avoid the calculation of the derivative term. A win-win situation.
In the above graph I've plotted the value of derivative term(orange) vs the gyro output(green) . You can see that there is a slight delay between the two. Hence using the gyro term can make your robot more responsive to sudden changes.
Next I will talk about a problem thats been bugging me for quite a while and I only saw it after plotting the values.
In this figure, Proportional output (green) is tracing the angle (orange). What i'm doing in this experiment is rocking the robot back and forth. and seeing the angle vs output. You can see that the angle dips a bit before changing direction and the motor output is tracing that dip.
This dip/rise is present whenever the robot changes direction. And the faster I turn the robot the bigger the dip. This is problematic because as the Robot lean to one side the angle first goes back a bit before going in the right direction, and the motor will follow it. So what happens is that once the robot tilts the motor will act to increase the tilt and then it becomes a forced oscillation.
Here you can see the dip before the sensor changes direction and the motor tracing the path.
A closer look at the Angle term, the faster I change the angle the bigger the dip/rise.
I've solved this problem partially by changing the constants in the complimentary filter for combining the angle from the gyro and the Accelerometer.
The way I reduced the deflection is by increasing the amount the accelerometer reading can affect the angle. I can't keep on increasing thee term as the more I do this, the more horizontal/vertical acceleration that can pass through the filter. The angle is a bit choppy now but the the deviation has reduced a bit.
Here you can see that the angle responds faster to change and there is less of a deviation. But the angle is a bit choppy than before due to the accelerometer reading.
Earlier I talked about a term called Motor Slack It is used to account for the lower end of the PWM values which cannot produce an effect on the motors. Here you can see it in effect. I've given very large values so that it is more defined on the graph.
For finding the motor slack, you have to look at the minimum value of PWM signal that will generate a response from your motor.
Here I have adjusted the motor slack after experimenting with the PWM values. Mine was around 50. The way you give motor slack in code is
if (motorPower > 0) motorPower = motorPower + MotorSlack;
if (motorPower < 0) motorPower = motorPower - MotorSlack;
What it is doing is skipping over the PWM value which cannot generate any response from the motor. So when the error is near zero, The output goes from -MotorSlack to +MotorSlack.
Next thing which I tried is to Invert the value of the D-term, I don't know if this is proper PID procedure but I wanted to find out what would happen.
What happened was that the D-term was fighting against the initial angle fluctuations. This is good. Now the robot has a quick response to any outside disturbing forces.
From the graph you can see that the D-term(yellow) acts quickly in the opposite direction of the change in angle. I hope that this will fix some of my problems.
This helped a lot, I was able to dampen the forced oscillations considerably. Now when I increase the D-term the robot would fight against the change in angle, but the problem is the the more I increase the D-term the less the robot moves and the more vibrations that occur.
so with considerable amount of Derivative constant the robot will stay in place and oscillate back and forth rapidly.
This is the graphical representation of the oscillations you can see in the video before.
In the graph the angle(blue), P-term(green), D-term(orange), and the I-term(violet). From the graph you can see that the the derivative term acts against the change in angle and the the proportional and the Integral is in phase with the change in angle.
This is a similar graph, here you can see that the vibrations occur most when the robot is near the zero angle, this is partially due to the fact that my DC geared motors have a lot of backlash(after all the testing I done).
I'm stopping my efforts to tune the PID controller here as I have less than a week for the final presentation. As Tony Stark said in IronMan 2, "I have tried every permutation of every combination of values, but it doesn't exist".
Its time to finish up on the rest of the work.
This is where it all comes together, you can see a lot of changes in some of the images and they might not be in order as I was rushing to fix problems and get things done.
I've switched form the Arduino to the modified Fabkit I made before. This created a whole new host of problems, When I first connected everything together and ran the example code for MPU6050, I could not get any response from the sensor.
I've spend some time checking the connections and verifying the code. The sensor talks to the board using I2C, I remembered that I could not get I2c to work with Satsha in my Networking week. I suspected that it might be the 20 Mhz resonator which I have put. The reason I had use it was because the crystal was not available in the FAB inventory.
I got a through hole crystal from the local shop and began by task of switching out the resonator. So now I have to fit two capacitors and a crystal in the pad of a resonator, Not an easy task.
I began aligning the 22pf capacitors and the soldered them very close to the each other, making sure that they don't touch. The next step was the hardest as I had to solder the crystal on top of them.
After many attempts I was successful. i was able to place a crystal and two capacitors in the place on resonator. I tested to see if the board will work and it does. What a relief.
After all this work, the next day, I showed it to my instructor and he told me that it was not necessary, I2C works with 20Mhz There might have been some other issue with my other board. Well whats done is done, I'm glad I didn't mess up the board.
Then when I tested the code with my board, I could not get any output from the serial port, I thought I had connected something wrong and then started checking all my connections, everything looks fine. I then tested the FTDI port with the ports in the Micro controller, They also tested fine. Me and my instructor sat down and started trouble shooting the board.
There was no output in the serial monitor, so we know that no data is being send. We spend a lot of time testing the connections and looking at other possible sources. After looking at the data sheet we realized that I had connected the Tx of the FTDI with the Tx of the MCU and the Rx to the Rx. I had not noticed it before.
I cut the traces and put some jumper cables to connect the Tx to the Rx and the problem was solved.
In my previous bot, the sensor was held in place using some foam board and some screws. This time I wanted to make a proper 3D printed case of the right height.
The height is important because the sensor should be place in the axis of rotation of the wheels so as to minimize the angular acceleration which occurs when the bot leans back and forth.
I designed the support in solidworks, I made two of them of varying height so that I could test which one works best, One at the height of the axis of rotation of heels and one above it.
I 3D printed the design by converting them to .stl and used Cura to generate the machine code.
After some brief trouble shooting wit=th the 3D printer(cleaning the Nozzle, and changing the material). The print came out beautifully.
I fixed the sensor on the support using some M2 screws and a rubber bushing.
Finally fixed the sensor on to the body. The support looks good. The oranges matches with the wood.
The next thing I had to do was to make a power board to power my bot. I neede two voltage outputs, a 5V for the board and 3.3V for the sensor. hence I began designing my own Power board.
The schematic is simple, it consists of two voltage regulators and some 10uF capacitors to filter out the output. I've connected LEDs to the Input and output lines so that it might help with debugging later.
I arranged all the components on the board and started routing them.
Finished routing the board. All connections look good and the board is compact.
Drew an outer cut line on the board so that it looks good. No more rectangular boards.
I milled the board on Modella using Fab modules and with the default settings. I then soldered all the components and tested to see if it was working.
It works. The lights lit up and I tested to how much output voltage I was getting. With a 10V input I was getting 5.4V and 3.4V at the output. I'm happy its working.
Now I proceeded with assembling the board. The first thing to do is to fix the Micro controller and the motor driver to the body.
I've made some modification to the body, I've given some holes so that all the wires now pass through the inside of the casing.
Similarly I've run wires through the 3-tiers of the body so that all the wiring will stay inside. This is the three tiers laid out flat.
I assembled the frame work of the body and connected all the wires. I powered the board with a battery and it turned on.
Now the frame work is done and its time to finish the rest of the body.
I stared designing a cover for the body in Rhino. I wanted the robot to have nice rounded edges, so I decided to do kerf bending. Why cut when we can bend, no more laser cut boxes.
The cover will have the kerf bending patter on all the corners. I calculated where the bending patterns should come and made the final cover.
I've included some Dovetail joints where the two ends meet. In the last two sections, I've reduced the length by over 10mm so that when I lock the two ends the kerf bend portion would act as a spring and keep the lock tight.
I've gone through a few iterations in the design of the outer cover after a few of them broke and finally was able to arrive at one that works well.
The cover fits nicely. The robot has a lovely finish to it now.
Fully assembled robot with all 3-tiers.
the back opens to reveal all the electronics inside. The spring idea worked out. I'm able to pull the joints together and the tension in the wood keeps it tight.
The Hero shot of my robot, which I used for the Poster. I've applied some Vinyl sticker to the sides to make it look cool. The design is same as the one I did in the Vinyl cutting week(week 3). I cut the Vinyl on Roland Desktop Vinyl cutter, using Fab modules.
I like the way the robot looks after applying the stickers. The red goes well with the wood.
I like the fact that I have easy access to the board and that I've given an ISP header to it, makes life a lot easier. I could burn the program easily but there are a lot of issues with my setup.
First of all I forgot to add a diode to my power board so as to stop any reverse current going into the voltage regulator. I noticed this when I pluged my ISp header and tried to program the board. The LEDs on the Power board lit up and I saw a small smoke from the 5V voltage regulator.
I then cut one of the traces and added a Schottky diode to the board. This solved my problem.
I added some hot glue on the Header pins and the Voltage regulators so that they would stay in place and be secure. They also serve as a warning when the regulator runs too hot, the glue melts, I can see this and turn it off before any damage is done.
Warning One thing which I did was, After burning the code and testing it, I connected the ISP header to programmer, while the board was still powered from the battery. This was a really dumb thing to do. Windows showed me waring saying that there is a power surge in one of my ports. I quickly removed the ISP programmer, but from then I lost two of my USB ports. They stopped responding completely. I had only 1 USB port left. I was devastated, it could have been much worse, I could have fried my Mother board.
I was trying frantically to find out what had gone wrong. But thankfully, I read about a feature in windows that shuts down any Port which draws more than 500mA of current. This saved me big time. My ports where not gone. I had to do a Hardware reset and Reboot my PC to get my ports back.
Always be cautious when there are multiple power sources on a single board
After all the trouble shooting, its time to power up the robot and see how it behaves. I have less than a day for the presentation. And I also need to make a video and a poster.
I did the Vinyl stickers on the last day as I was finishing up on the rest of the work and took the photo for the poster.
I made some final attempts to get the PID tuned. I was running out of time to make a proper video for it hence I made a quick one. The robot is almost Self-balancing. I held my hand over it and drove it like a Segway.
Here is the video with the cover removed. Its a heck lot of wires and LEDs, and dosen't make it look like a finished product.
We stayed up till 5 in the morning to finish up the video and the poster. I did the poster in Adobe Photoshop and the video in Windows Movie Maker.
Finally, the presentation day arrived and we presented our projects to Neil. He liked a lot of the other projects from Trivandrum. I told Neil about the problems I faced while tuning the PID controller. He said " PID controller tunning is, Messy." I totally agree with him on that one.
He suggested me to read about search and search for a controller instead of me tuning the PID. He said that PID assumes minimal computing, but with the power of computing in a system like this you can search for a controller.
The method he suggested was Nelder Mead Method Now I really wish I had paid more attention in school so that I could understand what he was talking about.
More about Nelder Mead Method
Mathematics have not been my strong suit but this method sounds awesome. After all the time I spend on tuning PID. This will be a welcome change.
I've started working on that method, but I don't know how far I'll get. Since I only have a week left to finish up on the documentation and pending works, I'll focus on that and update my progress as it happens.
So far It has been an awesome experience working on this project. I learned a lot about PID controllers and have an Idea on what I have to focus next. I hope I can keep building on the things I have learned through this journey and I'll keep sharing everything I have learned online.
rahulsrajan.com
rahulsrajan01@gmail.com
Trivandrum, Kerala ,
India.
Technopark, Kerala, India
