Estoy tratando de obtener un control preciso sobre la velocidad del robot basado en rover 5. Tiene cuatro motores controlados por PWM y 4 codificadores de cuadratura óptica. Estoy usando un controlador de motor de 4 canales con chasis rover 5 . Estoy usando arduino Nano para el control. Puedo leer la salida INT del codificador y cambiar PWM según el ancho de pulso para controlar la velocidad. Pero, como resultado, recibo fuertes oscilaciones en la salida de control. Eso hace que el robot se mueva por pasos, ya que PWM cambia constantemente. Necesito un algoritmo que pueda minimizar este timbre y tener un robot de movimiento suave. Aquí está mi fragmento de código de arduino.
void setup() {
Serial.begin(9600);
init_motors();
init_encoders();
req_speed[0] = 20;
req_speed[1] = 20;
req_speed[2] = 20;
req_speed[3] = 20;
}
void loop() {
update_encoders();
update_motors();
}
void update_motors() {
int i, err;
unsigned long req_width;
if(micros() - mtime > 2999) {
mtime = micros();
for(i=0; i<4; i++) {
digitalWrite(pins_dir[i], req_speed[i]>0);
if(mtime - change_time[i] > 50000ul && req_speed[i] != 0) {
cur_pwm[i] += 5;
}
if(req_speed[i] > 0)
cur_err[i] = req_speed[i]*10 - cur_speed[i];
else
cur_err[i] = (-req_speed[i]*10) - cur_speed[i];
if(cur_err[i] > 0 && cur_pwm[i] < 255) {
cur_pwm[i]++;
} else if(cur_err[i] < 0 && cur_pwm[i] > 0) {
cur_pwm[i]--;
}
analogWrite(pins_pwm[i], cur_pwm[i]);
}
}
}
void update_encoders() {
int i;
unsigned long w;
enc_new = PINC & B00001111;
unsigned long etime = micros();
for (i=0; i<4; i++) {
if((enc_old & (1 << i)) < (enc_new & (1 << i)))
{
w = (unsigned long)(((etime - change_time[i])));
pulse_width[i] = (w + pulse_width_h1[i] + pulse_width_h2[i])/3;
pulse_width_h2[i] = pulse_width_h1[i];
pulse_width_h1[i] = pulse_width[i];
change_time[i]=etime;
pulse_count[i]++;
cur_speed[i] = (3200000ul / pulse_width[i]);
}
}
enc_old=enc_new;
}
Aquí req_speed está entre -100 y 100, donde el signo indica la dirección. Considere todas las variables indefinidas como globales. Experimentalmente medí que, cuando el motor está funcionando a toda velocidad, el ancho de pulso es de alrededor de 3200us.
Las salidas INT de los codificadores (XOR de A y B) están conectadas de A0 a A3. El motor PWM está conectado a D3, D5, D6, D9.
¿El arduino no es lo suficientemente rápido como para ponerse al día con 4 codificadores? ¿Es PinChangeInts mejor que sondear?
Permítanme sugerir mejoras a este código y aconsejarme sobre lo que me estoy perdiendo aquí.
Me parece que su ciclo de control es esencialmente:
if speed is too slow:
increase PWM duty cycle one unit
if speed is too fast:
descreate PWM duty cycle one unit
Como has observado, esto no funciona tan bien. Su lazo de control sobrepasa cíclicamente la velocidad objetivo.
La solución canónica a este tipo de problema es un controlador PID . El concepto es esencialmente el mismo, medir algo y compararlo con un objetivo para calcular un error. Luego ajuste algo (en su caso, el ciclo de trabajo PWM) según el error.
Sin embargo, un controlador PID tiene tres términos de error:
Para cada uno de estos términos, el controlador ha programado alguna ganancia. Luego multiplica cada término por la ganancia respectiva para calcular cuánto se debe cambiar la entrada de control (ciclo de trabajo PWM). Con el ajuste adecuado, esto le permite obtener un circuito de retroalimentación que al principio acelera el ciclo de trabajo y luego, a medida que su vehículo se acerca a la velocidad objetivo, retrocede suavemente para que obtenga un mínimo de exceso.
Realmente creo que debería mirar las interrupciones en lugar de sondear al menos. También estoy haciendo un proyecto de codificador similar (aunque no para un robot) y he decidido gastar unos pocos dólares en un chip separado para contar los pulsos que luego puedo leer en los pines analógicos a través de un DAC.
Esto tiene varios beneficios:
El chip que me han recomendado está aquí.
Al usar, por ejemplo, un pin analógico Arduino -> multi/demultiplexor -> DAC -> chip de interfaz de codificador dedicado, puede liberar MUCHOS pines de arduino / tener muchos más codificadores;)
Es posible que deba reducir la velocidad de su respuesta PWM cuando haya medido que el motor no está funcionando a la velocidad correcta. Supongo que calcula un ciclo de trabajo PWM objetivo dado el error. Se llama control de velocidad y es a lo que la gente se refiere tradicionalmente como el término diferencial en un controlador de tres términos. Básicamente, una forma simple de lograr esto es decidir hacia dónde debe dirigirse pero limitar la cantidad por segundo que cambia el ciclo de trabajo de PWM.
De todos modos, ¡esa es la opinión de un ingeniero analógico y alguien solo escribe código en emergencias!
Punit Soni
phil escarcha
Punit Soni
Pablo Sullivan