Hardware
The R2-Z2, self-balancing, two wheeled robot is a hybrid of open source and self-developed software. The current, base structure is modeled after the Balanduino:
http://www.balanduino.net/home
The chassis of the R2-Z2 is modelled after the Balanduino. This is due to the simplicity of obtaining the require parts. The R2-Z2 uses readily available electronic components from several vendors, such as SainSmart for the Uno and MPU-6050 module. Whereas, the Balanduino uses a specially produced printed circuit board (PCB), which holds the MCU, IMU, and motor driver.
IMU Filter
The Kalman filter used in this project comes directly from the open-source part of the Balanduino project. Both projects use the MPU-6050 inertial measurement unit (IMU) MEM's based integrated circuit (IC). The only modification I made was to the filter's output. The pitch values that come from their Kalman filter are a bit noisy to the right of the decimal point. Thus, a simple algorithm to smooth out the data was added. This is particularly important because the pitch value is multiplied by a constant in the PID feedback system, which amplifies this noise.
PID Controller
The PID controller is composed of three parts: (1) the proportional, (2) the integral, and (3) the derivative. All three parts are a function of the pitch value coming out of the Kalman filter. The pitch, or θ, is the angle the robot’s current vertical axis (attitude) makes with standing vertical axis. Thus, the speed setting is derived from this equation as follows:
Speed = Kp θ + Ki ʃ θ(t) dt + Kd dθ(t)/dt
where, Kp, Ki, & Kd are constants.
The derivative term is typically not used, because it causes the robot to have the jitters (like it had too much coffee!). Therefore, PID controller is really a PI controller.
Picking the correct values for Kp and Ki is quite tricky. The overall speed value needs to take into account for the strength of your motors, whether you are using PWM (value range) or, in my case, are writing a scaled value to the motor controllers’ PIC microcontroller. The best method is to first start with Ki = 0, and work on finding a value for Kp through trial and error, which gives you a response that is in the ball park of what you want (robot can almost stand on its own). With Ki = 0, however, the robot will oscillate back and forth at a growing distance. So, next you will work on finding a value for Ki that stops or greatly decreases this oscillation.
Communications
The R2-Z2 uses the I2C bus for the MPU-6050 gyro/accel module, and a software serial port (can use any digital I/O ports for this) to communicate to the MD25 motor controller module. The dedicated serial port is used to display debug data on the console, when in the debug mode.
More to come!
