Paso 4: Código de Arduino para el equilibrio invertido sobre el terreno
Lo que sé, un péndulo invertido se logra controlando el par de motor conducción en la ecuación de Lagrange. Par del motor se puede controlar por una corriente al motor o una aceleración angular del motor.
En este proyecto se aplican control actual del motor.
Corriente para el equilibrio invertido del motor requerida se calcula como fórmula siguiente;
iValue = K1 * theta + K2 * phi + K3 * dtheta + K4 * dphi
iValue: requied corriente del motor (A)
Theta: ángulo de la rueda (rad)---calculado por integración de dtheta
Phi: ángulo (rad) – del cuerpo---obtenida del sensor de MPU6050
dTHETA: rueda velocidad angular (rad/seg)---Estimado usando el valor medido y el valor PWM
dphi: cuerpo velocidad angular (rad/seg)---computado por la diferenciación de phi
K1, K2, K3, K4: ganancias (estas cifras pueden encontrarse por prueba y error).
(Theta y dtheta Estimado son inexactos, pero éstos están trabajando en este proyecto).
Control de corriente directa es difícil. Por lo tanto, me mida al motor (iCurrent) y basado en error (iValue – iCurrent) alimentación proporcionalmente a valor PWM al motor con filtro de paso bajo.
Estos arriba mencionados se resumen a continuación;
flotador de K1, K2, K3, K4;
flotador dtheta, phi, theta, dphi;
iValue flotador, iCurrent, iError;
flotador Ka;
iMotor de flotador;
void loop()
{
iValue = K1 * theta + K2 * phi + K3 * dtheta + K4 * dphi;
iError = iValue - iCurrent;
iMotor = suave (Ka * iError, 0.05, iMotor);
MotorDrive(iMotor);
}
En-el-lugar-equilibrio no es tan divertido!
R/C coche debe ser remota controlada.
Luego, hacer control remoto!
