Go Down

Topic: Progetto quadricottero - Problema Sensori/Controllo motori. (Read 10413 times) previous topic - next topic

darksns

Buona sera a tutti.
Quest'anno dovrò affrontare gli esami di 5° superiore (indirizzo elettronico) e da appassionato che sono ho deciso di portare un quadricottero interamente programmato da zero.
Sia i professori sia altre persone mi hanno detto di lasciar perdere per via della difficoltà ma per me programmare questo quadricottero è un traguardo che mi sono posto e che voglio superare a tutti i costi.

Per prima cosa mi sono letto un sacco di post sia su questo forum che su altri specializzati in modellismo per farmi un'idea di che componenti hardware comprare e una vaga idea di programmarlo.
Dopo che mi sono arrivati tutti i pezzi ed arduino ( visto che non l'ho mai avuto e mai utilizzato ma lo conoscevo grazie ai mille video su youtube dove fanno vedere tutti i progetti) ho iniziato a fare un po pratica con arduino fino ad ora.

In questo momento mi trovo in un punto un po critico. Più precisamente sono riuscito a:

-leggere il segnale ppm che ricevo dalla ricevente (turnigy 9x);
-comandare i motori brushless tramite esc;
-leggere i valore dei vari sensori quali accelerometro a 3 assi, giroscopio a 3 assi e magnetometro a 3 assi. (imu GY-85)

Il problema sta nel fatto di unire il tutto. So, solo teoricamente, che dovrei leggere i dati dai sensori, filtrarli (io nel codice faccio semplicemente una media), trasformare il valore che ricevo (ad esempio dall'accelerometro dovrei ricavare i gradi) e poi tramite un controllo PID dovrei inviare il segnale ai motori.

Io ci sto provando e riprovando scrivendo sempre tutto da zero ma non riesco, da qualche parte sbaglio. Ovviamente non pretendo che me lo facciate voi tutta questa parte ma ho bisogno di una spintarella, di un aiutino per andare avanti.

Ve ne sarei molto grato, manca un mese e mezzo all'esame e trovarmi così bloccato non mi aiuta.
Per qualsiasi altra informazione sul progetto chiedete pure, può essere che mi sia scordato di scrivere qualcosa.

Grazie in anticipo.

p.s. scusate per la pergamena di spiegazioni! :)

ratto93

Ti sei studiato il funzionamento del Multiwii o del multicopter ?
;)
Il primo in particolare è abbastanza di facile comprensione..
Se corri veloce come un fulmine, ti schianterai come un tuono.

darksns

Sisi e non solo questi 2 ma con scarsi risultati.
è scritto in modo molto diverso dal normale codice per arduino e faccio difficoltà a capirlo purtroppo.

ratto93

E' scritto in Ansi C , un linguaggio che si deve usare in tutte quelle applicazioni in cui si devono avere prestazioni notevoli a discapito della semplicità, non ho mai visto un quadricotterro volare con codice scritto in wiring, e se vuoi farne uno devi impararti l'ANSI C
Se corri veloce come un fulmine, ti schianterai come un tuono.

gpb01


E' scritto in Ansi C , un linguaggio che si deve usare ....


Ti correggo ... non è UN linguaggio ... è IL LINGUAGGIO ... che si deve usare  :smiley-mr-green: ]:D :smiley-mr-green: ]:D :smiley-mr-green: ]:D

Guglielmo
Search is Your friend ... or I am Your enemy !

darksns

Ottimo. Grazie della specifica. Mi metto subito sotto. :)

ratto93



E' scritto in Ansi C , un linguaggio che si deve usare ....


Ti correggo ... non è UN linguaggio ... è IL LINGUAGGIO ... che si deve usare  :smiley-mr-green: ]:D :smiley-mr-green: ]:D :smiley-mr-green: ]:D

Guglielmo

Dettagli :)
Se corri veloce come un fulmine, ti schianterai come un tuono.

gpb01

Search is Your friend ... or I am Your enemy !

astrobeed


Dettagli :)


Si, come i 100 e passa miliardi di Euro che lo stato deve ai vari fornitori  :smiley-mr-green:
Scientia potentia est

darksns

Ho iniziato a vedermi l'ansi C ma è un argomento troppo grande per impararlo in meno di un mese.  :(
Non c'è un modo per scriverlo in wiring? Anche se non ha prestazioni alte non importa per ora. Per l'esame vorrei portarlo che fa un minimo di volo stabilizzato. Poi quando imparerò bene l'ansi c lo faccio meglio.

