Arduino Forum

International => Italiano => Software => Topic started by: ichigokurosaki on Apr 18, 2018, 02:05 pm

Title: Pitch e roll con giroscopio mpu6050
Post by: ichigokurosaki on Apr 18, 2018, 02:05 pm
Ciao, sto provando a calcolare gli angoli pitch e roll dai valori dell'accelerometro del sensore mpu6050. Quando collego il sensore ai pin della mia servo shield, per il roll va bene, ma mi dà valori sballati per il pitch. Se invece lo collego direttamente ad arduino (shield scollegata) mi dà valori giusti anche per il pitch. Cioè è possibile che arctan mi restituisca un valore pari a 500 solo per il pitch e solo se collegato alla shield?
Title: Re: Pitch e roll con giroscopio mpu6050
Post by: ichigokurosaki on Apr 18, 2018, 05:33 pm
Ah in realtà ho notato che il problema sorge anche se disconnetto la shield, ma se cancello la riga

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x41);

il problema svanisce. Cosa può essere?
allego le screenshot dei valori sballati di pitch, e ora ho notato che i valori si sballano anche in roll a meno che non lo inclino troppo
I valori li ho convertiti in gradi
Title: Re: Pitch e roll con giroscopio mpu6050
Post by: doppiozero on Apr 18, 2018, 06:40 pm
Ciao, posta lo sketch
Title: Re: Pitch e roll con giroscopio mpu6050
Post by: ichigokurosaki on Apr 18, 2018, 08:50 pm
Code: [Select]
#include <SPI.h>
#include <Wire.h>
#include <Servo.h>
#define MPU 0x68
#include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x41);
#define SERVOMIN  150
#define SERVOMAX 600
uint8_t servonum = 0;
 
double AcX,AcY,AcZ;
uint16_t Pitch, Roll;
 
void setup(){
  Serial.begin(9600);
  init_MPU();
  pwm.begin();
  pwm.setPWMFreq(60);  // Analog servos run at ~60 Hz updates
  yield();
}
void setServoPulse(uint8_t n, double pulse) {
  double pulselength;
  pulselength = 1000000;   
  pulselength /= 60;   
  Serial.print(pulselength); Serial.println(" us per period");
  pulselength /= 4096; 
  Serial.print(pulselength); Serial.println(" us per bit");
  pulse *= 1000;
  pulse /= pulselength;
  Serial.println(pulse);
  pwm.setPWM(n, 0, pulse);
}
 
void loop()
{
  FunctionsMPU();
   
  Roll = FunctionsPitchRoll(AcX, AcY, AcZ);   
  Pitch = FunctionsPitchRoll(AcY, AcX, AcZ); 
 
  int ServoRoll = map(Roll, -90, 90, 0, 179);
  int ServoPitch = map(Pitch, -90, 90, 179, 0);
  servonum=4;
  pwm.setPWM(servonum, 0, ServoPitch);
  servonum=5;
  pwm.setPWM(servonum, 0, ServoRoll);
 
 
  Serial.print("Pitch: "); Serial.print(ServoPitch);
  Serial.print("\t");
  Serial.print("Roll: "); Serial.print(ServoRoll);
  Serial.print("\n");
 
}
 
void init_MPU(){
  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);
}
 
//Funzione per il calcolo degli angoli Pitch e Roll
  double FunctionsPitchRoll(double A, double B, double C){
  double DatoA, DatoB, Value;
  DatoA = A;
  DatoB = (B*B) + (C*C);
  DatoB = sqrt(DatoB);
 
  Value = atan2(DatoA, DatoB);
  Value = Value * 450/3.14 + 150;
 
  return (uint16_t)Value;
}
 
//Funzione per l'acquisizione degli assi X,Y,Z del MPU6050
void FunctionsMPU(){
  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)
}


In realtà in questo sketch converto gli angoli da radianti a un valore compreso tra 150 e 600 per il segnale pwm per pilotare dei servo in base agli angoli pitch e roll, e addirittura come vedi in screenshot in certi casi mi restituisce 60000 (e la screenshot l'ho usata convertendo da radianti in gradi, quindi con questo sketch mi verrebbe più di 100000).
Title: Re: Pitch e roll con giroscopio mpu6050
Post by: Patrick_M on Apr 18, 2018, 09:45 pm
io son sempre qui a rompere i maroni con la stessa storia :D

occhio che questa

 init_MPU();

con questa

#define MPU 0x68

diventa

init_0x68();

poi non succede nulla e tutto funziona però.....
Title: Re: Pitch e roll con giroscopio mpu6050
Post by: ichigokurosaki on Apr 18, 2018, 09:54 pm
io son sempre qui a rompere i maroni con la stessa storia :D

occhio che questa

 init_MPU();

con questa

#define MPU 0x68

diventa

init_0x68();

poi non succede nulla e tutto funziona però.....
Ah grazie per la correzione non ci avevo fatto caso in effetti hai ragione
Title: Re: Pitch e roll con giroscopio mpu6050
Post by: ichigokurosaki on Apr 19, 2018, 10:46 am
io son sempre qui a rompere i maroni con la stessa storia :D

occhio che questa

 init_MPU();

con questa

#define MPU 0x68

diventa

init_0x68();

poi non succede nulla e tutto funziona però.....
Ma secondo te come mai mi si sballano i valori di pitch e roll? Ho pure usato map e poi come fa arctan a restituire un valore così alto?
Title: Re: Pitch e roll con giroscopio mpu6050
Post by: Patrick_M on Apr 19, 2018, 12:46 pm
non so, l'unica cosa diversa rispetto all'originale dove l'hai copia/incollata è


Value = Value * 450/3.14 + 150;

al posto di:

 Value = Value * 180/3.14;
Title: Re: Pitch e roll con giroscopio mpu6050
Post by: ichigokurosaki on Apr 19, 2018, 12:50 pm
non so, l'unica cosa diversa rispetto all'originale dove l'hai copia/incollata è


Value = Value * 450/3.14 + 150;

al posto di:

 Value = Value * 180/3.14;
Si e quindi più o meno è lo stesso, poi funzionava solo quando cancello la riga dove definisco l'indirizzo della shield adafruit a 0x41
Title: Re: Pitch e roll con giroscopio mpu6050
Post by: ichigokurosaki on Apr 19, 2018, 01:05 pm
Altrimenti dovrei usare due Arduini e farli comunicare tramite tx e rx così in uno uso la shield e nell'altro uso il giroscopio ?
Title: Re: Pitch e roll con giroscopio mpu6050
Post by: docdoc on Apr 19, 2018, 01:09 pm
Value = Value * 450/3.14 + 150;
Infatti aveva scritto "in questo sketch converto gli angoli da radianti a un valore compreso tra 150 e 600 per il segnale pwm" ;), per cui attualmente non mi viene in mente nulla se non qualcosa che provoca conflitto tra le schede...
Title: Re: Pitch e roll con giroscopio mpu6050
Post by: Patrick_M on Apr 19, 2018, 04:10 pm
si hai ragione docdoc l'ho riletto dopo

