Comprendere sketch robot autobilanciante

Buongiorno :slight_smile:
mi sto trovando ad affrontare l'aspetto della gestione di un robot autobilanciante (self balancing robot) che ormai ho quasi concluso di costruire.

Mi trovo qui a scrivere perchè mi possiate aiutare a comprendere meglio, correggere e magari migliorare le idee che fino ad ora ho raccolto per scrivere lo sketch che poi controllerà le funzioni del robot.
Ribadisco che fin ora sono ancora solo idee, ma consolidate queste ritengo sarebbe pi√Ļ efficace la scrittura dello sketch.

Se pur siano già disponibili diversi sketch per il medesimo fine, purtroppo non sono ancora stato in grado di comprendere appieno tutto ciò che in essi si trova scritto, a causa della mia inesperienza che comporta la mia mancanza, per ora, degli strumenti necessari alla comprensione deduttiva del problema. Spero possiate comprendere.

Per iniziare ecco i componenti elettronici essenziali del robot:
Arduino UNO come u-controllore
MPU6050 o (GY-521)= Accelerometro & giroscopio
GY-271 (HMC5883L)= Magnetometro
Modulo L298N = Controllore per i due motori con motoriduttori 12V 200RPM senza encoder.
JY-MCU= Modulo bluetooth

La mia idea, diffusissima fra le strategie di controllo, √® quella di utilizzare un regolatore PID che sfrutti i dati forniti dall'implementazione dei dati forniti dall' MPU6050 elaborati da filtro di Kalman per fare in modo che il robot possa mantenere una posizione "eretta" il pi√Ļ stabile possibile; a questo riguardo ho visto spesso usare dei potenziometri collegati a degli ingressi analogici per poter intervenire sulle costanti Kp; Ki; Kd, piuttosto che modificarli ogni volta all'interno del codice; e penso far√≤ cos√¨.
Giungendo al magnetometro: pensavo di sfruttarlo per fare in modo che durante gli spostamenti "retti" il robot mantenesse in concreto una traiettoria retta, questo potrebbe essere fattibile per mezzo di un regolatore PID o PI (senza l'impiego dell'azione derivativa) che sfrutti i dati forniti dal magnetometro? oppure mi consigliereste un'altra strategia?
Inoltre pensavo di sfruttare i dati del magnetometro elaborati in modo che possa dare una stima dell'angolazione in gradi rispetto al Nord (ho gi√† lo sketch) per poter dare comandi pi√Ļ fini tramite bluetooth in caso di rotazione quindi poter comunicare al robot direttamente di quanti gradi girare...

Ora chiedo a voi; potrebbe andare? conoscete mica strategie pi√Ļ performanti e me le consigliereste?

Ringrazio tutti moltissimo. Buona serata

Sapreste anche dirmi se sarebbe possibile controllare anche un cicalino un monitor LCD (con i2c) ed eventuali altri sensori senza compromettere l’ equilibrio del robot?

so che chiedo molto, vi ringrazio molto per la pazienza

Buongiorno!
ho completato la realizzazione del robot autobilanciante ed ho iniziato a provare a scrivere uno sketch che potesse permettergli di rimanere (pseudo) fermo in equilibrio.

Utilizza una controllore PID le cui costanti kP, kI, e kD sono regolabili tramite potenziometri da 10K;
l'errore è calcolato per mezzo di differenza fra un valore di equilibrio e l'angolo del robot (calcolato con filtro di Kalman) dai dati ottenuti dall' IMU MPU6050.

