Go Down

Topic: Pitch e roll con giroscopio mpu6050 (Read 548 times) previous topic - next topic

ichigokurosaki

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?

ichigokurosaki

#1
Apr 18, 2018, 05:33 pm Last Edit: Apr 18, 2018, 05:35 pm by ichigokurosaki
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

doppiozero

Runtime Clock Manager -- https://github.com/duezero/RCM---Runtime-Clock-Manager
GPX datalogger -- https://github.com/duezero/GPX-datalogger
AVR HV Rescue Board -- https://www.youtube.com/watch?v=EErKo0aTEio

ichigokurosaki

#3
Apr 18, 2018, 08:50 pm Last Edit: Apr 18, 2018, 08:54 pm by ichigokurosaki
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).

Patrick_M

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ò.....
per inserire (lo sketch) il programma, dall'IDE clicca modifica, clicca copia per il forum poi vieni qui e incolla nel tuo post (ctrl+v) ;)

ichigokurosaki

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

ichigokurosaki

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?

Patrick_M

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;
per inserire (lo sketch) il programma, dall'IDE clicca modifica, clicca copia per il forum poi vieni qui e incolla nel tuo post (ctrl+v) ;)

ichigokurosaki

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

ichigokurosaki

Altrimenti dovrei usare due Arduini e farli comunicare tramite tx e rx così in uno uso la shield e nell'altro uso il giroscopio ?

docdoc

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...
Alex "docdoc" - ** se ti sono stato d'aiuto, un punto karma sarà gradito, clicca su "add" qui a sinistra, vicino al mio nome ;) **

Patrick_M

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..
per inserire (lo sketch) il programma, dall'IDE clicca modifica, clicca copia per il forum poi vieni qui e incolla nel tuo post (ctrl+v) ;)

ichigokurosaki

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

docdoc

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".
Alex "docdoc" - ** se ti sono stato d'aiuto, un punto karma sarà gradito, clicca su "add" qui a sinistra, vicino al mio nome ;) **

ichigokurosaki

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?

Go Up