SUBSEA

Bè potresti far volare comandando solo i motori, ma alla prima brezza lo trovi in testa ad un tuo prof, e non so fino a che punto gli possa piacere... 8) altrimenti comici a studiati l'ambiente e usare giroscopi e accelerometri...

darksns

#11
Apr 23, 2013, 08:40 pm Last Edit: Apr 23, 2013, 08:44 pm by darksns Reason: 1
Sta sera tiro giù un po di codice e mi dite voi se va bene e se si può migliorare.  :)

EDIT: ho notato che comandando i motori con la libreria servo, ho dei cali di velocità a tempo quasi costante.. cosa potrebbe essere?

lestofante


Ho iniziato a vedermi l'ansi C ma è un argomento troppo grande per impararlo in meno di un mese.  :(
Non c'è un modo per scriverlo in wiring? Anche se non ha prestazioni alte non importa per ora. Per l'esame vorrei portarlo che fa un minimo di volo stabilizzato. Poi quando imparerò bene l'ansi c lo faccio meglio.


il Wiring è un'insieme di librerie/classi in c/c++ che ti semplificano la vita.
Se sei riuscito a leggere i sensori, leggere la ricevente (con interrupt o con pulseIn?) e comandare gli ESC direi che sai già programmare abbastanza per concludere qualcosa. Certo non in tempo per la maturità, ma almeno l'algoritmo di stabilizzazione dovresti completarlo.

Se segui questo mio progetto: https://github.com/lestofante/SerialToDCM
vedrai il codice della "Sensor fusion", ovvero l'agoritmo che dai sensori tira fuori un quaternione di rotazione. (NON li ho scritti io, i crediti sono nel nome e nei commenti)
https://github.com/lestofante/SerialToDCM/tree/master/Original%20C%20DCM%20code

il codice (java) si aspetta che invii la lettera M, A o G (se non hai un magnetometro fa niente! devi solo commentare un if e decommentare un altro un SerialReader.java), seguito da 2 byte che rappresentano il valore int "grezzo" letto dal sensore. Poi in myGame/DCMLogic.java devi cambiare il valore LSBtoRadiant che traduce il valore del giroscopio da valore letto in radiati al secondo con quello del tuo giroscopio. Per gli altri sensori non serve perchè vengono normalizzati, però devi sottrarre l'eventuale valore di "zero". Comunque questi dati li trovi sul datasheet del sensore.

Considera che passando i dati via seriale, o metti un baud-rate molto alto (e fuori standard, quindi non è detto che funzioni) come 460800 o più, altrimenti noterai come le prestazioni degradano. (anche se l'algoritmo che uso è molto buono anche a pochi Hz, il tizio che l'ha inventato lo usava a 10Hz, se non erro, con ottimi risultati

Quote
EDIT: ho notato che comandando i motori con la libreria servo, ho dei cali di velocità a tempo quasi costante.. cosa potrebbe essere?

codice? comunque in generale è perchè stai giocando con i timer.. magari usi il PWM? o un timer per leggere i PPM?
Guida per principianti http://playground.arduino.cc/Italiano/newbie
Unoffical Telegram group https://t.me/genuino

darksns

#13
Apr 23, 2013, 11:16 pm Last Edit: Apr 24, 2013, 12:05 am by darksns Reason: 1
Quante informazioni che mi hai dato. Grazie mille. :) ora vedo tutto. Ma spiegami una cosa.. cosa centra java con l'arduino? O.o

Comunque per i motori il codice è leggere il segnale con un pulseIn poi uso il comando map per portarlo da 51(vel minima) e 160( vel massima) e gli invio all'esc il segnale con motore1.write(vel).

Tra poco ti posto il codice completo di tutto. Dammi un attimo che lo metto a posto un pochino :)

EDIT: ecco un po di codice.. non guardate quello che c'è nel loop.. era per fare test con la seriale.
Code: [Select]
#include <Wire.h>
#include <I2Cdev.h>
#include <ADXL345.h>
#include <Servo.h>

//dichiarazioni
Servo motore_a_dx,motore_a_sx,motore_d_dx,motore_d_sx;
ADXL345 accel;

