Self Balancing MSP430 Bot
Pi's balancing bot is a miniature self balancing roboter (bot). The bot is based on the model of an inverted pendulum. The inverse pendulum is a very popular demonstration and training object for control algorithms.
There were about three reasons to build this balancing bot:
- First was the need to develop something to participate in Texas Instrument's MSP430 Design Contest 2006. I did a lot of different projects with this processor family and I really like this processor and so I want to contribute for all the fun I had with this chip. Especially for the ADC Two Block Data Transfer Mode I did't found any working example. Here is one!
- Second were all the following discussions if it is possible to get a inverse pendulum with just ONE gyroscope balanced. The answers ended up 50:50. So I really need to KNOW!
- Third: I thought it's going to be cool:-)
As I don't want to loose too much time and energy - and of course due to the lack of mechanic parts and tools - the mechanics should be as simple as possible.
Lucky me. I received a damaged mini car aka microsizer (this is a miniature radio controlled car) from a friend. The picture shows this nice toy before it got cannibalized (I think it's a kind of Honda ...). I just separated the complete tail of the car which included motor, gearbox, driveshaft and wheels.
Probably to most important thing is the mechanical decoupling between the drive system and the electronics (especially the gyroscope). The though vibrations of the drive systems can influence the micromechanics of the gyroscope. If the chassis/electronic has a very low mass and it is directly coupled with the drive system, the influence can get quite nasty. To achieve some damping of the vibration coupling I used some silicon tubes.
The rest of the chassis consists of PCB parts and a carbon rod. The PCB is required anyway. The carbon rod with the Batteries at it's end moves the center of gravity away from the rotation point. The bigger the distance between center of gravity and rotation point the easier it is to balance the system.
The bot is powered by two Lithium Polymer batteries (2x Kokam 145mAh 3.7V). Then there are two simple linear voltage regulators to generate 5V and 3.3V. The 5V are required for the gyroscope - unfortunately it doesn't work with 3.3V.
The ADXRS300 from Analog Devices is doing the gyroscopic work for the bot. A gyroscope measures the angular velocity (dPhi/dt). Since the ADXRS is inside a BGA package (which is really unsuited for hand-soldering) I used the available Evaluation board. The evaluation board holds the ADXRS300 itself and some required capacitors. Currently the ADXRS seems to be the best choice (more robust and accurate than normal piezo gyros, cost effective, ...). The operation amplifier between the ADXRS300 and the MSP430 is used to convert the different voltage levels (5V/3V3) and to refer the gyroscope output to it's internal reference generator.
Of course the processor is an MSP430F1232. The only external required peripherals are the ISP programming connector and the crystal. The crystal mainly is just required for the debug oututs via the UART. The Gyroscope is connected to the internal ADC and the motor controller to an internal GPIO/PWM module. That's all!
The motor is controlled with a FET H-bridge. The used FETs (IRLML 6401/2502) are perfectly suited for this application (low RDSon, ~ 4A, small SOT23). The control of th H-bridge is done by the MSP430. The signal with the PWM output of the MSP430 is fed to both path of the H-bridge via serial resistors. To select the direction one feeds can be disabled by pulling the signal to ground after the resistor. This methods is possible since the MSP430 is able to switch the control pin between an high impedance input and a low driven output. I like this idea. The high side FETs are driven by the opposite low side FET. This was the only simple method I found to handle the parasitic capacity of the FETs. The 5Ohm resistor was installed to avoid some pyrotechnical effects during development of the bot.
Read the gyroscope measurement, calculate the appropriate output and write it to the PWM module.
To implement the desired control algorithm the current angle and the current angular velocity of the bot need to be known. The Gyroscope measures the current angular speed - the actual angle can be calculated by integration of the angular velocity. Since even small errors or offsets in the angular velocity measurement are summarized by the integrator, the calculated angle is only valid for a short time period. Another problem is the drift of the gyroscopic sensor which can also influence the velocity measurement. To reduce the errors caused by the mentioned effects some simple algorithms are used. To get rid of drift problems the offset is continuously recalculated. Since the balancing bot will always swing around it's equilibrium state the long time average of the velocity measurement is equal to the real gyro offset. The bot will oscillate at a maximum period of about 5 seconds, so the average window needs to be at least 10 seconds long. Instead of a common windowed filter a very simple algorithm is used: each new measurement value is added to the filter pool and the average value of the filter pool is subtracted from the filter pool. Yes, sounds strange but in the source it's easier to understand. To get rid of integration errors a kind of washout algorithm is used. That means that the current calculated angle is tied a little bit to it's real position - since the real position is not know it's assumed as zero this is nearly true because the bot only can keep it's upright position when the angle is zero - this is basically the same assumption as for the offset algorithm.
The measurement itself is done with the internal ADC module and the Two Block Data Transfer Control of the MSP430. Especially the Automatic Two Block Transfer mode make the measurement really comfortable.
Simple. The control algorithm needs three inputs: angle, angular velocity and bot position. The angle and the angular velocity are necessary for balancing while the bot position is required to prevent the bot from running away. How to measure the bot position ? The position of the bot is equal to the integration of it's speed. To get a rough estimation of the position the PWM output values are integrated. Also here the integrated value is washed out with time. Of course this estimated position is more than unreliable, but it seems to be good enough for this purpose.
The control algorithm now look like this: (motor PWM) = (a1 * angle) + a2 * (angular velocity) + a3 * (bot position)
a1, a2 and a3 are evaluated by some basic experiments and playing around with the bot.
Even simpler. After applying a pre-correction value for some losses the calculated value directly fits in the PWM module. The PWM module is responsible for the speed and the two control pins for the direction.
For MSP430 projects I always use the great and free gnu toolchain for MSP430 (mspgcc). This toolchain is available at http://mspgcc.sourceforge.net/
The complete source of this project can be found here balancing_bot_source.zip
What should I say: It's working and it's cool. I could spend hours to play with the bot and even just watching it is amazing. A little demonstration of the bot can been seen in this little video. Of course it's cooler in real life :-)
- The control algorithm itself and it's parameters still leave enough room for big enhancements
- A smaller bot ?
- A bigger bot ? with separated left/right wheel and incremental encoders, radio controlled, ... need to stop!
All development credits go to Matthias Fend who contributed this design to the 2006 MSP430 Design Contest.