Go Down

Topic: [Progetto comune] - Robotica (Read 27 times) previous topic - next topic

Calamaro

Quote
Ciao a tutti. Ci troviamo domenica alle 15 per lavorare sulla piattaforma. Viste le richieste penso lavoreremo su un sistema di selfbalancing applicabile al robot che abbiamo sviluppato.
Vi aspettiamo


[media]http://www.youtube.com/watch?v=qq3vQ9mD8dM[/media]
Da Pistoia con amore :P

gbm

#91
May 20, 2010, 07:33 am Last Edit: May 20, 2010, 07:35 am by gioscarab Reason: 1
Ciao Calamaro. Mi fa piacere vedere che non solo a Milano si sperimenta  :D. Ottimo ma che codice hai usato? Usi il filtro di Jordi?

Aggiornamento workshop:
Scusate se posto solo Giovedi' ma sto impazzendo di lavoro.
Domenica scorsa abbiamo lavorato a questo codice:
Code: [Select]
#include <Servo.h>
int sensorPin = 3;    // asse x accelerometro
int ledPin = 13;      //led pin (inutile)
double orientation = 0;  // orientamento attuale
double correctPosition = 0; // orientamento corretto
double primoGiro = 0; // val per calibrazione iniziale
unsigned int giri = 0;
Servo left;
Servo right;

void setup() {
 Serial.begin(9600);
 pinMode(ledPin, OUTPUT);  
 left.attach(10);
 right.attach(9);
 correctPosition = calcolo(); //calcolo posizione corretta
 //mantenere il robot parallelo al terreno
}

double calcolo() {
 double position = 0;
 for (int i = 0; i < 100; i++) {
 position = analogRead(sensorPin) + position;
}
position = position / 100;
return position;
} // facciamo una media grossolana

void loop() {
orientation = calcolo();
int delta = orientation - correctPosition;
delta = delta * abs(delta) / 3.75; //delta per se stesso per ottenere valore esponenziale (3.75 è il threshold, potete usare un trimmer viene comodo)
if (delta > 90) {
  delta = 90;
}
if (delta < -90) {
  delta = -90;
}
left.write(90 - delta);
right.write(90 + delta);
Serial.print("////posizione corretta////");
Serial.print(correctPosition);
Serial.print("////delta////");
Serial.print(delta);
Serial.print("///orientation////");
Serial.print(orientation);
Serial.println();
}


Quello che manca in questo codice è l'algoritmo di over control che permetta al robot di tornare in posizione corretta fermandosi (cosa che cosi' non sa fare, sa solo tornare corretto e correre in avanti :P).

E soprattutto il filtro di Kalman che permetta lo smoothing della variabile e la sua eventuale interpolazione con gli altri due assi dell'accelerometro.

Domenica lavoreremo proprio su questo.
Speriamo con l'incontro di riuscire a farlo stare in piedi correttamente  ;D

Visto che Sergio e Agostino mi chiedevano se era possibile avere una visualizzazione grafica dell'output accelerometro ho sviluppato questo veloce tutorial:http://www.gioblu.com/index.php?option=com_content&view=article&id=90:grafico-con-processing&catid=38:programmazione&Itemid=7
spero possa esservi utile.


Community robotica / programmazione Arduino
www.gioblu.com

Calamaro

Mi sono accorto ora che te usi un 90- delta io invece ho usato un map. del tipo

Code: [Select]
 if(angle*57.295779513082<20){
   stica = map(angle*57.295779513082, -50,14,0,90);
   leftservo.write(stica);  
   rightservo.write(stica);
   Serial.println(stica);
 }
 if(angle*57.295779513082>20){
   stica = map(angle*57.295779513082, 14,78,90,180);
   leftservo.write(stica);  
   rightservo.write(stica);
   Serial.println(stica);
 }

