[RISOLTO] Software robot autobilanciante

Buongiorno

Premetto che sono nuovo del forum e quindi mi scuso già in anticipo se sbaglio qualcosa.

Da un po di mesi ho iniziato a costruire un robot autobilanciante composto da un giroscopio mpu6050 e da due motori passo passo controllati dai driver drv8825, il tutto l'utilizzo di Arduino nano.
Il mio problema e che non riesco a capire come fare il codice con questi tipi di motorini. Mi spiego meglio, ho già visto altri progetti uguali al mio solo che utilizzano, la maggior parte motori dc, e quindi il codice non è comparabile alle mie esigenze.

Il problema principale é che per far funzionare un motore dc basta scrivere un digitalWrite() e il motore funziona, invece nei stepper motor bisogna dare impulsi alternati da una pausa (che andrà poi ad essere la velocità) e in questo modo non so piú dove andar inserire il calcolo PID e i cambi di direzione. :o
Vorrei solo capire come riuscire a impostare il programma.
Grazie in anticipo per la risposta e arrivederci.

Basta UNA presentazione, quella che hai già messo QUI ... inutile scriverla più volte :smiley:

Guglielmo

Scusa non me ne sono accorto... Pensavo di non averla messa

buongiorno aggiorno la situazione:
ho provato di nuovo a scrivere un codice (o comunque l’inizio) per questo robot e questa volta lo ho impostato mettendo il calcolo PID tra gli impulsi dei motori passo e calcolando quindi il tempo da trascorrere.

Per capirci meglio vi posto il codice:

/*********************************************************
   Robot autobilancing
 *********************************************************/

#include <Wire.h>
#include <PID_v1.h>

//valori giroscopio
#define MPU 0x68  // I2C address of the MPU-6050
double AcX, AcY, AcZ;
int Roll;

//Define Variables we'll be connecting to
double Setpoint, Input, Output;
double TIME_PID, TIME_PID_NOW, TIME_INT_PID = 20; //inserire valore millisecondi 1:45 tempo di caduta
double TIME_COUNTER_LOOP;

//Specify the links and initial tuning parameters
double  Kp = 4,
        Ki = 0,
        Kd = 0;

//pin motori passo-passo
#define pinDirDX   2
#define pinStepDX  3
#define pinDirSX   4
#define pinStepSX  5

//pin motori passo-passo (velocità)
int VEL = 0;

#define STEP_MOTOR 5 //STEPPING PRIMA DI AGGIUSARE LA VELOCITà

//vallori velocita da impostare
#define vMax 40
#define vMin 450

#define M0d  7
#define M1d  6
#define M2d  8
#define M0s  10
#define M1s  9
#define M2s  11

//pin sensore ultrasuono
#define echoPort    14
#define triggerPort 15


PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

void setup() {
  Serial.begin(9600);

  //motori passo-passo
  pinMode( pinDirSX, OUTPUT );
  pinMode( pinDirDX, OUTPUT );
  pinMode( pinStepSX, OUTPUT );
  pinMode( pinStepDX, OUTPUT );

  //motori passo-passo (velocità)
  pinMode( M0s, OUTPUT );
  pinMode( M1s, OUTPUT );
  pinMode( M2s, OUTPUT );
  pinMode( M0d, OUTPUT );
  pinMode( M1d, OUTPUT );
  pinMode( M2d, OUTPUT );

  //settaggio mortori
  digitalWrite(M0s, LOW);
  digitalWrite(M1s, LOW);
  digitalWrite(M2s, LOW);
  digitalWrite(M0d, LOW);
  digitalWrite(M1d, LOW);
  digitalWrite(M2d, LOW);

  digitalWrite(M2s, HIGH);
  digitalWrite(M2d, HIGH);
  //sendore ultrasuoni
  pinMode(triggerPort , OUTPUT);
  pinMode(echoPort    , INPUT);

  //initialize the variables we're linked to
  Setpoint = 0;

  //turn the PID on
  myPID.SetMode(AUTOMATIC);
  //Input = abs(FunctionsMPU());
  myPID.SetSampleTime(200);

  //inizializazione giroscopio
  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  delay(1000);
}

