Autor Tema: Robot equilibrista  (Leído 211848 veces)

0 Usuarios y 1 Visitante están viendo este tema.

Desconectado dogflu66

  • Moderador Local
  • DsPIC30
  • *****
  • Mensajes: 3363
    • PICKEY ELECTRÓNICA
Re: Robot equilibrista
« Respuesta #390 en: 12 de Abril de 2011, 10:33:54 »
He estado revisando el esquema y para la tensión que utiliza no le veo fallos, yo en todo caso buscaría transistores que la Vce sea más baja.
Pensaba que a más baja impedancia mayor inmunidad al ruido. :z)
Lo que sí he podido observar es que las entradas de control no están polarizadas, y si se dejan al aire pueden provocar la destrucción del puente.
Otra cosa que no me ha gustado ha sido que en el datasheets que he revisado no viene la frecuencia máxima de trabajo, en una de las graficas hay una referencia a 0.1Mhz. Normalmente si no se hace referencia a la velocidad es que no son muy rápidos. En mi opinión no elidiría para trabajar con switching transistores de potencia de menos de 2 o 3Mhz.
« Última modificación: 12 de Abril de 2011, 10:47:04 por dogflu66 »
Saludos desde Granada, España.

Desconectado gera

  • Colaborador
  • PIC24H
  • *****
  • Mensajes: 2188
Re: Robot equilibrista
« Respuesta #391 en: 12 de Abril de 2011, 13:21:44 »
Yo tambien tenia por entendido que a menos impedancia mayor inmunidad al ruido. Ya vere que me dice mi profe mañana.
Trabajo el puente a una frecuencia de 1KHz, emite un ruido molesto, pero asi me mantengo lejos de los limites.
Adjunto un esquema de mi version del puente H


saludos!!

"conozco dos cosas infinitas: el universo y la estupidez humana. Y no estoy muy seguro del primero." A.Einstein

Desconectado rivale

  • Colaborador
  • PIC24H
  • *****
  • Mensajes: 1707
Re: Robot equilibrista
« Respuesta #392 en: 21 de Julio de 2011, 23:41:36 »
Hola Gera, lei el codigo que posteaste y tengo algunas dudas:

Código del PIC
Código: C
  1. #define __PCH__ 1
  2.  
  3. #define N_SAMPLES    64
  4. #define PRELOAD_TMR1 65043 //10 KHz prescaler=1
  5. ...
  6. ...
  7. ...
  8.  
  9. void get_samples()
  10. {
  11.    int i;
  12.    accel_x=0;
  13.    gyro_rate=0;
  14.    
  15.    for(i=0;i<N_SAMPLES;i++)
  16.    {
  17.       set_adc_channel(0);
  18.       delay_us(20);
  19.       accel_x += read_adc();
  20.      
  21.       set_adc_channel(2);
  22.       delay_us(20);
  23.       gyro_rate += read_adc();
  24.    }
  25.    
  26.    accel_x /= N_SAMPLES;
  27.    gyro_rate /= N_SAMPLES;
  28. }
  29.  





tu frecuencia de muestreo es cada 100us  :shock:, esto es posible?, no tiene que realizar todas las operaciones antes de que ocurra la siguiente interrupcion?.

y dentro de la toma de  muestras tienes 2 retardos que en suma te dan 40 us y esto lo realizas "N_SAMPLES"  veces.

que tanto influye el tiempo de muestreo en la respuesta del robot?, yo estoy haciendo un robot de este estilo y lo mas rapido que puedo muestrear es a 5ms, pero ya medi los tiempos de las operaciones y si las realiza todas antes de la siguiente interrupcion, aun asi aun no lo hago funcionar.
« Última modificación: 22 de Julio de 2011, 10:16:45 por rivale »
"Nada es imposible, no si puedes imaginarlo"

Desconectado gera

  • Colaborador
  • PIC24H
  • *****
  • Mensajes: 2188