io penso che sia un problema di librerie, probabilmente la pwm usa qualche funzione dove va a toccare i timer o qualche interrupt..
Title: Re: Pitch e roll con giroscopio mpu6050
Post by: ichigokurosaki on Apr 19, 2018, 04:14 pm
si hai ragione docdoc l'ho riletto dopo

io penso che sia un problema di librerie, probabilmente la pwm usa qualche funzione dove va a toccare i timer o qualche interrupt..

Forse allora dovrei definirmi un timer di aggiornamento dei valori del giroscopio? Più che altro mi sembra strano che i valori siano così alti, perché arctan come fa a restituire un valore così alto? Ho pure usato la funzione map
Title: Re: Pitch e roll con giroscopio mpu6050
Post by: docdoc on Apr 19, 2018, 04:36 pm
Mah, strano.
Magari per avere qualche indicazione prova a scrivere con Serial.print() qualcosa in più, come AcX, AcY e AcZ dentro FunctionsMPU, e poi Value in FunctionsPitchRoll sia dopo atan2() sia dopo la rimappatura.
E confronta anche questi tra il "prima" e "dopo".
Title: Re: Pitch e roll con giroscopio mpu6050
Post by: ichigokurosaki on Apr 19, 2018, 04:45 pm
Mah, strano.
Magari per avere qualche indicazione prova a scrivere con Serial.print() qualcosa in più, come AcX, AcY e AcZ dentro FunctionsMPU, e poi Value in FunctionsPitchRoll sia dopo atan2() sia dopo la rimappatura.
E confronta anche questi tra il "prima" e "dopo".

Ah ho capito così vedo cosa succede durante questi calcoli giusto?
Title: Re: Pitch e roll con giroscopio mpu6050
Post by: ichigokurosaki on Apr 20, 2018, 12:27 am
Mah, strano.
Magari per avere qualche indicazione prova a scrivere con Serial.print() qualcosa in più, come AcX, AcY e AcZ dentro FunctionsMPU, e poi Value in FunctionsPitchRoll sia dopo atan2() sia dopo la rimappatura.
E confronta anche questi tra il "prima" e "dopo".

Bene mi hai dato un' ottima idea! infatti i risultati sono strani:
dentro la funzione FunctionsPitchRoll:
roll (radianti): -0.81   
roll (gradi): -46.33   
pitch (radianti): 0.49   
pitch (gradi): 28.05

e fin qui ci siamo.

Ora il valore "ritornato" dalla funzione e assegnato a pitch

Pitch: 28

e fin qui ci siamo, poi quello assegnato a roll
   
Roll: 65490   

ma non era -46,33 gradi? Perché non ha ritornato il valore corretto? e poi è altissimo.

Pitch dopo la funzione map

Pitch: 62   

ma non era 28?

e per ultimo roll dopo la funzione map

Roll: 65215

insomma, i problemi sorgono all'interno della funzione main. Ho quindi pensato a un problema di conversione, che usando sempre int al posto di uint16_t si è risolto. Mi hai consigliato un ottimo metodo di debug ^.^
Title: Re: Pitch e roll con giroscopio mpu6050
Post by: ichigokurosaki on Apr 20, 2018, 09:58 am
Probabilmente il problema di conversione succedeva perché uint16_t è un unsigned giusto?
Title: Re: Pitch e roll con giroscopio mpu6050
Post by: docdoc on Apr 20, 2018, 07:08 pm
Probabilmente il problema di conversione succedeva perché uint16_t è un unsigned giusto?
Esatto. La "u" iniziale indica "unsigned", poi "int16" un intero 16 bit.
Per quello i valori negativi diventavano incasinati...

Contento di averti aiutato.
Title: Re: Pitch e roll con giroscopio mpu6050
Post by: ichigokurosaki on Apr 20, 2018, 07:41 pm
Esatto. La "u" iniziale indica "unsigned", poi "int16" un intero 16 bit.
Per quello i valori negativi diventavano incasinati...

Contento di averti aiutato.
Ok grazie ancora ^.^