Hi all, I am so close to having my very own self balancing bot, except well, it doesn't balance yet. I need to get the PID loop tuned up so that it will balance. I believe I have all of the relevant code below. I am using a sabertooth 2x12 RC controller and have the motors setup as servos so that the outputs are properly received at 50hz by the sabertooth controller, which accepts a standard 1000us to 2000us pwm signal where 1000us is full reverse, 1500us is stop, and 2000us is full forward.
My output values properly run between servo outputs of 0 to 90 and 90 to 180 to generate the required pulse outputs. However, I can't figure out how to get the darn thing to balance. Any tips on this?
I have read tons about how to tune PID loops. My method has been to increase proportion until I get some decent movement, and then up the integral to try minimize the time it takes to hit the stable state. However, I have been miserably unsuccessful. The bot seems to have a slow response time and tips far one way before sending the motors to high speed to try and compensate. The motors are plenty strong/fast enough, really too fast at full speed.
void FilterData()
{
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //Get accelerometer/gyroscope readings
Angle_Raw = (atan2(ay, ax) * 180 / pi + Angle_offset); //Angle Position
omega = gz / Gyr_Gain + Gry_offset; // Angular Velocity
unsigned long now = millis(); //Time program has been running
float dt = (now - preTime) / 1000.00; //Sample time in seconds
preTime = now;
float K = 0.8;
float A = K / (K + dt);
Angle_Filtered = A * (Angle_Filtered + omega * dt) + (1 - A) * Angle_Raw;
}
void PIDloop()
{
kp = 10; //10
ki = 0; //
kd = 0; //
unsigned long now = millis(); //Time program has been running
timeChange = (now - lastTime); //Prevent integer overflow issue
lastTime = now; //Set time measurement was taken
error = Angle_Filtered; // Proportion
errSum += error * timeChange; // Integration
dErr = (error - lastErr) / timeChange; // Differentiation
Output = kp * error + ki * errSum + kd * dErr; //Calculated Output
lastErr = error; //Reset for next loop
Motor1 = constrain(Output - Turn_Speed + Run_Speed, -90, 90);
Motor2 = constrain(Output + Turn_Speed + Run_Speed, -90, 90);
}
if(Motor1 > 0) //Forwards Motor1, same code for motor2
{
S1.write(map(Motor1 + Motor1_offset, 0, 90, 90, 140));
}
else if(Motor1 < 0) //Backwards Motor1
{
S1.write(map(Motor1 + Motor1_offset, -90, 0, 30, 90));
}
Any help on this would be greatly appreciated. Thank you guys so much for any advice that you may have. Also, I have already read through the 16 page thread from a few years ago on balancing bots and have done tons of googling on the topic.