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