I am very cuious, why do you use the double integration? What was the issue with leaving it out?
For team #492, we only used PI for our velocity control and PD for our arm had excellent results. Attached is our servo code. It is considerably different from the typical examples and I will be publishing (sometime) a white paper on the subject out at
http://www.titanrobotics.net on the subject. Highlights:
1. we modeled the drive motors and compensated for the back-EMF with some posative velocity driven feedback. By moving this large, known error, outside of the control loop, we were able to remove most of the integral term and vastly improve the stability of the system while keeping tight control.
When properly implemented, back-EMF compensation (when in "torque" control mode) makes the robot feels "frictionless". Very cool to be able to push the robot and have it glide across the floor... In fact, that is how I tuned it: pushed the robot back and forth and adjusted the kemf values (left, right, forward, reverse) until the robot glided without speeding up or slowing down, in a straight line. This has obvious control implications: speed errors generate a torque command, when up to speed, the torque command goes to zero (no integral term needed!) yet the robot continues to zoom along.
2. We used gain scheduling on our arm. Actually, all we did was divide the K term by 16 when overhead because of the drive slop and lack of gravity keeping things taut. Still, gain scheduling sounds cooler
3. We also limited the maximum torque (current) to our motors. Rather than using the FIRST supplied current sensors (which would have been the precise way to do this) we used the back-EMF technique. Anyway, by limiting the maximum torque we avoided all issues of overheating our motors and popping circuit breakers. With a dual motor, simple gearbox (no shifting) design we still had plenty of power to push goals and robots around
