Go Down

Topic: [Progetto comune] - Robotica (Read 28729 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

gbm

#95
Jun 22, 2010, 01:12 pm Last Edit: Jun 22, 2010, 01:45 pm by gioscarab Reason: 1
Ciao ragazzi. Scusate l'assenza ma siamo stati presi per approntare l'ecommerce e tutte le questioni burocratiche/legali.

Abbiamo portato avanti con Martino e Sergio il progetto self-balancing introducendo il PID come promesso.

Abbiamo costruito una proto con 3 potenziometri e due pulsanti digitali e le femmine dei pinhead x imu sparkfun. I tre potenziometri vengono utilizzati per regolare il PID in modo simile al sistema di Nicola Lugato, anche se abbiamo utilizzato un metodo differente per l'acquisizione del valore poteziometro e alcune rifiniture.

Il software combina e filtra (tramite filtro di Kalman) l'arco tangente dei due assi dell'accelerometro presi in considerazione e il valore ottenuto dal giroscopio, di conseguenza genera tramite l'algoritmo di correzione PID una reazione al movimento al fine di tornare nella posizione stazionaria.

Per creare il robot abbiamo utilizzato alcuni componenti del kit pico. Come potete vedere l'abbiamo abbassato per renderlo piu' stabile. Sperimentalmente parlando è decisamente opportuno partire da una base di sviluppo bassa e con il baricentro più in basso (rispetto all'asse delle ruote) e il più centrato possibile. Con questa forma sembra essere decisamente agile nei cambi di direzione.

C'è una grossa problematica che è decisamente il delay della reazione che lo porta ad entrare nella sorta di risonanza (vedi video). C'è qualcuno che ha qualche idea o qualche consiglio? Vi posto il codice:
Code: [Select]
#include <Servo.h>
#include <math.h>

float myangle = 0;
float pid = 0;

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

long calibration = 90;
int calibrationUP = 6;
int calibrationDOWN = 5;

#define NUMREADINGSCOEF 10
int readingscoef[3][NUMREADINGSCOEF];
int indexcoef = 0;


/////////////////////////////////////////
#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 = 1;             //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 = .025;


//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.001; //(Kalman)
int ledPin = 13;

byte displayState = 0;    
float oldAngle = 0.0;
Servo left;
Servo right;

void setup() {
 left.attach(10);
 right.attach(9);
 pinMode(ledPin, OUTPUT);
 Serial.begin(19200);
 
 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;
}

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(3); // 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);    // 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(4)- 338; // 338 è un valore utilizzato per regolare i gradi
   ay_m = analogRead(5)- 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  
 
 if (digitalRead(calibrationUP) == HIGH) {
  calibration = calibration + 1;
  delay(100);
 }
 if (digitalRead(calibrationDOWN) == HIGH) {
  calibration = calibration - 1;
  delay(100);
 }
 
 myangle = (angle*57.2957795130823) + calibration;
 if ((millis()-startmillis) > 6000 ) {
   float rate_deg = rate * 57.2957795130823;
   }
   
 readingscoef[0][indexcoef] = analogRead(0);
 readingscoef[1][indexcoef] = analogRead(1);
 readingscoef[2][indexcoef] = analogRead(2);
 
 indexcoef = (indexcoef + 1) % NUMREADINGSCOEF;
 
 int totalcoef[3];
 
 for(int j = 0; j < 3; j++) {
   totalcoef[j] = 0;
   
   for(int k = 0; k < NUMREADINGSCOEF; k++)
     totalcoef[j] += readingscoef[j][k];
     
   totalcoef[j] /= NUMREADINGSCOEF;
 }
 
 float coefP = (float) totalcoef[0] * 5 / 1024;
 float coefI = (float) totalcoef[1] * 5 / 1024;
 float coefD = (float) totalcoef[2] * 5 / 1024;
 
 P = myangle * coefP;
 I = (I + myangle) * coefI;
 
 if(I * myangle < 0)
   I = 0;
 
 D = (myangle - oldAngle) * coefD;
 
 pid = P+I+D;
 
 if (pid >= 90)
  pid = 90;
 
 if (pid <= -90)
  pid = -90;
 
 left.write(90 - pid);
 right.write(90 + pid);
 
 Serial.print("P ");
 Serial.print(coefP);
 Serial.print(" I ");
 Serial.print(coefI);
 Serial.print(" D ");
 Serial.print(coefD);
 Serial.print(" A ");
 Serial.print(myangle);
 Serial.print(" C ");
 Serial.println(calibration);

 
 oldAngle = myangle;
 }
}


