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?
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
Ciao, posta lo sketch
#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).
io son sempre qui a rompere i maroni con la stessa storia 
occhio che questa
init_MPU();
con questa
#define MPU 0x68
diventa
init_0x68();
poi non succede nulla e tutto funziona però.....
Patrick_M:
io son sempre qui a rompere i maroni con la stessa storiaocchio 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
Patrick_M:
io son sempre qui a rompere i maroni con la stessa storiaocchio 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?
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;
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;
Si e quindi più o meno è lo stesso, poi funzionava solo quando cancello la riga dove definisco l'indirizzo della shield adafruit a 0x41
Altrimenti dovrei usare due Arduini e farli comunicare tramite tx e rx così in uno uso la shield e nell'altro uso il giroscopio ?
Patrick_M:
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...
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..
Patrick_M:
si hai ragione docdoc l'ho riletto dopoio 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
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".
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".
Ah ho capito così vedo cosa succede durante questi calcoli giusto?
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".
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 ^.^
Probabilmente il problema di conversione succedeva perché uint16_t è un unsigned giusto?
ichigokurosaki:
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.
docdoc:
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 ^.^