comunque ho usato il filtro di kalman dell'esempio di jordi solo che lui ha anche un giroscopio, io un l'ho :(

gbm

Hahahah stica mi piace come variabile.
Sei riuscito a risolvere il problema di oscillazione che si vede nel video?
In teoria con il filtro dovresti riuscire.
Community robotica / programmazione Arduino
www.gioblu.com

gbm

#94
Jun 01, 2010, 07:04 am Last Edit: Jun 01, 2010, 07:06 am by gioscarab Reason: 1
Ciao a tutti ragazzi. Come promesso ci siamo trovati e abbiamo lavorato al filtro di Kalman. Devo ringraziare msx per i consigli utilissimi che ci ha dato per lo sviluppo del codice e chiaramente tutti i presenti che ci hanno lavorato.  

Siamo partiti dal codice di msx e lo abbiamo adattato, studiato e commentato in modo da funzionare con la IMU main board di sparkfun. Cosa che da un'attenta ricerca sul web sembra risultare a molti ostica (c'è davvero poco al riguardo e mal documentato).

Visto che il tempo è finito subito (e le chiacchiere non sono mancate, ammettiamolo  :))  ci siamo dilettati solo a test statici del sistema. Questo sembra essere davvero preciso.

Ho scritto un articolo riassuntivo sull'incontro:
http://www.gioblu.com/index.php?option=com_content&view=article&id=98:veloce-guida-al-filtro-di-kalman&catid=38:programmazione&Itemid=7http://www.gioblu.com/index.php?option=com_content&view=article&id=98:veloce-guida-al-filtro-di-kalman&catid=38:programmazione&Itemid=7

Spero possa esservi utile!!

Domenica prossima lavoreremo principalmente ai test pratici e in movimento del robot, per studiare il PID che è solo dichiarato:
Code: [Select]
#include <Servo.h>
#include <math.h>

Servo leftServo;
Servo rightServo;


/////////////////////////////////////////
#define NUMREADINGS 5       //Filtro spurie Giro [media di 5 valori in questo caso]
int readings[NUMREADINGS];  //Lettura del giroscopio
int index = 0;              //Index della lettura corrente
int total = 0;              //Il totale
int average = 0;            //La media
int inputPin =0;            //Gyro Analog input
///////////////////////////////////////

float dt = .1;              // .06; //( 1024.0 * 256.0 ) / 16000000.0; (Kalman)  // what does this means ?? hand tuned
 int mydt = 20;            //in ms.
 static float PP[2][2] = { //(Kalman)
   {
     1, 0   }
   ,                       //(Kalman)
   {
     0, 1   }
   ,                       //(Kalman)
};                          //(Kalman)


//I due stati, angolo ed errore sistematico del giroscopio. Come sottoprodotto
//della computazione dell'angolo, otteniamo anche la velocità angolare pulita
//dall'errore [bias]

float angle = 0.0;         //(Kalman)
float q_bias = 0.0;        //(Kalman)
float rate = 0.0;          //(Kalman)
float q_m = 0.0;           //(Kalman)

double ax_m=0;
double ay_m=0;
int cnt = 0;               //Counter
unsigned long lastread=0;
unsigned long startmillis=0;

//R rappresenta il valore della convarianza della lettura.
//In questo caso utiliziamo una matrice 1x1 che produce un' ipotesi
//di distorsione della lettura di 0.3 rad

float R_angle = .3;


//Q è una matrice 2x2 in cui viene processata la covarianza.
//Questa struttura ci permette di capire quanto, in rapporto alla
//covarianza stessa, è possibile fidarci dell'accelerometro o del giroscopio

static const float Q_angle = 0.001; //(Kalman)
static const float Q_gyro = 0.003; //(Kalman)
int ledPin = 13;

byte displayState = 0;    
float oldAngle = 0.0;

float P = 0.0; //dichiarazione P
float I = 0.0; //dichiarazione I
float D = 0.0; //dichiarazione D

void setup() {
 pinMode(ledPin, OUTPUT);
 Serial.begin(9600);
 
 for (int i = 0; i < NUMREADINGS; i++)
   readings[i] = 0;                      // initialize all the readings to 0 (gyro average filter)
   startmillis = millis();
 }
 float sqr(float a) {
  return a*a;  
 }
 float sgn (float a) {
   if (a > 0)
       return +1;
   else
       if (a < 0)
           return -1;
   else
       return 0;
}

 long readMAX127(byte chan) {
 byte control = 0x80 + (chan << 4);
 byte addr = 0x28;
 return 0;
}


