[Progetto comune] - Robotica

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.

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:

Spero possa esservi utile!!

Domenica prossima lavoreremo principalmente ai test pratici e in movimento del robot, per studiare il PID che è solo dichiarato:

#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.

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:

#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.

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.

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

per quanto riguarda la ps2 qui spiegano il protocollo

qui trovi delle librerie aggiornate ma che non ho testato personalmente

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 :smiley:

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 :smiley: :-/ i dati pero` andranno ugualmente filtrati che a quanto ho visto sti chippettini ti mandano anche un monte di robaccia.

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!!!!! :smiley: :smiley:

codice aggiornato:

#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'è :smiley: cosa ne dite?

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?.. :wink:
..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

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?

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.

#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?!?!)

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: Gioblu.com is for sale | HugeDomains

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.

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 :wink:

Comunque sia, complimenti per l'idea! Bravo!

senza almeno un accelerometro che determini il reale vettore gravità i nostri amici non potranno mai fare una salita o una discesa.

Utilizzando un semplice sensore ottico da mouse posto al centro, di fatto puoi fare a meno anche di un accelerometro, perchè ti basterà correggere il punto zero sino a quando non rileverai che il tutto si ferma, e con il "mouse" puoi determinare esattamente in quale direzione ti stai spostando e quindi dove si trova "all'incirca" anche il punto di equilibrio... o no? :-?

io ho avuto un'idea che potrebbe tornare utile...
l'idea è questa:
per diminuire l'effetto "beccheggio" potreste (tramite un servo centrale) spostare un peso messo in cima ad un braccetto di lunghezza adok in avanti o indietro in modo da controbilanciare l'inclinazione invece di farlo con le ruote, forse così si riesce a separare il bilanciamento dal movimento, e diventa tutto più semplice. :slight_smile:
Edit:
aggiungendo robe il peso potrebbe cambiare, ma basterebbe rendere il peso mobile sul braccio con un grano di bloccaggio.
altro piccolo problemino potrebbe essere la velocità di reazione del servo, ma adesso ci sono servi abbastanza veloci da poter superare il problema.

Anche il portare in peso da una parte all'altra richiede una capacità di reazione tale che dubito possa funzionare bene... e poi si rischia di peggiorare l'effetto (stiamo parlando di Arduino :wink: )

Me lo immagino che balzella in qua e la con un peso che oscilla nel tentativo di stabilizzare il tutto ;D (molto comico ;D )

Per stabilizzare eventuali oscillazioni può essere utilizzato anche un volano centrale... ma la cosa migliore è un controllo più preciso sulle ruote (maggiore la precisione minore il "beccheggio") e forse l'utilizzo di servi standard non è proprio la soluzione ideale, forse sarebbe meglio dei passo passo di precisione (ma tutto dipende anche dal costo totale :wink: )

Siccome ho visto modelli autobilancianti su questo forum anche di dimensioni notevoli e alti quasi un metro, credo che le richieste per andare bene siano due: la costruzione hardware e il software! :slight_smile:
Comunque da quello che ho visto in giro e' fattibile. Forza.
F

;D ;D per un istante "è partito il filmato" di quello che ha scritto kokiua ;D ;D
però con un servo tipo questo Digital | HITEC RCD USA forse il sistema sarebbe abbastanza rapido (costi permettendo), e poi nelle piattaforme, in mare aperto, si usa una cosa simile, se non sbaglio, per stabilizzarle ::slight_smile:

edit: se disponete di un imu allora qui ho trovato roba che potrebbe interessare (magari l'avete già visto):

Grazie dei complimenti raga ma è proprio una cavolata!! Piu' che altro mi fa piacere di aver creato un oggetto che sta in piedi da solo e corregge in un range di 25 / 30 gradi (totali), con 53 linee di codice :wink:
Un grande grazie va a Martino che come al solito si diverte con me a creare questi oggettini.

Quindi vorreste tiltare le batterie ;D.
E' sorto un mega-dibattito con Martino, proprio sulla problematica del rallentare. Cioè come si puo' rallentare con una struttura in equilibrio senza rovesciarla nella direzione del senso di marcia. Ho notato sperimentalmente che il principale problema di un selfbalancing che esca dalla situazione statica è rallentare la sua corsa per correggere. Nei programmi postati da me in precedenza manca una cosa fondamentale, prendere in considerazione l'inerzia ottenuta da una eventuale accelerazione. Molti di voi avranno impennato o visto moto impennare. C'è chi ha la dote che ci serve, cioè chi sa impennare decelerando, alcuni sanno anche quasi fermarsi ancora in equilibrio. Come? Controbilanciando il momento dato dalla decelerazione inclinando verso la direzione opposta al moto la struttura e quindi il peso. Piu' si vorrà frenare piu' il codino rischiera' di grattare a terra se si vuole rimanere in equilibrio (passatemi la metafora sono un motardista). Credo che il PID da solo non sia in grado di fare questo.

Credo ci voglia un termine da inserire che vari in rapporto a inclinazione * ultimo comando dato ai servi (cioè velocità attuale). Un valore che vari quando velocità e angolo aumentano. Come un motociclista deve saper togliere gas (o meglio frenare) se sta per ribaltarsi indietro e accelerare se sta per appoggiare la ruota e rimanere in quel sottile angolo che mantiene stabile il mezzo fino all'arresto e all'esaurimento della forza di inerzia, tutto questo variando l'intervento in rapporto alla forza di inerzia a cui è soggetto (e quindi calcolare il momento ottenuto dall'eventuale frenata in rapporto alla velocità). Per ora come potrete sperimentare il robot se viene spinto con forza si riposiziona corretto ma non sa che sta andando in una direzione, quindi toglie motore perchè per lui cosi' è ok, di conseguenza per momento dato dalla decelerazione si ri-inclina, questo ciclicamente fino a quando arriva alla massima velocità e non avendo piu' momento imprimibile (accelerazione) cade nel senso di marcia.
Sarei curioso di sapere cosa ne pensa Guido.
Magari muovento il peso si puo' fare le veci del motociclista meglio che semplicemente frenando e accelerando.

Mi fa piacere che vi abbia intrippato la cosa :wink:

ho visto il progetto sul mio rss che segue let's make robots!

Sono la persona meno adatta, non sono mai salito su una moto :slight_smile:
Scherzi a parte, secondo me si può anche fare senza muovere un peso, però devi sapere con una buona precisione tutto quello che succede. Come hai detto anche tu, i sensori IR ti fanno stare sempre parallelo al terreno, che non è necessariamente la stessa cosa di stare dritto.
Con un accelerometro puoi bilanciare il tutto, quando freni si inclina automaticamente indietro per compensare la decelerazione, a lui interessa che la somma vettoriale sia = 0. Se si aggiunge il vettore della decelerazione, si inclina per avere il vettore risultante sempre rivolto verso il centro della terra. Che è poi quello che fa il motociclista con il suo senso dell'equilibrio e i sensori vestibolari.
Questo in teoria, non ho la pratica necessaria per capire quanti accelerometri servono e/o se ci vogliono anche dei giroscopi.
Segaway docet:
http://www.segway.it/default.aspx?tabid=120