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, not coincidentally, at about the point where the knot appears in the wood.
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 4 fps (feet per second) so that if the joystick is advanced 50% toward its limit, the controller sends a message to TwoPotatoe to go 2.0 fps. TwoPotatoe receives this message goes through the following computation about 100 times a second:
- Find the Speed Error, which is the difference between the Target Speed, commanded from the controller, and CO 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.
A key factor in the performance of TwoPotatoe is the way it controls the motor speed. The above Speed Algorithm requires that the motors go at the speed that is commanded. This is done by controlling the motor directly from the interrupts generated by the motor encoders. The algorithm is very simple:
- The Target Speed is converted to a Target Interrupt Period, which is a value in microseconds.
- When a motor encoder interrupt occurs the time since the last interrupt is computed. If it is greater than the Target Interrupt Period, the motor is turned on. If it is less than the Target Interrupt Period, the motor is set to brake or set in the opposite direction.
Note that the motors are not controlled through the usual PWM outputs of the Arduino.
Also, note that the above 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 and adds pulses when necessary.
- 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
- Pololu MinIMU-9 v3 Gyro, Accelerometer, and Compass
- Pololu 36v20 CS High-Power Motor Drivers
- Pololu 30:1 Metal Gearmotor 37Dx52L mm with 64 CPR Encoder
- 1V or 18.5V LyPo battery (depending on top speed)
- XBee radios for communication with PC and Controller
- Banebot wheels
- Box elder wood harvested on Iowa farm
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. (Thank you Grant Katsumata at Pololu for suggesting just increasing the voltage on the motor to get more speed and power from the motors. The motors haven’t burned out yet!) 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.
The algorithm is still under development and has undergone many revisions over the past few months. Once things stabilize—hopefully in the next few months—I will publish the results on this page.
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 use 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.
TwoPotatoe thinks like the captain.