void loop()
{
 int delta = millis()-lastread;
 if( delta >= mydt) {    //lettura ogni dt ms -> 1000/dt hz.
   lastread = millis();
   
   total -= readings[index];        // Sottrai la scorsa lettura giroscopio
   readings[index] = analogRead(2); // Lettura giroscopio
   total += readings[index];        // Aggiungi lettura al totale
   index = (index + 1);             // Aggiornamento indice

   if (index >= NUMREADINGS)        // Se siamo alla fine dell'array
     index = 0;                     // Torniamo all'inizio

   average = (total / NUMREADINGS)-500;    // Calcoliamo la media dell'input

   dt = ((float)delta) / 1000.0;

   q_m= ((float)average)*(1500.0/1024.0)*PI/180 ;  // HAC remove 1.5 mult

   // Togliamo le spurie dal giroscopio
   const float q = q_m - q_bias;  //(Kalman)

   const float Pdot[2 * 2] = {
     Q_angle - PP[0][1] - PP[1][0],  /* 0,0 */ //(Kalman)
     - PP[1][1], /* 0,1 */
     - PP[1][1], /* 1,0 */
     Q_gyro /* 1,1 */
   };

   // Salviamo in rate la stima del valore giroscopio pulito
   rate = q; //(Kalman)

   /*
// Aggioniamo la stima dell'angolazione

   /* Aggiorniamo la matrice della covarianza*/
   PP[0][0] += Pdot[0] * dt; //(Kalman)
   PP[0][1] += Pdot[1] * dt; //(Kalman)
   PP[1][0] += Pdot[2] * dt; //(Kalman)
   PP[1][1] += Pdot[3] * dt; //(Kalman)


   //INSERIAMO QUI I PIN DEI DUE ASSI DELL'ACCELEROMETRO
   ax_m = analogRead(3)- 338; // 338 è un valore utilizzato per regolare i gradi
   ay_m = analogRead(4)- 338; // idem
   const float angle_m = atan2( ay_m, ax_m ); //(Kalman)
   const float angle_err = angle_m - angle;   //(Kalman)
   const float C_0 = 1;                       //(Kalman)
   const float PCt_0 = C_0 * PP[0][0];        //(Kalman)
   const float PCt_1 = C_0 * PP[1][0];        //(Kalman)
   const float E =R_angle+ C_0 * PCt_0;       //(Kalman)
   const float K_0 = PCt_0 / E;               //(Kalman)
   const float K_1 = PCt_1 / E;               //(Kalman)
   const float t_0 = PCt_0;
   /* C_0 * P[0][0] + C_1 * P[1][0] (Kalman) */
   const float t_1 = C_0 * PP[0][1];
   /* + C_1 * P[1][1]  = 0 (Kalman) */
   PP[0][0] -= K_0 * t_0;                     //(Kalman)
   PP[0][1] -= K_0 * t_1;                     //(Kalman)
   PP[1][0] -= K_1 * t_0;                     //(Kalman)
   PP[1][1] -= K_1 * t_1;                     //(Kalman)
   angle += K_0 * angle_err;                  //(Kalman)
   q_bias += K_1 * angle_err;                 //(Kalman)

 //VALORE DI CALIBRAZIONE
 float calibration = -4.5;
 float myangle=(angle*57.2957795130823)-90.0 + calibration;
 if ((millis()-startmillis) > 6000 ) {
   digitalWrite(ledPin, HIGH);
   float rate_deg = rate * 57.2957795130823;
   // myangle is up at 0
   // motors is stopped at 128
   }
   oldAngle = myangle;
   Serial.print(" Angolo ");  
   Serial.print(int(angle_m * 57.295779513082) + 180, DEC);
   Serial.print(" Accelerometro Y ");
   Serial.print(ay_m, DEC);
   Serial.print(" Accelerometro X ");
   Serial.print(ay_m, DEC);
 }
   digitalWrite(ledPin, LOW);
}


Questo codice manda via seriale il dato pulito dell'inclinazione analizzando due assi accelerometro e il giroscopio.

Community robotica / programmazione Arduino
www.gioblu.com

Go Up