# Self balancing robot problem

Hello everybody,

We have to make a Self Balancing Robot for our integraded test.
If the robot falls to the negative side, the robot reacts to slow but if the robot falls to the positive side he works excellent. Can anybody check our program?

``````drive = updatePid(setPoint, angle_y);
float x = angle_y-setPoint;

if( x > setPoint +0.5 and x < 25)
{
digitalWrite(Dir_A,LOW);
digitalWrite(Dir_B, HIGH);
//digitalWrite(Brake_A, LOW);
//digitalWrite(Brake_B, LOW);

analogWrite(PWM_A, abs(drive*1));
analogWrite(PWM_B, abs(drive*1));
//Serial.print("L");
}

else if( x < setPoint - 0.5 and x > -25)
{
digitalWrite(Dir_A, HIGH);
digitalWrite(Dir_B, LOW);
//digitalWrite(Brake_A, LOW);
//digitalWrite(Brake_B, LOW);

analogWrite(PWM_A, abs(drive*1.25));
analogWrite(PWM_B, abs(drive*1.25));
//Serial.print("R");
}

else
{

analogWrite(PWM_A, 0);
analogWrite(PWM_B, 0);

//Serial.print("N");
}
``````

and this is our PID

``````float GUARD_GAIN = 10.0;                // 20.0

float K = 1.064 ;       //1.9 * 1.12;  // wheels 80mm
//float K = 1.9 ;      // wheels 100mm
float Kp = 15;
float Ki = 6.9;
float Kd = 20;
float Kp_Wheel = -0.025;
float Kd_Wheel = -9;

int last_error = 0;
int integrated_error = 0;
float pTerm=0, iTerm=0, dTerm=0, pTerm_Wheel=0, dTerm_Wheel=0;

int updatePid(int targetPosition, int currentPosition)   {
int error = targetPosition - currentPosition;
pTerm = Kp * error;
integrated_error += error;
iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
dTerm = Kd * (error - last_error);
last_error = error;
pTerm_Wheel = Kp_Wheel * count;           //  -(Kxp/100) * count;
dTerm_Wheel = Kd_Wheel * (count - last_count);
last_count = count;
return -constrain(K*(pTerm + iTerm + dTerm + pTerm_Wheel + dTerm_Wheel), -255, 255);
}
``````
``````if( x > setPoint +0.5 and x < 25)
``````

It is hard to imagine that the above statement would compile without errors.

What is the point of multiplying by 1?

``````   analogWrite(PWM_B, abs(drive*1));
``````

there is no point about the multiplying.
Why would that statement have errors it, it works fine for the positive side and there we have the same code.