# Self balancing robot: Algorithm for turning about the vertical axis

I am working on a segway robot

and I have managed to make it balance, move forward or backward quiet well. Now the next step is to make it rotate about the vertical axis. From the papers I have read and some codes I read, I saw that I have to subtract voltage from one motor and add to other. But when I do so, either the robot doesnot rotate at all or when it does, it starts to lean in one direction and then goes in circles of increasing radius and then falls down. However while standing and moving forward and backward, it is very stable even to large disturbances. I am using a cascade control loop. So I think I am missing something in my code. Can somebody please guide me how to solve this last piece of puzzle? the following is the relevant code snippet responsible for balancing / moving. It is still incomplete because i am still experimenting wit it. I am only writing the general algorithm I tried.

``````Setpoint_bal = Output_trans; // Set the output [angle in deg] of the translation PID as Setpoint to the balancing PID loop

Input_bal = Theta_now + Theta_correction; // Set Theta_now as the input / current value to the PID algorithm (The correction is added to correct for the error in MPU calculated angle)

error_bal = Setpoint_bal - Input_bal; // To decide actuator / motor rotation direction

bal_PID.Compute_For_MPU(Kp_bal, Ki_bal, Kd_bal, omega_x_gyro);// Compute motor PWM using balancing PID

Output_bal = map(abs(Output_bal), 0, Out_max_bal, Output_lower_bal, Out_max_bal); // Map the computed output from Out_min to Outmax Output_lower_bal

Output_rmot =  motor_corr_fac * Output_bal; Output_lmot = Output_bal; // Seperate the output computed for both motors

if (abs(error_bal)<0.2 && mode_now == "balance" && rotating == false){Output_rmot = 0.0; Output_lmot = 0.0;} // To prevent continuous jerky behaviour, the robot starts balancing outside +- 0.2 deg

///////////////////////////////////////// If robot has fallen then stop the motors /////////////////////////////////////////////

if (abs(error_bal)>=fall_angle){
Output_rmot = 0.0;
Output_lmot = 0.0; // Stop the robot
rotating = false;
mode_now = "balance"; // Change mode to balance
mode_prev = "balance"; // Change mode to balance
}
///////////////////////////////////////// Apply motor controls /////////////////////////////////////////////
mot_cont(); // Apply the calculated output to control the motor

void mot_cont(){

if (error_bal>0){back_bot();}  // If error_bal is +ve go back
else if (error_bal<0){fwd_bot();} // If error_bal is -ve go forward
}

void fwd_bot(){
digitalWrite(Lmot1, LOW);
digitalWrite(Lmot2, HIGH);
digitalWrite(Rmot1, LOW);
digitalWrite(Rmot2, HIGH);

if (rotating == false){ // If not rotating, we donot need to apply different voltages to the motors
analogWrite(Lmot3,Output_lmot);
analogWrite(Rmot3,Output_rmot);
}
else if(rotating == true){ // For rotating clockwise in forward direction, apply extra voltage to left motor
if (rotation_direction == "clockwise"){
analogWrite(Lmot3,Output_lmot + Rot_Speed_init); // orig
analogWrite(Rmot3,Output_rmot);
}
else if (rotation_direction == "counter_clockwise"){// For rotating anti - clockwise in forward direction, apply extra voltage to right motor
analogWrite(Lmot3,Output_lmot);
analogWrite(Rmot3,Output_rmot + Rot_Speed_init); // orig
}
}

}

void back_bot(){
digitalWrite(Lmot1, HIGH);
digitalWrite(Lmot2, LOW);
digitalWrite(Rmot1, HIGH);
digitalWrite(Rmot2, LOW);

if (rotating == false){ // If not rotating, we donot need to apply different voltages to the motors
analogWrite(Lmot3,Output_lmot);
analogWrite(Rmot3,Output_rmot);
}
else if(rotating == true){ // For rotating clockwise in forward direction, apply extra voltage to left motor
if (rotation_direction == "clockwise"){
analogWrite(Lmot3,Output_lmot); // orig
analogWrite(Rmot3,Output_rmot + Rot_Speed_init);
}
else if (rotation_direction == "counter_clockwise"){// For rotating anti - clockwise in forward direction, apply extra voltage to right motor
analogWrite(Lmot3,Output_lmot + Rot_Speed_init);
analogWrite(Rmot3,Output_rmot); // orig
}
}
}
``````

the following is the relevant code snippet

In your opinion, that is the relevant code. Others may have other opinions.

I am only writing the general algorithm I tried.

The algorithm could be wrong.

Sorry, I only wrote the section where I was implementing the motor control and PID calculation. The whole code is attached herewith. I appreciate you guys replying.