Re: Robot equilibrista
« Respuesta #393 en: 22 de Julio de 2011, 09:47:43 »
Hola rivale!!
No se de que epoca es ese codigo, ha evolucionado bastante, sobretodo en los ultimos dias jeje. Despues de muchas pruebas descubri q la medicion mejoraba cuando bajaba la frecuencia de muestreo. Este limite me lo ponia el gyro que tiene un ancho de banda limitado (habria q ver en la hoja de datos del tuyo). Al final termine muestreando a aprox 100Hz, con muy buenos resultados.
Ademas para evitar esos delays que me decis, ya no intercalo los canales para leer los sensores, sino que cambio de canal una vez, tomo N_SAMPLES muestras, luego cambio de canal nuevamente vuelvo a muestrear, y asi. Al final solo tengo 4 o 5 retrasos. Te dejo el pedazo de codigo que estoy usando ahora:
Código: C
  1. void get_samples(){
  2.    int i=0;
  3.    
  4.    for(i=0;i<5;i++)
  5.       val[i]=0;
  6.    
  7.    // X_ACC
  8.    set_adc_channel(0);
  9.    delay_us(20);  
  10.    for(i=0;i<N_SAMPLES;i++)
  11.       val[0] += read_adc();
  12.    
  13.    // Y_ACC
  14.    set_adc_channel(1);
  15.    delay_us(20);  
  16.    for(i=0;i<N_SAMPLES;i++)
  17.       val[1] += read_adc();
  18.    
  19.    // Z_ACC
  20.    set_adc_channel(2);
  21.    delay_us(20);  
  22.    for(i=0;i<N_SAMPLES;i++)
  23.       val[2] += read_adc();
  24.    
  25.    // X_RATE
  26.    set_adc_channel(4);
  27.    delay_us(20);  
  28.    for(i=0;i<N_SAMPLES;i++)
  29.       val[3] += read_adc();
  30.    
  31.    // Y_RATE
  32.    set_adc_channel(5);
  33.    delay_us(20);  
  34.    for(i=0;i<N_SAMPLES;i++)
  35.       val[4] += read_adc();
  36.    
  37.    for(i=0;i<5;i++)
  38.       val[i] /= N_SAMPLES;
  39.    //printf("%li,%li,%li,%li,%li,\n",val[0],val[1],val[2],val[3],val[4]);
  40. }

Respondiendo tu pregunta, si tu robot trabaja con un PID, el periodo de muestreo va a influir directamente sobre todas las constantes de este. Si en un futuro cambias ese periodo, vas a tener que tunear nuevamente el PID.
Mucha suerte con tu robot, y me gustaria ir viendo tus avances :wink:
saludos!!

"conozco dos cosas infinitas: el universo y la estupidez humana. Y no estoy muy seguro del primero." A.Einstein

Desconectado rivale

  • Colaborador
  • PIC24H
  • *****
  • Mensajes: 1707
Re: Robot equilibrista
« Respuesta #394 en: 22 de Julio de 2011, 10:02:54 »
gracias por la respuesta gera, yo estoy muestreando a 200 Hz, pero solo tomo una muestra de cada sensor por ciclo(creo que cambiare eso), me parece que mi filtro kalman funciona bien, lo que no se es como sintonozar las constantes del PID, como le hiciste tu para encontrar los valores correctos? o como sabes en que escala deben estar aproximadamente?

como sensores usaba un mma7331(analogico de freescale), pero este murio y ahora voy a probar con uno de analog devices y mi giroscopio es un lpy510.

 voy a probar con un nuevo modelo de robot, ya que con el anterior tuve problemas con los motores y no funciono, en cuanto de muestras de vida subo un video.

Saludos
"Nada es imposible, no si puedes imaginarlo"

Desconectado rivale

  • Colaborador
  • PIC24H
  • *****
  • Mensajes: 1707
Re: Robot equilibrista
« Respuesta #395 en: 22 de Julio de 2011, 10:25:49 »
otra pregunta :oops:, porque lees tantos canales?, por lo que veo lees 2 ejes del gyro y 3 del acelerometro, los usas todos en tu filtro??
"Nada es imposible, no si puedes imaginarlo"

Desconectado gera

  • Colaborador
  • PIC24H
  • *****
  • Mensajes: 2188
Re: Robot equilibrista
« Respuesta #396 en: 22 de Julio de 2011, 11:25:29 »
El PID lo tunee a pura prueba y error jeje. En internet podes buscar métodos para hacer esto. Esta clase de robots solo necesitan control proporcional derivativo, asiq la constante Ki es siempre nula.
Para calcular el angulo solo necesitas leer dos canales: uno del gyro y uno del acelerometro. Podrias leer uno mas del acelerometro yc alcular el angulo como el arctg, pero no es necesario. Yo hice la lectura de los 5 para comprobar que funciona todo ok :P pero ahora leo solo 2.
Saludos!!

"conozco dos cosas infinitas: el universo y la estupidez humana. Y no estoy muy seguro del primero." A.Einstein

Desconectado rivale

  • Colaborador
  • PIC24H
  • *****
  • Mensajes: 1707