//variabili
int maxvel=0;
int minvel=0;
int media[3];
int vel=0;
int tempo_prima=0;
int tempo=0;
int ax_precedente;
float P = 0;                                 
float I = 0;                               
float D = 0;                             
float PID = 0;                               
float kP = 2;                               
float kI = 5;                           
float kD = 1;
int16_t ax, ay, az;

void setup(){
  Wire.begin();
  //Serial.begin(9600);
 
  // settaggio pin
  pinMode(4,INPUT);
  pinMode(13,OUTPUT);
 
  //motori
  motore_a_dx.attach(11);   //     ( 10 )   ( 11 )   
  motore_a_sx.attach(10);   //          -QQQ-
  motore_d_dx.attach(9);    //          -QQQ-
  motore_d_sx.attach(6);    //     ( 06 )   ( 09 )
 
  accel.initialize();
 
  //offset_radio();
 
  //arm();
 
}

void loop(){
 
  lettura_vel(); //leggo valore dal radiocomando
  imposta_vel_tutti(vel); // imposto la velocità letta a tutti i motori
  pid();
 
}

void offset_radio(){
 
  // inizializzazione visiva settaggio radio (OFFSET)
  digitalWrite(13,LOW);
  delay(200);
  digitalWrite(13,HIGH);
  delay(2000);
  digitalWrite(13,LOW);
  for(int i=0;i<=3;i++){
    media[i]=pulseIn(4,HIGH);
  }
  maxvel=(media[0]+media[1]+media[2]+media[3])/4;
  delay(200);
  digitalWrite(13,HIGH);
  delay(2000);
  digitalWrite(13,LOW);
  for(int i=0;i<=3;i++){
    media[i]=pulseIn(4,HIGH);
  }
  minvel=(media[0]+media[1]+media[2]+media[3])/4;
  delay(200);
  digitalWrite(13,HIGH);
  delay(200);
  digitalWrite(13,LOW);
  delay(200);
 
}

void lettura_vel(){
  //lettura velocità radio
  for(int i=0;i<=3;i++){
    media[i]=pulseIn(4,HIGH);
  }
  vel=(media[0]+media[1]+media[2]+media[3])/4;
  if(vel<minvel){
   vel=minvel;
  }
  if(vel>maxvel){
   vel=maxvel;
  }
  vel=map(vel,minvel,maxvel,51,160);
}

void imposta_vel_tutti(int x){

    motore_a_sx.write(x);
    motore_a_dx.write(x);
    motore_d_sx.write(x);
    motore_d_dx.write(x);
   
}

void imposta_vel(int x,char motore[]){

  if(motore="avanti-destro"){
   motore_a_dx.write(x);
  }
 
  if(motore="avanti-sinistro"){
   motore_a_sx.write(x);
  }
 
  if(motore="dietro-destro"){
   motore_d_dx.write(x);
  }
 
  if(motore="dietro-destro"){
   motore_d_sx.write(x);
  }
 
}

void arm(){
 
  imposta_vel_tutti(160);
  delay(2000);
  imposta_vel_tutti(51);
  delay(2000);
 
}

void agg_acc(){
 
  accel.getAcceleration(&ax, &ay, &az);
 
}

void pid(){
 
  agg_acc();
  tempo_prima = tempo;   
  tempo = millis();
  float intervallo = tempo - tempo_prima;     
 
  P = ax * kP;                                         
  I = I + intervallo * kI * P;                               
  D = kD * ( ax - ax_precedente ) / intervallo;   
 
  PID = P + I + D;                                             
 
  if( PID >= 255/2 ) PID = 255/2;                           
  if( PID <= -255/2 ) PID = -255/2;                         

  ax_precedente=ax;
 
  if(PID <= 2 && PID > 0) PID = 0;                           
  if(PID >= -2 && PID < 0) PID = 0;                               
   
}

astrobeed



Code: [Select]

void pid(){
 
 agg_acc();
 tempo_prima = tempo;  
 tempo = millis();
 float intervallo = tempo - tempo_prima;      
 
 P = ax * kP;                                          
 I = I + intervallo * kI * P;                              
 D = kD * ( ax - ax_precedente ) / intervallo;  
 
 PID = P + I + D;                                            
 
 if( PID >= 255/2 ) PID = 255/2;                          
 if( PID <= -255/2 ) PID = -255/2;                        

 ax_precedente=ax;
 
 if(PID <= 2 && PID > 0) PID = 0;                          
 if(PID >= -2 && PID < 0) PID = 0;                              
   
}



Questo non è un pid.
Scientia potentia est

Go Up