The following code is from a repository whose robot works fine. This function has the PID control algorithms and motor control functions. The turning logic is defined at
/* Steer robot sideways */

``````void PID(double restAngle, double offset, double turning) {
/* Steer robot */
if (steerForward) {
offset += (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing
restAngle -= offset;
}
else if (steerBackward) {
offset -= (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing
restAngle += offset;
}
/* Brake */
else if (steerStop) {
long positionError = wheelPosition - targetPosition;
if (abs(positionError) > zoneA) // Inside zone A
restAngle -= (double)positionError/positionScaleA;
else if (abs(positionError) > zoneB) // Inside zone B
restAngle -= (double)positionError/positionScaleB;
else // Inside zone C
restAngle -= (double)positionError/positionScaleC;
restAngle -= (double)wheelVelocity/velocityScaleStop;
if (restAngle < 160) // Limit rest Angle
restAngle = 160;
else if (restAngle > 200)
restAngle = 200;
}
/* Update PID values */
double error = (restAngle - pitch);
double pTerm = Kp * error;
iTerm += Ki * error;
double dTerm = Kd * (error - lastError);
lastError = error;
double PIDValue = pTerm + iTerm + dTerm;

/* Steer robot sideways */
double PIDLeft;
double PIDRight;
if (steerLeft) {
turning -= abs((double)wheelVelocity/velocityScaleTurning); // Scale down at high speed
if(turning < 0)
turning = 0;
PIDLeft = PIDValue-turning;
PIDRight = PIDValue+turning;
}
else if (steerRight) {
turning -= abs((double)wheelVelocity/velocityScaleTurning); // Scale down at high speed
if(turning < 0)
turning = 0;
PIDLeft = PIDValue+turning;
PIDRight = PIDValue-turning;
}
else {
PIDLeft = PIDValue;
PIDRight = PIDValue;
}

PIDLeft *= 0.95; // compensate for difference in the motors

/* Set PWM Values */
if (PIDLeft >= 0)
moveMotor(left, forward, PIDLeft);
else
moveMotor(left, backward, PIDLeft * -1);
if (PIDRight >= 0)
moveMotor(right, forward, PIDRight);
else
moveMotor(right, backward, PIDRight * -1);
}
``````

I have also tried other algorithms such as

• Increasing the rotation speed in steps and keep it constant thereafter.
• Increasing the rotation speed in steps and the decreasing it after it reaches maximum value and keep doing it until stop signal is sent
• Setting up the setpoint +ve, then increasing PWM value in one wheel, the Setting upsetpoint -ve, then increasing PWM in the other wheel

What am I doing wrong here ? Seems like other robots on youtube seems to be having no problem with turning. What's confusion me is the fact how do I balance and rotate at the same time?

If the code is available, read it to see what the successful robots are doing.

I did already try to implement what they are doing, but I don't know why it is not working in my robot. That's why I approached this forum.

NischalSehrawat:
I did already try to implement what they are doing, but I don't know why it is not working in my robot. That's why I approached this forum.

Maybe you need to review the assumptions again. For basic robot balancing, the two wheels are assumed to travel more or less at the same speed (including direction), and the balancing is done along a single vertical plane.

When the robot is turning in 1 spot (not moving along in straight line), then the dynamics are going to be different. Probably a lot different than for the basic case.

So ----- the thing to do is to first come up with a plan for the method(s) to keep the balancing robot up-right when 1 wheel is spin in 1 direction, while the other wheels is spinning in the opposite direction. If 'ideally' the two wheels are going in opposite directions, and at identical speeds (and the wheel diameters are identical) --- then perhaps the problem would be similar to 1-plane balancing. But this probably can't be assumed in practice.

Thanks @Southpark. I did try a new approach and somehow succeeded as well. However, when I see other robots, they can continuously rotate but when I attempt to do so, the robot falls down. But doing it in batches gives me no problems at all. If I look at equations of motion, then the rotation and balancing parts are not coupled at all. But I think, in actual scenario, the mass of the robot is not symmetrically distributed which might cause some imbalance. The following is the code I wrote and it works but with this I cannot keep the robot in continuous rotation mode. I have to stop in between.

``````    if (rotating == true){
if (start_again == true){Rot_Speed = Rot_Max; start_again = false;}
Rot_Speed-=rot_steps;
if (Rot_Speed<0){Rot_Speed = 0; rotating = false;}// If Rot_Speed <0, set rotating = false to get out of rotation if statement
if (rotation_direction == "clockwise"){
Output_lmot -=  Rot_Speed;
Output_rmot +=  Rot_Speed;
}
else if (rotation_direction == "anti_clockwise"){
Output_lmot +=   Rot_Speed;
Output_rmot -=  Rot_Speed;
}
}
``````