Go Down

Topic: Self balancing robot problem (Read 750 times) previous topic - next topic

CptNmars

Mar 09, 2015, 11:56 am Last Edit: Mar 10, 2015, 11:15 am by CptNmars
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?

Code: [Select]
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

Code: [Select]

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);
}

jremington

#1
Mar 09, 2015, 05:36 pm Last Edit: Mar 09, 2015, 05:38 pm by jremington
Code: [Select]
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?
Code: [Select]
   analogWrite(PWM_B, abs(drive*1));

CptNmars

#2
Mar 10, 2015, 11:09 am Last Edit: Mar 10, 2015, 11:17 am by CptNmars
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.

Go Up