Re: Robot equilibrista
« Respuesta #397 en: 22 de Julio de 2011, 11:34:54 »
gracias por la respuesta, esque solo sabia que se podia calcular el angulo con 1 y 2 ejes, pense que con el tercero tambien se podia hacer algo  :P.

Gracias por tu ayuda, seguire trabajando y cuando funcione lo subo.

Saludos
"Nada es imposible, no si puedes imaginarlo"

Desconectado rivale

  • Colaborador
  • PIC24H
  • *****
  • Mensajes: 1707
Re: Robot equilibrista
« Respuesta #398 en: 01 de Agosto de 2011, 16:56:45 »
Hola, pongo un video de las pruebas que hice del filtro kalman, el de la derecha(o arriba porque esta alrevez el video) es la salida del filtro

perdon por la resolucion pero lo grabe con mi telefono, y haciendo malabares para agarrar el carrito y la camara



ahora a sintonizar el PID

P.D. como vas con el tuyo Gera?
"Nada es imposible, no si puedes imaginarlo"

Desconectado gera

  • Colaborador
  • PIC24H
  • *****
  • Mensajes: 2188
Re: Robot equilibrista
« Respuesta #399 en: 01 de Agosto de 2011, 19:49:41 »
El mio esta en el cajon jaja. La facu no me da un respiro para dedicarle tiempo. Veo que tu filtro tiene un leve retraso, fijate si podes arreglar eso porq despues el robot se vuelve muy inestable.
Felicitaciones por los avances y exitos con el proyecto!! ;)
Por cierto, que tamaño va a tener el robot y que motores vas a usar??

"conozco dos cosas infinitas: el universo y la estupidez humana. Y no estoy muy seguro del primero." A.Einstein

Desconectado rivale

  • Colaborador
  • PIC24H
  • *****
  • Mensajes: 1707
Re: Robot equilibrista
« Respuesta #400 en: 01 de Agosto de 2011, 19:58:10 »
El mio esta en el cajon jaja. La facu no me da un respiro para dedicarle tiempo.
si, se lo que es eso :2]


los motores que voy a usar son de este tipo, tienen una velocidad de 200-250 rpms, mi prototipo actual es pequeño, es del tamaño de 2 protoboards de las pequeñas, alimentado con una beteria de 9V, como las de los cochesitos de radiocontrol



como podria corregir ese atraso? :huh:



"Nada es imposible, no si puedes imaginarlo"

Desconectado rivale

  • Colaborador
  • PIC24H
  • *****
  • Mensajes: 1707
Re: Robot equilibrista
« Respuesta #401 en: 13 de Agosto de 2011, 20:27:48 »
aca unos videos de mi robot, ya se queda de pie, pero si lo alejo mucho de la vertical empieza a oscilar mucho





Saludos
"Nada es imposible, no si puedes imaginarlo"

Desconectado gera

  • Colaborador
  • PIC24H
  • *****
  • Mensajes: 2188
Re: Robot equilibrista
« Respuesta #402 en: 27 de Septiembre de 2011, 02:52:25 »
Muy bueno rivale!! No habia visto tu robotito. Felicidades!
En realidad me asomaba por aca para pedir ayuda jeje.
Volvi a sacar a mi robot del cajon, y no logro hacer que se quede en equilibrio por un efecto de histeresis que hay en la medicion del angulo. Estaba probando con un filtro complementario, asiq decidi intentar nuevamente con el filtro de kalman a ver si puedo eliminar ese efecto.
El problema es q el angulo empieza a oscilar descontroladamente. Las mediciones del acelerometro y del gyro son excelentes. Intente jugar un poco con las constantes del filtro pero no hubo caso. Solo logre amortiguar un poco la oscilacion haciendo la constante R_angle=0.0001, pero de a poco comienza a descontrolarse nuevamente.
Les pego el codigo q estoy usando:

Código: C
  1. //filter variables
  2. float P[2][2] = {{ 1, 0 },{ 0, 1 }};
  3. float R_angle = 0.3; //0.6
  4. float Q_angle = 0.001;
  5. float Q_gyro = 0.003;
  6. float q_bias=0.0, angle=0, gyro=0, dt=0.02,acc_angle=0;
  7. float gyro_angle=0;
  8.  
  9. //.....
  10.  
  11. void update_angle()
  12. {
  13.    float y_acc = ((float)val[1]-Y_ACC_OFFSET)/Y_ACC_GAIN;
  14.    float z_acc = -((float)val[2]-Z_ACC_OFFSET)/Z_ACC_GAIN;
  15.    float gyro = -((float)val[3]-X_RATE_OFFSET)/X_RATE_GAIN;
  16.    float acc_angle = atan(z_acc/y_acc);
  17.    
  18.    gyro_angle += gyro * dt;
  19.    
  20.    state_update();
  21.    kalman_update(acc_angle);
  22.    
  23.    if(debug_mode)
  24.       printf("%f,%f,%f,\n",acc_angle,gyro_angle,angle);
  25. }
  26.  
  27.  
  28.  
  29. void state_update()   //Prediccion
  30. {
  31.   float Pdot[4];
  32.  
  33.   Pdot[0] =  - P[0][1] - P[1][0];
  34.   Pdot[1] = -P[1][1];
  35.   //Pdot[2] = -P[1][1];
  36.   //Pdot[3] = Q_gyro;
  37.  
  38.   gyro -= q_bias;
  39.   angle += gyro * dt ;
  40.  
  41.   P[0][0] += (Pdot[0] * dt) + (P[1][1]*dt*dt) + Q_angle;
  42.   P[0][1] += Pdot[1] * dt;
  43.   P[1][0] += Pdot[1] * dt;
  44.   P[1][1] += Q_gyro;
  45. }
  46.  
  47. void kalman_update( float angle_m )
  48. {
  49.   float K_0=0;
  50.   float K_1=0;
  51.  
  52.   K_0 = P[0][0] / (R_angle + P[0][0]);
  53.   K_1 = P[1][0] / (R_angle + P[0][0]);
  54.  
  55.   P[0][0] -= K_0 * P[0][0];
  56.   P[0][1] -= K_0 * P[0][1];
  57.   P[1][0] -= K_1 * P[0][0];
  58.   P[1][1] -= K_1 * P[0][1];
  59.  
  60.   angle += K_0 * (angle_m - angle);
  61.   q_bias += K_1 * (angle_m - angle);
  62. }
  63.  

Alguna idea de por qué pasa eso??
Gracias!

"conozco dos cosas infinitas: el universo y la estupidez humana. Y no estoy muy seguro del primero." A.Einstein

Desconectado rivale

  • Colaborador
  • PIC24H
  • *****
  • Mensajes: 1707
Re: Robot equilibrista
« Respuesta #403 en: 27 de Septiembre de 2011, 20:59:23 »
Hola Gera, acabo de revisar tu codigo y no le encuentro ningun error, deberia de funcionar. me da la impresion de que tu error no esta ahi.
--tu tiempo de muestreo si esta a 20ms?
--como estas viendo la respuesta de tu filtro, talvez al enviarlo a la PC tu muestreo es mas lento.
--como es la medida de tu angulo con tus 2 sensores por separado?.
"Nada es imposible, no si puedes imaginarlo"

Desconectado gera

  • Colaborador
  • PIC24H
  • *****
  • Mensajes: 2188
Re: Robot equilibrista
« Respuesta #404 en: 27 de Septiembre de 2011, 23:15:55 »
Hola Gera, acabo de revisar tu codigo y no le encuentro ningun error, deberia de funcionar. me da la impresion de que tu error no esta ahi.
--tu tiempo de muestreo si esta a 20ms?
--como estas viendo la respuesta de tu filtro, talvez al enviarlo a la PC tu muestreo es mas lento.
--como es la medida de tu angulo con tus 2 sensores por separado?.


Hola rivale! gracias por responder!
El tiempo de muestreo efectivamente es de 20ms. Pero como decis, tengo la sospecha de q ese tiempo varia cuando envio los datos a la PC. La respuesta de los sensores por separado es muy buena.
Ahora estoy usando otra funcion para el filtro de kalman que me parecio mas "elegante" y funciona bastante bien, ya no oscila. Sin embargo el robot sigue sin equilibrarse.
Voy a probar algunas cosas, creo que tengo una pequeña demora en el calculo del angulo y eso hace q no encuentre el equilibrio. Si tengo resultados positivos lo posteo (crucemos los dedos jeje)

Saludos y gracias de nuevo!!