Verso la fine del video potete vedere i primi test di avanzamento, con un buon software immaginate che agilità e che effetto un oggettino del genere che gira per la casa. Non vediamo l'ora di finirlo.

E' ancora abbastanza posticcia la posizione della batteria, vista la a-simmetria nella distribuzione del peso dei servomotori (il motore è da un lato non al centro) porta il robot a essere sbilanciato in un senso.

[media]http://www.youtube.com/watch?v=RaN0itBbVR4[/media]


Community robotica / programmazione Arduino
www.gioblu.com

Calamaro

Ragazzi io aggiungerei soltanto una cosa, questo codice e` veramente PRO quindi vi consiglio di leggervelo, studiarvelo e salvarlo da qualche parte. I filtri di Kalman sono utilizzati molto nell'ambito dei processi di acquisizione di dati per ripulire dai disturbi.
Trovo che l'implementazione di questi filtri su un Arduino sia una cosa assolutamente innovativa. Non lo dico solo perche` sono del team, avendo comunque studiato sta roba in linea teorica, vederla applicata mi ha aiutato a capirne le dinamiche.

Federico

Scusate mi sono poco chiare due cose.

@calamaro
hai qualche riferimento/documentazione per il wireless dal controller stile ps2? Si puo' fare anche con moduli 313mhz? Vorrei sperimentare in tal senso...

@gbm
leggo che avete inserito un giroscopio e un accellerometro (o forse uno solo o tutti e due?) ma non ho trovato riferimenti a cosa utilizzate. L'implementazione di questi rende molto piu' costoso il progetto o sbaglio? Possiedo un accellerometro adxl335 se non sbaglio ma non un giroscopio perche' appunto costicchiano...
Cosa usate di preciso? Mi indichi la pagina dove lo spiegate se c'e', perche' non trovo?

Grazie, Fede
Federico - Sideralis
Arduino &C: http://www.sideralis.org
Foto: http://blackman.amicofigo.com

Calamaro

per quanto riguarda la ps2 qui spiegano il protocollo
http://store.curiousinventor.com/guides/PS2#hardware
qui trovi delle librerie aggiornate ma che non ho testato personalmente
http://www.billporter.info/?p=240
qui trovi la libreria che uso io
http://www.arduino.cc/playground/Italiano/ArduinoPsx
Le manopole wireless della ps2 lavorano sui 2GHz e qualcosa, io ho i moduli a 700 e rotti MHz e sono sempre incartati perche` non ho avuto modo di giocarci :D

la IMU di gbm e` un 5 DOF di sparkfun ma credo che tu possa fare un autobalance con semplicemente un giroscopio (cit Gbm).
geometrie. con 5 dof viene bene perche` ti calcoli le velocita` angolari sia tramite accelerometro che tramite giroscopio, in questo modo sai sempre se uno dei due sta mentendo.
Puoi farlo anche con un 3 assi. Devi un attimino sbatterti sulle
Dovrai trovarti la velocita` angolare del corpo del robot, quindi calcolarti il movimento da fare per riportarlo a 0. Magari riprendendo in mano Metodi e Fisica1 ci si potrebbe arrivare :D  :-/ i dati pero` andranno ugualmente filtrati che a quanto ho visto sti chippettini ti mandano anche un monte di robaccia.

gbm

#99
Jun 23, 2010, 02:41 am Last Edit: Jun 23, 2010, 03:20 am by gioscarab Reason: 1
Ragazzi ho trovato il problema, all'alba delle 2.36 AM chiaramente.
Allora nel codice se spulciate c'è float R_angle, questo valore rappresenta la stima di quanto crediamo che sbaglierà il giroscopio, in radianti. Piu' la stima dell'errore è alta piu' il delay di funzionamento della IMU rispetto al movimento è alto, ma allo stesso tempo si riduce l'errore dato da vibrazioni e accelerazioni laterali. In poche parole esiste un setting di float R_angle che permette un equilibrio tra velocità di acquisizione e pulizia del dato acquisito.