double FunctionsMPU() {
  double DatoA, DatoB, Value;
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); // request a total of 14 registers
  AcX = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  AcY = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  DatoA = AcX;
  DatoB = (AcY * AcY) + (AcZ * AcZ);
  DatoB = sqrt(DatoB);
  Value = atan2(DatoA, DatoB);
  Value = Value * 180 / PI; //trasformare Rad in °
  return (int)Value;
}
/*
  void motorMove(){
  delayMicroseconds(abs(velocita + TIME_PID));
  TIME_PID=millis();
  do{
    digitalWrite( pinStepSX, HIGH );
    digitalWrite( pinStepDX, HIGH );
    delayMicroseconds(velocita);
    digitalWrite( pinStepSX, LOW );
    digitalWrite( pinStepDX, LOW );
    delayMicroseconds(velocita);
    
    TIME_PID_NOW=millis()-TIME_PID;
    /
  }while(TIME_PID_NOW<=TIME_INT_PID);
  }*/


float PIDcalc() {
  
  TIME_PID = micros();
  Input=abs(FunctionsMPU());
  //Input = 89 - abs(FunctionsMPU());
  myPID.Compute(); //PID COUNTER
  
  //VEL = map(constrain(Output , 0 , 255) ,0 ,255 ,vMax ,vMin);
  VEL = vMin - (map(constrain(Output , 0 , 255) , 0 , 255 , vMax , vMin) - vMax);
  if (Roll < 0) {
    digitalWrite( pinDirSX, LOW );
    digitalWrite( pinDirDX, HIGH );
  } else if (Roll > 0) {
    digitalWrite( pinDirSX, HIGH );
    digitalWrite( pinDirDX, LOW );
  }
  
  return TIME_PID -= micros();
}

void motorSetMove() {
  do {
    
    Serial.println(Output);
    
    digitalWrite( pinStepSX, HIGH );
    digitalWrite( pinStepDX, HIGH );
    delayMicroseconds(VEL - abs(PIDcalc()));
    digitalWrite( pinStepSX, LOW );
    digitalWrite( pinStepDX, LOW );
    delayMicroseconds(VEL - abs(PIDcalc()));
  } while (FunctionsMPU() != 0);
  
}

void loop() {
  
  motorSetMove();
}

Il problemi principali sono due:

  1. il primo è che non posso controllare il tempo tra un calcolo PID e il successivo

  2. il secondo è che attualmente il mio risultato di quando calcolo il PID viene completamente senza senso e il 90 percento delle volte è zero

A questo punto non so se devo cambiare completamente l’impostazione del programma perché probabilmente sto sbagliando tutto, compreso il calcolo del PID, oppure continuare così …

grazie per la risposta e arrivederci

Perché per controllare gli stepper non usi l'apposita libreria "stepper" che ti semplifica la vita ? ? ?

Guglielmo

gpb01:
Perché per controllare gli stepper non usi l'apposita libreria "stepper" che ti semplifica la vita ? ? ?

Guglielmo

Grazie per la risposta,

