Ok here's the deal: after putting the kinematics equations into the robot, theoretically everything was fine, but the robot was smashing into the table, it was having some deep dives heading for destruction. Why the heck was that happening?
My first thought was that maybe the controller was not calculating the right angle values, but after a quick check with a serial monitor, those values were fine. Then I thought that maybe the equations were wrong, so I simulate the different arm positions (below picture) but they were ok.
|Yep, I drew the trajectories using a Logo simulator... it was fast and convenient at the time ;)|
With all the directions from controller performing as desired, I realized that some of the servos just weren't capable to go into some positions.
My theory was that it was due to the arm weight and that with larger extensions of the arm, this effect will be greater. To demonstrate that I measured the angles of the joints with several arm extensions. The shoulder servo showed a notorious error increase with larger extensions, elbow showed a relative stable error with a constant value (possible due to some assembly issue). Wrist servo had almost no error.
|In summary: I had a shoulder servo that couldn't hold all the arm weight when extended and a elbow with a constant error value.|
How to fix this?
On first impression the solution would be to buy a more powerful servo motor for the shoulder, but since I'm saving to buy an apartment, I don't have so much money to throw away things so easily; and the idea is not just to give muscles to the robot, but to make it more efficient.
So, I tried to help the servomotor with some elastic force. First I used the spring that Lynxmotion suggests on their page (and comes with the arm assembly parts), It helped somehow, especially for short and medium extensions, but for large extensions it was not effective. Then I added also a rubber band but the assembly behaved just the same as with only the spring.
Then I thought that if the error in the joint angle behaved always similarly, I could find an equation to describe it, therefore having an error correction equation. I made an X-Y graph with the theoretical values vs the obtained angles, added a trend line and found the equation for it; so next time I would order the servo to go to angle A in order to really go to angle B. I applied this software correction procedure to shoulder and elbow joints.
|Shoulder and elbow angle functions|
Ultimately, I figured out that reducing the mass of the arm, servos would perform better. So I amputated the arm and removed the wrist rotate upgrade to save some weight (I prefer more accurate results even by eliminating 1 DOF). This last fixture gave me the best combination of low error values on shoulder and elbow joints.
Another effect I had was that servos moved so fast that they had a large underdamped movement (that decreasing oscillation when it stops moving), hitting things due to this inaccuracy.
When you have a motor with this issue, usually you apply a PID control to its movements and tuned it up, but since in a servo motor you do not apply a control signal directly (you just specify the desired position) this option was not feasible.
The technique used in several Arduino forums was to reduce the speed of servo, this is achieved by gradually increasing the desired position sent to the servo.
By using the error correction techniques described above and decreasing the speed of the servos, I was able to obtain smoother and more accurate results, however the best way to achieve more precise and accurate results would be to have some feedback of the joint position going to the controller, having this way a closed loop control for the position.
In conclusion: I need to get better servos (like the Dynamixel servos) or hack the ones I have to have that position feedback in real time.