X Fede. Nel video c'è un accenno, dormo qualche ora e poi iniziero' la documentazione sul sito.

Incredibile quanto sia preciso questo software, posso utilizzare arduino come un lazzo da cowboy per vari secondi e riposizionando la IMU a 90 gradi rispetto al terreno il valore torna su 90!! Perchè sente l'accelerazione gravitazionale!! E' bellissimo!!!!! :D :D

codice aggiornato:
Code: [Select]
#include <Servo.h>
#include <math.h>

float myangle = 0;
float pid = 0;

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

#define NUMREADINGSCOEF 1
int readingscoef[3][NUMREADINGSCOEF];
int indexcoef = 0;


/////////////////////////////////////////
#define NUMREADINGS 1    //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 = .1;             //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 = .02;


//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.001; //(Kalman)
int ledPin = 13;

byte displayState = 0;    
float oldAngle = 0.0;
Servo left;
Servo right;

void setup() {
 left.attach(10);
 right.attach(9);
 pinMode(ledPin, OUTPUT);
 Serial.begin(19200);
 
 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;
}

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(3); // 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);    // 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(4)- 338; // 338 è un valore utilizzato per regolare i gradi
   ay_m = analogRead(5)- 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  
 myangle = (angle*57.2957795130823) + 101;
 if ((millis()-startmillis) > 6000 ) {
   float rate_deg = rate * 57.2957795130823;
   }
   
 readingscoef[0][indexcoef] = analogRead(0);
 readingscoef[1][indexcoef] = analogRead(1);
 readingscoef[2][indexcoef] = analogRead(2);
 
 indexcoef = (indexcoef + 1) % NUMREADINGSCOEF;
 
 int totalcoef[3];
 
 for(int j = 0; j < 3; j++) {
   totalcoef[j] = 0;
   
   for(int k = 0; k < NUMREADINGSCOEF; k++)
     totalcoef[j] += readingscoef[j][k];
     
   totalcoef[j] /= NUMREADINGSCOEF;
 }
 
 float coefP = (float) totalcoef[0] * 5 / 1024;
 float coefI = (float) totalcoef[1] * 5 / 1024;
 float coefD = (float) totalcoef[2] * 5 / 1024;
 
 P = myangle * coefP;
 I = (I + myangle) * coefI;
 
 if(I * myangle < 0)
   I = 0;
 
 D = (myangle - oldAngle) * coefD;
 
 pid = P+I+D;
 
 if (pid >= 90)
  pid = 90;
 
 if (pid <= -90)
  pid = -90;
 
 left.write(90 - pid);
 right.write(90 + pid);
 
 /*Serial.print("P ");
 Serial.print(coefP);
 Serial.print(" I ");
 Serial.print(coefI);
 Serial.print(" D ");
 Serial.println(coefD);
 Serial.println(myangle);*/
 
 Serial.println(myangle);
 
 oldAngle = myangle;
 }
}


C'è solo una rifinitura che non riesco a inquadrare perfettamente... eventualmente utilizzare il terzo asse dell'accelerometro per avere maggiori info...ecco sempre se ha senso. Ma visto che c'è  :D cosa ne dite?
Community robotica / programmazione Arduino
www.gioblu.com

Saki_Kawa

Ehm.. domanda stupida.. ma non doveva essere un robot lava pavimenti?.. come siete approdati alla soluzione dell'autobalancing e dei filtri di Kalman??.. capisco che Arduino porti a voli pindarici non da poco, ma il progetto iniziale direi che è completamente andato?..  ;)
..ero fiducioso di arrivare qui e trovare qualcosa di utile anche per me, da poter implementare con il sistema di CV che sto sviluppando, per guidare un bel drone lava pavimenti in giro per la casa, ma ora mi aspetto di vedere dei razzi a propulsione e un sistema di navigazione orbitale da un momento all'altro..  ;D

gbm