Solo che la libreria stepper utilizza due pin per il funzionamento del motorino e il mio driver ha solo il pin step per il controllo (c'é anche il pin dir ma non serve a questo).

Stai utilizzando "Big Easy Driver" ? ... mi sembra di riconoscere il codice ... ::slight_smile:

In ogni caso, per semplificarti la vita ed il codice, scriviti allora delle semplici funzioni che richiami come fossero funzioni di libreria :wink:

Guglielmo

gpb01:
Stai utilizzando "Big Easy Driver" ? ... mi sembra di riconoscere il codice ... ::slight_smile:

In ogni caso, per semplificarti la vita ed il codice, scriviti allora delle semplici funzioni che richiami come fossero funzioni di libreria :wink:

Guglielmo

Buongiorno,
Io come pilotatori di motorini passo passo uso i driver drv8825 (comprati su Amazon) e al momento il problema principale é che non so come fare in modo di "attivare" i motorini come fossero dei motori dc e non dovendo sempre ripetere il codice:

digitalWrite(motorSX, HIGH);
digitalWrite(motorDX, HIGH);
delay(tempo);
digitalWrite(motorSX, LOW);
digitalWrite(motorDX, LOW);
delay(tempo);

Grazie e arrivederci

La fregatura che con un motore DC puoi dosare la coppia incrementando o riducendo il PWM, ma uno stepper ha coppia che si riduce con l'aumento della velocità.

A te serve un timer hardware da programmare in modo che generi un segnale ad onda quadra (duty cicle 50%) con frequenza variabile da software. Il timer hardware genera questo segnale senza impegnare la CPU e la libreria accelestepper mi pare che usi uno dei timer disponibile nell'atmega.

Poi vedo che il segnale di feedback non proviene da un sensore di posizione (encoder ecc) ma dal giroscopio e questo complica almeno per me le cose in quanto non ho mai realizzato un pid in questo modo.

Sostanzialmente il pid, tralasciando la i (integrative) e la d (derivative) e lasciano solo il proporzionale ricava l'errore come Perror = Setpoint - Input.

Posta un link al progetto simile al tuo che però usa motori DC, così vedo di dargli una occhiata.

Ciao.

Grazie mille per la risposta molto dettagliata maurotec.

Maurotec:
Poi vedo che il segnale di feedback non proviene da un sensore di posizione (encoder ecc) ma dal giroscopio e questo complica almeno per me le cose in quanto non ho mai realizzato un pid in questo modo.

Non ho capito solo cosa intendi per sensore di posizione se devo completamente togliere il giroscopio o devo cambiare i calcoli.

Prima di tutto grazie per il grande aiuto che mi stai dando :smiley: poi ho trovato una guida su come costruire robot autobilancing con motori dc che ti lascio al fondo e poi ho trovato anche un codice che sembra attendibile qui sul forum di Arduino: QUI (nell’ultima pagina del forum c’é il codice completo)

Grazie e arrivederci! :wink:

ProgettoSegway (1).pdf (1.54 MB)

Buongiorno a tutti, :slight_smile:

Forse oggi ho buone notizie

Nel mentre che cercavo su internet (dopo tanto tempo) forse ho trovato il modo, usando la libreria accelstepper(come diceva maurotec) , di pilotare i motori passo passo come se fossero dc.

Vi lascio al fondo il file dove a pagina 5 c’é scritto ciò che interessa e chiederei solo se secondo voi è fattibile fare cosí? Se così fosse allora avrei risolto il problema. (mi scuso che adesso non lo posso provare perché al momento non sono a casa e tornerò tra 3-4 giorni)
Lo so che nell’esempio usa il driver A4988 ed il mio è il drv8825 ma il funzionamento é pressoché simile.

Poi volevo chiedervi se il calcolo del pid va fatto a intervallo costante a circa 1:40 del tempo di caduta del robot oppure basta metterlo una volta nel loop e viene calcolato di continuo.

Grazie mille per l’aiuto e arrivederci :smiley:

Pilotare un motore passo passo con Arduino e il driver A4988.pdf (438 KB)

Buongiorno a tutti ho controllato svariate volte il codice e sembrerebbe funzionare!!! :smiley:

Quindi prima di concludere volevo chiedere un’ultima cosa: il calcolo PID lo devo fare ad intervalli costanti (o comunque il piú possibile costanti) oppure devo raccogliere diversi dati del giroscopio (ad esempio tra i 50 e i 100) poi farci la media e sottrarci il valore attuale per trovare l’errore ed calcolare il PID.

Grazie per la risposta e arrivederci :slight_smile:

Quindi prima di concludere volevo chiedere un’ultima cosa: il calcolo PID lo devo fare ad intervalli costanti (o comunque il piú possibile costanti) oppure devo raccogliere diversi dati del giroscopio (ad esempio tra i 50 e i 100) poi farci la media e sottrarci il valore attuale per trovare l’errore ed calcolare il PID.

Ho letto il pdf che hai allegato, però non ho approfondito più di tanto. Normalmente il calcolo viene fatto ad intervalli regolari, più regolari sono meglio è. Per quanto riguarda invece la durata dell’intervallo dipende da tempi di risposta che si desidera avere, cioè se viste le grandezze fisiche il pendolo non può oscillare a frequenza maggiore di 10Hz (T=1/F=1/10Hz=0.1s (100ms) e inutile calcolare ogni 10ms ammesso che la MCU ce la faccia.

Riguardo alla media dipende da quanto stabile è il valore ricavato dal giro, inoltre questo impiega un certo tempo a restituire i dati per cui, se è necessaria una media potresti ritrovarti a corto di tempo agendo in ritardo. Io proverei senza media al momento considerando il giro stabile e preciso quanto desiderato, poi se qualcosa non torna si prova a fare una media ma prima interrogati circa il tempo richiesto per acquisire e calcolare.

La mia esperienza con PID è limitata a motore DC che viene frenato manualmente per sperimentare il comportamento.

Ciao.

Buongiorno grazie per la risposta,

Riguardo il tempo di calcolo dei dati del sensore effettuerò una prova appena riesco (per vedere quanto tempo ci mette).
Il software avevo intenzione di strutturarlo con una condizione a fine ciclo loop che diventa vera se il tempo registrato ( dal richiamo precedente del pid) diventa >= al tempo che ho impostato in modo tale da richiamare il PID e settare i motori con i nuovi parametri. Nel resto del loop volevo metterci il calcolo della mpu e successivamente la possibilità di controllare il robot a distanza (bluthooth, comunque il mio obbiettivo per adesso è farlo stare in piedi). Quindi con questa impostazione é possibile che il programma riceva i dati della mpu piú volte prima che arrivi al calcolo pid e dunque mi sembrava giusto poter fare la media di questi valori per ottenere un risultato piú preciso. (la media é comunque in funzione di quante volte viene calcolato l’mpu)
Naturalmente il tempo che passa tra un ciclo pid e l’altro non può essere molto preciso perché magari un ciclo loop dura 6-7-8-9ms e magari l’impostazione del mio tempo é di 20ms e quindi non sarà mai preciso al millisecondi.
Ho ancora un dubbio sul pid perché se io gli inserisco il setpoint a 0° come input gli devo mettere il valore dell’angolo di inclinazione del giroscopio rispetto all’asse verticale? Oppure devo inserire l’errore dell’angolo precedente - l’angolo attuale?

Grazie e arrivederci :smiley:

Le tue perplessità nei confronti del pid mi fanno capire che ti serve documentarti su questo algoritmo. Io la teoria non la conosco così bene da spiegarla.

In linea di massima più rapidamente esegui il calcolo e ad intervalli precisi più aumenta la banda passante e la stabilità. Detto in altri termini il calcolo del pid corregge il tiro, eseguito 1000 volte al secondo è meglio di eseguirlo 100 volte al secondo.

Quando ho parlato di feedback e non mi hai capito mi riferivo al fatto che per la mia esperienza il pid lo sperimentato con motore DC e encoder ottico. L'obbiettivo è grazie al pid di mantenere la velocità del motore costante indipendentemente dalla coppia frenante. Quindi come segnale di feedback l'encoder e il software ricavano la velocità attuale di rotazione, mentre il setpoint è la velocità richiesta da mantenere.

Ora tu usi come setpoin 0° e come segnale di feedback dovresti usare l'angolo verticale, nel caso del PID senza ID, cioè solo proporzionale. L'integrale e la derivata mi pare invece accumulino l'errore. I coefficienti K, I, D sono:
K è il guadagno (quindi moltiplicazione) applicato a P, mentre I e D invece mi pare incrementano o riducono l'effetto integrale e derivativo.

Puoi analizzare il codice della libreria PID di arduino, vedi se riesci a capirci qualcosa.

Puoi velocizzare il calcolo impiegando interi con segno anziché float, perché la MCU non ha FPU ( Float point unit ). Ad esempio un STM32-F4 ha una FPU ed esegue in calcoli via hardware.

Il software avevo intenzione di strutturarlo con una condizione a fine ciclo loop che diventa vera se il tempo registrato ( dal richiamo precedente del pid) diventa >= al

Si, verrà eseguito ad intervalli regolari solo se non impegni la MCU eccessivamente, cioè il loop deve essere più rapido possibile così che la condizione venga verificata frequentemente, come dire se viene verificato ogni 1ms al massimo avrai un ritardo di 1ms.

Il valore dell'angolo non sarà mai 0° ma oscillerà intorno a questo valore, maggiore è la frequenza di oscillazione maggiore sarà la stabilità a patto che l'ampiezza (+- gradi) sia contenuta.

Ciao.

buongiorno,

grazie per la risposta maurotec adesso controllerò meglio la libreria.

Oggi ho provato finalmente la libreria accelsepper e malgrado pensassi che funzionassi non funziona, si ha sempre lo stesso problema di prima cioè per far muovere il motore devo richiamare la funzione runSpeed() di continuo con intervalli massimi di qualche millisecondo e ciò mi riporta alla domanda iniziale come posso fare in modo che in motore funzioni in modo indipendente da me?

Se non c'e una risposta a questa domanda allora secondo voi dovrei cambiare il progetto togliendo i stepper motor e mettendoci motori dc con endcoder e un ponte H?
se cosi è allora volevo chiedervi quanti rpm devono minimo avere i motori ?

ciao

ciao a tutti scrivo questo post perché forse ho trovato un codice di un robot auto-bilanciante che utilizza i motori passo passo.
solo che nel codice ci sono parti che mi lasciano piuttosto perplesso perché viene utilizzato un linguaggio di basso livello come per il settaggio del pwm delle porte di Arduino e il settaggio portuale. Per questo volevo chiedervi se sapreste indicarmi una guida completa su questi argomenti (perché ho cercato molto su internet ma molti siti si limitano al digitalWrite() :smiley: e altri non sono poi tanto approfonditi). Comunque sia per la maggior parte degli argomenti ho trovato la spiegazione, ma per un paio di cose non le riesco a capire:
1.come è passibile che inserisca la PORT-E come output e la usa per il pin step dei driver, perchè ho guardato qualunque datasheet di arduino e del atmega 328 e la PORT-E non esiste ci sono solo: D, B, C.
2.utilizza il timer3, che a quanto ne capisco (poco :-[ ) arduino arriva solo fino al timer 2 (timer0,1,2 tra cui il n 0 è utilizzato per millis() micros() e il n 1 per tone() )
(mi piacerebbe sapere anche di più sull’argomento)

Vi lascio il codice intero al fondo(non è proprio come l’originale perché gli ho tolto le parti che non mi interessavano come servomotori o il funzionamento del’esp8266…), solo che il creatore ha utilizzato una mpu6050 senza driver (GY-521) e quindi il codice per ricavare il valore dell’angolo è molto complesso con il filtro complementare(in una libreria apposita).

Mi scuso per le molte possibili incomprensioni di quello che dico, anche perché non me ne intendo per niente dell’argomento e quindi lo cerco di spiegare nel modo più semplice che riesco, anche se é senza una terminologia adatta.

grazie mille e arrivederci :o

PROVAROBOT.ino (17.4 KB)

Ciao a tutti,
Oggi ho ricontrollato il progetto di questo robot e ho scoperto che non utilizza arduino uno ma leomardo.
E questo risolve i miei dubbi perché questa scheda ha un totale di 4-5 timer interni(non ricordo bene il numero preciso ma di sicuro piú di quattro) quindi usa il timer 3 che esiste, ed é presente la PORT-E in questa scheda.

Dopo essermi informato bene ho scoperto che Arduino uno ha i tre timer con bit diversi :frowning: (timer 0=8bit, timer 1=16bit, timer 2=8bit) é questo mi complica di molto le cose perché quelli che hanno fatto l'altro progetto lo sapevano e allora hanno preso la scheda leomardo e hanno utilizzato i due timer liberi da 16 bit (timer1 e timer3).
Lo so che ci sarebbe anche il timer 0 (che é a 8bit) solo che se vado a cambiare il prescaler a quello avrei poi molti problemi nelle funzioni Millis(), delay() ecc.. (anche il timer1, 2 hanno delle funzioni come la prima é per il servo() e la seconda per tone(), ma tanto a me non serve nessuna delle due)
Quindi non so se devo provare ad utilizzare timer 1 e 2 anche se hanno bit diversi faccio in modo da poter convertire proporzionalmente i valori, oppure se é possibile spostare la funzione delay(), millis()... Nel timer 1 per poter utilizzare il timer 0 e 2.

Grazie mille per la risposta, sarebbe un grande aiuto.
Arrivederci :slight_smile:

... ho idea che, dato l'uso dei timer a 16 bit, DEVI prendere un Arduino Leonardo (o, se vuoi abbondare e non hai problemi di spazio, un Arduino MEGA).

Il Timer 0 NON va toccato ... è utilizzato da varie cose all'interno del "core" di Arduino.

Guglielmo

P.S.: ... questo però succede quando NON si fa un attenta analisi del progetto prima di acquistare l'hardware, ma si compra "per sentito dire" senza sapere cosa in realtà occorre ... ::slight_smile: Ricordalo per i futuri progetti.

Grazie per la risposta,

Se devo dire la verità questo era un progetto che avevo iniziato diversi mesi fa e che avevo poi messo da parte per altri progetti... Devo dire che quando lo avevo iniziato a progettare (appunto molti mesi fa) ho sbagliato perché come hai detto appunto tu non avevo controllato il progetto di persona (forse anche perché non avevo le capacità) ma ho preso spunto da altri progetti (alcuni progetti mi hanno tratto in inganno perché usavano Arduino nano con i motori passo passo ma questi robot stavano solo in piedi e non é possibile controllarli appunto perché usavano un solo timer l'1 di Arduino per controllare TUTTI E DUE i motori e quindi non era possibile controllare la velocità dei singoli) pensando che fosse semplice costruirlo con motori dc allora ho voluto provarlo a fare con motore passo passo(perché nella mia ignoranza ho pensato che ci fosse un modo di controllarli senza troppi problemi magari con anche solo qualche libreria). Anche se devo dire che sono stato piuttosto stupido perché la maggior parte dei robot erano costruiti con motori dc e non stepper (e chissà perché :roll_eyes: :smiley: :smiley: ) . E invece adesso sono qui con decisamente molte conoscenze di Arduino rispetto a prima (perché non ho mai provato ad programmare timer interni e settare i registri portuali e altre piccole cose), ma comunque sempre poche rispetto a ciò che c'é ancora da imparare, ed essendo auto-didatta certi argomenti sono per me confusi perché mancano le basi che ci sono nei libri dell'università che adesso non ho acesso. In un certo senso lo scopo di questo progetto ha funzionato mi ha fatto imparare molte lezioni come non fidarsi ciecamente di tutto ciò che si trova su internet (lo so che sembra scontato ma per l'elettronica é fondamentale) e sopratutto lavorare con la propria testa e non quella degli altri, quindi per i prossimi progetti non farò piú lo stesso errore. Prenderò sempre spunto da internet ma questa volta approfondirò ogni minimo dubbio finché non avrò fatto tutto il progetto completo su carta e allora inizierò a costruirlo e programmarlo.
Alla fine dei conti ho 16 anni quindi questi errori stupidi per adesso ci possono stare :stuck_out_tongue: :P.

Comunquesia tornando al progetto adesso vedrò se cambiare la scheda Arduino nano con un Arduino Leonardo e di conseguenza dovrei riprogettare tutto lo scheletro del robot (che avevo stampato con la stampante 3D :D).
Spero che questo thread serva a qualcun'altro per non rifare lo stesso errore.

E con questo ho finito e risolto il problema

Grazie mille a tutti e arrivederci ;D