TwoPotatoe Speed and Position
Understanding how TwoPotatoe determines its speed and position is central to the whole algorithm. The wheels are obviously not a good indicator of the speed and direction of the robot since at any given time, the wheels will be going faster or slower that the robot in order to keep it in balance. There is a point on the robot known as the Center of Oscillation (CO). If the robot is stable and upright and then the wheels start moving rapidly back and forth, there is a point about 5 inches above the wheels where the robot moves neither forward or backwards. This is the CO. It is located at about the point of the Norwegian flag.
We know the wheel speed, from the wheel encoders and we know the rate of change in the pitch (angle leaning forward or backward) from the gyroscope. We can compute the speed and position of the CO directly from this.
The speed of TwoPotatoe is controlled directly from the joystick on the hand controller. The top speed is about 12 fps (feet per second) so that if the joystick is advanced 50% toward its limit, the controller sends a message to TwoPotatoe to go 6.0 fps. TwoPotatoe receives this message goes through the following computation about 400 times a second:
- Find the Speed Error, which is the difference between the Target Speed, commanded from the controller, and CO speed calculated as described above.
- Convert the Speed Error to a Target Pitch Angle. This is the angle we want TwoPotatoe to be at so that it will accelerate to the Target Speed.
- Compute the Pitch Error, which is the difference between where TwoPotatoe currently is and where it needs to be.
- Compute the Target Wheel Speed from the Pitch Error. This speed is either faster or slower than the CO speed to make TwoPotatoe eventually reach the Target Angle. A command is sent to the motors to go at this speed.
The state of the motors (Accelerate, Brake, Coast) is controlled during routines called at each encoder interrupt. The motors are not controlled through the usual PWM outputs of the Arduino. This allows for almost instantaneous response to changing conditions. The IMU is read at400 HZ.
Also, note that the algorithm falls apart badly at low speeds—especially at zero speed where then no interrupts are being generated. To take care of this problem, the TwoPotatoe Algorithm also monitors the motors each time the IMU is read and adds pulses when necessary.
A number of people have asked to look at the code so they can understand how TwoPotatoe works. The code is rather large and arcane so that it is difficult to understand. To add to this problem is the simple fact that it is still under constant modification. I intend to clean up the algorithm someday so that it is properly structured and understandable. I have wanted to do this for the past two years but I haven’t had the time to do so. Realistically, this is not going to happen any time soon.
It is not that I am trying to keep the code a secret. I am simply to busy–or lazy– to publish it. Sorry.
However, if you want to look at the basic algorithm, it is contained in the FivePotatoe code. A link is available on the FivePotatoe page.
- Mosfet power control. This allows TwoPotatoe to be turned off (but not on) remotely. More importantly, TwoPotatoe can automatically power down when the battery charge drops to a dangerously low level, thereby preventing battery damage.
- Wheel pressure sensors. This allows TwoPotatoe to be picked up and set down while it is running.
- Extensive logging during operation to allow refining the algorithm.
- Arduino Due
- Sparkfun LSM-6 IMU
- Pololu 36v20 CS High-Power Motor Drivers
- Banebot P60 11/1 gearbox with RS700 24V motors
- 4 or 5 cell LyPo battery (depending on top speed)
- XBee Pro 900 radios for communication with Controller
- BlueSMIRF bluetooth modem for communication with the PC.
- Losi 1/5 scale DB XL wheels
- Ultrasonic Range Finders – XL-MaxSonar-EZ3
- Sparkfun Triple Axis Magnetometer – HMC5883
- Sparkfun Rotary Encoder – 1024 P/R
- Chain drive & suspension from mostly Actobotics components
- Baltic birch frame
Many people have noted that most (nearly all?) self-balancing robots use the PID algorithm to stay upright and that TwoPotatoe does not use this algorithm. This is because a PID algorithm always produces a sub-optimal solution to the problem. It is very difficult, if not impossible, to tune a PID algorithm do deal adequately with complex maneuvers, rough ground, and inclines. As the many videos on YouTube indicate, they can be made to work fairly well, but not ideally, on a flat surface. The Wikipedia article on PID algorithms states the issue very well:
While PID controllers are applicable to many control problems, and often perform satisfactorily without any improvements or only coarse tuning, they can perform poorly in some applications, and do not in general provide optimal control.
Since understanding the motion of a self-standing robot requires only high-school-level Newtonian physics, it should be possible to control the motors in a way that is optimal in all circumstances. TwoPotatoe attempts to solve the control problem in a way that is optimal in all circumstances. Ideally, the only limitation on performance of the robot should be the power of the motors and traction of the wheels. If the motors and wheels are capable of performing a particular action, then the algorithm should be capable of correctly controlling it. There should be rapid recovery from all situations with no overshoot or oscillation. A PID algorithm means that compromises must be made between rapid response and oscillation/overshoot.
PID algorithms were invented in the time of vacuum tubes and analog computation. Their simplicity is very attractive but with modern high-speed computation, there are better ways to solve this problem.
PID Thought Problem
It is interesting to go back to the original problem that the PID algorithm was designed to solve: steering a ship. Let’s say you are steering a ship and want to change course by 30 degrees. If you let a PID algorithm to do your steering for you, you simply adjust the PID parameters to achieve the new course relatively quickly with a minimum of oscillation at the end. This is simple and it works; it doesn’t really matter in the usual case that it is not as rapid as it could be or that it takes a little settling time at the end to eventually achieve the new course. If you are captain of the ship and you need to set the new course as rapidly as possible to avoid mines that have been planted by the enemy, you might have a different attitude. The captain would certainly disable the PID control, take control of the rudder and take the following actions:
- He (she?) would steer a hard right causing the ship to rotate as rapidly as possible toward the target orientation.
- At some point he would steer a hard left to stop the rotation.
- If everything was calculated correctly, the ship will stop rotating at the point that it has turned 30 degrees. At this point the rudder is brought back to the center position.
These actions produce the most rapid response possible.
TwoPotatoe thinks like the captain.