Ciao Saki. Si indubbiamente ci siamo lasciati prendere dalla matematica, ma la scelta democratica durante i workshop ci ha portati verso questa direzione. Ovviamente, abbiamo utilizzato strumenti matematici che potrebbero risultare utili a progetti come quello citato da te. Infatti l'algoritmo PID è una delle scelte più semplici ed utilizzate per movimentare un drone del genere.
Ma che tipo di robot vorresti realizzare?
Con che tipo di unità motrici?
Community robotica / programmazione Arduino
www.gioblu.com

gbm

#102
Jun 29, 2010, 06:07 am Last Edit: Jun 29, 2010, 07:09 am by gioscarab Reason: 1
Ciao a tutti ragazzi. Questo week-end sono stato inchiodato a Milano per varie vicissitudini familiari, perdendomi clamorosamente Neverland e quindi il grande incontro dedicato (al vino) e alla robotica. Sono sicuro vi siate divertiti!!

Tornando alla risposta di Saki:

"Ehm.. domanda stupida.. ma non doveva essere un robot lava pavimenti?.. come siete approdati alla soluzione dell'autobalancing e dei filtri di Kalman??.. capisco che Arduino porti a voli pindarici non da poco, ma il progetto iniziale direi che è completamente andato?..  

..ero fiducioso di arrivare qui e trovare qualcosa di utile anche per me, da poter implementare con il sistema di CV che sto sviluppando, per guidare un bel drone lava pavimenti in giro per la casa, ma ora mi aspetto di vedere dei razzi a propulsione e un sistema di navigazione orbitale da un momento all'altro..  "

Solo dopo questa risposta ho capito che si doveva partire da una base più semplice, seguendo il principio K.I.S. (keep it simple) ho sviluppato questo tutorial per permettere a chi non c'è stato di capire meglio cosa abbiamo sviluppato in questi workshop:

Questo robot si basa sul kit pico, che è un piccolissimo automa a 3 ruote, con classica ruotina posteriore pivotante. Diciamo che è studiato per dare capacità di movimento nello spazio a una scheda arduino e permettere di approcciare alla robotica economicamente, o almeno l'idea iniziale era quella poi siamo sfociati nel self-balancing e quindi il mio  è stato adattato per questo utilizzo.


Il robot è composto da:
2x ruote per servi/microservi con pneumatico in foam
2x staffe microservi
2x microservi (in questo caso Turborix)
2x sensori ir possibilmente analogici (in questo caso SHARP)
1x Arduino duemilanove
1x layer Pico
1x portabatterie 4xAAA
4x batterie stilo AAA

Questo primo prototipo è in grado solo di mantenere una posizione bilanciata e correggere eventuali errori di inclinazione. Per farlo utilizza due semplicissimi sensori Sharp, che in realtà non sono esattamente studiati per rilevare cosi' brevi distanze (in teoria sono utilizzati per range da 50 a 3cm io li uso tra 3 e 0 ), ma sembrano fare decisamente bene il loro lavoro.

L'algoritmo che ho scritto è semplicissimo: calcolo la distanza dal suolo per ognuno dei due sensori, e sottraggo la prima alla seconda. Se il robot è parallelo al terreno il valore sarà 0 o vicinissimo allo 0 (micro-correzioni). Quindi basterà scrivere: Servo.write(90 + il nostro valore); per ottenere una reazione in rapporto alle due distanze rilevate dai sensori. Cioè il nostro valore sarà negativo se il primo dei due sensori otterrà una rilevazione minore della seconda, o positivo in caso contrario.

Code: [Select]
#include <Servo.h>

Servo left;
Servo right;

#define servoLeftPin     9
#define servoRightPin   10
#define servoZero       81
#define IRFront         0
#define IRBack          1
#define ledPin          13

int frontSense = 0;
int backSense = 0;
int orientation = 0;


void setup() {
pinMode(ledPin, OUTPUT);
left.attach(servoLeftPin);
right.attach(servoRightPin);
left.write(servoZero);
right.write(servoZero);
Serial.begin(9600);
}


void getOrientation() {
 frontSense = 0;
 backSense = 0;
 orientation = 0;
for (int i = 0; i < 10; i++) {
 frontSense = analogRead(IRFront) + frontSense;
 backSense = analogRead(IRBack) + backSense;
 if (i == 9) {
  frontSense = frontSense / 10;
  backSense = backSense / 10;
  orientation = frontSense - backSense;
 }
}
}


