Need Help with Balancing Robot

amalfiCoast

Hi,

I'm trying to develop a balancing robot but can't get it properly tuned. I'm using a complementary filter to get angles. My controller algorithm as well as my motor controller code is below. I'm using the Pololu VNH5019 motor controller and a Propeller Activity Board with Parallax gyroscope and accelerometer modules. Maybe someone who has successfully built a balancing robot could post their code. Thanks.

//PI with Rate Feedback
    float Error=theta*180/PI;
    float Error_int=Error_int+Ts*Error;
    float servo=kp*Error+ki*Error_int+kd*gx*180/PI;
    
    // Add PWM control for motors
 
 if(servo<0){
      //Motor 1
      high(0);
      low(1);
      pwm_set(2,0,-servo+15.); // when pwmstart = 100, set these to 1.8
      //Motor 2
      high(3);
      low(4);
      pwm_set(5,1,-servo+18.);
    }
    else if(servo>0){
      //Motor 1
      high(1);
      low(0);
      pwm_set(2,0,servo+15.);
      //Motor 2
      high(4);
      low(3);
      pwm_set(5,1,servo+18);
    }           
           
  }    
}