Pensate che lo sketch sia valido? Ci sono errori o parti che devono essere migliorate? Ho passato due giorni a giocare con i valori delle costanti cercando di raggiungere lo scopo finale, che tuttavia non ho ancora raggiunto =( =(
Dipende dallo sketch? oppure "semplicemente" di tempo ne va perso ancora molto perchè il risultato finale è ottenibile già così?

Nel codice è presente un codice per il filtro di Kalman del quale riporto anche la parte inerente i diritti copyright. Il filtro genera l'angolo sottoforma del valore kalAngleY; quello di interesse in funzione dell'orientamento del sensore sul robot.
E' presente come sensore nel codice anche un magnetometro, che per ora non è utilizzato.

...

devo dividere il codice in pi√Ļ parti perch√® supera il numero massimo di caratteri inseribili nel messaggio‚Ķ

Prima parte:

/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.

 This software may be distributed and modified under the terms of the GNU
 General Public License version 2 (GPL2) as published by the Free Software
 Foundation and appearing in the file GPL2.TXT included in the packaging of
 this file. Please note that GPL2 Section 2[b] requires that all works based
 on this software must also be made publicly available under the terms of
 the GPL2 ("Copyleft").

 Contact information
 -------------------

 Kristian Lauszus, TKJ Electronics
 Web      :  http://www.tkjelectronics.com
 e-mail   :  kristianl@tkjelectronics.com
 */

#include <Wire.h>
#include <LCD.h>
#include <LiquidCrystal_I2C.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_HMC5883_U.h>
#include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter


#define I2C_ADDR    0x27
Adafruit_HMC5883_Unified mag = Adafruit_HMC5883_Unified(12345);
LiquidCrystal_I2C lcd(I2C_ADDR, 4, 5, 6, 0, 1, 2, 3, 7, NEGATIVE);


Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;

/* IMU Data */
int16_t accX, accY, accZ;
int16_t tempRaw;
int16_t gyroX, gyroY, gyroZ;

double accXangle, accYangle; // Angle calculate using the accelerometer
double temp; // Temperature
double gyroXangle, gyroYangle; // Angle calculate using the gyro
double kalAngleX, kalAngleY; // Calculate the angle using a Kalman filter

double kP , kI, kD;

uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data

//...................................Da qui algoritmo Robot...
  
    int DX=5;
    int IN1=2;
    int IN2=4;
    int IN3=7;
    int IN4=8; 
    int SX=6;
  float Equilibrio;    // maggiore di 275   da verificare!!!!
  int StartPointPWM = 140
  ; // da verificare; se il robot non riesce a risollevarsi abbastanza in fretta potrebbe dipendere da questo valore 119 da aumentere

  float Errore = kalAngleY - Equilibrio;
  float previousErrore = 0;           // Inclinazione dell'AutoBalance al ciclo precedente

// Variabili temporali, per il calcolo del PID 
  float time = 0; // Tempo auttuale, dall'accensione del dispositivo 
  float previousTime = 0; // Tempo del precedente ciclo 
  float intervallo = 0;  // Tempo impiegato per l'esecuzione di un ciclo di programma 

// Variabili PID 
  float P = 0;   // Variabile PROPORZIONALE del PID 
  float I = 0;   // Variabile INTEGRATIVA del PID 
  float D = 0;   // Variabile DERIVATIVA del PID 
  float PID = 0; // Parametro PID 

// Guadagni dei coefficenti PID 
//  float kP = 5;  // 14, 0.02, 0.001
//  float kI = 0.01;     // nel tempo corregge la tendenza ad inclinarsi da una parte
//  float kD = 0.05;  // Coefficiente DERIVATIVA del PID
//............................... FINE INIZIALIZZAZIONE ..................................................................

float sommaKal = 0;
int   contatore = 0;
int   fineSondaggio = 200; /*serve per la media dell'angolo di equilibrio; 
                             dopo che viene avviato il programma bisogna mantenere 
                             il robot nella posizione di equilibrio; la media dei 
                             valori registrati costituirà l'angolo di equilibrio */

  void setup() {
   
      pinMode(DX, OUTPUT);
      pinMode(SX, OUTPUT);
      pinMode(IN1,OUTPUT);
      pinMode(IN2,OUTPUT);
      pinMode(IN3,OUTPUT);
      pinMode(IN4,OUTPUT);
      pinMode(A0,INPUT);
      pinMode(A1,INPUT);
      pinMode(A2,INPUT);
  
   lcd.begin (16,2);
   lcd.setBacklight(1);
   
   if(!mag.begin())
  {
    /* There was a problem detecting the HMC5883 ... check your connections */
    Serial.println("Ooops, no HMC5883 detected ... Check your wiring!");
    while(1);
  }
  
  Serial.begin(115200);
  Wire.begin();
  TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz

  i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
 ¬†i2cData[2] = 0x00; // Set Gyro Full Scale Range to ¬Ī250deg/s
 ¬†i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ¬Ī2g
  while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
  while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode

  while (i2cRead(0x75, i2cData, 1));
  if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("Error reading sensor"));
    while (1);
  }


  /* Set kalman and gyro starting angle */
  while (i2cRead(0x3B, i2cData, 6));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  // atan2 outputs the value of -? to ? (radians) - see http://en.wikipedia.org/wiki/Atan2
  // We then convert it to 0 to 2? and then from radians to degrees
  accYangle = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
  accXangle = (atan2(accY, accZ) + PI) * RAD_TO_DEG;

  kalmanX.setAngle(accXangle); // Set starting angle
  kalmanY.setAngle(accYangle);
  gyroXangle = accXangle;
  gyroYangle = accYangle;

  timer = micros();
  
  delay(5000);
}

