We are currently making a balancing robot as a school project.
The robot has a gyroscope and an encoder to get the angle of the robot and the rpm of the wheels. By using the current angle of the robot and defining a target angle we are able to make the robot stand still and not fall over.
We don't understand how to incorporate the encoder (that measures the angular velocity of the wheels) to make the robot stand still when he motor speed is changing. The robot only knows the angle it is at so it won't compensate for the speed. Could one perhaps use the encoder in such a way that dictates the target angle and keep the robot still or even better control it?
We would also like to know if there is a way to automatically adjust TargetAngle to the needed one, like if the floor angle were to change.
Here is a sample of our code:
error = TargetAngle - CurrentAngle; integral = integral + error*ki; derivative = (error - lastError)*kd; lastError = error; pidsum = derivative+error*kp+integral;//this is what is used for throttling the engines