I'm trying to stabilize a robot with a servo motor attached to two wheels of a robot. I already built the system as following:
- Apply Kalman filter to gyrometer/accelerometer output
- Calculate a roll angle (accurate one)
Now I use these results and feed it back to incremental PID controller. My problem is that the PID controller output is very high or some times is negative, I want to scale it to a proper servo angle, for example:
The PID output is 5000, but the servo understands "commands" between 1 & 180 degrees. So how to scale the output via a scientific method to make sure that the motor receives the right commands from PID?
My target is when the robot wheel goes right, the motor should make it in the opposite direction and so on.
Update #1:
The error of PID output depends on the refrence roll angle i want to be at and the cureent roll angle measured from IMU (that depends on the the accelerometer & gyro meter ) ,
the input to the servo is the servo angle position which lies between 1 degree to 180 degree
Best Answer
To do this "scientifically", you need to decide exactly what physical quantity the output of your PID controller represents, and in exactly what units. Does it represent course error, heading error, steering angle or something else?
Also, what physical quantity does the input signal to the servo represent? Steering angle?
If both quantities represent, say, steering angle, then you're mostly done. You just need to apply a scale factor to convert the units.
But if they represent different physical quantities to begin with, you need to figure out a way to translate from one to the other. This is called "kinematics" (or sometimes "reverse kinematics", depending on how your control algorithm works). It's basically a mathematical description of how the various physical quantities (mainly angles and distances, but could also include forces, masses and velocities) are related in your system.