void loop() {
//****************************** COSTANTI ****************************************************************************

    int potKp = analogRead(A2);
    int potKi = analogRead(A1);
    int potKd = analogRead(A0);
        
    kP = map(potKp, 0, 1023, 0, 2500) / 100.0; //0 - 25 costante proporzionale regolabile con pot 10K
    kI = map(potKi, 0, 1023, 0, 1500) / 100.0; //0 - 15 costante integrale regolabile con pot 10K   
    kD = map(potKd, 0, 1023, 0, 500) / 100.0; //0 - 5   costante derivata regolabile con pot 10K
  
//**************************  ACCELLEROMETRO  ***************************************************************************
  /* Update all the values */
  while (i2cRead(0x3B, i2cData, 14));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  tempRaw = ((i2cData[6] << 8) | i2cData[7]);
  gyroX = ((i2cData[8] << 8) | i2cData[9]);
  gyroY = ((i2cData[10] << 8) | i2cData[11]);
  gyroZ = ((i2cData[12] << 8) | i2cData[13]);

  // atan2 outputs the value of -? to ? (radians) - see http://en.wikipedia.org/wiki/Atan2
  // We then convert it to 0 to 2? and then from radians to degrees
  accXangle = (atan2(accY, accZ) + PI) * RAD_TO_DEG;
  accYangle = (atan2(accX, accZ) + PI) * RAD_TO_DEG;

  double gyroXrate = (double)gyroX / 131.0;
  double gyroYrate = -((double)gyroY / 131.0);
  gyroXangle += gyroXrate * ((double)(micros() - timer) / 1000000); // Calculate gyro angle without any filter
  gyroYangle += gyroYrate * ((double)(micros() - timer) / 1000000);


  kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros() - timer) / 1000000); // Calculate the angle using a Kalman filter
  kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros() - timer) / 1000000);
  timer = micros();

//*************************  TERMOMETRO  ******************************************************************************************

temp = ((double)tempRaw + 12412.0) / 340.0;

//*************************  MAGNETOMETRO  ****************************************************************************************
 
  sensors_event_t event; 
  mag.getEvent(&event);
 

  // Hold the module so that Z is pointing 'up' and you can measure the heading with x&y
  // Calculate heading when the magnetometer is level, then correct for signs of axis.
  float heading = atan2(event.magnetic.x, event.magnetic.z);
  
  
  // Find yours here: http://www.magnetic-declination.com/
  // Mine is: -13* 2' W, which is ~13 Degrees, or (which we need) 0.22 radians
  float declinationAngle =  0.22;  
  heading += declinationAngle;
  
  if(heading < 0)
    heading += 2*PI;
    
  if(heading > 2*PI)
    heading -= 2*PI;
   
  // Convert radians to degrees for readability.
  float headingDegrees = heading * 180/M_PI; 
  
