# Balancing robot - How to control the velocity and angle

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

• Do not know if it is possible to do it with classic PID. You should take a look at a sewvay dynamics equations and build your control system in a state-space with speed and angle as your states. – Long Smith Jan 19 '18 at 6:22
• Maybe i did not cleary understand your question. Do you want to keep non-zero angle of your robot? – Long Smith Jan 19 '18 at 6:26
• Hi, yes, we want the robot to automatically adjust itself to the best angle as a function of the robots speed. What we want to know is how we can do this. We have managed to stablize the robot on flat ground, however, if were to to move the robot, add a weight or put the robot on a slope, the angle needed to stabilize the robot is different, and we believe this can be accomplished with the speed, provided by the encoders. – twmanar Jan 19 '18 at 19:46
• Hi, I am not quite sure I understand you. Are you saying you cannot balance the robot in one place? Does it have to move at a certain speed to balance itself? This is what I understand when you stated that you wanted the "robot to stand still without a continuous velocity". – goddar Jan 20 '18 at 0:27

First of all, well done on getting your robot to balance! If you’ve got this far the rest should be easy. A cascading PI loop (no D) works very well for this. You have an outer loop to control the speed and an inner loop to control the stability. In the outer PI loop, you set the speed and the necessary angle is calculated (hint: angular velocity can be calculated simply from the conversion of PE to KE). This is then passed down as the angle set point for the inner PI loop. This is what has been used here. I hope this helps.

Consider doing a PID loop on the motor angle.

Target angle can be calculated tick-by-tick based on the motion profile you want the robot to move compensated with the desired robot angle (you will likely want to lean forward when beginning to more forward, etc. like a person would do).

Actual angle can be determined by the encoder position combined with the gyro angle.

• Note: I Googled "pid for balancing robot" and saw several interesting entries. – markshancock Jan 18 '18 at 21:36

Balancing problem is all about eliminating all the torque on the robot with the motors. We are currently working on a balancing system which is more advanced but the idea is almost the same. You should use torque controller using the encoders.

$$\frac{V - Bw}{R} = I$$

Where $$V$$ is the voltage you give to the motor and $$w$$ is the angular velocity of the motor. $$-Bw$$ part is the back EMF from the rotating motor. Since torque is directly proportional to current, you can use PID to calculate the required velocity of the motors. You can calculate the required torque by using the gyro.