bla repository

A movie as it balances can be seen on Youtube, or from this avi. The film was made just after the robot could hold it's position, so there's still plenty of things to tune but it's not very important. Mechanical part of the robot is a rubbish. It wobbles which makes it hard to estimate acceleration used for calculating an inclination, has only one encoder and a common wheel axis. Alsa it has the mass center located pretty high, but that makes the things interesting.

Controller board for this design is described here -- it uses ATmega644 µC. Currently the code reached 10kB mostly because of floating point arithmetic required for the Kalman filter (KF). That's only about 1.5k lines of code. Most of the time it was developed in the C language, then for some experiments I switched to C++ and it worked... and I liked gain made by a few C++ features (mostly by namespaces) and learnt I can control what ASM the compiler generates so I decided to stick to it (I had some minor problems like bug #36310) then when I omitted constructions like templates parametrized by templates everything worked just fine.

Program is splitted into few .cc files, but compiled as a whole. All source files (version from the movie, so working, but soon to be old) are here. They are all under GNU GPLv3+ license (KalmanSimple.cc under GPLv2+). The main program modules are:

• Global.cc - type definitions, LED signaling, delay functions, some rather not used functions...
• Config.cc - reads PID configuration from eeprom (faster than full-flashing everytime you change one parameter)
• Serial.cc - buffered, interrupt-driven serial debugging
• Motor.cc - controlling motors -- speed and direction
• RunAvg.cc - some templates of static low-pass filters
• Main.cc - initializes everything, calls periodically Kalman filter and steering/balancing PIDs

Next, there're few files to read data from sensors and calculate our inclination:

• Angle.cc - using ADC reads data alternately from accel and gyro. Does some low-pass filtering.
• Encoder.cc - reads encoder data, determines direction and keeps track of our position from the start of a program
• KalmanSimple.cc - Kalman filter based on the source from autopilot project

I'm able to do about 6000 measurements of acceleration and angular speed per second, but KF is pretty slow (it takes about 40800 cycles [0.00204s] to calculate single angle -- using both gyro and accel parts). That's why I call gyro updates only every 0.0028s (357 times per s) and every five such updates I additionally update angle using the accel measurement. So to do something with this 5k of pretty unnecessary accel/gyro measurements I just smooth measurement with a low-pass filter.

Because of this problems I'd really love to rewrite KF from scratch. Measure the gyro bias offline. Use simpler state matrix, then check if P matrix and kalman gain stabilize around some value (they should), when they do -- hard-code the values into the code. Then try to rewrite it using only integers in calculations.

Update: Kalman rewritten using simpler state matrix and no bias auto-tunning. Now it reads all gyro measurements (6000/s) and minimum of 357/s accel readings.

Power sent to motors in order to stabilize the robot is calculated as often as accelerometer part of KF but with a phase shift. First I call the Steer function -- a PID controller with wheel position and speed as input. It calculates the inclination we'd like to achieve to steer correctly, e.g. not to have too big speed, to keep our position etc. Then second PID takes the robot tilt from defined "destination angle" and calculates power for motors. That's it, pretty simple.

To be able to calibrate PID controllers I sent all data about measurements, PIDs input and output over a serial. The nice thing was a simple python script which received it and plotted in the realtime. For example the angle estimated by KF during balancing:

It wasn't very fast (it would be faster when done in OCaml though) but it was very straight-forward to write using pylab module.

During this project I've read a lot of texts about other inverted pendulum robots and they helped me a lot. If you have any questions regarding this design, used tools etc. I'll answer them with pleasure. ;)