PD: dejo el codigo del filtro por si a alguien le sirve
Código: C
  1. void update_angle(){
  2.    float y_acc = ((float)val[1]-Y_ACC_OFFSET)/Y_ACC_GAIN;
  3.    float z_acc = -((float)val[2]-Z_ACC_OFFSET)/Z_ACC_GAIN;
  4.    float x_rate = -((float)val[3]-X_RATE_OFFSET)/X_RATE_GAIN;
  5.    
  6.    float acc_angle = atan(z_acc/y_acc);
  7.    gyro_angle += x_rate * delta_t;
  8.    angle = kalman_update(x_rate, acc_angle);
  9.    
  10.    if(debug_mode)
  11.       printf("%f,%f,%f,\n",acc_angle,gyro_angle,angle);
  12. }
  13.  
  14.  
  15. // Update the State Estimation and compute the Kalman Gain.
  16. // The estimated angle is returned.
  17. float kalman_update(float gyro_rate, float accel_angle){
  18.     // Inputs.
  19.     float u = gyro_rate;
  20.     float y = accel_angle;
  21.  
  22.     // Output.
  23.     static float x_00 = 0.0;
  24.     static float x_10 = 0.0;
  25.  
  26.     // Persistant states.
  27.     static float P_00 = 0.001;
  28.     static float P_01 = 0.003;
  29.     static float P_10 = 0.003;
  30.     static float P_11 = 0.003;
  31.  
  32.     // Constants.  
  33.    
  34.     // These are the delta in seconds between samples.
  35.     const float A_01 = -0.02;
  36.     const float B_00 = 0.02;
  37.  
  38.     // Data read from 1000 samples of the accelerometer had a variance of 0.07701688.
  39.     const float Sz = 0.07701688;
  40.  
  41.     // Data read from 1000 samples of the gyroscope had a variance of 0.00025556.
  42.     // XXX The values below were pulled from the autopilot site, but I'm not sure how to
  43.     // XXX plug them into the process noise covariance matrix.  This needs to be
  44.     // XXX further explored.
  45.     const float Sw_00 = 0.001;
  46.     const float Sw_01 = 0.003;
  47.     const float Sw_10 = 0.003;
  48.     const float Sw_11 = 0.003;
  49.  
  50.     // Temp.
  51.     float s_00;
  52.     float inn_00;
  53.     float K_00;
  54.     float K_10;
  55.     float AP_00;
  56.     float AP_01;
  57.     float AP_10;
  58.     float AP_11;
  59.     float APAT_00;
  60.     float APAT_01;
  61.     float APAT_10;
  62.     float APAT_11;
  63.     float KCPAT_00;
  64.     float KCPAT_01;
  65.     float KCPAT_10;
  66.     float KCPAT_11;
  67.  
  68.     // Update the state estimate by extrapolating current state estimate with input u.
  69.     // x = A * x + B * u
  70.     x_00 += (A_01 * x_10) + (B_00 * u);
  71.  
  72.     // Compute the innovation -- error between measured value and state.
  73.     // inn = y - c * x
  74.     inn_00 = y - x_00;
  75.  
  76.     // Compute the covariance of the innovation.
  77.     // s = C * P * C' + Sz
  78.     s_00 = P_00 + Sz;
  79.  
  80.     // Compute AP matrix for use below.
  81.     // AP = A * P
  82.     AP_00 = P_00 + A_01 * P_10;
  83.     AP_01 = P_01 + A_01 * P_11;
  84.     AP_10 = P_10;
  85.     AP_11 = P_11;
  86.  
  87.     // Compute the kalman gain matrix.
  88.     // K = A * P * C' * inv(s)
  89.     K_00 = AP_00 / s_00;
  90.     K_10 = AP_10 / s_00;
  91.    
  92.     // Update the state estimate.
  93.     // x = x + K * inn
  94.     x_00 += K_00 * inn_00;
  95.     x_10 += K_10 * inn_00;
  96.  
  97.     // Compute the new covariance of the estimation error.
  98.     // P = A * P * A' - K * C * P * A' + Sw
  99.     APAT_00 = AP_00 + (AP_01 * A_01);
  100.     APAT_01 = AP_01;
  101.     APAT_10 = AP_10 + (AP_11 * A_01);
  102.     APAT_11 = AP_11;
  103.     KCPAT_00 = (K_00 * P_00) + (K_00 * P_01) * A_01;
  104.     KCPAT_01 = (K_00 * P_01);
  105.     KCPAT_10 = (K_10 * P_00) + (K_10 * P_01) * A_01;
  106.     KCPAT_11 = (K_10 * P_01);
  107.     P_00 = APAT_00 - KCPAT_00 + Sw_00;
  108.     P_01 = APAT_01 - KCPAT_01 + Sw_01;
  109.     P_10 = APAT_10 - KCPAT_10 + Sw_10;
  110.     P_11 = APAT_11 - KCPAT_11 + Sw_11;
  111.  
  112.     // Return the estimate.
  113.     return x_00;
  114. }
  115.  

"conozco dos cosas infinitas: el universo y la estupidez humana. Y no estoy muy seguro del primero." A.Einstein


 

anything