Hi,
I am building @ mecanum bot and am in the process of tuning the PID-controller for the wheels. The PID-algorithm I implemented works pretty well (I think), but still I am not satisfied. I could do with some advice.
I define motor speed in terms of the number of state changes per second (SC/sec). Max number of SC/s is 5000. If I set the desired speed at 3000 the results are as depicted in the graph.
The green line reflects the actual speed on the motor shaft (left y-axis). The red line the PWM value to the motor controller (red y-axis). Blue is the error. Since I am not a experienced PID-tuner I wonder if “the patterns” in the graph are ok. The speed looks fine to me, but the motor is a bit shaky and the sound is not smooth (compared to the motor sound when I enter a fixed PWM value to the motor controller). Is the PWM-spread to large, is the pattern ok?
I use a DC 12V metal gearmotor with 64 CPR encoder from Pololu (http://www.pololu.com/product/1445). In conjunctions with a Pololu motor controller Dual MC33926 (http://www.pololu.com/product/1213).
At present I only use the P en D term. I have spent hours tuning the PID-controller. I used the following recepy found on the internet.
A simple empirical approach is to start by zeroing the integral and derivative gains, and just use the proportional term. Setting the proportional gain increasingly higher will finally cause the system to oscillate. This is bad behavior; don’t allow it to continue. Reduce the proportional gain until you are just below the point of incipient oscillation. You can then try bringing up the derivative gain, which should act to forestall the start of oscillatory behavior. And finally adding a small amount of integral gain may help bring the system to the final set point.
I turned out as I expected but as mentioned before I am not happy with the result.
I also experimented with gain scheduling, but no significant improvement. Should that indeed help or is it over the top for what I want?
The core of my sketch looks like:
void Motor(byte motorId, int motorSpeed){
int actualSpeed, desiredSpeed, error, pwmDelta;
int Kp, Ki, Kd,
if (motorSpeed >= 100) {motorSpeed = 100;}
if (motorSpeed <= -100) {motorSpeed = -100;}
desiredSpeed = motorSpeed * maxSpeed / 100; // in terms of #SC's/sec
if (motorSpeed < 0 )
{
desiredSpeed = desiredSpeed * -1;
}
switch (motorId)
{
case 1 :
actualSpeed = 1000000 / m1Pulse;
error = desiredSpeed - actualSpeed;
if(abs(error) < gainThreshold) // gainscheduling
{
//we're close to setpoint, use conservative tuning parameters
Kp = consKp;
Kd = consKd;
Ki = consKi;
}
else
{
//we're far from setpoint, use aggressive tuning parameters
Kp = aggKp;
Kd = aggKd;
Ki = aggKi;
}
pwmDelta = Kp * error / 100;
pwmDelta += Ki * m1SumError / 100;
pwmDelta += Kd * (error - m1PrevError) / 100;
m1PrevError = error;
if (abs(m1SumError) < aggThreshold_I)
{
m1SumError += error;
}
else
{
m1SumError = 0;
}
m1Pwm = m1Pwm + pwmDelta;
if(micros()- m1Time > 20000 && motorSpeed!=0) m1Pwm = 50; // JumpStart
if (motorSpeed >= 0)
{
digitalWrite(m1In1, HIGH); // Forward
digitalWrite(m1In2, LOW);
}
else
{
digitalWrite(m1In1, LOW); // Backward
digitalWrite(m1In2, HIGH);
}
if (m1Pwm > 255) { m1Pwm = 255;}
if (m1Pwm <= 0) { m1Pwm = 0; }
if (motorSpeed == 0) { m1Pwm = 0; }
analogWrite(m1D2,m1Pwm);
Could really use some help and tips at this point ;-)
Regards, Ko