//*********************  ON LCD  ************************************************************************************
  lcd.clear();

  
  lcd.setCursor(0,0);
  lcd.print(Errore);
  lcd.setCursor(8,0);
  lcd.print(Equilibrio);
  
  lcd.setCursor(0,1);
  lcd.print(kP);         //visualizza la costante kP regolata con potenziometro
    
  lcd.setCursor(6,1);
  lcd.print(kI);         //visualizza la costante kI regolata con potenziometro
       
  lcd.setCursor(12,1);   
  lcd.print(kD);         //visualizza la costante kD regolata con potenziometro
 

//************************** RICHIAMA LE FUNZIONI CIRCOSTANTI ***************

Grazie molte per l’attenzione :slight_smile:

//************************** RICHIAMA LE FUNZIONI CIRCOSTANTI ***************
    time = millis();                         // Salva il tempo corrente 
    intervallo = (time - previousTime)/1000; // Conta il tempo impiegato per eseguire un ciclo (sec) 
    ottieniErrore();                         
    ottieniPID();  
    controllomotori(); 
    previousTime = time;     // Memorizza il tempo  attuale 
    previousErrore = Errore; // Memorizza posizione attuale 

//************************** CERCA PUNTO DI EQUILIBRIO *******************************************************************
   if (contatore < fineSondaggio){
   contatore = contatore ++; 
   sommaKal = sommaKal + kalAngleY;}
   
   if (contatore >= fineSondaggio)
   {
   Equilibrio = sommaKal/fineSondaggio;  //attua la media dei valori dell'angolo ottenuti quando si mantiene il robot nella posizione di equilibrio
   }

}

//****************************** PID *********************************************************************************
void ottieniPID() // calcolo del PID 
{ 

  
  if( Errore  >= 255 ) 
  {Errore ==  255;   }
  if( Errore <= -255 ) 
  {Errore == -255;   }  
  
P = Errore * kP;                                      //Calcola la variabile proporzionale 
I = I + (P * intervallo * kI) ;                       // Calcola la variabile integrativa 
D = kD * ( Errore - previousErrore ) / intervallo;    // Calcola la variabile derivativa 
PID = (P + I + D);                                    // PID 


  if( PID + StartPointPWM >= 255 ) //impedisce all'analogeWrite del controllo motori di uscire dal range consentito
  {PID ==  255;  }
  if( PID - StartPointPWM <= -255 ) 
  {PID == -255;  }
  
  
}
//................................ Controllo Motori .................................................................
 void controllomotori () // controllo motori DC 
{
  if(PID>0 && contatore >=fineSondaggio )
  {  //Il robot è inclinato in avanti; quindi devo farlo muovere avanti
    analogWrite( DX,  StartPointPWM + PID ); 
    analogWrite( SX,  StartPointPWM + PID );
    
    digitalWrite(IN1,HIGH); //Sx avanti
    digitalWrite(IN2,LOW);
    digitalWrite(IN3,HIGH); //Dx avanti
    digitalWrite(IN4,LOW);
  }  
  
   if(PID<0 && contatore >=fineSondaggio)
  {  //Il robot è inclinato indietro; quindi devo farlo muovere indietro
    analogWrite( DX, StartPointPWM - PID );
    analogWrite( SX, StartPointPWM - PID );
    
    digitalWrite(IN1,LOW); //Sx indietro
    digitalWrite(IN2,HIGH);
    digitalWrite(IN3,LOW); //Dx indietro
    digitalWrite(IN4,HIGH);
  }

  if(PID=0 || contatore<fineSondaggio )
  {  //Il robot è in equilibrio. quindi deve stare fermo
    analogWrite( DX, 0);
    analogWrite( SX, 0);
    
    digitalWrite(IN1,LOW); //Sx fermo
    digitalWrite(IN2,LOW);
    digitalWrite(IN3,LOW); //Dx fermo
    digitalWrite(IN4,LOW);
  }
  
}
//GetErrore: 
float ottieniErrore()
{
Errore = kalAngleY - Equilibrio;
}

ciao e davvero molte grazie