void loop() {
getOrientation();

float delta = orientation / 10;
if(delta > 90) delta = 90;
if(delta < -90) delta = -90;

left.write(81 - delta);
right.write(81 + delta);

Serial.print(orientation);
Serial.print("   ");
Serial.println(delta);
}


Qui il video (non riesco a embeddarlo mi embedda quello precedentemente postato?!?!)
http://www.youtube.com/watch?v=18pmm4bZ20c

Ecco tutto, fatto il self-balancing .
Qui dentro ragazzi c'è tutto quello che serve. Se analizzate il primo programma da noi postato sul self-balancing grazie a IMU e questo, sono del tutto simili, ed entrambi utilizzano sistemi fondamentali per noi potenziali creatori di automi. Il valore delta, buttato direttamente in servo.write, è definibile come la P del PID. Infatti il robot è in grado solo di correggere non oltre una quindicina di gradi di inclinazione per lato e chiaramente solo sul posto, non in movimento, proporzionalmente all'errore di inclinazione. La mancanza della I e della D si sentono infatti se il robot accelera in una direzione.

La semplicità di questo sistema permette più o meno a chiunque di poterlo testare con ciarpame di recupero e componenti a costo irrisorio. Con questo bisogna chiaramente prendere atto delle elevate limitazioni di questo sistema, per esempio senza almeno un accelerometro che determini il reale vettore gravità i nostri amici non potranno mai fare una salita o una discesa.

Ho capito, scrivero' un articolo sul PID.  ;D

Qui l'articolo originale: http://www.gioblu.com/index.php?option=com_content&view=article&id=100:self-balancing-robot-per-noobs-p&catid=36:robotica&Itemid=34

Community robotica / programmazione Arduino
www.gioblu.com

guiott

Per farlo utilizza due semplicissimi sensori Sharp, che in realtà non sono esattamente studiati per rilevare cosi' brevi distanze (in teoria sono utilizzati per range da 50 a 3cm io li uso tra a 3 e 0 ), ma sembrano fare decisamente bene il loro lavoro

Complimenti per il lavoro. La capacità di fare cose semplici, ben documentate, alla portata di tutti e che funzionano anche bene non è una cosa banale.

Volevo solo chiarire un attimo il discorso degli Sharp. Non è una magia il fatto che funzionino anche da 0 a 3cm.

Se guardate a pag.9 di questo documento, potete vedere la curva di risposta dei sensori.

http://www.acroname.com/robotics/parts/GP2D120_SS.pdf

Sotto la distanza minima dichiarata il comportamento semplicemente si inverte.

Usandoli nel modo giusto, considerando che la tensione di uscita aumenta all'aumentare della distanza, invece che diminuire come nel range di misura di funzionamento normale, funzionano anche abbastanza linearmente. Tenendo conto che tu li stai usando non come misura assoluta ma relativa tra i due, è una furbata niente male.

Attenzione però che vanno usati esattamente esattamente in quel range, diverso da modello a modello come si può vedere in questo documento:

http://www.acroname.com/robotics/info/articles/sharp/sharp.html

Se analizzi la curva sul documento precedente infatti, puoi vedere che non c'è modo di capire se la distanza è, ad esempio, di 2 o di 5cm. La tensione di uscita è la stessa. Quindi: o sei sicuro che la distanza misurata è tra 0 e 3cm o sei sicuro che è tra 3 e 50cm. Non puoi usarli, ad esempio, tra 0 e 10cm.

kokiua

L'idea dei due sensori è decisamente intrigante, e anche farlo muovere avanti e indietro non è certo un problema visto che ti sarà sufficiente spostare (in proporzione alla velocità che vuoi ottenere)  in + o in - il punto zero nel confronto che fai, facendogli così "rincorrere" il punto di equilibrio che non arriva sino a quando non riporti lo zero nella posizione corretta.
Un pochino più complicato forse sarà il farlo girare, perchè si tratterà di lievissime differenze dello sbilanciamento tra i due servi  ;)

Comunque sia, complimenti per l'idea! Bravo!

Go Up